diff --git a/.agents/skills/add-atomic-action/SKILL.md b/.agents/skills/add-atomic-action/SKILL.md index da144d6e6..ee825ecfe 100644 --- a/.agents/skills/add-atomic-action/SKILL.md +++ b/.agents/skills/add-atomic-action/SKILL.md @@ -20,15 +20,19 @@ Inspect only the files relevant to the requested skill: |---|---| | Base action and descriptors | `embodichain/lab/sim/atomic_actions/core.py` | | Goals and dynamic pose references | `embodichain/lab/sim/atomic_actions/goals.py` | -| Role-to-resource binding | `embodichain/lab/sim/atomic_actions/bindings.py` | +| Skill endpoint requirements | `embodichain/lab/sim/atomic_actions/requirements.py` | +| Resolved endpoint bindings and targets | `embodichain/lab/sim/atomic_actions/bindings.py` | | Invocation, options, and resolved request | `embodichain/lab/sim/atomic_actions/invocation.py` | | Control-part semantic commands | `embodichain/lab/sim/atomic_actions/control.py` | | Invocation policies | `embodichain/lab/sim/atomic_actions/policies.py` | | Robot/task/scene state | `embodichain/lab/sim/atomic_actions/state.py` | | Dynamic scene provider contract | `embodichain/lab/sim/atomic_actions/scene.py` | | Effects and plans | `embodichain/lab/sim/atomic_actions/effects.py`, `plans.py` | +| Runtime command frames and payloads | `embodichain/lab/sim/atomic_actions/runtime_commands.py` | +| Endpoint command transports | `embodichain/lab/sim/atomic_actions/transports.py` | | Trajectory helpers | `embodichain/lab/sim/atomic_actions/trajectory_ops.py` | | Engine-owned planning resources | `embodichain/lab/sim/atomic_actions/runtime.py` | +| Declarative robot resources and adapters | `embodichain/lab/sim/skills/profiles.py` | | Reference implementations | `embodichain/lab/sim/atomic_actions/primitives/` | | Static compiler and execution session | `engine.py`, `execution.py` | | Controller-facing execution ports | `runner.py`, `sim_adapter.py` | @@ -92,24 +96,33 @@ class PushOptions(ActionOptions): push_distance: float = 0.05 ``` -Do not put arm/hand names, hand qpos, or named robot postures in options. Bind -participants with `ActionBinding`. Register embodiment-specific commands such -as `open`, `grasp`, or `ready` on `ControlPartCommandProfile`; use +Do not put arm/hand names, hand qpos, or named robot postures in options. +Declare robot-independent participant slots and endpoints with +`SkillBindingContract`; the engine or a bound robot skill profile produces the +engine-owned `ActionBinding`. Register embodiment-specific commands such as +`open`, `grasp`, or `ready` on `ControlPartCommandProfile`; use `ActionControlOverrides` only for one invocation revision. ## 3. Implement the planner -Inherit `AtomicAction[PushGoal, PushOptions]` directly. Declare stable metadata and resolve -resources from semantic binding roles. +Inherit `AtomicAction[PushGoal, PushOptions]` directly. Declare stable metadata +and an explicit, robot-independent endpoint contract. Every concrete action +class must declare `binding_contract` in its own class body; use +`SkillBindingContract()` for a skill that consumes no robot resource. ```python from typing import ClassVar from embodichain.lab.sim.atomic_actions import ( - ResolvedActionRequest, ActionPlan, AtomicAction, + CARTESIAN_POSE_CAPABILITY, + JointPositionTarget, PlanningContext, + ResolvedActionRequest, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, StateDelta, ) from embodichain.lab.sim.atomic_actions.trajectory_ops import ( @@ -122,7 +135,19 @@ class Push(AtomicAction[PushGoal, PushOptions]): skill_id: ClassVar[str] = "push" GoalType: ClassVar[type] = PushGoal OptionsType: ClassVar[type] = PushOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + ), + ), + ), + ), + ) def __init__(self, default_options: PushOptions | None = None) -> None: super().__init__(default_options) @@ -133,11 +158,13 @@ class Push(AtomicAction[PushGoal, PushOptions]): context: PlanningContext, ) -> ActionPlan: goal = self.require_goal(request) - options = request.skill_options - manipulator = request.binding.manipulator("primary") - control_part = manipulator.name - joint_ids = list(manipulator.joint_ids) + motion_target = request.binding.endpoint( + "primary", "motion" + ).require_target(JointPositionTarget) + control_part = motion_target.control_part + joint_ids = list(motion_target.joint_ids) start_qpos = context.robot.qpos[:, joint_ids] + target_poses = goal.contact_pose # Build planner states and generate controlled-joint motion using # request.motion_policy. Embed it into full robot DoF. @@ -168,6 +195,11 @@ Follow these invariants: - Let the engine supply `self.robot` and `self.motion_generator`; use `_on_bind()` only for robot/device-dependent setup. +- Keep slot and endpoint IDs semantic and robot-independent. Declare all-of + capabilities, required typed commands, and disjointness constraints in the + `SkillBindingContract`; do not infer resources from endpoint names. +- Resolve an endpoint with `request.binding.endpoint(slot_id, endpoint_id)` and + call `require_target(ExpectedTarget)` before using target-specific fields. - Import pure target-shaping, interpolation, pose-translation, and full-robot embedding helpers directly from `atomic_actions.trajectory_ops`; keep stateful planning inside `MotionGenerator`. @@ -176,8 +208,8 @@ Follow these invariants: `plan()` method; the latter injects the latest dynamic obstacle poses into a copied planner policy. - Plan from `context.robot.qpos`, never an implicit live robot start state. -- Return full-robot `(B, N, robot.dof)` motion as a tensor or - `TimedTrajectory` with matching `env_ids`. +- For joint-backed motion, return full-robot `(B, N, robot.dof)` motion as a + tensor or `TimedTrajectory` with matching `env_ids` through `build_plan()`. - Preserve row-local planner success. `build_plan()` normalizes the mask and replaces unsuccessful trajectory rows with the context's observed qpos. - Preserve backend timing/derivatives when available. @@ -196,7 +228,60 @@ Follow these invariants: `collision_entity_ids`; supported planners receive those entity poses through the framework-owned `plan()` entry point. -## 4. Register and invoke +## 4. Emit generic runtime commands when needed + +Use `build_command_plan()` when a skill targets a mobile base, whole-body +controller, tool, or another non-joint transport. Build immutable endpoint +commands; keep live controller and device handles in the transport: + +```python +target = request.binding.endpoint("primary", "tool").require_target(ToolTarget) +frames = tuple( + RuntimeCommandFrame( + commands=(EndpointCommand(target=target, payload=ToolPayload(value)),), + active_mask=torch.ones( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ), + env_ids=context.env_ids, + hold_duration=torch.full( + (context.batch_size,), + request.motion_policy.control_dt, + device=context.robot.qpos.device, + ), + ) + for value in command_values +) +return self.build_command_plan( + request, + context, + success=success, + commands=TimedCommandSequence(frames=frames, env_ids=context.env_ids), +) +``` + +For a new transport kind: + +1. Define an immutable `RuntimeEndpointTarget` and `RuntimeCommandPayload` with + the same stable `transport_id`; both must return independently owned + snapshots. Payloads also expose `batch_size` and `device`. If target-specific + addressing or safe hold depends on fields beyond the exact target type, + `transport_id`, and `target_id`, override `address_fingerprint` to include + those immutable fields; frames, replans, and revisions preserve it. +2. If declarative robot profiles select it, define a `ResourceEndpoint` and an + exact-type `ResourceEndpointAdapter` that returns `EndpointResolution` with + the runtime target and physical claim metadata. +3. Implement `EndpointCommandTransport.send()`, `hold()`, and `cancel()`, then + register it in `EndpointCommandRouter` used as the `ExecutionRunner` command + sink. The router validates payload types before dispatch. + +The default command-plan feedback mode is timed and `joint_trajectory` is +optional. Use joint-position feedback only when a matching full-robot +`joint_trajectory` is supplied. Test target/payload snapshot ownership, frame +batch/device consistency, routing, acknowledgement, hold, and cancel behavior. + +## 5. Register and invoke Register an instance by its class-level `skill_id`: @@ -213,10 +298,14 @@ register_action(Push) Construct a grounded invocation explicitly: ```python +binding = engine.bind_control_parts( + "push", + {"primary": {"motion": "left_arm"}}, +) invocation = ActionInvocation( skill_id="push", goal=PushGoal(contact_pose), - binding=ActionBinding(manipulators={"primary": "left_arm"}), + binding=binding, motion_policy=MotionPolicy(sample_count=60), recovery_policy=RecoveryPolicy(max_replans=2), ) @@ -228,26 +317,33 @@ For dynamic scene updates or online error recovery, create a session with through `ExecutionRunner`. Use non-blocking `runner.step()` in an existing event loop or `runner.run_until_blocked()` in a simple application. -## 5. Export and document +`engine.bind_control_parts()` is the explicit direct-core path for joint-backed +control parts. When a `RobotSkillProfile` is installed, prefer +`engine.skill_profile.resolve("push", selections).action_binding` so capability, +command, resource-claim, and custom-adapter validation remain declarative. + +## 6. Export and document Export the goal, options, and action from: 1. `embodichain/lab/sim/atomic_actions/primitives/__init__.py` 2. `embodichain/lab/sim/atomic_actions/__init__.py` -Add the stable skill ID, goal, roles, and effect to +Add the stable skill ID, goal, binding slots/endpoints, and effect to `docs/source/overview/sim/atomic_actions/builtin_actions.md`. Update API docs for new public classes. Do not create a compatibility re-export module or a closed built-in-goal union. -## 6. Test behavior +## 7. Test behavior Add pure pytest tests under `tests/sim/atomic_actions/`. Cover: -- descriptor `skill_id`, `GoalType`, and required roles; -- invalid goal and missing binding rejection; +- descriptor `skill_id`, `GoalType`, and explicit binding contract; +- invalid goal, wrong binding owner, and missing/extra endpoint rejection; - per-environment planning success/failure masks; - full-robot trajectory shape, `env_ids`, timing, and failed-row hold behavior; +- generic command target/payload ownership, frame batch/device consistency, and + optional `joint_trajectory` behavior when the skill emits command frames; - side-effect-free context handling; - masked `StateDelta` application for task effects; - `SceneEntityPose` replanning when the action accepts a dynamic goal; @@ -264,9 +360,12 @@ then use the `pre-commit-check` skill before committing. |---|---| | Inherit another action | Inherit `AtomicAction` directly; compose helpers. | | Add one generic target with many optional fields | Define a narrow action-owned goal. | -| Put hardware names in the goal | Bind semantic roles through `ActionBinding`. | -| Put arm/hand control-part names in skill options | Use `ActionBinding` as their only source. | -| Bind a joint, link, TCP frame, or arbitrary name | Every binding value must be a key in `RobotCfg.control_parts`. | +| Put hardware names in the goal | Declare semantic slots/endpoints and resolve an engine-owned binding. | +| Put arm/hand control-part names in skill options | Read typed runtime targets from bound endpoints. | +| Declare legacy role tuples on the action | Declare a class-local `SkillBindingContract`. | +| Use role-specific binding accessors | Use `binding.endpoint(...).require_target(...)`. | +| Construct a binding from role dictionaries | Use a bound skill profile, or `engine.bind_control_parts()` for the direct joint path. | +| Pass an arbitrary joint/link/TCP name to the direct path | `bind_control_parts()` values must be keys in `RobotCfg.control_parts`; add an endpoint adapter for another resource kind. | | Put hand qpos or named robot postures in skill options | Register semantic commands on the concrete control-part profile. | | Put planner/recovery knobs in skill options | Move them to invocation policies. | | Pass a motion generator to each action | Pass it once to `AtomicActionEngine`; construct actions from default options only. | @@ -277,4 +376,6 @@ then use the `pre-commit-check` skill before committing. | Mutate held state after planning | Declare a `StateDelta`. | | Treat `plan_success` as physical success | Verify effects during execution. | | Step the simulator from the action | Emit plans; connect execution through `ExecutionRunner`. | +| Put live controller handles in targets or payloads | Keep immutable addressing/data in values and own handles in the transport. | +| Force a non-joint endpoint into a fake trajectory | Emit typed frames with `build_command_plan()` and install its transport. | | Override public `plan()` | Implement `_plan()` so scene binding cannot be bypassed. | diff --git a/.agents/skills/add-task-env/SKILL.md b/.agents/skills/add-task-env/SKILL.md index d063fa460..2492fa5bb 100644 --- a/.agents/skills/add-task-env/SKILL.md +++ b/.agents/skills/add-task-env/SKILL.md @@ -89,9 +89,6 @@ from . import Env __all__ = [..., "Env"] ``` -Optional compatibility re-export may also be added in -`embodichain/lab/gym/envs/tasks/__init__.py`. - ### 4. Create Test Stub Place at `tests/gym/envs/tasks/test_.py` (or `tests/learning/` for diff --git a/.agents/skills/add-test/SKILL.md b/.agents/skills/add-test/SKILL.md index 8d7f6ddeb..423e61cbb 100644 --- a/.agents/skills/add-test/SKILL.md +++ b/.agents/skills/add-test/SKILL.md @@ -21,7 +21,7 @@ Tests mirror the source tree under `tests/`: embodichain/lab/sim/solvers/pytorch_solver.py → tests/sim/solvers/test_pytorch_solver.py embodichain/lab/gym/envs/managers/rewards.py → tests/gym/envs/managers/test_reward_functors.py embodichain/toolkits/graspkit/pg_grasp/foo.py → tests/toolkits/test_pg_grasp.py -embodichain/lab/gym/envs/tasks/rl/push_cube.py → tests/gym/envs/tasks/test_push_cube.py +embodichain_tasks/embodichain_tasks/rl/push_cube.py → tests/gym/envs/tasks/test_push_cube.py ``` Rules: diff --git a/.agents/skills/pr/SKILL.md b/.agents/skills/pr/SKILL.md index 933f54279..a52af2078 100644 --- a/.agents/skills/pr/SKILL.md +++ b/.agents/skills/pr/SKILL.md @@ -82,6 +82,17 @@ git add -A git commit -m "Format code with black" ``` +Then run the read-only public API documentation gate used by CI: + +```bash +python docs/scripts/check_api_docs.py +``` + +If it reports missing public exports, invoke `$update-api-docs` to generate or +update the relevant Sphinx entries and descriptions, then rerun the checker. +Keep generation in that specialized skill; do not add placeholder API docs in +the PR workflow merely to make the gate pass. + ### 6. Create or Update Branch For a single PR, create a feature branch if needed: @@ -325,6 +336,7 @@ Fixes # - [x] I have run the `black .` command to format the code base. - [ ] I have made corresponding changes to the documentation +- [ ] Public API changes are reflected in the API docs (`python docs/scripts/check_api_docs.py`), if applicable - [ ] I have added tests that prove my fix is effective or that my feature works - [ ] Dependencies have been updated, if applicable. ``` @@ -345,6 +357,7 @@ Fixes # | `git status` | Check current state | | `git diff HEAD` | Show changes | | `black .` | Format code | +| `python docs/scripts/check_api_docs.py` | Check public API documentation coverage | | `git checkout -b branch-name` | Create branch | | `git push -u origin branch` | Push to remote | | `gh pr create` | Create PR | diff --git a/.agents/skills/pre-commit-check/SKILL.md b/.agents/skills/pre-commit-check/SKILL.md index 40f8a92f2..8e132ed86 100644 --- a/.agents/skills/pre-commit-check/SKILL.md +++ b/.agents/skills/pre-commit-check/SKILL.md @@ -39,7 +39,19 @@ black --check --diff --color ./ If it fails, run `black .` and review the formatting changes. -### 3. Check Apache 2.0 Copyright Header +### 3. Check Public API Documentation Coverage + +Run the same read-only gate used by CI: + +```bash +python docs/scripts/check_api_docs.py +``` + +If it reports missing exports, use `$update-api-docs` to add useful Sphinx +entries and descriptions. Do not change `__all__` solely to make this check +pass. + +### 4. Check Apache 2.0 Copyright Header Every `.py` file must begin with the 15-line copyright block. For each changed/new `.py` file, verify the first line is: @@ -67,11 +79,11 @@ The full header template: # ---------------------------------------------------------------------------- ``` -### 4. Check `from __future__ import annotations` +### 5. Check `from __future__ import annotations` Every `.py` file must have this import (after the header, before other imports). This enables `A | B` syntax and forward references. -### 5. Check `__all__` in Public Modules +### 6. Check `__all__` in Public Modules For any new or modified module under `embodichain/`, verify it defines `__all__` listing all public symbols. Example: @@ -81,7 +93,7 @@ __all__ = ["MyClass", "my_function"] Skip this check for `__init__.py` files that only re-export via `from . import *`. -### 6. Check Docstrings on Public APIs +### 7. Check Docstrings on Public APIs For any new public function, class, or method: - Must have a Google-style docstring @@ -89,7 +101,7 @@ For any new public function, class, or method: - Must include `Returns:` section if it returns a value - Use `.. attention::` or `.. tip::` directives for non-obvious behavior -### 7. Check Type Annotations +### 8. Check Type Annotations For any new public API: - All parameters must have type hints @@ -97,14 +109,14 @@ For any new public API: - Use `A | B` over `Union[A, B]` - Use `TYPE_CHECKING` guard for imports that would cause circular dependencies -### 8. Check `@configclass` Usage +### 9. Check `@configclass` Usage For any new configuration class: - Must use `@configclass` decorator (not bare `@dataclass`) - Must use `from dataclasses import MISSING` for required fields - Import from `embodichain.utils import configclass` -### 9. Select and Run Relevant Tests +### 10. Select and Run Relevant Tests Do not treat the CI test job as a requirement to run `pytest tests` locally for every change. Choose the smallest command set that exercises the affected @@ -132,14 +144,14 @@ Before starting a command likely to take more than two minutes, report the selected scope and why narrower validation is insufficient. Honor explicit user instructions to skip or narrow tests. -### 10. Check Test Coverage +### 11. Check Test Coverage For any new public module or function: - A corresponding test must exist at `tests//test_.py` - Test file must also have the Apache 2.0 header - Report if tests are missing -### 11. Summary Report +### 12. Summary Report Output a pass/fail summary: @@ -147,6 +159,7 @@ Output a pass/fail summary: Pre-Commit Check Results ======================== [PASS] Black formatting +[PASS] Public API docs coverage [PASS] Apache 2.0 headers (5/5 files) [FAIL] from __future__ import annotations — missing in: foo.py [PASS] __all__ exports @@ -165,8 +178,9 @@ Fix the above issues before committing. The project's CI pipeline (`.github/workflows/main.yml`) runs: 1. **lint** job: `black --check --diff --color ./` -2. **test** job: `pytest tests` -3. **build** job: Sphinx docs build +2. **lint** job: `python docs/scripts/check_api_docs.py` +3. **test** job: proportional pytest groups after lint passes +4. **build** job: Sphinx docs build after lint passes This skill always covers the relevant lint and structural checks, then selects tests proportionally. It does not require reproducing the entire CI pipeline for @@ -188,6 +202,7 @@ every local change. |-------|---------------| | Black formatting | `black --check --diff --color ./` | | Auto-fix formatting | `black .` | +| Public API docs | `python docs/scripts/check_api_docs.py` | | Header check | Verify first line is `# ---...---` | | `__future__` import | Grep for `from __future__ import annotations` | | `__all__` export | Grep for `__all__` in module | diff --git a/.agents/skills/project-dev-context/SKILL.md b/.agents/skills/project-dev-context/SKILL.md index 28a9e952d..e6b3b08ad 100644 --- a/.agents/skills/project-dev-context/SKILL.md +++ b/.agents/skills/project-dev-context/SKILL.md @@ -1,22 +1,17 @@ --- name: project-dev-context -description: Use when a request asks to reference, refresh, write, or register project development context so the agent resolves the topic through agent_context/MAP.yaml and reads or updates the mapped Markdown context files. +description: > + Route EmbodiChain development-context and codebase-navigation requests through + agent_context/MAP.yaml. Use when asked to locate files, configs, defaults, + entry points, registration paths, or change sites; explain a code or + configuration resolution chain; reference project context; refresh or write + context; register a context topic; or work with a named topic such as + simulation-system, env-framework, rl-learning, manager-functor, ik-solvers, + or atomic-actions. Chinese triggers include 文件在哪里、配置或默认值在哪里、 + 入口或注册逻辑在哪里、应该修改哪个文件、参考项目上下文、刷新项目上下文。 --- -# Project Dev Context - -Use this skill when: -- the request says `reference project development docs` -- the request says `reference project context` -- the request says `refresh project context` -- the request says `update project context` -- the request says `write project context` -- the request says `参考项目开发文档` -- the request says `参考项目上下文` -- the request says `刷新项目上下文` -- the request says `更新项目上下文` -- the request says `写项目上下文` -- the request names a known project topic such as `env-framework`, `manager-functor`, `ik-solvers`, or `atomic-actions` +# Project Development Context and Codebase Navigation ## Start here @@ -25,18 +20,50 @@ Use this skill when: - Read `agents/openai.yaml` for the canonical agent metadata - Read `agent_context/conventions/*.md` when creating or updating context files -## Workflow - -1. Resolve the topic through `agent_context/MAP.yaml` -2. Match in this order: exact `id`, then `aliases`, then `keywords` -3. Choose the operation mode: - - **read**: load only the matched Markdown files under `agent_context/` - - **refresh existing topic**: re-read `source_of_truth` and rewrite the mapped topic Markdown so it matches current implementation - - **add new topic**: write a new topic Markdown file and register it in `agent_context/MAP.yaml` -4. Load `agent_context/conventions/*.md` if you add or update context files -5. Do not re-read `docs/source/` unless the user explicitly asks for Sphinx documentation - -This skill routes context. It does not replace the underlying source-of-truth files listed in each topic entry. +## Select the operation + +- **navigate**: locate a file, symbol, config, default, entry point, + registration path, or recommended change site. +- **read**: load the matched agent context without changing it. +- **refresh**: rebuild an existing topic from its current + `source_of_truth`. +- **add**: create and register a new topic from current source code. + +An explicit refresh or add request determines the mode. If the user asks to +implement work covered by a specialized skill such as `add-robot`, +`add-solver`, or `add-functor`, let that skill own the implementation and +use this skill only for orientation or mapped context. + +## Route the request + +1. Resolve the topic through `agent_context/MAP.yaml`. +2. Match in this order: exact `id`, then `aliases`, then `keywords`. +3. For a matched read request, load only the Markdown files in `paths`. +4. For a matched navigation request, load the mapped topic and verify the + relevant path or behavior against the current `source_of_truth`. +5. For an unmatched navigation request: + - list candidate files with `rg --files`; + - search symbols, flags, config keys, registries, and imports with `rg -n`; + - inspect the closest package `__init__.py`, config loader, registry, + test, and entry point as relevant; + - inspect `pyproject.toml` and `embodichain/__main__.py` for CLI or + package-discovery questions. +6. Do not add a topic merely because navigation did not match. Propose or add + one only when the user requests it or the missing area is recurring. +7. Do not read `docs/source/` unless the user explicitly asks for Sphinx + documentation. + +Never treat topic Markdown as a substitute for current code. For navigation, +report only paths and behavior verified in the working tree. + +## Navigation answer contract + +Include the parts relevant to the request: + +- the entry point or owning location; +- the call, registration, or configuration-resolution path; +- the file or symbol to change; +- the focused tests or documentation affected by that change. ## Explicit refresh mode @@ -53,6 +80,16 @@ In refresh mode: 2. Re-read the files listed in `source_of_truth` 3. Rewrite the mapped topic Markdown from current implementation, not stale notes 4. Update `aliases`, `keywords`, `paths`, `related_topics` if needed +5. Load and follow `agent_context/conventions/*.md` + +## Add mode + +1. Choose a stable kebab-case topic id. +2. Read the current source files that define the topic. +3. Write one focused Markdown file under + `agent_context/topics//`. +4. Register the topic in `agent_context/MAP.yaml`. +5. Load and follow `agent_context/conventions/*.md`. ## Update contract @@ -66,7 +103,8 @@ If code behavior changes a routed topic, update all relevant pieces in the same ## Source-of-truth -This skill does not store the project knowledge itself. The canonical project context lives in: +This skill stores the routing procedure, not project facts. Canonical project +context lives in: - `agent_context/MAP.yaml` - `agent_context/topics/**/*.md` - `agent_context/conventions/*.md` diff --git a/.agents/skills/project-dev-context/references/context-system.md b/.agents/skills/project-dev-context/references/context-system.md index 511131707..835bb7c9c 100644 --- a/.agents/skills/project-dev-context/references/context-system.md +++ b/.agents/skills/project-dev-context/references/context-system.md @@ -5,14 +5,34 @@ EmbodiChain keeps agent-facing context in `agent_context/`, indexed by Claude Code project adapters use `.claude/skills//SKILL.md`, and GitHub Copilot adapters under `.github/copilot/` should stay thin. +## Operation Modes + +- `navigate`: locate current files, symbols, configs, defaults, entry points, + registration paths, and recommended change sites. +- `read`: load an existing topic without changing it. +- `refresh`: rebuild an existing topic from its current source of truth. +- `add`: create and register a new topic from current source code. + ## Routing Rules 1. Read `agent_context/MAP.yaml` first. 2. Resolve the requested topic by exact `id`, then `aliases`, then `keywords`. -3. Load only the matched Markdown files listed in the topic `paths`. -4. Do not read `docs/source/` unless the user explicitly asks for Sphinx +3. For read requests, load only the matched Markdown files listed in `paths`. +4. For navigation requests, verify the relevant mapped facts against the + current `source_of_truth`. +5. If navigation does not match a topic, search the working tree with + `rg --files` and `rg -n`. Inspect package exports, config loaders, + registries, tests, `pyproject.toml`, and `embodichain/__main__.py` as + relevant. +6. Do not create a topic for a one-off unmatched lookup unless the user asks + for it. +7. Do not read `docs/source/` unless the user explicitly asks for Sphinx documentation. +Navigation answers should identify the owning entry point, the call or config +resolution path, the recommended change site, and the focused validation +surface when those details are relevant. + ## Update Rules When behavior covered by a context topic changes, update the topic Markdown and diff --git a/.agents/skills/update-api-docs/SKILL.md b/.agents/skills/update-api-docs/SKILL.md new file mode 100644 index 000000000..4eb322515 --- /dev/null +++ b/.agents/skills/update-api-docs/SKILL.md @@ -0,0 +1,83 @@ +--- +name: update-api-docs +description: Generate or update EmbodiChain Sphinx API documentation for public Python exports. Use when the API docs checker or CI reports missing __all__ exports, after adding or changing public APIs, or when asked to fill, generate, or synchronize API-reference pages and their descriptions. +--- + +# Update API Docs + +Generate useful API documentation for every public export reported by the +read-only checker. Treat static ``__all__`` declarations as the public API +contract and preserve existing hand-written documentation. + +## Workflow + +1. Run the checker in machine-readable mode: + + ```bash + python docs/scripts/check_api_docs.py --format json + ``` + + Exit status 1 is expected when exports are missing. Read the JSON report; if + ``missing_count`` is zero, report that the docs are aligned and stop. + +2. Group missing entries by module. Read each reported source file and inspect + the exported definition, signature, type annotations, and docstring. Locate + existing API pages with: + + ```bash + rg -n "automodule:: |currentmodule:: " docs/source/api_reference + ``` + +3. Choose the documentation location: + + - Add the export to an existing curated module page when one exists. Follow + that page's headings, autosummary groups, and detailed autodoc directives. + - Document a package-level re-export under its public import path, not only + under the implementation module. + - If no suitable curated page exists, add or extend the module section in + ``docs/source/api_reference/public_api.rst``. This file is an + agent-maintained fallback, not checker output. Keep fallback module + headings sorted by import path and entries in their declared ``__all__`` + order to minimize diff churn. + +4. Write documentation that explains the API: + + - Add the export to the appropriate ``autosummary`` block. + - Add the matching ``autoclass``, ``autofunction``, ``autodata``, or other + detailed directive when the surrounding page provides detailed entries. + - Add a concise section overview when names alone do not explain the group. + - If the source docstring is missing or too vague for autodoc, improve it + with a meaningful summary and Google-style ``Args``, ``Returns``, and + ``Raises`` sections where applicable. + - Derive descriptions from the implementation and tests. Do not invent + behavior, examples, guarantees, or parameter semantics. + +5. Keep the change scoped to documentation. Do not alter runtime behavior, + signatures, or ``__all__`` merely to silence the checker. Do not replace + curated prose with generic generated text or add placeholders such as + "part of the public API." + +6. Rerun the checker until it reports zero missing exports: + + ```bash + python docs/scripts/check_api_docs.py + ``` + +7. Run Black on every changed Python file, then validate the documentation + workflow: + + ```bash + pytest tests/docs/test_check_api_docs.py -q --confcutdir=tests/docs + python -m sphinx -b dummy docs/source docs/build/api-docs-check + ``` + + When source docstrings were changed, also run focused tests for those + modules. Fix new Sphinx warnings caused by the edit; distinguish them from + unrelated pre-existing warnings. + +## Completion Report + +Report the documented import paths, the pages or docstrings updated, and the +validation results. If an export cannot be documented accurately from the +repository, identify the exact missing semantic information instead of +guessing. diff --git a/.agents/skills/update-api-docs/agents/openai.yaml b/.agents/skills/update-api-docs/agents/openai.yaml new file mode 100644 index 000000000..30b336340 --- /dev/null +++ b/.agents/skills/update-api-docs/agents/openai.yaml @@ -0,0 +1,4 @@ +interface: + display_name: "Update API Docs" + short_description: "Generate API reference docs for missing exports" + default_prompt: "Use $update-api-docs to document every Python API reported missing by the API docs checker." diff --git a/.claude/skills/project-dev-context/SKILL.md b/.claude/skills/project-dev-context/SKILL.md index cbb1487b7..bc258ddd7 100644 --- a/.claude/skills/project-dev-context/SKILL.md +++ b/.claude/skills/project-dev-context/SKILL.md @@ -1,6 +1,6 @@ --- name: project-dev-context -description: Claude adapter for the canonical EmbodiChain project-dev-context skill. +description: Claude adapter for locating EmbodiChain code, configs, defaults, entry points, and change sites or for reading and maintaining registered project context. --- # Project Dev Context - Claude Adapter @@ -9,6 +9,8 @@ Canonical source: `.agents/skills/project-dev-context/` ## When to use +- locate a file, config, default, entry point, registration path, or change site +- explain a code or configuration resolution chain - reference project development docs - reference project context - refresh project context @@ -20,10 +22,14 @@ Canonical source: `.agents/skills/project-dev-context/` - 刷新项目上下文 - 更新项目上下文 - 写项目上下文 +- 文件在哪里 +- 配置或默认值在哪里 +- 应该修改哪个文件 ## Start here -1. Use this adapter when the request asks to reference, refresh, write, or register project development context. +1. Use this adapter for codebase navigation or when the request asks to + reference, refresh, write, or register project development context. 2. Then follow `.agents/skills/project-dev-context/SKILL.md`. 3. Resolve topics through `agent_context/MAP.yaml`. @@ -32,4 +38,3 @@ Canonical source: `.agents/skills/project-dev-context/` Keep this file thin. If canonical routing behavior changes, update the canonical skill first, then only adjust this adapter if Claude needs a different local entry hint. - diff --git a/.claude/skills/update-api-docs/SKILL.md b/.claude/skills/update-api-docs/SKILL.md new file mode 100644 index 000000000..74fab917a --- /dev/null +++ b/.claude/skills/update-api-docs/SKILL.md @@ -0,0 +1,19 @@ +--- +name: update-api-docs +description: Claude adapter for the canonical EmbodiChain update-api-docs skill. +--- + +# Update API Docs - Claude Adapter + +Canonical source: `.agents/skills/update-api-docs/` + +## When to use + +- the API docs checker or CI reports missing public exports +- a change adds or updates an API declared through `__all__` +- API-reference pages and source descriptions need synchronization + +## Start here + +1. Run the read-only checker to discover missing import paths. +2. Follow `.agents/skills/update-api-docs/SKILL.md` to generate the docs. diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index 1bff3dbd0..a3ab0b57f 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -43,6 +43,7 @@ To upload images to a PR -- simply drag and drop an image while in edit mode and - [ ] I have run the `black .` command to format the code base. - [ ] I have made corresponding changes to the documentation +- [ ] Public API changes are reflected in the API docs (`python docs/scripts/check_api_docs.py`), if applicable - [ ] I have added tests that prove my fix is effective or that my feature works - [ ] Dependencies have been updated, if applicable. diff --git a/.github/copilot/instructions.md b/.github/copilot/instructions.md index 4abcbbb53..a0d6ecdb9 100644 --- a/.github/copilot/instructions.md +++ b/.github/copilot/instructions.md @@ -14,7 +14,7 @@ follow the canonical routing rules in `.agents/skills/project-dev-context/`. - Add atomic actions: `.github/copilot/add-atomic-action.md` - Add robots: `.github/copilot/add-robot.md` - Add tests: `.github/copilot/add-test.md` +- Update public API docs: `.github/copilot/update-api-docs.md` - Run pre-commit checks: `.github/copilot/pre-commit-check.md` - Draft or create pull requests: `.github/copilot/pr.md` - Write benchmarks: `.github/copilot/benchmark.md` - diff --git a/.github/copilot/project-dev-context.md b/.github/copilot/project-dev-context.md index ebfa7ef2c..3dacb149d 100644 --- a/.github/copilot/project-dev-context.md +++ b/.github/copilot/project-dev-context.md @@ -4,6 +4,8 @@ Canonical source: `.agents/skills/project-dev-context/` ## When to use +- locate a file, config, default, entry point, registration path, or change site +- explain a code or configuration resolution chain - reference project development docs - reference project context - refresh project context @@ -15,10 +17,14 @@ Canonical source: `.agents/skills/project-dev-context/` - 刷新项目上下文 - 更新项目上下文 - 写项目上下文 +- 文件在哪里 +- 配置或默认值在哪里 +- 应该修改哪个文件 ## Start here -1. Use this adapter when the request asks to reference, refresh, write, or register project development context. +1. Use this adapter for codebase navigation or when the request asks to + reference, refresh, write, or register project development context. 2. Then follow `.agents/skills/project-dev-context/SKILL.md`. 3. Resolve topics through `agent_context/MAP.yaml`. @@ -27,4 +33,3 @@ Canonical source: `.agents/skills/project-dev-context/` Keep this file thin. If canonical routing behavior changes, update the canonical skill first, then only adjust this adapter if Copilot needs a different local entry hint. - diff --git a/.github/copilot/update-api-docs.md b/.github/copilot/update-api-docs.md new file mode 100644 index 000000000..4a26ffccd --- /dev/null +++ b/.github/copilot/update-api-docs.md @@ -0,0 +1,7 @@ +# Update API Docs for GitHub Copilot + +Canonical source: `.agents/skills/update-api-docs/` + +Use this adapter when the API docs checker reports missing public exports or +when public APIs change. Then follow `.agents/skills/update-api-docs/SKILL.md` +to generate the corresponding Sphinx entries and descriptions. diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 742b31283..260ace159 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -44,6 +44,9 @@ jobs: exit 1 fi + - name: Public API docs check + run: python docs/scripts/check_api_docs.py + build: if: ${{ !startsWith(github.ref, 'refs/tags/v') }} needs: lint diff --git a/AGENTS.md b/AGENTS.md index 0f8a03e8d..2c6b2cac4 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -6,22 +6,36 @@ EmbodiChain keeps agent-facing context in a structured topic registry: - `agent_context/` — agent-readable Markdown context, indexed by `agent_context/MAP.yaml` - `docs/source/` — human-facing Sphinx documentation -- `.agents/skills/project-dev-context/` — the skill that routes "reference project context" requests +- `.agents/skills/project-dev-context/` — the skill that routes project-context and codebase-navigation requests - `.claude/skills/` and `.github/copilot/` — thin tool-specific adapters that point back to `.agents/skills/` When a request says things like: - `reference project development docs` - `reference project context` +- `where is X` +- `where do I change X` +- `配置或默认值在哪里` +- `入口或注册逻辑在哪里` the agent should: 1. Read `agent_context/MAP.yaml` first 2. Resolve the topic by `id`, `aliases`, then `keywords` -3. Load only the matched Markdown files under `agent_context/` -4. Avoid reading `docs/source/` unless the user explicitly asks for the Sphinx documentation - -Available topics: `env-framework`, `manager-functor`, `ik-solvers`, `robot-system`, `sensor-system`, `sim-visualization`, `motion-planning`, `atomic-actions`, `rl-learning`, `configclass-pattern`, `randomization`. +3. For context reads, load only the matched Markdown files under + `agent_context/` +4. For navigation, verify matched facts against the current + `source_of_truth`; if no topic matches, use `rg --files` and `rg -n` + against the current tree +5. Report the owning entry point, resolution path, recommended change site, + and focused validation surface when relevant +6. Avoid reading `docs/source/` unless the user explicitly asks for the + Sphinx documentation + +Available topics: `simulation-system`, `env-framework`, +`manager-functor`, `ik-solvers`, `robot-system`, `sensor-system`, +`sim-visualization`, `motion-planning`, `atomic-actions`, `rl-learning`, +`configclass-pattern`, `randomization`. --- @@ -43,33 +57,33 @@ EmbodiChain/ ├── .claude/ # Claude adapters for canonical skills │ └── skills/ ├── embodichain/ # Main Python package -│ ├── agents/ # AI agents -│ │ ├── hierarchy/ # LLM-based hierarchical agents (task, code, validation) -│ │ ├── mllm/ # Multimodal LLM prompt scaffolding -│ │ └── prompts/ # Agent prompt templates -│ ├── data_pipeline/ # Datasets and online data streaming -│ ├── learning/ # Learning systems: RL, IL, frontier model architectures -│ │ └── rl/ # RL: PPO/GRPO algo, rollout buffer, collectors, policies │ ├── data/ # Assets, datasets, constants, enums +│ ├── data_pipeline/ # Datasets and online data streaming +│ ├── gen_sim/ # Scene Engine and SimReady generation pipelines +│ ├── learning/ # Learning systems +│ │ └── rl/ # RL: PPO/GRPO/APG, buffers, collectors, policies │ ├── lab/ # Simulation lab │ │ ├── visualization/ # Browser visualization protocol, runtime, and Viser backend │ │ ├── gym/ # OpenAI Gym-compatible environments │ │ │ ├── envs/ # BaseEnv, EmbodiedEnv │ │ │ │ ├── managers/ # Observation, event, reward, record, dataset managers │ │ │ │ │ └── randomization/ # Physics, geometry, spatial, visual randomizers -│ │ │ │ ├── tasks/ # Deprecated import shim for official tasks │ │ │ │ ├── action_bank/ # Configurable action primitives │ │ │ │ └── wrapper/ # Env wrappers (e.g. no_fail) │ │ │ └── utils/ # Gym registration, misc helpers │ │ ├── sim/ # Simulation core +│ │ │ ├── atomic_actions/ # Typed planning and execution primitives │ │ │ ├── objects/ # Robot, RigidObject, Articulation, Light, Gizmo, SoftObject │ │ │ ├── sensors/ # Camera, StereoCamera, BaseSensor │ │ │ ├── robots/ # Robot-specific configs and params (dexforce_w1, cobotmagic) │ │ │ ├── planners/ # Motion planners (TOPPRA, motion generator) -│ │ │ └── solvers/ # IK solvers (SRS, OPW, pink, pinocchio, pytorch) +│ │ │ ├── solvers/ # IK solvers (SRS, OPW, pink, pinocchio, pytorch) +│ │ │ ├── skills/ # Semantic scene and robot-skill binding contracts +│ │ │ └── workspace/ # Reachability analysis and runtime workspace queries │ │ ├── devices/ # Real-device controllers -│ │ └── scripts/ # Entry-point scripts (run_env, run_agent) +│ │ └── scripts/ # Environment, preview, and analysis entry points │ ├── toolkits/ # Standalone tools +│ │ ├── acd/ # URDF convex-decomposition CLI │ │ ├── graspkit/pg_grasp/ # Parallel-gripper grasp sampling │ │ └── urdf_assembly/ # URDF builder utilities │ └── utils/ # Shared utilities @@ -82,6 +96,7 @@ EmbodiChain/ │ └── source/ # .md doc pages (overview, quick_start, features, resources) ├── tests/ # Test suite ├── .github/ # CI workflows, issue/PR templates, Copilot adapters +├── pyproject.toml # Distribution metadata and unified CLI entry point ├── setup.py # Package setup └── VERSION # Package version file ``` @@ -192,6 +207,12 @@ __all__ = ["MyClass", "my_function"] ### Documentation - Docs are built with **Sphinx** using **Markdown** source files (`docs/source/`). +- Check public API coverage without modifying files: + ```bash + python docs/scripts/check_api_docs.py + ``` +- Use the `/update-api-docs` skill to generate or update documentation for + missing public exports; the checker itself never writes documentation. - Build locally: ```bash pip install -r docs/requirements.txt @@ -234,7 +255,7 @@ Include: ### Adding a New Robot -Refer to `docs/source/tutorial/add_robot.rst` for a detailed guide. The basic structure requires: +Refer to `docs/source/guides/add_robot.rst` for a detailed guide. The basic structure requires: - A config class (inheriting from `RobotCfg`) - URDF configuration for the robot @@ -271,6 +292,7 @@ Tool-specific adapter files should stay thin and point back to the canonical ski | Add Task Env | `/add-task-env` | Scaffold a new `EmbodiedEnv` task | | Add Functor | `/add-functor` | Scaffold observation/reward/event/action/dataset/randomization functors | | Add Test | `/add-test` | Scaffold tests following project conventions | +| Update API Docs | `/update-api-docs` | Document public exports reported by the read-only API checker | | Pre-Commit Check | `/pre-commit-check` | Run all local CI checks before committing | | Create PR | `/pr` | Create a PR following the project template | | Benchmark | `/benchmark` | Write benchmark scripts for EmbodiChain modules | diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index af1401cf5..af23c24c7 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -30,7 +30,16 @@ We welcome pull requests for bug fixes, new features, and documentation improvem black . ``` > Currently, we use black==26.3.1 for formatting. Make sure to use the same version to avoid inconsistencies. -4. **Submit a Pull Request**. +4. **Check public API documentation coverage**. The checker is read-only and + verifies that exports declared through `__all__` appear in the Sphinx API + reference. + ```bash + python docs/scripts/check_api_docs.py + ``` + If it reports missing exports, update the appropriate API-reference page + and public docstrings. Agent users can invoke `/update-api-docs` to generate + these changes. +5. **Submit a Pull Request**. * Use the [Pull Request Template](.github/PULL_REQUEST_TEMPLATE.md). * Keep PRs small and focused. * Include a summary of the changes and link to any relevant issues (e.g., `Fixes #123`). @@ -39,113 +48,52 @@ We welcome pull requests for bug fixes, new features, and documentation improvem ## Contribute specific robots -To contribute a new robot, please check the documentation on [Adding a New Robot](https://dexforce.github.io/EmbodiChain/guides/add_robot.html). +To contribute a new robot, please check the documentation on [Adding a New Robot](https://dexforce.github.io/EmbodiChain/main/guides/add_robot.html). ## Contribute specific environments -To contribute a new environment, please check the documentation on [Embodied Environments](https://dexforce.github.io/EmbodiChain/overview/gym/env.html) and see the tutorial below: -- [Creating a Basic Environment](https://dexforce.github.io/EmbodiChain/tutorial/basic_env.html) -- [Creating a Modular Environment](https://dexforce.github.io/EmbodiChain/tutorial/modular_env.html) +To contribute a new environment, please check the documentation on [Embodied Environments](https://dexforce.github.io/EmbodiChain/main/overview/gym/env.html) and see the tutorial below: +- [Creating a Basic Environment](https://dexforce.github.io/EmbodiChain/main/tutorial/basic_env.html) +- [Creating a Modular Environment](https://dexforce.github.io/EmbodiChain/main/tutorial/modular_env.html) If you want to implement your tasks in a new repo and with some customized functors and utilities, you can also use the [Task Template Repo](https://github.com/DexForce/embodichain_task_template). -## Using Claude Code for Contributions - -
-Setup, skills, and tips for using Claude Code +## Using AI Coding Agents for Contributions -[Claude Code](https://docs.anthropic.com/en/docs/claude-code/overview) is an AI-powered CLI that can assist you throughout the contribution workflow — from understanding the codebase to writing, reviewing, and debugging code. +EmbodiChain supports both [OpenAI Codex](https://developers.openai.com/codex/cli) and [Claude Code](https://docs.anthropic.com/en/docs/claude-code/getting-started). Either agent can help explore the codebase, implement focused changes, write tests, review diffs, and prepare pull requests. ### Setup -Install Claude Code and authenticate: - -```bash -npm install -g @anthropic-ai/claude-code -claude -``` - -A `CLAUDE.md` file is present at the root of this repository. Claude Code reads it automatically at startup to load project conventions, structure, and style rules, so it is context-aware from the first prompt. - -### Skills - -Claude Code skills are built-in slash commands that automate common development tasks. They scaffold code, run checks, and enforce project conventions so you can focus on your logic instead of boilerplate. Invoke any skill by typing its command in the Claude Code prompt. - -| Skill | Command | Purpose | -|-------|---------|---------| -| Add Functor | `/add-functor` | Scaffold a new observation, reward, event, action, dataset, or randomization functor with the correct signature, style, and module placement | -| Add Task Env | `/add-task-env` | Scaffold a new task environment with the correct file structure, `@register_env` decorator, base class, and test stub | -| Add Test | `/add-test` | Scaffold tests with the correct file placement, style (pytest vs class), mock patterns, and project conventions | -| Pre-Commit Check | `/pre-commit-check` | Run all local CI checks — code style, headers, annotations, exports, and docstrings — before committing | -| Create PR | `/pr` | Create a pull request following the project template and label conventions | -| Benchmark | `/benchmark` | Write benchmark scripts for measuring performance of solvers, samplers, and other computationally intensive components | - -#### When to use each skill - -**`/add-functor`** — Use when adding a new observation, event, reward, action, dataset, or randomization functor to an EmbodiChain environment. The skill will ask for the functor type and name, then generate the function- or class-style implementation with proper docstrings, type hints, and `__all__` exports. - -**`/add-task-env`** — Use when creating a new task environment, including expert demonstration tasks, RL tasks, or any `EmbodiedEnv` subclass. The skill scaffolds the task file with `_setup_scene`, `_reset_idx`, and evaluation logic, plus a test stub. - -**`/add-test`** — Use when writing tests for any EmbodiChain module — functors, solvers, sensors, environments, or utilities. The skill determines the correct test file location, style (pytest function vs class), and generates tests with the standard Apache 2.0 header and named constants. - -**`/pre-commit-check`** — Run this before committing or creating a PR. It verifies code formatting (`black`), file headers, type annotations, `__all__` exports, and docstring completeness — the same checks the CI pipeline enforces. - -**`/pr`** — Use after committing your changes to create a pull request. The skill checks git state, determines the PR type, drafts a description following the project template, runs formatting, creates a feature branch, and opens the PR via `gh` CLI with the correct labels. - -**`/benchmark`** — Use when you need to measure the performance of a module (IK solvers, grasp samplers, metrics, etc.). The skill generates a well-structured benchmark script following project conventions. - -### Suggested workflows +Follow the official setup guide for your preferred agent, then start it from the repository root: -**Explore the codebase before making changes** +| Agent | Setup guide | Start command | +|-------|-------------|---------------| +| OpenAI Codex | [Codex CLI](https://developers.openai.com/codex/cli) | `codex` | +| Claude Code | [Claude Code setup](https://docs.anthropic.com/en/docs/claude-code/getting-started) | `claude` | -``` -> Explain how the Functor/Manager pattern works in embodichain/lab/gym/envs/managers/ -> How does the Action Manager work with EmbodiedEnv for RL tasks? -> Show me an example of how a randomization functor is registered in a task config. -``` - -**Implement a new feature** +The repository uses [`AGENTS.md`](AGENTS.md) as the canonical source for project structure, conventions, and contribution instructions. Codex reads it directly; Claude Code reads [`CLAUDE.md`](CLAUDE.md), which imports the same instructions. Run either agent from the repository root so it can discover these files and the project skills. -``` -> I want to add a new observation functor that returns the end-effector velocity. - Which existing functor should I model it after? -> /add-functor -``` +### Agent development context -**Validate style and formatting before submitting** +Agent-facing development context lives in [`agent_context/`](agent_context/). The registry at [`agent_context/MAP.yaml`](agent_context/MAP.yaml) maps topic IDs, aliases, and keywords to focused Markdown files. When a task depends on project internals, ask the agent to reference the relevant project context before making changes, for example: +```text +Reference the project context for manager-functor before implementing this change. ``` -> Review my changes in embodichain/lab/gym/envs/managers/randomization/visual.py - for style issues, missing type hints, and docstring completeness. -> /pre-commit-check -``` - -**Write or update tests** -``` -> /add-test -``` +Both agents are instructed to read `agent_context/MAP.yaml` first, resolve the requested topic, and load only the matching context files. For codebase-navigation questions they also verify mapped paths against the current source tree and fall back to live search when no topic matches. The files under `docs/source/` remain the human-facing Sphinx documentation and should be consulted only when explicitly requested. -**Understand a bug** - -``` -> I'm getting a KeyError in observation_manager.py at line 42 when env_ids is None. - What could cause this and how should it be fixed? -``` - -**Create a pull request** - -After you've made your changes and committed them: - -``` -> /pr -``` +### Shared project skills -The `/pr` skill will guide you through checking git state, determining the PR type, drafting a description, running formatting, and creating the PR with proper labels. +Canonical skills live in [`.agents/skills/`](.agents/skills/). Claude Code uses thin adapters under [`.claude/skills/`](.claude/skills/) that point back to the same instructions, so both agents follow a consistent workflow. Ask the agent to use the relevant skill by name; common examples include: -### Tips +| Skill | Purpose | +|-------|---------| +| `/project-dev-context` | Navigate, resolve, or update agent development context | +| `/add-functor`, `/add-task-env`, `/add-robot`, `/add-solver`, `/add-atomic-action` | Scaffold project components following repository conventions | +| `/add-test`, `/benchmark` | Add validation or performance benchmarks | +| `/update-api-docs` | Generate API-reference entries and descriptions for missing public exports | +| `/pre-commit-check` | Run proportional checks before committing | +| `/pr`, `/release` | Prepare a pull request or release | -* Always run `/pre-commit-check` after making changes — it catches the same issues the CI pipeline checks. -* Claude Code respects the `CLAUDE.md` conventions. If you notice it deviating (wrong docstring style, missing `__all__`, etc.), point it out and it will correct the output. -* For large features, break the work into small, focused tasks and handle them one at a time using the appropriate skill for each step. -* If you add a new skill to `.claude/skills/`, make sure to also add it to the Skills table and "When to use each skill" list in this document so contributors can discover it. \ No newline at end of file +Review all agent-generated changes, run the relevant tests, and use `/pre-commit-check` before submitting a pull request. diff --git a/README.md b/README.md index c6fff2784..3a40a0582 100644 --- a/README.md +++ b/README.md @@ -66,11 +66,11 @@ for details. ## Contribution Guide -We welcome contributions! Please see the [CONTRIBUTING.md](CONTRIBUTING.md) file in this repository for guidelines on how to get started. +We welcome contributions! Please see the [CONTRIBUTING.md](https://github.com/DexForce/EmbodiChain/blob/main/CONTRIBUTING.md) file in this repository for guidelines on how to get started. ## Publications -See [Academic Publications](docs/source/resources/publications/README.md) for a complete list of academic papers related to EmbodiChain. +See [Academic Publications](https://dexforce.github.io/EmbodiChain/main/resources/publications/README.html) for a complete list of academic papers related to EmbodiChain. ## Citation diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 60a5145e0..1374d7740 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -5,6 +5,61 @@ defaults: - conventions/naming.md - conventions/topic-lifecycle.md topics: + - id: simulation-system + title: Simulation System + aliases: + - simulation system + - simulation core + - sim system + - lab sim + - simulation manager + - 仿真系统 + - 仿真核心 + - 仿真管理器 + keywords: + - sim + - simulation + - SimulationManager + - SimulationManagerCfg + - DexSim + - world + - arena + - physics step + - sim update + - scene object + - asset registry + - manual update + - GPU physics + - simulation lifecycle + paths: + - topics/simulation-system/simulation-system.md + source_of_truth: + - embodichain/lab/sim/__init__.py + - embodichain/lab/sim/sim_manager.py + - embodichain/lab/sim/cfg.py + - embodichain/lab/sim/common.py + - embodichain/lab/sim/material.py + - embodichain/lab/sim/profiler.py + - embodichain/lab/sim/objects/__init__.py + - embodichain/lab/sim/sensors/__init__.py + - embodichain/lab/sim/solvers/__init__.py + - embodichain/lab/sim/planners/__init__.py + - embodichain/lab/sim/skills/__init__.py + - embodichain/lab/sim/workspace/__init__.py + - embodichain/lab/gym/envs/base_env.py + - embodichain/lab/gym/envs/embodied_env.py + related_topics: + - env-framework + - robot-system + - sensor-system + - sim-visualization + - ik-solvers + - motion-planning + - atomic-actions + - rl-learning + - configclass-pattern + status: active + - id: env-framework title: Environment Framework aliases: @@ -47,6 +102,7 @@ topics: - embodichain_tasks/embodichain_tasks/ - embodichain/lab/gym/utils/ related_topics: + - simulation-system - manager-functor - sim-visualization status: active @@ -143,6 +199,7 @@ topics: - embodichain/lab/sim/solvers/pytorch_solver.py - embodichain/lab/sim/solvers/differential_solver.py related_topics: + - simulation-system - robot-system - motion-planning status: active @@ -179,6 +236,7 @@ topics: - embodichain/lab/sim/robots/ - embodichain/lab/sim/cfg.py related_topics: + - simulation-system - ik-solvers - motion-planning - sensor-system @@ -215,6 +273,7 @@ topics: - embodichain/lab/sim/sensors/stereo.py - embodichain/lab/sim/sensors/contact_sensor.py related_topics: + - simulation-system - robot-system - env-framework - sim-visualization @@ -236,7 +295,6 @@ topics: keywords: - visualization - viser - - SimulationManager - VisualizationCfg - ViserServerCfg - VisualizationRuntime @@ -266,6 +324,7 @@ topics: - embodichain/lab/sim/objects/soft_object.py - embodichain/lab/sim/objects/cloth_object.py related_topics: + - simulation-system - env-framework - sensor-system - robot-system @@ -280,6 +339,8 @@ topics: - toppra - motion generator - trajectory planning + - curobo collision world + - dynamic collision integration - 运动生成 - 运动规划 - 轨迹规划 @@ -291,11 +352,26 @@ topics: - motion generator - resolve_plan_options - MotionGenOptions.strategy + - MotionGenOptions.interpolation_dt + - PlanResult.dt + - explicit trajectory timing - ik_interp - path - waypoint - velocity - acceleration + - canonical obstacle ID + - logical collision source ID + - physical YAML obstacle name + - sphere derived obstacle names + - empty collision mesh + - CollisionWorldInfo + - collision_world_info + - dynamic_collision_entity_ids + - collision_world_entity_ids + - collision_world_batch_mode + - collision_geometry_by_id + - make_planning_scene_provider paths: - topics/motion-planning/motion-planning.md source_of_truth: @@ -304,9 +380,14 @@ topics: - embodichain/lab/sim/planners/curobo/curobo_planner.py - embodichain/lab/sim/planners/curobo/curobo_yaml.py - embodichain/lab/sim/planners/motion_generator.py + - embodichain/lab/sim/planners/neural_planner.py + - embodichain/lab/sim/planners/utils.py + - embodichain/lab/sim/skills/scene.py related_topics: + - simulation-system - robot-system - ik-solvers + - atomic-actions status: active - id: rl-learning @@ -315,6 +396,10 @@ topics: - rl learning - learning rl - rl training + - rl pipeline + - rl config + - train rl + - train-rl - reinforcement learning - ppo - apg @@ -325,6 +410,7 @@ topics: - rollout buffer - 强化学习 - RL训练 + - 强化学习训练 keywords: - rl - reinforcement @@ -340,23 +426,42 @@ topics: - collector - policy - reward + - RolloutKind + - DifferentiableTrainer + - SyncCollector + - learning_env + - gym_config + - checkpoint + - evaluation + - distributed paths: - topics/rl-learning/rl-learning.md source_of_truth: + - embodichain/__main__.py + - embodichain/learning/rl/train.py - embodichain/learning/rl/env.py - embodichain/learning/rl/evaluation.py - embodichain/learning/rl/routing.py - embodichain/learning/rl/differentiable_trainer.py - - embodichain/learning/rl/experimental/newton/ - - embodichain/learning/rl/train.py + - embodichain/learning/rl/utils/config.py + - embodichain/learning/rl/utils/helper.py + - embodichain/learning/rl/utils/optimizer.py + - embodichain/learning/rl/utils/trainer.py - embodichain/learning/rl/algo/ - embodichain/learning/rl/buffer/ - embodichain/learning/rl/models/ - embodichain/learning/rl/collector/ - - embodichain_tasks/embodichain_tasks/rl/basic/point_mass.py + - embodichain/learning/rl/experimental/newton/ + - embodichain/lab/gym/utils/gym_utils.py + - embodichain/lab/gym/utils/registration.py + - embodichain/utils/utility.py + - embodichain_tasks/configs/agents/rl/ + - embodichain_tasks/embodichain_tasks/rl/ related_topics: + - simulation-system - env-framework - manager-functor + - configclass-pattern status: active - id: configclass-pattern @@ -382,6 +487,7 @@ topics: source_of_truth: - embodichain/utils/configclass.py related_topics: + - simulation-system - env-framework - manager-functor - robot-system @@ -427,6 +533,19 @@ topics: - atomic actions - motion primitive - action primitive + - object semantics + - scene grounding + - scene registry + - semantic scene + - robot skill profile + - resource graph + - resource DAG + - semantic skill catalog + - semantic skill runtime + - expert program + - declarative expert program + - atomic demo bridge + - capability binding - AtomicAction - ActionInvocation - AtomicActionEngine @@ -444,6 +563,30 @@ topics: - PlanningContext - ExecutionSession - EffectVerificationRequest + - EffectVerificationResult + - attempt_generation + - SemanticEffectSpec + - EffectMonitorRef + - EffectMonitorRegistry + - EffectMonitorDecision + - PoseRelationEvidenceBatch + - relation hysteresis + - SkillRuntime + - SkillResult + - AtomicSkills + - SemanticCallSpec + - SemanticSkillCompiler + - ExpertProgramCfg + - ExpertProgramCompiler + - AtomicDemoBridge + - BufferedGymCommandSink + - ControlCommandStateEvidenceTracker + - DynamicSettleMonitor + - ParallelSkillRuntime + - program segment metadata + - eligible_mask + - deactivate_rows + - effect verification deadline - ExecutionRunner - ObservationProvider - CommandSink @@ -451,12 +594,86 @@ topics: - SceneSnapshotSupplier - SceneProvider - RigidObjectSceneProvider + - SceneRegistry + - RegistrySceneProvider + - SceneEntityRef + - SceneObjectRef + - SceneArticulationRef + - SceneLinkRef + - SceneAffordanceRef + - SceneEntityRegistration + - SceneCollisionRole + - SceneCollisionWorldMode + - from_simulation + - validate_collision_integration + - make_planning_scene_provider + - collision_geometry_by_id + - collision_world_entity_ids + - canonical scene ID + - logical collision source ID + - sphere derived obstacle names + - empty collision mesh + - flat entity namespace + - native_name + - parent native source identity + - ObjectSemantics + - entity_id + - frozen ObjectSemantics + - legacy uid + - stable entity identity + - snapshot grounding + - AssembleGoal + - AssembleAffordance + - base_pose + - _scene_dependencies - collision world revision - dynamic obstacle - StateDelta - held_objects + - HeldObjectState - ActionBinding + - EndpointBinding + - RuntimeEndpointTarget + - JointPositionTarget + - SkillBindingContract + - SkillResourceSlot + - SkillEndpointRequirement + - DisjointSlotEndpoints + - DisjointResourceSlots + - RobotSkillProfile + - BoundRobotSkillProfile + - RobotResource + - ResourceEndpoint + - ResourceEndpointAdapter + - ControlPartEndpoint + - ControlPartEndpointAdapter + - EndpointResolution + - ResolvedResourceEndpoint + - ResourceBinding + - ResourceClaim + - ResolvedRobotResource + - ResolvedSkillBinding + - SkillPolicyPreset + - effect_monitors + - semantic effect monitor + - binding_contract + - engine.skills + - skill_profile + - command_profiles + - action_control_profiles + - endpoint_adapters + - endpoint snapshot + - requires_command_profile + - claim_tokens + - capability + - whole body resource + - leaf resource claim - SceneEntityPose + - PressGoal.target_pose + - SlideGoal.target_pose + - TwistGoal.target_pose + - open_loop + - axis_translation_keyframes - dynamic goal - error recovery - ActionOptions @@ -464,9 +681,26 @@ topics: - ControlPartCommandProfile - ActionControlOverrides - JointPositionCommand + - RuntimeCommandPayload + - JointPositionPayload + - EndpointCommand + - RuntimeCommandFrame + - TimedCommandSequence + - EndpointCommandTransport + - EndpointCommandRouter + - endpoint transport + - transport_id + - target_id + - safe stop + - cancel then hold + - ActionPlan.commands + - joint_trajectory - invocation revision - MotionPolicy - MotionPolicy.strategy + - PlanningContext.control_dt + - explicit trajectory timing + - TimedTrajectory.from_uniform_step - RecoveryPolicy - MotionGenOptions.strategy - motion_gen @@ -474,22 +708,29 @@ topics: - trajectory_ops - build_pose_plan_states - build_joint_plan_states - - register_action + - engine.register + - engine.make_invocation - BUILTIN_ACTION_TYPES - load_builtins - engine.plan - engine.compile - engine.start + - eligible_mask paths: - topics/atomic-actions/atomic-actions.md source_of_truth: - embodichain/lab/sim/atomic_actions/core.py - embodichain/lab/sim/atomic_actions/goals.py + - embodichain/lab/sim/atomic_actions/effects.py + - embodichain/lab/sim/atomic_actions/affordance.py - embodichain/lab/sim/atomic_actions/bindings.py - embodichain/lab/sim/atomic_actions/control.py - embodichain/lab/sim/atomic_actions/invocation.py - embodichain/lab/sim/atomic_actions/policies.py + - embodichain/lab/sim/atomic_actions/requirements.py - embodichain/lab/sim/atomic_actions/runtime.py + - embodichain/lab/sim/atomic_actions/runtime_commands.py + - embodichain/lab/sim/atomic_actions/transports.py - embodichain/lab/sim/atomic_actions/state.py - embodichain/lab/sim/atomic_actions/plans.py - embodichain/lab/sim/atomic_actions/execution.py @@ -500,7 +741,25 @@ topics: - embodichain/lab/sim/atomic_actions/trajectory_ops.py - embodichain/lab/sim/atomic_actions/primitives/ - embodichain/lab/sim/atomic_actions/__init__.py + - embodichain/lab/sim/skills/scene.py + - embodichain/lab/sim/skills/calls.py + - embodichain/lab/sim/skills/compiler.py + - embodichain/lab/sim/skills/effects.py + - embodichain/lab/sim/skills/evidence.py + - embodichain/lab/sim/skills/integration.py + - embodichain/lab/sim/skills/runtime.py + - embodichain/lab/sim/skills/parallel.py + - embodichain/lab/sim/skills/parallel_runtime.py + - embodichain/lab/sim/skills/profiles.py + - embodichain/lab/sim/skills/calls.py + - embodichain/lab/sim/skills/integration.py + - embodichain/lab/sim/skills/compiler.py + - embodichain/lab/sim/skills/runtime.py + - embodichain/lab/sim/skills/__init__.py + - embodichain/lab/gym/envs/expert_program/ + - embodichain/lab/gym/envs/settling.py related_topics: + - simulation-system - motion-planning - robot-system - ik-solvers diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 112d2596b..0ab5b6994 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -13,41 +13,54 @@ There is no `ActionTarget`, `WorldState`, `ActionResult`, `execute()`, or `ActionInvocation` separates: -- an action-owned typed goal (`goal_kind` is its stable discriminator); -- `ActionBinding`, which maps semantic roles to names from the engine robot's - `control_parts` mapping; -- reusable `MotionPolicy` planner/timing choices; +- an action-owned typed goal, validated against the action's `GoalType`; +- an engine-owned `ActionBinding`, which covers the skill contract by exact + `(slot_id, endpoint_id)` keys and terminates every endpoint at an immutable + `RuntimeEndpointTarget`; +- reusable `MotionPolicy` strategy, sampling, collision, and backend options; - bounded `RecoveryPolicy` thresholds and retry budgets; -- optional typed `skill_options` and role-scoped `control_overrides` for one - invocation revision. +- optional typed `skill_options` and endpoint-scoped `control_overrides` for + one invocation revision. `PlanningContext` separates measured `RobotObservation`, verified symbolic -`TaskState`, versioned `SceneSnapshot`, and environment IDs. An `ActionPlan` -contains per-environment planning success, one full-robot `TimedTrajectory`, -action-level recovery and scene-invalidation metadata, planner diagnostics, -named `TrajectorySegment` ranges, and an uncommitted `StateDelta`. Segments are -inspection/tracing metadata inside one trajectory; they are not independently -replannable execution boundaries. - -`AtomicAction.build_plan()` normalizes the success mask and freezes unsuccessful -trajectory rows at the context's observed qpos; skill implementations should -return row-local success instead of duplicating failure-row masking. +`TaskState`, versioned `SceneSnapshot`, environment IDs, and an optional +explicit `control_dt` used only by action-owned interpolation. An `ActionPlan` +contains per-environment planning success, an authoritative +`TimedCommandSequence` in `commands`, an optional full-robot `TimedTrajectory` +in `joint_trajectory`, action-level recovery and scene-invalidation metadata, +planner diagnostics, named `TrajectorySegment` frame ranges, and an uncommitted +`StateDelta`. Segments are inspection/tracing metadata inside one command +sequence; they are not independently replannable execution boundaries. + +`AtomicAction.build_plan()` is the planner-backed joint convenience path: it +normalizes the success mask, freezes unsuccessful trajectory rows at the +context's observed qpos, and lowers the trajectory through bound +`JointPositionTarget` values. `AtomicAction.build_command_plan()` is the generic +extension boundary for transport-neutral command sequences. Both mask failed +rows; skill implementations should return row-local success instead of +duplicating that work. Use `plan.segment(name)` for action-local half-open ranges and `compiled.segment(action_index, name)` for concatenated coordinates; do not recompute private sample splits in callers. Each `AtomicActionEngine` exclusively owns one `ActionPlanningServices` instance, which contains its robot, one `MotionGenerator`/planner backend, and -control-part command profiles. `MotionGenerator.generate()` is the only -stateful motion-planning entry point. `MotionPolicy.to_motion_gen_options()` -passes the invocation's `strategy` directly into `MotionGenOptions`; it is either -`"motion_gen"` or `"ik_interp"`. Target shaping, world-frame pose translation, -hand/joint interpolation used by composite actions, and full-robot trajectory -embedding are pure functions in `trajectory_ops.py`. Actions retain only an -owned copy of typed default options and borrow engine services. Engine -construction creates and binds a fresh instance of every type in -`BUILTIN_ACTION_TYPES`; use `load_builtins=False` only for isolated tests or a -fully custom action set. A bound action cannot be reused by another engine. +its direct control-part command-profile snapshot. It also issues an opaque +binding-owner ID, so an `ActionBinding` cannot cross engine instances. It does +not own a timing fallback. Planner results with positions require explicit `dt`; +`duration` is derived from it. Actions must pass a complete `TimedTrajectory` to +`build_plan()`. Environment-backed integrations put `BaseEnv.step_dt` on +`PlanningContext.control_dt` when action-owned interpolation needs a cadence. +`MotionGenerator.generate()` is the only stateful motion-planning entry point. +`MotionPolicy.to_motion_gen_options()` passes the invocation's `strategy` +directly into `MotionGenOptions`; it is either `"motion_gen"` or `"ik_interp"`. +Target shaping, world-frame pose translation, hand/joint interpolation used by +composite actions, and full-robot trajectory embedding are pure functions in +`trajectory_ops.py`. Actions retain only an owned copy of typed default options +and borrow engine services. Engine construction creates and binds a fresh +instance of every type in `BUILTIN_ACTION_TYPES`; use `load_builtins=False` only +for isolated tests or a fully custom action set. A bound action cannot be +reused by another engine. ## Engine entry points @@ -57,7 +70,7 @@ Choose the public engine entry point by lifecycle, not by skill type: |---|---|---| | `engine.plan(invocation, context)` | Inspect or plan one registered action | Returns one `ActionPlan`; does not project a context for another action | | `engine.compile(invocations, context)` | Plan an ordered sequence against a fixed scene | Returns a concatenated `CompiledTrajectory`; propagates hypothetical qpos and expected effects through `projected_context` | -| `engine.start(invocations, context)` | Execute incrementally from observations | Returns an `ExecutionSession`; `tick(latest_context)` emits commands and performs bounded recovery | +| `engine.start(invocations, context, *, eligible_mask=None)` | Execute incrementally from observations | Returns an `ExecutionSession`; the optional initial cohort is sticky, and `tick(latest_context)` emits commands and performs bounded recovery | None steps simulation directly. `compile()` never observes physical execution; split compilation at observation boundaries when later goals depend on measured @@ -68,8 +81,359 @@ must remain active during execution. called by the engine, not a fourth application entry point. It binds collision entities from the current scene into a copied motion policy before delegating to the skill-specific `_plan()` hook. New actions implement `_plan()` and must -not override `plan()`. `engine.plan_action(...)` is only an extension/testing -escape hatch for an unregistered instance. +not override `plan()`. Custom actions must be installed with +`engine.register()` before using the same public entry points. + +The `_plan()` extension boundary is an intentional hard break with no legacy +adapter. A subclass that defines `plan()` raises `TypeError` at class definition; +migrate an older custom action by renaming that implementation to `_plan()`. + +## Robot skill profiles and resource binding + +`embodichain.lab.sim.skills.RobotSkillProfile` is the authoritative +embodiment-level catalog for semantic resource binding. Its resource model is a +generic DAG, not a fixed arm/tool schema: + +- `RobotResource.resource_id` is a stable logical ID. `endpoints` maps + skill-local endpoint protocol names such as `motion` or `grasp` to + `ResourceEndpoint` values, and `members` declares physical composition. +- `members` determines transitive claim closure only. It does not inherit or + synthesize endpoint capabilities. A whole-body composite must declare its own + whole-body capability and endpoint explicitly. +- `ResourceEndpoint` is the extension boundary for controller kinds. An exact + endpoint-type `ResourceEndpointAdapter` resolves each declaration against the + engine into an `EndpointResolution`: a `RuntimeEndpointTarget`, an optional + generic command-profile key, joint IDs, adapter-defined claim tokens, and + exclusivity. `ControlPartEndpointAdapter` is installed by default for + `ControlPartEndpoint` and produces a `JointPositionTarget`. Integrations pass + additional `endpoint_adapters` to profile or engine binding for mobile bases, + whole-body controllers, or other endpoint kinds. Registration is by exact + endpoint type, and the built-in adapter cannot be overridden; distinct + controller semantics use a distinct endpoint subtype. +- Resources, profiles, and resolved bindings own independent endpoint + snapshots. A custom endpoint whose nested payload cannot be deep-copied must + override `snapshot()` and return a new value of its exact type. +- Binding snapshots adapter output as a `ResolvedResourceEndpoint`, including + its resolved commands and claims. An exclusive resolution must declare at + least one joint ID or claim token; a deliberately non-exclusive endpoint may + omit both. +- A leaf must expose at least one endpoint. Member references must exist and the + graph must be acyclic. On engine binding, physical leaves must own disjoint + robot joints and adapter claim tokens; a composite endpoint may control only + joints already covered by its transitive members. + +Skills own the robot-independent side of the contract. A concrete +`AtomicAction` opts into semantic discovery by declaring a +`SkillBindingContract` in its own class body. The contract contains +skill-local `SkillResourceSlot` values; every slot requires named +`SkillEndpointRequirement` values with all-of capabilities, optional typed +semantic commands, and no fixed arm/tool role or route layer. Selecting one +resource per slot keeps related endpoints together, so a participant cannot +silently combine endpoint views from unrelated resources. Endpoint views within +that resource may overlap by default, which permits an arm, mobile base, and +whole-body view to describe the same physical system. Add +`DisjointSlotEndpoints` to a slot only when selected endpoint views must be +physically disjoint. `DisjointResourceSlots` separately expresses pairwise +claim separation between selected participant resources. + +Profile binding lowers every selected endpoint directly into an +`EndpointBinding`. Its `target` supplies immutable runtime addressing +(`transport_id`, `target_id`); its semantic commands, capabilities, and claim +tokens remain attached to the same endpoint. `BoundRobotSkillProfile.resolve()` +returns a `ResolvedSkillBinding` that retains the selected logical resources, +the engine-owned `ActionBinding`, each resource's resolved endpoint data, and +one combined `ResourceClaim`. + +Advanced callers without a profile use +`engine.bind_control_parts(skill, endpoints)` with an exact nested +`slot -> endpoint -> control_part` mapping. The engine accepts an installed +skill ID or an explicit action instance later passed to `plan_action()`, checks +contract coverage, control-part existence, required commands, ownership, and +disjointness, then emits the same generic `ActionBinding` with +`JointPositionTarget` endpoints. Callers do not construct bindings manually, +and this path deliberately does not perform profile resource discovery or +capability matching. + +`engine.make_invocation(skill_id, goal, ...)` is the convenience construction +boundary when callers do not need to retain a binding separately. Pass +`control_parts` for the direct path, or rely on a bound `RobotSkillProfile` and +optionally pass `resources` as `slot -> resource_id` selections. The two binding +sources are mutually exclusive. Without a profile, `control_parts` is required; +with a profile, omitting `resources` uses unique or configured-default profile +resolution. The method returns an ordinary `ActionInvocation` and does not plan +or execute it. It resolves bindings only; profile policy presets and runner +configuration remain semantic-runtime concerns. + +Discovery boundaries are distinct: + +- `engine.actions` contains every installed action instance and is the + direct-core registry. +- `engine.skills` contains descriptors only for installed, `agent_visible` + actions whose concrete class explicitly declares a binding contract. A + subclass does not inherit semantic exposure implicitly. +- `engine.skill_profile.skills` filters `engine.skills` again to contracts with + at least one valid assignment on the bound robot. Registering or replacing an + action invalidates the engine's bound profile; an independently retained + `BoundRobotSkillProfile` also rejects use after the engine skill catalog + changes and must be rebound. + +Binding and policy authority is split deliberately: + +- the action class owns its slot/endpoint/command requirement contract; +- the `RobotSkillProfile` owns the resource DAG, capability declarations, + complete per-skill default `ResourceBinding` values, semantic command + profiles keyed by generic profile IDs, and named `SkillPolicyPreset` + snapshots that also select exact semantic-effect monitors; endpoint + declarations or adapters select those profile IDs; +- the bound robot owns actual control-part membership and joint IDs, and its + configured solver is checked for known solver-backed capabilities; +- endpoint adapters own controller-specific validation, physical claims, and + immutable runtime-target lowering; +- runtime payload types own immutable command values, while + `EndpointCommandTransport` implementations own live controller/client state + and execute only payloads whose `transport_id` matches their targets; +- the engine owns installed actions, one planner backend, its binding identity, + and direct control-part command-profile snapshots. + +Constructing `AtomicActionEngine(..., skill_profile=profile)` makes the +profile's generic `command_profiles` the single authoritative constructor +source; passing `control_profiles` at the same time is rejected. +`command_profiles` values currently use `ControlPartCommandProfile` as their +immutable command container, but their mapping keys are generic profile IDs +rather than necessarily being control-part names. +`ControlPartEndpointAdapter` plus `RobotSkillProfile.action_control_profiles()` +provides the direct control-part lookup used by built-in joint planners when an +engine is constructed from a profile; it is not a binding route. Binding a +profile to an already constructed engine instead requires equivalent direct +control-part commands to have been installed already. Profile resolution still +places all resolved semantic commands, including commands for custom endpoint +types, on their `EndpointBinding`. A profile `JointPositionCommand` is +one-dimensional and sized to the adapter-resolved endpoint joint IDs; +invocation `ActionControlOverrides` remain the authority for one revision's +per-environment endpoint-command replacements. + +Resolution selects a sole valid assignment automatically. If several remain, +it uses only a complete, currently valid per-skill default or enough explicit +slot selections; partial defaults and mapping/lexical order never disambiguate. +Preset lookup order is explicit preset, per-skill preset, then profile default, +and every returned preset is an owned snapshot. Planner-pinned presets must +match the engine's configured planner. + +`ResourceClaim` contains transitive leaf-resource IDs, sorted concrete joint +IDs, and adapter-defined `claim_tokens`. Claims conflict when any category +overlaps, so a `whole_body` composite conflicts with a contained arm even when +their endpoint or control-part names differ. This is deterministic conflict +metadata only: a `ResourceClaim` by itself is not a resource lease manager, +parallel scheduler, or concurrency guarantee. The separate explicit +`ParallelSkillRuntime` described below coordinates analyzed branch lanes and +still requires an authoritative safety validator. Dynamic execution can +dispatch multiple endpoint commands in one synchronized frame, but that alone +does not imply resource scheduling or safe parallelism. A custom mobile/base or whole-body endpoint is +executable only when its adapter supplies a target, the action emits a matching +runtime payload, and the target's transport is registered with the +`EndpointCommandRouter`. Successful binding or a non-conflicting claim alone is +not proof that a planner/controller path or safe concurrent execution exists. + +## Object identity and pose grounding + +`ObjectSemantics.entity_id` is the typed core's canonical snapshot-key lowering +target. The registry-backed path obtains it from a resolved `SceneEntityRef`. +It remains optional for advanced direct-core compatibility but, when supplied, +must be a non-empty string. Pose grounding with an explicit ID is strict: +resolve it only from the current `PlanningContext.scene`; a missing snapshot +entry is an error and never falls back to the live `entity`. Only when no ID is +supplied may the core read `ObjectSemantics.entity`; that path emits +`DeprecationWarning`, reads live state, and cannot declare a scene-motion +dependency. + +`ObjectSemantics` is shallow-frozen. Top-level fields such as `entity_id`, +`entity`, and `label` cannot be rebound after construction; create a new +semantics value to change identity. Nested affordance and metadata objects may +remain mutable, but they never establish identity. + +`SceneSnapshot` owns copies of input entity states and returns a defensive +`EntityState`/pose copy on every public mapping lookup. Mutating an input tensor +or a previously returned pose cannot change the published snapshot. Publish a +new scene version for every material dynamic-state change. + +## Scene registry integration + +`embodichain.lab.sim.skills.SceneRegistry` is the canonical integration catalog. +It owns immutable registration metadata: typed identity, aliases, pose source, +parent relationships, backend-local names, dynamics, geometry, collision role, +semantic type, and affordance data. A `SceneSnapshot` does not duplicate that +catalog; it contains only versioned dynamic pose/confidence and collision +revision state. + +All object, articulation, link, and affordance IDs occupy one flat globally +unique namespace. Store link/affordance ancestry in +`SceneEntityRegistration.parent`, not by nesting or qualifying the ID. String +lookups may resolve aliases once to a canonical typed reference. An already +typed ref must contain a canonical ID and match the registered ref class. +Duplicate IDs, ambiguous aliases, alias/canonical collisions, missing parents, +and type mismatches fail at construction or lookup. Within one reference type, +the same `(parent, native_name)` physical source cannot be assigned multiple +canonical IDs; the same local name remains valid under different parents or +for different reference types. + +`SceneRegistry.from_simulation()` is explicit opt-in. Its `rigid_objects` and +`articulations` mappings are `registry_id -> simulation_uid`; selected UIDs are +installed as aliases, and unlisted simulation entities are never scanned. +Collision participation defaults to `NONE`, and every static/dynamic collision +registration requires a geometry provider. + +`SceneEntityMetadata` is the single provider-free scene declaration model. +`SceneEntityManifest` specializes that value without redeclaring its fields, +and both `SceneManifest` and `SceneRegistry` use the same canonical ID, alias, +parent, native-member, and affordance index. A `SceneManifest` additionally +snapshots `collision_world_mode`; `SemanticIntegrationManifest.bind()` rejects +live metadata or collision-mode drift before installing a robot profile. + +## Semantic workflow compilation + +Semantic calls (`Pick`, `Place`, `HandOver`, and catalog-registered values) are +robot-independent declarations. `SemanticCallDescriptor` has one canonical +atomic `target_descriptor`; its `skill_id` and `binding_contract` are derived +views, not separately stored values. Curated call targets cannot be remapped. +Registered calls require an explicit agent-visible target plus an installed +`RegisteredSemanticLowerer` with a matching call ID and schema version. + +`SemanticSkillCompiler.analyze()` performs provider-free linking, resource and +affordance validation, held-object flow analysis, and first-release look-ahead. +A `Place` with no explicit `primary` resource inherits the workflow's known +holder resource, and a `HandOver` with no explicit `source` does the same. The +inferred selection is snapshotted onto the canonical linked call before +binding; an explicit conflicting selection still fails with +`held_resource_mismatch`. Inference never crosses a registered-call boundary. +`HandOver` selects participants only through the `source` and `destination` +resource slots; there is no separate receiver alias. +A pick therefore owns zero or one downstream object target rather than an +arbitrary target tuple. Relation targets retain affordance payload type and +revision metadata and stay late-bound through an explicitly installed +`RelationTargetGrounder`; handover poses stay behind a named +`HandOverPoseProvider` selected by the robot profile. + +`SemanticSkillCompiler.ground()` lowers exactly one analyzed call from the +latest `PlanningContext` and returns a `GroundedSemanticCall`. Its +`eligible_mask` is an owned snapshot and must be handed to execution together +with the invocation: + +```python +grounded = compiler.ground(workflow, call_index, context, eligible_mask=cohort) +session = engine.start( + (grounded.invocation,), + context, + eligible_mask=grounded.eligible_mask, +) +``` + +The compiler identity prevents a workflow from crossing lowerer/grounder +registries. Engine/profile staleness is checked through the bound integration; +the workflow does not duplicate engine-owner or catalog-revision fields. + +## Semantic runtime + +`embodichain.lab.sim.skills.SkillRuntime` is the single application execution +boundary. `start()` analyzes the complete semantic call window, captures a fresh +observation before each executed call, JIT-grounds one `ActionInvocation`, and +delegates planning, transport, recovery, acknowledgement, and safe stop to the +canonical Atomic Action runtime. `execution_prefix_length` lets later calls +participate in static look-ahead without executing them. + +`SkillResult` is the only runtime result adopted by higher layers. It retains +verified `TaskState`, row-local eligibility/success/failure masks, typed events, +effect traces, and failures. `step()` is non-blocking; `run()` is the synchronous +convenience path. `cancel()` delegates safe stop to the active runner. The +runtime never exposes a second physical state or command scheduler. + +`ParallelSkillRuntime` is the only physical-concurrency boundary. It requires +disjoint canonical `ResourceClaim` values and an explicit +`ParallelCommandSafetyValidator`; resource non-overlap alone is insufficient. +`AtomicSkills` is a thin facade over the same `SkillRuntime`, not another +execution implementation. + +`registry.make_planning_scene_provider(motion_generator, batch_size=...)` +returns a fresh `RegistrySceneProvider` with independent baselines and revision +counters after eager registry/provider/planner validation. Snapshots expose +canonical IDs only. The provider requires stable ordered `env_ids` and +monotonic timestamps, derives relative affordance poses from the same +observation, compares movement against the last materially published pose, and +maintains per-row collision revisions. Plain `make_scene_provider()` is only +for perception and advanced direct-core consumers without planner agreement. + +For an external perception/hardware provider, call +`registry.validate_collision_integration(..., scene_provider=provider)` +directly. The registry's complete `STATIC ∪ DYNAMIC` ID set must exactly +match `MotionGenerator.collision_world_entity_ids`; separately, the registry, +provider, and planner dynamic ID sets must match exactly in the canonical +namespace. The planner must support live updates for a non-empty dynamic set, +and planner/registry batch mode must agree. With dynamic entities, one +environment may infer `SHARED`; multiple environments must explicitly select +`SceneCollisionWorldMode.SHARED` or `PER_ENV`. + +Construct a registry-backed cuRobo world with +`registry.collision_geometry_by_id()`. Its default mapping includes only +`STATIC` and `DYNAMIC` registrations and excludes `NONE`. Mapping keys are +canonical logical/source IDs for cache identity and full-world validation. With +`cuboid` or `mesh`, they are also the physical YAML and runtime-update keys. +Static `sphere` sources expand to backend names such as `id_0`; dynamic sphere +configuration is rejected, while cache/full-world identity stays on `id`. +Registry mappings fail fast when a source lacks geometry required by the chosen +representation. List-valued cuRobo worlds and `RigidObjectSceneProvider` remain +advanced direct-core paths. + +Stable object identity follows these exact rules: + +1. The same `ObjectSemantics` instance is identical to itself. +2. If either side has an explicit `entity_id`, both sides must have an explicit + ID and the strings must match. Never compare an explicit ID directly with a + legacy UID, even when the spellings are equal. +3. Only when both explicit IDs are absent, compare non-empty legacy + `entity.uid` values. If either side has a valid UID, both must have one and + the strings must match. +4. Only when neither side has an explicit ID or valid UID may identity fall back + to the same live entity handle. `label` is descriptive and never establishes + identity. + +The direct-core identity rules do not perform alias resolution; normalization +belongs only to `SceneRegistry`. Partial-batch `StateDelta` attachment merges +use the same stable identity rules, so equivalent semantic wrappers update one +held object instead of creating label-based duplicates. + +For both individual and coordinated attachments, a same-identity partial merge +preserves scalar metadata: if any previously active environment row remains, +the merged relation keeps `previous.semantics` and selects only the per-row +mask, transforms, and grasp poses from previous/candidate values. It adopts +`candidate.semantics` only when no previously active row survives the update. +This prevents an update for some environments from silently replacing the +semantic metadata shared by untouched rows. + +Scene dependencies must match the poses each primitive actually consumes: + +| Primitive | Scene dependencies | +|---|---| +| `MoveEndEffector` | A `SceneEntityPose` in `xpos`. | +| `MoveJoints` | None; its target is qpos or a named control-profile command. | +| `PickUp` | Always its semantic `entity_id`, when present, because the object pose is grounded once and reused; plus any goal-owned `SceneEntityPose`, such as `grasp_xpos`. These dependencies are monitored only through the `approach` segment. | +| `CoordinatedPickment` | Goal-owned target/initial `SceneEntityPose` values; the semantic `entity_id` only when `object_initial_pose` is omitted and semantic grounding supplies that pose. | +| `Place` | A `SceneEntityPose` in ordinary `xpos`; for `AssembleGoal`, `base_pose` when supplied. Omitting `base_pose` uses the deprecated live `AssembleAffordance.base_object_entity` fallback with no dependency. | +| `MoveHeldObject` | A `SceneEntityPose` in `object_target_pose`; current object orientation is derived from observed EEF pose plus verified `object_to_eef`, not a scene-object read. | +| `Press` | `PressGoal.target_pose` when it is a `SceneEntityPose`; affordance data is entity-free. | +| `Slide` | `SlideGoal.target_pose` when it is a `SceneEntityPose`; the local grasp mesh does not own the link. | +| `Twist` | `TwistGoal.target_pose` when it is a `SceneEntityPose`; affordance data is entity-free. | +| `CoordinatedPlacement` | `SceneEntityPose` values in the placing or support object target pose. | +| `HandOver` | `SceneEntityPose` values in `HandOverOptions.middle_object_pose` or `final_object_pose`. Its current held-object pose is derived from verified attachment state and observed EEF pose; the reused `GraspGoal.grasp_xpos` field is ignored. | + +`collect_scene_dependencies()` deliberately stops at `ObjectSemantics`. +Therefore, a custom action that consumes a snapshot pose through semantic data +must override `_scene_dependencies()`, union `super()` dependencies, and add the +consumed semantic ID. Do not declare an ID merely because semantics are present. +`ActionPlan.scene_dependency_monitor_until` can bound each dynamic dependency +to an exclusive command-frame index. `PickUp` stops monitoring after its +approach is dispatched: target motion before contact still replans, while +contact-, grasp-, and lift-induced object motion is not misclassified as an +external target update. Collision-world and joint-tracking checks remain +independent of this boundary. ## Static compilation @@ -81,12 +445,21 @@ compiled = engine.compile(invocations, context=None) Compilation does not step simulation. It concatenates timed trajectories and applies successful expected effects only to `compiled.projected_context`, so a -following action can be checked against hypothetical state. Failed rows hold -their last successful qpos. +following action can be checked against hypothetical state. Because +`CompiledTrajectory` is a joint-trajectory result, every action plan in a +compiled sequence must own `joint_trajectory`; `compile()` rejects a generic +runtime-command plan without one. Use `start()` plus an execution runner for +plans whose authoritative `commands` target non-joint transports. Failed joint +rows hold their last successful qpos. Use invocation `skill_options` for multiple variants with the same stable `skill_id`; do not create per-variant built-in instances. +Composite actions allocate their named trajectory segments from the total +sample budget with `split_three_segments()`. The first motion allocation rounds +`(sample_count - hand_interp_steps) * first_segment_ratio`; callers must not +reproduce that calculation or assume truncation. + ## Dynamic execution and recovery `SceneEntityPose(entity_id, relative_pose)` is resolved from the latest scene @@ -94,39 +467,156 @@ snapshot every time the action plans. Its entity ID is recorded in `ActionPlan.scene_dependencies`. ```python -session = engine.start(invocations, initial_context) +session = engine.start( + invocations, + initial_context, + eligible_mask=initial_eligible_mask, +) runner = ExecutionRunner( session, observation_provider, command_sink, clock=execution_clock, ) -result = runner.step(effect_success=None) +result = runner.step(effect_result=None) ``` `ExecutionSession` owns deterministic planning progress and recovery state. It -emits at most one `JointCommand` per tick. The command's per-environment -`hold_duration` schedules the next feedback cycle from `TimedTrajectory.dt`: -command `i` carries the arrival interval `dt[:, i + 1]` leading to the next -waypoint. The final command reuses its own interval as a settling window. The -session monitors: +emits at most one synchronized `RuntimeCommandFrame` per tick from the plan's +authoritative `TimedCommandSequence`. A frame contains one or more +`EndpointCommand` values, a shared environment batch and active mask, and a +per-environment `hold_duration`. Every command pairs a +`RuntimeEndpointTarget` with a `RuntimeCommandPayload`; their `transport_id` +values must match, destinations must be unique within the frame, and joint +targets may not overlap. `ExecutionFeedbackMode.JOINT_POSITION` requires an +owned `joint_trajectory` and joint-position targets/payloads; generic command +plans default to timed completion and retain external semantic-effect +verification. Framework authorization replaces every emitted target with its +binding-owned snapshot and rejects unbound destinations, target substitution, +and endpoint claim conflicts. A plan's non-empty frames and its recovery +replans retain a stable destination set. Empty failed plans retain previously +active targets so the caller can still hold them. The session monitors: -- joint tracking error against the previous command; +- joint tracking error against the previous command in joint-position mode; - translation/rotation drift of referenced scene entities; - per-environment collision-world revision changes for collision-sensitive actions; - action-attempt timeout; - planner and semantic-effect failure. -It replans from the latest observation within per-environment budgets. The -budgets and eligibility masks are row-local, while the action waypoint cursor -is batch-synchronized: one allowed replan regenerates the active cohort and -restarts its action trajectory without charging unaffected rows. Unknown -or exhausted failures are reported as structured `ExecutionEvent` objects. A -non-empty `StateDelta` is not committed until the caller supplies an external -`effect_success` mask. While verification is outstanding, -`ExecutionTick.pending_effect` retains a typed `EffectVerificationRequest` on -every tick; `EFFECT_VERIFICATION_REQUIRED` is only the one-time audit event. +It replans from the latest observation within per-environment budgets. Pass an +owned boolean `eligible_mask` to `engine.start()` when a previous semantic call +has already deactivated rows. Eligibility can only shrink; use +`runner.deactivate_rows(mask, reason=...)` while a runner owns scheduling so its +cached effect request stays correlated. The budgets, verified task state, and +eligibility masks are row-local, while the action waypoint cursor and call +barrier are batch-synchronized. One allowed replan regenerates the still-pending +cohort without charging unaffected rows. Exhausted rows hold and never become +eligible again. + +A non-empty `StateDelta` is not committed until the caller supplies a +correlated `EffectVerificationResult`. Its disjoint `success_mask` and +`failure_mask` must be subsets of the current request mask; requested rows in +neither mask remain unresolved. Partial successes commit immediately while +unresolved rows keep the barrier pending. `EffectVerificationRequest` carries a +monotonic `verification_id`, stable `requested_at`/`deadline` values in the +robot-observation timestamp domain, a session-local `attempt_generation`, and +an owned effect snapshot. Mask shrinkage creates a new ID without extending the +deadline or changing the generation; installing a replacement plan increments +the generation. Results for an old ID are rejected. `RecoveryPolicy.action_timeout` +covers the trajectory and terminal effect wait together, and only timestamps +strictly greater than the deadline time out. While verification is outstanding, +`ExecutionTick.pending_effect` retains the request on every tick; +`EFFECT_VERIFICATION_REQUIRED` is only the one-time audit event. + +For synchronous verification, pass `effect_verifier(context, request)` to +`runner.step()` or `run_until_blocked()`. The runner calls it after the fresh +due-cycle observation and supplies its result to `session.tick()` in that same +cycle. It does not call the verifier when the observation timestamp is already +past the request deadline. A verifier must return an exact +`EffectVerificationResult`; all-false masks mean unresolved. External +asynchronous integrations instead pass `effect_result` explicitly on a due +`step()` call. + +```python +import torch + +request = tick.pending_effect +effect_result = EffectVerificationResult( + verification_id=request.verification_id, + success_mask=observed_success, + failure_mask=observed_failure, + invalidation_mask=observed_failure, + retry_mask=torch.zeros_like(observed_failure), +) +result = runner.step(effect_result=effect_result) +``` + +Both failure-policy masks must be subsets of `failure_mask`. +`invalidation_mask` selects rows on which the core applies the request-owned, +removal-only `failure_invalidation` delta; it does not let the verifier inject +state. `retry_mask` is reserved for rows whose physical preconditions still +make replay of the same invocation valid. Other failed rows require external +recovery. Unresolved evidence at the action deadline is reconciled fail-closed +when the pending effect covers active verified state. + +The semantic layer keeps physical observation separate from symbolic effect +commit. `SkillPolicyPreset.effect_monitors` maps exact semantic call IDs to +versioned, bounded-declarative `EffectMonitorRef` values. Omitting the mapping +selects the built-in `builtin.composite_effect@1` monitor for `pick`, +`place`, and `hand_over`; an explicit empty mapping disables the default and +makes analysis of those curated calls fail with `missing_effect_monitor`. +`SemanticIntegrationManifest` rejects monitor keys absent from its call +catalog. `SemanticSkillCompiler.analyze()` resolves the exact factory and +validates monitor parameters without observing scene providers or constructing +stateful monitors. + +Grounding creates an immutable `SemanticEffectSpec` and an independent monitor +for the call. The spec separates typed symbolic state expectations from typed +physical clauses. Pick declares an attached destination, Place a detached +source with an owned pre-effect pose baseline, and HandOver both. Endpoint +adapters publish immutable `EffectEvidenceSourceRef` values and a logical +`task_state_key`; evidence routes use `EffectEvidenceAddress`, never the +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 +observation revisions. Providers do not apply policy thresholds. The composite +monitor evaluates clauses as a conjunction per state expectation, applies +pose/force/joint hysteresis, treats invalid rows as unresolved, and reports +explicit contradictory evidence as failure. It never uses `TaskState` as +physical proof. The `SkillRuntime` adapter validates the decision, attaches +only the current verification ID, and returns an exact +`EffectVerificationResult` in the same due observation cycle. Request shrink +within one `attempt_generation` preserves remaining-row hysteresis; installing +a retry/replan/revision increments the generation and resets it. Evidence at +the exact deadline is allowed; evidence after it is rejected and normal runner +timeout/recovery remains authoritative. + +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 @@ -135,52 +625,80 @@ policy, binding, or control command during execution, submit a strictly newer revision explicitly: ```python -session.revise_current(revised_invocation) +runner.revise_current(revised_invocation) ``` The replacement must keep the active `skill_id` and `invocation_id`. The session resolves a new snapshot, resets that revision's recovery budgets, and -replans from the latest context. +replans from the latest context. Once runtime destinations are owned, the +replacement must preserve a non-empty destination set and every target address +fingerprint; changing a base, whole-body, arm, controller, or safe-hold +footprint requires a new invocation. The runner snapshots the revision, keeps +the current frame deadline, then observes and installs it at the next due +boundary. Pending physical effects must be verified first, or the caller must +cancel and start a new invocation. A caller that owns manual session ticks may +use `session.revise_current(..., context=fresh_context)` directly. `ExecutionRunner` owns the controller-facing lifecycle around a session: - `ObservationProvider.observe(task_state)` supplies a fresh, monotonically timestamped `PlanningContext` when a feedback cycle is due; -- `CommandSink.send/hold/cancel` returns a `CommandAcknowledgement` with - `accepted`, `rejected`, or `timed_out` status; +- `CommandSink.send(frame)`, `hold(targets, context)`, and `cancel(targets)` + return a `CommandAcknowledgement` with `accepted`, `rejected`, or + `timed_out` status; +- `EndpointCommandRouter` is the standard mixed-controller sink. It preflights + every frame, groups commands or targets by exact transport ID, dispatches to + registered `EndpointCommandTransport` implementations, and accepts only when + every addressed transport accepts; - `ExecutionClock` supplies monotonic time and backend waiting; - non-blocking `step()` dispatches only when the current command's `hold_duration` has elapsed; +- `revise_current()` stages an owned same-address revision, preserves the active + frame deadline, and replans it from the next due observation; - `run_until_blocked()` is a convenience loop that waits through the clock and stops at a terminal state or an unhandled effect-verification boundary; the runner remembers that boundary so a later verifier call can resume it; -- cancellation, observation/session exceptions, and negative acknowledgements - enter a best-effort cancel-then-hold path. +- before dispatch, the runner records every target that may become armed by + `(transport_id, target_id)`; cancellation, observation/session/controller + exceptions, and negative acknowledgements enter target-scoped safe stop: + cancel all recorded targets first, then hold them from a fresh observation or + the last validated context when one is available. + +Every transport must actively neutralize inactive rows for each addressed +target. Omission is unsafe for persistent controllers: position transports +hold those rows and velocity transports normally command zero velocity. `TimedTrajectory.dt[:, i]` is the interval leading to sample `i`. -`ExecutionSession` dispatches sample zero immediately, then maps each following -arrival interval to the preceding command's `JointCommand.hold_duration`. The -final sample uses its own interval again as a settling window before terminal -validation. Batched execution currently advances at a synchronized barrier -using the longest active row interval. - -`SimulationExecutionAdapter` implements observation, command, and clock ports -for a `SimulationManager`/`Robot` pair. Its `sleep()` advances an integral +The built-in joint lowerer dispatches sample zero immediately, then maps each +following arrival interval to the preceding `RuntimeCommandFrame`'s +`hold_duration`. The final frame uses its own interval again as a settling +window before terminal validation. Generic action implementations set frame +hold durations directly. Batched execution currently advances at a +synchronized barrier using the longest active row interval. + +`SimulationExecutionAdapter` implements observation and clock ports plus the +exact `robot.joint_position` endpoint transport for a +`SimulationManager`/`Robot` pair. It can serve directly as the command sink for +joint-only plans or be registered in an `EndpointCommandRouter` beside mobile, +whole-body, or device-specific transports. Its `sleep()` advances an integral number of physics steps, so simulation execution does not depend on wall time. Stable context IDs are correlation identifiers; the adapter maps command rows to simulation robot indices rather than using those IDs as array indices. -Real-device adapters should implement the same protocols and enforce the passed -acknowledgement timeout in their transport/controller layer. +Real-device transports should implement `EndpointCommandTransport` and enforce +the passed acknowledgement timeout in their controller/client layer. `SceneProvider.snapshot(timestamp=..., env_ids=...)` is the scene-observation boundary used by execution adapters. `SceneSnapshot.collision_entity_ids` identifies obstacle poses consumed by a planner, while `collision_world_revision` is either global or per environment. A newer -revision invalidates only affected batch rows. `RigidObjectSceneProvider` -tracks live simulation objects, filters sub-threshold pose noise, advances the -general scene version, and maintains per-environment collision revisions. -Thresholds are measured from the last materially published pose per entity and -environment, so repeated sub-threshold motion eventually becomes observable. +revision invalidates only affected batch rows. `RegistrySceneProvider` is the +canonical provider and derives its entity/collision sets from one immutable +`SceneRegistry`. It filters sub-threshold pose noise, advances the general scene +version, and maintains per-environment collision revisions. Thresholds are +measured from the last materially published pose per entity and environment, so +repeated sub-threshold motion eventually becomes observable. +`RigidObjectSceneProvider` retains that lower-level revision behavior for +advanced direct-core integrations. For lightweight sources that do not need environment correlation IDs, `SimulationExecutionAdapter` also accepts a mutually exclusive `SceneSnapshotSupplier(timestamp)` callback. @@ -188,13 +706,20 @@ For lightweight sources that do not need environment correlation IDs, The public `AtomicAction.plan()` copies `MotionPolicy` and binds collision entity poses through `MotionGenerator.bind_collision_world()`. The motion generator owns option copying and the backend capability boundary, then forwards the -update through `BasePlanner.with_collision_world()`. Backends opt in via -`supports_collision_world_updates`; cuRobo implements this bridge using -`CuroboPlanOptions.dynamic_obstacle_poses`. Replanning therefore consumes the -same scene snapshot that triggered invalidation without adding obstacle -parameters to each skill. Add/remove/geometry mutations are not yet supported -by this pose-update path; providers should revision only pose-updatable -registered obstacles. +update through `BasePlanner.with_collision_world()`. Backends opt in through +`BasePlanner.collision_world_info.supports_updates`; cuRobo implements this +bridge using `CuroboPlanOptions.dynamic_obstacle_poses`. Replanning therefore +consumes the same scene snapshot that triggered invalidation without adding +obstacle parameters to each skill. Add/remove/geometry mutations are not yet +supported by this pose-update path; providers should revision only +pose-updatable registered obstacles. + +`BasePlanner.collision_world_info` exposes the backend's complete world, +dynamic subset, batching mode, and update capability as one immutable contract. +`MotionGenerator` validates and forwards it for +`SceneRegistry.make_planning_scene_provider()`. External providers call +`validate_collision_integration(..., scene_provider=...)`. These construction +checks are separate from per-plan `bind_collision_world()`. Runnable closed-loop examples live under `scripts/tutorials/atomic_action/`: `tracking_error_recovery.py`, `moving_target_recovery.py`, and @@ -206,13 +731,168 @@ live observation fails. Environment IDs must remain stable and ordered for the entire session; robot and scene timestamps and scene versions must be monotonic. Collision-world revisions must also remain monotonic per environment. +## Semantic runtime and Expert Programs + +`embodichain.lab.sim.skills` is the semantic frontend over the core contracts. +`Pick`, `Place`, `HandOver`, `OperateArticulation`, and registered extension +calls are immutable, robot-independent intent values. `SemanticSkillCompiler` +performs provider-free workflow analysis first, then grounds exactly one call +from a fresh `PlanningContext`. It resolves the authoritative `SceneRegistry`, +profile resource binding and preset, downstream target look-ahead, typed goal, +effect specification, and effect monitor before producing one +`ActionInvocation`. + +`SkillRuntime` owns the shared call barrier and persistent verified `TaskState`. +Every call creates exactly one one-invocation `ExecutionSession` and re-observes +before the next call. Eligibility, success, failure, cancellation, recovery, +and effect state are row-local; active rows share the call boundary. The +runtime exposes non-blocking `start()`/`step()` and synchronous `run()` over the +same path. `AtomicSkills` is a convenience facade. `AtomicSkills.from_env()` +accepts only an explicit `SkillRuntimeProvider` and never scans arbitrary +environment attributes; Gym demo environments use the lazy bridge below so +commands cannot bypass `env.step()`. + +`embodichain.lab.gym.envs.expert_program` owns strict declarative programs. +Schema version 1 supports bounded `Sequence`, `Repeat`, `Segment`, and `Invoke`; +version 2 adds deterministic `Parallel` branches and explicit `Barrier` nodes. +The decoder rejects unknown fields/discriminators, duplicate serialized keys, +unsupported versions, executable values, dotted environment traversal, +unbounded expansion, and invalid registry/catalog references before runtime. +JSON and YAML files are loaded with `load_expert_program()`. A Gym config can +select one with `expert_program_path`, resolved relative to that config file. + +`ExpertProgramCompiler` expands program/demo segments lazily while preserving +typed target selections, post-policies, validators, and parallel blocks. +`AtomicDemoBridge` assembles each segment around the canonical runtime and a +buffered command sink. A `ProcessedEnvAction` marks controller-ready output so +the action manager does not transform it twice, but every command and +post-policy hold still passes through ordinary `env.step()`. `BaseEnv.step_dt` +is authoritative; frame durations must be integral multiples of that cadence. +Parallel lanes are aligned on that strict grid and shorter lanes repeat their +last safe target as hold padding; fractional frames are rejected rather than +implicitly resampled. Early generator termination performs the bridge's +explicit cancel-then-hold handshake before the iterator is closed. + +Bridge creation materializes the bounded segment stream and performs +provider-aware semantic preflight before the first command is emitted. +Sequential stretches analyze their remaining downstream calls together, so a +Pick retains target look-ahead across logical segment boundaries; an explicit +parallel block is a conservative look-ahead barrier. Runtime grounding remains +just-in-time against the latest observation. Relation Place calls require an +exact typed/versioned `RelationTargetGrounder`, and HandOver requires the +profile-selected `HandOverPoseProvider`; neither provider is inferred from +names. + +The production simulation path is +`create_simulation_expert_program_adapter(environment, scene_binding=..., +robot_profile_binding=...)`. `SimulationSceneBinding` declares canonical/native +scene data, while `SimulationRobotSkillProfileBinding` declares reusable robot +resources, capabilities, commands, defaults, and presets. The factory creates +the registry, profile, motion generator, engine, shared-tick observation/evidence +port, command encoder, runtime, and segment policy port. Task classes combine an +external declarative program with typed scene/profile integration declarations +and install the returned adapter; they do not assemble skill trajectories. + +`SimulationRobotSkillProfileBinding` accepts generic `RobotResourceBinding` +declarations containing arbitrary typed `ResourceEndpoint` values; +`ControlPartResourceBinding` is the joint-backed convenience. Mobile-base, +whole-body, and non-joint integrations install a matching +`ResourceEndpointAdapter` and `RuntimeTransportActionEncoder` through the same +standard simulation factory. Task-level Expert Programs remain unchanged. This +is an extension seam rather than built-in locomotion: current curated semantic +skills do not consume the example base/whole-body capabilities. A reusable +production capability also installs its semantic descriptor/lowerer, atomic +skill, payload, safe-state transport behavior, and effect integration as +applicable. + +The standard Gym encoder currently composes custom transports over a full-qpos +hold and the standard simulation factory owns a `MotionGenerator`. A robot may +omit named control parts, but a truly jointless or natively structured mobile +controller still needs a reusable base-action composition/provider +integration. That integration must not add base- or whole-body-shaped fields to +the generic resource, binding, runner, or router contracts. + +Task vertical slices may keep typed profile bindings locally during API +stabilization, but repeated use should promote them into an embodiment-owned +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. 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 +`ControlCommandStateEvidenceTracker`. The tracker changes row-local state only +after an exact profile-owned `open` or `grasp` command is successfully encoded +and buffered. Intermediate commands and inactive rows retain prior state; +cancel, discard, or observer failure invalidates affected evidence. Stable +`env_ids`, not simulator array assumptions, correlate full and subset batches. +This command state is evidence of accepted controller intent, not physical +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. 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, +plan-attempt trajectory segments, scene/collision revisions and dependencies, +plus effect decisions and monitor evidence. Segment metadata adds post-policy +settling and validator results. Trajectory segments are trace ranges inside an +atomic plan and never own separate recovery, effect, or timeout state. + +Parallel execution is an explicit schema/runtime layer rather than a second +atomic scheduler. Static analysis rejects overlapping `ResourceClaim` values. +Independent lane runtimes share one clock and barrier, command frames are +merged only after destination/claim/safety validation, failure handling is +row-local, and verified `StateDelta` values merge deterministically at the +barrier. Parallel execution also requires an authoritative +`ParallelCommandSafetyValidator`; resource disjointness alone is never promoted +to physical-safety evidence, and a missing validator fails closed. Schema +version 2 intentionally uses strict task-state key-level merge conflicts; +mask-aware same-key branch merges are not part of this version. + ## Parameter ownership Goal dataclasses carry only semantic task intent. They do not carry robot part names, planner configuration, retry policy, or runtime state. -`MotionPolicy` owns planner selection, motion strategy, sample count, fallback -control period, limits, dynamic-collision mode, and typed planner options. +`MotionPolicy` owns motion strategy, sample count, dynamic-collision mode, and +typed planner options. Optional planner-backend compatibility belongs to +`SkillPolicyPreset.required_planner`; velocity and acceleration constraints +belong to the selected backend's typed `PlanOptions`. Timing belongs to the +trajectory producer: planners return explicit `dt` with derived `duration`, +while custom or composite interpolation constructs a `TimedTrajectory` using +an explicit cadence such as `PlanningContext.require_control_dt()`. Missing +timing is an error rather than an engine-owned default. `DynamicCollisionMode.AUTO` consumes a live collision world when available, `OFF` ignores snapshot collision entities and their revisions, and `REQUIRED` fails unless the motion strategy, scene, and planner support that path. These @@ -227,24 +907,44 @@ There is no `ActionCfg` or built-in `*Cfg` layer. built-in can be replaced only with explicit `replace=True`. Registration means an implementation is installed; it does not prove that the current embodiment has compatible control parts, profiles, bindings, or task state. Capability -adapters must filter registered descriptors before exposing skills to an Agent. -The module-level `register_action()` API is a process-wide extension-type -discovery catalog only; it neither binds actions nor changes an engine's -default built-in set. +discovery is separate: `engine.skills` +contains only agent-visible installed actions whose concrete classes explicitly +declare a `binding_contract`; when a robot profile is bound, +`engine.skill_profile.skills` further filters that catalog to valid resource +assignments. Registration is engine-local; there is no independent process-wide +action catalog. Construct extensions explicitly and install them with +`engine.register()` so discovery and execution cannot observe disconnected +registries. `ExecutionRunnerCfg` is intentionally separate from action options. It configures controller acknowledgement deadlines, scheduler cadence, and final safe-hold behavior for one runner instance; it does not change skill planning semantics and does not belong in `ActionInvocation` or an invocation revision. -Every `ActionBinding` value is a `RobotCfg.control_parts` key. It is not a link, -TCP-frame, joint, or scene-object name. Planning services validate those names -and resolve immutable `ResolvedControlPart` values containing full-robot joint -indices. Built-ins use the binding as the only source for participating arm and -hand names; attachment state and `StateDelta` keys use the bound manipulator. +`ActionBinding` is an engine-owned tuple of `EndpointBinding` values, not a map +of arm/tool roles. Each endpoint is addressed by the contract's exact +`(slot_id, endpoint_id)` key and contains its logical `resource_id`, adapter ID, +capabilities, semantic commands, claims, and immutable runtime target. A +`RuntimeEndpointTarget` is controller addressing, not a live controller: its +`transport_id` selects a transport and its `target_id` selects the destination +within that transport. `JointPositionTarget` is the built-in target for a named +`RobotCfg.control_parts` entry and additionally owns its full-robot joint IDs. +Built-in joint primitives explicitly require that target type when they need +IK, joint interpolation, or current attachment keys; a custom mobile or +whole-body skill is not required to masquerade as an arm or hand. -Embodiment-specific joint commands do not belong to Action options. Register -them once by actual control-part name: +Attachment state and `StateDelta` keys use the bound target's concrete control +part. `TaskState.held_objects` is the sole attachment map. A multi-manipulator +grasp stores one `HeldObjectState` per manipulator with the same +`ObjectSemantics` instance. `TaskState.held_object_mask()` exposes active rows, +while `exclusive_held_object_mask()` excludes rows where another manipulator +holds the same semantic object or live entity. Single-arm transport, release, +and handover operations only succeed on exclusive rows; coordinated placement +likewise requires two distinct, exclusively held objects. + +Embodiment-specific semantic commands do not belong to action options. A caller +using direct control-part binding without a `RobotSkillProfile` registers them +by actual control-part name: ```python engine = AtomicActionEngine( @@ -259,46 +959,102 @@ engine = AtomicActionEngine( ) ``` -Actions request semantic commands (`open`, `grasp`, or a named joint target) -from the `ResolvedControlPart`. `ActionControlOverrides` may replace commands -by semantic binding role for one invocation revision. Joint limits constrain -commands but do not define semantic open/grasp states; a robot integration or -tutorial may derive a simple profile from limits explicitly. +Actions request semantic commands (`open`, `grasp`, or a named target) from an +`EndpointBinding`; `joint_positions()` is the typed convenience for a +`JointPositionCommand`. `ActionControlOverrides` may replace commands under the +exact `slot -> endpoint -> command` path for one invocation revision. Joint +limits constrain commands but do not define semantic open/grasp states; a robot +integration or tutorial may derive a simple profile from limits explicitly. +Profile-based integrations instead own commands under generic +`command_profiles` IDs and let endpoint declarations/adapters resolve those +IDs. `action_control_profiles()` additionally exposes applicable control-part +commands to the built-in joint planning helpers; custom endpoint commands stay +on their resolved endpoint. ## Built-ins -| Skill ID | Goal type | Roles | +| Skill ID | Goal type | Required slot endpoints | |---|---|---| -| `move_end_effector` | `EndEffectorPoseGoal` | manipulator `primary` | -| `move_joints` | `JointPositionGoal` (`target` is explicit qpos or a profile command name) | manipulator `primary` | -| `pick_up` | `GraspGoal` | manipulator/end effector `primary` | -| `move_held_object` | `HeldObjectPoseGoal` | manipulator/end effector `primary` | -| `place` | `PlaceGoal`, `AssembleGoal` | manipulator/end effector `primary` | -| `press` | `PressGoal` | manipulator/end effector `primary` | -| `coordinated_pickment` | `CoordinatedPickGoal` | `left`, `right` | -| `coordinated_placement` | `CoordinatedPlacementGoal` | `placing`, `support` | -| `hand_over` | `GraspGoal` | `source`, `destination` | +| `move_end_effector` | `EndEffectorPoseGoal` | `primary.motion` | +| `move_joints` | `JointPositionGoal` (`target` is explicit qpos or a profile command name) | `primary.motion` | +| `pick_up` | `GraspGoal` | `primary.motion`, `primary.grasp` | +| `move_held_object` | `HeldObjectPoseGoal` | `primary.motion`, `primary.grasp` | +| `place` | `PlaceGoal`, `AssembleGoal` | `primary.motion`, `primary.grasp` | +| `press` | `PressGoal` | `primary.motion`, `primary.grasp` | +| `slide` | `SlideGoal` | `primary.motion`, `primary.grasp` | +| `twist` | `TwistGoal` | `primary.motion`, `primary.grasp` | +| `coordinated_pickment` | `CoordinatedPickGoal` | `left.motion`, `left.grasp`, `right.motion`, `right.grasp` | +| `coordinated_placement` | `CoordinatedPlacementGoal` | `placing.motion`, `placing.grasp`, `support.motion`, `support.grasp` | +| `hand_over` | `GraspGoal` | `source.motion`, `source.grasp`, `destination.motion`, `destination.grasp` | +| `operate_articulation` | `OperateArticulationGoal` | `primary.motion`, `primary.interaction` | + +`PressAffordance`, `SlideAffordance`, and `TwistAffordance` contain only +target-local geometry and interaction semantics. Their goals own an explicit +`target_pose`, which may be a deterministic tensor snapshot or a late-bound +`SceneEntityPose`. Never put an `Articulation`, `RigidObject`, or live link pose +reader in these affordances. + +`Press` and `Slide` use dense axis-aligned Cartesian targets for their contact +motion. The linear motion-generator path solves every output sample with IK; +it does not resample sparse IK endpoints in joint space. `Press` has a distinct +contact segment before penetration. `TwistAffordance.axis_origin` and +`twist_axis` together define the full 3D rotation axis. + +These three motion-centric primitives declare `SkillDescriptor.open_loop=True` +and an empty `StateDelta`. Their completion means motion execution only, not +verified button actuation, grasp retention, or articulation travel. Applications +that need semantic completion must observe and verify those physical outcomes. `GraspGoal.grasp_xpos` accepts an explicit pose tensor, a late-bound `SceneEntityPose`, or `None` for affordance sampling. A `SceneEntityPose` registers the referenced entity as a recovery dependency, allowing an executing -`PickUp` to replan when the grasp target moves. +`PickUp` to replan when the grasp target moves. `PickUp` also resolves its +semantic object's pose once per planning attempt and declares the semantic +`entity_id` because grasp sampling, upright adjustment, and the held +`object_to_eef` relation all consume that same pose. + +`AssembleGoal.base_pose=SceneEntityPose(...)` is the canonical assembly anchor +and becomes a recovery dependency. An omitted `base_pose` permits the deprecated +live `AssembleAffordance.base_object_entity` fallback for direct-core callers +only; it is not dependency-tracked. The current `assemble.py` tutorial exercises +that legacy fallback, while `moving_target_recovery.py` is the canonical +snapshot-grounded object example. ## Extension rules -1. Define a frozen action-owned goal dataclass with `goal_kind`. +1. Define a frozen action-owned goal dataclass. 2. Define a frozen `ActionOptions` subclass only when runtime behavior exists. -3. Declare `skill_id`, `GoalType`, `OptionsType`, and required semantic roles. +3. Declare `skill_id`, `GoalType`, `OptionsType`, and a class-local + `SkillBindingContract` when the skill should appear in `engine.skills`. + Express only semantic slots, endpoint requirements, capabilities, required + commands, and any real disjointness constraints; do not introduce arm/tool + roles for a mobile-base or whole-body endpoint. 4. Implement `_plan()`; do not override the framework-owned `plan()` method. -5. Validate with `require_goal(request)` and consume only the resolved binding. +5. Validate with `require_goal(request)` and consume endpoints only through + `request.binding.endpoint(slot_id, endpoint_id)`. Require a concrete target + subtype only when the planner or payload implementation genuinely needs it. 6. Plan from `context.robot.qpos`; never read an implicit live start state. -7. Return full-robot positions or a `TimedTrajectory` through `build_plan()`. - Build batched `list[PlanState]`, translate the policy with - `request.motion_policy.to_motion_gen_options()`, and call - `self.motion_generator.generate()`. Import pure operations directly from - `trajectory_ops.py`. -8. Declare symbolic changes with `StateDelta`; do not mutate context or commit - physical effects during planning. -9. Keep scene stepping, controller I/O, and task-graph/MLLM logic outside the +7. If planning consumes a semantic object's snapshot pose, override + `_scene_dependencies()`, preserve `super()` dependencies, and add exactly + that semantic ID. +8. For planner-backed joint motion, return a full-robot `TimedTrajectory` + through `build_plan()`; raw position tensors are rejected. Preserve planner + `dt`, or use `TimedTrajectory.from_uniform_step()` with an explicitly + selected cadence for action-owned interpolation. Build batched + `list[PlanState]`, translate the policy with + `request.motion_policy.to_motion_gen_options()` (including + `interpolation_dt=context.control_dt` when applicable), call + `self.motion_generator.generate()`, and import pure operations directly from + `trajectory_ops.py`. For mobile, whole-body, or other controller-native + motion, build `EndpointCommand` frames and a `TimedCommandSequence`, then use + `build_command_plan()`. A new transport family must define matching + `RuntimeEndpointTarget` and `RuntimeCommandPayload` types with the same + `transport_id`, plus an `EndpointCommandTransport` registered in the runner's + router. +9. Declare symbolic changes with `StateDelta`; do not mutate context or commit + physical effects during planning. For partial attachment updates, retain + previous scalar semantics while any previous row remains; merge only batched + masks/transforms and adopt candidate semantics only on full replacement. +10. Keep scene stepping, controller I/O, and task-graph/MLLM logic outside the atomic action. Put execution-loop I/O behind the runner protocols rather than calling a simulator or device from `plan()` or `ExecutionSession`. diff --git a/agent_context/topics/env-framework/env-framework.md b/agent_context/topics/env-framework/env-framework.md index 3504ab25e..fc68947c4 100644 --- a/agent_context/topics/env-framework/env-framework.md +++ b/agent_context/topics/env-framework/env-framework.md @@ -12,7 +12,7 @@ | `embodichain/lab/gym/envs/base_env.py` | `BaseEnv(gym.Env)` + `EnvCfg` — low-level env loop | | `embodichain/lab/gym/envs/embodied_env.py` | `EmbodiedEnv(BaseEnv)` + `EmbodiedEnvCfg` — modular task base class | | `embodichain/lab/gym/utils/registration.py` | `@register_env` decorator + `REGISTERED_ENVS` registry + `make()` | -| `embodichain/lab/gym/envs/tasks/__init__.py` | All concrete task imports (forces registration on import) | +| `embodichain_tasks/embodichain_tasks/__init__.py` | Recursively imports official tasks to trigger registration | | `embodichain/lab/gym/envs/managers/__init__.py` | Manager re-exports: `EventManager`, `ObservationManager`, `RewardManager`, `ActionManager`, `DatasetManager` | | `embodichain/lab/gym/envs/wrapper/no_fail.py` | `NoFailWrapper` — forces `is_task_success() → True` | | `embodichain/lab/gym/envs/wrapper/replay.py` | `ReplayWrapper` — record-and-replay trajectories (kinematic/dynamic/control) | @@ -208,11 +208,11 @@ from the event config before the event manager is created. Use the `/add-task-env` skill. It scaffolds: -1. A new file under `embodichain/lab/gym/envs/tasks//`. +1. A new file under `embodichain_tasks/embodichain_tasks//`. 2. `@register_env("")` decorator on the class. 3. `EmbodiedEnvCfg` subclass with robot, sensor, object configs. 4. Stub implementations of `_setup_robot()`, `evaluate()`, `get_reward()`. -5. Import entry in `tasks/__init__.py`. +5. Export entry in the category package's `__init__.py`. 6. Test stub. ### Minimal manual skeleton @@ -355,7 +355,7 @@ parent; the first `warmup_steps` samples are discarded. | Symptom | Cause | Fix | |---|---|---| -| `KeyError: "Env X not found in registry"` | Task module not imported → `@register_env` never ran | Add import to `tasks/__init__.py` | +| `KeyError: "Env X not found in registry"` | Task entry-point package not imported → `@register_env` never ran | Check the `embodichain.tasks` entry point and package import | | `RuntimeError: non json dumpable kwargs` | Passing class/type objects to `@register_env(…, kwarg=SomeClass)` | Use string keys + lookup mapping instead | | `single_action_space is None` | `_setup_robot()` didn't set `self.single_action_space` | Set it before returning the Robot | | `_setup_robot()` returns `None` | Forgot to return the Robot instance | Ensure `return robot` | diff --git a/agent_context/topics/motion-planning/motion-planning.md b/agent_context/topics/motion-planning/motion-planning.md index 4f5644cc2..41483d2cf 100644 --- a/agent_context/topics/motion-planning/motion-planning.md +++ b/agent_context/topics/motion-planning/motion-planning.md @@ -5,7 +5,7 @@ | What | Path | |---|---| | Planner registry | `embodichain/lab/sim/planners/__init__.py` | -| Base planner class & config | `embodichain/lab/sim/planners/base_planner.py` → `BasePlanner`, `BasePlannerCfg`, `PlanOptions`, `validate_plan_options` | +| Base planner class & config | `embodichain/lab/sim/planners/base_planner.py` → `BasePlanner`, `BasePlannerCfg`, `CollisionWorldInfo`, `PlanOptions`, `validate_plan_options` | | TOPPRA planner | `embodichain/lab/sim/planners/toppra_planner.py` → `ToppraPlanner`, `ToppraPlannerCfg`, `ToppraPlanOptions` | | Neural planner | `embodichain/lab/sim/planners/neural_planner.py` → `NeuralPlanner`, `NeuralPlannerCfg`, `NeuralPlanOptions` | | cuRobo planner | `embodichain/lab/sim/planners/curobo/curobo_planner.py` → `CuroboPlanner`, `CuroboPlannerCfg`, `CuroboWorldCfg`, `CuroboPlanOptions` | @@ -97,6 +97,19 @@ Learning-based EEF waypoint planner. Franka Panda only. ### CuroboPlanner collision worlds +`CuroboWorldCfg.rigid_objects` accepts either a mapping or a sequence. Use +`Mapping[registry_id, RigidObject]` for a registry-backed integration. The +mapping key is the authoritative logical/source obstacle ID used by the +content-cache key, `collision_world_entity_ids`, and registry validation. For +`cuboid` and `mesh`, it is also the physical YAML obstacle name and dynamic +update key. For `sphere`, one static source expands to physical YAML names such +as `registry_id_0`; dynamic sphere configuration is rejected, while cache and +full-world identity remain keyed by `registry_id`. A registry mapping whose +source lacks mesh geometry required by the selected representation fails fast +instead of silently dropping the source. The sequence form is an advanced +direct-core path that derives names from each object's `uid` or an +`obstacle_` fallback. + `CuroboWorldCfg.multi_env` controls collision-world batching, not whether robot states or goals are batched: @@ -116,16 +129,44 @@ differences require `"cuboid"` or `"mesh"` representation, registration in data and collision caches, so retain the shared default for identical rebased layouts. -`BasePlanner.supports_collision_world_updates` and +`BasePlanner.collision_world_info` and `with_collision_world(options, obstacle_poses=...)` form the generic per-plan -dynamic-world bridge. The base implementation opts out and leaves options -unchanged. `CuroboPlanner` opts in, clones the supplied pose tensors, and merges -them into `CuroboPlanOptions.dynamic_obstacle_poses`. +dynamic-world bridge. The base property returns `None` and the base hook leaves +options unchanged. `CuroboPlanner` returns an immutable `CollisionWorldInfo` +with updates enabled, clones the supplied pose tensors, and merges them into +`CuroboPlanOptions.dynamic_obstacle_poses`. `MotionGenerator.supports_dynamic_collision_world` exposes the capability and `MotionGenerator.bind_collision_world()` owns option copying before forwarding to the backend hook. Atomic actions use that facade from their framework-owned `plan()` template when a `SceneSnapshot` declares collision entities; individual skills must not construct backend obstacle options themselves. +`CollisionWorldInfo` carries the complete canonical world, its dynamic subset, +the `"shared"` / `"per_env"` mode, and update capability as one validated +contract. It requires unique canonical IDs and requires the dynamic subset to +belong to the complete world. `MotionGenerator.collision_world_info` forwards +that contract and retains derived ID/mode properties for callers. For cuRobo, +the complete set is every mapping key (or inferred sequence name), while the +dynamic set is exactly `CuroboWorldCfg.dynamic_obstacle_names`. Sphere-expanded +physical YAML names are not part of either logical ID declaration. +`CuroboWorldCfg` rejects duplicate obstacle names and requires every +`dynamic_obstacle_name` to match an object registered in `rigid_objects`, so a +planner-local mismatch fails before backend construction. + +For the canonical path, pass `SceneRegistry.collision_geometry_by_id()` into +`CuroboWorldCfg.rigid_objects`, derive dynamic names from the registry, and call +`SceneRegistry.make_planning_scene_provider(motion_generator, batch_size=...)` +before execution. The geometry mapping excludes `NONE` registrations. The +factory first requires the registry's complete `STATIC ∪ DYNAMIC` set to +equal `MotionGenerator.collision_world_entity_ids`, then requires exact +registry/derived-provider/planner dynamic-subset agreement. It also checks +update capability for a non-empty dynamic set and the same collision-world +batch mode. An external perception/hardware provider instead uses +`validate_collision_integration(..., scene_provider=provider)`. + +One environment may infer `SHARED`; a multi-environment registry with dynamic +entities must explicitly choose `SHARED` or `PER_ENV`. Alias normalization +happens before planner construction, so planner IDs must never be simulator +UIDs unless that string is also the chosen canonical registry ID. `MotionGenerator.resolve_plan_options()` is the corresponding option-ownership boundary. It copies caller-supplied typed options, otherwise obtains backend @@ -142,7 +183,8 @@ Unified interface for trajectory planning with optional pre-interpolation. - `MotionGenCfg.planner_cfg` is **MISSING** — must be provided. - `generate()` and `interpolate_trajectory()` are env-batched (`B, N, DOF`). - `generate()` always returns a normalized `PlanResult`; failed rows hold the - supplied `start_qpos`. + supplied `start_qpos`, and every returned trajectory has explicit `dt` and + a `duration` derived from it. `MotionGenOptions` fields: @@ -156,6 +198,7 @@ Unified interface for trajectory planning with optional pre-interpolation. | `control_part` | `str \| None` | `None` | Robot control part name (must match `RobotCfg.control_parts` key) | | `plan_opts` | `PlanOptions \| None` | `None` | Passed to the underlying planner | | `is_interpolate` | `bool` | `False` | Pre-interpolate waypoints before planning | +| `interpolation_dt` | `float \| None` | `None` | Required explicit waypoint interval for `strategy="ik_interp"` and automatic joint interpolation fallback | | `interpolate_nums` | `int \| list[int]` | `10` | Points per segment (scalar or per-segment list) | | `is_linear` | `bool` | `False` | `True` = Cartesian linear interpolation; `False` = joint-space | | `interpolate_position_step` | `float` | `0.002` | Cartesian step size (meters) or joint step size (radians) | @@ -192,10 +235,14 @@ Convenience constructors: | `positions` | `torch.Tensor \| None` | Joint positions `(B, N, DOF)` | | `velocities` | `torch.Tensor \| None` | Joint velocities `(B, N, DOF)` | | `accelerations` | `torch.Tensor \| None` | Joint accelerations `(B, N, DOF)` | -| `dt` | `torch.Tensor \| None` | Per-step time durations `(B, N)` | -| `duration` | `float \| torch.Tensor` | Total trajectory time per env `(B,)` | +| `dt` | `torch.Tensor \| None` | Per-step arrival intervals `(B, N)`; required whenever `positions` is present | +| `duration` | `torch.Tensor \| None` | Read-only total trajectory time `(B,)`, derived as `dt.sum(dim=1)` | Helper: `PlanResult.is_all_success() -> bool` returns `True` only when every env succeeded. +`PlanResult` rejects positions with missing, malformed, or inconsistent timing. +A failed result may omit the trajectory entirely by leaving `positions=None`. +When `MotionGenerator` resamples a fully timed result, it preserves each row's +total duration and emits new explicit arrival intervals. ### MoveType enum @@ -221,11 +268,11 @@ Helper: `PlanResult.is_all_success() -> bool` returns `True` only when every env ### Registering a new planner -1. Create a `BasePlanner` subclass with a `plan()` method decorated with `@validate_plan_options`. +1. Create a `BasePlanner` subclass with a `plan()` method decorated with `@validate_plan_options`; every result containing positions must include `dt`, from which `duration` is derived. 2. Create a `BasePlannerCfg` subclass with a unique `planner_type` string. 3. Optionally create a `PlanOptions` subclass for planner-specific options. -4. For a planner that accepts live obstacles, set - `supports_collision_world_updates = True` and implement +4. For a planner that accepts live obstacles, override `collision_world_info` + with a `CollisionWorldInfo` whose `supports_updates=True`, and implement `with_collision_world()` without mutating caller-owned reusable options. 5. Register in `MotionGenerator._support_planner_dict`: ```python @@ -260,8 +307,19 @@ The decorator checks that every `PlanState` in `target_states` shares the same l - **IK interpolation with unsupported MoveType** — `strategy="ik_interp"` accepts only `EEF_MOVE` and `JOINT_MOVE` and raises for other target types. - **Missing interpolation inputs** — `strategy="ik_interp"` requires explicit - `start_qpos` and `sample_count`; it never reads live robot state implicitly. + `start_qpos`, `sample_count`, and `interpolation_dt`; it never reads live robot + state or guesses a command period implicitly. +- **Missing planner timing** — constructing a `PlanResult` with positions but + without `dt` raises immediately; `duration` is derived from `dt`. +- **CUDA requested on a CPU-only runtime** — planner success-mask normalization + raises a direct `ValueError` before querying the active CUDA device. It never + silently falls back to CPU. - **Constraint tolerance** — `is_satisfied_constraint` allows 10% velocity / 25% acceleration overshoot. Dense waypoint trajectories may appear to violate constraints but pass validation. - **Fork safety with GPU sim** — `ToppraPlannerCfg.mp_context=None` defaults to `spawn` on GPU to avoid fork-after-CUDA-init hazards. Force `fork` only when the sim device is CPU or you have verified it is safe. - **cuRobo shared-world mismatch** — World-frame poses may differ solely because replicated arenas are offset. Compare poses after robot-base rebasing: keep `multi_env=False` if they match, and enable it only when robot-relative layouts differ. -- **Dynamic obstacles silently stale** — A planner participates in atomic-action collision revision recovery only when it declares `supports_collision_world_updates=True`; its hook must bind every `collision_entity_id` pose into the current planning attempt. +- **Dynamic obstacles silently stale** — A planner participates in atomic-action collision revision recovery only when `collision_world_info.supports_updates=True`; its hook must bind every `collision_entity_id` pose into the current planning attempt. +- **Registry/planner identity drift** — Registry-backed cuRobo worlds must use a + canonical-ID mapping, not a list whose names are inferred from UIDs. Validate + exact full registry/planner collision-world agreement, dynamic + registry/provider/planner agreement, and batch-mode agreement through + `SceneRegistry` before starting execution. diff --git a/agent_context/topics/rl-learning/rl-learning.md b/agent_context/topics/rl-learning/rl-learning.md index bf53f73f1..0a330c349 100644 --- a/agent_context/topics/rl-learning/rl-learning.md +++ b/agent_context/topics/rl-learning/rl-learning.md @@ -4,262 +4,230 @@ | What | Path | |------|------| -| CLI entry | `embodichain/learning/rl/train.py` → `parse_args()` + `train_from_config(config_path)` | -| Trainer class | `embodichain/learning/rl/utils/trainer.py` → `Trainer` | -| Package init | `embodichain/learning/rl/__init__.py` — re-exports `algo`, `buffer`, `models`, `utils` | +| Unified CLI | `embodichain train-rl --config ` | +| CLI implementation | `embodichain/learning/rl/train.py` → `cli()` | +| Programmatic training | `embodichain/learning/rl/train.py` → `train_from_config()` | +| Algorithm registry | `embodichain/learning/rl/algo/__init__.py` | +| Policy registry | `embodichain/learning/rl/models/__init__.py` | +| Lightweight env registry | `embodichain/learning/rl/env.py` | +| Standard trainer | `embodichain/learning/rl/utils/trainer.py` → `Trainer` | +| Differentiable trainer | `embodichain/learning/rl/differentiable_trainer.py` | +| Official configs | `embodichain_tasks/configs/agents/rl/` | + +The compatibility module entry point is: -Run training: ```bash -python -m embodichain.learning.rl.train --config [--distributed | --no-distributed] +python -m embodichain.learning.rl.train --config ``` -## Overview - -The RL subsystem implements on-policy reinforcement learning with a modular pipeline: - -1. **Config** — JSON/YAML file defines `trainer`, `policy`, and `algorithm` blocks. -2. **Environment** — Simulator tasks use `build_env()`; lightweight tensor - tasks use the `learning_env` registry. Official tasks live in - `embodichain_tasks`. -3. **Policy** — Neural-network module (`Policy` ABC) producing actions from observations. -4. **Collector** — Steps the env, writes transitions into a preallocated `TensorDict`. -5. **Buffer** — `RolloutBuffer` owns the preallocated storage; marks it full after collection. -6. **Algorithm** — Consumes the rollout, computes losses, and updates policy weights. -7. **Trainer** — Orchestrates the collect → update → log → eval → checkpoint loop. - -Standard PPO/GRPO rollout data flows as `TensorDict` objects (from the -`tensordict` library). - -## Differentiable RL - -**Sources**: `env.py`, `collector/differentiable.py`, `algo/apg.py`, -`differentiable_trainer.py` - -- `DifferentiableVecEnv.step()` preserves gradients through rewards and the - differentiable next state. -- `detach_state()` creates the TBPTT boundary without resetting the environment. -- `DifferentiableCollector` keeps graph-connected transitions outside the - standard preallocated buffer and preserves the policy mode selected by its - caller. -- Differentiable policy sampling uses `get_differentiable_action()` and - `rsample()`. -- `APG` maximizes segmented discounted return with optional entropy, gradient - clipping, and non-finite update protection. -- `DifferentiableTrainer.segment_length` controls the TBPTT boundary, while - `update_horizon` independently controls environment steps per optimizer - update. Segment losses backpropagate immediately, state is detached after - each segment, and policy gradients accumulate until one update horizon is - complete. -- Checkpoints store policy, optimizer, optional LR scheduler, trainer counters, and best-evaluation - state. -- Training selects `policy.train()`, while evaluation temporarily selects - `policy.eval()` and restores the previous mode afterward. - -The collector paths are intentionally separate: -- PPO/GRPO use `SyncCollector` and `TensorDict`. -- APG uses `DifferentiableCollector` and `DifferentiableRollout`. - -APG is registered with `RolloutKind.DIFFERENTIABLE`. The shared `train.py` -entry point routes it to `DifferentiableTrainer`; PPO/GRPO declare -`RolloutKind.STANDARD` and continue to use `Trainer`. Collectors and rollout -types remain separate even though environment construction, policy building, -evaluation, logging, and checkpoint conventions are shared. - -`PointMassRL` in -`embodichain_tasks/embodichain_tasks/rl/basic/point_mass.py` is the canonical -dual-path task. The same differentiable PyTorch dynamics run under -`torch.no_grad()` for PPO and preserve the action-to-reward graph for APG. - -### Newton Reference - -`experimental/newton/planar_reach.py` is an FK-only two-link test environment. -It bridges reward, joint-state, and end-effector gradients from -`newton.eval_fk` and a Warp tape into PyTorch. It is not a dynamics simulator -or a task implementation for NMG. - -The training demo runs APG through `DifferentiableTrainer`, samples new initial -joints and FK-reachable targets for every update, then evaluates the learned -policy on held-out random seeds: +Prefer the unified `embodichain train-rl` command. -```bash -python -m embodichain.learning.rl.experimental.newton.train_planar_reach \ - --device cuda:0 -``` +## Configuration Resolution -With the default seeds, 600 updates improve the mean minimum distance from -`1.85` to `0.039` and reach an `86.7%` success rate across 512 held-out samples -at the `0.05` threshold. Tests also cover analytical FK, finite-difference -gradients, TBPTT, APG updates, and held-out improvement. +Training configs are JSON or YAML mappings with three top-level blocks: -## Architecture +- `trainer`: environment selection, runtime, rollout, evaluation, logging, + checkpoint, and distributed settings; +- `policy`: registered policy name and network definitions; +- `algorithm`: registered algorithm name plus its config mapping. -``` -train_from_config() - ├─ build_env() | build_learning_env() - ├─ build_policy(policy_block, ...) → Policy (ActorCritic | ActorOnly | custom) - ├─ build_algo(name, cfg, policy) → Algorithm (PPO | GRPO | APG) - └─ route by RolloutKind - ├─ STANDARD → Trainer + SyncCollector + RolloutBuffer - └─ DIFFERENTIABLE → DifferentiableTrainer + DifferentiableCollector -``` - -Both trainers call `evaluation.evaluate_episodes()` with an independent -evaluation environment. It counts actual completed episodes under asynchronous -auto-reset and logs terminal-only metrics as `eval/*`. +The resolution flow is: ```text -Trainer(policy, env, algorithm, ...) - ├─ RolloutBuffer [buffer/standard_buffer.py] - ├─ SyncCollector [collector/sync_collector.py] - └─ .train(total_timesteps) - loop: - _collect_rollout() → buffer.start_rollout() → collector.collect() → buffer.add() - algorithm.update(buffer.get()) - _log_train(losses) - _eval_once() (if eval_freq hit) - save_checkpoint() (if save_freq hit) +CLI --config + → load_config() + → trainer / policy / algorithm blocks + → choose trainer.learning_env or trainer.gym_config + → build environment + → build policy and optional MLP modules + → build algorithm config from the registry + → route by algorithm.rollout_kind + → train, evaluate, log, and checkpoint ``` -## PPO Algorithm - -**Source**: `embodichain/learning/rl/algo/ppo.py` - -- Config: `PPOCfg(AlgorithmCfg)` — `n_epochs=10`, `clip_coef=0.2`, `ent_coef=0.01`, `vf_coef=0.5`. -- Inherits `AlgorithmCfg` defaults: nested `optimizer` (`adam`, `lr=3e-4`), optional `lr_scheduler`, `batch_size=64`, `gamma=0.99`, `gae_lambda=0.95`, `max_grad_norm=0.5`. -- `update(rollout)` flow: - 1. `compute_gae(rollout, gamma, gae_lambda)` — writes `advantage` and `return` into the TensorDict. - 2. `transition_view(rollout, flatten=True)` — drops padded final slot, flattens to `[N*T]`. - 3. For `n_epochs` × minibatch iterations: - - Evaluate current policy: `policy.evaluate_actions(batch)` → `logprobs`, `entropy`, `values`. - - Clipped surrogate objective + value loss + entropy bonus. - - Adam step with `max_grad_norm` clipping. - -### GRPO Algorithm - -**Source**: `embodichain/learning/rl/algo/grpo.py` - -- Config: `GRPOCfg(AlgorithmCfg)` — `group_size=4`, `kl_coef=0.02`, `ent_coef=0.0`, `reset_every_rollout=True`, `truncate_at_first_done=True`. -- Maintains a frozen `ref_policy` deepcopy for KL penalty when `kl_coef > 0`. -- Requires `group_size >= 2` for within-group advantage normalization. - -### Algorithm Registry - -**Source**: `embodichain/learning/rl/algo/__init__.py` - -```python -_ALGO_REGISTRY = { - "apg": (APGCfg, APG), - "ppo": (PPOCfg, PPO), - "grpo": (GRPOCfg, GRPO), -} -build_algo(name, cfg_kwargs, policy, device, distributed=False) -``` +Read concrete values from the selected config and current config classes. Do +not assume example-config values are global defaults. -`APG.rollout_kind` is `RolloutKind.DIFFERENTIABLE`; PPO/GRPO use -`RolloutKind.STANDARD`. When `distributed=True`, wraps the policy in -`DistributedDataParallel` before passing to the algorithm and rejects -differentiable algorithms. +## Environment Paths -## Rollout Buffer +### Simulator Gym Environment -**Source**: `embodichain/learning/rl/buffer/standard_buffer.py` +Select this path with `trainer.gym_config`. -- `RolloutBuffer(num_envs, rollout_len, obs_dim, action_dim, device)`. -- Preallocates a single TensorDict with batch shape `[num_envs, rollout_len + 1]`. -- The `+1` slot holds the bootstrap observation/value; transition-only fields (`action`, `reward`, `done`) pad the final index. -- API: `start_rollout()` → returns the shared TensorDict for the collector to write into; `add(rollout)` → marks full; `get(flatten=True)` → returns transition view and clears. -- **Invariant**: the buffer holds at most one rollout at a time. Calling `start_rollout()` when full raises `RuntimeError`. +1. The CLI discovers installed task packages and executes their init hooks. +2. `train_from_config()` loads the gym config. +3. `config_to_cfg()` builds the environment config and manager functors. +4. Trainer runtime fields override simulation device, GPU, renderer, headless + mode, environment count, and optional profiling. +5. `build_env()` constructs the registered Gym environment. +6. A sample reset determines flattened observation and action dimensions. -### Buffer Utilities +Simulator environments use standard rollouts. A differentiable algorithm on +this path is rejected. -**Source**: `embodichain/learning/rl/buffer/utils.py` +Direct callers of `train_from_config()` that bypass `cli()` must ensure +task packages and init hooks needed by a simulator environment have already +been loaded. -- `transition_view(rollout, flatten)` — slices `[:, :-1]` on transition fields, optionally reshapes to `[N*T]`. -- `iterate_minibatches(rollout, batch_size, device)` — yields shuffled minibatches from a flattened rollout. +### Lightweight Learning Environment -## Actor-Critic Models +Select this path with `trainer.learning_env`, either as a registered name or +as a mapping with `name` and `cfg`. -**Source**: `embodichain/learning/rl/models/` +`build_learning_env()` resolves factories registered with +`@register_learning_env`. A learning environment implements the +`LearningVecEnv` protocol. Differentiable algorithms additionally require +`DifferentiableVecEnv.detach_state()` as the truncated-backpropagation +boundary. -### Policy ABC (`policy.py`) -- `Policy(nn.Module, ABC)` — requires `forward()`, `get_value()`, `evaluate_actions()`. -- `get_action()` — convenience wrapper calling `forward()` under `torch.no_grad()`. -- `get_differentiable_action()` — explicit graph-preserving action API; - implementations must provide reparameterized stochastic sampling. -- All methods consume and return `TensorDict`. +This path supports both standard algorithms and differentiable algorithms, +but currently rejects distributed training and environment profiling. -### ActorCritic (`actor_critic.py`) -- Gaussian policy with learnable `log_std` per action dim (clamped `[-5, 2]`). -- Requires externally injected `actor` and `critic` `nn.Module` instances. -- `forward(td)` → samples action from `Normal(actor(obs), exp(log_std))`, writes `action`, `sample_log_prob`, `value`. +## Rollout and Trainer Routing -### ActorOnly (`actor_only.py`) -- Same interface but `value` is always zeros (for algorithms like GRPO that don't use a critic). +`BaseAlgorithm.rollout_kind` is the routing contract: -### MLP (`mlp.py`) -- `MLP(nn.Sequential)` — configurable hidden dims, activation, LayerNorm, dropout, orthogonal init. - -### Policy Registry (`__init__.py`) -```python -_POLICY_REGISTRY: {"actor_critic": ActorCritic, "actor_only": ActorOnly} -build_policy(policy_block, obs_space, action_space, device, actor, critic) -build_mlp_from_cfg(module_cfg, in_dim, out_dim) # expects {"type": "mlp", "network_cfg": {...}} +```text +RolloutKind.STANDARD + → Trainer + → SyncCollector + → RolloutBuffer backed by TensorDict + → PPO or GRPO update + +RolloutKind.DIFFERENTIABLE + → DifferentiableTrainer + → DifferentiableCollector + → graph-connected DifferentiableRollout segments + → APG update ``` -## Training Pipeline - -**Source**: `embodichain/learning/rl/utils/trainer.py` - -`Trainer.__init__` creates `RolloutBuffer` and `SyncCollector`. - -`Trainer.train(total_timesteps)` loop: -1. `_collect_rollout()` — calls `buffer.start_rollout()`, then `collector.collect(buffer_size, rollout, on_step_callback)`, then `buffer.add(rollout)`. -2. `algorithm.update(buffer.get(flatten=False))` — algorithm decides its own flatten/GAE logic. -3. `_log_train(losses)` — writes to TensorBoard + optional W&B. -4. Periodic `_eval_once(num_episodes)` and `save_checkpoint()`. - -Distributed training: -- `train_from_config` initializes NCCL process group, offsets seed by rank. -- Only rank 0 creates log dirs, TensorBoard writer, and W&B. -- Timestamps are broadcast from rank 0 to ensure consistent run directories. - -### Collector - -**Source**: `embodichain/learning/rl/collector/sync_collector.py` - -`SyncCollector(env, policy, device, reset_every_rollout)`: -- `collect(num_steps, rollout, on_step_callback)` — steps env synchronously, writing obs/action/reward/done into the preallocated rollout TensorDict. -- Observations are flattened via `flatten_dict_observation()` before storage. -- Requires a preallocated rollout (`rollout=None` raises `ValueError`). - -`DifferentiableCollector(env, policy, device)`: -- Collects short segments without `torch.no_grad()` or preallocated copies. -- Returns `DifferentiableRollout` with immutable transition records. -- `rollout.rewards` stacks rewards as `[time, num_envs]` while retaining their - autograd history. -- `detach_state()` updates the collector to the environment's detached boundary - observation before the next segment. - -### Helper Utilities - -**Source**: `embodichain/learning/rl/utils/helper.py` - -- `flatten_dict_observation(obs: TensorDict)` → `[num_envs, obs_dim]` tensor. -- `dict_to_tensordict(obs_dict, device)` → converts env observation mapping to TensorDict. +PPO and GRPO use `STANDARD`. APG uses `DIFFERENTIABLE`. +`get_trainer_class()` centralizes this selection for lightweight learning +environments. + +The standard buffer reserves shape `[num_envs, rollout_length + 1]`; the +last slot holds the bootstrap observation/value while transition-only fields +use it as padding. The collector writes into the preallocated rollout and the +algorithm consumes it after collection. + +The differentiable path does not copy transitions into the standard buffer. +It preserves the action-to-reward autograd graph across short segments. +`segment_length` sets TBPTT boundaries, while `update_horizon` controls +how many environment steps contribute to one optimizer update. + +## Component Ownership + +| Component | Owner | +|-----------|-------| +| Algorithm base, rollout kind, optimizer scheduling | `algo/base.py`, `utils/optimizer.py` | +| PPO, GRPO, APG implementations | `algo/ppo.py`, `algo/grpo.py`, `algo/apg.py` | +| Standard rollout storage and views | `buffer/` | +| Standard and differentiable collection | `collector/` | +| Policy interface, actor-critic, actor-only, MLP builder | `models/` | +| Standard collect/update loop | `utils/trainer.py` | +| Differentiable TBPTT/update loop | `differentiable_trainer.py` | +| Shared completed-episode evaluation | `evaluation.py` | +| Learning environment protocol and registry | `env.py` | +| End-to-end config and runtime assembly | `train.py` | + +Rollout payloads on the standard path are `TensorDict` objects. Policies +consume observations and write action, log-probability, entropy, and value +fields needed by their algorithm. Differentiable policies must expose +graph-preserving action sampling. + +## Training and Evaluation Lifecycle + +The standard trainer repeats: + +1. start and collect a rollout; +2. update the algorithm; +3. log train metrics; +4. evaluate when the configured step boundary is reached; +5. save periodic and best-evaluation checkpoints. + +Evaluation uses an independent environment and +`evaluate_episodes()`. It counts completed asynchronous episodes, reports +terminal metrics, temporarily switches the policy to evaluation mode, and +restores its prior mode. + +Checkpoints include policy parameters, trainer counters, best-evaluation +state, and optimizer or LR-scheduler state when present. + +On the simulator path, distributed mode initializes NCCL, assigns one CUDA +device per local rank, wraps the policy in +`DistributedDataParallel`, aggregates step and episode statistics, and +keeps logging, evaluation, and checkpoint ownership on rank zero. +Differentiable algorithms and lightweight learning environments do not +currently support this distributed path. + +## Official Examples + +| Example | Environment path | Config location | +|---------|------------------|-----------------| +| CartPole | registered simulator Gym env | `embodichain_tasks/configs/agents/rl/basic/cart_pole/` | +| PushCube | registered simulator Gym env | `embodichain_tasks/configs/agents/rl/push_cube/` | +| PointMass PPO | registered lightweight env, standard rollout | `embodichain_tasks/configs/agents/rl/basic/point_mass/train_ppo.yaml` | +| PointMass APG | differentiable lightweight env | `embodichain_tasks/configs/agents/rl/basic/point_mass/train_apg.yaml` | +| Newton planar reach | experimental differentiable FK reference | `embodichain/learning/rl/experimental/newton/` | + +`PointMassRL` is the reference environment for comparing standard and +differentiable training over the same task dynamics. The Newton planar-reach +example is an experimental gradient reference, not a general simulator task. + +## Extension Points + +### Add an Algorithm + +1. Implement a `BaseAlgorithm` subclass and config under `algo/`. +2. Declare the correct `RolloutKind`. +3. Register the config/class pair in `algo/__init__.py`. +4. Add focused algorithm, routing, and rollout-contract tests. + +### Add a Policy + +1. Implement the `Policy` contract under `models/`. +2. Register it in `models/__init__.py`. +3. Ensure its outputs satisfy every intended algorithm. +4. Provide graph-preserving sampling if used with differentiable rollouts. + +### Add a Lightweight Environment + +1. Implement `LearningVecEnv`, or `DifferentiableVecEnv` for APG. +2. Register the factory with `@register_learning_env`. +3. Ensure finished rows auto-reset while returning terminal reward/done with + the next initial observation. +4. Add an official config under `embodichain_tasks/configs/agents/rl/` when + it is a bundled task. + +Use `add-task-env` for simulator-backed task environments and +`manager-functor` for their observation, reward, event, and action +components. + +## Invariants + +- The selected environment must expose batched observation/action spaces and + `num_envs`. +- Policy observation and action dimensions must match the built environment. +- An algorithm's `RolloutKind` must match its collector, rollout type, and + trainer. +- The standard buffer holds at most one unconsumed rollout. +- APG must retain differentiable rewards until its optimizer boundary; + `detach_state()` must not reset or resample the task. +- GRPO environment count must satisfy its grouping contract. +- Evaluation must use completed episodes and an independent environment. +- Only rank zero owns external logging and checkpoints in distributed runs. ## Common Failure Modes -| Symptom | Likely Cause | -|---------|-------------| -| `RuntimeError: RolloutBuffer already contains a rollout` | Called `start_rollout()` without consuming via `get()`. | -| `ValueError: Preallocated rollout batch size mismatch` | `buffer_size` in trainer config doesn't match `num_steps` passed to collector. | -| `ValueError: Algorithm 'X' not found` | Algo name not in `_ALGO_REGISTRY`. Check `get_registered_algo_names()`. | -| `ValueError: ActorCritic policy requires external 'actor' and 'critic' modules` | Config uses `actor_critic` policy but doesn't define `actor`/`critic` MLP blocks in the JSON. | -| `ValueError: Configured policy.action_dim=N does not match env action dim M` | `policy.action_dim` in config disagrees with the env's action manager. | -| `RuntimeError: torch.distributed is not initialized` | `distributed=True` but `init_process_group()` was not called (launch via `torchrun`). | -| `GRPO: group_size >= 2` | GRPO requires at least 2 environments per group for normalization. | -| NaN losses | Check `log_std` bounds, gradient clipping, and reward scale. `max_grad_norm` defaults to 0.5. | -| Stale observations after reset | `SyncCollector` resets obs via `_reset_env()` on init; set `reset_every_rollout=True` if episodes must fully reset between rollouts. | -| `Learning environment 'X' is not registered` | Task package was not imported. Call `discover_task_packages()` / `execute_init_hooks()`, or import the task module that uses `@register_learning_env`. | -| `Differentiable algorithms require trainer.learning_env` | APG cannot train simulator `gym_config` envs; use a registered `learning_env` such as `PointMassRL`. | -| Eval success rate stuck near zero | Confirm `evaluate_episodes` reads terminal-only `success`/`metrics`, and that the eval env uses a held-out `eval_seed`. | +| Symptom | Likely cause | +|---------|--------------| +| Algorithm or policy name is not found | Name is absent from the corresponding registry | +| Learning environment is not found | Its task package was not discovered or the module containing its decorator was not imported | +| Differentiable algorithm rejects the config | The config selected `trainer.gym_config` instead of a differentiable `learning_env` | +| Distributed training is rejected | The request uses a lightweight/differentiable path, or the process group/CUDA device is not initialized | +| Policy dimension mismatch | Policy config disagrees with the built environment's observation or action space | +| Standard buffer is already full | A rollout was started before the previous one was consumed with `get()` | +| APG gradients disappear | Actions were sampled under `no_grad`, transitions were copied/detached, or the state was detached too early | +| GRPO reshape or grouping fails | `num_envs` is not divisible by `group_size` | +| Evaluation never completes | The environment does not emit completed asynchronous episodes or terminal metrics correctly | +| Output/checkpoint directories diverge across ranks | Distributed run metadata was not coordinated through rank zero | diff --git a/agent_context/topics/simulation-system/simulation-system.md b/agent_context/topics/simulation-system/simulation-system.md new file mode 100644 index 000000000..cadacd54b --- /dev/null +++ b/agent_context/topics/simulation-system/simulation-system.md @@ -0,0 +1,147 @@ +# Simulation System + +## Entry Points + +| What | Path | +|------|------| +| Public simulation package | `embodichain/lab/sim/__init__.py` | +| World and scene owner | `embodichain/lab/sim/sim_manager.py` → `SimulationManager` | +| Global simulation config | `embodichain/lab/sim/sim_manager.py` → `SimulationManagerCfg` | +| Object and physics configs | `embodichain/lab/sim/cfg.py` | +| Gym lifecycle integration | `embodichain/lab/gym/envs/base_env.py` | +| Task scene construction | `embodichain/lab/gym/envs/embodied_env.py` | + +`embodichain.lab.sim` exports the manager, its config, shared material +types, `BatchEntity`, and the simulation profiler. Import a specialized +object, sensor, solver, planner, or atomic-action API from its own subpackage. + +## Ownership + +`SimulationManager` owns one DexSim `World`, its global environment, +parallel arenas, and the Python registries for scene resources: + +- rigid objects and rigid-object groups; +- soft and cloth objects; +- articulations and robots; +- rigid constraints, sensors, lights, gizmos, and markers; +- visual materials and texture caches; +- the optional browser-visualization runtime. + +The manager owns simulation resources and physics stepping. `BaseEnv` owns +the Gym reset/step contract. `EmbodiedEnv` translates task configuration +into robots, sensors, lights, backgrounds, articulations, and interactive +objects added through the manager. + +## Lifecycle + +The environment-owned lifecycle is: + +```text +EnvCfg.sim_cfg + → BaseEnv._setup_scene() + → SimulationManager(SimulationManagerCfg) + → create World, global environment, defaults, and N arenas + → EmbodiedEnv adds robot, objects, lights, and sensors + → initialize GPU physics after scene construction when using CUDA + → BaseEnv.step() + → preprocess/apply action + → SimulationManager.update(physics_dt, sim_steps_per_control) + → update task state, observations, rewards, and termination + → BaseEnv.reset() + → SimulationManager.reset_objects_state(env_ids) + → task episode-initialization hook + → BaseEnv.close() + → profiler report + → SimulationManager.destroy() +``` + +`BaseEnv._setup_scene()` temporarily constructs the manager headlessly so +the scene can be assembled before a native window is opened. It sets +`SimulationManagerCfg.num_envs` from `EnvCfg.num_envs`. + +`SimulationManager` enables physics, selects manual physics updates, creates +the configured arenas, installs default plane/background/lighting resources, +and starts configured visualization during initialization. A Viser backend +forces `headless=True`; Viser and the native DexSim window are mutually +exclusive. + +`SimulationManager.update()` initializes GPU physics lazily if needed and +then advances the world for the requested number of physics steps. Each +environment control step normally calls it with +`sim_steps_per_control`. + +## Module Boundaries + +| Area | Owner | Routed topic | +|------|-------|--------------| +| World, arenas, asset registries, physics update, cleanup | `sim_manager.py` | `simulation-system` | +| Shared object, render, physics, drive, and URDF configs | `cfg.py` | `configclass-pattern` for config mechanics | +| Rigid, deformable, articulation, robot, light, constraint, gizmo | `objects/` | `robot-system` for robots | +| Camera, stereo camera, contact sensor | `sensors/` | `sensor-system` | +| Robot-specific configuration | `robots/` | `robot-system` | +| Inverse kinematics | `solvers/` | `ik-solvers` | +| Trajectory and motion generation | `planners/` | `motion-planning` | +| Typed action planning and execution | `atomic_actions/` | `atomic-actions` | +| Semantic scene and robot skill bindings | `skills/` | `atomic-actions` | +| Reachability analysis and runtime workspace queries | `workspace/` | `robot-system` | +| Browser scene export and Viser runtime | `embodichain/lab/visualization/` | `sim-visualization` | + +Use the narrow topic when a request names one of these subsystems. Use +`simulation-system` for the overall `lab/sim` architecture, manager +lifecycle, scene ownership, or cross-module flow. + +## Configuration Flow + +`SimulationManagerCfg` owns window size, headless mode, rendering, GPU/CPU +selection, arena count and spacing, physics timestep, physics and GPU-memory +settings, recording, profiling, and browser visualization. + +`EnvCfg` embeds `SimulationManagerCfg` and supplies the control-to-physics +step ratio. CLI and task config loaders may override runtime fields before +constructing the environment. Trace those overrides through the caller rather +than changing a default in the manager blindly. + +Object-specific configuration belongs in `lab/sim/cfg.py` or the +corresponding robot/sensor module. Scene composition belongs in +`EmbodiedEnv` or a task config, not in `SimulationManagerCfg`. + +## Where to Make Changes + +| Change | Primary location | +|--------|------------------| +| Global world, renderer, device, arena, or physics lifecycle | `sim_manager.py` | +| Shared object or physics config type | `cfg.py` | +| Add/get/remove behavior for a scene entity | `sim_manager.py` plus its `objects/` implementation | +| Task scene composition | `embodied_env.py` or the task config | +| Environment timing, reset, or control-step behavior | `base_env.py` and `env-framework` | +| Robot, sensor, solver, planner, or atomic action | Follow the corresponding routed topic and add-* skill | +| Browser visualization | `embodichain/lab/visualization/` and `sim-visualization` | + +## Invariants + +- Configure `num_envs`, device, renderer, and physics settings before + constructing `SimulationManager`. +- Treat resource UIDs as registry identities; retrieve and mutate resources + through the manager instead of maintaining a parallel scene registry. +- Keep batched object and sensor state aligned with the manager's arena count. +- Build scene assets before explicitly initializing GPU physics. The manager + will warn and initialize lazily on the first update if this was missed. +- Manual update is the default; normal environment stepping must advance + physics through `SimulationManager.update()`. +- Reset only the requested environment rows and honor + `excluded_uids` for resources detached from automatic reset. +- `destroy()` queues deferred cleanup. Tests and non-exiting standalone + callers that use `exit_process=False` must call + `SimulationManager.flush_cleanup_queue()`. + +## Common Failure Modes + +| Symptom | Likely cause | +|---------|--------------| +| Scene resource cannot be found or the wrong object is returned | UID mismatch or code bypassed the manager registry | +| CUDA physics data is stale on the first step | GPU physics was initialized before all assets were added, or not initialized explicitly | +| Native window does not open | `headless=True`, often forced by the Viser backend | +| Device and renderer use the wrong GPU | `sim_device` and `gpu_id` disagree; the device index takes precedence for CUDA simulation | +| Simulation advances at the wrong control rate | `physics_dt` and `sim_steps_per_control` were configured inconsistently; see `env-framework` | +| A test leaks a DexSim world | `destroy(exit_process=False)` was called without flushing the cleanup queue | +| Python exits during cleanup | `destroy()` used its process-exit default; pass `exit_process=False` for embedded or test lifecycles | diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index 66c78b4e4..8c140b707 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -1,10 +1,29 @@ # Declarative Expert Programs and Unified Semantic Skill Runtime -- Status: design plan -- Baseline: `main@26b69c22d7efbf96cb35f5487f6922c8645f91d7` -- Last updated: 2026-08-10 +- Status: core contracts are implemented through Phase 7 on stacked feature + branches. A real CUDA/cuRobo dynamic-obstacle recovery gate is landed and + runs conditionally when cuRobo is installed, CUDA is available, and GPU/slow + tests are explicitly enabled. Open Drawer has completed its + supported-simulation physical run; repeated cube pick/place now completes all + three independently observed Pick/Place/settle/validator cycles. A physical + gripper-command fault gate also proves held-object loss, symbolic invalidation, + real re-acquisition, and retry without simulator-side repair. Dual-UR5/PGI + HandOver has completed three consecutive + supported-simulation Pick/transfer/settle/validator runs using contact + dynamics only. Named trajectory-segment effect gates now block Pick lift, + Place retract, and HandOver source release until fresh physical evidence + confirms the required acquisition or release. Preset-owned, row-local + workflow recovery now executes a real re-acquisition `Pick` when the source + relation is lost, or directly retries the failed semantic call when verified + state proves the source relation remains. +- 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) +- Related implementation: + [#475](https://github.com/DexForce/EmbodiChain/pull/475), + [#517](https://github.com/DexForce/EmbodiChain/pull/517), + [#487](https://github.com/DexForce/EmbodiChain/pull/487) ## 1. Executive summary @@ -42,11 +61,13 @@ same layer and run through one runtime built on `ExecutionRunner`. The target authoring cost is: -- a new task that uses existing semantic capabilities: scene configuration, - Expert Program configuration, and optionally a declarative validator; +- a new task that uses existing semantic capabilities: Expert Program + configuration plus typed scene/profile integration declarations, and + optionally a declarative validator, with no task-specific motion code; - a new robot: one reusable `RobotSkillProfile`, not task-specific motion code; -- a genuinely new physical interaction: one reusable semantic skill/compiler/ - monitor implementation, after which tasks use it from configuration. +- a genuinely new physical interaction: one reusable capability bundle + containing its semantic skill/compiler/monitor and controller integration as + applicable, after which tasks select it through program and integration data. This design preserves the core direction of #471. Issue #474 changes the middle of the architecture: ordinary configuration must describe semantic @@ -63,7 +84,7 @@ sessions, or verifiers. verification, and recovery path. 3. Make object identity, observation, geometry, affordance, and collision data come from one scene registry. -4. Infer robot-part bindings and stable runtime policies from reusable profiles; +4. Infer robot-resource bindings and stable runtime policies from reusable profiles; require explicit choices only when the request is genuinely ambiguous. 5. Preserve lazy observation and per-environment recovery for programs whose later goals depend on earlier physical effects. @@ -88,8 +109,12 @@ sessions, or verifiers. ## 4. Baseline on current `main` -This plan is based on commit `26b69c22` rather than uncommitted working-tree -changes. +This plan is updated against committed `main@bcccb787` after PRs #475 and #476. The +implementation series is stacked from that baseline: PR1 is complete on +`refactor/atomic-actions-phase0`, PR2A is implemented by +`feat/atomic-action-pr2a-scene-registry`, and PR2B is implemented by +`feat/atomic-action-pr2b-robot-skill-profile`. These status statements do not +imply that the stacked changes have landed on `main`. | Capability | Current main | Design consequence | |---|---|---| @@ -97,24 +122,33 @@ changes. | Lazy `DemoSegment` execution and legacy demo compatibility (#460) | Available | Use a thin demo adapter; do not create a second dataset executor. | | Closed-loop `ExecutionRunner` and simulator ports (#449) | Available | `SkillRuntime` wraps/reuses the runner rather than scheduling commands itself. | | Dynamic scene recovery and `DynamicCollisionMode` (#450) | Available | Profiles select precise collision semantics and fail early when required capabilities are unavailable. | -| Environment cadence through `BaseEnv.step_dt` (#472) | Available | Expert configuration does not expose a separate control period. | +| Refined planning architecture (#475) | `MotionGenerator.generate()` is the single planning facade; each `ActionPlan` owns one trajectory and one recovery boundary; named `TrajectorySegment`s are metadata | Do not reintroduce `TrajectoryBuilder`, `MotionPlanningAdapter`, or trajectory-segment recovery. | +| Environment cadence through `BaseEnv.step_dt` (#472) | Available; planner and action trajectories now require explicit timing | Expert configuration, `MotionPolicy`, and the engine do not own a fallback period. Environment integrations put `BaseEnv.step_dt` on `PlanningContext` only for action-owned interpolation. | | Adaptive dynamic-object settling (#470) | Reset/event implementation exists | Extract a reusable monitor; demo post-policies must advance through `env.step()`. | +| Authoritative scene registry (#487) | Foundation available; official environments not migrated | Reuse it from the semantic compiler and opt in task scenes explicitly. | +| Declarative robot skill profiles (#487) | Foundation available; official profiles not yet installed | Bind reusable embodiment profiles through the semantic integration layer; put optional backend compatibility in `SkillPolicyPreset.required_planner`, not `MotionPolicy`. | | Repeated cube pick/place demo | Manually constructs invocations and transform math | First configuration-only vertical slice. | | Open Drawer task (#473) | Manually builds approach, grasp, pull, and command trajectories | Evidence that the semantic layer needs articulation/link/affordance references and a reusable articulation skill. | | Action Bank | Configuration plus task-specific Python node/edge functions | Keep only as a compatibility path while semantic coverage is built. | -Several #474 findings remain prerequisites on this baseline: +PR #475 resolved cumulative translation/rotation publication, removed the dead +`MotionPolicy.interpolation` field, and unified strategy dispatch. It also made +`AtomicAction.plan()` framework-owned and `_plan()` the only custom-action +extension hook. Rejecting a subclass that overrides `plan()` is an intentional +hard break: the project will not provide a compatibility adapter or deprecation +window for that former extension contract. Custom actions must migrate to +`_plan()` so framework-owned scene binding cannot be bypassed. + +Phase 1 closes the core identity, registry-backed collision integration, and +embodiment-owned capability/profile prerequisites. The remaining #474 work is +adoption and semantic orchestration: -- `RigidObjectSceneProvider` still updates its pose baseline on every snapshot, - so repeated sub-threshold movement may never publish a revision. -- `AtomicAction` rejects the formerly documented `plan()` extension override - and requires `_plan()` without a compatibility window. -- scene pose, semantics, affordance, and collision registration still have - multiple sources of truth; - ordinary callers still see a large low-level public surface and must perform semantic transform and verifier plumbing; -- `MotionPolicy` still exposes implementation-level tuning, including an - unused/misleading interpolation option. +- official environments do not yet provide authoritative registry population; +- official robot configurations do not yet install reusable skill profiles; +- named presets are not yet selected by a semantic facade/runtime; and +- effect monitoring and the configuration/demo path remain later-phase work. One #474 finding has changed since its review branch: the ambiguous `collision_check` switch has been replaced by `DynamicCollisionMode.OFF`, @@ -130,9 +164,12 @@ The following #471 decisions remain valid: - lazy re-observation when later goals depend on physical effects; - distinct action-effect verification, segment post-policy, and task-level validation responsibilities; -- named phases instead of trajectory indices; +- stable named trajectory segments for tracing instead of recomputed trajectory + indices; - sequential execution first, then resource-aware parallel execution; -- continued legacy compatibility during migration. +- continued Action Bank compatibility and only the explicitly documented + direct-core fallbacks during migration. This does not include the intentional + `plan()` to `_plan()` hard break. The following parts must be adjusted: @@ -145,6 +182,22 @@ The following parts must be adjusted: | Callers may supply place EEF poses and pickup look-ahead options. | `Place` is object-centric; the compiler derives EEF targets from verified held state and propagates downstream targets automatically. | | Configuration and handwritten code are separate entry paths. | Both construct the same semantic call specification and converge before binding or grounding. | +### 5.1 Segment terminology after #475 + +The design uses three different segment layers. Bare "segment" should be +avoided wherever the layer would be ambiguous. + +| Term | Type | Meaning | +|---|---|---| +| Program segment | `SegmentCfg` | Expert Program logical transaction boundary; owns post-policies, validators, and re-observation semantics. | +| Demo segment | `DemoSegment` | Lazy Gym/demo executor carrier and dataset boundary produced from a program segment. | +| Trajectory segment | `TrajectorySegment` | Named half-open waypoint range within one `ActionPlan`; used for inspection, visualization, tracing, and terminal-effect correlation only. | + +A trajectory segment is not an independent planning, recovery, effect, or +timeout boundary. One atomic action remains the recovery/effect boundary. +"Phase 0" through "Phase 8" below refer only to implementation-plan stages; +atomic motion structure is called a trajectory segment, not a phase. + ## 6. Proposed architecture ### 6.1 API layers @@ -191,10 +244,13 @@ callers. #### Core/advanced layer -The current `ActionGoal`, `ActionInvocation`, `ActionBinding`, policies, -`PlanningContext`, `ActionPlan`, `ExecutionSession`, `ExecutionRunner`, and -provider protocols remain available for framework authors and unusual -integrations. They are no longer prerequisites for ordinary task authoring. +The current action-owned goal dataclasses, `ActionInvocation`, generic endpoint +`ActionBinding`, policies, `PlanningContext`, `ActionPlan`, +`ExecutionSession`, `ExecutionRunner`, and provider protocols remain available +for framework authors and unusual integrations. `ActionPlan.commands` is the +runtime authority; a joint-backed plan may additionally retain a +`TimedTrajectory` for joint feedback and offline compilation. These contracts +are no longer prerequisites for ordinary task authoring. ### 6.2 Proposed package ownership @@ -211,7 +267,9 @@ embodichain/lab/sim/skills/ effects.py # built-in EffectMonitor contracts/implementations embodichain/lab/sim/atomic_actions/ - ... # existing typed core and built-in atomic planners + runtime_commands.py # transport-neutral endpoint payloads and timed frames + transports.py # endpoint transport protocol and exact-ID router + ... # typed core and built-in atomic planners embodichain/lab/gym/envs/expert_program/ cfg.py # strict @configclass schema @@ -250,48 +308,220 @@ SceneEntityRef Rules: -1. An entity is registered once. Planner obstacles, scene dependencies, effect - monitors, and semantic calls consume that registration. -2. Grounding reads pose and geometry from one immutable snapshot. It must not - mix a snapshot with a live simulation entity pose. -3. Automatic grasp selection declares a target dependency automatically. -4. Dynamic collision setup is derived and cross-validated at construction - time. The `safe` preset requests `DynamicCollisionMode.REQUIRED` when the - registry declares dynamic collision entities and fails early if the active - planner cannot satisfy it. -5. Environment scene configuration should populate the registry automatically; - explicit providers are reserved for perception and hardware integration. +1. The registry ID is the authoritative entity identity used by semantic calls, + snapshots, scene dependencies, effect monitors, and planner obstacles. An + entity is registered once under that ID. +2. A simulation object's existing `uid` may be imported as a legacy alias only. + Aliases are resolved once at an integration boundary and normalized to the + registry ID; they never replace the authoritative ID. Duplicate registry IDs, + ambiguous aliases, or an alias colliding with another registry ID fail during + registry construction. + For links and affordances, one typed `(parent, native_name)` physical source + may have only one canonical ID; changing the canonical spelling does not + create a second entity. +3. Grounding reads dynamic pose/confidence from one immutable snapshot and + static geometry/affordance metadata from the immutable registry that + produced it. It must not mix a snapshot with a live simulation entity pose. +4. Automatic grasp selection declares a target dependency automatically. +5. Collision setup is derived from authoritative registry IDs. + `collision_geometry_by_id()` derives planner geometry while excluding + non-collision registrations, and `make_planning_scene_provider()` performs + the complete provider/planner cross-validation. The registry's full + `STATIC ∪ DYNAMIC` collision ID set must exactly equal the planner's complete + collision-world ID set. Within it, the registry's dynamic subset, the + provider's `collision_entity_ids`, and the planner's dynamic-obstacle IDs must + also agree exactly. Every collision ID must have the required geometry, and + the selected planner must support the declared dynamic update mode. Aliases + are resolved before these contracts are constructed, never inside the + planner. The current + planner-local name check remains a lower-level defensive validation, not the + integration contract. +6. The `safe` preset requests `DynamicCollisionMode.REQUIRED` when the registry + declares dynamic collision entities and fails early if the active planner + cannot satisfy it. +7. Environment scene configuration opts into registry population explicitly; + it is not inferred by scanning the simulation. Explicit providers remain + available for perception and hardware integration. + +PR2A fixes three public identity and collision-world choices: + +- **Authoritative planner IDs:** registry-backed cuRobo configuration passes an + explicit `registry_id -> RigidObject` mapping derived by + `collision_geometry_by_id()`. Mapping keys are canonical logical/source IDs + for cache identity and the complete registry/planner collision-world + contract. Cuboid and mesh worlds also use them unchanged as physical YAML + obstacle names and runtime pose-update keys. A static sphere source expands + to derived physical names such as `registry_id_0`; dynamic sphere worlds are + rejected. A registry mapping with geometry missing for the selected + representation fails fast instead of silently omitting that source. The list + form remains an advanced direct-core path and continues to derive names from + simulator UIDs or fallback names. +- **Flat reference IDs:** object, articulation, link, and affordance IDs share + one globally unique flat namespace. Link and affordance ancestry is stored in + `SceneEntityRegistration.parent`; callers do not encode hierarchy into an ID. + Within one reference type, the same `(parent, native_name)` cannot be assigned + more than one canonical ID. +- **Explicit vectorized-world semantics:** one-environment dynamic collision + setup may infer a shared collision world. A multi-environment registry with + dynamic collision entities must explicitly select shared or per-environment + collision worlds, and integration validation requires the planner mode to + match that selection. + +PR1 provides the core migration bridge consumed by the registry. +`ObjectSemantics.entity_id` remains a string lowering target in the typed core; +the canonical semantic path obtains that value from a resolved +`SceneEntityRef`. `ObjectSemantics` is shallow-frozen so top-level fields, +including `entity_id`, cannot be rebound after attachment state captures the +semantics; identity changes require a new instance. Nested affordance and +metadata objects remain mutable but never establish identity. + +For object identity, explicit and legacy namespaces stay separate. If either +side supplies `entity_id`, both sides must supply the same explicit ID; a +same-spelled simulation `entity.uid` is not sufficient. Only when both explicit +IDs are absent may the bridge compare non-empty legacy UIDs, requiring both UIDs +to exist and match. Only when neither side has an explicit ID or valid UID may +comparison fall back to the same semantic object or live entity handle. +Semantic labels are never identity. Arbitrary alias mapping, uniqueness +enforcement, and normalization to an authoritative registry ID belong to PR2A. + +For pose grounding, an explicit `entity_id` is strict: the pose comes only from +the current versioned `PlanningContext.scene`, and a missing entry is an error. +The planner never falls back to a live entity after an explicit ID fails. A live +`ObjectSemantics.entity` read remains temporarily available, with a deprecation +warning and without a scene dependency, only when no `entity_id` was supplied. +The same boundary applies to `AssembleGoal.base_pose`: the snapshot reference is +canonical, while an omitted reference permits the deprecated direct-core +`AssembleAffordance.base_object_entity` path. + +PR2A hardens `SceneSnapshot` at the public boundary. Construction owns a copy of +every dynamic `EntityState`, and every public entity lookup returns a defensive +copy, so mutating an input state or a previously returned pose cannot mutate the +published snapshot. The registry continues to own static integration metadata, +including typed identity, aliases, parent relationships, geometry, collision +role, dynamics classification, semantic type, and affordances. A snapshot owns +only versioned dynamic pose/confidence plus collision revision metadata; it does +not duplicate the registration catalog. ### 7.2 Robot skill profiles -A `RobotSkillProfile` is reusable per embodiment and contains: - -- capability declarations for arms, grippers, hands, and tools; -- mappings from semantic roles to compatible control parts; -- semantic commands such as `open`, `grasp`, `release`, and `ready`; -- available planners/motion strategies and their constraints; -- default grasp, effect-monitor, and runtime preset selections; -- optional preference rules when more than one binding is valid. - -The compiler resolves the only valid binding automatically. If two arms are -equally valid and the profile has no deterministic preference, validation asks -for a semantic choice such as `arm: left`; it never asks the task to construct -an `ActionBinding`. +A `RobotSkillProfile` is reusable per embodiment, but its resource model is not +an `arm + tool` schema. It contains a generic resource DAG: + +- each `RobotResource` has a stable logical ID, zero or more named execution + endpoints, and optional member resources; +- each endpoint declares open, namespaced capabilities explicitly and lowers + through a `ResourceEndpoint` implementation; `ControlPartEndpoint` is the + current joint/control-part declaration, while registered + `ResourceEndpointAdapter`s resolve any endpoint kind into generic + `EndpointResolution` metadata (a typed runtime target, command-profile key, + physical claim tokens, and optional joint IDs) without changing the graph, + matcher, or slot model. Adapters register by exact endpoint type; the + built-in control-part adapter is not overrideable, and different controller + semantics use a new endpoint subtype; +- members describe physical composition and claim closure, not capability + inheritance. A composite must explicitly declare `motion.whole_body`; it + does not acquire that capability because it contains a base, torso, or arms; +- semantic control commands such as `open`, `grasp`, or a future `stop` remain + embodiment data owned by generic profile IDs selected by each endpoint + adapter; only the current core bridge lowers applicable profiles to robot + control-part keys; +- versioned `SkillPolicyPreset` values own motion, atomic recovery, bounded + workflow recovery, and runner policy; +- per-skill defaults map every skill-local slot to one resource ID. + +Resource and endpoint declarations are owned snapshots. A custom endpoint with +non-trivial nested payloads implements `snapshot()` to return an independent +value of its exact type, so caller-owned mutation cannot rewrite a bound +profile. + +Skills own the robot-independent half of the contract. A concrete atomic action +must explicitly publish a `SkillBindingContract`; inheriting the default +`primary` role or inheriting another action's contract does not expose a new +semantic skill. The contract declares skill-local participant slots and the +endpoint requirements inside each participant. For example, `pick_up` has one +`primary` participant with a `motion` endpoint and a `grasp` endpoint. A profile +can satisfy it with `left_actor`, whose endpoints lower to `left_arm` and +`left_hand`. Selecting the participant as one unit prevents invalid cross-side +combinations such as `left_arm + right_hand`. + +Endpoint names are local protocols, not global robot-part categories. A future +`navigate` skill can require `body.motion: motion.base.se2`; a +`whole_body_reach` skill can require `body.motion: motion.whole_body`. Neither +requires new `RobotSkillProfile` fields. Profile resolution lowers every +required endpoint directly into an engine-owned `ActionBinding` keyed by +`(slot_id, endpoint_id)` and carrying its typed runtime target; there is no +arm/tool-shaped intermediate binding layer. + +Binding follows strict rules: + +1. Filter each slot by endpoint presence, all required capabilities, typed + semantic commands, explicit caller selection, and installed endpoint + support. +2. Apply explicit physical-claim constraints. Built-in manipulation contracts + declare their `motion` and `grasp` views disjoint, while coupled whole-body + views may overlap when the skill omits that constraint. Multi-participant + contracts such as handover use pairwise-disjoint resource claims. +3. No candidates means the skill is unsupported on this profile and is omitted + from the profile-backed semantic catalog. +4. One complete candidate is selected automatically. +5. Multiple candidates are resolved only by a complete, still-valid per-skill + default or enough explicit slot selections. Partial defaults, mapping order, + and lexical order never break ambiguity. + +`ResourceClaim` contains transitive leaf-resource IDs, concrete joint IDs, and +adapter-defined physical/controller claim tokens. It makes `whole_body` +conflict with `base`, `torso`, or a contained arm even when the underlying +`Robot.control_parts` names are different, and lets a non-joint base adapter +claim a controller without inventing joints. PR2B +exposes deterministic claim/conflict data only. PR2C runners emit endpoint +command frames and transports own target-scoped safe holds, but claims still do +not imply safe parallel execution. Parallel scheduling still requires one +coordinator, deterministic command arbitration/merge, planner serialization or +isolation, cancellation semantics, and inter-trajectory collision checks. + +`AtomicActionEngine.actions` remains the direct-core implementation registry. +`engine.skills` contains only installed, agent-visible actions whose concrete +class explicitly declares a binding contract. A bound profile filters that +catalog again by the current robot resources. Constructing an engine with +`skill_profile=...` installs the profile's command snapshots as the single +authoritative source and binds the validated profile after built-ins load. +Known FK/IK capabilities on the control-part adapter are checked against the +selected control part's configured solver; Cartesian motion is not equated with +solver presence because native planners may provide it directly. Profile joint +commands must be one-dimensional and broadcastable; per-environment values +belong in invocation overrides. ### 7.3 Semantic call specification Version 1 should provide first-class calls for: -- `Pick(object, grasp?, arm?)`; -- `Place(object, pose?|on?|in?, arm?)`; -- `HandOver(object, receiver?, final_target?)`; +- `Pick(object, grasp?, resources?)`; +- `Place(object, pose?|on?|in?, resources?)`; +- `HandOver(object, final_target?, resources?)`; - a registered semantic call for shared extensions. +`resources`, when present, is a mapping from the selected skill's local slot +IDs to profile resource IDs (for example, `{"primary": "left_actor"}` or +`{"body": "mobile_base"}`). It is an explicit ambiguity override, not a +fixed arm/tool field. Ordinary calls omit it and use unique or profile-default +resolution. Within one analyzed workflow, an omitted `Place.primary` or +`HandOver.source` inherits the known resource holding that object. Explicit +consumer selections remain authoritative constraints and fail if they conflict +with the known holder; inference never crosses a registered-call boundary. + `Place` consumes verified held-object state. The compiler computes the release EEF pose from the requested object-space target and the verified `object_to_eef` relation. Task code and configuration never perform `desired_object_pose @ object_to_eef`. +As the core migration path for assembly, `AssembleGoal` gains +`base_pose: SceneEntityPose | None`. The semantic compiler always supplies a +`SceneEntityPose` containing the authoritative base-object registry ID, so the +base pose is resolved from the same immutable snapshot and automatically becomes +a scene dependency. `None` preserves the existing live +`AssembleAffordance.base_object_entity` lookup only for legacy direct-core +callers; the semantic facade and Expert Program never emit that fallback. + The workflow compiler inspects later calls and propagates downstream object targets to pickup/grasp selection. The caller does not repeat later goals in `PickUpOptions`. @@ -304,7 +534,8 @@ Compilation has two stages. - validate references, presets, capabilities, resources, and bounded loops; - infer ordering and data/effect dependencies; - propagate downstream object goals for grasp selection; - - identify static stages versus observation-dependent boundaries; + - identify every call boundary that requires fresh observation or verified + effects, without coalescing calls in Version 1; - reject ambiguous bindings and unsupported semantic relations before execution. 2. **Runtime grounding and lowering** @@ -314,11 +545,18 @@ Compilation has two stages. - lower to a typed `ActionInvocation`; - dispatch through the canonical `SkillRuntime`. -Static `engine.compile()` is valid only when later goals do not depend on -observations or effects produced by earlier calls. `engine.start()` and observed -execution are required for grasp/release verification, moving targets, -recovery, post-settling, or any JIT-grounded goal. The default mode is `auto`: -the compiler partitions safe static stages and inserts observed boundaries. +Version 1 executes exactly one semantic call per `ExecutionSession`. The runtime +captures a fresh registry snapshot, lowers one call to one `ActionInvocation`, +constructs a one-invocation session, drives it through terminal effect +verification, commits the verified per-environment task state, and only then +advances to the next call. It never places multiple semantic calls in one +`ExecutionSession`. + +Static `engine.compile()` remains an advanced core API for explicitly +observation-independent offline planning. The Version 1 semantic runtime does +not coalesce calls into static stages; such an optimization requires a later +design proving that it preserves the call, effect, and re-observation +boundaries. ### 7.5 Skill runtime @@ -328,10 +566,11 @@ the compiler partitions safe static stages and inserts observed boundaries. - synchronous `run(...)` and non-blocking `step()` entry points; - planning-context refresh through registered observation ports; - JIT lowering of the next semantic call; +- exactly one semantic call and one invocation per `ExecutionSession`; - persistent, per-environment verified `TaskState`; - built-in effect-monitor selection and feedback to `ExecutionSession`; - uniform `SkillResult`, cancellation, timeout, and safe-stop behavior; -- semantic and named-phase events. +- semantic action events and optional trajectory-segment trace metadata. Catalog discovery and runtime installation should have distinct names. For example, a catalog can `discover` a descriptor while an engine explicitly @@ -353,6 +592,90 @@ 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 trajectory +segment, observes segment-scoped held-object invariants from fresh physical +evidence, and +applies removal-only ``StateDelta`` reconciliation to failed rows before any +retry or recovery hand-off. The monitor publishes one current-observation +outcome per physical expectation, including the stronger proof that every +clause reached its inverse band. ``Pick`` can use the existing bounded action +retry. ``Place`` retries only when that complete inverse proof shows the source +is still attached; otherwise it invalidates the relation and emits a typed +``RECOVERY_REQUIRED`` boundary. ``HandOver`` always hands terminal failure to +workflow recovery, retaining the source relation only when complete inverse +evidence proves it is still attached. A verifier selects row-local retry versus +external recovery, but the core owns the removal-only invalidation delta and +applies it before either path. Evidence that remains unresolved at the action +deadline is reconciled fail-closed: any active verified state covered by the +pending effect is removed before external recovery. Workflow-level +re-acquisition is owned by the same ``SkillRuntime`` and may not repair the +scene implicitly. ``SkillPolicyPreset`` schema version 3 adds a +``WorkflowRecoveryPolicy`` whose per-row attempt budget defaults to zero. The +runtime consults it only after the atomic core emits ``RECOVERY_REQUIRED``. A +row whose reconciled ``TaskState`` still proves the source held-object relation +retries the failed semantic call from a fresh observation. A row whose source +relation was invalidated executes a real semantic ``Pick`` using the failed +call's resolved source resource, then retries the original call. Each recovery +call receives normal analysis, grounding, planning, command dispatch, physical +effect verification, and trace metadata; it is not a state edit or simulator +repair. Attempts are bounded independently per row, while already successful +rows wait at the existing shared call barrier. This is runtime policy, not an +Expert Program ``Retry`` node or a second workflow executor. + +Blocking physical-effect gates are enforced at named trajectory-segment +entries. ``Pick`` requires destination attachment before ``lift``; ``Place`` +requires source detachment before ``retract``; and ``HandOver`` requires +destination attachment before the source ``release`` segment. While a gate is +unresolved, the session does not advance its waypoint cursor and replays the +preceding command for the complete synchronized active cohort, so gripper +preload or open intent remains active under real dynamics. Gate success only +unlocks motion and never commits ``TaskState``; terminal effect verification +remains authoritative. Contradiction uses the enclosing action's bounded retry +policy, stale request IDs are rejected, and the action deadline covers gate +polling. Every gate owns a fresh monitor instance independent from the terminal +monitor and in-flight loss guard. For handover, terminal 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 @@ -462,13 +785,14 @@ examples. Stable names should be preferred over internal fields: ```yaml advanced: - phase_presets: - secure_grasp: precise + call_presets: + pick: precise recovery_preset: dynamic_scene ``` Raw planner instances, callables, arbitrary imports, and environment paths are -never serializable configuration values. +never serializable configuration values. Version 1 does not attach motion or +recovery policies to individual `TrajectorySegment`s. ## 9. Demonstration execution semantics @@ -479,11 +803,11 @@ Gym-aware runtime ports: - observation provider: captures a current planning context from the environment and scene registry; -- command sink: buffers the next full-robot command for the environment action - manager; +- command sink: buffers the next transport-neutral endpoint-command frame for + the environment action manager; - clock: advances only when the demo executor calls `env.step()`; -- metadata sink: records compiler decisions, phases, effects, recovery, scene - revisions, and post-policy results. +- metadata sink: records compiler decisions, action trajectory segments, + effects, recovery, scene revisions, and post-policy results. The existing `SimulationExecutionAdapter` is not the demo execution loop because direct simulator updates can bypass environment managers and recorders. @@ -493,31 +817,55 @@ normally, then resume with a fresh observation. ### 9.2 Timing `BaseEnv.step_dt` is the authoritative control cadence. Semantic task -configuration does not expose `control_dt`. - -Version 1 should require every emitted `JointCommand.hold_duration` to be -representable by an integer number of environment steps, preferably one step -per yielded command. An incompatible command is rejected with a clear timing -error; it is not silently resampled. Explicit timed-command resampling can be a -later, separately tested feature. - -Timeout for a named phase starts when its first command is dispatched, not when -an earlier phase or the whole segment is compiled. - -### 9.3 Named phases - -Plans and execution events need stable semantic phase names. Initial built-ins -should expose at least: - -- pick: `approach`, `grasp_close`, `lift`; -- place: `lower`, `release`, `retract`; -- handover: role-specific approach, transfer, release, and retreat phases; -- articulation operation: `approach`, `grasp_close`, `operate`, `release`, - `retract`. - -Post-policies and effect monitors subscribe to names, not trajectory sample -indices. The runtime validates requested phase names against the active skill -descriptor before execution. +configuration, `MotionPolicy`, and `AtomicActionEngine` do not expose or own a +fallback `control_dt`. Environment integrations copy `BaseEnv.step_dt` into +`PlanningContext.control_dt` when an action performs deterministic interpolation. + +Timing is a strict producer contract. A planner result with positions includes +per-waypoint `dt` and derives its per-environment `duration`; an atomic action +passes a complete `TimedTrajectory` to `build_plan()`. Missing or inconsistent +timing is rejected at construction. No layer repairs an untimed planner result +or raw action position tensor with a default period. + +Version 1 should require every emitted +`RuntimeCommandFrame.hold_duration` to be representable by an integer number +of environment steps, preferably one step per yielded frame. An incompatible +frame is rejected with a clear timing error; it is not silently resampled. +Explicit timed-command resampling can be a later, separately tested feature. + +Recovery timeout and retry budgets are scoped to the enclosing action attempt. +A `TrajectorySegment` does not start an independent timer or own a recovery +policy. Program-segment settling and validation use separate post-policy +deadlines. + +### 9.3 Named atomic trajectory segments + +Version 1 freezes the trajectory-segment names already emitted by current +built-ins. A successful non-empty plan exposes the following ordered names; +zero-length optional segments are omitted: + +| Atomic skill ID | Ordered trajectory-segment names | +|---|---| +| `move_joints` | `move_joints` | +| `move_end_effector` | `move_end_effector` | +| `move_held_object` | `transport` | +| `pick_up` | `approach`, `close`, `lift` | +| `place` (including `AssembleGoal`) | `approach`, `release`, `retract` | +| `press` | `close`, `press`, `retract` | +| `hand_over` | `transfer`, `approach`, `close`, optional `hold`, `release`, `deliver` | +| `coordinated_pickment` | `approach`, `close`, `lift`, `move`, optional `hold` | +| `coordinated_placement` | `approach`, optional `hold`, optional `release`, `retreat` | + +These spellings are a trace/metadata contract. Renaming or removing one requires +an explicit API review and migration rather than a silent change in a primitive. + +Names are validated by `ActionPlan`; ranges may change after replanning when a +backend returns a different sample count. Effect monitors run at the action +effect boundary and may use `EffectVerificationRequest.terminal_segment` for +correlation. Program post-policies and validators subscribe to program/demo +segment boundaries, not trajectory segments. Articulation segment names should +be stabilized with the reusable articulation skill rather than predeclared in +the configuration schema. ### 9.4 Dynamic settling @@ -541,15 +889,22 @@ clear object dynamics. All runtime state is indexed by stable environment IDs: - scene revisions and active collision dependencies; -- current call/phase and command deadline; +- current program segment, semantic call, action waypoint, and command deadline; - recovery budgets and failure masks; - verified held-object/effect state; - post-policy progress and segment validation; - result and metadata. -One environment may finish, recover, settle, or fail without blocking or -overwriting another. Program structure is shared, but runtime progress is -masked per environment. +Version 1 uses a shared program/call barrier for the environment batch; it does +not maintain a divergent AST program counter or a separate `ExecutionSession` +per environment. The runtime advances to the next semantic call or program +segment only when every still-eligible active row reaches the current boundary. +A slower or recovering active row therefore keeps the batch at that boundary. + +Within the shared barrier, task state, effects, recovery budgets, eligibility, +success, and failure remain independent per environment. Completed, failed, or +otherwise inactive rows emit hold behavior and cannot overwrite another row's +state while the active cohort catches up. ## 10. Action Bank migration @@ -558,7 +913,7 @@ capability parity. | Action Bank concept | Expert Program / semantic runtime | |---|---| -| scope | `Segment` or nested `Sequence` | +| scope | Program `SegmentCfg` or nested `SequenceCfg` | | custom node function | registered semantic call and shared compiler | | custom edge/target function | typed target provider or goal grounder | | graph edge | explicit sequence/effect dependency inferred by compiler | @@ -573,7 +928,10 @@ Migration rules: working during the transition. 2. Add `EmbodiedEnvCfg.expert_program` and a CLI input such as `--expert_program`; reject simultaneous legacy and new program inputs. -3. Migrate sequential tasks first and compare generated metadata and outcomes. +3. Do not require official-task migration in PR1. Start opt-in sequential-task + migration with the repeated-cube vertical slice after the registry, compiler, + runtime, and demo bridge contracts are available, then compare generated + metadata and outcomes. 4. Add `Parallel` only with deterministic resource conflict checks, trajectory alignment, synchronization barriers, and per-environment `StateDelta` merging. @@ -600,10 +958,11 @@ OperateArticulation( ) ``` -Its compiler selects an affordance pose, binds an arm/tool, builds the approach -and constrained operation, and installs an articulation effect monitor. Once -implemented once in the shared layer, Open Drawer variants should differ only -in scene/affordance data, target state, presets, and validators. +Its compiler selects an affordance pose, resolves one participant resource and +its required motion/interaction endpoints, builds the approach and constrained +operation, and installs an articulation effect monitor. Once implemented once +in the shared layer, Open Drawer variants should differ only in +scene/affordance data, target state, resource defaults, presets, and validators. This is the precise meaning of "almost no action-layer code": task expansion is configuration-only when a compatible semantic capability already exists; new @@ -615,12 +974,21 @@ Each item below should remain a focused PR with its own public-API review and tests. The dependency order is: ```text -Phase 0 correctness +Phase 0 correctness (complete) | v -SceneRegistry + RobotSkillProfile +PR1 snapshot/identity bridge (complete) | - v + +-----------------------+ + v v +PR2A SceneRegistry PR2B RobotSkillProfile + (implemented) (implemented) + | | + | v + | PR2C Runtime Endpoints + | (implemented) + +-----------+-----------+ + v Semantic calls/compiler --> SkillRuntime/effect monitors | | +---------------+--------------+ @@ -638,39 +1006,244 @@ Semantic calls/compiler --> SkillRuntime/effect monitors Action Bank deprecation ``` -### Phase 0: correctness and compatibility prerequisites - -Deliverables: +### Phase 0: correctness and core-contract decisions (complete) + +Landed on `main` through #475: + +- cumulative sub-threshold translation and rotation compare against the last + published pose; +- target/general-scene and per-environment collision revisions have regression + coverage; +- the dead `MotionPolicy.interpolation` field is removed and strategy dispatch + is unified; +- one action owns one trajectory and one recovery/effect boundary, while named + `TrajectorySegment`s remain metadata; +- `_plan()` is the only supported custom-action extension hook. The immediate + class-definition failure for a legacy `plan()` override is a documented, + tested, intentional hard break with no compatibility adapter; +- planner-local dynamic-obstacle name validation remains in place as a defensive + core check. Complete provider/planner cross-validation is deliberately owned + by the authoritative `SceneRegistry` integration in Phase 1. + +Exit criteria were met by PR #475 and remain the foundation for the completed +Phase 1 bridge. That bridge adds neither a legacy `plan()` adapter nor a +pre-registry duplicate of the integration-level obstacle validator. + +### PR1: core snapshot and identity bridge (complete) + +PR1 is deliberately smaller than Phase 1. It establishes the core seams that +the later registry and profile integrations consume: + +- add optional, validated `ObjectSemantics.entity_id` as the stable + `SceneSnapshot` key for canonical object grounding; +- resolve explicit IDs only from `PlanningContext.scene`, with a hard error and + no live fallback when the snapshot entry is missing; +- keep `ObjectSemantics.entity` only as a deprecated no-ID compatibility path; +- shallow-freeze `ObjectSemantics` fields so captured `entity_id` values cannot + be rebound without constructing a new semantic value; +- define stable held-object identity and partial-batch `StateDelta` merging: + if either side has an explicit `entity_id`, both explicit IDs must exist and + match; only two explicit-ID-less values may compare matching legacy + `entity.uid` strings, and only values with neither ID form may fall back to the + same semantic object or live handle; +- preserve scalar semantics during same-identity partial `StateDelta` merges: + while any previously active row remains, retain `previous.semantics` and + merge only per-environment masks, transforms, and grasp poses; adopt + `candidate.semantics` only when all previously active rows are replaced; +- add an action-owned scene-dependency hook. `PickUp` declares its semantic + object ID, coordinated pickup declares it only for the implicit initial-pose + path, and goal-owned `SceneEntityPose` values remain automatic dependencies; +- resolve each pickup object pose once per planning attempt and reuse that + tensor for grasp sampling, upright adjustment, and `object_to_eef`; +- derive held-object pose for `MoveHeldObject` and `HandOver` from the observed + EEF pose and verified `object_to_eef` instead of a live entity read; +- add `AssembleGoal.base_pose: SceneEntityPose | None`; the explicit reference + is snapshot-backed and dependency-tracked, while `None` retains the deprecated + `AssembleAffordance.base_object_entity` fallback; +- add focused tests, documentation, and one canonical snapshot-grounded moving + target tutorial. Keep `scripts/tutorials/atomic_action/assemble.py` explicitly + documented as a legacy fallback example until its later registry migration. + +PR1 does not add a `SceneRegistry`, a `SceneEntityRef` hierarchy, alias maps, +cross-source uniqueness or collision validation, a `RobotSkillProfile`, or +semantic presets. It does not require official task environments to migrate; +they remain on the compatibility path until a later opt-in vertical slice. + +Exit criteria are met on `main` through PR #487: canonical object grounding +never mixes snapshot and live poses; explicit missing IDs fail; dependency +metadata matches the poses actually consumed; stable-identity merges are +deterministic; and existing direct-core callers remain usable only through the +documented deprecated fallbacks. -- fix cumulative sub-threshold translation and rotation publication in - `RigidObjectSceneProvider` by comparing with the last published/significant - pose; -- add regression tests for target and collision-world revisions; -- decide the supported `plan()`/`_plan()` custom-action extension contract and - provide a compatibility/deprecation path before enforcing a break; -- remove or implement misleading `MotionPolicy` fields, keeping collision - semantics expressed by `DynamicCollisionMode`; -- add early cross-validation for registry/provider/planner obstacle names. +### Phase 1: unified integration data -Exit criteria: all #474 P0 items are resolved on main and custom actions have a -documented, tested upgrade path. +Phase 1 is implemented as three focused follow-up PRs. PR2A and PR2B branch +from the PR1 foundation; PR2C follows PR2B and joins PR2A before the semantic +facade/compiler work. -### Phase 1: unified integration data +#### PR2A: SceneRegistry (landed in PR #487) Deliverables: - `SceneEntityRef` hierarchy and `SceneRegistry`; -- immutable snapshot as the only grounding pose authority; -- environment-to-registry population and collision/provider derivation; -- `RobotSkillProfile`, capability-based binding, semantic tool commands, and - stable presets; +- authoritative registry IDs with simulation `uid` values accepted only as + normalized legacy aliases; +- immutable snapshots as the only grounding pose authority for the canonical + semantic/compiler path; +- opt-in environment-to-registry population and collision/provider derivation; +- complete construction-time agreement checks between registry and planner + full collision-world IDs, plus registry/provider/planner dynamic subsets, + geometry, mode, and planner capability; - explicit catalog-discovery versus engine-installation terminology. -Exit criteria: an object is registered once and a dynamic-object configuration -error fails before execution with an entity-centric diagnostic. +The implemented scope also records the A+C+E decisions from the PR2A API +review: + +- registry-backed cuRobo worlds use a canonical-ID mapping, while the list form + remains an advanced direct-core escape hatch; +- all reference IDs are globally unique and flat, with link/affordance parent + relations retained only by their registrations; +- one-environment dynamic worlds may default to shared, while vectorized + dynamic worlds require an explicit shared/per-environment choice. + +`ObjectSemantics.entity_id` and `AssembleGoal.base_pose` already provide the +lowering targets from PR1. PR2A replaces manually coordinated IDs/providers with +one authoritative registration and performs alias normalization exactly once at +the integration boundary. + +PR2A exit criteria are met by the feature change: registry-ID and alias +collisions fail at construction, typed lookups cannot silently change entity +kind, one typed parent/native physical source cannot be registered twice, +registry-derived snapshots contain only canonical keys, independent providers +keep independent revision state, full registry/planner collision-world IDs and +dynamic registry/provider/planner subsets are validated before execution, the +collision-world batch mode agrees, and cuRobo uses canonical mapping keys end +to end as logical source IDs (and as physical keys for cuboid/mesh worlds). + +#### PR2B: RobotSkillProfile (landed in PR #487) + +Deliverables: + +- a generic `RobotResource` DAG whose named `ResourceEndpoint`s are not tied to + arm/tool categories, plus a formal `ResourceEndpointAdapter` registry and + `EndpointResolution` protocol; `ControlPartEndpointAdapter` is the first + implementation; +- action-owned `SkillBindingContract`s with participant-local endpoint, + capability, typed-command, and disjoint-claim requirements; +- capability-based candidate filtering, complete per-skill defaults, explicit + selection overrides, and deterministic ambiguity/unsupported diagnostics; +- profile-owned semantic commands plus immutable, versioned planning/recovery/ + runner presets; +- validation against installed agent-visible engine skills, robot control + parts, joint ownership, endpoint overlap, configured solvers, commands, and + presets; +- immutable leaf/joint/adapter-token `ResourceClaim` data and explicit + same-slot endpoint disjointness for future conflict analysis, without + claiming safe parallel execution. + +The profile and endpoint-runtime APIs can represent mobile-base and whole-body +resources today, and the generic paths are covered by whole-body joint and +custom planar-velocity tests. They are extension seams, not built-in navigation +or whole-body behavior: no current curated semantic skill consumes the example +`motion.base.*` or `motion.whole_body` capabilities. A production shared +capability still needs its semantic descriptor/lowerer, atomic skill, payload, +endpoint adapter, transport, and effect integration as applicable. Once that +reusable bundle exists, another task supplies an Expert Program plus typed +scene/profile integration declarations without task-specific motion code. + +The standard Gym bridge currently composes every custom transport action over +a full-qpos hold and the standard simulation factory owns a +`MotionGenerator`. This supports robots without named control parts, but a +truly jointless or natively structured mobile controller still needs a reusable +base-action composition/provider integration. That extension must not add +base- or whole-body-shaped fields to the generic resource, binding, runner, or +router contracts. + +The current task vertical slices still construct their typed profile bindings +from task modules. Promoting stable bindings into an embodiment-owned profile +catalog is rollout packaging needed for cross-task reuse; it does not require a +new resource or runtime contract. + +PR2A and PR2B landed together through PR #487. That foundation does not migrate +official tasks; the repeated-cube vertical slice opts in only after the +compiler, runtime, and demo bridge are available. + +#### PR2C: generic runtime endpoints (implemented on the feature branch) + +PR2C removes the temporary arm/tool lowering seam and makes the profile's +generic endpoint model executable end to end: + +- `ActionBinding` is an engine-owned collection keyed only by + `(slot_id, endpoint_id)`; `ActionBindingRoute`, arm/tool role maps, and the + intermediate resolved-control-part binding types are removed as an + intentional clean break; +- every resolved profile endpoint owns a typed immutable + `RuntimeEndpointTarget`, while `EndpointCommand` combines that destination + with a transport-specific `RuntimeCommandPayload`; +- `RuntimeCommandFrame` synchronizes per-environment endpoint commands and + timing, and `TimedCommandSequence` becomes the authoritative runtime content + of `ActionPlan`; +- `EndpointCommandTransport` and `EndpointCommandRouter` perform exact-ID + registration, preflight payload validation, transport grouping, + acknowledgement aggregation, cancellation, and transport-owned safe holds; +- the framework authorizes planned commands against binding-owned targets and + physical claims, requires stable destinations across frames and recovery + replans, and retains previously active targets when a failed plan is empty; +- transports actively neutralize inactive environment rows for every addressed + target instead of treating an omitted write as a safe state; +- `SimulationExecutionAdapter` implements the built-in joint-position + transport and writes or holds only the joints claimed by each addressed + endpoint; +- joint-backed planners retain an optional full-robot `TimedTrajectory` for + existing joint feedback and `engine.compile()`, while non-joint plans use + timed completion plus the existing semantic-effect verification boundary; +- full-body joint control and a custom planar-velocity endpoint are exercised + from binding/profile resolution through planning, session execution, routing, + completion, and safe hold without arm/tool-shaped fields. +- an explicit invocation revision declares the same non-empty runtime + destination set and preserves each target's address/safe-hold fingerprint. + The runner keeps the active frame deadline and replans from a fresh due-time + observation; a pending physical effect must be verified first. Changing a + base, arm, whole-body, controller destination, or hold footprint starts a new + invocation rather than hot-switching controller ownership in place. + +PR2C does not add parallel scheduling, claim merging, transport rollback, or a +generic endpoint-feedback evaluator. It also does not add cross-destination +hot revision. Those require separate contracts. + +PR2C exit criteria: an installed custom endpoint kind needs one reusable +endpoint declaration/adapter, payload, transport, and shared atomic skill, but +no core binding or runner changes; whole-body joint endpoints use the same +path; unknown transports and incompatible payloads fail before dispatch; and +cancel/hold behavior remains transport-owned and auditable. + +Combined Phase 1 exit criteria: an object is registered once under an +authoritative ID, aliases cannot introduce ambiguity, dynamic-object +configuration mismatches fail before execution with an entity-centric +diagnostic, robot capabilities resolve bindings/presets without task-owned +motion code, and generic resolved endpoints can reach their registered runtime +transports without adding arm/tool-specific core paths. + +Implementation status: when `safe` is reachable and the registry declares +dynamic collision entities, binding rejects an unsupported active planner +before observation or planning. Linking produces an effective +`DynamicCollisionMode.REQUIRED` preset snapshot without mutating the profile's +source preset. A real-simulation gate now covers semantic lowering, CUDA/cuRobo +planning, a post-plan dynamic-obstacle world change, collision-revision-aware +replanning, and successful completion. This is a conditional GPU gate: the +module skips when cuRobo is unavailable or CUDA is unavailable, and pytest runs +it only when GPU and slow tests are explicitly selected. ### Phase 2: semantic facade and compiler +Implementation status: the semantic facade, provider-free linking, canonical +compiler, bounded program preflight, and cross-segment sequential look-ahead are +implemented. Relation placement remains an exact typed integration capability. +Production support-surface and container bindings now declare a desired object +target frame relative to an object, articulation, or link parent; registration +installs their exact versioned grounders automatically. No geometric target is +guessed from a name, mesh, or bounding box. + Deliverables: - `SemanticCallSpec`, object-centric `Pick`, `Place`, and `HandOver`; @@ -687,12 +1260,37 @@ effect verifier. ### Phase 3: canonical runtime and effects +Implementation status: core contracts are implemented in the current stack. +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. Segment-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 covers Open Drawer and all three +cube Pick/Place/settle/validator cycles. The embodiment-owned +dual-UR5/PGI HandOver slice now completes Pick, transfer, terminal +physical-effect verification, settling, and target validation through real +contact dynamics. Per-expectation terminal outcomes, core-owned failure +invalidation, row-local retry/recovery decisions, fail-closed deadline +reconciliation, and blocking named-segment effect gates are implemented. +Workflow-level re-acquisition is implemented through the preset-owned bounded +policy and canonical runtime. A supported-simulation fault gate now replaces a +bounded window of outgoing gripper commands with a real open command during +Place, observes pose-relation contradiction and symbolic invalidation, performs +a real Pick, retries Place, and completes the remaining program. The wrapper +never writes object pose, velocity, attachment, constraint, or task state. + Deliverables: - `SkillRuntime` wrapping `ExecutionRunner` for sync and step-wise use; +- exactly one semantic call lowered to one invocation in one + `ExecutionSession`; - built-in simulation effect monitors for grasp, release, and handover; - uniform per-environment `SkillResult` and persistent verified `TaskState`; -- automatic static/observed stage selection; +- a shared Version 1 program/call barrier with independent per-environment task, + effect, recovery, eligibility, and result state; - safe cancellation, timeout, and hold behavior inherited from the runner. Exit criteria: Python calls and a programmatic `SemanticCallSpec` use identical @@ -700,46 +1298,71 @@ compiler/runtime code and produce equivalent results. ### Phase 4: demo integration primitives +Implementation status: implemented. The bridge uses buffered runtime commands and +an environment-step clock, dynamic settling is shared with reset behavior, and +JSON-safe lifecycle metadata covers every installed plan attempt, named +trajectory segment, effect decision/evidence, recovery event, scene/collision +revision, post-policy outcome, and validator result. + Deliverables: -- stable named phases in plans/descriptors/events; +- expose the existing named plan trajectory segments through optional demo + trace metadata without adding segment-level recovery; - reusable `DynamicSettleMonitor` shared by reset and demo paths; - Gym observation, buffered command, and environment-clock ports; - thin `AtomicDemoBridge` yielding lazy `DemoSegment`s; - exact `BaseEnv.step_dt` timing validation; -- runtime metadata for calls, phases, effects, recovery, scene revisions, - settling, and validation. +- runtime metadata for calls, trajectory segments, effects, recovery, scene + revisions, settling, and validation. -Exit criteria: no demo command bypasses `env.step()`, and phase/post-policy -behavior contains no hard-coded trajectory index. +Exit criteria: no demo command bypasses `env.step()`, and no post-policy, +effect, or trace integration contains a hard-coded trajectory index. ### Phase 5: Expert Program version 1 and repeated-cube vertical slice +Implementation status: configuration and task migration are implemented. The +strict decoder/loader, lazy compiler, environment/CLI integration, shared +simulation factory, and three-segment cube program are implemented. The task +combines declarative program configuration with typed scene/profile integration +declarations and installs the shared adapter without overriding task motion +generation. The UR5 embodiment preset uses 100 motion samples while retaining +the 0.08-rad tracking gate and bounded replanning. A supported-simulation run +now completes all three physical Pick/Place/settle/validator cycles. + Deliverables: - strict `@configclass` schema and versioned decoder; - `Sequence`, bounded `Repeat`, `Segment`, and `Invoke`; - registered targets, post-policies, and validators; - `EmbodiedEnvCfg` and CLI integration with legacy fallback; -- configuration-only migration of repeated cube pick/place. +- motion-code-free migration of repeated cube pick/place using a declarative + program and typed scene/profile integration declarations. Exit criteria: -- three lazy segments complete in supported simulation; -- each segment re-observes the cube after free-fall settling; +- three lazy program/demo segments complete in supported simulation; +- each program/demo segment re-observes the cube after free-fall settling; - grasp and release effects are verified; - placement uses verified held-object state; - settle and validation data are present in metadata; -- multi-environment success, failure, and recovery masks remain independent; +- the environment batch advances through the shared call barrier while success, + failure, effect, recovery, and eligibility masks remain independent; - the task contains no task-specific motion-generation code. ### Phase 6: sequential skill coverage and articulated interaction +Implementation status: the articulation path and task migration are +implemented. Articulation/link/operation-affordance registration, +`OperateArticulation`, typed joint-state effects/evidence, and the declarative +Open Drawer program with typed integration declarations use the same +compiler/runtime path as pick/place. Its supported-simulation physical run now +completes and reaches the configured drawer joint target. + Deliverables: - articulation/link/affordance registry integration; - reusable articulation-operation semantic call, compiler, effect monitor, and - named phases; + named trajectory segments; - configuration-based Open Drawer migration; - migrate additional sequential tasks to reveal missing reusable grounders, monitors, and validators. @@ -749,11 +1372,52 @@ trajectories in task code. ### Phase 7: parallel execution and PourWater +Implementation status: the schema/runtime contracts, fail-closed safety +boundary, and production simulation validator are implemented. Schema +version 2 provides explicit parallel branches and barriers; static resource +conflict analysis, shared-clock lane coordination, deterministic hold padding, +transport/safety validation, row-local failure and cancellation, timeouts, and +deterministic state merge are covered by tests. The cuRobo validator assembles +the exact aggregate joint segment, densifies it under a configured maximum joint +step, and checks every sample against joint bounds, self collision, and the +registry-backed live world without replanning or replacing the command. A +parallel physical integration remains pending. The PourWater task +migration is outside the current scope because it would require modifying +Action Bank code. + +The current follow-up also makes task registration the sole standard-runtime +extension owner. `SkillPolicyPreset` schema version 3 requires exact typed +action-option templates for every reachable semantic call; lowering may fill +only explicitly compiler-owned dynamic target fields. Endpoint adapters, +ordered Gym transports, and a parallel-safety factory are declared on +`SimulationExpertProgramRegistration`, enter its provider-free fingerprint, +and are cross-checked again against live endpoint resolution. The standard +factory consumes the same registration objects, freezes the assembled command +encoder, takes runner timing from the selected preset, and creates a fresh live +safety validator for every runtime assembly. No helper argument can replace +those registered components after preflight. Stateful extension declarations +must be frozen dataclasses with recursively immutable configuration, preventing +nested mutable values from becoming a post-registration runtime side channel. + +This registration slice deliberately covers command transport, not arbitrary +closed-loop backend injection. In C1, every custom endpoint adapter must declare +empty tracking and effect-evidence route sets and therefore supports only +timed/open-loop completion. The built-in `ControlPartEndpoint` retains its exact +built-in routes. A non-joint feedback provider, desired-state projector, metric +evaluator, or effect-evidence backend needs a separate registration-owned +live-provider factory contract before it can be advertised as standard +mobile/whole-body closed-loop support. Such providers must become fingerprinted +capabilities; they must not return as task-side runtime callbacks. Transport +`hold()` remains a trusted safe primitive owned and tested by each transport, +while the parallel safety validator authorizes active merged command frames +before dispatch. + Deliverables: - `Parallel` and explicit `Barrier` nodes in a new schema version; - robot-resource conflict analysis; -- deterministic trajectory alignment/resampling policy; +- deterministic strict-step-grid alignment with hold padding; fractional frame + durations are rejected rather than implicitly resampled; - synchronization and timeout behavior; - deterministic per-environment `StateDelta` merge rules; - PourWater migration from its Action Bank subclass. @@ -763,6 +1427,18 @@ tests pass before the legacy task is switched. ### Phase 8: rollout, documentation, and deprecation +The deterministic framework/integration capability matrix and migration-size +snapshot are maintained in +[`expert_program_rollout_report.md`](expert_program_rollout_report.md). Demo +success collection uses the no-retry benchmark harness; real success-rate +claims and gates remain deferred until the repeated Cube threshold contract and +three-cycle physical acceptance are settled. + +Implementation status: partial. The canonical semantic/Expert Program documentation, +project-development context, task vertical slices, and public integration +guidance are included in this stack. Metrics, migrations that touch Action +Bank, and any deprecation proposal remain explicitly separate follow-up work. + Deliverables: - semantic quickstart and advanced-core integration guide; @@ -781,66 +1457,114 @@ independent of adoption of the new path. - strict decoder, unknown fields, schema versioning, bounded repeats, and registry reference errors; +- authoritative registry-ID normalization, legacy-`uid` alias collisions, + typed parent/native-source collisions, complete registry/planner collision- + world agreement, and registry/provider/planner dynamic-subset agreement; - cumulative scene movement and collision dependency revision behavior; - profile capability matching, deterministic binding, and ambiguity errors; -- static versus observed stage partitioning; +- `AssembleGoal.base_pose` snapshot resolution and its automatic scene + dependency, with the `None` fallback isolated to legacy direct-core use; +- same-identity partial `StateDelta` merges retain previous scalar semantics + until every previously active row is replaced, for both individual and + coordinated attachments; +- exactly one semantic call and one invocation per `ExecutionSession`; - downstream target propagation for grasp selection; - object-centric place conversion from one immutable snapshot and verified held state; - effect monitor state transitions and timeout/recovery feedback; -- named phase validation and exact step-duration conversion; +- trajectory-segment coverage/name validation and exact step-duration + conversion; - Action Bank compatibility adapters where introduced. ### Integration tests with fake ports - Python facade and Expert Program lower to equivalent invocations; - runner scheduling, acknowledgement, safe stop, and cancellation are reused; -- one environment can complete while another recovers or fails; +- the Version 1 shared call barrier holds active rows together while completed, + recovering, and failed rows retain independent masks and state; - command buffering advances only through the environment clock; -- segment metadata is deterministic and serializable. +- program/demo-segment and trajectory-segment metadata are deterministic and + serializable. ### Simulation tests -- three-segment repeated cube pick/place with free-fall re-observation; +- three-program/demo-segment repeated cube pick/place with free-fall + re-observation; - moving target and dynamic collision recovery with the `safe` preset; - grasp/release/handover effect monitors; - settling success and timeout metadata; - Open Drawer articulation effect; -- GPU-backed dynamic cuRobo coverage where supported; +- conditionally executed real CUDA/cuRobo dynamic-obstacle recovery coverage + where cuRobo and CUDA are available and GPU/slow tests are enabled; - parallel PourWater only after Phase 7 contracts land. ## 14. Acceptance criteria The design is complete when all of the following hold: -- [ ] A versioned Expert Program is fully validated before execution and cannot +- [x] A reachable `safe` preset in a dynamic-collision scene resolves to + `DynamicCollisionMode.REQUIRED` and rejects an unsupported active planner + before observation, planning, or command emission without mutating the + profile configuration. +- [x] On supported CUDA/cuRobo installations, a conditional real-simulation + gate moves a dynamic obstacle after the initial plan, observes the + collision-world change and replan, and reaches the target successfully; + environments without cuRobo or CUDA skip this GPU/slow gate. +- [x] A versioned Expert Program is fully validated before execution and cannot evaluate arbitrary code or traverse environment attributes by string. -- [ ] Python, configuration, and future MLLM calls share one semantic compiler, +- [x] Python, configuration, and MLLM calls share one semantic compiler, typed atomic-action core, and runtime. -- [ ] A common new task using existing semantic skills needs no task-specific +- [x] A common new task using existing semantic skills needs no task-specific motion-generation code. -- [ ] Each scene entity is registered once across semantics, observation, - affordance, and collision handling. -- [ ] The default pick/place path does not expose raw qpos, grasp/EEF matrix +- [x] Robot capability binding is expressed through generic participant + resources and endpoints, so mobile-base and whole-body skills do not + require new arm/tool-shaped profile fields. +- [x] Runtime binding, command framing, routing, and safe stop are endpoint + generic; joint trajectories remain an optional planning/feedback artifact + rather than the only runtime carrier. +- [x] Each scene entity is registered once under an authoritative registry ID + across semantics, observation, affordance, and collision handling; + simulation `uid` values are legacy aliases only. +- [x] The default pick/place path does not expose raw qpos, grasp/EEF matrix math, planner construction, session plumbing, or custom verification. -- [ ] Automatic grasping tracks target revisions and receives downstream object +- [x] Automatic grasping tracks target revisions and receives downstream object goals without caller duplication. -- [ ] `Place` is object-centric and consumes verified held-object state. -- [ ] Built-in grasp, release, handover, and supported articulation effect - monitors work in simulation. -- [ ] Repeated sub-threshold motion eventually publishes the correct scene +- [x] `Place` is object-centric and consumes verified held-object state. +- [x] Built-in grasp, release, handover, and supported articulation effect + monitors work in simulation across the repeated cube, dual-UR5/PGI + HandOver, and Open Drawer vertical slices. +- [x] 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. +- [x] 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 segment-aware observation, + row-local core-owned invalidation, per-expectation terminal + reconciliation, fail-closed deadline handling, bounded Pick/retained-Place + retry, typed recovery boundary, and blocking acquisition/release gates are + implemented. Preset-owned per-row workflow re-acquisition now performs + real `Pick` and semantic-call retries. The supported-simulation gate opens + the real gripper during Place, observes the loss, re-acquires the fallen + cube, and completes the retried call. +- [x] Repeated sub-threshold motion eventually publishes the correct scene revision. -- [ ] Custom actions have a documented and tested compatibility path. -- [ ] Demonstration timing is derived from `BaseEnv.step_dt` and commands pass +- [x] Custom actions have a documented and tested intentional hard-break + migration from overriding `plan()` to implementing `_plan()`; no + compatibility adapter is required. +- [x] Version 1 creates exactly one one-invocation `ExecutionSession` for each + semantic call and re-observes before lowering the next call. +- [x] Demonstration timing is derived from `BaseEnv.step_dt` and commands pass through `env.step()`. -- [ ] Phase hooks use stable names rather than trajectory indices. -- [ ] Repeated cube pick/place completes at least three lazy, independently - observed segments with settle/effect/validation metadata. -- [ ] Multi-environment progress, effects, recovery, and failures remain +- [x] No program post-policy, effect, or tracing integration depends on + hard-coded waypoint indices. +- [x] Repeated cube pick/place completes at least three lazy, independently + observed program/demo segments with settle/effect/validation metadata. +- [x] Version 1 uses one shared program/call barrier while per-environment task + state, effects, recovery, eligibility, success, and failure remain independent. -- [ ] Advanced users retain typed goals, invocations, policies, providers, +- [x] Advanced users retain typed goals, invocations, policies, providers, sessions, and planners as escape hatches. -- [ ] Parallel resource conflicts, synchronization, timing, cancellation, and +- [x] Parallel resource conflicts, synchronization, timing, cancellation, and state merging are tested before PourWater migration. - [ ] Action Bank remains usable until feature parity and a deprecation window are documented. @@ -854,7 +1578,7 @@ The design is complete when all of the following hold: | Automatic binding makes surprising choices | Use capability validation and deterministic profile preferences; surface semantic ambiguity rather than silently selecting. | | Presets become opaque or unstable | Version preset semantics, emit the resolved core policies in runtime metadata, and keep typed overrides available to advanced users. | | Built-in effect monitors overfit simulation | Keep the contract backend-neutral and provide replaceable hardware implementations; record monitor evidence and thresholds. | -| Static compilation uses stale state | Default to dependency-driven `auto` partitioning and force observed boundaries after external effects or dynamic post-policies. | +| Static compilation uses stale state | Version 1 never coalesces semantic calls into one session or static stage; keep `engine.compile()` as an explicit advanced-core API until a later optimization proves equivalent observation/effect boundaries. | | Demo bridge duplicates runner logic | Keep scheduling, acknowledgement, recovery, timeout, and safe stop in `ExecutionRunner`; bridge only the Gym step boundary. | | Configuration grows into a programming language | Keep version 1 bounded and discriminated; add only registered nodes and no expressions or arbitrary DAG scheduler. | | Articulation and parallel work delay useful delivery | Ship the sequential cube vertical slice first; add reusable capabilities independently. | diff --git a/docs/design/expert_program_rollout_report.md b/docs/design/expert_program_rollout_report.md new file mode 100644 index 000000000..175e74e53 --- /dev/null +++ b/docs/design/expert_program_rollout_report.md @@ -0,0 +1,69 @@ +# Declarative Expert Program Rollout Report + +This is a deterministic, static Phase 8 snapshot of checked-in framework and integration code. It does not run simulation, report physical acceptance, or certify production readiness for an embodiment. + +## Framework Contract Matrix + +`framework-tested` describes the reusable framework contract only. A task appears in the matrix below only when its integration/production code is checked in; that code status does not imply physical acceptance. + +| Capability | Framework status | Integration gate | Scope | +| --- | --- | --- | --- | +| Pick + Place(at) | framework-tested | per-embodiment integration | Typed goals, compilation, execution, and terminal effects are covered. | +| Attach/release effect | framework-tested | per-embodiment integration | Effects use accepted commands plus live object-to-endpoint pose evidence. | +| OperateArticulation | framework-tested | per-embodiment integration | Typed articulation goals and execution contracts are covered. | +| Articulation effect | framework-tested | per-embodiment integration | Joint-state terminal effect validation is covered. | +| V1 sequential | framework-tested | per-task integration | Ordered call execution and failure propagation are covered. | +| HandOver | framework-tested | per-embodiment integration | Coordinated effects and bounded recovery are covered. | +| Place relation (on/inside) | framework-tested | per-scene integration | Standard support/container target-frame bindings install exact grounders. | +| Registered call | framework-tested | integration-required | Production registration must declare and validate its concrete contract. | +| V2 parallel | framework-tested | integration-required | Joint/cuRobo validation is available; physical parallel acceptance remains. | + +Parallel execution remains fail-closed by default. Resource declarations alone do not authorize production concurrency; the selected embodiment must provide an authoritative validator. + +## Checked-in Integration Matrix + +Only the checked-in vertical slices below are classified as integration/production code. Physical acceptance is tracked separately. + +| Embodiment | Task | Skill contract | Terminal effect | Program schema | Code status | Physical acceptance | +| --- | --- | --- | --- | --- | --- | --- | +| UR5 | Cube Pick + Place | Pick + Place(at) | attach/release | V1 sequential | checked in | fixed-seed three-cycle and physical-loss recovery slow gates | +| CobotMagic | Open Drawer | OperateArticulation | articulation effect | V1 sequential | checked in | fixed-seed supported-simulation slow gate; not release-required | +| Dual UR5 + PGI | HandOver | Pick + HandOver | attach/transfer | V1 sequential | checked in | three consecutive supported-simulation contact-dynamics runs | + +Place-relation bindings are reusable scene integration rather than a task vertical slice. Registered calls and V2 parallel remain integration-required; physical parallel acceptance is still open. + +The checked-in environment classes have zero task-local motion or demo-generation overrides; `test_task_classes_do_not_override_motion_or_demo_generation` keeps that structural metric at zero. + +## Migration Size Snapshot + +The baseline is a fixed, manually recorded pre-migration snapshot: Cube is 598 lines / 23912 bytes and Drawer is 245 lines / 8833 bytes. The tool does not inspect Git history. Current values are recomputed only from the four explicit files in the table. + +Baseline identity: Cube uses Git blob `1965563b060d1fc889f03ad13d47655c2edcd99b` and Drawer uses Git blob `3b4cbdc09537098b4f109d46efb8785b88f31ce1` at each task's Python path listed in the current-source column. Blob IDs remain stable across stack rebases. + +Counting rule: `lines` is the number of raw LF (`0x0A`) bytes; `bytes` is the raw on-disk byte length. Counts are summed per task without normalizing encoding or line endings. + +| Task | Baseline lines | Current lines | Line delta | Baseline bytes | Current bytes | Byte delta | Current source files | +| --- | --- | --- | --- | --- | --- | --- | --- | +| Cube | 598 | 395 | -203 (-33.9%) | 23912 | 13272 | -10640 (-44.5%) | `embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py`
`embodichain_tasks/configs/expert_program/multi_segments/repeated_cube_pick_place.yaml` | +| Drawer | 245 | 265 | +20 (+8.2%) | 8833 | 8916 | +83 (+0.9%) | `embodichain_tasks/embodichain_tasks/tableware/open_drawer.py`
`embodichain_tasks/configs/expert_program/tableware/open_drawer.json` | +| Total | 843 | 660 | -183 (-21.7%) | 32745 | 22188 | -10557 (-32.2%) | the four files above | + +## Demo Success Measurement + +`scripts/benchmark/expert_program/demo_success.py` executes each fixed seed exactly once, always discards the episode buffer, and counts executor exceptions as failed rows. It writes raw JSON plus a three-table Markdown report. Its CLI supports offline raw-JSON re-aggregation and an explicit `--run-simulation` mode that constructs one standard Gym environment from Gym and Expert Program configurations. + +No multi-seed success-rate or release gate is checked in yet. Open Drawer has a real-simulation smoke pass; repeated Cube has a fixed-seed three-cycle pass plus physical-loss/re-acquisition gate; and HandOver has three consecutive contact-dynamics runs. + +## Drift Check + +Regenerate the checked-in report after an intentional source or capability snapshot change: + +```bash +python scripts/tools/expert_program_rollout_report.py +``` + +CI and local validation can reject stale output without rewriting it: + +```bash +python scripts/tools/expert_program_rollout_report.py --check +``` diff --git a/docs/design/gen_sim_semantic_skill_integration_plan.md b/docs/design/gen_sim_semantic_skill_integration_plan.md new file mode 100644 index 000000000..34b23db75 --- /dev/null +++ b/docs/design/gen_sim_semantic_skill_integration_plan.md @@ -0,0 +1,877 @@ +# GenSim PR #532-#538 and Semantic Skill Integration Plan + +- Status: proposed +- Last updated: 2026-08-21 +- Guiding principle: **Semantic Skill is the only unified lower-level execution architecture** +- Related Semantic Skill PR: [#492](https://github.com/DexForce/EmbodiChain/pull/492) +- Related GenSim PRs: + [#532](https://github.com/DexForce/EmbodiChain/pull/532), + [#533](https://github.com/DexForce/EmbodiChain/pull/533), + [#534](https://github.com/DexForce/EmbodiChain/pull/534), + [#535](https://github.com/DexForce/EmbodiChain/pull/535), + [#536](https://github.com/DexForce/EmbodiChain/pull/536), + [#537](https://github.com/DexForce/EmbodiChain/pull/537), and + [#538](https://github.com/DexForce/EmbodiChain/pull/538) +- Related Semantic Skill stack: + [#495](https://github.com/DexForce/EmbodiChain/pull/495), + [#496](https://github.com/DexForce/EmbodiChain/pull/496), + [#497](https://github.com/DexForce/EmbodiChain/pull/497), + [#498](https://github.com/DexForce/EmbodiChain/pull/498), + [#500](https://github.com/DexForce/EmbodiChain/pull/500), + [#501](https://github.com/DexForce/EmbodiChain/pull/501), + [#504](https://github.com/DexForce/EmbodiChain/pull/504), and + [#480-#483](https://github.com/DexForce/EmbodiChain/pull/480) + +## 1. Executive summary + +PR #532-#538 should not introduce a second Action Engine beside the existing +Semantic Skill and Atomic Action stack. The adjusted design keeps the parts of +GenSim that provide real additional value: + +- task interpretation and typed `TaskSpec` values; +- offline and online task-plan candidates; +- an immutable task DAG and revision log; +- candidate selection and TaskGroup-level fusion; +- unfinished-suffix replanning; +- task-level recovery, audit artifacts, and A/B evaluation; +- cross-engine orchestration and final task inspection. + +The following duplicate implementations should be removed from GenSim: + +- an independent atomic capability registry; +- action-specific motion policies and robot-part routing; +- a second live action grounder; +- an adapter that directly materializes `ActionInvocation` values; +- a second physical execution loop and trajectory scheduler; +- independent held-object, effect, and recovery truth. + +The final layering is: + +```text +Task Engine + owns TaskSpec, SemanticTaskGraph, candidates, task scheduling, + task-level recovery, and final success + | + v +Semantic Skill + owns semantic calls, scene/profile binding, JIT grounding, + physical effects, workflow recovery, and the application runtime + | + v +Atomic Actions + owns typed goals, planning, command execution, tracking recovery, + transport acknowledgement, and safe stop +``` + +This is not a compatibility exercise for the current GenSim runtime. PR +#533-#538 should be structurally rewritten around the canonical Semantic Skill +interfaces instead of accumulating adapters around the current implementation. + +## 2. Scope and assumptions + +### 2.1 In scope + +- Reassign responsibilities across PR #532-#538. +- Define the target task graph and execution boundary. +- Identify GenSim components to keep, transform, move, or remove. +- Define dynamic-task and error-recovery behavior. +- Define the landing order and review gates. +- Define architecture and end-to-end acceptance tests. + +### 2.2 Out of scope + +- Replacing `AtomicActionEngine`, `ExecutionSession`, or `ExecutionRunner`. +- Replacing the canonical `SceneRegistry` or `RobotSkillProfile`. +- Implementing a second controller or simulation stepping loop in GenSim. +- Persisting robot trajectories, qpos, grasp poses, or controller routing in a + task-planning artifact. +- Allowing task-graph rewrites during an unverified physical-effect boundary. +- Claiming arbitrary per-environment task-program divergence in the first + adjusted version. + +### 2.3 Canonical runtime naming + +The current #492 head contains `SemanticSkillRuntime`, while downstream #496 +defines the intended canonical `SkillRuntime`. Before adapting #536 and #537, +these implementations must be consolidated into one public runtime. This plan +uses `SkillRuntime` as the conceptual name; the exact final class name is less +important than having exactly one implementation and one ownership boundary. + +## 3. Target architecture + +```text +Scene Engine + GeneratedSceneGraph / SceneAuthoringGraph + | + v +Task Engine SceneAdapter + SceneManifest + SceneRegistry registration plan + evidence sidecar + | + +------------------------------------------+ + | +TaskSpec | + | | + v v +Offline / Online Semantic Task Planner Semantic Integration Catalog + | - SceneManifest + | - RobotSkillProfile + | - SemanticCallCatalog + | - provider declarations + | - integration fingerprint + v +SemanticTaskGraph candidates + | + v +Candidate selection / TaskGroup fusion / preflight + | + v +TaskGraphScheduler + | + | selected route and semantic look-ahead suffix + v +SkillRuntime / ParallelSkillRuntime + | + v +SemanticSkillCompiler + | + v +ActionInvocation + | + v +AtomicActionEngine -> ExecutionRunner -> controller or simulation +``` + +The main runtime resolution path is: + +```text +SemanticTaskGraph node + -> canonical SemanticCallCfg decoder + -> SemanticCallSpec + -> SemanticSkillCompiler.analyze() + -> fresh observation + -> SemanticSkillCompiler.ground() + -> ActionInvocation + -> AtomicActionEngine / ExecutionRunner + -> verified SkillResult and TaskState + -> TaskGraphScheduler transition +``` + +## 4. Ownership matrix + +| Concern | Canonical owner | Allowed GenSim responsibility | +|---|---|---| +| Generated scene authoring | Scene Engine | Generate, edit, import, export, and preserve provenance | +| Provider-free scene identity | `SceneManifest` | Adapt generated scene data once into canonical IDs | +| Live scene identity and metadata | `SceneRegistry` | Hold only references to the canonical integration | +| Dynamic scene state | `SceneSnapshot` and `SceneProvider` | Consume snapshots through Semantic Skill ports | +| Semantic call discovery | `SemanticCallCatalog` | Consume a JSON-safe planner projection | +| Robot capabilities and resources | `RobotSkillProfile` | Express semantic participant constraints only | +| Exact physical resource claims | Bound `RobotSkillProfile` | Use claims returned by canonical preflight/runtime | +| Semantic grounding | `SemanticSkillCompiler` and registered grounders | Bind language roles to canonical scene references | +| Motion and action lowering | `SemanticSkillCompiler` and Atomic Actions | Do not materialize goals, options, or invocations | +| Physical execution | `SkillRuntime` and `ExecutionRunner` | Invoke a narrow semantic execution port | +| Effect verification | Semantic effect monitors and evidence collectors | Consume verified results and evidence summaries | +| Tracking and transport recovery | `ExecutionSession` and `ExecutionRunner` | Observe terminal typed failures only | +| Reacquisition and workflow recovery | `SkillRuntime` | Wait until workflow recovery is exhausted | +| DAG scheduling and route selection | `TaskGraphScheduler` | Own node readiness, route selection, and graph revisions | +| Task-level recovery | `TaskGraphScheduler` | Replace a TaskGroup or unfinished suffix at a safe boundary | +| Task or scene regeneration | Task Engine orchestration | Start a new safe execution transaction | +| Final task success | `SuccessSpec` and final inspection | Inspect final observed state without duplicating call effects | + +## 5. SemanticTaskGraph contract + +### 5.1 Purpose + +`SemanticTaskGraph` replaces the current direct-AtomicAction `SeedGraph`. It is +an immutable, JSON-safe task-planning artifact. It describes semantic intent, +dependencies, candidate routes, and task-level recovery, but contains no +grounded execution data. + +Each node contains exactly one canonical semantic-call payload. A scheduler +may give the selected future suffix to `SkillRuntime` for static look-ahead +while executing only the current node. + +### 5.2 Conceptual schema + +```json +{ + "schema_version": "semantic_task_graph/v1", + "task_id": "place_cube", + "instruction": "Place the cube on the tray", + "planner_route": "selected", + "integration_fingerprint": "", + "nodes": [ + { + "id": "pick_cube", + "call": { + "kind": "pick", + "object": "cube" + }, + "depends_on": [], + "task_instance_id": "e1_0", + "task_type": "E1", + "role": "primary" + }, + { + "id": "place_cube", + "call": { + "kind": "place", + "object": "cube", + "on": "tray" + }, + "depends_on": ["pick_cube"], + "task_instance_id": "e1_0", + "task_type": "E1", + "role": "primary" + } + ], + "task_groups": [ + { + "id": "e1_0", + "task_type": "E1", + "node_ids": ["pick_cube", "place_cube"], + "depends_on": [], + "success": { + "kind": "object_supported_by", + "object": "cube", + "support": "tray" + } + } + ], + "success": { + "kind": "all_task_groups" + } +} +``` + +The concrete schema should reuse the exact `SemanticCallCfg` and decoder from +the Expert Program stack. GenSim must not define another semantic-call JSON +format. + +### 5.3 Allowed node data + +- Stable node and TaskGroup IDs. +- A versioned `SemanticCallCfg` payload. +- Canonical scene-reference IDs. +- Task dependencies and TaskGroup membership. +- Task-level route guards and completion importance. +- Bounded task-level failure routes. +- Planner provenance and confidence. +- Optional semantic resource selections when explicitly bound to the exact + integration fingerprint. + +### 5.4 Forbidden node data + +- Atomic action class or implementation name. +- `ActionInvocation`, typed atomic goal, or action options. +- Arm, hand, control-part, solver, or controller route. +- `MotionPolicy`, planner backend, or recovery thresholds. +- qpos, EEF pose, grasp pose, waypoint, trajectory, or command frame. +- Runtime `ResourceClaim` snapshots. +- Held-object state or speculative physical effects. +- Action-level preconditions and postconditions already owned by the semantic + compiler or effect monitor. + +Task-level object pose constraints may be stored when they are part of the +task intent. Grounded robot or motion coordinates may not be stored. + +### 5.5 Look-ahead without premature execution + +For a selected `Pick -> Place` route, the scheduler should pass both calls to +the canonical runtime analysis window but execute only the first call: + +```python +runtime.start( + pick_call, + place_call, + execution_prefix_length=1, +) +``` + +This lets the compiler use the Place target during grasp selection while +retaining a verified physical boundary after Pick. If the route changes after +Pick, the scheduler can replace only the unfinished suffix. + +If the final canonical runtime does not expose an analysis-window/execution- +prefix boundary, that capability belongs in Semantic Skill, not in GenSim. + +## 6. PR-by-PR adjustment plan + +### 6.1 PR #532: Scene Authoring only + +Recommended title: + +> `feat(scene-engine): add generated-scene authoring and editing` + +Keep: + +- scene generation and editing pipelines; +- layout, gravity settling, and asset preparation; +- image and geometry service boundaries; +- scene import/export; +- semantic evidence and provenance. + +Adjust: + +- Rename the public `SceneGraph` concept to `GeneratedSceneGraph` or + `SceneAuthoringGraph`, or make its authoring-only status explicit. +- Emit stable canonical IDs, parent relationships, affordance evidence, + geometry metadata, and physics provenance. +- Do not construct or own a live `SceneRegistry`. +- Keep richer generation evidence in an audit-only sidecar rather than a + second execution manifest. +- Put conversion to `SceneManifest` in #538's cross-engine adapter. + +Exit criteria: + +- The generated artifact converts deterministically to one canonical + `SceneManifest`. +- Import/export preserves canonical identity and ancestry. +- No authoring artifact contains live simulator handles or pose readers. +- Scene Engine types cannot become a second runtime scene authority. + +### 6.2 PR #533: TaskSpec and SemanticTaskGraph contracts + +Recommended title: + +> `feat(task-engine): add TaskSpec and semantic task-graph contracts` + +Keep: + +- `TaskSpec`, E1-E9 ontology, and `SuccessSpec`; +- strict JSON validation and stable hashing; +- DAG validation and TaskGroup metadata; +- planner provenance and bounded failure policy. + +Transform: + +- Replace direct-AtomicAction `SeedGraph` with `SemanticTaskGraph`. +- Make every graph node contain one canonical `SemanticCallCfg`. +- Replace `capability_catalog_hash` with the canonical semantic integration + fingerprint. +- Validate TaskGroup semantic coverage using calls and success conditions, + rather than hard-coded action sequences. + +Remove: + +- `AtomicCapabilityRegistry`; +- the parallel generic `CapabilityRegistry` when it represents the same + executable surface; +- `build_atomic_capability_registry()`; +- GenSim-owned runtime policy and motion-policy defaults; +- node fields for actor, control, atomic action, and motion policy. + +Unsupported tasks: + +- A TaskSpec may still express a task such as Pour. +- If the semantic catalog has no executable call, planning returns a structured + `unsupported_semantic_capability` result. +- The planner must not emit planning-only fake AtomicAction nodes. + +Exit criteria: + +- Graph JSON round-trips through the same Expert Program semantic-call decoder. +- Forbidden grounded or atomic fields are rejected recursively. +- Graph hashes are deterministic. +- There is one capability/fingerprint source. + +### 6.3 PR #534: Task interpretation and canonical scene binding + +Recommended title: + +> `feat(task-engine): add task interpretation and canonical scene binding` + +Keep: + +- typed instruction interpretation; +- multiple TaskDraft candidates; +- explicit role grounding and ambiguity diagnostics; +- `SceneRequirements` and task-level success conditions; +- strict structured-model boundaries where an MLLM is used. + +Adjust: + +- Bind scene roles only to canonical `SceneObjectRef`, + `SceneArticulationRef`, `SceneLinkRef`, and `SceneAffordanceRef` identities. +- Resolve references through the canonical `SceneManifest`. +- Consume a planner projection generated from `RobotSkillProfile` and + `SemanticCallCatalog`. +- Keep language grounding separate from physical goal grounding. + +Remove: + +- robot/action configuration construction from + `generation/config_builder.py`; +- hard-coded `left_arm`, `right_arm`, gripper states, and packaged robot-action + policy profiles; +- keyword-based affordance or object inference; +- knowledge of planner backends, solvers, controllers, or atomic goal types. + +Exit criteria: + +- Interpretation produces only TaskSpec, role bindings, and semantic call + candidates. +- Canonical identity is resolved once and ambiguity fails explicitly. +- MLLM output cannot bypass the same local schema and catalog validation used + by deterministic callers. + +### 6.4 PR #535: Semantic graph planning and bundle generation + +Recommended title: + +> `feat(task-engine): add semantic task-graph planning and bundles` + +Keep: + +- deterministic offline recipes; +- online semantic task planning; +- candidate scoring and selection; +- conservative TaskGroup-level fusion; +- stable graph loader, hashing, visualization, and bundle artifacts; +- candidate-local preparation failures. + +Adjust: + +- Make all recipes produce Semantic Calls rather than AtomicActions. +- Compile and validate only `SemanticTaskGraph` topology and task semantics. +- Preflight calls through the canonical Expert Program and Semantic Skill + catalogs. +- Derive exact resource conflicts from bound `ResourceClaim` values instead of + persisting claims in the graph. +- Move robot, sensor, and light templates to their owning scene/robot + integration packages. + +Recommended bundle: + +```text +task_spec.json +scene_requirements.json +semantic_task_graph.json +semantic_task_graph.png +integration_fingerprint.json +planner_report.json +``` + +Exit criteria: + +- Offline and online candidates use the same graph schema. +- Candidate fusion occurs only at complete TaskGroup boundaries. +- Every executable candidate passes Semantic Skill provider-free preflight. +- No graph compiler imports or constructs atomic-action implementation types. + +### 6.5 PR #536: Thin Semantic Skill runtime binding + +Recommended title: + +> `feat(task-engine): bind semantic task graphs to SkillRuntime` + +Remove: + +- `ActionGrounder`; +- `AtomicActionAdapter`; +- `atomic_compat.py`; +- `robot_parts.py`; +- `solver_compat.py`; +- GenSim-owned frame-to-action goal lowering; +- GenSim motion-policy materialization; +- duplicate qpos and held-object execution state. + +Replace with a narrow semantic execution boundary: + +1. Decode one graph call through the canonical semantic-call decoder. +2. Validate it against the exact semantic integration fingerprint. +3. Build the selected route's semantic analysis window. +4. Call `SkillRuntime` with an execution prefix. +5. Return the canonical `SkillResult` without reconstructing physical truth. + +Move remaining responsibilities: + +| Current responsibility | New owner | +|---|---| +| camera/depth target materialization | registered Semantic Skill target provider or lowerer | +| live object and articulation goal grounding | `SemanticSkillCompiler` | +| robot arm and endpoint selection | `RobotSkillProfile` | +| grasp collision cache | Atomic Action/graspkit planning service | +| action effect predicate | semantic effect monitor/evidence collector | +| task completion predicate | Task Engine final inspection | + +Exit criteria: + +- One graph node creates one canonical semantic runtime execution boundary. +- Every executed node captures a fresh observation. +- Pick look-ahead can inspect the selected suffix without executing it. +- GenSim does not construct `ActionInvocation` values. +- GenSim does not read qpos or held-object state as an independent authority. + +### 6.6 PR #537: TaskGraphScheduler, graph recovery, and reporting + +Recommended title: + +> `feat(task-engine): add graph scheduling, recovery, and reporting` + +Keep: + +- immutable original graph; +- a detached runtime graph and ordered revision log; +- bounded route and transition budgets; +- unfinished-suffix replanning; +- execution recording and tensor-free reporting; +- shared-state A/B evaluation contracts. + +Replace `ProgramExecutor` with `TaskGraphScheduler`. + +`TaskGraphScheduler` owns only: + +- DAG readiness and deterministic tie-breaking; +- selected-route and TaskGroup transitions; +- a shared task-node barrier with row-local masks; +- calls to `SkillRuntime` or `ParallelSkillRuntime`; +- consumption of verified `SkillResult` and `TaskState` values; +- task-level route substitution after runtime recovery is exhausted; +- graph revision, transition, and task-recovery records. + +Remove: + +- merged-trajectory and per-arm command scheduling; +- controller dispatch and simulation stepping; +- action retry and grasp retry; +- physical effect verification; +- raw qpos and held-object execution state; +- failure classification from exception strings; +- command-level timeout and safe-stop logic. + +Parallel behavior: + +- Delegate physical concurrency to the canonical `ParallelSkillRuntime` and its + required safety validator. +- If those contracts are unavailable, serialize independent ready nodes + deterministically. +- Never merge command frames or trajectories in `TaskGraphScheduler`. + +Naming: + +- Rename `ActionAgent` to `SemanticTaskPlanner` or `TaskPlanAgent`. +- Avoid exposing `gen_sim.action_engine` as a second public execution system. +- Prefer moving planning/runtime-graph code under + `embodichain.gen_sim.task_engine` while the PRs remain unmerged. + +Exit criteria: + +- Graph recovery begins only after a canonical runtime terminal result. +- Verified `TaskState` is the only physical-state transition input. +- Scheduler cancellation delegates safe stop to the active runtime. +- Parallel paths use the canonical parallel safety boundary. +- Reports retain original Semantic Skill events and failures without replacing + them with string-derived classifications. + +### 6.7 PR #538: End-to-end orchestration + +Recommended title: + +> `feat(task-engine): add semantic orchestration and end-to-end integration` + +Keep: + +- Scene/Task/Planner orchestration; +- run-directory isolation; +- feasibility reports; +- final task inspection; +- CLI and artifact publication; +- task or scene regeneration; +- end-to-end benchmarks. + +Adjust: + +- Make `SceneAdapter` produce one canonical `SceneManifest`, a + SceneRegistry-registration plan, and optional audit evidence. +- Make `FeasibilityBroker` consume the Semantic Integration Catalog rather than + an atomic capability registry. +- Make the coordinator invoke `TaskGraphScheduler` rather than + `ProgramExecutor` or `AtomicActionAdapter`. +- Keep final inspection at TaskSpec success level; do not reimplement grasp, + release, handover, or articulation effect checks. +- Treat `StaticSceneManifest`, redacted manifests, and + `ConservativeSceneGraph` as derived evidence or reports, not execution + identity sources. + +Split out unrelated changes: + +- upright-grasp ranking changes in `pick_up.py`; +- yaw-equivalent downstream-reachability changes; +- graspkit implementation changes; +- corresponding Atomic Action tests. + +These are reusable lower-layer enhancements and should land as a focused +Atomic Action prerequisite before the adjusted task-planning stack. + +Exit criteria: + +- One end-to-end path runs from generated scene data through canonical scene + integration, SemanticTaskGraph, SkillRuntime, and final inspection. +- No orchestration component sends controller commands directly. +- Infeasible tasks publish structured failure artifacts without stale bundles. +- Scene/task regeneration starts only after the active runtime reaches a safe + terminal boundary. + +## 7. Dynamic-task support + +Dynamic behavior has several different meanings and must be assigned to the +correct layer. + +### 7.1 Supported behavior + +- A moving target can invalidate and replan the current action through + `SceneEntityPose`, scene dependencies, and `ExecutionSession`. +- Collision-world pose revisions can trigger row-local replanning through the + canonical scene/planner integration. +- Every Semantic Call is observed and JIT-grounded again before execution. +- Pick can use the selected downstream suffix for grasp look-ahead. +- The TaskGraphScheduler can replace an unfinished suffix after a verified + semantic-call boundary. +- Offline and online candidates can be selected or fused at TaskGroup + boundaries. +- The immutable source graph can retain an auditable sequence of runtime + revisions. +- Final task inspection can trigger a bounded repair TaskGroup or new route. + +### 7.2 Explicit limitations + +- An active call cannot be replaced by an unrelated skill in place. +- A graph cannot be rewritten while a physical effect is pending verification. +- Changing runtime controller destinations requires a new invocation and safe + ownership transition. +- Dynamic obstacle pose updates are supported only where the registered scene + provider and planner support them. +- Entity add/remove and geometry changes require a new scene integration and + runtime session; they are not an in-place pose revision. +- The first adjusted version should retain a shared task-node barrier. It may + use row-local success, failure, eligibility, and recovery masks, but should + not claim arbitrary divergent graph program counters per environment. + +## 8. Error-recovery ownership + +| Failure class | Owner | Behavior | +|---|---|---| +| tracking error | `ExecutionSession` / `ExecutionRunner` | bounded row-local replan | +| moving semantic target | `ExecutionSession` | re-ground the same invocation revision within policy | +| collision-world revision | `ExecutionSession` | invalidate and replan affected rows | +| planner failure | Atomic Action runtime | retry within the action policy and emit typed failure | +| controller rejection or timeout | `ExecutionRunner` | cancel addressed targets, then hold safely | +| semantic effect not achieved | `SkillRuntime` | verify evidence and apply the semantic workflow policy | +| held relation lost | `SkillRuntime` | perform bounded physical reacquisition where configured | +| current TaskGroup route exhausted | `TaskGraphScheduler` | choose a different TaskGroup or unfinished suffix | +| final task predicate failed | Task Engine | add a bounded repair route or regenerate the plan | +| task or scene infeasible | Task Engine orchestration | publish failure or create a new transaction | + +Important constraints: + +- The graph scheduler must not insert its own Pick merely because it infers + that an object was dropped. Canonical workflow recovery owns real + reacquisition. +- Task-level recovery begins only after `SkillRuntime` exhausts its own action + and workflow budgets. +- Original typed failures and evidence remain in reports. A task-level category + may summarize them but must not replace them. +- Every layer owns a separate bounded budget so nested recovery cannot form an + unbounded loop. + +Recommended budget hierarchy: + +```text +Atomic recovery budget + inside one ActionInvocation + +Semantic workflow recovery budget + retry or physical reacquisition of one semantic call + +Task graph revision budget + alternate TaskGroup or unfinished suffix + +Task orchestration regeneration budget + new task or scene transaction +``` + +## 9. Components to keep, transform, move, and remove + +### 9.1 Keep + +- `TaskSpec`, E1-E9 ontology, and `SuccessSpec`. +- Offline and online candidate generation. +- TaskGroup grouping and conservative fusion. +- Immutable graph hash, loader, visualization, and artifacts. +- Runtime graph revision log. +- Task-level route recovery and suffix replanning. +- Final task inspection. +- Run directories, recording, reporting, and A/B comparison. + +### 9.2 Transform + +| Current concept | Target concept | +|---|---| +| direct AtomicAction SeedGraph | `SemanticTaskGraph` | +| ActionAgent | `SemanticTaskPlanner` or `TaskPlanAgent` | +| ProgramExecutor | `TaskGraphScheduler` | +| capability registry | planner projection of Semantic Integration Catalog | +| runtime graph node action | canonical `SemanticCallCfg` | +| action postcondition | semantic effect result or task success predicate | +| static scene execution manifest | canonical `SceneManifest` plus evidence sidecar | + +### 9.3 Move + +- Visual target providers to registered Semantic Skill grounders/providers. +- Robot and endpoint selection to `RobotSkillProfile`. +- Grasp collision preparation to Atomic Action/graspkit planning services. +- Atomic grasp-quality improvements to a focused lower-layer prerequisite PR. +- Task success predicates to Task Engine final inspection. +- CLI-only execution assembly to the final orchestration layer. + +### 9.4 Remove + +- `AtomicCapabilityRegistry` and repeated registry builders. +- `ActionGrounder`. +- `AtomicActionAdapter`. +- `ProgramExecutor` as a physical executor. +- GenSim qpos and held-object execution truth. +- GenSim motion policy and robot-part compatibility layers. +- Independent command scheduling, effect verification, and safe-stop logic. +- Hard-coded arm names and robot action templates. +- Planning-only fake actions in executable graph artifacts. + +## 10. Dependency and landing plan + +### Gate 0: consolidate the Semantic Skill stack + +Before adapting the GenSim runtime layers: + +1. Decide whether runtime remains in #492 or is owned by #496. +2. Produce exactly one canonical runtime API. +3. Restack #495-#504 and #480-#483 on the current #492 head. +4. Preserve or land the following shared contracts: + - canonical Semantic Call config/decoder; + - Semantic Integration Catalog and fingerprint; + - physical effect/evidence runtime; + - workflow recovery and reacquisition; + - parallel runtime and safety validator if physical parallelism is required. + +### Recommended merge order + +```text +Semantic Skill runtime consolidation + | + +------ focused Atomic Action grasp enhancement + | + +------ adjusted PR #532 Scene Authoring + | + v +PR #533 SemanticTaskGraph contracts + | + v +PR #534 task interpretation and scene binding + | + v +PR #535 semantic graph planning and bundles + | + v +PR #536 thin SkillRuntime binding + | + v +PR #537 graph scheduling and task-level recovery + | + v +PR #538 end-to-end orchestration +``` + +Operationally, #533-#538 should be marked Draft while their contracts and +branches are rewritten. Their PR bodies should be updated with the new +dependency chain, ownership rules, and explicit removal of the duplicate +execution architecture. + +## 11. Validation plan + +### 11.1 Architecture guards + +- `embodichain/gen_sim/task_engine` must not directly import + `embodichain.lab.sim.atomic_actions`. +- GenSim must not define `AtomicCapabilityRegistry`, `ActionGrounder`, + `AtomicActionAdapter`, or a physical `ProgramExecutor`. +- An architecture test must reject direct controller, simulator-step, or + command-frame ownership in Task Engine. +- A graph-schema test must recursively reject atomic action, motion policy, + qpos, grasp pose, and trajectory fields. +- Capability and integration fingerprints must come from one canonical + Semantic Integration Catalog. + +### 11.2 Contract tests + +- Semantic graph calls round-trip through the Expert Program decoder. +- Canonical scene aliases normalize exactly once. +- Unknown or ambiguous references fail with pathful diagnostics. +- Unsupported semantic capabilities reject a candidate before bundle + publication. +- Integration fingerprint drift fails before execution. +- Offline and online candidates use the same graph and call schema. + +### 11.3 Runtime-boundary tests + +- One graph node creates one semantic runtime execution boundary. +- A fresh observation is captured before every call. +- A selected future suffix participates in compiler look-ahead without + premature execution. +- Verified `TaskState` is the only state adopted by the graph scheduler. +- A controller rejection invokes canonical cancel-then-hold before graph + recovery. +- Workflow reacquisition completes or exhausts before TaskGraphScheduler route + substitution. +- Parallel nodes either use canonical `ParallelSkillRuntime` with a safety + validator or execute serially. + +### 11.4 End-to-end tests + +At minimum, cover: + +1. deterministic Pick -> Place through a generated scene; +2. dual-resource HandOver through one SemanticTaskGraph; +3. moving target recovery during one semantic call; +4. real held-object loss followed by canonical SkillRuntime reacquisition; +5. exhausted semantic recovery followed by TaskGroup route replacement; +6. unsupported task capability producing a preparation failure artifact; +7. offline/online A/B candidates starting from identical state and using the + same Semantic Integration Catalog; +8. final task inspection failing and producing a bounded repair or terminal + report. + +## 12. Stack-wide acceptance criteria + +The adjusted stack is ready only when all of the following are true: + +- Semantic Skill is the only path from semantic intent to `ActionInvocation`. +- Atomic Action and `ExecutionRunner` are the only owners of command execution + and safe stop. +- GenSim artifacts contain semantic calls and task dependencies, not physical + action configuration. +- Scene Engine authoring data converts once into the canonical scene + integration. +- Robot-specific behavior comes from `RobotSkillProfile`, not task recipes. +- Each graph node is JIT-grounded from a fresh observation. +- Effects are committed only from verified Semantic Skill results. +- Graph recovery occurs only at a safe semantic boundary. +- Task-level dynamic replanning preserves completed physical effects and + revises only unfinished work. +- Reports preserve call-, effect-, recovery-, graph-, and final-task evidence + without duplicating physical truth. +- The end-to-end tutorials and benchmarks use the same runtime path as Python, + Expert Program, and model-generated callers. + +## 13. Expected result + +After this adjustment, the two systems become complementary: + +- Semantic Skill supplies one reliable, robot-generic, effect-aware, and + recoverable physical execution architecture. +- GenSim supplies task interpretation, candidate planning, immutable task + graphs, dynamic suffix replanning, task-level recovery, audit artifacts, and + A/B evaluation. + +The final system has one physical truth, one capability source, one grounding +path, and one execution path, while retaining GenSim's task-level planning and +dynamic orchestration strengths. diff --git a/docs/requirements.txt b/docs/requirements.txt index 87db2c491..ffa6f97ea 100644 --- a/docs/requirements.txt +++ b/docs/requirements.txt @@ -1,11 +1,11 @@ -sphinx>=4.0 -autodocsumm -sphinx-book-theme>=0.3.0 -sphinx-tabs -sphinx-copybutton -myst-parser -sphinx-autosummary-accessors -sphinxcontrib-bibtex -sphinx-design -sphinx_autodoc_typehints -pypandoc_binary \ No newline at end of file +sphinx==7.4.7 +autodocsumm==0.2.14 +sphinx-book-theme==1.1.4 +sphinx-tabs==3.4.7 +sphinx-copybutton==0.5.2 +myst-parser==3.0.1 +sphinx-autosummary-accessors==2025.3.1 +sphinxcontrib-bibtex==2.6.5 +sphinx-design==0.6.1 +sphinx-autodoc-typehints==2.3.0 +pypandoc-binary==1.17 diff --git a/docs/scripts/check_api_docs.py b/docs/scripts/check_api_docs.py new file mode 100644 index 000000000..24a0735ee --- /dev/null +++ b/docs/scripts/check_api_docs.py @@ -0,0 +1,533 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Check Sphinx API-reference coverage without modifying repository files. + +Every non-private module's static ``__all__`` declaration is treated as its +public API contract. The checker compares those exports with explicit Sphinx +autodoc and autosummary directives and reports any missing import paths. + +Usage: + python docs/scripts/check_api_docs.py + python docs/scripts/check_api_docs.py --format json +""" + +from __future__ import annotations + +import argparse +import ast +import json +import re +import sys +from dataclasses import dataclass +from pathlib import Path +from typing import Iterator, Sequence + +__all__ = [ + "ApiDocsError", + "CheckResult", + "MissingExport", + "PackageRoot", + "PublicModule", + "check_api_docs", + "collect_documented_exports", + "discover_public_modules", + "find_missing_exports", + "format_json_report", + "format_text_report", +] + + +REPO_ROOT = Path(__file__).resolve().parents[2] +API_REFERENCE_ROOT = REPO_ROOT / "docs" / "source" / "api_reference" + +_DIRECTIVE_RE = re.compile( + r"^(?P[ \t]*)\.\.\s+(?P[\w-]+)::\s*(?P.*?)\s*$" +) +_OPTION_RE = re.compile(r"^:(?P[\w-]+):\s*(?P.*)$") +_OBJECT_DIRECTIVES = frozenset( + { + "autoattribute", + "autoclass", + "autodata", + "autoexception", + "autofunction", + "automethod", + } +) + + +class ApiDocsError(ValueError): + """Raised when the API contract cannot be checked statically.""" + + +@dataclass(frozen=True) +class PackageRoot: + """Map a Python import package to its source directory.""" + + module: str + path: Path + + +@dataclass(frozen=True) +class PublicModule: + """Public exports declared by one Python module.""" + + name: str + exports: tuple[str, ...] + source: Path + + +@dataclass(frozen=True) +class MissingExport: + """One public import path missing from the API reference.""" + + module: str + name: str + source: Path + + @property + def qualified_name(self) -> str: + """Return the complete public import path.""" + return f"{self.module}.{self.name}" + + +@dataclass(frozen=True) +class CheckResult: + """Coverage result returned by the read-only checker.""" + + total_exports: int + missing: tuple[MissingExport, ...] + + @property + def documented_exports(self) -> int: + """Return the number of public exports covered by API docs.""" + return self.total_exports - len(self.missing) + + @property + def is_aligned(self) -> bool: + """Return whether every declared export is documented.""" + return not self.missing + + +DEFAULT_PACKAGE_ROOTS = ( + PackageRoot("embodichain", REPO_ROOT / "embodichain"), + PackageRoot( + "embodichain_tasks", + REPO_ROOT / "embodichain_tasks" / "embodichain_tasks", + ), + PackageRoot( + "embodichain_tasks.configs", + REPO_ROOT / "embodichain_tasks" / "configs", + ), +) + + +def _module_scope_statements(node: ast.AST) -> Iterator[ast.stmt]: + """Yield statements reachable without entering a nested Python scope.""" + scope_boundaries = (ast.AsyncFunctionDef, ast.ClassDef, ast.FunctionDef, ast.Lambda) + for child in ast.iter_child_nodes(node): + if isinstance(child, scope_boundaries): + continue + if isinstance(child, ast.stmt): + yield child + yield from _module_scope_statements(child) + + +def _static_all(tree: ast.Module, source: Path) -> tuple[str, ...] | None: + values: list[ast.expr] = [] + for node in _module_scope_statements(tree): + if isinstance(node, ast.Assign) and any( + isinstance(target, ast.Name) and target.id == "__all__" + for target in node.targets + ): + values.append(node.value) + elif ( + isinstance(node, ast.AnnAssign) + and isinstance(node.target, ast.Name) + and node.target.id == "__all__" + and node.value is not None + ): + values.append(node.value) + + if not values: + return None + + combined_exports: list[str] = [] + seen_exports: set[str] = set() + for value in values: + try: + exports = ast.literal_eval(value) + except (ValueError, TypeError) as exc: + raise ApiDocsError( + f"{source}: __all__ must be a static list of strings" + ) from exc + + if not isinstance(exports, (list, tuple)) or not all( + isinstance(name, str) and name.isidentifier() for name in exports + ): + raise ApiDocsError( + f"{source}: __all__ must contain only valid Python identifiers" + ) + if len(exports) != len(set(exports)): + raise ApiDocsError(f"{source}: __all__ contains duplicate names") + + for name in exports: + if name not in seen_exports: + seen_exports.add(name) + combined_exports.append(name) + + return tuple(combined_exports) + + +def discover_public_modules( + package_roots: Sequence[PackageRoot] = DEFAULT_PACKAGE_ROOTS, +) -> tuple[PublicModule, ...]: + """Discover module APIs declared through static ``__all__`` values. + + Args: + package_roots: Import-package to source-directory mappings to inspect. + + Returns: + Public modules sorted by import path. + + Raises: + ApiDocsError: If a package root is missing or an ``__all__`` declaration + cannot be evaluated statically. + """ + modules: list[PublicModule] = [] + seen_modules: set[str] = set() + + for package_root in package_roots: + if not package_root.path.is_dir(): + raise ApiDocsError(f"Package root does not exist: {package_root.path}") + + for source in sorted(package_root.path.rglob("*.py")): + relative_parts = list( + source.relative_to(package_root.path).with_suffix("").parts + ) + if relative_parts[-1] == "__init__": + relative_parts.pop() + if any(part.startswith("_") for part in relative_parts): + continue + + tree = ast.parse( + source.read_text(encoding="utf-8-sig"), filename=str(source) + ) + exports = _static_all(tree, source) + if not exports: + continue + + module = ".".join((package_root.module, *relative_parts)) + if module in seen_modules: + raise ApiDocsError(f"Duplicate package mapping for {module}") + seen_modules.add(module) + modules.append(PublicModule(module, exports, source)) + + return tuple(sorted(modules, key=lambda item: item.name)) + + +def _directive_end(lines: list[str], start: int, indent: int) -> int: + index = start + 1 + while index < len(lines): + line = lines[index] + if line.strip() and len(line) - len(line.lstrip()) <= indent: + break + index += 1 + return index + + +def _directive_options(lines: list[str], start: int, end: int) -> dict[str, str]: + options: dict[str, str] = {} + active_option: str | None = None + active_indent = 0 + + for line in lines[start + 1 : end]: + stripped = line.strip() + option_match = _OPTION_RE.match(stripped) + if option_match: + active_option = option_match.group("name") + active_indent = len(line) - len(line.lstrip()) + options[active_option] = option_match.group("value") + elif ( + active_option + and stripped + and len(line) - len(line.lstrip()) > active_indent + ): + options[active_option] = f"{options[active_option]} {stripped}".strip() + elif stripped: + active_option = None + return options + + +def _normalize_target(target: str) -> str: + target = target.strip().lstrip("~") + link_match = re.fullmatch(r".*<([^>]+)>", target) + if link_match: + target = link_match.group(1).strip() + return re.sub(r"\(.*\)$", "", target).strip() + + +def _qualify_target( + target: str, context: str | None, public_exports: set[str] +) -> str | None: + normalized = _normalize_target(target) + if normalized in public_exports: + return normalized + if context: + qualified = f"{context}.{normalized}" + if qualified in public_exports: + return qualified + return None + + +def _autosummary_entries(lines: list[str], start: int, end: int) -> tuple[str, ...]: + entries: list[str] = [] + for line in lines[start + 1 : end]: + stripped = line.strip() + if not stripped or stripped.startswith((":", "..")): + continue + entries.append(stripped) + return tuple(entries) + + +def collect_documented_exports( + api_reference_root: Path, + public_modules: Sequence[PublicModule], +) -> set[str]: + """Collect public exports covered by Sphinx API-reference directives. + + Args: + api_reference_root: Root containing API-reference RST files. + public_modules: Public modules to check. + + Returns: + Fully qualified public export paths covered by API documentation. + """ + exports_by_module = { + module.name: {f"{module.name}.{name}" for name in module.exports} + for module in public_modules + } + public_exports = set().union(*exports_by_module.values()) + documented: set[str] = set() + + for rst_path in sorted(api_reference_root.rglob("*.rst")): + relative_path = rst_path.relative_to(api_reference_root) + if "_autosummary" in relative_path.parts: + continue + lines = rst_path.read_text(encoding="utf-8-sig").splitlines() + directives: list[tuple[int, int, str, str, int]] = [] + for index, line in enumerate(lines): + match = _DIRECTIVE_RE.match(line) + if not match: + continue + indent = len(match.group("indent")) + directives.append( + ( + index, + indent, + match.group("name"), + match.group("argument"), + _directive_end(lines, index, indent), + ) + ) + + primary_module = next( + ( + argument + for _, _, name, argument, _ in directives + if name == "automodule" and argument + ), + None, + ) + current_module: str | None = None + + for index, indent, name, argument, end in directives: + if name == "currentmodule": + current_module = argument + continue + + enclosing_module = next( + ( + parent_argument + for parent_index, parent_indent, parent_name, parent_argument, parent_end in reversed( + directives + ) + if parent_name == "automodule" + and parent_index < index < parent_end + and parent_indent < indent + ), + None, + ) + context = enclosing_module or current_module or primary_module + + if name == "automodule" and argument in exports_by_module: + options = _directive_options(lines, index, end) + if "members" in options: + member_option = options["members"] + if member_option: + member_names = { + item.strip() + for item in member_option.split(",") + if item.strip() + } + covered_members = { + f"{argument}.{member}" + for member in member_names + if f"{argument}.{member}" in public_exports + } + else: + covered_members = exports_by_module[argument] + + excluded = { + f"{argument}.{item.strip()}" + for item in options.get("exclude-members", "").split(",") + if item.strip() + } + documented.update(covered_members - excluded) + + if name in _OBJECT_DIRECTIVES: + qualified = _qualify_target(argument, context, public_exports) + if qualified: + documented.add(qualified) + elif name == "autosummary": + for entry in _autosummary_entries(lines, index, end): + qualified = _qualify_target(entry, context, public_exports) + if qualified: + documented.add(qualified) + + return documented + + +def find_missing_exports( + public_modules: Sequence[PublicModule], documented: set[str] +) -> tuple[MissingExport, ...]: + """Return declared public exports absent from API documentation.""" + return tuple( + MissingExport(module.name, name, module.source) + for module in public_modules + for name in module.exports + if f"{module.name}.{name}" not in documented + ) + + +def check_api_docs( + *, + package_roots: Sequence[PackageRoot] = DEFAULT_PACKAGE_ROOTS, + api_reference_root: Path = API_REFERENCE_ROOT, +) -> CheckResult: + """Check API-reference coverage without writing any files. + + Args: + package_roots: Import-package to source-directory mappings to inspect. + api_reference_root: Root containing API-reference RST files. + + Returns: + Coverage counts and missing public import paths. + """ + public_modules = discover_public_modules(package_roots) + documented = collect_documented_exports(api_reference_root, public_modules) + return CheckResult( + total_exports=sum(len(module.exports) for module in public_modules), + missing=find_missing_exports(public_modules, documented), + ) + + +def _source_label(source: Path) -> str: + try: + return source.resolve().relative_to(REPO_ROOT).as_posix() + except ValueError: + return str(source) + + +def format_text_report(result: CheckResult) -> str: + """Format a human-readable coverage report.""" + if result.is_aligned: + return ( + "API docs are aligned: " + f"{result.documented_exports}/{result.total_exports} exports documented." + ) + + lines = [ + "API docs are missing " + f"{len(result.missing)} of {result.total_exports} public exports:" + ] + lines.extend( + f"- {item.qualified_name} ({_source_label(item.source)})" + for item in result.missing + ) + lines.extend( + [ + "", + "Use $update-api-docs to generate or update the corresponding API pages.", + ] + ) + return "\n".join(lines) + + +def format_json_report(result: CheckResult) -> str: + """Format a machine-readable coverage report for agent workflows.""" + payload = { + "documented_exports": result.documented_exports, + "missing": [ + { + "module": item.module, + "name": item.name, + "qualified_name": item.qualified_name, + "source": _source_label(item.source), + } + for item in result.missing + ], + "missing_count": len(result.missing), + "total_exports": result.total_exports, + } + return json.dumps(payload, indent=2, sort_keys=True) + + +def _parse_args(argv: Sequence[str] | None = None) -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="Check Sphinx coverage of module-level __all__ exports." + ) + parser.add_argument( + "--format", + choices=("text", "json"), + default="text", + help="Report format (default: text).", + ) + return parser.parse_args(argv) + + +def main(argv: Sequence[str] | None = None) -> int: + """Run the read-only API documentation checker.""" + args = _parse_args(argv) + try: + result = check_api_docs() + except (ApiDocsError, OSError, SyntaxError) as exc: + print(f"API docs check failed: {exc}", file=sys.stderr) + return 1 + + report = ( + format_json_report(result) + if args.format == "json" + else format_text_report(result) + ) + output = sys.stdout if args.format == "json" or result.is_aligned else sys.stderr + print(report, file=output) + return 0 if result.is_aligned else 1 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/docs/source/_static/atomic_actions/press.gif b/docs/source/_static/atomic_actions/press.gif index 1eafdc319..8013cd5db 100644 Binary files a/docs/source/_static/atomic_actions/press.gif and b/docs/source/_static/atomic_actions/press.gif differ diff --git a/docs/source/_static/atomic_actions/slide_pull.gif b/docs/source/_static/atomic_actions/slide_pull.gif new file mode 100644 index 000000000..415b5b5e5 Binary files /dev/null and b/docs/source/_static/atomic_actions/slide_pull.gif differ diff --git a/docs/source/_static/atomic_actions/slide_push.gif b/docs/source/_static/atomic_actions/slide_push.gif new file mode 100644 index 000000000..4da01f04f Binary files /dev/null and b/docs/source/_static/atomic_actions/slide_push.gif differ diff --git a/docs/source/_static/atomic_actions/twist.gif b/docs/source/_static/atomic_actions/twist.gif new file mode 100644 index 000000000..2c4b720fe Binary files /dev/null and b/docs/source/_static/atomic_actions/twist.gif differ diff --git a/docs/source/_static/tutorials/open_drawer.mp4 b/docs/source/_static/tutorials/open_drawer.mp4 new file mode 100644 index 000000000..d9e1fc146 Binary files /dev/null and b/docs/source/_static/tutorials/open_drawer.mp4 differ diff --git a/docs/source/_static/tutorials/open_drawer_poster.jpg b/docs/source/_static/tutorials/open_drawer_poster.jpg new file mode 100644 index 000000000..57283ed15 Binary files /dev/null and b/docs/source/_static/tutorials/open_drawer_poster.jpg differ diff --git a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst new file mode 100644 index 000000000..970e5dadd --- /dev/null +++ b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.expert_program.rst @@ -0,0 +1,219 @@ +embodichain.lab.gym.envs.expert_program +======================================= + +.. automodule:: embodichain.lab.gym.envs.expert_program + + .. autosummary:: + + ExpertProgramCfg + ExpertProgramIntegrationCfg + ExpertProgramCompiler + CompiledProgram + load_expert_program + loads_expert_program_json + parse_expert_program_json + decode_expert_program + ExpertProgramEnvironmentMixin + ExpertProgramEnvironmentAdapter + SimulationSceneBinding + SimulationResourceEndpointBinding + SimulationRobotResourceBinding + RobotResourceBinding + ControlPartEndpointBinding + ControlPartResourceBinding + SimulationRobotSkillProfileBinding + SimulationExpertProgramFactory + SimulationSegmentPolicyPort + ControlCommandStateEvidenceTracker + ConfiguredHandOverPoseProvider + AcceptedRuntimeCommandObserver + AcceptedRuntimeCommandObserverFactory + AntipodalGraspAffordanceBinding + ArticulationOperationAffordanceBinding + ArticulationOperationTargetBinding + AtomicDemoBridge + BarrierCfg + BufferedGymCommandSink + CompiledBarrier + CompiledParallelBlock + CompiledParallelBranch + CompiledPostPolicy + CompiledProgramAnalysis + CompiledProgramCall + CompiledProgramSegment + CompiledProgramValidator + CompiledRepeatFrame + CompiledTargetSelection + ConfigPath + ConfigPathPart + ControlPartCommandPreset + ContainerAffordanceBinding + CyclicPoseTargetCfg + CuroboParallelCommandSafetyValidator + CuroboParallelSafetyValidatorFactory + DeclarativeCfgValue + DemoBridgeError + EXPERT_PROGRAM_SCHEMA_VERSION + EXPERT_PROGRAM_SCHEMA_VERSION_V2 + EnvironmentStepClock + EnvironmentStepTimingError + ExpertProgramCompileError + ExpertProgramConfigError + ExpertProgramDecodeError + ExpertProgramEnvironmentFactory + ExpertProgramRuntimeAssembly + ExpertProgramSceneResolver + ExpertProgramValidationContext + ExpertProgramValidationError + GymPlanningObservationProvider + HandOverCfg + InvokeCfg + MAX_DECLARATIVE_DEPTH + MAX_DECLARATIVE_NODES + MAX_EXPANDED_CALLS + MAX_EXPERT_PROGRAM_BYTES + MAX_PROGRAM_DEPTH + MAX_PROGRAM_NODES + MAX_REPEAT_COUNT + MaterializedCompiledProgram + MotionGeneratorFactory + ObjectNearTargetValidatorCfg + OperateArticulationCfg + ParallelCfg + PickCfg + PlaceCfg + PlanningObservationPort + PoseCfg + PostPolicyCfg + ProgramNodeCfg + RegisteredSemanticCallCfg + RepeatCfg + RuntimeCommandFrameEncoder + RuntimeTransportActionEncoder + SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS + SceneReferenceRole + SceneRegistryProgramResolver + SegmentCfg + SegmentPostPolicyMetadataPort + SegmentPostPolicyPort + SegmentPostPolicyResultPort + SegmentValidatorMetadataPort + SegmentValidatorPort + SemanticCallCfg + SequenceCfg + SharedTickSceneProvider + SimulationArticulationBinding + SimulationArticulationLinkBinding + SimulationExpertProgramEnvironment + SimulationPlanningObservationProvider + SimulationRigidObjectBinding + SupportSurfaceAffordanceBinding + SkillRuntimeAssemblyPort + TargetCfg + TargetRefCfg + UnsupportedRuntimeTransportError + ValidatorCfg + WaitStablePostCfg + create_simulation_expert_program_adapter + decode_semantic_call + encode_semantic_call + render_config_path + validate_expert_program + EndpointAdapterDeclaration + ExpertProgramIntegrationCatalog + IntegrationFingerprintMismatch + ParallelCommandSafetyValidatorFactory + ParallelSafetyDeclaration + RuntimeTransportDeclaration + SimulationExpertProgramRegistration + StandardExtensionDeclarations + VersionedKey + default_simulation_settle_presets + +.. currentmodule:: embodichain.lab.gym.envs.expert_program + +Schema and loading +------------------ + +The public decoders and file loaders support Expert Program schema versions 1 +and 2. Version 2 adds deterministic parallel blocks with explicit barriers. + +.. autoclass:: ExpertProgramCfg + :members: + +.. autoclass:: ExpertProgramIntegrationCfg + :members: + +.. autofunction:: load_expert_program + +.. autofunction:: loads_expert_program_json + +.. autofunction:: parse_expert_program_json + +.. autofunction:: decode_expert_program + +MLLM frontend +------------- + +The MLLM frontend intentionally accepts only the constrained schema version 1 +surface. Trusted host code remains responsible for authoring version 2 +parallel structure and the integration selection. + +.. autofunction:: embodichain.agents.mllm.decode_mllm_expert_program + +.. autofunction:: embodichain.agents.mllm.compile_mllm_expert_program + +Compilation and environment integration +--------------------------------------- + +.. autoclass:: ExpertProgramCompiler + :members: + +.. autoclass:: CompiledProgram + :members: + +.. autoclass:: ExpertProgramEnvironmentMixin + :members: + +.. autoclass:: ExpertProgramEnvironmentAdapter + :members: + +.. autoclass:: SkillRuntimeAssemblyPort + :members: + +.. autoclass:: ExpertProgramIntegrationCatalog + :members: + +.. autoclass:: SimulationExpertProgramRegistration + :members: + +Simulation integration +---------------------- + +.. autoclass:: SimulationSceneBinding + :members: + +.. autoclass:: SimulationResourceEndpointBinding + +.. autoclass:: SimulationRobotResourceBinding + +.. autoclass:: RobotResourceBinding + :members: + +.. autoclass:: ControlPartEndpointBinding + :members: + +.. autoclass:: ControlPartResourceBinding + :members: + +.. autoclass:: SimulationRobotSkillProfileBinding + :members: + +.. autoclass:: SimulationExpertProgramFactory + :members: + +.. autoclass:: SimulationSegmentPolicyPort + :members: + +.. autoclass:: ControlCommandStateEvidenceTracker + :members: diff --git a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.rst b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.rst index f5617a955..6c6c5dd91 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.rst @@ -21,9 +21,15 @@ through :func:`~embodichain.lab.gym.utils.registration.make`. .. autosummary:: demo + expert_program managers wrapper +.. toctree:: + :hidden: + + embodichain.lab.gym.envs.expert_program + .. currentmodule:: embodichain.lab.gym.envs Environment Classes @@ -60,6 +66,9 @@ segment spans. .. autoclass:: DemoSegment :members: +.. autoclass:: ProcessedEnvAction + :members: + .. autoclass:: DemoSegmentResult :members: diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.primitives.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.primitives.rst index 32e46c1e5..5a8c12268 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.primitives.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.primitives.rst @@ -29,6 +29,10 @@ full-robot timed trajectory and uncommitted expected effects. PlaceOptions Press PressOptions + Slide + SlideOptions + Twist + TwistOptions CoordinatedPickment CoordinatedPickmentOptions CoordinatedPlacement @@ -47,6 +51,8 @@ full-robot timed trajectory and uncommitted expected effects. PlaceGoal AssembleGoal PressGoal + SlideGoal + TwistGoal CoordinatedPickGoal CoordinatedPlacementGoal @@ -100,6 +106,22 @@ Press :show-inheritance: :exclude-members: __init__, copy, replace, to_dict +Slide +----- + +.. automodule:: embodichain.lab.sim.atomic_actions.primitives.slide + :members: + :show-inheritance: + :exclude-members: __init__, copy, replace, to_dict + +Twist +----- + +.. automodule:: embodichain.lab.sim.atomic_actions.primitives.twist + :members: + :show-inheritance: + :exclude-members: __init__, copy, replace, to_dict + CoordinatedPickment ------------------- diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.rst index c2a3d4dde..7544f9d80 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.rst @@ -7,15 +7,16 @@ embodichain.lab.sim.atomic_actions .. autosummary:: - ActionGoal ActionBinding - ResolvedActionBinding - ResolvedControlPart + EndpointBinding + RuntimeEndpointTarget + JointPositionTarget ControlCommand JointPositionCommand ControlPartCommandProfile ActionControlOverrides ActionInvocation + PhaseEffectGateRequirement ResolvedActionRequest ActionOptions MotionPolicy @@ -27,11 +28,28 @@ embodichain.lab.sim.atomic_actions PlanningContext StateDelta TimedTrajectory + RuntimeCommandPayload + JointPositionPayload + EndpointCommand + RuntimeCommandFrame + TimedCommandSequence TrajectorySegment PlannerDiagnostics + ExecutionFeedbackMode ActionPlan CompiledTrajectory + .. rubric:: Semantic resource contracts + + .. autosummary:: + + SkillDescriptor + SkillBindingContract + SkillResourceSlot + SkillEndpointRequirement + DisjointSlotEndpoints + DisjointResourceSlots + .. rubric:: Execution contracts .. autosummary:: @@ -45,6 +63,8 @@ embodichain.lab.sim.atomic_actions RunnerStatus ObservationProvider CommandSink + EndpointCommandTransport + EndpointCommandRouter CommandAcknowledgement CommandAckStatus CommandDispatch @@ -53,7 +73,11 @@ embodichain.lab.sim.atomic_actions SimulationExecutionAdapter ExecutionTick EffectVerificationRequest - JointCommand + EffectVerificationResult + PhaseEffectGateRequest + PhaseEffectGateResult + HeldObjectGuardRequest + HeldObjectGuardResult ExecutionEvent ExecutionEventKind ExecutionStatus @@ -69,6 +93,14 @@ embodichain.lab.sim.atomic_actions PlaceGoal AssembleGoal PressGoal + PressOptions + PressAffordance + SlideGoal + SlideOptions + SlideAffordance + TwistGoal + TwistOptions + TwistAffordance CoordinatedPickGoal CoordinatedPlacementGoal MoveEndEffector @@ -77,6 +109,8 @@ embodichain.lab.sim.atomic_actions MoveHeldObject Place Press + Slide + Twist CoordinatedPickment CoordinatedPlacement HandOver @@ -89,16 +123,55 @@ embodichain.lab.sim.atomic_actions .. currentmodule:: embodichain.lab.sim.atomic_actions +Semantic resource contracts +--------------------------- + +.. autoclass:: SkillDescriptor + :members: + +.. autoclass:: SkillBindingContract + :members: + +.. autoclass:: SkillResourceSlot + :members: + +.. autoclass:: SkillEndpointRequirement + :members: + +.. autoclass:: DisjointSlotEndpoints + :members: + +.. autoclass:: DisjointResourceSlots + :members: + +Standard capability identifiers +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +.. autodata:: JOINT_POSITION_CAPABILITY + +.. autodata:: CARTESIAN_POSE_CAPABILITY + +.. autodata:: FORWARD_KINEMATICS_CAPABILITY + +.. autodata:: INVERSE_KINEMATICS_CAPABILITY + +.. autodata:: BATCH_INVERSE_KINEMATICS_CAPABILITY + +.. autodata:: GRASP_CAPABILITY + Planning and state ------------------ .. autoclass:: ActionBinding :members: -.. autoclass:: ResolvedActionBinding +.. autoclass:: EndpointBinding + :members: + +.. autoclass:: RuntimeEndpointTarget :members: -.. autoclass:: ResolvedControlPart +.. autoclass:: JointPositionTarget :members: .. autoclass:: ControlCommand @@ -151,6 +224,24 @@ Planning and state .. autoclass:: TimedTrajectory :members: +.. autoclass:: RuntimeCommandPayload + :members: + +.. autoclass:: JointPositionPayload + :members: + +.. autoclass:: EndpointCommand + :members: + +.. autoclass:: RuntimeCommandFrame + :members: + +.. autoclass:: TimedCommandSequence + :members: + +.. autoclass:: ExecutionFeedbackMode + :members: + .. autoclass:: ActionPlan :members: @@ -179,6 +270,12 @@ Engine and execution .. autoclass:: CommandSink :members: +.. autoclass:: EndpointCommandTransport + :members: + +.. autoclass:: EndpointCommandRouter + :members: + .. autoclass:: ExecutionClock :members: @@ -209,9 +306,6 @@ Engine and execution .. autoclass:: ExecutionTick :members: -.. autoclass:: JointCommand - :members: - .. autoclass:: ExecutionEvent :members: @@ -229,6 +323,3 @@ Semantic objects and helpers .. autoclass:: HeldObjectState :members: - -.. autoclass:: CoordinatedHeldObjectState - :members: diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.rst index 716df56ef..0f1390158 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.rst @@ -10,9 +10,9 @@ The ``sim`` package is EmbodiChain's simulation core. It is organized around the :class:`SimulationManager` (the DexSim scene handle), the scene-object hierarchy (lights, rigid/soft/cloth bodies, articulations, robots, gizmos, constraints), the sensor suite (cameras, stereo cameras, contact sensors), IK -solvers and motion planners, the atomic-action motion-primitive layer, a -reusable workspace-analysis and sampling toolkit, and the shared configuration -types and utilities that wire all of these together. +solvers and motion planners, the semantic scene registry, the atomic-action +motion-primitive layer, a reusable workspace-analysis and sampling toolkit, and +the shared configuration types and utilities that wire all of these together. .. rubric:: Submodules @@ -132,6 +132,14 @@ Planners embodichain.lab.sim.planners +Semantic Scene Integration +-------------------------- + +.. toctree:: + :maxdepth: 1 + + embodichain.lab.sim.skills + Atomic Actions -------------- diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst new file mode 100644 index 000000000..cfcf25534 --- /dev/null +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst @@ -0,0 +1,390 @@ +embodichain.lab.sim.skills +========================== + +.. automodule:: embodichain.lab.sim.skills + + .. rubric:: Scene integration contracts + + .. autosummary:: + + SceneRegistry + RegistrySceneProvider + SceneEntityRegistration + SceneEntityRef + SceneObjectRef + SceneArticulationRef + SceneLinkRef + SceneAffordanceRef + SceneEntityStateProvider + SceneGeometryProvider + SceneDynamics + SceneCollisionRole + SceneCollisionWorldMode + + .. rubric:: Robot skill profiles + + .. autosummary:: + + RobotSkillProfile + BoundRobotSkillProfile + RobotResource + ResourceEndpoint + ResourceEndpointAdapter + EndpointResolution + ControlPartEndpoint + ControlPartEndpointAdapter + ResourceBinding + ResolvedResourceEndpoint + ResolvedRobotResource + ResolvedSkillBinding + ResourceClaim + SkillPolicyPreset + ProfileValidationError + UnsupportedSkillError + AmbiguousSkillBindingError + + .. rubric:: Semantic calls and runtime + + .. autosummary:: + + SemanticCallSpec + SemanticPose + Pick + Place + HandOver + OperateArticulation + RegisteredSemanticCall + SemanticCallCatalog + SemanticSkillCompiler + AtomicSkills + SkillRuntime + SkillResult + SkillCallTrace + SkillPlanAttemptTrace + SkillEffectTrace + + .. rubric:: Effects, evidence, and parallel execution + + .. autosummary:: + + SemanticEffectSpec + EffectMonitorRef + EffectMonitor + EffectEvidenceCollector + ParallelSkillRuntime + ParallelSkillResult + ParallelCommandSafetyValidator + + .. rubric:: Additional public contracts + + .. autosummary:: + + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY + AmbiguousSceneAffordanceError + AnalyzedSemanticCall + ArticulationJointEvidenceAddress + ArticulationJointObservationCallback + ArticulationJointStateExpectation + BinaryEffectClause + BinaryEffectEvidenceBatch + BinaryEffectEvidenceQuery + BinaryEffectObservation + BinaryEvidenceKind + BinaryObservationCallback + BoundSemanticCall + BoundSemanticIntegration + COMPOSITE_EFFECT_MONITOR_ID + COMPOSITE_EFFECT_MONITOR_REVISION + CONSTRAINT_EFFECT_CHANNEL + CONTACT_EFFECT_CHANNEL + CONTROL_PART_EVIDENCE_PROVIDER_ID + CONTROL_PART_EVIDENCE_PROVIDER_REVISION + CompositeEffectMonitor + CompositeEffectMonitorCfg + CompositeEffectMonitorFactory + ContainerAffordance + ContainerRelationTargetGrounder + ControlPartEvidenceAddress + ControlPartRobotEvidenceSource + ControlPartSimulationEvidenceProvider + CoordinatedHeldObjectCleanupExpectation + DeclarativeValue + EffectClause + EffectEvidenceAddress + EffectEvidenceBatch + EffectEvidenceCollectionContext + EffectEvidenceCollectorPort + EffectEvidenceProvider + EffectEvidenceProviderRegistry + EffectEvidenceQuery + EffectEvidenceQueryValue + EffectEvidenceSourceRef + EffectMonitorDecision + EffectMonitorFactory + EffectMonitorParam + EffectMonitorRegistry + EffectStateExpectation + FORCE_EFFECT_CHANNEL + GRASP_AFFORDANCE_CAPABILITY + GroundedSemanticCall + GroundedHeldObjectGuard + GroundedPhaseEffectGate + HeldObjectGuardBaseline + HandOverPoseProvider + HandOverPoseTargets + HeldObjectRelation + HeldObjectStateExpectation + JOINT_STATE_EFFECT_CHANNEL + JointStateEffectClause + JointStateEvidenceBatch + JointStateEvidenceQuery + JointStateObservation + LinkedSemanticCall + PLACE_IN_AFFORDANCE_CAPABILITY + PLACE_ON_AFFORDANCE_CAPABILITY + PLACEMENT_TARGET_AFFORDANCE_REVISION + POSE_RELATION_EFFECT_CHANNEL + ParallelBarrierUpdate + ParallelBranchPlan + ParallelBranchRuntime + ParallelBranchStaticAnalysis + ParallelConflictError + ParallelLaneCommandSink + ParallelRuntimeBranch + ParallelSafetyError + ParallelStateConflictError + ParallelTimingError + ParallelTimingPolicy + PathPart + PlaceRelationTarget + PoseRelationClause + PoseRelationEvidenceBatch + PoseRelationEvidenceQuery + PoseRelationExpectation + RegisteredSemanticLowerer + RelationTargetGrounder + ResolvedCorePolicyTrace + SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID + SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION + ScalarEffectClause + ScalarEffectEvidenceBatch + ScalarEffectEvidenceQuery + ScalarEffectObservation + ScalarEvidenceKind + ScalarExpectation + ScalarObservationCallback + SceneArticulationEvidenceProvider + SceneArticulationJointStateProvider + SceneEntityManifest + SceneEntityMetadata + SceneManifest + SupportSurfaceAffordance + SupportSurfaceRelationTargetGrounder + SemanticCallDescriptor + SemanticDiagnostic + SemanticEffectDependency + SemanticEffectKind + SemanticHandOverTarget + SemanticIntegrationManifest + SemanticLowering + SemanticObjectTarget + SemanticRelationTarget + SemanticValidationError + SemanticWorkflow + SkillEndpointBindingTrace + SkillEndpointTrackingChannelTrace + SkillWorkflowRecoveryRole + SkillWorkflowRecoveryTrace + WorkflowRecoveryPolicy + EffectExpectationDecision + SkillFailure + SkillRuntimeProvider + SkillScene + SkillStatus + SymbolicStateDomain + SymbolicStateKey + UnsupportedSceneAffordanceError + align_parallel_commands + analyze_parallel_branches + build_effect_evidence_queries + builtin_semantic_call_catalog + merge_parallel_effects + resolve_parallel_barrier + task_state_to_metadata + validate_parallel_claims + +.. currentmodule:: embodichain.lab.sim.skills + +Robot resources and profiles +---------------------------- + +.. autoclass:: RobotSkillProfile + :members: + +.. autoclass:: BoundRobotSkillProfile + :members: + +.. autoclass:: RobotResource + :members: + +.. autoclass:: ResourceEndpoint + :members: + +.. autoclass:: ResourceEndpointAdapter + :members: + +.. autoclass:: EndpointResolution + :members: + +.. autoclass:: ControlPartEndpoint + :members: + +.. autoclass:: ControlPartEndpointAdapter + :members: + +.. autoclass:: ResourceBinding + :members: + +.. autoclass:: ResolvedResourceEndpoint + :members: + +.. autoclass:: ResolvedRobotResource + :members: + +.. autoclass:: ResolvedSkillBinding + :members: + +.. autoclass:: ResourceClaim + :members: + +.. autoclass:: SkillPolicyPreset + :members: + +Profile errors +-------------- + +.. autoclass:: ProfileValidationError + +.. autoclass:: UnsupportedSkillError + +.. autoclass:: AmbiguousSkillBindingError + +Semantic calls and runtime +-------------------------- + +.. autoclass:: SemanticCallSpec + :members: + +.. autoclass:: SemanticPose + :members: + +.. autoclass:: Pick + :members: + +.. autoclass:: Place + :members: + +.. autoclass:: HandOver + :members: + +.. autoclass:: OperateArticulation + :members: + +.. autoclass:: RegisteredSemanticCall + :members: + +.. autoclass:: SemanticCallCatalog + :members: + +.. autoclass:: SemanticSkillCompiler + :members: + +.. autoclass:: AtomicSkills + :members: + +.. autoclass:: SkillRuntime + :members: + +.. autoclass:: SkillResult + :members: + +.. autoclass:: SkillCallTrace + :members: + +.. autoclass:: SkillPlanAttemptTrace + :members: + +.. autoclass:: SkillEffectTrace + :members: + +Effects, evidence, and parallel execution +----------------------------------------- + +.. autoclass:: SemanticEffectSpec + :members: + +.. autoclass:: EffectMonitorRef + :members: + +.. autoclass:: EffectMonitor + :members: + +.. autoclass:: EffectEvidenceCollector + :members: + +.. autoclass:: ParallelSkillRuntime + :members: + +.. autoclass:: ParallelSkillResult + :members: + +.. autoclass:: ParallelCommandSafetyValidator + :members: + +Registry and provider +--------------------- + +.. autoclass:: SceneRegistry + :members: + +.. autoclass:: RegistrySceneProvider + :members: + +Registration contracts +---------------------- + +.. autoclass:: SceneEntityRegistration + :members: + +.. autoclass:: SceneEntityStateProvider + :members: + +.. autoclass:: SceneGeometryProvider + :members: + +References and enums +-------------------- + +.. autoclass:: SceneEntityRef + :members: + +.. autoclass:: SceneObjectRef + :members: + +.. autoclass:: SceneArticulationRef + :members: + +.. autoclass:: SceneLinkRef + :members: + +.. autoclass:: SceneAffordanceRef + :members: + +.. autoclass:: SceneDynamics + :members: + +.. autoclass:: SceneCollisionRole + :members: + +.. autoclass:: SceneCollisionWorldMode + :members: diff --git a/docs/source/api_reference/embodichain/embodichain.utils.rst b/docs/source/api_reference/embodichain/embodichain.utils.rst index 36aa780f0..18b6021c8 100644 --- a/docs/source/api_reference/embodichain/embodichain.utils.rst +++ b/docs/source/api_reference/embodichain/embodichain.utils.rst @@ -19,6 +19,7 @@ and image processing. warp cfg configclass + config_paths device_utils file img_utils @@ -46,6 +47,12 @@ Configuration Classes :undoc-members: :show-inheritance: +Configuration Paths +------------------- + +.. automodule:: embodichain.utils.config_paths + :members: + Configuration Nodes ------------------- diff --git a/docs/source/api_reference/index.rst b/docs/source/api_reference/index.rst index e632cffb4..cc9e16c5f 100644 --- a/docs/source/api_reference/index.rst +++ b/docs/source/api_reference/index.rst @@ -57,3 +57,26 @@ The core ``embodichain`` framework is split into six top-level packages: toolkits learning utils + +Public API Coverage +------------------- + +Public Python APIs are declared through static ``__all__`` values in non-private +modules. Curated API pages remain the preferred place for explanations and +examples. The fallback supplement keeps less prominent exports visible through +their signatures and source docstring summaries. + +Run the read-only checker after changing module exports or API docs: + +.. code-block:: bash + + python docs/scripts/check_api_docs.py + +The checker never edits repository files. If it reports missing exports, use +the ``/update-api-docs`` agent skill to add the appropriate API entries and +documentation. CI runs this same checker after style checks and before tests. + +.. toctree:: + :maxdepth: 1 + + public_api diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst new file mode 100644 index 000000000..c11b81455 --- /dev/null +++ b/docs/source/api_reference/public_api.rst @@ -0,0 +1,2371 @@ +Public API Supplement +===================== + +.. This page is maintained by the update-api-docs agent skill. + docs/scripts/check_api_docs.py reads it but never writes it. + +This page lists module exports declared through ``__all__`` that are not +covered by a more focused API-reference page. Prefer curated pages for APIs +that need deeper explanations or examples. Sphinx obtains signatures and +summaries here from the canonical Python docstrings. + +embodichain.agents.mllm.expert_program +-------------------------------------- + +.. currentmodule:: embodichain.agents.mllm.expert_program + +.. autosummary:: + + compile_mllm_expert_program + decode_mllm_expert_program + +embodichain.data.assets.planner_assets +-------------------------------------- + +.. currentmodule:: embodichain.data.assets.planner_assets + +.. autosummary:: + + download_neural_planner_checkpoint + +embodichain.data.assets.solver_assets +------------------------------------- + +.. currentmodule:: embodichain.data.assets.solver_assets + +.. autosummary:: + + download_neural_ik_checkpoint + +embodichain.data_pipeline.depth_video +------------------------------------- + +.. currentmodule:: embodichain.data_pipeline.depth_video + +.. autosummary:: + + DEFAULT_DEPTH_MIN + DEFAULT_DEPTH_MAX + DEFAULT_DEPTH_SHIFT + DEFAULT_DEPTH_USE_LOG + DEFAULT_DEPTH_PIX_FMT + DEPTH_METER_UNIT + DEPTH_MILLIMETER_UNIT + DEPTH_QMAX + +embodichain.gen_sim.simready_pipeline.cli.start +----------------------------------------------- + +.. currentmodule:: embodichain.gen_sim.simready_pipeline.cli.start + +.. autosummary:: + + cli_ingest_single + main + +embodichain.lab.gym.envs.base_env +--------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.base_env + +.. autosummary:: + + BaseEnv + EnvCfg + +embodichain.lab.gym.envs.demo +----------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.demo + +.. autosummary:: + + DEMO_ANNOTATION_KEYS + DEMO_SCHEMA_VERSION + DemoEpisodeResult + DemoSegment + DemoSegmentResult + ProcessedEnvAction + execute_demo_episode + resolve_demo_segments + +embodichain.lab.gym.envs.embodied_env +------------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.embodied_env + +.. autosummary:: + + EmbodiedEnvCfg + EmbodiedEnv + +embodichain.lab.gym.envs.expert_program.bridge +------------------------------------------------ + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.bridge + +.. autosummary:: + + AcceptedRuntimeCommandObserver + AtomicDemoBridge + BufferedGymCommandSink + CompiledProgramPort + CurrentQposProvider + DemoBridgeError + EnvironmentStepClock + EnvironmentStepTimingError + GymPlanningObservationProvider + JointPositionGymTransportEncoder + ParallelCommandSafetyValidator + RuntimeCommandFrameEncoder + RuntimeTransportActionEncoder + SegmentPostPolicyMetadataPort + SegmentPostPolicyPort + SegmentPostPolicyResultPort + SegmentValidatorMetadataPort + SegmentValidatorPort + SequentialSkillRuntimePort + UnsupportedRuntimeTransportError + +embodichain.lab.gym.envs.expert_program.catalog +------------------------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.catalog + +.. autosummary:: + + ExpertProgramIntegrationCatalog + IntegrationFingerprintMismatch + SimulationExpertProgramRegistration + +embodichain.lab.gym.envs.expert_program.cfg +--------------------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.cfg + +.. autosummary:: + + BarrierCfg + CyclicPoseTargetCfg + DeclarativeCfgValue + EXPERT_PROGRAM_SCHEMA_VERSION + EXPERT_PROGRAM_SCHEMA_VERSION_V2 + ExpertProgramCfg + ExpertProgramIntegrationCfg + HandOverCfg + InvokeCfg + MAX_DECLARATIVE_DEPTH + MAX_DECLARATIVE_NODES + MAX_EXPANDED_CALLS + MAX_PROGRAM_DEPTH + MAX_PROGRAM_NODES + MAX_REPEAT_COUNT + ObjectNearTargetValidatorCfg + OperateArticulationCfg + ParallelCfg + PickCfg + PlaceCfg + PoseCfg + PostPolicyCfg + ProgramNodeCfg + RegisteredSemanticCallCfg + RepeatCfg + SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS + SegmentCfg + SemanticCallCfg + SequenceCfg + TargetCfg + TargetRefCfg + ValidatorCfg + WaitStablePostCfg + +embodichain.lab.gym.envs.expert_program.compiler +-------------------------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.compiler + +.. autosummary:: + + CompiledBarrier + CompiledParallelBlock + CompiledParallelBranch + CompiledPostPolicy + CompiledProgram + CompiledProgramAnalysis + CompiledProgramCall + CompiledProgramSegment + CompiledProgramValidator + CompiledRepeatFrame + CompiledTargetSelection + ExpertProgramCompileError + ExpertProgramCompiler + ExpertProgramSceneResolver + MaterializedCompiledProgram + SceneRegistryProgramResolver + +embodichain.lab.gym.envs.expert_program.decoder +------------------------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.decoder + +.. autosummary:: + + ConfigPath + ConfigPathPart + ExpertProgramConfigError + ExpertProgramDecodeError + ExpertProgramValidationContext + ExpertProgramValidationError + SceneReferenceRole + decode_expert_program + decode_semantic_call + encode_semantic_call + render_config_path + validate_expert_program + +embodichain.lab.gym.envs.expert_program.environment +----------------------------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.environment + +.. autosummary:: + + AcceptedRuntimeCommandObserverFactory + ExpertProgramEnvironmentAdapter + ExpertProgramEnvironmentFactory + ExpertProgramEnvironmentMixin + ExpertProgramRuntimeAssembly + PlanningObservationPort + SkillRuntimeAssemblyPort + +embodichain.lab.gym.envs.expert_program.extensions +---------------------------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.extensions + +.. autosummary:: + + EndpointAdapterDeclaration + ParallelCommandSafetyValidatorFactory + ParallelSafetyDeclaration + RuntimeTransportDeclaration + StandardExtensionDeclarations + VersionedKey + build_standard_extension_declarations + declare_endpoint_adapter + declare_parallel_safety_factory + declare_runtime_transport + validate_immutable_extension_declaration + +embodichain.lab.gym.envs.expert_program.loader +------------------------------------------------ + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.loader + +.. autosummary:: + + MAX_EXPERT_PROGRAM_BYTES + load_expert_program + loads_expert_program_json + parse_expert_program_json + +embodichain.lab.gym.envs.expert_program.simulation +---------------------------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.simulation + +.. autosummary:: + + AntipodalGraspAffordanceBinding + ArticulationOperationAffordanceBinding + ArticulationOperationTargetBinding + ControlPartCommandPreset + ContainerAffordanceBinding + ControlPartEndpointBinding + ControlPartResourceBinding + RobotResourceBinding + SimulationArticulationBinding + SimulationArticulationLinkBinding + SimulationResourceEndpointBinding + SimulationRigidObjectBinding + SimulationRobotResourceBinding + SimulationRobotSkillProfileBinding + SimulationSceneBinding + SupportSurfaceAffordanceBinding + +embodichain.lab.gym.envs.expert_program.simulation_environment +---------------------------------------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.simulation_environment + +.. autosummary:: + + ControlCommandStateEvidenceTracker + MotionGeneratorFactory + SharedTickSceneProvider + SimulationExpertProgramEnvironment + SimulationExpertProgramFactory + SimulationPlanningObservationProvider + create_simulation_expert_program_adapter + +embodichain.lab.gym.envs.expert_program.simulation_policies +------------------------------------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.simulation_policies + +.. autosummary:: + + SimulationSegmentPolicyPort + default_simulation_settle_presets + +embodichain.lab.gym.envs.expert_program.simulation_parallel_safety +------------------------------------------------------------------ + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.simulation_parallel_safety + +.. autosummary:: + + CuroboParallelCommandSafetyValidator + CuroboParallelSafetyValidatorFactory + +embodichain.lab.gym.envs.expert_program.simulation_handover +------------------------------------------------------------ + +.. currentmodule:: embodichain.lab.gym.envs.expert_program.simulation_handover + +.. autosummary:: + + ConfiguredHandOverPoseProvider + +embodichain.lab.gym.envs.settling +--------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.settling + +.. autosummary:: + + DynamicSettleMonitor + DynamicSettleMonitorCfg + DynamicSettleSample + DynamicSettleState + +embodichain.lab.gym.envs.managers.action_manager +------------------------------------------------ + +.. currentmodule:: embodichain.lab.gym.envs.managers.action_manager + +.. autosummary:: + + ActionTerm + ActionManager + +embodichain.lab.gym.envs.managers.actions +----------------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.managers.actions + +.. autosummary:: + + DeltaQposTerm + QposTerm + QposDenormalizedTerm + QposNormalizedTerm + EefPoseTerm + QvelTerm + QfTerm + +embodichain.lab.gym.envs.wrapper.replay +--------------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.wrapper.replay + +.. autosummary:: + + ReplayWrapper + +embodichain.lab.gym.utils +------------------------- + +.. currentmodule:: embodichain.lab.gym.utils + +.. autosummary:: + + EnvProfiler + EnvProfilerCfg + capture_trajectory_state + restore_trajectory_state + +embodichain.lab.gym.utils.gym_utils +----------------------------------- + +.. currentmodule:: embodichain.lab.gym.utils.gym_utils + +.. autosummary:: + + DEFAULT_MANAGER_MODULES + add_env_launcher_args_to_parser + assign_data_to_dict + batch + build_env_cfg_from_args + cat_tensor_with_ids + clip_and_scale_action + config_to_cfg + convert_observation_to_space + dict_array_to_torch_inplace + fetch_data_from_dict + flatten_state_dict + get_dtype_bounds + get_manager_modules + init_rollout_buffer_from_config + init_rollout_buffer_from_gym_space + map_qpos_to_eef_pose + merge_args_with_gym_config + register_manager_modules + to_cpu_tensor + to_tensor + +embodichain.lab.gym.utils.profiler +---------------------------------- + +.. currentmodule:: embodichain.lab.gym.utils.profiler + +.. autosummary:: + + EnvProfilerCfg + EnvProfiler + +embodichain.lab.gym.utils.trajectory_state +------------------------------------------ + +.. currentmodule:: embodichain.lab.gym.utils.trajectory_state + +.. autosummary:: + + capture_trajectory_state + restore_trajectory_state + +embodichain.lab.scripts.analyze_workspace +----------------------------------------- + +.. currentmodule:: embodichain.lab.scripts.analyze_workspace + +.. autosummary:: + + build_sim_cfg + build_robot_cfg + build_preset_robot_cfg + build_analyzer_config + preview_cache + parse_args + main + cli + +embodichain.lab.scripts.preview_asset +------------------------------------- + +.. currentmodule:: embodichain.lab.scripts.preview_asset + +.. autosummary:: + + build_sim_cfg + cli + load_assets + main + preview + +embodichain.lab.scripts.preview_joint_control +--------------------------------------------- + +.. currentmodule:: embodichain.lab.scripts.preview_joint_control + +.. autosummary:: + + ArticulationPreviewController + +embodichain.lab.scripts.preview_lerobot_data +-------------------------------------------- + +.. currentmodule:: embodichain.lab.scripts.preview_lerobot_data + +.. autosummary:: + + EpisodePreview + SegmentPreview + build_episode_preview + cli + inspect_dataset + main + resolve_dataset_root + +embodichain.lab.scripts.run_env +------------------------------- + +.. currentmodule:: embodichain.lab.scripts.run_env + +.. autosummary:: + + cli + generate_and_execute_action_list + generate_function + main + preview + +embodichain.lab.sim +------------------- + +.. currentmodule:: embodichain.lab.sim + +.. autosummary:: + + VisualMaterialCfg + VisualMaterial + VisualMaterialInst + ReuseSegmentState + BatchEntity + SimulationManager + SimulationManagerCfg + SIM_CACHE_DIR + MATERIAL_CACHE_DIR + CONVEX_DECOMP_DIR + REACHABLE_XPOS_DIR + +embodichain.lab.sim.atomic_actions +---------------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_actions + +.. autosummary:: + + ActionPlanningServices + Affordance + AntipodalAffordance + ArticulationJointState + ArticulationOperationAffordance + ArticulationOperationTarget + AssembleAffordance + BASE_POSE_CHANNEL + BUILTIN_ACTION_TYPES + CoordinatedPickmentOptions + CoordinatedPlacementOptions + DynamicCollisionMode + EndpointTrackingChannelBinding + EndpointTrackingFeedbackAddress + EntityState + EffectVerificationRequirement + EffectVerificationResult + EffectExpectationResult + EffectVerifier + ExecutionPlanAttempt + FeedbackTerminalAcceptance + GRASP_COMMAND + InFlightTrackingPolicy + JOINT_POSITION_CHANNEL + JointPositionTrackingEvaluator + JointPositionTrackingMetric + JointPositionTrackingProjector + JointPositionTrackingState + HandOverOptions + HeldObjectGuardVerifier + InteractionPoints + MoveEndEffectorOptions + MoveHeldObjectOptions + MoveJointsOptions + ObjectActionGoal + ObservedArticulationJointState + OPEN_COMMAND + OperateArticulation + OperateArticulationGoal + OperateArticulationOptions + PickUpOptions + PlaceOptions + PhaseEffectGateVerifier + PlanningContextTrackingFeedbackProvider + PoseTrackingEvaluator + PoseTrackingMetric + PoseTrackingState + PoseGoalValue + RigidObjectSceneProvider + RigidObjectSceneProviderCfg + RunnerStepCallback + SceneProvider + SceneArticulationOperationGeometry + SceneSnapshotSupplier + TimedTerminalAcceptance + TimedTrackingSequence + TerminalAcceptance + TrackingCommandProjector + TrackingEvaluation + TrackingEvaluatorRegistry + TrackingFeedbackAddress + TrackingFeedbackBatch + TrackingFeedbackProvider + TrackingFeedbackProviderRegistry + TrackingFeedbackSourceRef + TrackingFrame + TrackingMetricCfg + TrackingMetricEvaluator + TrackingPolicy + TrackingProjectorRef + TrackingProjectorRegistry + TrackingRuntime + TrackingSetpoint + TrackingState + WHOLE_BODY_POSE_CHANNEL + WholeBodyPoseTrackingEvaluator + WholeBodyPoseTrackingMetric + WholeBodyPoseTrackingState + get_registered_actions + register_action + unregister_action + +embodichain.lab.sim.atomic_actions.affordance +--------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_actions.affordance + +.. autosummary:: + + Affordance + AntipodalAffordance + ArticulationOperationAffordance + ArticulationOperationTarget + SlideAffordance + PressAffordance + TwistAffordance + InteractionPoints + AssembleAffordance + +embodichain.lab.sim.atomic_actions.bindings +------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_actions.bindings + +.. autosummary:: + + ActionBinding + EndpointBinding + JointPositionTarget + RuntimeEndpointTarget + +embodichain.lab.sim.atomic_actions.control +------------------------------------------ + +.. currentmodule:: embodichain.lab.sim.atomic_actions.control + +.. autosummary:: + + ActionControlOverrides + ControlCommand + ControlPartCommandProfile + GRASP_COMMAND + JointPositionCommand + OPEN_COMMAND + +embodichain.lab.sim.atomic_actions.core +--------------------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_actions.core + +.. autosummary:: + + AtomicAction + ObjectSemantics + SkillDescriptor + resolve_runtime_device + +embodichain.lab.sim.atomic_actions.effects +------------------------------------------ + +.. currentmodule:: embodichain.lab.sim.atomic_actions.effects + +.. autosummary:: + + StateDelta + +embodichain.lab.sim.atomic_actions.engine +----------------------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_actions.engine + +.. autosummary:: + + AtomicActionEngine + +embodichain.lab.sim.atomic_actions.execution +-------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_actions.execution + +.. autosummary:: + + EffectVerificationRequest + EffectVerificationResult + EffectExpectationResult + ExecutionEvent + ExecutionEventKind + ExecutionPlanAttempt + ExecutionSession + ExecutionStatus + ExecutionTick + HeldObjectGuardRequest + HeldObjectGuardResult + PhaseEffectGateRequest + PhaseEffectGateResult + +embodichain.lab.sim.atomic_actions.goals +---------------------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_actions.goals + +.. autosummary:: + + ActionGoal + ObjectActionGoal + PoseGoalValue + SceneArticulationOperationGeometry + SceneEntityPose + collect_scene_dependencies + resolve_pose_goal + validate_pose_goal + validate_pose_tensor + +embodichain.lab.sim.atomic_actions.invocation +--------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_actions.invocation + +.. autosummary:: + + ActionInvocation + ActionOptions + GoalT + OptionsT + PhaseEffectGateRequirement + ResolvedActionRequest + +embodichain.lab.sim.atomic_actions.plans +---------------------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_actions.plans + +.. autosummary:: + + ActionPlan + CompiledTrajectory + EffectVerificationRequirement + ExecutionFeedbackMode + PlannerDiagnostics + TimedTrajectory + TrajectorySegment + normalize_success_mask + +embodichain.lab.sim.atomic_actions.policies +------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_actions.policies + +.. autosummary:: + + DynamicCollisionMode + MotionPolicy + RecoveryPolicy + +embodichain.lab.sim.atomic_actions.primitives +--------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_actions.primitives + +.. autosummary:: + + BUILTIN_ACTION_TYPES + OperateArticulation + OperateArticulationGoal + OperateArticulationOptions + +embodichain.lab.sim.atomic_actions.primitives.operate_articulation +----------------------------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_actions.primitives.operate_articulation + +.. autosummary:: + + OperateArticulation + OperateArticulationGoal + OperateArticulationOptions + +embodichain.lab.sim.atomic_actions.requirements +----------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_actions.requirements + +.. autosummary:: + + BATCH_INVERSE_KINEMATICS_CAPABILITY + CARTESIAN_POSE_CAPABILITY + DisjointResourceSlots + DisjointSlotEndpoints + FORWARD_KINEMATICS_CAPABILITY + GRASP_CAPABILITY + INVERSE_KINEMATICS_CAPABILITY + JOINT_POSITION_CAPABILITY + SkillBindingContract + SkillEndpointRequirement + SkillResourceSlot + +embodichain.lab.sim.atomic_actions.runner +----------------------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_actions.runner + +.. autosummary:: + + CommandAckStatus + CommandAcknowledgement + CommandDispatch + CommandOperation + CommandSink + EffectVerifier + ExecutionClock + ExecutionRunner + ExecutionRunnerCfg + HeldObjectGuardVerifier + MonotonicExecutionClock + ObservationProvider + PhaseEffectGateVerifier + RunnerStatus + RunnerStep + RunnerStepCallback + +embodichain.lab.sim.atomic_actions.runtime +------------------------------------------ + +.. currentmodule:: embodichain.lab.sim.atomic_actions.runtime + +.. autosummary:: + + ActionPlanningServices + +embodichain.lab.sim.atomic_actions.runtime_commands +--------------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_actions.runtime_commands + +.. autosummary:: + + EndpointCommand + JointPositionPayload + RuntimeCommandFrame + RuntimeCommandPayload + TimedCommandSequence + +embodichain.lab.sim.atomic_actions.scene +---------------------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_actions.scene + +.. autosummary:: + + SceneProvider + +embodichain.lab.sim.atomic_actions.sim_adapter +---------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_actions.sim_adapter + +.. autosummary:: + + RigidObjectSceneProvider + RigidObjectSceneProviderCfg + SceneSnapshotSupplier + SimulationExecutionAdapter + +embodichain.lab.sim.atomic_actions.state +---------------------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_actions.state + +.. autosummary:: + + EntityState + ArticulationJointState + CoordinatedHeldObjectState + HeldObjectState + ObservedArticulationJointState + PlanningContext + RobotObservation + SceneSnapshot + TaskState + +embodichain.lab.sim.atomic_actions.tracking +------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_actions.tracking + +.. autosummary:: + + BASE_POSE_CHANNEL + FeedbackTerminalAcceptance + InFlightTrackingPolicy + JOINT_POSITION_CHANNEL + JointPositionTrackingEvaluator + JointPositionTrackingMetric + JointPositionTrackingProjector + JointPositionTrackingState + EndpointTrackingChannelBinding + EndpointTrackingFeedbackAddress + PlanningContextTrackingFeedbackProvider + PoseTrackingEvaluator + PoseTrackingMetric + PoseTrackingState + TerminalAcceptance + TimedTerminalAcceptance + TimedTrackingSequence + TrackingChannelId + TrackingCommandProjector + TrackingEvaluation + TrackingEvaluatorRegistry + TrackingFeedbackAddress + TrackingFeedbackBatch + TrackingFeedbackProvider + TrackingFeedbackProviderRegistry + TrackingFeedbackSourceRef + TrackingFrame + TrackingMetricCfg + TrackingMetricEvaluator + TrackingPolicy + TrackingProjectorRef + TrackingProjectorRegistry + TrackingRuntime + TrackingSetpoint + TrackingState + WHOLE_BODY_POSE_CHANNEL + WholeBodyPoseTrackingEvaluator + WholeBodyPoseTrackingMetric + WholeBodyPoseTrackingState + +embodichain.lab.sim.atomic_actions.trajectory_ops +------------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_actions.trajectory_ops + +.. autosummary:: + + axis_translation_keyframes + build_joint_plan_states + build_pose_plan_states + interpolate_hand_qpos + interpolate_joint_trajectory + resolve_joint_target + resolve_pose_target + split_three_segments + to_full_robot_trajectory + translate_pose_world + +embodichain.lab.sim.atomic_actions.transports +--------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_actions.transports + +.. autosummary:: + + EndpointCommandRouter + EndpointCommandTransport + +embodichain.lab.sim.objects.articulation +---------------------------------------- + +.. currentmodule:: embodichain.lab.sim.objects.articulation + +.. autosummary:: + + ArticulationData + Articulation + +embodichain.lab.sim.objects.cloth_object +---------------------------------------- + +.. currentmodule:: embodichain.lab.sim.objects.cloth_object + +.. autosummary:: + + ClothBodyData + ClothObject + ClothObjectCfg + +embodichain.lab.sim.objects.constraint +-------------------------------------- + +.. currentmodule:: embodichain.lab.sim.objects.constraint + +.. autosummary:: + + RigidConstraint + +embodichain.lab.sim.objects.gizmo +--------------------------------- + +.. currentmodule:: embodichain.lab.sim.objects.gizmo + +.. autosummary:: + + Gizmo + GizmoCfg + +embodichain.lab.sim.objects.rigid_object +---------------------------------------- + +.. currentmodule:: embodichain.lab.sim.objects.rigid_object + +.. autosummary:: + + RigidBodyData + RigidObject + RigidObjectCfg + +embodichain.lab.sim.objects.rigid_object_group +---------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.objects.rigid_object_group + +.. autosummary:: + + RigidBodyGroupData + RigidObjectGroup + RigidObjectGroupCfg + +embodichain.lab.sim.objects.robot +--------------------------------- + +.. currentmodule:: embodichain.lab.sim.objects.robot + +.. autosummary:: + + ControlGroup + Robot + +embodichain.lab.sim.objects.soft_object +--------------------------------------- + +.. currentmodule:: embodichain.lab.sim.objects.soft_object + +.. autosummary:: + + SoftBodyData + SoftObject + SoftObjectCfg + +embodichain.lab.sim.planners.base_planner +----------------------------------------- + +.. currentmodule:: embodichain.lab.sim.planners.base_planner + +.. autosummary:: + + BasePlannerCfg + CollisionWorldInfo + PlanOptions + BasePlanner + validate_plan_options + +embodichain.lab.sim.planners.curobo.curobo_planner +-------------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.planners.curobo.curobo_planner + +.. autosummary:: + + CuroboAutoGenCfg + CuroboPlanOptions + CuroboPlanner + CuroboPlannerCfg + CuroboWorldCfg + +embodichain.lab.sim.planners.curobo.curobo_yaml +----------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.planners.curobo.curobo_yaml + +.. autosummary:: + + generate_curobo_robot_yaml + generate_curobo_world_yaml + +embodichain.lab.sim.planners.motion_generator +--------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.planners.motion_generator + +.. autosummary:: + + MotionGenerator + MotionGenCfg + MotionGenOptions + +embodichain.lab.sim.planners.neural_planner +------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.planners.neural_planner + +.. autosummary:: + + NeuralPlanner + NeuralPlannerCfg + NeuralPlanOptions + +embodichain.lab.sim.planners.toppra_planner +------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.planners.toppra_planner + +.. autosummary:: + + ToppraPlanner + ToppraPlannerCfg + ToppraPlanOptions + +embodichain.lab.sim.planners.utils +---------------------------------- + +.. currentmodule:: embodichain.lab.sim.planners.utils + +.. autosummary:: + + TrajectorySampleMethod + MovePart + MoveType + PlanState + PlanResult + normalize_success_mask + calculate_point_allocations + interpolate_xpos + interpolate_xpos_batched + +embodichain.lab.sim.robots.cobotmagic +------------------------------------- + +.. currentmodule:: embodichain.lab.sim.robots.cobotmagic + +.. autosummary:: + + CobotMagicCfg + +embodichain.lab.sim.robots.dexforce_w1.hand_specs +------------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.robots.dexforce_w1.hand_specs + +.. autosummary:: + + W1HandSideSpec + W1HandSpec + get_default_w1_hand_version + get_w1_hand_spec + normalize_w1_hand_mappings + +embodichain.lab.sim.robots.dexforce_w1.specs +-------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.robots.dexforce_w1.specs + +.. autosummary:: + + W1VersionSpec + get_w1_version_spec + +embodichain.lab.sim.robots.dexforce_w1.types +-------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.robots.dexforce_w1.types + +.. autosummary:: + + DexforceW1Version + DexforceW1HandVersion + DexforceW1ArmSide + DexforceW1Type + DexforceW1HandBrand + +embodichain.lab.sim.robots.dexforce_w1.utils +-------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.robots.dexforce_w1.utils + +.. autosummary:: + + ChassisManager + TorsoManager + HeadManager + ArmManager + HandManager + EyesManager + build_dexforce_w1_assembly_urdf_cfg + +embodichain.lab.sim.robots.dual_arm +----------------------------------- + +.. currentmodule:: embodichain.lab.sim.robots.dual_arm + +.. autosummary:: + + DualArmRobotCfg + build_dual_arm_cfg + resolve_mounts + +embodichain.lab.sim.robots.franka_panda +--------------------------------------- + +.. currentmodule:: embodichain.lab.sim.robots.franka_panda + +.. autosummary:: + + FrankaPandaCfg + +embodichain.lab.sim.robots.ur_robot +----------------------------------- + +.. currentmodule:: embodichain.lab.sim.robots.ur_robot + +.. autosummary:: + + URRobotCfg + +embodichain.lab.sim.sensors.camera +---------------------------------- + +.. currentmodule:: embodichain.lab.sim.sensors.camera + +.. autosummary:: + + Camera + CameraCfg + +embodichain.lab.sim.sim_manager +------------------------------- + +.. currentmodule:: embodichain.lab.sim.sim_manager + +.. autosummary:: + + SIM_CACHE_DIR + MATERIAL_CACHE_DIR + CONVEX_DECOMP_DIR + REACHABLE_XPOS_DIR + +embodichain.lab.sim.skills.calls +-------------------------------- + +.. currentmodule:: embodichain.lab.sim.skills.calls + +.. autosummary:: + + DeclarativeValue + HandOver + OperateArticulation + Pick + Place + PlaceRelationTarget + RegisteredSemanticCall + SemanticCallCatalog + SemanticCallDescriptor + SemanticCallSpec + SemanticPose + builtin_semantic_call_catalog + +embodichain.lab.sim.skills.compiler +----------------------------------- + +.. currentmodule:: embodichain.lab.sim.skills.compiler + +.. autosummary:: + + AnalyzedSemanticCall + GroundedSemanticCall + GroundedHeldObjectGuard + GroundedPhaseEffectGate + ContainerRelationTargetGrounder + HandOverPoseProvider + HandOverPoseTargets + HeldObjectGuardBaseline + RelationTargetGrounder + RegisteredSemanticLowerer + SemanticEffectDependency + SemanticEffectKind + SemanticHandOverTarget + SemanticLowering + SemanticObjectTarget + SemanticRelationTarget + SemanticSkillCompiler + SupportSurfaceRelationTargetGrounder + SemanticWorkflow + +embodichain.lab.sim.skills.effects +---------------------------------- + +.. currentmodule:: embodichain.lab.sim.skills.effects + +.. autosummary:: + + ArticulationJointStateExpectation + BinaryEffectClause + BinaryEffectEvidenceBatch + BinaryEvidenceKind + COMPOSITE_EFFECT_MONITOR_ID + COMPOSITE_EFFECT_MONITOR_REVISION + CONSTRAINT_EFFECT_CHANNEL + CONTACT_EFFECT_CHANNEL + CONTROL_PART_EVIDENCE_PROVIDER_ID + CONTROL_PART_EVIDENCE_PROVIDER_REVISION + CompositeEffectMonitor + CompositeEffectMonitorCfg + CompositeEffectMonitorFactory + ControlPartEvidenceAddress + CoordinatedHeldObjectCleanupExpectation + EffectClause + EffectEvidenceAddress + EffectEvidenceBatch + EffectEvidenceSourceRef + EffectExpectationDecision + EffectExpectationDecision + EffectMonitor + EffectMonitorDecision + EffectMonitorFactory + EffectMonitorParam + EffectMonitorRef + EffectMonitorRegistry + EffectStateExpectation + FORCE_EFFECT_CHANNEL + HeldObjectRelation + HeldObjectStateExpectation + JOINT_STATE_EFFECT_CHANNEL + JointStateEffectClause + JointStateEvidenceBatch + POSE_RELATION_EFFECT_CHANNEL + PoseRelationClause + PoseRelationEvidenceBatch + PoseRelationExpectation + ScalarEffectClause + ScalarEffectEvidenceBatch + ScalarEvidenceKind + ScalarExpectation + SemanticEffectKind + SemanticEffectSpec + SymbolicStateDomain + SymbolicStateKey + +embodichain.lab.sim.skills.evidence +----------------------------------- + +.. currentmodule:: embodichain.lab.sim.skills.evidence + +.. autosummary:: + + ArticulationJointObservationCallback + BinaryEffectEvidenceQuery + BinaryEffectObservation + BinaryObservationCallback + ControlPartRobotEvidenceSource + ControlPartSimulationEvidenceProvider + EffectEvidenceCollectionContext + EffectEvidenceCollector + EffectEvidenceProvider + EffectEvidenceProviderRegistry + EffectEvidenceQuery + EffectEvidenceQueryValue + JointStateEvidenceQuery + JointStateObservation + PoseRelationEvidenceQuery + ScalarEffectEvidenceQuery + ScalarEffectObservation + ScalarObservationCallback + SceneArticulationEvidenceProvider + build_effect_evidence_queries + +embodichain.lab.sim.skills.integration +-------------------------------------- + +.. currentmodule:: embodichain.lab.sim.skills.integration + +.. autosummary:: + + BoundSemanticCall + LinkedSemanticCall + PathPart + SceneEntityManifest + SceneManifest + SemanticDiagnostic + SemanticIntegrationManifest + SemanticValidationError + +embodichain.lab.sim.skills.parallel +----------------------------------- + +.. currentmodule:: embodichain.lab.sim.skills.parallel + +.. autosummary:: + + ParallelBarrierUpdate + ParallelBranchPlan + ParallelConflictError + ParallelStateConflictError + ParallelTimingError + ParallelTimingPolicy + align_parallel_commands + merge_parallel_effects + resolve_parallel_barrier + validate_parallel_claims + +embodichain.lab.sim.skills.parallel_runtime +------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.skills.parallel_runtime + +.. autosummary:: + + ParallelBranchRuntime + ParallelBranchStaticAnalysis + ParallelCommandSafetyValidator + ParallelLaneCommandSink + ParallelRuntimeBranch + ParallelSafetyError + ParallelSkillResult + ParallelSkillRuntime + analyze_parallel_branches + +embodichain.lab.sim.skills.profiles +----------------------------------- + +.. currentmodule:: embodichain.lab.sim.skills.profiles + +.. autosummary:: + + AmbiguousSkillBindingError + BoundRobotSkillProfile + ControlPartEndpoint + ControlPartEndpointAdapter + EndpointResolution + ProfileValidationError + ResourceEndpoint + ResourceEndpointAdapter + ResolvedRobotResource + ResolvedResourceEndpoint + ResolvedSkillBinding + ResourceBinding + ResourceClaim + RobotResource + RobotSkillProfile + SkillPolicyPreset + WorkflowRecoveryPolicy + UnsupportedSkillError + +embodichain.lab.sim.skills.runtime +---------------------------------- + +.. currentmodule:: embodichain.lab.sim.skills.runtime + +.. autosummary:: + + AtomicSkills + EffectEvidenceCollectorPort + ResolvedCorePolicyTrace + SkillCallTrace + SkillEndpointBindingTrace + SkillEndpointTrackingChannelTrace + SkillEffectTrace + SkillFailure + SkillPlanAttemptTrace + SkillResult + SkillRuntime + SkillRuntimeProvider + SkillScene + SkillStatus + SkillWorkflowRecoveryRole + SkillWorkflowRecoveryTrace + task_state_to_metadata + +embodichain.lab.sim.skills.scene +-------------------------------- + +.. currentmodule:: embodichain.lab.sim.skills.scene + +.. autosummary:: + + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY + ArticulationJointEvidenceAddress + ContainerAffordance + SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID + SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION + SceneArticulationJointStateProvider + RegistrySceneProvider + SceneAffordanceRef + SceneArticulationRef + SceneCollisionRole + SceneCollisionWorldMode + SceneDynamics + SceneEntityRef + SceneEntityMetadata + SceneEntityRegistration + SceneEntityStateProvider + SceneGeometryProvider + SceneLinkRef + SceneObjectRef + SceneRegistry + AmbiguousSceneAffordanceError + GRASP_AFFORDANCE_CAPABILITY + PLACE_IN_AFFORDANCE_CAPABILITY + PLACE_ON_AFFORDANCE_CAPABILITY + PLACEMENT_TARGET_AFFORDANCE_REVISION + SupportSurfaceAffordance + UnsupportedSceneAffordanceError + +embodichain.lab.sim.solvers.neural_ik_solver +-------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.solvers.neural_ik_solver + +.. autosummary:: + + NeuralIKSolverCfg + NeuralIKSolver + +embodichain.lab.sim.solvers.srs_solver +-------------------------------------- + +.. currentmodule:: embodichain.lab.sim.solvers.srs_solver + +.. autosummary:: + + SRSSolver + SRSSolverCfg + +embodichain.lab.sim.utility.render_utils +---------------------------------------- + +.. currentmodule:: embodichain.lab.sim.utility.render_utils + +.. autosummary:: + + select_default_renderer + +embodichain.lab.sim.workspace.caches.cache_utils +------------------------------------------------ + +.. currentmodule:: embodichain.lab.sim.workspace.caches.cache_utils + +.. autosummary:: + + clean_all_sessions + clean_session + format_size + get_cache_root + get_dir_size + list_sessions + main + show_session_info + show_total_size + +embodichain.lab.sim.workspace.caches.results_cache +-------------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.workspace.caches.results_cache + +.. autosummary:: + + DEFAULT_RESULTS_CACHE_DIR + ResultsCache + compute_cache_key + serialize_results + deserialize_results + +embodichain.lab.sim.workspace.constraints.base_constraint +--------------------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.workspace.constraints.base_constraint + +.. autosummary:: + + IConstraintChecker + BaseConstraintChecker + +embodichain.lab.sim.workspace.constraints.workspace_constraint +-------------------------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.workspace.constraints.workspace_constraint + +.. autosummary:: + + WorkspaceConstraintChecker + +embodichain.lab.sim.workspace.samplers.base_sampler +--------------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.workspace.samplers.base_sampler + +.. autosummary:: + + ISampler + BaseSampler + +embodichain.lab.sim.workspace.samplers.gaussian_sampler +------------------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.workspace.samplers.gaussian_sampler + +.. autosummary:: + + GaussianSampler + +embodichain.lab.sim.workspace.samplers.halton_sampler +----------------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.workspace.samplers.halton_sampler + +.. autosummary:: + + HaltonSampler + +embodichain.lab.sim.workspace.samplers.importance_sampler +--------------------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.workspace.samplers.importance_sampler + +.. autosummary:: + + ImportanceSampler + +embodichain.lab.sim.workspace.samplers.iniform_sampler +------------------------------------------------------ + +.. currentmodule:: embodichain.lab.sim.workspace.samplers.iniform_sampler + +.. autosummary:: + + UniformSampler + +embodichain.lab.sim.workspace.samplers.lhs_sampler +-------------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.workspace.samplers.lhs_sampler + +.. autosummary:: + + LatinHypercubeSampler + +embodichain.lab.sim.workspace.samplers.random_sampler +----------------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.workspace.samplers.random_sampler + +.. autosummary:: + + RandomSampler + +embodichain.lab.sim.workspace.samplers.sobol_sampler +---------------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.workspace.samplers.sobol_sampler + +.. autosummary:: + + SobolSampler + +embodichain.lab.sim.workspace.visualizers.axis_visualizer +--------------------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.workspace.visualizers.axis_visualizer + +.. autosummary:: + + AxisVisualizer + +embodichain.lab.sim.workspace.visualizers.base_visualizer +--------------------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.workspace.visualizers.base_visualizer + +.. autosummary:: + + IVisualizer + BaseVisualizer + +embodichain.lab.sim.workspace.visualizers.point_cloud_visualizer +---------------------------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.workspace.visualizers.point_cloud_visualizer + +.. autosummary:: + + PointCloudVisualizer + +embodichain.lab.sim.workspace.visualizers.sphere_visualizer +----------------------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.workspace.visualizers.sphere_visualizer + +.. autosummary:: + + SphereVisualizer + +embodichain.lab.sim.workspace.visualizers.visualizer_factory +------------------------------------------------------------ + +.. currentmodule:: embodichain.lab.sim.workspace.visualizers.visualizer_factory + +.. autosummary:: + + VisualizerFactory + create_visualizer + +embodichain.lab.sim.workspace.visualizers.voxel_visualizer +---------------------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.workspace.visualizers.voxel_visualizer + +.. autosummary:: + + VoxelVisualizer + +embodichain.lab.visualization.backends +-------------------------------------- + +.. currentmodule:: embodichain.lab.visualization.backends + +.. autosummary:: + + VisualizationBackend + ViserBackend + +embodichain.lab.visualization.backends.base +------------------------------------------- + +.. currentmodule:: embodichain.lab.visualization.backends.base + +.. autosummary:: + + VisualizationBackend + +embodichain.lab.visualization.backends.viser +-------------------------------------------- + +.. currentmodule:: embodichain.lab.visualization.backends.viser + +.. autosummary:: + + ViserBackend + +embodichain.lab.visualization.cfg +--------------------------------- + +.. currentmodule:: embodichain.lab.visualization.cfg + +.. autosummary:: + + VisualizationCfg + ViserServerCfg + +embodichain.lab.visualization.cli +--------------------------------- + +.. currentmodule:: embodichain.lab.visualization.cli + +.. autosummary:: + + add_viser_args_to_parser + visualization_cfg_from_args + +embodichain.lab.visualization.protocol +-------------------------------------- + +.. currentmodule:: embodichain.lab.visualization.protocol + +.. autosummary:: + + SCHEMA_VERSION + CameraImage + CameraImageFrame + CameraSpec + DynamicMeshUpdate + FrameOverlay + GizmoCommand + GizmoSpec + GizmoState + JointControlCommand + JointControlProvider + JointControlSpec + JointControlState + MeshGeometry + PointCloudOverlay + SceneFrame + SceneManifest + SceneNode + SceneOverlays + TargetOverlay + TrajectoryOverlay + estimate_camera_image_frame_bytes + estimate_frame_bytes + estimate_manifest_bytes + pose_to_position_wxyz + +embodichain.lab.visualization.runtime +------------------------------------- + +.. currentmodule:: embodichain.lab.visualization.runtime + +.. autosummary:: + + GizmoCommandQueue + JointControlCommandQueue + LatestFrameQueue + RuntimeHealth + RuntimeStats + VisualizationRuntime + +embodichain.lab.visualization.scene_exporter +-------------------------------------------- + +.. currentmodule:: embodichain.lab.visualization.scene_exporter + +.. autosummary:: + + CameraImageCaptureResult + CaptureResult + SceneExporter + mesh_geometry_id + safe_path_component + +embodichain.learning.rl.algo.apg +-------------------------------- + +.. currentmodule:: embodichain.learning.rl.algo.apg + +.. autosummary:: + + APG + APGCfg + segmented_discounted_return + +embodichain.learning.rl.algo.base +--------------------------------- + +.. currentmodule:: embodichain.learning.rl.algo.base + +.. autosummary:: + + BaseAlgorithm + RolloutKind + +embodichain.learning.rl.algo.common +----------------------------------- + +.. currentmodule:: embodichain.learning.rl.algo.common + +.. autosummary:: + + compute_gae + +embodichain.learning.rl.algo.grpo +--------------------------------- + +.. currentmodule:: embodichain.learning.rl.algo.grpo + +.. autosummary:: + + GRPO + GRPOCfg + +embodichain.learning.rl.algo.ppo +-------------------------------- + +.. currentmodule:: embodichain.learning.rl.algo.ppo + +.. autosummary:: + + PPO + PPOCfg + +embodichain.learning.rl.collector.base +-------------------------------------- + +.. currentmodule:: embodichain.learning.rl.collector.base + +.. autosummary:: + + BaseCollector + +embodichain.learning.rl.collector.differentiable +------------------------------------------------ + +.. currentmodule:: embodichain.learning.rl.collector.differentiable + +.. autosummary:: + + DifferentiableCollector + DifferentiableRollout + DifferentiableTransition + +embodichain.learning.rl.collector.sync_collector +------------------------------------------------ + +.. currentmodule:: embodichain.learning.rl.collector.sync_collector + +.. autosummary:: + + SyncCollector + +embodichain.learning.rl.experimental.newton +------------------------------------------- + +.. currentmodule:: embodichain.learning.rl.experimental.newton + +.. autosummary:: + + NewtonPlanarReachEnv + NewtonPlanarReachEnvCfg + +embodichain.learning.rl.experimental.newton.planar_reach +-------------------------------------------------------- + +.. currentmodule:: embodichain.learning.rl.experimental.newton.planar_reach + +.. autosummary:: + + NewtonPlanarReachEnv + NewtonPlanarReachEnvCfg + +embodichain.learning.rl.experimental.newton.train_planar_reach +-------------------------------------------------------------- + +.. currentmodule:: embodichain.learning.rl.experimental.newton.train_planar_reach + +.. autosummary:: + + NewtonPlanarReachTrainingCfg + train_planar_reach + +embodichain.learning.rl.models.actor_critic +------------------------------------------- + +.. currentmodule:: embodichain.learning.rl.models.actor_critic + +.. autosummary:: + + ActorCritic + +embodichain.learning.rl.models.actor_only +----------------------------------------- + +.. currentmodule:: embodichain.learning.rl.models.actor_only + +.. autosummary:: + + ActorOnly + +embodichain.learning.rl.models.policy +------------------------------------- + +.. currentmodule:: embodichain.learning.rl.models.policy + +.. autosummary:: + + Policy + +embodichain.learning.rl.utils.optimizer +--------------------------------------- + +.. currentmodule:: embodichain.learning.rl.utils.optimizer + +.. autosummary:: + + bind_scheduler_horizon + build_lr_scheduler + build_optimizer + coerce_lr_scheduler_cfg + coerce_optimizer_cfg + get_registered_lr_scheduler_names + get_registered_optimizer_names + scheduler_needs_horizon + +embodichain.toolkits.acd +------------------------ + +.. currentmodule:: embodichain.toolkits.acd + +.. autosummary:: + + generate_urdf_collision_convexes + +embodichain.toolkits.acd.cli +---------------------------- + +.. currentmodule:: embodichain.toolkits.acd.cli + +.. autosummary:: + + main + +embodichain.toolkits.acd.urdf_modifider +--------------------------------------- + +.. currentmodule:: embodichain.toolkits.acd.urdf_modifider + +.. autosummary:: + + URDFModifider + +embodichain.toolkits.graspkit.pg_grasp.antipodal_generator +---------------------------------------------------------- + +.. currentmodule:: embodichain.toolkits.graspkit.pg_grasp.antipodal_generator + +.. autosummary:: + + GraspGenerator + GraspGeneratorCfg + +embodichain.toolkits.graspkit.pg_grasp.antipodal_sampler +-------------------------------------------------------- + +.. currentmodule:: embodichain.toolkits.graspkit.pg_grasp.antipodal_sampler + +.. autosummary:: + + AntipodalSamplerCfg + AntipodalSampler + +embodichain.toolkits.graspkit.pg_grasp.collision_checker +-------------------------------------------------------- + +.. currentmodule:: embodichain.toolkits.graspkit.pg_grasp.collision_checker + +.. autosummary:: + + ConvexCollisionCheckerCfg + ConvexCollisionChecker + +embodichain.toolkits.graspkit.pg_grasp.gripper_collision_checker +---------------------------------------------------------------- + +.. currentmodule:: embodichain.toolkits.graspkit.pg_grasp.gripper_collision_checker + +.. autosummary:: + + GripperCollisionCfg + GripperCollisionChecker + box_surface_grid + +embodichain.toolkits.graspkit.scripts.annotate_grasp +---------------------------------------------------- + +.. currentmodule:: embodichain.toolkits.graspkit.scripts.annotate_grasp + +.. autosummary:: + + cli + +embodichain.toolkits.urdf_assembly.component +-------------------------------------------- + +.. currentmodule:: embodichain.toolkits.urdf_assembly.component + +.. autosummary:: + + ComponentRegistry + URDFComponent + URDFComponentManager + +embodichain.toolkits.urdf_assembly.connection +--------------------------------------------- + +.. currentmodule:: embodichain.toolkits.urdf_assembly.connection + +.. autosummary:: + + URDFConnectionManager + +embodichain.toolkits.urdf_assembly.file_writer +---------------------------------------------- + +.. currentmodule:: embodichain.toolkits.urdf_assembly.file_writer + +.. autosummary:: + + URDFFileWriter + +embodichain.toolkits.urdf_assembly.logging_utils +------------------------------------------------ + +.. currentmodule:: embodichain.toolkits.urdf_assembly.logging_utils + +.. autosummary:: + + URDFAssemblyLogger + +embodichain.toolkits.urdf_assembly.mesh +--------------------------------------- + +.. currentmodule:: embodichain.toolkits.urdf_assembly.mesh + +.. autosummary:: + + URDFMeshManager + +embodichain.toolkits.urdf_assembly.sensor +----------------------------------------- + +.. currentmodule:: embodichain.toolkits.urdf_assembly.sensor + +.. autosummary:: + + SensorRegistry + SensorAttachment + URDFSensorManager + +embodichain.toolkits.urdf_assembly.signature +-------------------------------------------- + +.. currentmodule:: embodichain.toolkits.urdf_assembly.signature + +.. autosummary:: + + URDFAssemblySignatureManager + +embodichain.toolkits.urdf_assembly.urdf_assembly_manager +-------------------------------------------------------- + +.. currentmodule:: embodichain.toolkits.urdf_assembly.urdf_assembly_manager + +.. autosummary:: + + URDFAssemblyManager + +embodichain.workspace_cache_cli +------------------------------- + +.. currentmodule:: embodichain.workspace_cache_cli + +.. autosummary:: + + main + +embodichain.utils +----------------- + +.. currentmodule:: embodichain.utils + +.. autosummary:: + + GLOBAL_SEED + is_configclass + resolve_config_path + set_seed + +embodichain_tasks.configs +------------------------- + +.. currentmodule:: embodichain_tasks.configs + +.. autosummary:: + + get_config_path + +embodichain_tasks.multi_segments +-------------------------------- + +.. currentmodule:: embodichain_tasks.multi_segments + +.. autosummary:: + + MultiSegmentsCubePickPlaceEnv + +embodichain_tasks.multi_segments.cube_pick_place +------------------------------------------------ + +.. currentmodule:: embodichain_tasks.multi_segments.cube_pick_place + +.. autosummary:: + + MultiSegmentsCubePickPlaceEnv + CUBE_EXPERT_PROGRAM_REGISTRATION + create_cube_robot_profile_binding + create_cube_scene_binding + +embodichain_tasks.rl +-------------------- + +.. currentmodule:: embodichain_tasks.rl + +.. autosummary:: + + build_env + +embodichain_tasks.rl.basic +-------------------------- + +.. currentmodule:: embodichain_tasks.rl.basic + +.. autosummary:: + + CartPoleEnv + PointMassEnv + +embodichain_tasks.rl.basic.point_mass +------------------------------------- + +.. currentmodule:: embodichain_tasks.rl.basic.point_mass + +.. autosummary:: + + PointMassEnv + +embodichain_tasks.special.simple_task +------------------------------------- + +.. currentmodule:: embodichain_tasks.special.simple_task + +.. autosummary:: + + SimpleTaskEnv + +embodichain_tasks.special.stay_still_save +----------------------------------------- + +.. currentmodule:: embodichain_tasks.special.stay_still_save + +.. autosummary:: + + StayStillSaveEnv + +embodichain_tasks.tableware +--------------------------- + +.. currentmodule:: embodichain_tasks.tableware + +.. autosummary:: + + HandOverEnv + OpenDrawerEnv + +embodichain_tasks.tableware.hand_over +------------------------------------- + +.. currentmodule:: embodichain_tasks.tableware.hand_over + +.. autosummary:: + + HandOverEnv + HAND_OVER_EXPERT_PROGRAM_REGISTRATION + HAND_OVER_POSE_PROVIDER + create_hand_over_robot_profile_binding + create_hand_over_scene_binding + +embodichain_tasks.tableware.blocks_ranking_rgb +---------------------------------------------- + +.. currentmodule:: embodichain_tasks.tableware.blocks_ranking_rgb + +.. autosummary:: + + BlocksRankingRGBEnv + +embodichain_tasks.tableware.blocks_ranking_size +----------------------------------------------- + +.. currentmodule:: embodichain_tasks.tableware.blocks_ranking_size + +.. autosummary:: + + BlocksRankingSizeEnv + +embodichain_tasks.tableware.match_object_container +-------------------------------------------------- + +.. currentmodule:: embodichain_tasks.tableware.match_object_container + +.. autosummary:: + + MatchObjectContainerEnv + +embodichain_tasks.tableware.open_drawer +--------------------------------------- + +.. currentmodule:: embodichain_tasks.tableware.open_drawer + +.. autosummary:: + + OpenDrawerEnv + OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION + create_open_drawer_robot_profile_binding + create_open_drawer_scene_binding + +embodichain_tasks.tableware.place_object_drawer +----------------------------------------------- + +.. currentmodule:: embodichain_tasks.tableware.place_object_drawer + +.. autosummary:: + + PlaceObjectDrawerEnv + +embodichain_tasks.tableware.pour_water.action_bank +-------------------------------------------------- + +.. currentmodule:: embodichain_tasks.tableware.pour_water.action_bank + +.. autosummary:: + + PourWaterActionBank + +embodichain_tasks.tableware.pour_water.pour_water +------------------------------------------------- + +.. currentmodule:: embodichain_tasks.tableware.pour_water.pour_water + +.. autosummary:: + + PourWaterEnv + PourWaterAgentEnv + +embodichain_tasks.tableware.rearrangement +----------------------------------------- + +.. currentmodule:: embodichain_tasks.tableware.rearrangement + +.. autosummary:: + + RearrangementEnv + RearrangementAgentEnv + +embodichain_tasks.tableware.scoop_ice +------------------------------------- + +.. currentmodule:: embodichain_tasks.tableware.scoop_ice + +.. autosummary:: + + ScoopIce + +embodichain_tasks.tableware.stack_blocks_two +-------------------------------------------- + +.. currentmodule:: embodichain_tasks.tableware.stack_blocks_two + +.. autosummary:: + + StackBlocksTwoEnv + +embodichain_tasks.tableware.stack_cups +-------------------------------------- + +.. currentmodule:: embodichain_tasks.tableware.stack_cups + +.. autosummary:: + + StackCupsEnv + +embodichain_tasks.utils.importer +-------------------------------- + +.. currentmodule:: embodichain_tasks.utils.importer + +.. autosummary:: + + import_packages diff --git a/docs/source/conf.py b/docs/source/conf.py index 8065112b5..6f385b71c 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -18,7 +18,7 @@ project = "EmbodiChain" -copyright = "2025, The EmbodiChain Project Developers" +copyright = "2021-2026, The EmbodiChain Project Developers" author = "The EmbodiChain Project Developers" # Read version from VERSION file if it exists @@ -59,6 +59,7 @@ # If using MyST and writing .md API stubs: myst_enable_extensions = ["colon_fence", "deflist", "html_admonition"] +myst_heading_anchors = 4 templates_path = ["_templates"] @@ -98,12 +99,8 @@ } html_theme_options = { - "title": "EmbodiChain", - "logo_only": False, "show_toc_level": 2, - "collapse_navigation": True, - "sticky_navigation": True, - "navigation_depth": 4, - "includehidden": True, - "prev_next_buttons_location": "bottom", + "show_navbar_depth": 1, + "max_navbar_depth": 4, + "collapse_navbar": True, } diff --git a/docs/source/features/generative_sim/simready_pipeline.md b/docs/source/features/generative_sim/simready_pipeline.md index c448a431d..298388125 100644 --- a/docs/source/features/generative_sim/simready_pipeline.md +++ b/docs/source/features/generative_sim/simready_pipeline.md @@ -195,6 +195,6 @@ The source preparation mode only affects the ingest step. The downstream geometr ## See Also -- [Asset Preview](../interaction/preview_asset.md): Load generated meshes and USD assets in the simulator. +- [Previewing Assets](../../guides/preview_asset.md): Load generated meshes and USD assets in the simulator. - [Installation](../../quick_start/install.md): Install EmbodiChain with Blender and rendering dependencies. - [Toolkits](../toolkits/index.rst): Other asset preparation utilities. diff --git a/docs/source/features/interaction/gizmo.md b/docs/source/features/interaction/gizmo.md new file mode 100644 index 000000000..637fb4065 --- /dev/null +++ b/docs/source/features/interaction/gizmo.md @@ -0,0 +1,75 @@ +# Interactive Gizmos + +```{currentmodule} embodichain.lab.sim +``` + +A Gizmo is a registered transform control for manipulating a simulation target +from either the native DexSim window or a trusted Viser browser. The simulation +owns the authoritative state: UI callbacks enqueue requested poses, and +`SimulationManager` applies them on the simulation thread. + +## Supported Targets + +| Target | Gizmo behavior | +|---|---| +| Robot | Moves the selected control part through its configured FK/IK solver. | +| Rigid object | Sets the object's local arena pose. | +| Camera | Sets the camera's local pose. | + +Gizmo interaction currently requires `num_envs=1`. Robot targets also require +a valid `control_part` and solver configuration. + +## Quick Start + +Run the robot Gizmo tutorial in the native window: + +```bash +python scripts/tutorials/sim/gizmo_robot.py +``` + +Use the same target through Viser: + +```bash +python scripts/tutorials/sim/gizmo_robot.py --viser +``` + +Register controls through the manager rather than constructing or destroying +Gizmo instances directly: + +```python +sim.enable_gizmo( + uid="robot", + control_part="arm", +) + +if not sim.has_gizmo("robot", control_part="arm"): + raise RuntimeError("Gizmo setup failed") +``` + +`SimulationManager.update()` drains pending Gizmo commands during a normal +manual-physics loop. An automatic-physics loop that does not call `update()` +must continue calling: + +```python +sim.update_gizmos() +sim.capture_visualization_safely() # Publish the authoritative pose to Viser. +``` + +Use `disable_gizmo()`, `set_gizmo_visibility()`, and +`toggle_gizmo_visibility()` for lifecycle and visibility changes. + +## Frontend Behavior + +- The native window uses the DexSim Gizmo controller and requires an open + window. +- Viser exports browser-native transform controls when + `VisualizationCfg.allow_commands=True`; otherwise it shows read-only frames. +- The standard `--viser` launcher enables registered commands for trusted + clients. EmbodiChain does not add authentication to the Viser endpoint. +- A Viser Gizmo is owned by one browser client from drag start until drag end + or disconnect. Other clients continue receiving authoritative poses. + +For the complete robot setup and IK walkthrough, continue with the +{doc}`Gizmo tutorial `. See +{doc}`Viser browser visualization ` for +server configuration and remote-access guidance. diff --git a/docs/source/features/interaction/index.rst b/docs/source/features/interaction/index.rst index 5b2e521cf..ffaab4e26 100644 --- a/docs/source/features/interaction/index.rst +++ b/docs/source/features/interaction/index.rst @@ -1,11 +1,53 @@ Interactive Simulation ====================== -The Interactive Simulation module provides tools and interfaces for interacting with the simulation environment, including window management, input handling, and real-time control of simulated assets. +Interactive Simulation covers the frontends and controls used to inspect or +manipulate a running simulation. Task-specific commands such as asset preview, +workspace visualization, and grasp annotation remain in their own guides or +feature sections. + +Interaction Frontends +--------------------- + +.. list-table:: + :header-rows: 1 + :widths: 24 38 38 + + * - Frontend + - Use it for + - Documentation + * - Native DexSim window + - Local camera navigation, selection, keyboard and mouse input, viewer + recording, and custom window events. + - :doc:`Native window interaction ` + * - Viser browser + - Headless or remote scene inspection, camera previews, environment + visibility, overlays, and trusted browser controls. + - :doc:`Viser browser visualization ` + +The native window and Viser are mutually exclusive visualization frontends. +Both can expose the same registered Gizmo targets. + +Interaction Controls +-------------------- + +- :doc:`Interactive Gizmos ` provide cross-frontend transform controls + for robots, rigid objects, and cameras. +- :ref:`Interactive replay ` lets users scrub a + recorded trajectory from the terminal or the Viser frame slider. + +Related Workflows +----------------- + +- :doc:`Preview an asset ` to inspect rigid objects and + articulations without creating a Gym environment. +- :doc:`Visualize a robot workspace ` + through its dedicated analyzer backends. +- :doc:`Annotate grasp regions ` with the + grasp-generation toolkit's browser interface. .. toctree:: :maxdepth: 2 - Window interaction - Asset preview - \ No newline at end of file + Native window interaction + Interactive Gizmos diff --git a/docs/source/features/interaction/preview_asset.md b/docs/source/features/interaction/preview_asset.md index 7ef98bda2..8b8177f57 100644 --- a/docs/source/features/interaction/preview_asset.md +++ b/docs/source/features/interaction/preview_asset.md @@ -1,167 +1,8 @@ -# Asset Preview +--- +orphan: true +--- -The `preview_asset` script loads a USD or mesh asset into the simulation for visual inspection and debugging, without requiring a full gym environment. It supports both rigid objects (meshes) and articulations (robot-like assets), with an optional interactive session for manipulation. +# Asset Preview Guide Moved -## Quick Start - -Preview a rigid object from a USD file: - -```bash -embodichain preview-asset \ - --asset_path /path/to/sugar_box.usda \ - --asset_type rigid -``` - -Preview an articulation: - -```bash -embodichain preview-asset \ - --asset_path /path/to/robot.usd \ - --asset_type articulation -``` - -## Viser Browser Preview - -Use `--viser` to inspect rigid objects and articulations in a browser without -opening the native simulation window: - -```bash -embodichain preview-asset \ - --asset_path /path/to/asset.usda \ - --asset_type rigid \ - --viser -``` - -Viser implies headless simulation. The command keeps stepping the simulation -until `Ctrl+C`, so dynamic assets continue moving and their poses update in the -browser. Assets are published immediately after loading because the Viser -server starts together with `SimulationManager`. - -### Articulation Joint Controls - -For articulation previews, `--viser` adds an **Articulation joints** panel by -default. Each articulation has its own folder: - -- joints with two finite position limits use sliders; -- joints with one or both limits missing use numeric inputs; -- rotational values are displayed in degrees and prismatic values in meters; -- mimic joints are omitted, and reset buttons restore the pose captured when - the asset was loaded. - -Commands are validated and applied on the simulation thread before every -physics step. The preview controller writes both the current and target joint -positions, and clears velocity and effort, so the selected pose remains stable -even when the asset has no configured joint drive. Articulations whose active -joint names do not map one-to-one to scalar DOFs are left read-only. - -Disable the panel with `--no-joint-control`. Joint controls are currently a -Viser-only preview feature; the native DexSim window remains unchanged until it -provides a GUI integration point. When `--preview` is also active, queued -browser changes are applied the next time the REPL executes `s `. - -Combine `--viser` with `--preview` to retain the interactive REPL: - -```bash -embodichain preview-asset \ - --asset_path /path/to/robot.urdf \ - --viser \ - --preview -``` - -Use `--viser-host`, `--viser-port`, `--viser-fps`, and the other standard -`--viser-*` options to configure the browser server and update rates. - -## Asset Type Detection - -The asset type is determined as follows: - -1. **Explicit**: use `--asset_type rigid` or `--asset_type articulation`. -2. **URDF files**: automatically treated as articulations. -3. **Other files**: loaded as rigid objects when `--asset_type` is omitted. - -## Interactive Preview Mode - -Pass `--preview` to enter an interactive REPL after the asset is loaded: - -```bash -embodichain preview-asset \ - --asset_path /path/to/robot.usd \ - --asset_type articulation \ - --preview -``` - -Available commands inside the REPL: - -| Command | Description | -|-------------|--------------------------------------------------------------------| -| `p` | Enter an IPython embed session. `sim` and `asset` are in scope. | -| `s ` | Step the simulation *N* times (default 10). | -| `q` | Quit the simulation. | - -Inside the IPython embed session you can freely inspect and manipulate the asset: - -```python -# Inspect articulation joint positions -asset.get_qpos() - -# Step the simulation -sim.update(step=10) - -# Change asset position -asset.set_root_pose(pos=[0, 0, 1.0], rot=[0, 0, 0]) -``` - -## Command-Line Arguments - -| Argument | Description | Default | -|----------------------|--------------------------------------------------------------------|----------------------| -| `--asset_path` | Path to the asset file (`.usd`/`.usda`/`.usdc`/`.obj`/`.stl`/`.glb`/`.urdf`) | **required** | -| `--asset_type` | Type of non-URDF asset: `rigid` or `articulation` | `rigid` | -| `--uid` | Unique identifier in the scene | Derived from filename | -| `--init_pos` | Initial position as `x y z` | `0 0 0.5` | -| `--init_rot` | Initial rotation in degrees as `rx ry rz` | `0 0 0` | -| `--body_type` | Body type for rigid objects: `dynamic`, `kinematic`, `static` | `kinematic` | -| `--use_usd_properties` | Use physical properties from the USD file instead of defaults | `False` | -| `--fix_base` / `--no-fix_base` | Fix or unfix the base of articulations | `True` | -| `--sim_device` | Simulation device | `cpu` | -| `--headless` | Run without rendering window | `False` | -| `--renderer` | Renderer backend: `hybrid`, `fast-rt` or `rt` | `hybrid` | -| `--preview` | Enter interactive embed mode after loading | `False` | -| `--joint-control` / `--no-joint-control` | Enable or disable Viser articulation controls | `True` | -| `--viser` | Enable the headless Viser browser preview | `False` | -| `--viser-host` | Viser bind host | `127.0.0.1` | -| `--viser-port` | Viser bind port | `8080` | -| `--viser-fps` | Maximum scene update rate | `15.0` | -| `--viser-image-fps` | Maximum camera RGB preview rate | `2.0` | -| `--viser-soft-body-fps` | Maximum deformable mesh update rate | `5.0` | -| `--viser-env-ids` | Environment IDs published to Viser, or `all` | `0` | - -## Examples - -**Headless smoke test** (no render window): - -```bash -embodichain preview-asset \ - --asset_path /path/to/asset.usda \ - --headless -``` - -**Custom position and rotation**: - -```bash -embodichain preview-asset \ - --asset_path /path/to/robot.usd \ - --asset_type articulation \ - --init_pos 0.5 0 0.0 \ - --init_rot 0 0 90 \ - --preview -``` - -**Dynamic rigid body** (falls under gravity): - -```bash -embodichain preview-asset \ - --asset_path /path/to/box.obj \ - --body_type dynamic \ - --preview -``` +Asset preview is a command-line workflow rather than an interaction frontend. +Its documentation now lives in {doc}`Previewing Assets `. diff --git a/docs/source/features/interaction/window.md b/docs/source/features/interaction/window.md index f9afed48c..eda9a35a3 100644 --- a/docs/source/features/interaction/window.md +++ b/docs/source/features/interaction/window.md @@ -10,6 +10,8 @@ The native window and the Viser backend are mutually exclusive; native window while Viser is enabled. Likewise, `SimulationManager.start_visualization()` rejects Viser startup while the native window is open. +See {doc}`Viser browser visualization ` for +the headless browser frontend. ## Default Window Controls diff --git a/docs/source/features/toolkits/grasp_generator.rst b/docs/source/features/toolkits/grasp_generator.rst index 64ae1a27d..2de92b306 100644 --- a/docs/source/features/toolkits/grasp_generator.rst +++ b/docs/source/features/toolkits/grasp_generator.rst @@ -242,7 +242,7 @@ This will: 1. Load the mesh file via ``trimesh``. 2. Launch a browser-based annotator (default port ``15531``). -3. Open http://localhost:15531 in your browser, use *Rect Select Region* to highlight the graspable area, then click *Confirm Selection*. +3. Open ``http://localhost:15531`` in your browser, use *Rect Select Region* to highlight the graspable area, then click *Confirm Selection*. 4. Compute antipodal point pairs on the selected region and cache them to disk. Common options:: diff --git a/docs/source/features/workspace_analyzer/samplers.md b/docs/source/features/workspace_analyzer/samplers.md index 50cac5fe6..70087aa23 100644 --- a/docs/source/features/workspace_analyzer/samplers.md +++ b/docs/source/features/workspace_analyzer/samplers.md @@ -1,156 +1,114 @@ # Workspace Analyzer Samplers -The samplers module provides various sampling strategies for workspace analysis, from uniform grids to quasi-random sequences. +Workspace analysis supports regular grids, pseudo-random sampling, +low-discrepancy sequences, stratified sampling, and targeted distributions. +All samplers return a tensor with shape `(num_samples, dimensions)` (a uniform +grid may return a nearby grid-sized count). -## Table of Contents - -- [Quick Start](#quick-start) -- [Overview](#overview) -- [Basic Usage](#basic-usage) -- [Available Samplers](#available-samplers) - - [1. UniformSampler](#1-uniformsampler) - - [2. RandomSampler](#2-randomsampler) - - [3. GaussianSampler](#3-gaussiansampler-) -- [Factory Pattern Usage](#factory-pattern-usage) -- [Integration with Workspace Analyzer](#integration-with-workspace-analyzer) -- [Quick Reference](#quick-reference) - -## Quick Start +## Quick start ```python import torch -from embodichain.lab.sim.workspace.samplers import UniformSampler - -# Define sampling bounds -bounds = torch.tensor([[0.0, 1.0], # x-axis range - [0.0, 1.0]]) # y-axis range - -# Create uniform sampler and generate 1000 samples -sampler = UniformSampler(samples_per_dim=10) -samples = sampler.sample(1000, bounds) # Note: (num_samples, bounds) order -``` - -## Overview - -Available sampling strategies: -- **UniformSampler**: Grid-based regular sampling ✅ -- **RandomSampler**: Pure random sampling ✅ -- **GaussianSampler**: Normal distribution sampling ✅ - -## Basic Usage +from embodichain.lab.sim.workspace.samplers import UniformSampler -```python -import torch -import numpy as np -from embodichain.lab.sim.workspace.samplers import ( - UniformSampler, RandomSampler, GaussianSampler +bounds = torch.tensor( + [ + [-1.0, 1.0], + [-1.0, 1.0], + [0.0, 2.0], + ], + dtype=torch.float32, ) -# Define bounds: [[min, max], [min, max], ...] -bounds = torch.tensor([[-1, 1], [-1, 1], [0, 2]], dtype=torch.float32) - -# Generate samples with implemented samplers -samples = UniformSampler(samples_per_dim=10).sample(1000, bounds) -print(f"Generated {samples.shape[0]} samples") +sampler = UniformSampler(samples_per_dim=10, seed=42) +samples = sampler.sample(num_samples=1000, bounds=bounds) +print(samples.shape) ``` -## Available Samplers - -### 1. UniformSampler - -Grid-based sampling with regular spacing. +Use the keyword arguments `num_samples` and `bounds` as shown. This form works +consistently across all built-in samplers. -```python -import torch -from embodichain.lab.sim.workspace.samplers import UniformSampler +## Available strategies -bounds = torch.tensor([[-1, 1], [-1, 1], [0, 2]], dtype=torch.float32) -# Create uniform sampler -uniform_sampler = UniformSampler(seed=42, samples_per_dim=10) -samples = uniform_sampler.sample(1000, bounds) -``` +| Strategy | Class | Best suited to | Notes | +|----------|-------|----------------|-------| +| `uniform` | `UniformSampler` | Systematic low-dimensional coverage | Creates a regular grid; count is `samples_per_dim ** dimensions` when explicitly set. | +| `random` | `RandomSampler` | Fast baselines and high-dimensional spaces | Independent uniform samples inside each bound. | +| `halton` | `HaltonSampler` | Low- to medium-dimensional quasi-Monte Carlo | Deterministic low-discrepancy sequence with an optional initial skip. | +| `sobol` | `SobolSampler` | Higher-dimensional quasi-Monte Carlo | Uses SciPy when available and otherwise falls back to the built-in implementation. | +| `lhs` | `LatinHypercubeSampler` | Experimental design and sensitivity analysis | SciPy enables optimized Latin-hypercube layouts. | +| `gaussian` | `GaussianSampler` | Local exploration around a target | Available through the factory or its concrete submodule; clips to bounds by default. | +| `importance` | `ImportanceSampler` | Concentrating samples in task-relevant regions | Requires a `weight_fn` when constructed. | -**Best for**: Low-dimensional spaces (2-4D), systematic exploration +`SamplingStrategy.SPHERE` is currently a compatibility alias for uniform +sampling; it does not apply a spherical geometric constraint. -### 2. RandomSampler +## Factory usage -Pure random sampling with uniform distribution. +Use `create_sampler` when the strategy comes from configuration: ```python -from embodichain.lab.sim.workspace.samplers import RandomSampler +from embodichain.lab.sim.workspace.configs import SamplingStrategy +from embodichain.lab.sim.workspace.samplers import create_sampler -random_sampler = RandomSampler(seed=42) -samples = random_sampler.sample(1000, bounds) +sampler = create_sampler( + SamplingStrategy.SOBOL, + seed=42, + scramble=True, +) +samples = sampler.sample(num_samples=1000, bounds=bounds) ``` -**Best for**: High-dimensional spaces, baseline comparisons - -### 3. GaussianSampler ✅ - -Gaussian distribution sampling around specified mean. +The factory accepts either a `SamplingStrategy` value or its string value: ```python -from embodichain.lab.sim.workspace.samplers import GaussianSampler - -gaussian_sampler = GaussianSampler(seed=42, std=0.2) -samples = gaussian_sampler.sample(1000, bounds) +random_sampler = create_sampler("random", seed=42) +halton_sampler = create_sampler("halton", seed=42, skip=100) +gaussian_sampler = create_sampler("gaussian", seed=42, std=0.2) ``` -**Best for**: Uncertainty analysis, robustness studies around workspace center - -## Factory Pattern Usage - -Use the factory to create samplers by strategy (only implemented samplers): +Importance sampling additionally needs a non-negative weighting function: ```python -from embodichain.lab.sim.workspace.samplers import create_sampler -from embodichain.lab.sim.workspace.configs import SamplingStrategy +def center_weight(points: torch.Tensor) -> torch.Tensor: + return torch.exp(-torch.linalg.vector_norm(points, dim=1)) -# Create implemented samplers by strategy -uniform_sampler = create_sampler( - SamplingStrategy.UNIFORM, - seed=42, - samples_per_dim=10 -) - -random_sampler = create_sampler( - SamplingStrategy.RANDOM, - seed=42 -) -gaussian_sampler = create_sampler( - SamplingStrategy.GAUSSIAN, +importance_sampler = create_sampler( + SamplingStrategy.IMPORTANCE, seed=42, - std=0.2 + weight_fn=center_weight, ) +samples = importance_sampler.sample(num_samples=1000, bounds=bounds) ``` -## Integration with Workspace Analyzer +## Workspace Analyzer integration + +Select a strategy through `SamplingConfig`; `WorkspaceAnalyzer` creates the +matching sampler and uses it for joint- and Cartesian-space sampling: ```python -from embodichain.lab.sim.workspace.configs import SamplingConfig, SamplingStrategy +from embodichain.lab.sim.workspace.configs import ( + SamplingConfig, + SamplingStrategy, +) -# Configure sampling in analyzer -sampling_config = SamplingConfig( - strategy=SamplingStrategy.UNIFORM, +sampling = SamplingConfig( + strategy=SamplingStrategy.HALTON, num_samples=1000, - seed=42 + seed=42, ) - -# Analyzer will use the specified sampler automatically ``` -## Quick Reference - -**Available Samplers**: - -- **UniformSampler**: Use for 2-4D systematic exploration -- **RandomSampler**: Baseline for any dimensional spaces -- **GaussianSampler**: Uncertainty and robustness analysis +For importance sampling, construct the sampler directly with its `weight_fn`; +`SamplingConfig` does not currently forward that callable to the factory. -**Sample Size Guidelines**: +## Choosing a sampler -- **Uniform**: `samples_per_dim^n_dims` (exponential growth) -- **Random**: Any size (flexible) -- **Gaussian**: Any size (flexible) +- Start with `random` for a fast baseline. +- Use `uniform` when complete grid coverage is practical. +- Prefer `halton`, `sobol`, or `lhs` when coverage quality matters more than + strict randomness. +- Use `gaussian` or `importance` only when you intentionally want a biased + sampling distribution. diff --git a/docs/source/guides/add_robot.rst b/docs/source/guides/add_robot.rst index f6557f60e..3601c37d4 100644 --- a/docs/source/guides/add_robot.rst +++ b/docs/source/guides/add_robot.rst @@ -1,10 +1,10 @@ .. _guide_add_robot: -Adding a New Robot — Quick Reference -===================================== +Adding a New Robot +================== -This guide is a checklist + reference for adding a new robot to EmbodiChain. For -the full step-by-step walkthrough with code examples, see :doc:`/tutorial/add_robot`. +This guide provides a practical checklist and reference for adding a new robot +to EmbodiChain. The protocol ------------ @@ -81,26 +81,28 @@ Key parameters Common mistakes --------------- -+-----------------------------------+----------------------------------------------------------+ -| Mistake | Fix | -+===================================+==========================================================+ -| ``all`` instead of ``__all__`` | Use ``__all__`` — lowercase ``all`` breaks ``import *``. | -+-----------------------------------+----------------------------------------------------------+ -| ``solver_cfg`` set twice | Set it once in ``_build_defaults`` only. | -+-----------------------------------+----------------------------------------------------------+ -| PK URDF drifts from sim URDF | Route PK through ``_pk_urdf_path``; keep the DOF guard. | -+-----------------------------------+----------------------------------------------------------+ -| Reimplementing ``from_dict`` | Keep the 3-line template; put logic in ``_build_defaults``.| -+-----------------------------------+----------------------------------------------------------+ -| ``root_link_name`` as a tuple | It must be a ``str``. | -+-----------------------------------+----------------------------------------------------------+ -| Calling a nonexistent ``validate``| Don't call methods that don't exist. | -+-----------------------------------+----------------------------------------------------------+ +.. list-table:: + :header-rows: 1 + :widths: 35 65 + + * - Mistake + - Fix + * - ``all`` instead of ``__all__`` + - Use ``__all__`` — lowercase ``all`` breaks ``import *``. + * - ``solver_cfg`` set twice + - Set it once in ``_build_defaults`` only. + * - PK URDF drifts from sim URDF + - Route PK through ``_pk_urdf_path``; keep the DOF guard. + * - Reimplementing ``from_dict`` + - Keep the 3-line template; put logic in ``_build_defaults``. + * - ``root_link_name`` as a tuple + - It must be a ``str``. + * - Calling a nonexistent ``validate`` + - Don't call methods that don't exist. See Also -------- -- :doc:`/tutorial/add_robot` — Full step-by-step tutorial - :doc:`/tutorial/robot` — Using robots in simulation - :doc:`/overview/sim/solvers/index` — IK solver reference - :doc:`/resources/robot/index` — Existing robot documentation diff --git a/docs/source/guides/agent_skills.md b/docs/source/guides/agent_skills.md index 8eee31133..9cc90287c 100644 --- a/docs/source/guides/agent_skills.md +++ b/docs/source/guides/agent_skills.md @@ -21,6 +21,7 @@ needs a different local entry hint. | `/add-task-env` | `.agents/skills/add-task-env/SKILL.md` | Scaffold `EmbodiedEnv` task environments. | | `/add-functor` | `.agents/skills/add-functor/SKILL.md` | Add observation, reward, event, action, dataset, or randomization functors. | | `/add-test` | `.agents/skills/add-test/SKILL.md` | Write tests following project conventions. | +| `/update-api-docs` | `.agents/skills/update-api-docs/SKILL.md` | Document public exports reported by the API checker. | | `/pre-commit-check` | `.agents/skills/pre-commit-check/SKILL.md` | Run local CI-style checks before committing. | | `/pr` | `.agents/skills/pr/SKILL.md` | Draft or create pull requests. | | `/benchmark` | `.agents/skills/benchmark/SKILL.md` | Write benchmark scripts for EmbodiChain modules. | diff --git a/docs/source/guides/cli.md b/docs/source/guides/cli.md index 35cdcda08..134ae8fd3 100644 --- a/docs/source/guides/cli.md +++ b/docs/source/guides/cli.md @@ -59,64 +59,18 @@ The generated output contains the canonical source mesh under ``asset_source/``, --- +(cli-preview-asset)= ## Preview Asset -Preview a USD or mesh asset in the simulation without writing code. +Load one or more USD, mesh, or URDF assets without writing a simulation script: ```bash -# Preview a rigid object -embodichain preview-asset \ - --asset_path /path/to/sugar_box.usda \ - --asset_type rigid \ - --preview - -# Preview an articulation -embodichain preview-asset \ - --asset_path /path/to/robot.usd \ - --asset_type articulation \ - --preview - -# Headless check (no render window) -embodichain preview-asset \ - --asset_path /path/to/asset.usda \ - --headless - -# Control articulation joints in Viser -embodichain preview-asset \ - --asset_path /path/to/robot.urdf \ - --viser +embodichain preview-asset --asset_path /path/to/robot.urdf --viser ``` -### Arguments - -| Argument | Default | Description | -|---|---|---| -| ``--asset_path`` | *(required)* | One or more asset paths (``.usd``/``.usda``/``.usdc``/``.obj``/``.stl``/``.glb``/``.urdf``) | -| ``--asset_type`` | ``rigid`` | Asset type: ``rigid`` or ``articulation``. URDF files are auto-detected as articulation. | -| ``--uid`` | *(from filename)* | Unique identifier for the asset in the scene | -| ``--init_pos X Y Z`` | ``0 0 0.5`` | Initial position | -| ``--init_rot RX RY RZ`` | ``0 0 0`` | Initial rotation in degrees | -| ``--body_type`` | ``kinematic`` | Body type for rigid objects: ``dynamic``, ``kinematic``, or ``static`` | -| ``--use_usd_properties`` | ``False`` | Use physical properties from the USD file | -| ``--fix_base`` | ``True`` | Fix the base of articulations | -| ``--sim_device`` | ``cpu`` | Simulation device | -| ``--headless`` | ``False`` | Run without rendering window | -| ``--renderer`` | ``hybrid`` | Renderer backend: ``hybrid``, ``fast-rt``, or ``rt`` | -| ``--preview`` | ``False`` | Enter interactive embed mode after loading | -| ``--joint-control`` / ``--no-joint-control`` | ``True`` | Enable or disable articulation joint controls in Viser previews | - -The Viser articulation panel displays rotational joints in degrees and -prismatic joints in meters. It excludes mimic joints, leaves articulations with -unsupported multi-DOF mappings read-only, and provides per-articulation reset -buttons. The native DexSim window does not yet expose these controls. - -### Preview Mode - -When ``--preview`` is enabled, an interactive REPL is available: - -- **``p``** — enter an IPython embed session with ``sim`` and ``asset`` in scope -- **``s ``** — step the simulation *N* times (default 10) -- **``q``** — quit +Run `embodichain preview-asset --help` for the authoritative option list. See +{doc}`preview_asset` for visualization modes, multi-asset placement, Viser +joint controls, the interactive terminal, and worked examples. --- @@ -395,7 +349,7 @@ embodichain train-rl --config embodichain_tasks/configs/agents/rl/push_cube/trai # Multi-GPU distributed training torchrun --nproc_per_node=2 -m embodichain train-rl \ - --config embodichain_tasks/configs/agents/rl/push_cube/train_config.yaml \ + --config embodichain_tasks/configs/agents/rl/push_cube/train_config.json \ --distributed ``` diff --git a/docs/source/guides/index.rst b/docs/source/guides/index.rst index a03589c39..220cbb89d 100644 --- a/docs/source/guides/index.rst +++ b/docs/source/guides/index.rst @@ -5,11 +5,11 @@ Practical guides for common tasks in EmbodiChain. .. toctree:: :maxdepth: 1 - :hidden: custom_functors configuration agent_skills add_robot + preview_asset run_env cli diff --git a/docs/source/guides/preview_asset.md b/docs/source/guides/preview_asset.md new file mode 100644 index 000000000..a83fa49bc --- /dev/null +++ b/docs/source/guides/preview_asset.md @@ -0,0 +1,194 @@ +# Previewing Assets + +The `preview-asset` command loads one or more USD, mesh, or URDF assets for +visual inspection and debugging without requiring a Gym environment. It can +open the native DexSim window, publish the scene through Viser, run as a +headless smoke test, or enter an interactive terminal session. + +## Quick Start + +Preview a rigid object: + +```bash +embodichain preview-asset \ + --asset_path /path/to/sugar_box.usda \ + --asset_type rigid +``` + +URDF files are detected as articulations automatically: + +```bash +embodichain preview-asset \ + --asset_path /path/to/robot.urdf +``` + +Pass multiple paths to compare assets in one scene. They are placed along the +positive X axis using `--asset_spacing`: + +```bash +embodichain preview-asset \ + --asset_path /path/to/first.usda /path/to/second.usda \ + --asset_spacing 1.5 +``` + +## Visualization Modes + +| Mode | Option | Behavior | +|---|---|---| +| Native window | Default | Opens the DexSim viewer and keeps stepping until `Ctrl+C`. | +| Viser browser | `--viser` | Runs headlessly, publishes the live scene, and enables trusted preview controls. | +| Headless check | `--headless` | Loads and validates the asset without opening a viewer. | +| Interactive terminal | `--preview` | Adds a REPL for inspecting state and stepping manually. | + +Viser and the native window are mutually exclusive. `--viser` implies +headless simulation; `--headless` alone does not start Viser. + +### Viser Browser Preview + +Use `--viser` to inspect assets without opening a native window: + +```bash +embodichain preview-asset \ + --asset_path /path/to/asset.usda \ + --viser +``` + +The command prints the browser endpoint, normally +`http://127.0.0.1:8080`, and continues stepping the simulation so dynamic +assets and browser poses stay current. + +For articulation previews, Viser adds an **Articulation joints** panel by +default: + +- joints with two finite position limits use sliders; +- joints with one or both limits missing use numeric inputs; +- revolute values are displayed in degrees and prismatic values in meters; +- mimic joints are omitted; +- reset buttons restore the joint pose captured after loading. + +Commands are validated and applied on the simulation thread before each +physics step. The controller writes current and target positions and clears +velocity and effort, so the selected pose remains stable without configured +joint drives. Articulations whose active joint names do not map one-to-one to +scalar DOFs remain read-only. + +Disable the panel with `--no-joint-control`. When `--preview` is also active, +queued browser changes are applied the next time the REPL executes `s `. + +```bash +embodichain preview-asset \ + --asset_path /path/to/robot.urdf \ + --viser \ + --preview +``` + +Use `--viser-host`, `--viser-port`, `--viser-fps`, and the other standard +`--viser-*` options to configure the browser server and update rates. See +{doc}`Viser browser visualization ` for the +complete backend reference and remote-access guidance. + +## Asset Type and Placement + +Asset types are resolved as follows: + +1. URDF files are always loaded as articulations. +2. Other files use `--asset_type rigid` by default. +3. Pass `--asset_type articulation` for non-URDF articulated assets. + +When `--uid` is supplied for multiple assets, it becomes their shared base +identifier and each object receives an `_` suffix. Without it, each +filename supplies the base identifier. `--asset_spacing` controls separation +along the positive X axis. + +## Interactive Terminal + +Pass `--preview` to enter the preview REPL after loading: + +```bash +embodichain preview-asset \ + --asset_path /path/to/robot.usd \ + --asset_type articulation \ + --preview +``` + +| Command | Description | +|---|---| +| `p` | Enter an IPython session with `sim` and the `assets` list in scope. | +| `s ` | Step the simulation `N` times; the default is 10. | +| `q` | Quit the preview. | + +Inside IPython, use the regular simulation APIs: + +```python +# Select one loaded asset when several paths were supplied. +asset = assets[0] + +# Inspect articulation joint positions. +asset.get_qpos() + +# Step the simulation. +sim.update(step=10) + +# Change an asset pose. +pose = asset.get_local_pose() +pose[:, 2] = 1.0 +asset.set_local_pose(pose) +``` + +## Command-Line Options + +| Option | Default | Description | +|---|---:|---| +| `--asset_path PATH ...` | required | One or more `.usd`, `.usda`, `.usdc`, `.obj`, `.stl`, `.glb`, or `.urdf` files. | +| `--asset_type` | `rigid` | Type for non-URDF files: `rigid` or `articulation`. | +| `--uid` | each filename | Optional shared base identifier; multiple assets receive an index suffix. | +| `--asset_spacing` | `1.0` | Spacing in meters between multiple assets along positive X. | +| `--init_pos X Y Z` | `0 0 0.5` | Initial position of the first asset. | +| `--init_rot RX RY RZ` | `0 0 0` | Initial rotation in degrees. | +| `--body_type` | `kinematic` | Rigid body type: `dynamic`, `kinematic`, or `static`. | +| `--use_usd_properties` | disabled | Use physical properties stored in the USD file. | +| `--fix_base` / `--no-fix_base` | fixed | Fix or unfix articulation bases. | +| `--sim_device` | `cpu` | Simulation device. | +| `--renderer` | `hybrid` | Renderer: `hybrid`, `fast-rt`, or `rt`. | +| `--env_map` | none | Built-in IBL resource name or absolute `.hdr`, `.png`, or `.exr` path. | +| `--headless` | disabled | Run without the native window. | +| `--preview` | disabled | Enter the interactive terminal after loading. | +| `--joint-control` / `--no-joint-control` | enabled | Enable or disable the Viser articulation panel. | +| `--viser` | disabled | Enable the headless Viser browser preview. | +| `--viser-host` | `127.0.0.1` | Viser bind host. | +| `--viser-port` | `8080` | Viser TCP port. | +| `--viser-fps` | `15.0` | Maximum scene update rate. | +| `--viser-image-fps` | `2.0` | Maximum camera RGB preview rate. | +| `--viser-soft-body-fps` | `5.0` | Maximum deformable mesh update rate. | +| `--viser-env-ids` | `0` | Published environment IDs, or `all`. | + +Run `embodichain preview-asset --help` for the authoritative option list. + +## Additional Examples + +Run a headless load check: + +```bash +embodichain preview-asset \ + --asset_path /path/to/asset.usda \ + --headless +``` + +Set a custom initial transform: + +```bash +embodichain preview-asset \ + --asset_path /path/to/robot.usd \ + --asset_type articulation \ + --init_pos 0.5 0 0 \ + --init_rot 0 0 90 \ + --preview +``` + +Preview a dynamic rigid body: + +```bash +embodichain preview-asset \ + --asset_path /path/to/box.obj \ + --body_type dynamic +``` diff --git a/docs/source/guides/run_env.md b/docs/source/guides/run_env.md index 2c79fb181..f0dc5d923 100644 --- a/docs/source/guides/run_env.md +++ b/docs/source/guides/run_env.md @@ -404,6 +404,7 @@ re-simulated rather than merely restored. Results may diverge if the replay configuration changes physics, control, randomization, assets, or timestep settings that are not stored in the trajectory file. +(run-env-interactive-replay)= ### Interactive control replay Control mode uses kinematic state restoration and lets you scrub the trajectory diff --git a/docs/source/index.rst b/docs/source/index.rst index e43dfd3c0..c972e2e53 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -28,7 +28,6 @@ Table of Contents quick_start/install.md tutorial/index guides/index - quick_start/docs.md .. toctree:: :maxdepth: 1 @@ -61,6 +60,12 @@ Table of Contents resources/roadmap.md resources/publications/README.md +.. toctree:: + :maxdepth: 1 + :caption: Development + + quick_start/docs.md + .. toctree:: :maxdepth: 2 :caption: API Reference diff --git a/docs/source/introduction.rst b/docs/source/introduction.rst index cbb909ef6..068ec3eeb 100644 --- a/docs/source/introduction.rst +++ b/docs/source/introduction.rst @@ -80,15 +80,15 @@ Contribution Guide ------------------ We welcome contributions! Please see the -`CONTRIBUTING.md `__ file in this repository for -guidelines on how to get started. +`CONTRIBUTING.md `__ +file in this repository for guidelines on how to get started. Publications ------------ See `Academic -Publications `__ for a -complete list of academic papers related to EmbodiChain. +Publications `__ +for a complete list of academic papers related to EmbodiChain. Citation -------- diff --git a/docs/source/overview/rl/algorithm.md b/docs/source/overview/rl/algorithm.md index 44a14ee82..e58e2b1c1 100644 --- a/docs/source/overview/rl/algorithm.md +++ b/docs/source/overview/rl/algorithm.md @@ -97,5 +97,3 @@ algo.begin_update() algo.accumulate_segment(segment_rollout) # repeated until update_horizon metrics = algo.finish_update() ``` - ---- diff --git a/docs/source/overview/rl/buffer.md b/docs/source/overview/rl/buffer.md index 955e11b67..8b006a2b2 100644 --- a/docs/source/overview/rl/buffer.md +++ b/docs/source/overview/rl/buffer.md @@ -72,5 +72,3 @@ class RolloutBuffer: - The rollout buffer stores flattened RL observations; structured observations should be flattened or encoded before entering this buffer. - `value[:, -1]` stores the bootstrap value of the final observation. The final slot of transition-only fields is padding and should be ignored during optimization. - Use `transition_view()` plus `iterate_minibatches()` instead of duplicating rollout slicing logic in each algorithm. - ---- diff --git a/docs/source/overview/rl/config.md b/docs/source/overview/rl/config.md index 2afbda394..4bb9caaf9 100644 --- a/docs/source/overview/rl/config.md +++ b/docs/source/overview/rl/config.md @@ -78,5 +78,3 @@ GRPO example (for Embodied AI / from-scratch training): ## Practical Tips - It is recommended to manage all experiment parameters via JSON or YAML config files for reproducibility and tuning. - Supports multi-algorithm config for easy comparison and automation. - ---- diff --git a/docs/source/overview/rl/index.rst b/docs/source/overview/rl/index.rst index ab65cafab..4d59be1bc 100644 --- a/docs/source/overview/rl/index.rst +++ b/docs/source/overview/rl/index.rst @@ -13,7 +13,7 @@ Overview The embodichain RL module is used to train agents to accomplish tasks in simulation environments. It mainly includes algorithm implementations, policy networks, data buffers, training processes, and utility tools. Architecture Diagram Example ---------------------------- +---------------------------- .. code-block:: text @@ -51,7 +51,7 @@ Extension and Customization - It is recommended to manage all parameters via config files for reproducibility and batch experiments. Common Issues and Best Practices -------------------------------- +-------------------------------- - Config files may use JSON or YAML for easy management and reproducibility. - Parallel environment sampling can significantly improve training efficiency. - The event-driven mechanism allows flexible insertion of custom logic (such as evaluation, saving, callbacks). diff --git a/docs/source/overview/rl/models.md b/docs/source/overview/rl/models.md index 9c85cd9f3..d0509c5b7 100644 --- a/docs/source/overview/rl/models.md +++ b/docs/source/overview/rl/models.md @@ -61,5 +61,3 @@ value = step_td["value"] - It is recommended to configure all network architectures and hyperparameters for reproducibility. - Supports multi-environment parallelism and distributed training to improve sampling efficiency. - Extend the Policy interface as needed for multi-modal input, hierarchical policies, etc. - ---- diff --git a/docs/source/overview/rl/multi_gpu.md b/docs/source/overview/rl/multi_gpu.md index e9573e0cc..308e01425 100644 --- a/docs/source/overview/rl/multi_gpu.md +++ b/docs/source/overview/rl/multi_gpu.md @@ -21,7 +21,7 @@ torchrun --nproc_per_node=2 -m embodichain train-rl --config --dis Example: ```bash -torchrun --nproc_per_node=2 -m embodichain train-rl --config embodichain_tasks/configs/agents/rl/push_cube/train_config.yaml --distributed +torchrun --nproc_per_node=2 -m embodichain train-rl --config embodichain_tasks/configs/agents/rl/push_cube/train_config.json --distributed ``` No config file changes needed; `device` and `gpu_id` are overridden automatically per rank. @@ -46,4 +46,3 @@ CUDA_VISIBLE_DEVICES=0,1 torchrun --nproc_per_node=2 -m embodichain train-rl --c - **Episode stats**: `episode_reward_avg_100` and `episode_length_avg_100` are aggregated across all ranks via `all_gather` for accurate global metrics. - **Evaluation**: Only rank 0 creates and runs the evaluation environment. - **Checkpoints**: Only rank 0 saves; the underlying policy state (without DDP wrapper) is stored. - diff --git a/docs/source/overview/rl/train_script.md b/docs/source/overview/rl/train_script.md index b3133b37e..31ca5a179 100644 --- a/docs/source/overview/rl/train_script.md +++ b/docs/source/overview/rl/train_script.md @@ -61,5 +61,3 @@ completed episodes rather than episodes per parallel environment. - It is recommended to manage all experiment parameters via JSON or YAML config files for reproducibility and tuning. - Supports multi-environment and event extension to improve training flexibility. - Logging and checkpoint management help with experiment tracking and recovery. - ---- diff --git a/docs/source/overview/rl/trainer.md b/docs/source/overview/rl/trainer.md index 433d278e9..61002ec4b 100644 --- a/docs/source/overview/rl/trainer.md +++ b/docs/source/overview/rl/trainer.md @@ -64,5 +64,3 @@ trainer.save_checkpoint() - It is recommended to perform periodic evaluation and model saving to prevent loss of progress during training. - The event mechanism can be used for automated experiments, data collection, and environment reset. - Logging and monitoring help analyze training progress and tune hyperparameters. - ---- diff --git a/docs/source/overview/sim/atomic_actions/builtin_actions.md b/docs/source/overview/sim/atomic_actions/builtin_actions.md index 7e7aeef73..2a767bf37 100644 --- a/docs/source/overview/sim/atomic_actions/builtin_actions.md +++ b/docs/source/overview/sim/atomic_actions/builtin_actions.md @@ -5,12 +5,12 @@ ```{currentmodule} embodichain.lab.sim.atomic_actions ``` -EmbodiChain ships nine built-in action implementations with stable skill IDs; +EmbodiChain ships eleven built-in action implementations with stable skill IDs; `AtomicActionEngine` creates and registers a fresh instance of every built-in by default. Applications select them by stable skill ID rather than registering routine instances themselves. `Place` additionally accepts an `AssembleGoal`, so assembly reuses the same -release primitive instead of introducing a tenth skill ID. +release primitive instead of introducing another skill ID. All built-ins implement `plan(request, context) -> ActionPlan`, where `request` is the engine-resolved @@ -21,9 +21,10 @@ the built-in catalog. Generic motion and recovery choices belong to the invocation, and per-call primitive behavior belongs to `skill_options`. Registration only installs an implementation. Whether a built-in is executable -for a particular call still depends on its binding roles, the robot's control -parts, semantic command profiles, and task-state preconditions. Action Agent -adapters must also honor `agent_visible` and filter by embodiment capability. +for a particular call still depends on its `SkillBindingContract`, the selected +resource endpoints, semantic command profiles, and task-state preconditions. +Action Agent adapters must also honor `agent_visible` and filter by embodiment +capability. ```{note} The current manipulation primitives consume semantic `open` and `grasp` @@ -99,11 +100,30 @@ The animations below are the focused simulator demos under :link: builtin-press :link-type: ref -`press` · close, contact, and return +`press` · close, approach, press, and retract Press demo ::: +:::{grid-item-card} `Slide` +:link: builtin-slide +:link-type: ref + +`slide` · grasped translation along a constrained axis + +Slide pull demo +Slide push demo +::: + +:::{grid-item-card} `Twist` +:link: builtin-twist +:link-type: ref + +`twist` · grasped rotation about a configured axis + +Twist demo +::: + :::{grid-item-card} `CoordinatedPickment` :link: builtin-coordinated-pickment :link-type: ref @@ -135,27 +155,28 @@ The animations below are the focused simulator demos under ## Capability matrix -| Skill ID | Accepted goal | Required binding roles | Required profile commands | Required task state | Expected task effect | +| Skill ID | Accepted goal | Required endpoints | Required profile commands | Required task state | Expected task effect | |---|---|---|---|---|---| -| `move_end_effector` | `EndEffectorPoseGoal` | manipulator `primary` | none | none | none | -| `move_joints` | `JointPositionGoal` | manipulator `primary` | named target only: command matching `target` | none | none | -| `pick_up` | `GraspGoal` | manipulator + end effector `primary` | primary: `open`, `grasp` | semantic object/entity | attach object to `primary` manipulator | -| `move_held_object` | `HeldObjectPoseGoal` | manipulator + end effector `primary` | primary: `grasp` | object held by `primary` | preserve attachment | -| `place` | `PlaceGoal`, `AssembleGoal` | manipulator + end effector `primary` | primary: `open`, `grasp` | `AssembleGoal` requires an object held by `primary`; ordinary `PlaceGoal` has no planner-enforced attachment precondition | detach object | -| `press` | `PressGoal` | manipulator + end effector `primary` | primary: `grasp` | none | none | -| `coordinated_pickment` | `CoordinatedPickGoal` | manipulator + end effector `left`, `right` | both: `open`, `grasp` | semantic object/entity | create coordinated attachment; clear individual attachments | -| `coordinated_placement` | `CoordinatedPlacementGoal` | manipulator + end effector `placing`, `support` | placing: `open`, `grasp`; support: `grasp` | one individually held object per arm | optionally detach placing object; preserve support attachment | -| `hand_over` | `GraspGoal` | manipulator + end effector `source`, `destination` | both: `open`, `grasp` | object held by source arm | transfer attachment to destination arm | - -### Binding role meanings - -Roles are action-local semantic participant slots. They are keys declared by an -action, while the corresponding `ActionBinding` values are concrete -`Robot.control_parts` keys. A role that appears in both binding maps identifies -the manipulator and actuated hand/tool serving the same functional participant; -it does not make the two maps interchangeable. - -| Role | Used by | Meaning | +| `move_end_effector` | `EndEffectorPoseGoal` | `primary.motion` | none | none | none | +| `move_joints` | `JointPositionGoal` | `primary.motion` | named target only: command matching `target` on `primary.motion` | none | none | +| `pick_up` | `GraspGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `open`, `grasp` | semantic object/entity | attach object to the `primary.motion` target | +| `move_held_object` | `HeldObjectPoseGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `grasp` | object held by the `primary.motion` target | preserve attachment | +| `place` | `PlaceGoal`, `AssembleGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `open`, `grasp` | `AssembleGoal` requires an object held by the `primary.motion` target; ordinary `PlaceGoal` has no planner-enforced attachment precondition | detach object | +| `press` | `PressGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `grasp` | none | none | +| `coordinated_pickment` | `CoordinatedPickGoal` | `left.motion`, `left.grasp`, `right.motion`, `right.grasp` | both grasp endpoints: `open`, `grasp` | semantic object/entity | create coordinated attachment; clear individual attachments | +| `coordinated_placement` | `CoordinatedPlacementGoal` | `placing.motion`, `placing.grasp`, `support.motion`, `support.grasp` | `placing.grasp`: `open`, `grasp`; `support.grasp`: `grasp` | one individually held object per motion target | optionally detach placing object; preserve support attachment | +| `hand_over` | `GraspGoal` | `source.motion`, `source.grasp`, `destination.motion`, `destination.grasp` | both grasp endpoints: `open`, `grasp` | object held by the source motion target | transfer attachment to the destination motion target | +| `operate_articulation` | `OperateArticulationGoal` | `primary.motion`, `primary.interaction` | `primary.interaction`: `open`, `grasp` | registered articulation and handle operation affordance | update and physically verify the target articulation joint position | + +### Participant slot meanings + +Slots are action-local semantic participants declared by +`SkillBindingContract`. Each slot contains endpoint requirements such as +`motion` and `grasp`; the profile binder matches their capabilities and typed +commands to a robot resource, then adapters produce the generic +`EndpointBinding` values owned by `ActionBinding`. + +| Slot | Used by | Meaning | |---|---|---| | `primary` | Single-participant skills | Principal participant for this invocation; it has no inherent left/right or default-robot meaning | | `source` | `hand_over` | Participant that initially holds and transfers the object | @@ -164,10 +185,12 @@ it does not make the two maps interchangeable. | `placing` | `coordinated_placement` | Participant that aligns and optionally releases the placing object | | `support` | `coordinated_placement` | Participant that keeps holding and positioning the support object | -The action's `manipulator_roles` and `end_effector_roles` declarations determine -which entries are required. The engine checks that those entries exist and that -every value resolves through `Robot.control_parts`; the caller or capability -binder must select a physically compatible arm and hand/tool combination. +Each endpoint requirement declares an open capability set and optional typed +semantic commands. Intra-slot and inter-slot disjointness constraints express +physical compatibility without global arm/tool categories. The built-in +control-part adapter resolves current joint-backed endpoints through +`Robot.control_parts`; custom adapters may instead return mobile, whole-body, or +other runtime targets. `MoveJoints` is intentionally `agent_visible=False`: it is useful for home, recovery, calibration, and scripted postures, but is not exposed to an Action @@ -196,57 +219,96 @@ entity as a recovery dependency. | Skill / field | `SceneEntityPose` accepted | Automatic scene-motion replan | |---|---:|---:| | `MoveEndEffector.xpos` | yes | yes | +| `MoveJoints.target` | no | no | | `MoveHeldObject.object_target_pose` | yes | yes | | `Place.xpos` | yes | yes | -| `Press.xpos` | yes | yes | | `CoordinatedPickGoal.object_target_pose` / `object_initial_pose` | yes | yes | | `CoordinatedPlacementGoal` placing/support poses | yes | yes | -| `PickUp.grasp_xpos` | yes | yes | -| `PickUp` / `HandOver` `ObjectSemantics.entity` lookup | not through `SceneEntityPose` | no automatic scene dependency | -| `AssembleGoal` base entity lookup | not through `SceneEntityPose` | latest pose is used when replanning, but base movement alone does not trigger it | +| `PickUp.grasp_xpos` | yes | yes; monitored through `approach` only | +| `PickUp` `ObjectSemantics.entity_id` grounding | implicit snapshot reference | yes; monitored through `approach` only | +| Coordinated pickup implicit initial pose via `ObjectSemantics.entity_id` | implicit snapshot reference | yes; only when `object_initial_pose` is omitted | +| `AssembleGoal.base_pose` | yes | yes | +| Deprecated `ObjectSemantics.entity` / `AssembleAffordance.base_object_entity` fallback | no | no | +| `HandOver` current held-object pose | no scene lookup | no; derived from observed EEF pose and verified attachment state | +| `HandOverOptions.middle_object_pose` / `final_object_pose` | yes | yes | + +### Object identity and grounding + +`ObjectSemantics.entity_id` is the canonical scene-snapshot key. It must be a +non-empty string when set. An explicit ID is strict: object grounding reads only +`PlanningContext.scene.entities[entity_id]`, and a missing entry is an error. It +never falls back to `ObjectSemantics.entity` after an explicit lookup fails. + +The live `entity` field remains a deprecated direct-core compatibility path only +when `entity_id` is absent. That read emits `DeprecationWarning` and cannot +create a scene-motion dependency. `collect_scene_dependencies()` intentionally +does not recurse into `ObjectSemantics`; each primitive declares a semantic ID +only when its planner actually consumes that object's snapshot pose. + +Attachment and handover identity are not based on `label`. The core resolves an +explicit `entity_id` only against another explicit ID. If either compared side +has one, both sides must have the same explicit value; an equal legacy +`entity.uid` does not match it. When both explicit IDs are absent, two non-empty +legacy UIDs may match. Only when neither side has either ID form may comparison +fall back to the same semantic object or live entity handle. Future +`SceneRegistry` integration will own arbitrary alias normalization; this core +bridge does not. + +`ObjectSemantics` is shallow-frozen. Its top-level fields, including +`entity_id`, cannot be rebound after construction; create a new semantics value +to change identity. Nested affordance and metadata objects remain mutable but +do not participate in identity. ### Parameter ownership Use this rule when configuring a built-in or adding a new one: - the **goal** carries only the requested outcome; -- the **binding** carries semantic-role mappings to control-part names selected - for this call; every value must be a key in the engine robot's - `control_parts` mapping; +- the skill's **binding contract** declares participant slots, endpoint + capabilities, required typed commands, and physical disjointness; +- the engine-owned **binding** carries adapter-resolved `EndpointBinding` + snapshots and immutable runtime targets selected for this call; - typed **skill options** carry segment-specific behavior that may vary by invocation; an action may provide defaults; - the engine's **control-part profiles** carry embodiment-specific semantic commands such as `open`, `grasp`, and named postures; -- `MotionPolicy` carries sample count, timing, motion strategy, limits, - collision choice, and planner options; +- `MotionPolicy` carries sample count, motion strategy, collision choice, and + typed planner options; +- planner-backed segments preserve explicit planner timing, while action-owned + interpolation reads the environment cadence from `PlanningContext.control_dt`; +- missing planner or action timing is an error; the engine has no fallback + control period; - `RecoveryPolicy` carries all replan/retry thresholds and budgets. -All built-ins resolve participating arm and hand names exclusively from -`ActionBinding`. The engine then resolves the selected control part's profile +All built-ins resolve their `motion` and `grasp` endpoints exclusively from the +generic `ActionBinding`. The built-in control-part adapter resolves joint IDs and checks each joint-position command against its DoF. Invocation-level -`ActionControlOverrides` may replace a command by binding role for one explicit -revision. +`ActionControlOverrides` may replace a command by `(slot, endpoint)` for one +explicit revision. ### Planning and effect semantics -Every action returns a per-environment `plan_success` mask and one or more -full-robot trajectories. `plan_success=True` means motion planning succeeded; -it does not prove contact or object transfer. Actions that change attachment -state declare a `StateDelta`. Offline `compile()` projects it hypothetically; -closed-loop execution commits it only after external effect verification. +Every action returns a per-environment `plan_success` mask and an +`ActionPlan.commands` sequence of `RuntimeCommandFrame` values. Current +joint-planned built-ins also retain `ActionPlan.joint_trajectory` for joint +feedback, inspection, and static projection. `plan_success=True` means planning +succeeded; it does not prove contact or object transfer. Actions that change +attachment state declare a `StateDelta`. Offline `compile()` projects it +hypothetically; closed-loop execution commits it only after external effect +verification. (builtin-move-end-effector)= ## `MoveEndEffector` -Plans a free-space motion for a bound manipulator to reach one EEF pose or an -ordered set of pose waypoints. +Plans a free-space motion for the bound `primary.motion` endpoint to reach one +EEF pose or an ordered set of pose waypoints. | Contract | Value | |---|---| | Skill ID | `move_end_effector` | | Goal | `EndEffectorPoseGoal(xpos=...)` | -| Binding | manipulator role `primary` | +| Binding contract | `primary.motion` with Cartesian-pose capability | | Motion | EEF planning from observed arm qpos; output expanded to full robot DoF | | Completion | `EEF_GOAL_REACHED` | | Effect | none | @@ -270,7 +332,7 @@ than an EEF pose. |---|---| | Skill ID | `move_joints` | | Goal | `JointPositionGoal(target=...)` | -| Binding | manipulator role `primary` | +| Binding contract | `primary.motion` with joint-position capability | | Motion | joint planning/interpolation from observed qpos; supports joint waypoints | | Completion | `JOINT_GOAL_REACHED` | | Effect | none | @@ -278,7 +340,7 @@ than an EEF pose. `target` accepts an explicit qpos tensor with shape `(control_dof,)`, `(B, control_dof)`, or `(B, N, control_dof)`, or a non-empty string resolved -from the bound manipulator's `ControlPartCommandProfile`. Named poses remain +from the bound `primary.motion` endpoint's command profile. Named poses remain embodiment knowledge without becoming separate goal types: ```python @@ -300,25 +362,42 @@ named_goal = JointPositionGoal(target="home") ## `PickUp` Plans **approach -> close hand -> lift** and declares the object attached to the -bound manipulator. +bound motion target. | Contract | Value | |---|---| | Skill ID | `pick_up` | | Goal | `GraspGoal(semantics=..., grasp_xpos=None)` | -| Binding | manipulator + end effector role `primary` | -| Precondition | `ObjectSemantics.entity` is set; an `AntipodalAffordance` is required when no explicit grasp pose is supplied | -| Effect | write `HeldObjectState` for the bound manipulator and clear overlapping coordinated attachment state | +| Binding contract | `primary.motion` plus disjoint `primary.grasp` | +| Precondition | `ObjectSemantics.entity_id` resolves in the planning snapshot; the deprecated live `entity` fallback remains temporarily; an `AntipodalAffordance` is required when no explicit grasp pose is supplied | +| Effect | write `HeldObjectState` for the bound motion target | | Verification | the attachment effect must be verified during closed-loop execution | `grasp_xpos` may be `(4, 4)`, `(B, 4, 4)`, or a `SceneEntityPose`. A scene reference resolves the latest grasp pose and registers its entity as a recovery dependency, so material target motion invalidates and replans an executing -`PickUp`. When omitted, the action samples valid affordance grasps, evaluates +`PickUp` while its `approach` segment is active. Once approach has been +dispatched, dependency monitoring stops: contact-, close-, and lift-induced +object motion must not be misclassified as an external target update. Tracking +and collision-world checks remain active independently. When `grasp_xpos` is +omitted, the action samples valid affordance grasps, evaluates reachability, and stores the selected `object_to_eef` transform in the expected held-object state. Later object-centric skills reuse that transform. -`PickUp` requires `open` and `grasp` commands on the bound end-effector profile. +Set `ObjectSemantics.entity_id` to the same stable ID used by the scene +snapshot. `PickUp` resolves that object pose once per planning attempt, uses the +same tensor for grasp sampling, upright adjustment, and `object_to_eef`, and +automatically records the ID as a scene dependency. An explicit ID never falls +back to a live simulation entity when the snapshot entry is missing. + +The object dependency is monitored only while the `approach` segment is active. +Its exclusive cutoff is `close.start`: object motion observed before that frame +invalidates the plan, while motion from gripper closure and lift does not. After +the cutoff, every object-pose change is ignored by scene recovery, including an +external disturbance, so the accepted `grasp` command and live +object-to-endpoint effect evidence become the authoritative completion check. + +`PickUp` requires typed `open` and `grasp` commands on `primary.grasp`. Important `PickUpOptions` fields: | Field | Purpose | @@ -330,11 +409,13 @@ Important `PickUpOptions` fields: | `downstream_object_target_poses` | Optional future reachability constraints used in grasp selection | | `obj_upright_direction`, `rotate_upright` | Optional orientation-selection behavior | -Reading `ObjectSemantics.entity` remains a live planning lookup rather than an -automatic dependency. Use an explicit `SceneEntityPose` in `grasp_xpos` when -object motion should trigger dynamic-goal replanning. +`ObjectSemantics.entity` without an ID is a deprecated compatibility path. Its +live pose does not create an automatic scene dependency. -**Example:** `scripts/tutorials/atomic_action/pickup.py` +**Example:** `scripts/tutorials/atomic_action/pickup.py` currently exercises the +deprecated entity-only fallback. For canonical snapshot grounding and moving +target recovery, see +`scripts/tutorials/atomic_action/moving_target_recovery.py`. (builtin-move-held-object)= @@ -343,21 +424,27 @@ object motion should trigger dynamic-goal replanning. Moves an already attached object to an object-frame target while keeping the hand closed. The caller specifies the desired **object pose**, not an EEF pose; the action derives `target_object_pose @ object_to_eef` from verified task state. +When upright transport needs the current object orientation, it derives it from +the observed EEF pose and verified `object_to_eef` relation rather than reading +a live scene entity. | Contract | Value | |---|---| | Skill ID | `move_held_object` | | Goal | `HeldObjectPoseGoal(object_target_pose=...)` | -| Binding | manipulator + end effector role `primary` | -| Precondition | a `HeldObjectState` exists for the bound manipulator, normally from `PickUp` | +| Binding contract | `primary.motion` plus disjoint `primary.grasp` | +| Precondition | a `HeldObjectState` exists exclusively for the bound motion target, normally from `PickUp` | | Motion | single object-centric transport segment with closed-hand qpos | | Effect | none; the existing attachment is preserved | | Dynamic target | explicit pose or `SceneEntityPose` | -The bound end-effector profile must provide `grasp`; optional upright-transport -settings belong to `MoveHeldObjectOptions`. The arm and hand are selected by -`ActionBinding`; generic timing and trajectory sampling remain in -`MotionPolicy`. +The bound `primary.grasp` endpoint must provide `grasp`; optional +upright-transport settings belong to `MoveHeldObjectOptions`. The participant's +motion and grasp endpoints are selected through `ActionBinding`; generic timing +is explicit on the planner result or planning context, while trajectory +sampling remains in `MotionPolicy`. In a vectorized batch, rows +where another manipulator holds the same semantic object or live entity are +marked unsuccessful and held in place. **Example:** `scripts/tutorials/atomic_action/move_held_object.py` @@ -373,18 +460,19 @@ one. |---|---| | Skill ID | `place` | | Goal | `PlaceGoal(xpos=..., tcp_symmetry="none")` | -| Binding | manipulator + end effector role `primary` | -| State | consumes the bound manipulator's attachment when present | -| Effect | detach the object and clear overlapping coordinated attachment state | +| Binding contract | `primary.motion` plus disjoint `primary.grasp` | +| State | consumes the bound motion target's attachment when present and exclusive | +| Effect | detach the object from the bound motion target | | Verification | release must be verified during closed-loop execution | | Dynamic target | explicit pose/waypoints or `SceneEntityPose` | Set `tcp_symmetry="z_roll_180"` only if TCP x/y can be flipped while TCP z and translation remain physically equivalent. The action selects the closer orientation variant from the observed starting state and uses it consistently -across all waypoints. +across all waypoints. An ordinary `PlaceGoal` may still open an unattached +gripper, but it will not release one side of a shared multi-manipulator object. -The bound end-effector profile must provide `open` and `grasp`. Important +The bound `primary.grasp` endpoint must provide `open` and `grasp`. Important `PlaceOptions` fields: | Field | Purpose | @@ -400,52 +488,138 @@ The bound end-effector profile must provide `open` and `grasp`. Important ### Assembly through `Place` -`Place` also accepts `AssembleGoal(affordance=...)`. There is no separate -assembly skill: it derives the assemble-object target from the base object's -live pose and reuses the normal place/release segments. +`Place` also accepts +`AssembleGoal(affordance=..., base_pose=SceneEntityPose("base"))`. There is no +separate assembly skill: it derives the assemble-object target from the base +object's snapshot pose and reuses the normal place/release segments. ```text base_object_pose @ assemble_to_base_pose = assemble_object_target_pose assemble_object_target_pose @ held.object_to_eef = release_eef_pose ``` -The `AssembleAffordance` identifies the base and assemble objects, stores the -relative pose, and must provide `base_object_entity`. A prior verified `PickUp` -must have populated the held object's `object_to_eef` transform. Planning then -declares the same detach effect as a normal place. +The `AssembleAffordance` stores the relative assembly pose. A prior verified +`PickUp` must have populated the held object's `object_to_eef` transform, and +that attachment must be exclusive. `base_pose` is resolved from each planning +snapshot and automatically becomes a recovery dependency. Omitting it +temporarily falls back to the affordance's `base_object_entity` with a +deprecation warning; that fallback is not a scene dependency. Planning declares +the same detach effect as a normal place. -The base entity's current pose is read each time `plan()` runs. Because the -goal does not yet encode that entity through `SceneEntityPose`, base movement by -itself does not invalidate an executing plan; another recovery trigger is -required before the newer pose is resolved. - -**Example:** `scripts/tutorials/atomic_action/assemble.py` +**Example:** `scripts/tutorials/atomic_action/assemble.py` currently exercises +the legacy `base_object_entity` fallback and is not the canonical `base_pose` +form. It remains a compatibility example until the registry-backed tutorial +migration. (builtin-press)= ## `Press` -Plans **close hand -> move to contact pose -> return to the observed starting -arm qpos**. It is intended for button-like or contact interactions where the -arm should retreat along its planned path after reaching the target. +Plans **close hand -> approach target -> contact -> press along axis -> return +to the approach pose**. `PressAffordance` is entity-free and stores an explicit +target-local surface `press_position` and `press_axis`. `PressGoal.target_pose` +is either a pose snapshot or `SceneEntityPose`, which resolves through the +current `PlanningContext.scene` and participates in dynamic-goal recovery. + +The contact, press, and retract segments use axis-aligned Cartesian keyframes; +each output sample is grounded with IK instead of being interpolated only in +joint space. The generated tool frame uses an adaptive reference axis and is a +right-handed orthonormal rotation even for vertical or oblique press axes. | Contract | Value | |---|---| | Skill ID | `press` | -| Goal | `PressGoal(xpos=...)` | -| Binding | manipulator + end effector role `primary` | -| Motion | close, press, joint-space return | -| Effect | none; existing attachment state is unchanged | +| Goal | `PressGoal(semantics=..., target_pose=...)` | +| Binding contract | `primary.motion` plus disjoint `primary.grasp` | +| Motion | close, approach, contact, axis-constrained press, axis-constrained retract | +| Effect | explicitly open-loop; no physical button/contact effect is claimed | | Dynamic target | explicit pose or `SceneEntityPose` | -The bound end-effector profile must provide `grasp`, while -`PressOptions.hand_interp_steps` controls the close interpolation. The arm and -hand control parts come from `ActionBinding`. Contact detection is not itself a -symbolic effect in the current action; applications that require force/contact -confirmation should verify it externally. +`PressOptions` controls hand-close interpolation, approach distance, +press distance, and an optional target-local `press_position`. An options-level +position overrides the affordance's explicit surface point. The bound +`primary.grasp` endpoint must provide `grasp`; both endpoints come from the +generic `ActionBinding`, and the action keeps the gripper closed for all arm +motion segments. Applications that require force/contact confirmation must +verify it externally. **Example:** `scripts/tutorials/atomic_action/press.py` +(builtin-slide)= + +## `Slide` + +Plans a grasped linear interaction for one articulation link. The entity-free +`SlideAffordance` stores the link-local grasp mesh, `translation_axis`, and +optional joint name/limits. `SlideGoal.target_pose` supplies the link pose as a +snapshot or `SceneEntityPose`. The positive axis direction means approach and +push/close; pull/open uses its negative direction. The affordance inherits +`AntipodalAffordance` and selects a grasp with `get_best_grasp_poses()`. The grasp +approach direction is the link-frame translation axis transformed by the +current link rotation. + +With `direction="pull"`, the sequence is **approach -> reach -> close -> pull -> +open**. With `direction="push"`, it is **approach -> reach -> close -> push -> open +-> return**, where `return` moves the open gripper back to the original approach +pose. + +| Contract | Value | +|---|---| +| Skill ID | `slide` | +| Goal | `SlideGoal(semantics=..., target_pose=...)` | +| Binding contract | `primary.motion` plus disjoint `primary.grasp` | +| Motion | pull: approach, reach, close, pull, open; push adds return to approach | +| Effect | explicitly open-loop; no articulation travel or grasp success is claimed | + +`SlideOptions` controls `direction`, hand close/open +interpolation, approach distance, and translation distance. The link-frame +translation axis belongs to `SlideAffordance`; the bound `primary.grasp` +endpoint must provide `open` and `grasp`. Reach, pull/push, and push-return use +axis-aligned Cartesian samples rather than sparse joint-space endpoints. + +**Example:** `scripts/tutorials/atomic_action/slide.py` +plans and replays a pull first, then replans a push from the drawer's measured +post-pull link pose. + +(builtin-twist)= + +## `Twist` + +Plans **approach -> reach -> close -> twist -> open -> retract** for an +articulation link or a rigid object. The entity-free `TwistAffordance` stores an +explicit local `grasp_position`, `twist_axis`, and `axis_origin`, plus optional +joint name/limits. `TwistGoal.target_pose` supplies the grounded target pose. + +The grasp frame's z-axis follows the world-transformed twist axis; an adaptive +reference completes a right-handed orthonormal frame. Twist keyframes rotate +around the full 3D axis defined by `axis_origin + twist_axis`, not implicitly +around the target link origin. + +| Contract | Value | +|---|---| +| Skill ID | `twist` | +| Goal | `TwistGoal(semantics=..., target_pose=...)` | +| Binding contract | `primary.motion` plus disjoint `primary.grasp` | +| Motion | approach, reach, close, rotate about the target-local axis, open, retract | +| Effect | explicitly open-loop; no articulation travel or grasp success is claimed | + +`TwistOptions` controls the pre-grasp distance, close/open interpolation, +Cartesian twist keyframes, and twist angle. The pre-grasp pose is offset along +the grasp pose's negative z-axis; the target-local twist axis belongs to +`TwistAffordance`. The bound `primary.grasp` endpoint must provide `open` and +`grasp`. + +`Twist` is intentionally a pure-rotation primitive. Thread pitch, coupled axial +translation, and regrasping are outside its contract; an `Unscrew` action should +model those behaviors separately. + +For all three primitives, `SkillDescriptor.open_loop` is `True`. Trajectory +completion therefore means commanded motion completion only. Applications that +need semantic success must observe button/contact or articulation state and +verify it outside the side-effect-free planner. + +**Example:** `scripts/tutorials/atomic_action/twist.py` + (builtin-coordinated-pickment)= ## `CoordinatedPickment` @@ -457,25 +631,32 @@ both hands -> lift -> move object -> hold**. |---|---| | Skill ID | `coordinated_pickment` | | Goal | `CoordinatedPickGoal` | -| Binding | manipulator + end effector roles `left` and `right` | -| Precondition | `ObjectSemantics.entity` is set and the affordance is an `AntipodalAffordance` | +| Binding contract | disjoint `left` and `right` slots, each with disjoint `motion` and `grasp` endpoints | +| Precondition | an `AntipodalAffordance`; when `object_initial_pose` is omitted, `ObjectSemantics.entity_id` resolves in the snapshot or the deprecated no-ID live fallback is available | | Goal geometry | shared-object target pose and optional initial object pose; left/right grasps are sampled from the affordance | -| Effect | clear individual left/right attachments and create `CoordinatedHeldObjectState[(left, right)]` | +| Effect | write one `HeldObjectState` per bound manipulator; both entries share the same object semantics | | Verification | coordinated attachment must be externally verified | The left/right grasp poses are not supplied by the caller. At planning time the action calls `AntipodalAffordance.get_dual_arm_valid_grasp_poses` with the `approach_direction`, `left_to_right_arm_direction`, and `middle_empty_ratio` options to partition the object into left/right grasp regions and select the -lowest-cost grasp on each side. The derived `object_to_eef` transforms are -stored in the projected `CoordinatedHeldObjectState` and reused by later -object-centric skills. - -The object target and optional initial pose may use `SceneEntityPose`. When no -initial pose is supplied, `ObjectSemantics.entity` provides the object's current -pose. - -Both bound end-effector profiles must provide `open` and `grasp`. Important +lowest-cost grasp on each side. Each derived `object_to_eef` transform is stored +in the corresponding projected `HeldObjectState`. Later object-centric skills +can inspect those per-manipulator entries directly; sharing the same +`ObjectSemantics` instance identifies the common object. Single-arm transport, +release, and handover skills reject those shared rows rather than moving or +detaching just one participant. + +The object target and optional initial pose may use `SceneEntityPose`. Those +references declare their own scene dependencies. When `object_initial_pose` is +omitted, the action grounds the initial pose from +`ObjectSemantics.entity_id` and declares that ID as a dependency; the deprecated +no-ID `entity` fallback is live and therefore cannot trigger scene-motion +replanning. Supplying `object_initial_pose` disables this implicit semantic +dependency because the explicit pose value is authoritative. + +Both bound grasp endpoints must provide `open` and `grasp`. Important `CoordinatedPickmentOptions` fields group into: - `pre_grasp_distance` and `lift_height`; @@ -483,8 +664,9 @@ Both bound end-effector profiles must provide `open` and `grasp`. Important - `approach_direction`, `left_to_right_arm_direction`, and `middle_empty_ratio` for affordance-based left/right grasp sampling. -The left/right arms and hands come exclusively from the corresponding binding -roles. Coordinated dual-arm planning with `strategy="motion_gen"` is not +The left/right motion and grasp endpoints come exclusively from the +corresponding participant slots. Coordinated dual-arm planning with +`strategy="motion_gen"` is not supported by the cuRobo backend; use the supported IK/interpolation path for this primitive. @@ -501,24 +683,26 @@ hold -> optionally release the placing hand -> retreat the placing arm**. |---|---| | Skill ID | `coordinated_placement` | | Goal | `CoordinatedPlacementGoal` | -| Binding | manipulator + end effector roles `placing` and `support` | -| Precondition | separate `HeldObjectState` entries exist for both bound arms | +| Binding contract | disjoint `placing` and `support` slots, each with disjoint `motion` and `grasp` endpoints | +| Precondition | each bound motion target exclusively holds a different object | | Goal geometry | placing/support object target poses, optional height offsets, optional release override | -| Effect | preserve support attachment; remove or preserve placing attachment according to `release`; clear overlapping coordinated state | +| Effect | preserve support attachment; remove or preserve placing attachment according to `release` | Both object targets may use `SceneEntityPose`, so either can participate in dynamic-goal invalidation. Goal-level height/release values override -`CoordinatedPlacementOptions` for that invocation. +`CoordinatedPlacementOptions` for that invocation. Two entries that identify +the same semantic object or live entity are a shared grasp, not a placing and +support pair, and their environment rows are rejected. -The placing profile must provide `open` and `grasp`; the support profile must -provide `grasp`. Important `CoordinatedPlacementOptions` fields group into: +The `placing.grasp` endpoint must provide `open` and `grasp`; `support.grasp` +must provide `grasp`. Important `CoordinatedPlacementOptions` fields group into: - default `release`, placing/support height offsets, and `lift_height`; - `hand_interp_steps`, `hold_steps`, and `retreat_steps`. -The placing/support arms and hands come exclusively from the corresponding -binding roles. The same cuRobo restriction as coordinated pickment applies to dual-arm -`strategy="motion_gen"` planning. +The placing/support motion and grasp endpoints come exclusively from the +corresponding participant slots. The same cuRobo restriction as coordinated +pickment applies to dual-arm `strategy="motion_gen"` planning. **Example:** `scripts/tutorials/atomic_action/coordinated_placement.py` @@ -534,30 +718,77 @@ retreats -> destination delivers**. |---|---| | Skill ID | `hand_over` | | Goal | `GraspGoal(semantics=...)` | -| Binding | manipulator + end effector roles `source` and `destination` | -| Precondition | source arm has a verified `HeldObjectState`; semantic object supports destination grasp selection | +| Binding contract | disjoint `source` and `destination` slots, each with disjoint `motion` and `grasp` endpoints | +| Precondition | source motion target exclusively has a verified `HeldObjectState`; goal semantics identify that object and support destination grasp selection | | Effect | remove source attachment and create destination `HeldObjectState` | | Verification | attachment transfer must be externally verified | -Both source and destination end-effector profiles must provide `open` and -`grasp`. `HandOverOptions` owns the destination grasp region and approach +Both source and destination grasp endpoints must provide `open` and `grasp`. +`HandOverOptions` owns the destination grasp region and approach direction, middle/final object poses, and segment distances/counts. The -source/destination arm and hand control parts come exclusively from the -corresponding `ActionBinding` roles. - -The middle and final poses are currently option tensors rather than -`SceneEntityPose` goal fields. Consequently, handover supports tracking-error -and timeout recovery, but does not automatically invalidate a moving handover -point. An application can submit a newer invocation revision with updated -`HandOverOptions`; the action also queries the semantic object's live -orientation when replanning and preserves it at the supplied middle/final -positions. +source/destination motion and grasp endpoints come exclusively from the +corresponding generic `ActionBinding` slots. The destination attachment reuses +the source relation's canonical `ObjectSemantics` instance. + +`middle_object_pose` and `final_object_pose` accept `(4, 4)`, `(B, 4, 4)`, or +`SceneEntityPose`. Scene-relative option values register their referenced +entities as recovery dependencies, so material movement of a handover or final +target can invalidate and replan the action. Tensor options remain fixed for the +invocation revision; an application can submit a newer compatible revision when +it intentionally changes them. + +The action verifies that the goal and source attachment have the same stable +object identity, then derives the current object orientation from the observed +source EEF pose and verified `object_to_eef` relation. The reused +`GraspGoal.grasp_xpos` field is not consumed by `HandOver` and does not create a +scene dependency; only the middle and final option poses do. As with the other coordinated primitive, cuRobo does not currently support its dual-arm `strategy="motion_gen"` path. **Example:** `scripts/tutorials/atomic_action/hand_over.py` +(builtin-operate-articulation)= + +## `OperateArticulation` + +Runs one reusable **approach -> engage -> operate -> release -> retract** +interaction for a drawer, slider, or another handle-driven articulation. + +| Contract | Value | +|---|---| +| Skill ID | `operate_articulation` | +| Goal | `OperateArticulationGoal(articulation_id, joint_id, geometry, source_position, target_position, target_displacement)` | +| Binding contract | disjoint `primary.motion` and `primary.interaction` endpoints | +| Required commands | `primary.interaction`: `open`, `grasp` | +| Effect | `ArticulationJointState[(articulation_id, joint_id)] = target_position` | +| Verification | explicit joint-state evidence is required before committing the effect | + +The first-class semantic call takes an articulation reference, an optional +handle affordance reference, and either a named target or an explicit +`target_position` plus `target_displacement` pair. The pair is intentionally +not inferred from simulator state: the core scene snapshot contains entity +poses, not articulation qpos. + +`ArticulationOperationAffordance` owns the joint ID, approach/contact/ +operation/retract offsets, operation axis, position scale, and optional named +position/displacement pairs. At every JIT grounding boundary the compiler +reads the latest registered handle pose and derives all four end-effector +poses. The displacement is measured from that observed handle pose. A named +target also supplies both its absolute joint postcondition and its explicit +handle-relative displacement. + +The grounded semantic effect uses an `ArticulationJointStateExpectation` and a +`JointStateEffectClause` addressed by canonical articulation and joint IDs. +Planning success alone never commits the symbolic joint state. + +The handle scene dependency has an exclusive cutoff at `operate.start`. Motion +before engagement can still invalidate and replan the trajectory; motion after +that boundary is expected to be caused by the operation and is not classified +as target drift. The joint-state effect monitor remains authoritative for +completion, and the cutoff also ignores unrelated external handle motion after +the operation starts. + ## Running the demos Every focused script is interactive by default. Add `--auto_play` to skip diff --git a/docs/source/overview/sim/atomic_actions/expert_programs.md b/docs/source/overview/sim/atomic_actions/expert_programs.md new file mode 100644 index 000000000..95275883a --- /dev/null +++ b/docs/source/overview/sim/atomic_actions/expert_programs.md @@ -0,0 +1,301 @@ +(expert-programs)= + +# Declarative Expert Programs + +Expert Programs let a task describe semantic intent without implementing a +task-local motion generator. A program names registered scene entities, robot +profiles, runtime presets, semantic calls, post-policies, and validators. The +shared compiler lowers every call just in time through the same +`SemanticSkillCompiler`, `AtomicActionEngine`, and `SkillRuntime` used by the +Python semantic API. + +Use an Expert Program when later motion depends on the physical result of an +earlier call. Each call receives a fresh scene observation, owns one +`ExecutionSession`, verifies its physical effect, and commits verified symbolic +state before the next call is grounded. + +## Author a program + +Schema version 1 supports bounded `sequence`, `repeat`, `segment`, and `invoke` +nodes. Schema version 2 additionally supports deterministic `parallel` blocks +and explicit `barrier` nodes. Unknown fields, unsupported discriminators, +unbounded structures, executable values, and dotted environment traversal are +rejected before physical execution or command emission. + +`RegisteredSemanticCall` is an opaque extension boundary in these schema +versions. An extension with a physical effect must also register its typed +compiler/effect contract; a serialized call ID alone cannot manufacture effect +verification semantics. + +The repeated-cube task is configured entirely as semantic calls: + +```yaml +schema_version: 1 +program_id: repeated_cube_pick_place +integration: + robot_profile: ur5_parallel_gripper_v1 + scene_registry: multi_segments_cube_v1 + runtime_preset: safe +targets: + drop_pose: + kind: cyclic_pose + values: + - position: [-0.40, 0.48, 0.10] + quaternion_wxyz: [1.0, 0.0, 0.0, 0.0] +program: + kind: repeat + count: 3 + body: + kind: segment + name: move_cube + steps: + kind: sequence + items: + - kind: invoke + call: {kind: pick, object: cube} + - kind: invoke + call: + kind: place + object: cube + at: {kind: target_ref, target: drop_pose} + post: + - {kind: wait_stable, entity: cube, preset: rigid_object} + validators: + - kind: object_near_target + object: cube + target: drop_pose + position_tolerance: 0.12 +``` + +The top-level Gym configuration selects the file with a path relative to that +configuration file: + +```json +{ + "expert_program_path": "../../expert_program/my_task.yaml" +} +``` + +`run_env` can override it explicitly: + +```bash +python -m embodichain.lab.scripts.run_env \ + --gym_config path/to/gym.json \ + --expert-program path/to/program.yaml +``` + +## Accept untrusted model output + +Model-generated programs use the same decoder and compiler, but enter through +the narrower MLLM frontend. The trusted host owns the scene, robot profile, and +runtime preset; the model response must omit `integration` entirely: + +```python +from embodichain.agents.mllm import compile_mllm_expert_program +from embodichain.lab.gym.envs.expert_program import ExpertProgramIntegrationCfg + +compiled = compile_mllm_expert_program( + model_response, + adapter=adapter, + integration=ExpertProgramIntegrationCfg( + robot_profile="my_robot_v1", + scene_registry="my_scene_v1", + runtime_preset="safe", + ), +) +``` + +This entry point accepts exactly one bounded JSON document. It rejects duplicate +keys, non-finite or overflowing numeric values, invalid Unicode, Markdown +fences, trailing text, and every normal schema violation. Its initial policy is +deliberately smaller than the file format: only schema version 1 and curated +`pick`, `place`, `hand_over`, and `operate_articulation` calls are admitted. +The model cannot select `resources`, a hand-over `receiver`, a runtime preset, +or an explicit articulation position/displacement; articulation operations must +use a host-declared named target. Registered calls and parallel nodes remain +host-authored extensions. + +`compile_mllm_expert_program` delegates to the existing +`ExpertProgramEnvironmentAdapter.compile` method. It neither creates a second +compiler nor assembles a runtime while validating model output. + +## Integrate a simulation task + +Task code supplies typed scene and robot integration declarations once, while +the external Expert Program configuration owns task sequence and targets. The +task then delegates runtime assembly to the shared factory; it does not +construct approach, grasp, pull, or placement trajectories: + +```python +MY_EXPERT_PROGRAM_REGISTRATION = SimulationExpertProgramRegistration( + scene_binding=create_my_scene_binding(), + robot_profile_binding=create_my_robot_profile_binding(), +) + + +class MyTaskEnv(ExpertProgramEnvironmentMixin, EmbodiedEnv): + def __init__(self, cfg, **kwargs): + super().__init__(cfg, **kwargs) + self._expert_program_adapter = create_simulation_expert_program_adapter( + self, + registration=MY_EXPERT_PROGRAM_REGISTRATION, + ) + + @property + def expert_program_adapter(self): + return self._expert_program_adapter +``` + +The scene binding is authoritative for semantic identity, live pose sources, +geometry, affordances, and collision roles. The robot profile owns reusable +resources, endpoint capabilities, semantic commands, policy presets, and effect +monitor selection. `SimulationRobotSkillProfileBinding` accepts generic +`RobotResourceBinding` declarations containing arbitrary typed +`ResourceEndpoint` values; `ControlPartResourceBinding` is its stricter +joint-backed convenience. Endpoint adapters and runtime transports are the +extension boundary for mobile-base, whole-body, or non-joint controllers and +are owned by `SimulationExpertProgramRegistration`, not passed as live helper +overrides. Their exact static target, payload, route, and transport declarations +enter the catalog fingerprint, while transport tuple order defines deterministic +Gym-action composition order. Task programs keep the same semantic calls and do +not gain controller-shaped fields. + +The current standard registration installs built-in joint tracking and effect +evidence providers only for `ControlPartEndpoint`. Every custom endpoint adapter +must declare empty tracking/evidence route sets and therefore uses +timed/open-loop completion. A non-joint closed-loop projector, feedback source, +or effect-evidence backend still requires the planned registration-owned +provider-factory extension; it must not be injected from a task after preflight. +Whole-body controllers expressed through existing joint control parts continue +to use the built-in joint route. + +Relation and rendezvous semantics are also explicit integration capabilities. +`Place(on=...)` and `Place(inside=...)` require an exact typed/versioned +`RelationTargetGrounder` for the selected affordance payload. `HandOver` +requires the profile-selected `HandOverPoseProvider`. A direct `Place(at=...)` +does not require a relation grounder. Missing, ambiguous, or stale providers +fail during provider-aware program preflight, before the first physical action. + +The standard simulation registration supplies the common relation path without +task-owned grounder code. Declare `SupportSurfaceAffordanceBinding` or +`ContainerAffordanceBinding` in `SimulationSceneBinding`; its +`object_target_pose` is the desired object pose relative to the explicitly named +object, articulation, or link parent. Mark one capability-scoped target per +parent as `is_default=True` when calls should reference only that parent. The +registration installs the matching exact grounder automatically and resolves +the target from a fresh scene snapshot. It never infers a placement frame from +an entity name, mesh, or bounding box. + +The same preflight rejects a reachable `safe` preset for a dynamic scene before +the first observation when the active motion generator cannot provide the +required dynamic collision world. + +## Execution and physical effects + +`AtomicDemoBridge` yields lazy `DemoSegment` actions. Every command and settling +hold is consumed by normal `env.step()`, so action managers, recorders, rewards, +timing, and dataset boundaries remain authoritative. `BaseEnv.step_dt` is the +only control cadence; a command duration that is not representable on that grid +fails instead of being silently resampled. + +Creating a bridge materializes the bounded segment stream and performs +provider-aware semantic analysis before any command can be emitted. Sequential +stretches retain downstream object-target look-ahead across segment boundaries; +an explicit parallel block is a conservative look-ahead barrier. Runtime still +re-observes and grounds each call just in time after prior verified effects. + +The standard simulation integration verifies grasp and release with two pieces +of evidence: + +- the last exact open/grasp command accepted by the buffered Gym command sink, + tracked independently for every stable environment ID; and +- the live object-to-endpoint pose relation from the shared scene snapshot. + +The command-state update is transactional: encoder, buffer, cancellation, or +safe-stop failures invalidate it. The current C1 standard path does not accept +task-side evidence callbacks: custom endpoint adapters must expose empty +tracking and effect-evidence route sets. Contact, constraint, force, wrench, or +other custom closed-loop sensing requires a future registration-owned provider +factory whose declaration enters the integration fingerprint; this will not +change the semantic call or program. + +Program/demo-segment metadata records runtime call results, named trajectory +segments, effect decisions, recovery events, scene and collision revisions, +settling outcomes, and validator results in deterministic JSON-safe values. +Trajectory segments are trace ranges inside one atomic plan; they do not create +independent recovery or timeout boundaries. + +Schema-version-2 parallel blocks additionally require an authoritative +`ParallelCommandSafetyValidator`. Resource-claim disjointness is necessary but +is not treated as proof of physical safety. If no validator is installed, the +parallel block refuses to start; the standard simulation adapter intentionally +does not invent one from resource names. Its task registration must instead +declare a safety factory covering the exact registered transport set; each +runtime assembly receives a fresh validator instance from that factory. Every +parallel frame must occupy +exactly one `BaseEnv.step_dt`; shorter lanes repeat their last safe target as +hold padding, while fractional frames are rejected rather than resampled. +Version 2 also uses strict symbolic key-level conflict detection at the barrier: +two branches may not commit the same task-state key, even when their physical +changes occurred in disjoint environment rows. + +Joint-position integrations using the active cuRobo planner can declare +`CuroboParallelSafetyValidatorFactory(validation_control_part="dual_arm")`. +The selected aggregate control part must contain every joint commanded by any +lane. Before dispatch, the validator combines the exact merged target with the +current measured joint state, interpolates the segment under `max_joint_step`, +and asks cuRobo to check every supplied sample against joint bounds, +self-collision, and the registry-backed static/dynamic world. It fails closed +when a joint is uncovered or the configured sample cap would under-sample the +segment. This gate does not replan or replace the trajectory. + +## Python semantic calls + +Standalone applications can use the same compiler and runtime through +`AtomicSkills`: + +```python +skills = AtomicSkills.from_env(runtime_provider, preset="safe") +cube = skills.scene.object("cube") +tray = skills.scene.object("tray") +result = skills.run(Pick(object=cube), Place(object=cube, on=tray)) +``` + +In this example, `runtime_provider` owns the typed relation grounder for the +tray's placement affordance. A standard simulation registration obtains it from +the support/container binding above. Applications without such a provider can +use a direct `SemanticPose` through `Place(at=...)`. + +`from_env` requires an explicit `SkillRuntimeProvider`; it never scans arbitrary +environment attributes. Gym demonstration environments intentionally use the +lazy bridge instead, because a synchronous runtime would bypass the required +`env.step()` handshake. Advanced applications may use +`AtomicSkills.from_components(...)` with explicit observation, command, +evidence, and clock ports. + +For the lower-level planning and execution contracts, see {doc}`index`. For +robot resource and endpoint declarations, see {doc}`robot_skill_profiles`. + +## Capability status + +| Surface | Shared contract | Standard simulation integration | +| --- | --- | --- | +| `Pick` | Compiler, runtime, effect verification | Antipodal grasp binding plus motion/grasp resources | +| `Place(at=...)` | Object-centric lowering with verified held state | Direct semantic pose target | +| `Place(on=...)` / `Place(inside=...)` | Exact typed relation dispatch | Support/container bindings install standard target-frame grounders; custom payloads install an exact `RelationTargetGrounder` | +| `HandOver` | Coordinated call, state flow, and effect contract | Embodiment must install its named `HandOverPoseProvider` and evidence sources | +| `OperateArticulation` | Named/absolute/displacement target and joint effect | Link, joint, operation-affordance, and interaction endpoint bindings | +| Registered calls | Typed call catalog and explicit lowerer | Physical extensions must add an explicit effect contract | +| Mobile/whole-body extensions | Generic resources, claims, endpoint targets, command frames, and routing | Requires a reusable semantic skill/lowerer plus matching adapter, payload, transport, and effect integration; no curated navigation or whole-body skill is installed today | +| Parallel blocks | Shared-clock coordinator and strict barrier merge | Joint-position/cuRobo tasks may declare the aggregate collision validator; other transports require an authoritative validator | + +The table separates implemented reusable contracts from embodiment-specific +providers. It is not a claim that every row has completed task-level physical +simulation acceptance. + +The Open Drawer vertical slice has completed its supported-simulation physical +run and reached the configured drawer joint target. Repeated cube pick/place +completes three independently observed physical Pick/Place/settle/validator +cycles. Its physical-fault gate opens the commanded gripper during Place, +observes held-object loss, performs a real re-acquisition Pick, retries Place, +and completes without simulator-side repair. diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index 6b38c9164..dc8ea5192 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -6,22 +6,26 @@ :hidden: builtin_actions +robot_skill_profiles +expert_programs ``` ```{currentmodule} embodichain.lab.sim.atomic_actions ``` Atomic actions are the typed planning and execution boundary between a semantic -task request and robot joint commands. A caller describes **what** should happen -with an action-owned goal, grounds semantic roles onto robot resources, and -supplies the latest measured context. The action returns a full-robot, -time-aware plan without stepping simulation or claiming that a physical effect -has occurred. +task request and runtime endpoint commands. A caller describes **what** should +happen with an action-owned goal, selects resources for the skill's participant +slots, and supplies the latest measured context. The action returns a +transport-neutral, time-aware plan without stepping simulation or claiming that +a physical effect has occurred. ```{note} -The current built-ins focus on arm-and-gripper manipulation. They already emit -full-robot-DoF trajectories, but dexterous-hand policies, lower-body locomotion, -and whole-body control are not implemented by this module yet. +The current built-ins focus on arm-and-gripper manipulation and retain an +optional full-robot joint trajectory for planning feedback and inspection. The +binding and runtime-command contracts are not limited to joints: locomotion, +whole-body, or other controllers add capabilities, endpoint adapters, command +payloads, and transports without adding fixed resource categories to the core. ``` ## Architecture and responsibility boundary @@ -33,8 +37,8 @@ and whole-body control are not implemented by this module yet. +---------------+----------------+ +---------------+----------------+ | | v | - agent adapter: schema validation, | - scene grounding, capability binding | + SemanticSkillCompiler / SkillRuntime: | + schema validation, SceneRegistry grounding, binding | | | +------------------+------------------+ | @@ -59,14 +63,17 @@ and whole-body control are not implemented by this module yet. one ActionPlan fixed projection observed closed loop | | v v - CompiledTrajectory JointCommand + events + CompiledTrajectory RuntimeCommandFrame + events | v ExecutionRunner observe / schedule / dispatch | v - ObservationProvider + CommandSink + Clock + ObservationProvider + EndpointCommandRouter + Clock + | + v + EndpointCommandTransport(s) ``` The boundary is deliberate: @@ -75,36 +82,46 @@ The boundary is deliberate: |---|---|---| | Task intent and sequencing | Action Agent, task graph, or user-authored application | Selects skills, goals, and execution order | | Invocation construction | Agent adapter or user-authored code/config loader | Produces the same typed `ActionInvocation`; the engine has no agent-only interface | -| Perception and grounding | Agent adapter or user application | Builds scene snapshots and resource bindings, or supplies already-grounded values directly | +| Perception and grounding | `SceneRegistry` on the canonical path; adapter or user application on the advanced path | Normalizes aliases to canonical typed references and publishes snapshots, or supplies already-grounded values directly | | Deterministic motion planning | Atomic action module | Produces an `ActionPlan` from an invocation and context | | Motion-generation resources | `AtomicActionEngine` | Owns one robot, motion generator, planner backend, device, trajectory builder, and control-part command profiles | -| Recovery state | `ExecutionSession` | Consumes fresh contexts, emits at most one `JointCommand` per tick, and owns bounded recovery/revision state | -| Scene observation | `SceneProvider` | Captures ordered entities plus monotonic global or per-environment collision-world revisions | +| Recovery state | `ExecutionSession` | Consumes fresh contexts, emits at most one `RuntimeCommandFrame` per tick, and owns bounded recovery/revision state | +| Scene observation | Registry-derived `SceneProvider` | Captures canonical ordered entities plus monotonic global or per-environment collision-world revisions | | Scheduling and controller lifecycle | `ExecutionRunner` | Observes only when due, dispatches timed commands, records acknowledgements, and performs safe stop | -| Robot/simulator I/O | `ObservationProvider`, `CommandSink`, and `ExecutionClock` adapters | Isolates observation, command transport, and time/physics advancement from planning and session state | -| Physical-effect verification | Application observer | Verifies grasp, release, handover, and other symbolic effects | +| Robot/simulator I/O | `ObservationProvider`, `EndpointCommandRouter`, `EndpointCommandTransport`, and `ExecutionClock` adapters | Isolates observation, per-controller command transport, and time/physics advancement from planning and session state | +| Physical-effect evidence | Backend provider or application adapter | Acquires typed pose/contact/controller evidence without applying policy thresholds | +| Effect decision and correlation | `EffectMonitor` plus the semantic runtime adapter, or an application verifier on the direct-core path | Interprets evidence, attaches the current request ID, and reports grasp, release, handover, or other symbolic effects | `ExecutionRunner.step()` is non-blocking. Its convenience `run_until_blocked()` loop waits or advances simulation through an injected clock. Observation errors, rejected or timed-out commands, session failures, and explicit cancellation trigger a best-effort cancel-then-hold sequence. -`SimulationExecutionAdapter` implements all three ports for a simulation robot; -real hardware integrations implement the same protocols without changing -action planning or recovery state. +`SimulationExecutionAdapter` provides observation, clock, and the built-in +`robot.joint_position` transport for a simulation robot. Register it with an +`EndpointCommandRouter`; real hardware integrations provide transports for the +same or additional endpoint kinds without changing action planning or recovery +state. ### Caller entry points -The engine supports two first-class caller paths. An Action Agent emits a -semantic skill call that an adapter validates, grounds, and converts into an -`ActionInvocation`. A user can instead author the typed invocation directly in -Python or load it from an application-owned configuration layer: +The engine supports two first-class caller paths. An Action Agent or +configuration-driven application can emit a semantic call for +{class}`~embodichain.lab.sim.skills.SemanticSkillCompiler` and +{class}`~embodichain.lab.sim.skills.SkillRuntime` to validate, ground, +and convert into an `ActionInvocation`. A user can instead author the typed +invocation directly in Python or load it from an application-owned +configuration layer: ```python +binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "left_arm"}}, +) manual_invocation = ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(xpos=target_pose), - binding=ActionBinding(manipulators={"primary": "left_arm"}), - motion_policy=MotionPolicy(sample_count=80, control_dt=1.0 / 60.0), + binding=binding, + motion_policy=MotionPolicy(sample_count=80), recovery_policy=RecoveryPolicy(max_replans=2), ) @@ -115,10 +132,10 @@ live_session = engine.start((manual_invocation,), latest_context) ``` A manual caller may bypass the semantic-schema adapter only when its target and -robot-resource binding are already grounded. Scene-relative goals still need a -current `PlanningContext`, and object names or semantic roles still need to be -resolved by the user application (or by reusing the same grounding adapter as -the Agent path). +robot-resource endpoints are already grounded. Scene-relative goals still need +a current `PlanningContext`, and object names or participant selections still +need to be resolved by the user application (or by reusing the same grounding +adapter as the Agent path). Both paths converge at `ActionInvocation + PlanningContext`. They therefore use the same goal validation, capability checks, planning backend, execution @@ -133,14 +150,14 @@ Application code normally chooses between these three public entry points: | API | Choose it when | Returns | State and observation behavior | |---|---|---|---| | `engine.plan(invocation, context)` | You need to inspect or plan exactly one registered action | `ActionPlan` | Reads one context; does not project its terminal qpos or expected task effect for another action | -| `engine.compile(invocations, context)` | All goals for an ordered static sequence are known before execution | `CompiledTrajectory` | Plans in order and propagates hypothetical qpos and expected effects through `projected_context`; never observes execution | +| `engine.compile(invocations, context)` | All goals are known and every action provides an inspectable joint trajectory | `CompiledTrajectory` | Plans in order and propagates hypothetical qpos and expected effects through `projected_context`; never observes execution | | `engine.start(invocations, context)` | Commands must be issued incrementally from fresh observations with bounded recovery | `ExecutionSession` | `tick(latest_context)` consumes measured state, emits at most one command, requests effect verification, and can replan | The short selection rule is: ```text one action to inspect or plan -> plan -one or more actions in a fixed scene -> compile +joint-trajectory actions in a fixed scene -> compile observed execution and error recovery -> start, then tick ``` @@ -157,6 +174,11 @@ observe a new `PlanningContext`, and plan or compile the next stage. Use `start()` when that observe/replan loop should be managed continuously by an `ExecutionSession`. +`compile()` is intentionally an offline **joint-trajectory** projection API. It +rejects an `ActionPlan` whose optional `joint_trajectory` is absent. Generic +non-joint command plans remain valid for `plan()` and `start()`; composing their +hypothetical state requires a future endpoint-specific projection contract. + ## Core contracts The public contracts separate values with different owners and lifetimes. This @@ -165,83 +187,120 @@ from leaking into an Action Agent schema. | Contract | Contains | Does not contain | |---|---|---| -| `ActionGoal` | Action-specific desired outcome, such as an EEF pose or object pose | Arm names, planner instances, recovery counters | -| `ActionBinding` | Semantic-role mappings to keys from the engine robot's `control_parts`, such as `primary -> left_arm` and `primary -> left_hand` | Link/TCP names, arbitrary scene objects, motion settings, or task geometry | +| Action-owned goal dataclass | Action-specific desired outcome, such as an EEF pose or object pose | Arm names, planner instances, recovery counters | +| `SkillBindingContract` | Skill-local participant slots, required endpoint capabilities and commands, and disjointness constraints | Concrete robot resources, controller handles, or transport configuration | +| `ActionBinding` / `EndpointBinding` | Engine-owned endpoint snapshots keyed by `(slot_id, endpoint_id)`, including capabilities, semantic commands, claims, and an immutable runtime target | Live controllers, planner settings, task geometry, or caller-owned mutable mappings | | `ActionOptions` / built-in `*Options` | Frozen invocation-varying skill behavior: segment counts, offsets, grasp-selection rules | Robot resource names, hand qpos, planner backend | -| `ControlPartCommandProfile` | Embodiment-specific semantic commands such as `open`, `grasp`, and `ready`, keyed by actual control-part name | Action roles, task goals, recovery state | -| `ActionControlOverrides` | Optional role-scoped command replacements for one invocation revision | Persistent robot configuration | -| `MotionPolicy` | Motion strategy, sample count, timing, limits, dynamic-collision mode, typed planner options | Skill semantics or robot-resource names | +| `ControlPartCommandProfile` | Embodiment-specific semantic commands such as `open`, `grasp`, and `ready`, keyed by actual control-part name | Skill slots/endpoints, task goals, recovery state | +| `ActionControlOverrides` | Optional `(slot, endpoint)`-scoped command replacements for one invocation revision | Persistent robot configuration | +| `MotionPolicy` | Motion strategy, sample count, dynamic-collision mode, typed planner options | Execution cadence, skill semantics, or robot-resource names | | `RecoveryPolicy` | Action replan/retry budgets, tracking and dynamic-goal thresholds, action-attempt timeout | Controller state or mutable counters | | `ExecutionRunnerCfg` | Runner-level acknowledgement deadlines, minimum feedback cadence, and completion hold policy | Skill behavior, planning resources, or invocation revision data | -| `PlanningContext` | Measured `RobotObservation`, verified `TaskState`, versioned `SceneSnapshot`, stable environment IDs | Hypothetical simulator mutation | -| `ActionPlan` | Per-environment result, one scene-bound timed trajectory, named segments, action-level recovery metadata, diagnostics, expected `StateDelta` | Proof that a grasp/release/contact physically succeeded; independently recoverable segment boundaries | +| `PlanningContext` | Measured `RobotObservation`, verified `TaskState`, versioned `SceneSnapshot`, stable environment IDs, and optional explicit control cadence for action-owned interpolation | Hypothetical simulator mutation or a planner timing fallback | +| `ActionPlan` | Per-environment result, `TimedCommandSequence`, optional joint trajectory, named segments, action-level recovery metadata, diagnostics, expected `StateDelta` | Proof that a grasp/release/contact physically succeeded; independently recoverable segment boundaries | +| `RuntimeCommandFrame` | Synchronized endpoint commands, active rows, stable environment IDs, and per-row hold duration | Live transport or controller objects | `MotionPolicy.strategy` accepts exactly `"motion_gen"` or `"ik_interp"`; the same value is forwarded to `MotionGenOptions.strategy` without an adapter layer. +Every planner result that contains positions must also contain per-waypoint +`dt`; its per-environment `duration` is derived from those intervals. Every +action passes a `TimedTrajectory` to `build_plan()`; raw position tensors are +rejected. For +action-owned deterministic interpolation, the integration supplies its +authoritative cadence as `PlanningContext.control_dt` (normally +`BaseEnv.step_dt`). The engine never supplies or guesses missing timing. +Planner-backend compatibility is a profile-level concern expressed by +`SkillPolicyPreset.required_planner`, not a per-invocation motion choice. + +Each action owns one or more frozen goal dataclasses and declares the accepted +type through `AtomicAction.GoalType`. The action validates that type when the +engine resolves an invocation. There is no marker protocol, shared +`ActionTarget` base class, or closed union that must change whenever a skill is +added. + +### Skill contracts and endpoint binding + +The canonical semantic path uses a +{doc}`RobotSkillProfile ` to match skill-local slots and +endpoint capabilities against a generic robot resource graph. It validates +participant pairing, typed commands, physical claims, complete defaults, and +policy presets before producing the engine-owned `ActionBinding` used by an +invocation. + +Each `AtomicAction` declares one explicit `SkillBindingContract`. A **slot** is +an action-local participant such as `primary`, `source`, or `destination`. Each +slot contains one or more named endpoint requirements. An endpoint name is also +local to the skill contract: current manipulation skills use `motion` and +`grasp`, while a future navigation or whole-body skill can declare different +names and open, namespaced capabilities. There are no global `manipulator`, +`end_effector`, `base`, or `whole_body` fields to extend. + +For example, `PickUp` requires `primary.motion` with its motion capabilities and +`primary.grasp` with the `interaction.grasp` capability plus typed `open` and +`grasp` commands. Its contract also requires those two endpoint views to have +disjoint physical claims. A profile can satisfy that contract with a composite +participant resource whose endpoints resolve to an arm and hand. Another skill +may deliberately permit overlapping views of one coupled whole-body +controller. + +The canonical path resolves the skill through a bound profile: -Goals follow the structural `ActionGoal` protocol: each action owns one or more -frozen dataclasses with a stable `goal_kind`. There is no shared `ActionTarget` -base class and no closed union that must change whenever a skill is added. - -### Semantic resource binding - -A **role** is an action-owned semantic participant slot: it describes the job a -robot resource performs in that action, not the identity of the resource. Each -`AtomicAction` declares its required slots through `manipulator_roles` and -`end_effector_roles`; the same declarations are exposed through its -`SkillDescriptor` so an Agent adapter or manual caller can construct a complete -binding before planning. +```python +resolved = engine.skill_profile.resolve( + "pick_up", + selections={"primary": "left_participant"}, +) +binding = resolved.action_binding +``` -Role names are local to both the skill and the resource category. For example, -`primary` in `manipulators` and `primary` in `end_effectors` are two separate -slots. Using the same role name expresses that the selected arm and hand/tool -serve the same functional participant in the action: +Advanced direct-core code can select joint-backed endpoints by actual +`Robot.control_parts` names. Use the engine helper rather than constructing an +`ActionBinding` manually; the helper validates the installed skill's contract, +resolves joint indices and commands, and stamps the engine ownership identity: ```python -binding = ActionBinding( - manipulators={"primary": "left_arm"}, - end_effectors={"primary": "left_hand"}, +binding = engine.bind_control_parts( + "pick_up", + { + "primary": { + "motion": "left_arm", + "grasp": "left_hand", + } + }, ) ``` -In this example, `primary` is the role and `left_arm` / `left_hand` are the -bound resources. `primary` does not mean left, right, the first configured -arm, or a globally preferred arm; it simply denotes the principal participant -of a single-participant skill. Changing the values can bind the same action to -another compatible arm and tool without changing its goal or implementation. - -Every bound value is the name of a control part declared by the engine-owned -robot. Both `left_arm` and `left_hand` must therefore be keys in -`robot.control_parts` (originating from `RobotCfg.control_parts`). They are not -joint names, link names, TCP frame names, or scene-object identifiers. -`end_effectors` specifically selects the actuated tool/hand control part; the -manipulator's IK/TCP frame remains part of the robot and solver configuration. -The engine validates every name and resolves its full-robot joint indices -before calling the action planner. - -The validation boundary is intentionally narrow: the engine verifies required -roles, `control_parts` membership, resolvable joint indices, command type, and -command dimensions. The Agent adapter or application binder remains responsible -for capability compatibility, such as pairing an arm with the hand mounted on -it and choosing a semantic command supported by that tool. - -Role names should describe action responsibilities rather than robot-specific -joint, link, or model names. Single-resource skills use `primary`; handover uses +The resulting `ActionBinding` is generic. Each `EndpointBinding` records its +`slot_id`, `endpoint_id`, logical `resource_id`, adapter ID, capabilities, +commands, claim tokens, and a typed `RuntimeEndpointTarget`. A target contains +only immutable addressing information such as transport ID and destination ID; +the live simulator entity, hardware client, or controller belongs to the +registered transport. Profile endpoint adapters can therefore return a mobile, +whole-body, joint-position, or custom target without changing `ActionBinding`. + +Slot names describe action responsibilities rather than robot-specific joint, +link, or model names. Single-participant skills use `primary`; handover uses `source` and `destination`; coordinated placement uses `placing` and `support`. The current coordinated-pick contract uses `left` and `right` because its goal -geometry also distinguishes left/right grasps. New skills should prefer -functional roles unless a spatial distinction is intrinsic to their semantics. +geometry distinguishes left/right grasps. New skills should prefer functional +slot names unless a spatial distinction is intrinsic to their semantics. -All built-ins resolve participating arm and hand control parts from the binding. -They obtain hardware-specific `open` and `grasp` commands from the resolved -end-effector profile; no action or option duplicates arm names, hand names, or -hand qpos. Attachment state and expected effects are keyed by the bound -manipulator control-part name. +Current built-ins resolve joint-backed `motion` and `grasp` endpoints from the +binding. They obtain hardware-specific `open` and `grasp` commands from the +resolved grasp endpoint; no action or option duplicates arm names, hand names, +or hand qpos. Their attachment state and expected effects are currently keyed +by the motion endpoint's control-part target. ### Control-part semantic commands -Register embodiment commands once when constructing the engine. The keys are -concrete names from `robot.control_parts`; the command names remain semantic: +On the canonical semantic path, declare embodiment commands on the +{doc}`RobotSkillProfile ` and pass the profile through the +engine's `skill_profile` argument. For a direct-core integration, register the +same command profiles explicitly when constructing the engine. Profile command +IDs are generic and selected by endpoint adapters; the built-in control-part +adapter defaults them to concrete `robot.control_parts` names. Direct-core +engine keys are always concrete control-part names. The command names remain +semantic: ```python engine = AtomicActionEngine( @@ -259,12 +318,13 @@ engine = AtomicActionEngine( ``` `MoveJoints(JointPositionGoal("ready"))` resolves `ready` from its bound -manipulator. Manipulation primitives resolve `open` and/or `grasp` from their -bound end effectors. A one-dimensional `JointPositionCommand` broadcasts over -the planning batch; a two-dimensional value must match the selected batch. +`primary.motion` endpoint. Manipulation primitives resolve `open` and/or +`grasp` from their bound grasp endpoints. A one-dimensional +`JointPositionCommand` broadcasts over the planning batch; a two-dimensional +value must match the selected batch. -For a one-off change, override by action role rather than by concrete robot -name: +For a one-off change, override by action-local slot and endpoint rather than by +concrete robot name: ```python invocation = ActionInvocation( @@ -272,9 +332,11 @@ invocation = ActionInvocation( goal=goal, binding=binding, control_overrides=ActionControlOverrides( - end_effectors={ + endpoints={ "primary": { - "grasp": JointPositionCommand(object_specific_grasp_qpos), + "grasp": { + "grasp": JointPositionCommand(object_specific_grasp_qpos), + } } } ), @@ -282,9 +344,9 @@ invocation = ActionInvocation( ) ``` -The engine merges the override after resolving `primary` and captures the -result in `ResolvedActionRequest`. Automatic recovery for revision 1 sees the -same command snapshot. Joint limits remain constraints; they do not define the +The engine merges the override into `primary.grasp` and captures the result in +`ResolvedActionRequest`. Automatic recovery for revision 1 sees the same +command snapshot. Joint limits remain constraints; they do not define the semantic meaning of `open` or `grasp`. Tutorials may explicitly derive a simple profile from limits, while a robot integration should normally provide calibrated commands. @@ -298,7 +360,7 @@ instances to the engine's planning services: ```python engine = AtomicActionEngine(motion_generator, control_profiles=profiles) -# All nine built-ins are immediately usable by stable skill ID. +# All eleven built-ins are immediately usable by stable skill ID. assert "move_end_effector" in engine.actions assert "pick_up" in engine.actions ``` @@ -317,11 +379,17 @@ entity poses from the current `SceneSnapshot` into copied backend options, then calls the skill-specific `_plan()` hook. Individual skills therefore do not own dynamic-obstacle parameters or mutate caller-owned motion policies. +For a canonical integration, construct the snapshot provider and collision +world from one {doc}`SceneRegistry <../scene_registry>`. Direct use of +`RigidObjectSceneProvider` remains an advanced-core path. + Registration means that an implementation is installed, not that every robot -can execute it. Required roles, control parts, profiles, and task-state -preconditions are validated while an invocation is resolved and planned. Agent -adapters must additionally filter the catalog by `agent_visible` and -embodiment capability instead of exposing every `engine.actions` entry blindly. +can execute it. `engine.actions` contains direct-core implementations; +`engine.skills` contains installed, agent-visible implementations with an +explicit generic binding contract; and `engine.skill_profile.skills` applies +embodiment capability filtering. Required task-state preconditions remain +runtime conditions and are validated while an invocation is resolved and +planned. Use invocation `skill_options` whenever behavior varies per call. Two variants with the same stable skill ID therefore share one built-in implementation: @@ -355,10 +423,9 @@ custom_engine.register(MyAction()) engine.register(CustomPickUp(), replace=True) ``` -The module-level `register_action()` catalog is only for process-wide extension -type discovery. It does not mutate existing engines or join their default -built-in set; instantiate a discovered extension and pass it to -`engine.register()` explicitly. +Registration is deliberately engine-local. Construct an extension and pass it +to `engine.register()` explicitly; there is no separate process-wide catalog +whose contents can drift from the actions installed in an engine. ### Implementation and advanced APIs @@ -366,13 +433,20 @@ The similarly named `AtomicAction.plan()` method is not a fourth application entry point. It is a framework-owned template method called by the engine after resolving an invocation; skill implementations provide `_plan()`: +This is a deliberate hard extension boundary. Defining `plan()` on a subclass +raises `TypeError` at class definition and has no compatibility adapter. Migrate +an older custom action by renaming its implementation to `_plan()`. + | API | Intended caller | Behavior | |---|---|---| | `AtomicAction.plan(request, context)` | `AtomicActionEngine` | Binds the current collision scene into a copied policy, then delegates to `_plan()` | | `AtomicAction._plan(request, context)` | Atomic-action implementer | Consumes the prepared immutable `ResolvedActionRequest` and returns an `ActionPlan` | | `engine.plan_action(action, invocation, context)` | Extension or isolated test | Temporarily binds and plans an unregistered action instance; built-in parameter variants should use invocation `skill_options` instead | -| `session.revise_current(invocation)` | Runtime orchestrator or Action Agent | Replaces the active logical call with a newer revision and replans from the latest observed context | -| `runner.step(effect_success=...)` | Non-blocking controller integration | Observes and dispatches only when the next timed command is due | +| `engine.start(invocations, context, eligible_mask=...)` | Runtime orchestrator | Starts a session whose owned row cohort can only shrink across action barriers and recovery | +| `session.revise_current(invocation)` | Manually ticked runtime orchestrator | Replaces the active logical call with a newer same-destination revision and replans from the latest observed context | +| `runner.revise_current(invocation)` | Runner-driven runtime orchestrator or Action Agent | Snapshots a revision, preserves the current frame deadline, then replans from a fresh due-time observation | +| `runner.deactivate_rows(mask, reason=...)` | Runner-driven runtime orchestrator | Permanently removes rows and refreshes the runner's cached effect request; prefer it over direct session mutation | +| `runner.step(effect_result=..., effect_verifier=...)` | Non-blocking controller integration | Observes only when due; accepts either an asynchronous correlated result or a synchronous verifier, never both | | `runner.run_until_blocked(...)` | Simple blocking application or tutorial | Advances the injected clock until terminal or external effect verification is required | | `runner.cancel(reason)` | Explicit safe stop | Requests controller cancellation followed by an observed-position hold | @@ -385,28 +459,39 @@ Use `engine.plan()` when one registered action needs to be inspected, tested, or integrated into application-owned orchestration: ```python +binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "left_arm"}}, +) invocation = ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(xpos=target_pose), - binding=ActionBinding(manipulators={"primary": "left_arm"}), - motion_policy=MotionPolicy(sample_count=80, control_dt=1.0 / 60.0), + binding=binding, + motion_policy=MotionPolicy(sample_count=80), ) plan = engine.plan(invocation, latest_context) if plan.plan_success.all(): - positions = plan.trajectory.positions + command_frames = plan.commands.frames + if plan.joint_trajectory is not None: + positions = plan.joint_trajectory.positions ``` -The result contains that action's trajectory, named segment ranges, -diagnostics, action-level recovery metadata, and uncommitted expected effects. -`plan()` does not automatically create a next context. If another action must -be planned against this action's hypothetical result, use `compile()` instead -of manually reproducing its state projection rules. +The result always contains that action's transport-neutral command sequence. A +joint-planned action may additionally retain `joint_trajectory` for feedback, +inspection, and static qpos projection. The plan also contains named segment +ranges, diagnostics, action-level recovery metadata, and uncommitted expected +effects. `plan()` does not automatically create a next context. If another +action must be planned against this action's hypothetical result, use +`compile()` instead of manually reproducing its state projection rules. `AtomicAction.build_plan()` normalizes scalar or per-environment planner success and replaces unsuccessful rows with the context's observed joint position. Primitive implementations therefore preserve row-local failures in `plan_success`; they do not need to duplicate failure-row hold logic. +It accepts only `TimedTrajectory`. Interpolation code can construct one with +`TimedTrajectory.from_uniform_step(..., step_dt=context.require_control_dt())`; +planner-backed code should preserve the planner's explicit `dt`. `TrajectorySegment.start` and `.stop` form an action-local half-open waypoint range. `plan.segment(name)` resolves that local metadata, while @@ -418,14 +503,13 @@ still replans and retries the enclosing action as one unit. ## Static compilation -`compile()` plans invocations in order. For every successful action it projects -the terminal qpos and expected task-state effect into a new context so the next -action can be checked against a hypothetical result. The observed context and -simulator remain unchanged. +`compile()` plans joint-trajectory invocations in order. For every successful +action it projects the terminal qpos and expected task-state effect into a new +context so the next action can be checked against a hypothetical result. The +observed context and simulator remain unchanged. ```python from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, @@ -433,8 +517,11 @@ from embodichain.lab.sim.atomic_actions import ( ) engine = AtomicActionEngine(motion_generator) -binding = ActionBinding(manipulators={"primary": "left_arm"}) -motion_policy = MotionPolicy(sample_count=80, control_dt=1.0 / 60.0) +binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "left_arm"}}, +) +motion_policy = MotionPolicy(sample_count=80) approach = ActionInvocation( skill_id="move_end_effector", @@ -487,7 +574,10 @@ moving_goal = ActionInvocation( minimum_confidence=0.8, ) ), - binding=ActionBinding(manipulators={"primary": "left_arm"}), + binding=engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "left_arm"}}, + ), recovery_policy=RecoveryPolicy( max_replans=3, max_action_retries=2, @@ -503,21 +593,23 @@ session = engine.start((moving_goal,), latest_context) while session.status is ExecutionStatus.RUNNING: tick = session.tick(latest_context) if tick.command is not None: - send_joint_command(tick.command) + dispatch_runtime_frame(tick.command) latest_context = observe_context() ``` For most applications, use `ExecutionRunner` to keep scheduling and controller -acknowledgement handling outside the session: +acknowledgement handling outside the session. The following snippet shows the +advanced direct-core provider path: ```python scene_provider = RigidObjectSceneProvider({"moving_tray": moving_tray}) adapter = SimulationExecutionAdapter(sim, robot, scene_provider=scene_provider) +router = EndpointCommandRouter((adapter,)) initial_context = adapter.observe( TaskState.empty(robot.get_qpos().shape[0], robot.device) ) session = engine.start((moving_goal,), initial_context) -runner = ExecutionRunner(session, adapter, adapter, clock=adapter) +runner = ExecutionRunner(session, adapter, router, clock=adapter) result = runner.run_until_blocked() ``` @@ -526,17 +618,42 @@ pass a `scene_supplier(timestamp)` callback instead. `scene_provider` and `scene_supplier` are mutually exclusive. `ExecutionRunner.step()` is the non-blocking entry point for an application -that already owns its event loop. It observes only when the previous command's -`hold_duration` has elapsed, dispatches active commands through `CommandSink`, -and records accepted, rejected, or timed-out acknowledgements. Cancellation, +that already owns its event loop. It observes only when the previous +`RuntimeCommandFrame.hold_duration` has elapsed, dispatches active endpoint +commands through `EndpointCommandRouter`, and records accepted, rejected, or +timed-out acknowledgements. The router preflights a whole frame, groups commands +by exact `transport_id`, and aggregates transport acknowledgements, so an +unknown or incompatible transport cannot cause partial dispatch. Cancellation, observation/session exceptions, and negative acknowledgements enter a -best-effort cancel-then-hold path. - -`TimedTrajectory.dt[:, i]` is the interval leading to sample `i`. -`ExecutionSession` maps each following arrival interval onto the preceding -command's post-dispatch hold, while the final sample reuses its own interval as -a settling window before terminal validation. A batched runner uses the longest -active row interval as its synchronized barrier. +best-effort cancel-then-hold path for every armed runtime target. + +Pass an owned `eligible_mask` to `engine.start()` when only a subset of rows may +enter the invocation sequence. This cohort is sticky: eligibility can only +shrink across action barriers and replans. Later failures outside the atomic +runtime should call `runner.deactivate_rows(mask, reason=...)`; the operation is +idempotent, the next command neutralizes changed rows, and removing the final +eligible row fails and terminates the session. When effect verification is +pending, deactivation narrows the request and assigns a new +`verification_id`. Do not mutate `session` directly while its runner owns +scheduling, because the runner must refresh its cached effect boundary. + +The engine authorizes every emitted command against the immutable target and +physical claims in the resolved binding. A command cannot address an unbound +destination, substitute target metadata, or overlap another endpoint's joints +or claim tokens. Non-empty frames and recovery plans keep one stable +destination set. If a failed replan emits no frames, the session retains the +previous targets so the runner can still request a transport-owned hold. + +An inactive row is not equivalent to omitting a write: each transport must +actively neutralize inactive rows for every addressed target. The simulation +joint-position transport holds observed positions for those rows; a velocity +transport would normally send zero velocity. + +Each `RuntimeCommandFrame` carries the delay before the next frame. A batched +runner uses the longest active row duration as its synchronized barrier. The +joint-trajectory lowering helper derives these holds from trajectory arrival +intervals; non-joint planners set them directly when building their +`TimedCommandSequence`. `SimulationExecutionAdapter.sleep()` converts that interval to an integral number of physics steps instead of using wall-clock sleep. Stable `env_ids` remain correlation identifiers and are not used as simulator array indices. @@ -564,17 +681,38 @@ per-environment action scheduling belongs in a higher-level scheduler rather tha this atomic-action session. `SceneProvider.snapshot(timestamp=..., env_ids=...)` is the scene-observation -boundary. `SceneSnapshot.collision_entity_ids` identifies obstacle poses -consumed by a planner, while `collision_world_revision` can be global or -per-environment. `RigidObjectSceneProvider` tracks live simulation objects, -filters sub-threshold pose noise, and advances those revisions. Its threshold -baseline is the last materially published pose for each entity/environment, so -cumulative sub-threshold motion cannot remain hidden indefinitely. Backends opt -in through `supports_collision_world_updates` and `with_collision_world()`; +boundary. On the canonical planning path, +`SceneRegistry.make_planning_scene_provider()` derives an independent provider +and eagerly validates its collision contract against the motion generator. +Its snapshots expose canonical registry IDs only. +The registry owns static identity, aliases, geometry, affordances, hierarchy, +and collision roles; `SceneSnapshot` owns versioned dynamic pose/confidence and +collision revisions. Snapshot states are defensively copied on construction and +public read. + +`SceneSnapshot.collision_entity_ids` identifies obstacle poses consumed by a +planner, while `collision_world_revision` can be global or per-environment. +Registry-derived providers filter sub-threshold pose noise and advance those +revisions from the last materially published pose, so cumulative motion cannot +remain hidden indefinitely. Backends opt in through +`supports_collision_world_updates` and `with_collision_world()`; `MotionGenerator.bind_collision_world()` owns that backend boundary, and cuRobo maps the snapshot poses to `CuroboPlanOptions.dynamic_obstacle_poses`. A newer revision invalidates only affected rows before synchronized cohort replanning. +`make_planning_scene_provider()` requires two exact canonical-ID agreements: +the registry's complete `STATIC ∪ DYNAMIC` set must equal the planner's +complete collision-world set, and the registry, derived provider, and planner +dynamic subsets must equal one another. It also requires planner update support +for a non-empty dynamic subset and matching shared or per-environment world +semantics. A one-environment registry may infer `SHARED`; a multi-environment +dynamic registry must choose `SHARED` or `PER_ENV` explicitly. External +perception/hardware providers use +`validate_collision_integration(..., scene_provider=...)` directly. Plain +`make_scene_provider()` and `RigidObjectSceneProvider` are perception or +advanced direct-core paths without eager planner agreement. See +{doc}`../scene_registry` for setup. + `MotionPolicy.dynamic_collision_mode` controls this live-scene path. `AUTO` (the default) consumes collision entities when the selected motion strategy and planner support them, `OFF` ignores snapshot collision entities and their @@ -590,14 +728,12 @@ varies only the measured context. Mutable goal values such as tensors and metadata containers are copied, while simulator-backed `BatchEntity` handles retain their runtime identity. -Each emitted `JointCommand` carries a per-environment `hold_duration` derived -from the plan's `TimedTrajectory.dt`. The application control loop must respect -that timing after dispatching the command and before requesting the next -observation. `dt[:, i]` is the arrival interval leading to waypoint `i`, so the -first waypoint is dispatched immediately and command `i` carries `dt[:, i + 1]` -until the next waypoint is due. The final command reuses `dt[:, -1]` as a -settling window. For a synchronized batch, the caller should wait for the -longest duration among active rows. A passive hold command has zero duration. +Each emitted `RuntimeCommandFrame` carries a per-environment `hold_duration`. +The application control loop must respect that timing after dispatch and before +requesting the next observation. For a synchronized batch, the caller waits +for the longest duration among active rows. Safe stop is a separate transport +lifecycle: the runner cancels every armed target and then asks each transport to +hold that target from the latest observed context. Use an explicit newer revision when the application or Action Agent decides to change runtime behavior: @@ -614,20 +750,36 @@ revised = ActionInvocation( invocation_id=current.invocation_id, revision=current.revision + 1, ) -session.revise_current(revised) +runner.revise_current(revised) ``` `skill_id` and `invocation_id` must still identify the active logical call. Revision replacement preserves verified task state and environment eligibility, resets the new revision's local recovery counters, emits -`INVOCATION_REVISED`, and replans from the latest context. +`INVOCATION_REVISED`, and replans from the latest context. Once the current +action owns runtime destinations, the revision must declare the same non-empty +destination set and preserve every target's exact address fingerprint, including +its safe-hold footprint. Switching to a base, another arm, or another controller +is a new invocation boundary. `runner.revise_current()` stages the owned request, +keeps the current frame deadline, and plans only after collecting the next due +observation. A physical effect awaiting verification cannot be abandoned by a +revision; verify it first, or cancel and start a new invocation. Callers that +drive `ExecutionSession.tick()` directly can use `session.revise_current()` and +should pass their fresh context explicitly. ```{attention} Automatic dynamic-goal invalidation is dependency-driven. A goal must contain a -`SceneEntityPose` for the session to track that scene entity. A primitive that -directly queries a simulation entity during planning will use its latest pose -when planning happens, but that query alone does not trigger scene-motion -replanning. +`SceneEntityPose`, or an object-centric primitive must explicitly declare the +`ObjectSemantics.entity_id` whose snapshot pose it consumes. `PickUp` and the +implicit-initial-pose path of coordinated pickup declare that dependency +automatically. The deprecated live-entity fallback does not trigger +scene-motion replanning. + +`ActionPlan.scene_dependency_monitor_until` may bound each dependency to an +exclusive command-frame index. `PickUp` stops monitoring its object after the +`approach` segment is dispatched so contact-, close-, and lift-induced movement +does not trigger a false replan. Joint-tracking and collision-world checks +remain active. Dynamic collision invalidation is provider-driven. Only registered, pose-updatable collision entities are supported; adding/removing obstacles or @@ -636,69 +788,172 @@ changing their geometry requires rebuilding the planner world. ## Planning success versus physical success -`ActionPlan.plan_success` only means a valid trajectory was produced for an +`ActionPlan.plan_success` only means a valid command plan was produced for an environment row. Pick, place, handover, and coordinated skills also return an uncommitted `StateDelta` describing the attachment state expected after execution. -At the terminal waypoint, an `ExecutionSession` requests an external -per-environment verification mask before committing a non-empty effect: +`TaskState.held_objects` uses one `HeldObjectState` per bound manipulator. +Multi-arm grasps use multiple entries that share the same `ObjectSemantics`; +there is no parallel coordinated-attachment representation to synchronize. +Consumers query per-environment active and exclusive-hold masks from that one +map. A single-arm transport, release, or handover row fails safely while a +second manipulator still holds the same semantic object or live entity. + +At the terminal waypoint, an `ExecutionSession` requests an external, +correlated per-environment result before committing a non-empty effect: ```python +import torch + +from embodichain.lab.sim.atomic_actions import EffectVerificationResult + tick = session.tick(latest_context) if tick.pending_effect is not None: - effect_success = verify_grasp_or_release() - tick = session.tick(latest_context, effect_success=effect_success) + request = tick.pending_effect + success_mask, failure_mask = verify_grasp_or_release(request.env_mask) + effect_result = EffectVerificationResult( + verification_id=request.verification_id, + success_mask=success_mask, + failure_mask=failure_mask, + invalidation_mask=failure_mask, + retry_mask=torch.zeros_like(failure_mask), + ) + tick = session.tick(latest_context, effect_result=effect_result) ``` -This prevents a collision-free plan or well-tracked trajectory from being +This prevents a collision-free or well-tracked command plan from being misreported as a successful grasp, release, or handover. The typed `EffectVerificationRequest` persists on subsequent ticks while waiting; -`EFFECT_VERIFICATION_REQUIRED` remains a one-time observability event. +`EFFECT_VERIFICATION_REQUIRED` remains a one-time observability event. Success +and failure masks are disjoint subsets of the request mask; omitted request rows +remain unresolved. Request IDs change after mask shrinkage or whole-action +retry, so a delayed result cannot commit a newer attempt. + +Every result also classifies failed rows with `invalidation_mask` and +`retry_mask`, both subsets of `failure_mask`. Invalidation applies the +request's core-owned removal-only `failure_invalidation`; the verifier cannot +inject replacement state. Retry is valid only when the same invocation's +physical preconditions remain satisfied. Failed rows outside `retry_mask` +enter external recovery, and unresolved evidence at the action deadline removes +covered active verified state before recovery. + +`request.deadline` is expressed in the robot-observation timestamp domain. +`RecoveryPolicy.action_timeout` covers both trajectory execution and the +terminal effect wait; a retry invalidates the old request ID. With +`ExecutionRunner.step()`, a call made before the next due cycle does not consume +its `effect_result`: schedule another call using `wait_duration`, re-read the +current request, and submit a result for that current ID. Partial resolution and +row deactivation can also replace the request before the delayed result arrives. + +The semantic layer provides a reusable verifier kernel for the curated +`Pick`, `Place`, and `HandOver` calls. A +{class}`~embodichain.lab.sim.skills.SemanticEffectSpec` binds the canonical +object and expected attach/detach relations to concrete runtime endpoints. Its +fresh per-call {class}`~embodichain.lab.sim.skills.EffectMonitor` consumes +backend-neutral {class}`~embodichain.lab.sim.skills.PoseRelationEvidenceBatch` +values and returns an uncorrelated +{class}`~embodichain.lab.sim.skills.EffectMonitorDecision`. The semantic runtime +must validate that decision, attach the *current* request ID, and pass the +result to the runner in the same due observation cycle. + +This split is deliberate: the evidence provider owns physical observation, +the monitor owns thresholds and hysteresis, and `ExecutionSession` remains the +only owner of deadlines, retries, partial-row commits, and verified +`TaskState`. A request-mask shrink keeps monitor history for remaining rows via +`attempt_generation`; a replacement plan or retry increments that generation +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 segment-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 curated `Pick`, `Place`, and `HandOver` paths additionally install blocking +positive-effect gates at named trajectory-segment entries. Pick must verify the +destination attachment before `lift`, Place must verify source detachment before +`retract`, and HandOver must verify the destination attachment before source +`release`. The compiler creates a monitor instance for each gate independently +from both the terminal monitor and negative held-object guard. + +`ExecutionSession` exposes a correlated `PhaseEffectGateRequest` at the segment +boundary. While its result remains unresolved, the waypoint cursor does not +advance and the preceding command is replayed for the complete synchronized +active cohort. This preserves gripper preload or open intent instead of +replacing it with an observed-position hold. A successful +`PhaseEffectGateResult` only unlocks the segment; it does not commit +`TaskState`. Contradiction uses the enclosing action's bounded retry policy, +request IDs are single-use, and the action deadline covers all polling. Calling +`run_until_blocked()` without a gate verifier returns this boundary for an +external verifier. + +The guards and gates are observational. Neither a monitor nor the runtime +creates a simulator attachment, freezes an object, or overrides its pose. +Workflow-level re-acquisition remains a separate recovery policy. ## Action Agent integration An MLLM should not construct `ActionInvocation` by copying arbitrary JSON into -runtime objects. An adapter should expose stable `SkillDescriptor` metadata and -agent-facing goal schemas, validate the semantic call, resolve object references -and embodiment capabilities, then produce the typed invocation: +runtime objects. The `embodichain.lab.sim.skills` package provides the semantic +boundary: stable call descriptors, immutable call values, scene/profile +manifests, a compiler, and a runtime facade. The agent selects among +the `SemanticCallCatalog` descriptors and supplies declarative object-centric +values; the compiler performs validation and grounding before the atomic engine +sees the request: ```text -MLLM SkillCallSpec - -> schema validation - -> object / scene grounding - -> capability and role binding - -> safe skill-option selection - -> semantic command selection (never raw qpos) - -> ActionInvocation - -> AtomicActionEngine - -> ActionPlan / execution events +MLLM / application SemanticCallSpec + -> SemanticCallCatalog discovery + -> SemanticIntegrationManifest validation + -> SemanticSkillCompiler.analyze() + object / affordance / resource / effect-flow validation + -> SemanticSkillCompiler.ground(latest_context) + participant binding + safe options + ActionInvocation + -> SkillRuntime / AtomicActionEngine + -> verified task state + structured execution events ``` -The adapter may expose a curated subset of `OptionsType`, but engine-only -profiles, `JointPositionCommand` payloads, planner instances, and concrete joint -groups should remain outside the MLLM schema. If the agent needs an -object-specific grasp mode, it should choose a semantic command or capability; -the grounding layer turns that choice into `ActionControlOverrides`. Invocation -IDs and monotonic revisions correlate updated agent decisions with planner -diagnostics and execution events, providing structured feedback for the next -decision without mutating an in-flight request implicitly. +Engine-only profiles, `JointPositionCommand` payloads, planner instances, live +objects, and concrete joint groups remain outside semantic call payloads. A +registered extension accepts only declarative data and requires an explicitly +installed version-matched lowerer. Invocation IDs and monotonic revisions +correlate compatible in-flight updates with planner diagnostics and execution +events without mutating a request implicitly. + +The semantic runtime is also useful without an agent. `start()` and `step()` +provide the non-blocking path, while `run()` is the synchronous convenience. +An analysis window may include future calls while `execution_prefix_length` +limits physical execution to a verified prefix. Call-local recovery remains +owned by `ExecutionRunner`; automatic task-level route replacement is not +provided by this layer. ## Extending the module A new primitive should: -1. define a frozen, action-owned goal dataclass with a stable `goal_kind`; +1. define a frozen, action-owned goal dataclass; 2. define a frozen `ActionOptions` subclass only for behavior that can vary per invocation; -3. declare `skill_id`, `GoalType`, `OptionsType`, required semantic roles, and - agent visibility; +3. declare `skill_id`, `GoalType`, `OptionsType`, an explicit + `SkillBindingContract`, and agent visibility; 4. put reusable embodiment commands on control-part profiles and generic motion/recovery choices in invocation policies; 5. implement side-effect-free `_plan(request, context)` using the engine-owned - planning services; do not override the framework-owned public `plan()`; -6. return full-robot timed motion, per-environment planning success, optional - named segment metadata, diagnostics, and uncommitted effects; + planning services; do not override the framework-owned public `plan()`—the + class definition is rejected if it does; +6. return a `TimedCommandSequence`, per-environment planning success, optional + joint-trajectory and named-segment metadata, diagnostics, and uncommitted + effects; joint planners can use `build_plan()`, while other endpoint types + use `build_command_plan()`; 7. add registration coverage, contract tests, execution/recovery tests, a runnable example, and documentation. @@ -707,6 +962,8 @@ See {doc}`builtin_actions` for the shipped skill catalog and visual demos, and ## Further reading +- {doc}`../scene_registry` — canonical scene identity, snapshots, and collision integration +- {doc}`robot_skill_profiles` — semantic calls, resource binding, and runtime presets - {doc}`../planners/motion_generator` — the motion generator owned by the engine - {doc}`../sim_robot` — robot control parts and kinematic configuration - {doc}`/tutorial/atomic_actions` — static, closed-loop, and recovery examples diff --git a/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md b/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md new file mode 100644 index 000000000..9e827d7ee --- /dev/null +++ b/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md @@ -0,0 +1,461 @@ +(robot-skill-profiles)= + +# Robot skill profiles + +```{currentmodule} embodichain.lab.sim.skills +``` + +A {class}`RobotSkillProfile` describes how robot-independent atomic-skill +requirements map onto one robot embodiment. Configure the robot's resources, +semantic commands, default choices, and policy presets once; task code can then +select skill-local participants instead of constructing an `ActionBinding` from +robot-specific control-part names. + +The model is deliberately generic. It does not define global `arm` and `tool` +fields. Each atomic skill publishes its own participant slots and endpoint +requirements, while a robot resource may expose any endpoints appropriate to +that embodiment: manipulation motion and grasping, a mobile base, a torso, or a +whole-body controller. + +## Contracts on the two sides + +An atomic action owns a +{class}`~embodichain.lab.sim.atomic_actions.SkillBindingContract`: + +- a {class}`~embodichain.lab.sim.atomic_actions.SkillResourceSlot` names each + skill-local participant, such as `primary`, `source`, or `destination`; +- a {class}`~embodichain.lab.sim.atomic_actions.SkillEndpointRequirement` + declares the all-of capabilities and typed semantic commands needed from that + participant; +- {class}`~embodichain.lab.sim.atomic_actions.DisjointSlotEndpoints` declares + endpoint views that must not share physical channels within one participant; + coupled whole-body views may overlap when the skill does not declare this + constraint; and +- {class}`~embodichain.lab.sim.atomic_actions.DisjointResourceSlots` requires + multi-participant skills to select physically disjoint resources. + +The robot side supplies {class}`RobotResource` values. A resource exposes named +{class}`ResourceEndpoint` values and may contain other resources through +`members`. Members form a directed acyclic graph and describe the physical +claim; endpoint capabilities are always explicit and are never inherited or +inferred from names. {class}`ControlPartEndpoint` is the built-in joint-backed +endpoint type, not the resource schema itself. + +```text +skill contract robot profile + +slot primary resource left_participant ++-- endpoint motion <--------------> +-- endpoint motion -> left_arm +`-- endpoint grasp <--------------> `-- endpoint grasp -> left_hand + capabilities + commands + members/physical claim +``` + +Binding the profile to an engine resolves each endpoint through a registered +{class}`ResourceEndpointAdapter` and validates physical claims, known +solver-backed kinematics capabilities, command types and dimensions, complete +defaults, policy presets, and installed skill contracts. The resulting +{class}`BoundRobotSkillProfile` exposes only installed, agent-visible skills +with at least one valid resource assignment. + +Endpoint and resource declarations are snapshotted when owned by a resource, +profile, or resolved binding. Custom endpoint types whose payloads cannot be +deep-copied must override {meth}`ResourceEndpoint.snapshot` and return a new +value of the same exact type. + +## Configure a manipulation participant + +The following profile groups two physical leaves into one participant. The +`motion` and `grasp` endpoint names come from the built-in manipulation +contracts; they are local protocol names, not global robot-resource categories. + +```python +import torch + +from embodichain.lab.sim.atomic_actions import ( + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + ControlPartCommandProfile, + HandOverOptions, + MotionPolicy, + PickUpOptions, + PlaceOptions, +) +from embodichain.lab.sim.skills import ( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + ControlPartEndpoint, + EffectMonitorRef, + ResourceBinding, + RobotResource, + RobotSkillProfile, + SkillPolicyPreset, +) + +left_motion_capabilities = frozenset( + { + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + BATCH_INVERSE_KINEMATICS_CAPABILITY, + } +) + +profile = RobotSkillProfile( + profile_id="example_robot", + resources={ + # Physical leaves own disjoint robot joints. + "left_arm_leaf": RobotResource( + resource_id="left_arm_leaf", + endpoints={"control": ControlPartEndpoint("left_arm")}, + ), + "left_hand_leaf": RobotResource( + resource_id="left_hand_leaf", + endpoints={"control": ControlPartEndpoint("left_hand")}, + ), + # A skill selects this participant as one indivisible resource. + "left_participant": RobotResource( + resource_id="left_participant", + endpoints={ + "motion": ControlPartEndpoint( + "left_arm", + capabilities=left_motion_capabilities, + ), + "grasp": ControlPartEndpoint( + "left_hand", + capabilities=frozenset({GRASP_CAPABILITY}), + ), + }, + members=("left_arm_leaf", "left_hand_leaf"), + ), + }, + command_profiles={ + "left_hand": ControlPartCommandProfile.joint_positions( + open=torch.tensor([0.04, 0.04]), + grasp=torch.tensor([0.0, 0.0]), + ), + }, + defaults={ + "pick_up": ResourceBinding( + resources={"primary": "left_participant"}, + ), + }, + presets={ + "default": SkillPolicyPreset( + preset_id="default", + action_option_templates={ + "pick": PickUpOptions(), + "place": PlaceOptions(), + "hand_over": HandOverOptions(), + }, + motion_policy=MotionPolicy(strategy="ik_interp"), + effect_monitors={ + semantic_id: EffectMonitorRef( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + { + "attached_translation_threshold": 0.02, + "detached_translation_threshold": 0.05, + "consecutive_samples": 2, + }, + ) + for semantic_id in ("pick", "place", "hand_over") + }, + ), + }, + default_preset="default", +) +``` + +During binding, each resolved endpoint also receives a logical +`task_state_key` and immutable, channel-keyed `effect_sources`. By default the +logical key is the selected resource ID, so the `motion` and `grasp` endpoints +of `left_participant` share one symbolic held-object state even though they use +different control parts. An effect source contains an `EffectEvidenceAddress`; +it is intentionally separate from the endpoint's command-only +`RuntimeEndpointTarget`. + +Every `ControlPartEndpoint.control_part` must be a key in +`robot.control_parts`. A composite endpoint may reuse a member's control part, +but all joints controlled directly by the composite must already be covered by +its members. Two physical leaf resources may not claim the same joint; model a +shared physical part once and reference that leaf from multiple composites. + +`command_profiles` are generic IDs selected by endpoint adapters; the built-in +control-part adapter defaults the ID to its `control_part`, and the engine +installs those profiles into the current action core automatically. +One-dimensional joint-position commands are broadcast across environments. +Their last dimension must equal the resolved endpoint's degree of freedom. Use +invocation-level command overrides for object- or environment-specific values. + +## Safe preset and dynamic collision worlds + +When the authoritative scene registry declares dynamic collision entities and +`safe` is reachable through the integration-wide, per-skill, or +profile-default preset selection, semantic integration validates that path +conservatively during binding. The `safe` preset must use `motion_gen`, and the +active motion generator must explicitly support dynamic collision worlds; +otherwise binding fails before provider observation, planning, or command +emission. + +A linked call receives an effective immutable preset snapshot with +`DynamicCollisionMode.REQUIRED`; the source profile preset is not mutated. +Other presets, and scenes without dynamic collision entities, retain their +configured collision mode. + +## Configure semantic action behavior with the preset + +`SkillPolicyPreset.action_option_templates` is the required, typed action- +behavior table for semantic calls that can select the preset. Each key is the +exact semantic call ID (`pick`, `place`, `hand_over`, or +`operate_articulation`), and each value must be the target action's exact frozen +`ActionOptions` dataclass. Static linking rejects a missing entry, an unknown +call ID, or an options value of the wrong exact type before simulation starts. + +The preset owns independent snapshots of each template. Pick and HandOver +grounding only replace their compiler-owned dynamic target fields; distances, +directions, waypoint counts, and other reusable behavior remain configuration. +A registered semantic lowerer may build a goal but cannot return replacement +options. This keeps task extensions from silently moving action parameters back +into Python code. + +Pick's `downstream_object_target_poses` and HandOver's +`middle_object_pose`/`final_object_pose` are reserved for the semantic compiler +and must remain empty in a template. Planner choice, sample count, tracking, +recovery, runner timing, and effect monitors stay in their dedicated preset +fields rather than `ActionOptions`. + +## Select semantic effect monitors with the preset + +A {class}`SkillPolicyPreset` owns one coherent runtime choice: planning and +recovery policy, runner cadence, and the exact semantic-effect monitors used to +confirm physical postconditions. `effect_monitors` maps a semantic call ID to a +versioned {class}`EffectMonitorRef`. Its parameters are bounded declarative +values; executable objects, tensors, cyclic containers, and non-finite numbers +are rejected. + +When `effect_monitors` is omitted, the preset selects the built-in +pose-relation hysteresis monitor for `pick`, `place`, and `hand_over`. Passing an +explicit empty mapping disables that default; static analysis then reports +`missing_effect_monitor` if a curated effectful call selects that preset. A +manifest also rejects monitor entries whose semantic ID is absent from its call +catalog, and the compiler requires the exact monitor ID/revision and validates +its parameters before grounding. + +The semantic compiler creates a fresh monitor for every grounded call. Pick +expects one attached destination relation, place one detached source relation, +and handover both source-detached and destination-attached relations in the +same observation. The monitor compares fresh backend evidence with owned +object-to-endpoint baselines; it never treats the planned `StateDelta` or +current `TaskState` as proof that the physical effect occurred. Invalid or +missing per-environment evidence remains unresolved. Consecutive-sample state +survives request-mask shrinkage within one attempt and resets when recovery +installs a new attempt. + +```{note} +The monitor contract is backend-neutral. Simulation, hardware perception, or +controller feedback supplies typed pose-relation evidence. The semantic +runtime adapter that connects that evidence to `ExecutionRunner` is separate +from the profile and monitor configuration. +``` + +## Bind, discover, and resolve + +Pass the profile to +{class}`~embodichain.lab.sim.atomic_actions.AtomicActionEngine`. The engine +installs its command profiles and binds it after loading built-in actions: + +```python +from embodichain.lab.sim.atomic_actions import AtomicActionEngine + +engine = AtomicActionEngine(motion_generator, skill_profile=profile) +bound = engine.skill_profile +assert bound is not None + +# This is the embodiment-filtered semantic catalog, not every installed action. +assert "pick_up" in bound.skills + +resolved = bound.resolve("pick_up") +assert resolved.resource_ids == {"primary": "left_participant"} +binding = resolved.action_binding +preset = bound.preset(skill_id="pick_up") +``` + +{meth}`BoundRobotSkillProfile.resolve` returns a {class}`ResolvedSkillBinding` +containing the selected logical resources, their adapter-resolved endpoints, +their combined {class}`ResourceClaim`, and an engine-owned generic +{class}`~embodichain.lab.sim.atomic_actions.ActionBinding`. A +semantic compiler uses that binding and the selected preset when constructing +an invocation; profile resolution does not plan or execute the action itself. + +If exactly one assignment is valid, resolution selects it. If several remain, +the caller must provide enough skill-local selections or the profile must define +a complete per-skill default: + +```python +left = bound.resolve("pick_up", selections={"primary": "left_participant"}) +candidates = bound.candidates("pick_up") +``` + +Incomplete defaults are rejected when the profile is bound. Without an +unambiguous choice, resolution raises {class}`AmbiguousSkillBindingError` rather +than selecting a resource by declaration order. An unsupported selection raises +{class}`UnsupportedSkillError` with endpoint, capability, command, or claim +rejection details. + +`engine.actions` remains the direct-core implementation registry. +`engine.skills` is the installed semantic catalog before embodiment filtering, +and `bound.skills` is the profile-supported catalog. Registering or replacing an +action invalidates the bound profile; bind it again before discovery or +resolution. + +## Extend the graph beyond manipulation + +Resource and capability identifiers are open strings. A joint-driven mobile +robot can model a base and a whole-body controller without changing the profile +schema: + +```python +base = RobotResource( + resource_id="base", + endpoints={ + "motion": ControlPartEndpoint( + "base", + capabilities=frozenset({"motion.planar_pose"}), + ) + }, +) +torso = RobotResource( + resource_id="torso", + endpoints={"motion": ControlPartEndpoint("torso")}, +) +whole_body = RobotResource( + resource_id="whole_body", + endpoints={ + "motion": ControlPartEndpoint( + "full_body", + capabilities=frozenset({"motion.whole_body"}), + ) + }, + members=("base", "torso", "left_arm_leaf", "right_arm_leaf"), +) +``` + +Here `base`, `torso`, and `full_body` must be real, non-empty robot control +parts, and the `full_body` joint set must be covered by the listed members. A +future locomotion or whole-body skill can require the corresponding endpoint +and capability in its own binding contract. Existing built-in actions do not +consume these example capabilities. + +Non-joint controllers add one endpoint declaration type and one adapter. The +adapter returns {class}`EndpointResolution` with a typed immutable +{class}`~embodichain.lab.sim.atomic_actions.RuntimeEndpointTarget`, an optional +command-profile key, joint IDs when applicable, and adapter-defined claim +tokens. The generic graph, matching, command, default, and conflict code does +not change. For example, a twist controller can return a target addressed to a +`base_velocity` transport and `claim_tokens={"controller:base"}` with no joint +IDs. Exclusive endpoints must provide joint IDs or claim tokens; a read-only or +otherwise shareable virtual endpoint must opt into `exclusive=False` +explicitly. + +Adapters are registered by exact endpoint type. The built-in +{class}`ControlPartEndpointAdapter` cannot be overridden; define a distinct +endpoint subtype and adapter when controller semantics differ. An adapter may +set `requires_command_profile=True` when a missing generic command-profile ID +must make profile binding fail immediately. + +The standard Expert Program simulation declaration accepts these endpoints +directly for robots that expose the normal full-state/qpos action base; a task +does not need a custom runtime factory solely to register the endpoint and Gym +transport: + +```python +profile = SimulationRobotSkillProfileBinding( + profile_id="mobile_v1", + resources=( + RobotResourceBinding( + resource_id="mobile_base", + endpoints={ + "motion": MobileVelocityEndpoint( + controller_id="base_controller", + capabilities=frozenset({"motion.base.velocity"}), + ) + }, + ), + ), +) + +adapter = create_simulation_expert_program_adapter( + env, + registration=SimulationExpertProgramRegistration( + scene_binding=scene_binding, + robot_profile_binding=profile, + endpoint_adapters=(MobileVelocityEndpointAdapter(),), + runtime_transports=(MobileVelocityGymEncoder(),), + ), +) +``` + +The adapter and encoder publish exact class-level declarations before a live +robot is created. The adapter declares its endpoint type, runtime target types, +transport IDs, and versioned tracking/evidence routes. The encoder declares its +transport ID plus exact target and payload types; each target and payload type +declares the same `TRANSPORT_ID`. Registration rejects missing, unused, +duplicate, or conflicting declarations, and runtime profile binding verifies +that `adapter.resolve()` returns only those declared routes. A stateful adapter, +transport, grounding provider, or safety factory must be a frozen dataclass whose +configuration is recursively immutable; mutable leaves such as lists, mappings, +sets, byte arrays, and tensors are rejected before registration. + +The standard factory currently accepts its built-in tracking feedback, +projector, evaluator, and effect-evidence routes only for +`ControlPartEndpoint`. In C1, every custom endpoint adapter must declare empty +route sets and therefore supports timed/open-loop execution only. Custom +closed-loop mobile or whole-body tracking/evidence needs a registration-owned +provider factory in C2; task code must not supply a live provider side channel. + +`RobotResourceBinding` snapshots arbitrary typed `ResourceEndpoint` values. +`ControlPartResourceBinding` remains the stricter joint-backed convenience and +continues to validate native control parts, joint IDs, and command-preset +widths. + +Endpoint registration is not a navigation or whole-body planner. Existing +built-in semantic skills do not consume the example base/whole-body +capabilities. A reusable capability must also install its semantic descriptor +and lowerer, atomic planner, command payload, safe-state transport behavior, and +effect integration as applicable. The current standard Gym encoder composes +custom transports over a full-qpos hold and the standard simulation factory +owns a `MotionGenerator`; a truly jointless or natively structured controller +therefore needs a reusable base-action composition/provider integration. This +does not require base- or whole-body-specific fields in the generic profile or +runtime core. + +Task vertical slices may declare a typed profile binding locally while the API +stabilizes. Repeated use should move that binding into an embodiment-owned +profile catalog so new tasks select it instead of redefining robot data. + +A resolved action binding is keyed only by the skill-local +`(slot_id, endpoint_id)` pair. A reusable non-joint capability supplies a +matching {class}`~embodichain.lab.sim.atomic_actions.RuntimeCommandPayload`, a +shared atomic skill that emits +{class}`~embodichain.lab.sim.atomic_actions.RuntimeCommandFrame` values, and an +{class}`~embodichain.lab.sim.atomic_actions.EndpointCommandTransport` registered +with {class}`~embodichain.lab.sim.atomic_actions.EndpointCommandRouter`. The +core binding, session, runner, and router do not need controller-specific +changes. Once that shared capability exists, new tasks and robot variants reuse +it through profile and task configuration rather than task-specific motion +code. + +```{important} +`ResourceClaim` combines leaf IDs, concrete joint IDs, and adapter claim tokens. +It and explicit disjoint constraints detect physical overlap for binding. A +claim alone does not enable or prove safe parallel action execution. The +separate explicit `ParallelSkillRuntime` can coordinate disjoint branch lanes, +but it merges command frames only through an authoritative +`ParallelCommandSafetyValidator`. Joint-backed plans may retain a full-robot +trajectory for feedback and offline compilation, while runtime dispatch remains +scoped to the endpoints in each command frame. +``` + +See {doc}`index` for the direct atomic-action core and +{doc}`../scene_registry` for canonical scene identity and snapshots. diff --git a/docs/source/overview/sim/index.rst b/docs/source/overview/sim/index.rst index 168fad8db..6127e4634 100644 --- a/docs/source/overview/sim/index.rst +++ b/docs/source/overview/sim/index.rst @@ -9,7 +9,8 @@ designed around a small set of composable components: a :class:`SimulationManager` owns the simulation lifecycle, asset classes represent objects in the scene, sensors produce batched observations, solvers convert between joint space and task space, planners generate feasible -trajectories, and atomic actions package common manipulation skills. +trajectories, atomic actions package common manipulation primitives, and +semantic skills bind robot-independent task intent to those primitives. Like EmbodiChain's environment and learning modules, the simulation framework is configuration driven. Scene elements are declared through config classes, spawned @@ -43,8 +44,14 @@ The simulation stack can be read from the bottom up: |-- planners | |-- joint-space and Cartesian trajectory generation | `-- time parameterization and sampling utilities - `-- atomic actions - `-- reusable manipulation primitives built from assets, solvers, and planners + |-- scene registry + | `-- canonical semantic identity, snapshots, and collision integration + |-- robot skill profiles + | `-- generic resource graphs, capabilities, commands, and policy presets + |-- atomic actions + | `-- reusable manipulation primitives built from assets, solvers, and planners + `-- semantic skills + `-- manifests, call catalogs, workflow compilation, and task execution The :class:`SimulationManager` is the entry point for most workflows. It creates the physics world, configures rendering and time stepping, lays out multiple @@ -84,10 +91,23 @@ Submodule Relationships timing, and feasibility handling. - Use robot state and solver results to produce trajectories that can be replayed in the manager loop. + * - Scene registry + - Owns canonical typed entity IDs, aliases, pose sources, geometry, + affordances, hierarchy, and collision roles. + - Publishes registry-derived snapshots for atomic actions and validates + dynamic collision-world agreement with planners. + * - Robot skill profiles + - Describe embodiment resources as a generic graph with explicit + endpoints, capabilities, semantic commands, defaults, and presets. + - Match skill-local participants to robot resources and lower validated + selections to the current atomic-action binding contract. * - Atomic actions - Package complete manipulation primitives such as move, pick, and place. - Compose semantic targets, solvers, planners, and robot control into reusable higher-level skills. + * - Semantic skills + - Declare object-centric Pick, Place, HandOver, and registered calls without robot-specific bindings. + - Validate scene/profile manifests, JIT-ground calls to atomic actions, and retain verified state across dynamic task segments. Typical Data Flow ----------------- @@ -106,6 +126,11 @@ planner calls. An action engine receives semantic targets or poses, resolves the motion primitive sequence, and returns a trajectory that can be replayed in the simulation. +For robot-independent task code, the semantic-skill layer validates typed scene +references and robot capabilities before lowering one call at a time to that +same action engine. It adds task segmentation and structured results without +adding a separate planner or controller implementation. + Choosing Where to Start ----------------------- @@ -120,6 +145,14 @@ Choosing Where to Start kinematics. - Use :doc:`planners/index` when a target pose or joint goal must become a time-ordered trajectory. +- Use :doc:`scene_registry` when semantic calls, snapshots, and planner + obstacles must share one authoritative entity namespace. +- Use :doc:`atomic_actions/robot_skill_profiles` when semantic skills should + resolve robot resources and policy presets from reusable embodiment + configuration. +- Use :doc:`atomic_actions/expert_programs` when a task should declare semantic + calls, settling, validation, or parallel barriers from JSON/YAML without + implementing task-local motion generation. - Use :doc:`atomic actions ` when building scripted manipulation from reusable motion primitives. @@ -145,4 +178,5 @@ See Also viser_visualization.md solvers/index planners/index + scene_registry.md atomic_actions/index diff --git a/docs/source/overview/sim/planners/curobo_planner.md b/docs/source/overview/sim/planners/curobo_planner.md index 915f7f098..82eaa16db 100644 --- a/docs/source/overview/sim/planners/curobo_planner.md +++ b/docs/source/overview/sim/planners/curobo_planner.md @@ -14,7 +14,14 @@ cuRobo, and constructing this planner requires a CUDA-capable NVIDIA GPU. cuRobo V2 is installed separately from EmbodiChain because public package indexes do not accept Git dependencies in published package metadata. Select -exactly one CUDA-matched source requirement: +exactly one source requirement that matches the CUDA runtime used by PyTorch: + +~~~bash +python -c "import torch; print(torch.version.cuda)" +~~~ + +Use `cu12` for a `12.x` result and `cu13` for a `13.x` result. Do not select the +extra from the maximum CUDA version displayed by `nvidia-smi`. ~~~bash # Recommended for the normal EmbodiChain environment, where PyTorch is present. @@ -31,8 +38,9 @@ pytest --pyargs curobo.tests These commands follow [NVIDIA's official cuRobo installation guide](https://nvlabs.github.io/curobo/latest/getting-started/installation.html) -and pin the source dependency to the cuRobo V2 `v0.8.0` release. Use a Python -3.10--3.13 environment on Linux with a supported NVIDIA GPU and driver. The +and pin the source dependency to the cuRobo V2 `v0.8.0` release. Although +cuRobo supports Python 3.10--3.13, use EmbodiChain's supported Python 3.10 or +3.11 environment on Linux with a supported NVIDIA GPU and driver. The non-`torch` variants are preferred for EmbodiChain because the simulation environment normally already provides PyTorch; the `-torch` variants delegate the PyTorch version requirement to cuRobo. Keep cuRobo in the same Python @@ -44,7 +52,7 @@ The cuRobo robot model and the per-control-part profile are both auto-generated internally - no external cuRobo robot YAML (e.g. `franka.yml`) and no `robot_profiles` config are needed. On the first plan, the adapter fits collision spheres to each link of the robot's URDF and writes a cuRobo V2 robot YAML (see -[Auto-generated robot YAML](curobo-auto-generated-robot-yaml). The tool frame, TCP +[Auto-generated robot YAML](#auto-generated-robot-yaml)). The tool frame, TCP offset, and base link are read from the control part's IK solver, and the simulator->cuRobo joint mapping is identity (the generated YAML reuses the URDF's own joint names). The control part is selected at plan time through @@ -59,6 +67,9 @@ locks both fingers at `0.04`, so use the same simulated finger state or include the fingers in the planned control part. A mismatch means cuRobo validates a different collision geometry from the one replayed in DexSim. +Assuming the scene has been registered as shown in +{doc}`../scene_registry`, construct the planner world from that catalog: + ~~~python from embodichain.lab.sim.planners import ( CuroboPlannerCfg, @@ -66,13 +77,27 @@ from embodichain.lab.sim.planners import ( MotionGenCfg, MotionGenerator, ) +from embodichain.lab.sim.skills import SceneCollisionWorldMode + +collision_mode = registry.resolve_collision_world_mode( + batch_size=robot.num_instances, +) planner_cfg = CuroboPlannerCfg( robot_uid="my_franka", planner_type="curobo", - world=CuroboWorldCfg(rigid_objects=[demo_block]), + world=CuroboWorldCfg( + rigid_objects=registry.collision_geometry_by_id(), + obstacle_representation="cuboid", + dynamic_obstacle_names=list(registry.dynamic_collision_entity_ids), + multi_env=collision_mode is SceneCollisionWorldMode.PER_ENV, + ), ) motion_generator = MotionGenerator(MotionGenCfg(planner_cfg=planner_cfg)) +scene_provider = registry.make_planning_scene_provider( + motion_generator, + batch_size=robot.num_instances, +) ~~~ cuRobo's Python logger defaults to error-only output. Set @@ -135,20 +160,55 @@ second one-time warmup and its graph-resident memory, but still no subprocess or second CUDA context. The collision world is always auto-generated from live `RigidObject` meshes via -`CuroboWorldCfg.rigid_objects`: the adapter reads each object's mesh -(`get_vertices` / `get_triangles`) and world pose (`get_local_pose`) and writes a -cached cuRobo scene YAML on the first plan, using +`CuroboWorldCfg.rigid_objects`. The canonical, registry-backed form is a mapping +from authoritative registry ID to live object; the adapter reads each object's +mesh (`get_vertices` / `get_triangles`) and world pose (`get_local_pose`) and +writes a cached cuRobo scene YAML on the first plan, using `CuroboWorldCfg.obstacle_representation` (`"sphere"` by default for fast collision queries; use `"cuboid"` for a local-frame AABB placed as an OBB via the object pose, or `"mesh"` for the exact triangle mesh). Generated poses are authored in the cuRobo base/world frame, so this is exact -when the robot base sits at the simulator world origin. For obstacles that move -or live in an offset base frame, also declare their names in +when the robot base sits at the simulator world origin. The mapping key, rather +than `RigidObject.uid`, is the canonical logical/source ID used by cache +identity and collision-world validation. For `"cuboid"` and `"mesh"`, that ID +is also used unchanged as the physical YAML obstacle name and runtime update +key. For obstacles that move or live in an offset base frame, also declare their +canonical IDs in `CuroboWorldCfg.dynamic_obstacle_names` and update poses at plan time through `CuroboPlanOptions.dynamic_obstacle_poses` (provision `CuroboWorldCfg.collision_cache` before planning). Dynamic updates require the `"cuboid"` or `"mesh"` representation because sphere fitting expands one object -into multiple independently named obstacles. +into physical YAML obstacles named `_0`, `_1`, and +so on; dynamic sphere configuration is rejected. These derived names are +backend details. The cache and registry/planner full-world contract continue to +use the unexpanded canonical source ID. + +Registry-backed mappings fail fast if a selected source has no mesh geometry +required by the chosen representation. This prevents a canonical collision ID +from being silently skipped during YAML generation. The advanced sequence form +retains its lower-level behavior independently of this registry contract. + +`CuroboPlanner.collision_world_entity_ids` reports every configured logical +source ID: each mapping key on the registry path, or each inferred name on the +advanced sequence path. It deliberately does not expose sphere-expanded +physical YAML names. `dynamic_collision_entity_ids` reports exactly the +configured dynamic subset. Static entries therefore participate in +construction-time identity validation even though they do not receive per-plan +pose updates. + +`CuroboWorldCfg` validates this planner-local registration at construction: +obstacle IDs must be unique, and every dynamic obstacle ID must match an entry +in `rigid_objects`. A sequence of objects is retained only as an advanced +direct-core path; it derives names from each `uid` or an `obstacle_` +fallback. Do not use that form for a registry-backed world. + +The {doc}`../scene_registry` integration performs two higher-level checks before +execution. First, all registry `STATIC ∪ DYNAMIC` IDs must exactly equal +`MotionGenerator.collision_world_entity_ids`. Second, registry, derived scene +provider, and planner dynamic-ID subsets must exactly agree. The planner must +also support pose updates and its shared/per-environment batch mode must agree +with the registry. Aliases are normalized at the registry boundary; cuRobo +never translates a canonical ID back to a simulator UID. ### Shared and per-environment collision worlds @@ -182,21 +242,25 @@ pose differs by environment must also: 3. Have its current `(B, 4, 4)` simulator-world poses passed through `CuroboPlanOptions.dynamic_obstacle_poses` when planning. -For example: +For a registry-backed world, derive both the geometry mapping and dynamic ID +list from the same catalog: ```python world_cfg = CuroboWorldCfg( - rigid_objects=[block], + rigid_objects=registry.collision_geometry_by_id(), obstacle_representation="cuboid", - dynamic_obstacle_names=["block"], + dynamic_obstacle_names=list(registry.dynamic_collision_entity_ids), multi_env=True, ) +current_snapshot = scene_provider.snapshot(timestamp=now, env_ids=env_ids) plan_options = CuroboPlanOptions( control_part="arm", - dynamic_obstacle_poses={ - "block": block.get_local_pose(to_matrix=True), # (B, 4, 4) - }, + dynamic_obstacle_poses=current_snapshot.collision_obstacle_poses( + batch_size=robot.num_instances, + device=robot.device, + dtype=robot.get_qpos().dtype, + ), ) ``` @@ -207,6 +271,12 @@ does not insert new geometry at runtime. Independent worlds replicate scene data and collision caches across the batch, so retain the shared default when the rebased layouts are identical. +For a registry-backed integration, a single-environment dynamic world may infer +the registry's shared mode. A multi-environment registry with dynamic collision +entities must explicitly choose shared or per-environment semantics, then set +`multi_env=False` or `True` to match. The registry validator rejects a mismatch +before planning. + (curobo-auto-generated-robot-yaml)= ## Auto-generated robot YAML @@ -271,7 +341,7 @@ assert result.success.all() Single-arm MoveEndEffector is supported through the normal `strategy="motion_gen"` route. MoveJoints can opt in to collision-aware joint-space planning with `strategy="motion_gen"`; the action uses the planner -already owned by its MotionGenerator. Movement phases of PickUp, Place, Press, +already owned by its MotionGenerator. Movement phases of PickUp, Place, and MoveHeldObject can use the same single-arm static-world route. This first release intentionally has the following limits: diff --git a/docs/source/overview/sim/planners/motion_generator.md b/docs/source/overview/sim/planners/motion_generator.md index 6cde5ed93..08f71b6fb 100644 --- a/docs/source/overview/sim/planners/motion_generator.md +++ b/docs/source/overview/sim/planners/motion_generator.md @@ -12,8 +12,10 @@ explicit cuRobo world. * **Unified planning interface**: Supports interpolation-oriented planners and collision-aware cuRobo V2 planning through one `generate()` API. * **Explicit strategy**: Accepts only `"motion_gen"` or `"ik_interp"`; no planner bypass is inferred from a missing backend-options object. -* **Normalized results**: Validates batched positions, success, derivatives and - timing, applies requested resampling, and holds failed rows at `start_qpos`. +* **Strict timed results**: A planner result with positions must include + per-waypoint `dt`; `duration` is derived from it. The generator validates that + contract, preserves total duration when resampling, and holds failed rows at + `start_qpos`. * **Flexible planner selection**: Supports TOPPRA, NeuralPlanner (experimental), and the optional CuroboPlanner backend, which plans on CUDA with either CPU or CUDA physics simulation. * **Automatic constraint handling**: Retrieves velocity and acceleration limits from the robot or uses user-specified/default values. * **Backend-aware target handling**: Generates discrete trajectories using joint or Cartesian interpolation where appropriate; cuRobo receives original Cartesian goals so it can perform collision-aware IK itself. @@ -29,7 +31,8 @@ through `supported_move_types` and exposes them through * convert EEF targets into joint waypoints only for joint-only backends such as TOPPRA when `MotionGenOptions.is_interpolate=True`; * fall back to deterministic joint interpolation when a backend cannot consume - a `JOINT_MOVE` target and explicit `start_qpos`/`sample_count` are available; + a `JOINT_MOVE` target and explicit `start_qpos`/`sample_count`/ + `interpolation_dt` are available; * reject unsupported target types before entering the backend. The built-in declarations are: @@ -139,6 +142,23 @@ result = motion_gen.generate( ) ``` +For deterministic interpolation, select the timing explicitly: + +```python +motion_opts = MotionGenOptions( + strategy="ik_interp", + sample_count=50, + interpolation_dt=0.02, + start_qpos=start_qpos, + control_part="arm", +) +``` + +Missing interpolation timing is an error; it is never inferred from an engine +or global default. Custom planners likewise must return `PlanResult.dt` with +shape `(B, N)` whenever they return positions; `duration` is exposed as the +derived value `dt.sum(dim=1)`. + #### Cartesian Space Planning ```python diff --git a/docs/source/overview/sim/planners/toppra_planner.md b/docs/source/overview/sim/planners/toppra_planner.md index 62ca60d7f..ca02433b6 100644 --- a/docs/source/overview/sim/planners/toppra_planner.md +++ b/docs/source/overview/sim/planners/toppra_planner.md @@ -1,6 +1,6 @@ # ToppraPlanner -`ToppraPlanner` is a trajectory planner based on the [TOPPRA](https://toppra.readthedocs.io/) (Time-Optimal Path Parameterization via Reachability Analysis) library. It generates time-optimal joint trajectories under velocity and acceleration constraints. +`ToppraPlanner` is a trajectory planner based on the [TOPPRA](https://github.com/hungpham2511/toppra) (Time-Optimal Path Parameterization via Reachability Analysis) library. It generates time-optimal joint trajectories under velocity and acceleration constraints. ## Features diff --git a/docs/source/overview/sim/scene_registry.md b/docs/source/overview/sim/scene_registry.md new file mode 100644 index 000000000..a2a97bf46 --- /dev/null +++ b/docs/source/overview/sim/scene_registry.md @@ -0,0 +1,339 @@ +(scene-registry)= + +# Scene registry + +```{currentmodule} embodichain.lab.sim.skills +``` + +`SceneRegistry` is the canonical integration boundary between semantic scene +identity, atomic-action snapshots, and planner collision worlds. Register an +entity once under an authoritative ID, resolve external names at that boundary, +then use only the canonical ID in semantic calls, snapshots, dependencies, and +dynamic-obstacle configuration. + +The registry is an immutable catalog. A {class}`RegistrySceneProvider` created +from it owns changing observation state, publication baselines, and revisions. +This separation lets multiple runtimes share one catalog without sharing their +revision counters. + +## What the registry owns + +Each {class}`SceneEntityRegistration` contains static integration metadata: + +- a typed canonical reference; +- aliases for simulator, perception, or hardware names; +- an explicit pose/confidence provider; +- optional parent and backend-local name; +- dynamics and planner collision role; +- optional geometry, semantic type, and affordance data. + +A `SceneSnapshot` contains only versioned dynamic pose/confidence values and +collision-world revisions. Snapshot construction copies every entity state, and +each public entity lookup returns a defensive copy. Mutating an original tensor +or a previously returned value therefore cannot change a published snapshot. + +References use one flat, globally unique namespace: + +```text +SceneEntityRef ++-- SceneObjectRef ++-- SceneArticulationRef ++-- SceneLinkRef +`-- SceneAffordanceRef +``` + +Do not encode hierarchy into link or affordance IDs. Store ancestry in +`SceneEntityRegistration.parent` and the backend-local member name in +`native_name`. A link parent must be an articulation; an affordance parent may +be an object, articulation, or link. The registry rejects duplicate canonical +IDs, ambiguous aliases, aliases that collide with another canonical ID, +unregistered parents, and typed-reference mismatches. Within one reference +type, a `(parent, native_name)` pair identifies one physical source and cannot +be registered under multiple canonical IDs. The same local name may still be +used under different parents or by different reference types. + +String lookups may use an alias and are normalized once: + +```python +cube = registry.resolve("sim_cube", expected_type=SceneObjectRef) +assert cube.entity_id == "cube" +``` + +An already typed reference is expected to contain a canonical ID. It cannot use +an alias or silently change entity kind. + +## Explicit simulation opt-in + +Use {meth}`SceneRegistry.from_simulation` to select simulator entities +explicitly. Mapping keys are authoritative registry IDs and values are existing +simulation UIDs. The UIDs are installed as legacy aliases; unlisted simulation +entities are not scanned or imported. + +```python +from embodichain.lab.sim.skills import SceneObjectRef, SceneRegistry + +registry = SceneRegistry.from_simulation( + sim, + rigid_objects={ + "cube": "sim_cube", + "tray": "task_tray_0", + }, + articulations={"drawer": "cabinet_articulation"}, +) + +cube = registry.resolve("cube", expected_type=SceneObjectRef) +assert registry.resolve("sim_cube", expected_type=SceneObjectRef) == cube +``` + +For perception or hardware, construct registrations with an implementation of +{class}`SceneEntityStateProvider` instead. Collision registrations also require +a {class}`SceneGeometryProvider`; the geometry belongs to the catalog even +though the snapshot contains only its current pose and confidence. + +## Semantic affordance capabilities + +An affordance is a registered direct child of an object, articulation, or link. +Its {class}`SceneAffordanceRef` has its own canonical ID, while `parent` and +`native_name` describe topology and the backend-local member. Semantic calls +select affordances by open, namespaced capabilities rather than by payload class +or declaration order. The built-in capabilities are: + +- {data}`GRASP_AFFORDANCE_CAPABILITY` (`affordance.grasp`); +- {data}`PLACE_ON_AFFORDANCE_CAPABILITY` (`affordance.place.on`); +- {data}`PLACE_IN_AFFORDANCE_CAPABILITY` (`affordance.place.in`). + +Register a capability-bearing grasp affordance and a scoped parent default as +follows. `antipodal_affordance` is an existing +`AntipodalAffordance` value, for example one produced by the grasp annotation +pipeline: + +```python +from dataclasses import replace + +import torch + +from embodichain.lab.sim.skills import ( + GRASP_AFFORDANCE_CAPABILITY, + SceneAffordanceRef, + SceneEntityRegistration, + SceneObjectRef, + SceneRegistry, +) + +object_ref = SceneObjectRef("workpiece") +grasp_ref = SceneAffordanceRef("workpiece.grasp.antipodal") + +simulation_registry = SceneRegistry.from_simulation( + sim, + rigid_objects={"workpiece": "cube"}, +) +object_registration = replace( + simulation_registry.lookup(object_ref), + semantic_type="cube", + default_affordances={GRASP_AFFORDANCE_CAPABILITY: grasp_ref}, +) +registry = SceneRegistry( + ( + object_registration, + SceneEntityRegistration( + ref=grasp_ref, + parent=object_ref, + native_name="antipodal_grasp", + affordance=antipodal_affordance, + affordance_capabilities=frozenset( + {GRASP_AFFORDANCE_CAPABILITY} + ), + affordance_revision="antipodal-v1", + relative_pose=torch.eye(4), + ), + ) +) +``` + +The registry enforces these rules: + +- capabilities belong only to an affordance registration; +- a capability-bearing affordance declares an explicit + `affordance_revision`; +- `affordance.grasp` requires an `AntipodalAffordance` payload; +- `default_affordances` belongs to the parent and maps each capability to one + compatible direct child; +- an affordance has either a live `state_provider` or a parent-relative + `relative_pose`, never neither. + +{meth}`SceneRegistry.affordances` lists compatible direct children in canonical +ID order without selecting one. {meth}`SceneRegistry.resolve_affordance` applies +one strict selection rule: + +1. validate and use the explicit affordance, when supplied; +2. otherwise use the only compatible child; +3. otherwise use the parent's capability-scoped default; +4. otherwise raise {class}`AmbiguousSceneAffordanceError`. + +No compatible child, a parent mismatch, or a capability mismatch raises +{class}`UnsupportedSceneAffordanceError`. There is no declaration-order +fallback. Once selected, {meth}`SceneRegistry.object_semantics` creates an owned +atomic-action `ObjectSemantics` value using the canonical object ID and a copied +affordance payload. + +{attr}`SceneRegistry.entity_metadata` projects provider-free +{class}`SceneEntityMetadata` values. {class}`SceneManifest` uses the same value +model, so semantic integration can validate IDs, aliases, topology, capability +sets, payload types and revisions, relative affordance poses, and collision mode +before observing the scene. Changing any of that metadata requires rebuilding +and rebinding the semantic integration. Relation grounders dispatch by the exact +capability, payload type, and revision tuple; revisions therefore form part of +the integration contract rather than runtime pose state. + +## Publish canonical snapshots + +For an atomic-action planning runtime, create the provider through +{meth}`SceneRegistry.make_planning_scene_provider` and pass it to +`SimulationExecutionAdapter`: + +```python +provider = registry.make_planning_scene_provider( + motion_generator, + batch_size=robot.num_instances, +) +adapter = SimulationExecutionAdapter( + sim, + robot, + scene_provider=provider, +) +``` + +This factory constructs a fresh provider and eagerly validates the complete +registry/provider/planner collision contract. Use +{meth}`SceneRegistry.make_scene_provider` only for perception and advanced +direct-core consumers that do not need planner agreement. Every factory call +returns an independent provider. Its snapshots contain canonical registry IDs +only; aliases never leak into `SceneSnapshot.entities` or +`collision_entity_ids`. + +The provider observes entities in the supplied `env_ids` order. Those IDs must +remain stable and ordered for the provider lifetime, and timestamps must be +monotonic. Translation and rotation thresholds are measured from the last +materially published pose per entity and environment, so repeated +sub-threshold motion eventually publishes a new scene version. Dynamic +collision entities additionally advance per-environment collision revisions. + +Parent-relative affordances are derived from the parent pose inside the same +observation. Their static relative transforms remain registry metadata. + +## Validate the complete collision world + +Collision setup has one canonical namespace. For a registry-backed +cuRobo world, derive both the explicit `registry_id -> RigidObject` mapping and +the dynamic-obstacle ID list from the registry: + +```python +from embodichain.lab.sim.planners import ( + CuroboPlannerCfg, + CuroboWorldCfg, + MotionGenCfg, + MotionGenerator, +) +from embodichain.lab.sim.skills import ( + SceneCollisionRole, + SceneCollisionWorldMode, + SceneRegistry, +) + +registry = SceneRegistry.from_simulation( + sim, + rigid_objects={"cube": "sim_cube"}, + collision_roles={"cube": SceneCollisionRole.DYNAMIC}, + collision_world_mode=SceneCollisionWorldMode.PER_ENV, +) + +world = CuroboWorldCfg( + rigid_objects=registry.collision_geometry_by_id(), + obstacle_representation="cuboid", + dynamic_obstacle_names=list(registry.dynamic_collision_entity_ids), + multi_env=True, +) +motion_generator = MotionGenerator( + MotionGenCfg( + planner_cfg=CuroboPlannerCfg( + robot_uid=robot.uid, + planner_type="curobo", + world=world, + ) + ) +) + +provider = registry.make_planning_scene_provider( + motion_generator, + batch_size=robot.num_instances, +) +``` + +`collision_geometry_by_id()` derives the cuRobo mapping from the catalog. By +default it includes only `STATIC` and `DYNAMIC` registrations and excludes +`NONE`; an optional exact role filter is available when a backend needs one +subset. `from_simulation()` automatically exposes a selected live rigid object +as its geometry source. Articulations and manually constructed collision +registrations still need an appropriate explicit geometry provider. + +The registry validator checks two nested identity contracts before execution: + +1. The registry's complete `STATIC ∪ DYNAMIC` collision ID set exactly equals + `MotionGenerator.collision_world_entity_ids`. This rejects a missing static + obstacle as well as planner geometry not owned by the registry. +2. The registry's `DYNAMIC` subset exactly equals both the provider's + `collision_entity_ids` and + `MotionGenerator.dynamic_collision_entity_ids`. +3. Every ID in that complete collision world has materialized registered + geometry. +4. The planner supports dynamic collision-world updates when that subset is + non-empty. +5. The planner's shared/per-environment mode equals the registry mode. + +Every collision registration has already proved that geometry exists. Planner +IDs are canonical logical/source IDs, not aliases. For cuRobo `cuboid` and +`mesh` worlds, each mapping key is also the physical YAML obstacle key and the +runtime pose-update key. A `sphere` world instead expands one canonical source +ID into physical YAML names such as `cube_0`, `cube_1`, and so on. Those derived +names are backend details: cache identity and the full-world contract remain +keyed by the canonical source ID, and dynamic sphere obstacles are rejected. + +A registry-backed mapping also fails fast when a selected collision source has +no mesh geometry required by its representation. It never silently omits that +canonical ID from generated planner geometry. + +When an external perception or hardware provider supplies snapshots, validate +that provider's dynamic subset explicitly instead of constructing a +registry-derived one. The complete registry/planner world check still applies: + +```python +registry.validate_collision_integration( + motion_generator, + batch_size=batch_size, + scene_provider=external_scene_provider, +) +``` + +{class}`SceneCollisionWorldMode` follows this rule: + +| Batch and collision setup | Required registry choice | cuRobo setting | +|---|---|---| +| No dynamic collision entities | No mode required | Planner-specific | +| One environment | Omitted mode resolves to `SHARED`; explicit mode also allowed | Match the effective mode | +| Multiple environments | Explicit `SHARED` or `PER_ENV` is required | `multi_env=False` or `True`, respectively | + +Choose `SHARED` only when obstacle poses are equal after rebasing every +environment into its robot-base frame. Choose `PER_ENV` for independently +randomized robot-relative layouts. + +## Advanced direct-core paths + +`RigidObjectSceneProvider` and a list-valued `CuroboWorldCfg.rigid_objects` +remain available to advanced callers that intentionally assemble the atomic +core by hand. The list form derives obstacle names from each object's `uid` (or +an `obstacle_` fallback). It is not the registry-backed path and does not +provide alias normalization or registry/provider/planner construction checks. + +See {doc}`atomic_actions/robot_skill_profiles` for manifest and semantic-call +integration, {doc}`atomic_actions/index` for snapshot grounding and recovery semantics, and +{doc}`planners/curobo_planner` for cuRobo world representation and frame details. diff --git a/docs/source/overview/sim/sim_assets.md b/docs/source/overview/sim/sim_assets.md index a3b6669ad..30ab3e0b4 100644 --- a/docs/source/overview/sim/sim_assets.md +++ b/docs/source/overview/sim/sim_assets.md @@ -124,7 +124,7 @@ The {class}`~cfg.RigidBodyAttributesCfg` class defines physical properties for r | `dynamic_friction` | `float` | `0.5` | Dynamic friction coefficient. | | `static_friction` | `float` | `0.5` | Static friction coefficient. | -For Rigid Object tutorial, please refer to the [Create Scene](https://dexforce.github.io/EmbodiChain/tutorial/create_scene.html) tutorial. +For a runnable rigid-object example, see the {doc}`Create Scene ` tutorial. ## Rigid Object Groups diff --git a/docs/source/overview/sim/sim_cloth.md b/docs/source/overview/sim/sim_cloth.md index 11b657f3b..dfc19caad 100644 --- a/docs/source/overview/sim/sim_cloth.md +++ b/docs/source/overview/sim/sim_cloth.md @@ -48,7 +48,7 @@ Cloth bodies require both voxelization and physical attributes. | `min_position_iters` | `int` | `4` | Minimum solver iterations for position correction. | | `min_velocity_iters` | `int` | `1` | Minimum solver iterations for velocity updates. | -For Cloth Object tutorial, please refer to the [Cloth Body Simulation](https://dexforce.github.io/EmbodiChain/tutorial/create_cloth.html). +For a runnable example, see the {doc}`Cloth Body Simulation ` tutorial. ### Setup & Initialization @@ -135,9 +135,9 @@ For cloth objects, the state is represented by the positions and velocities of i | Method | Return Shape | Description | | :--- | :--- | :--- | -| `get_current_vertex_position()` | `(n_envs, n_vert, 3)` | Current positions of mesh vertices. | -| `get_current_vertex_velocity()` | `(n_envs, n_vert, 3)` | Current positions of mesh vertices. | -| `get_rest_vertex_position()` | `(n_envs, n_vert, 3` | Rest (initial) positions of collision vertices. | +| `get_current_vertex_position()` | `(num_envs, n_vert, 3)` | Current positions of mesh vertices. | +| `get_current_vertex_velocity()` | `(num_envs, n_vert, 3)` | Current positions of mesh vertices. | +| `get_rest_vertex_position()` | `(num_envs, n_vert, 3` | Rest (initial) positions of collision vertices. | > Note: N is the number of environments/instances, V_col is the number of collision vertices, and V_sim is the number of simulation vertices. diff --git a/docs/source/overview/sim/sim_manager.md b/docs/source/overview/sim/sim_manager.md index 21b555040..46755a22c 100644 --- a/docs/source/overview/sim/sim_manager.md +++ b/docs/source/overview/sim/sim_manager.md @@ -289,10 +289,10 @@ In this mode, the physics simulation stepping is automatically handling by the p > Currently, multiple instances are not supported for ray tracing rendering backend. Good news is that we are working on adding this feature in future releases. -For more methods and details, refer to the [SimulationManager](https://dexforce.github.io/EmbodiChain/api_reference/embodichain/embodichain.lab.sim.html#embodichain.lab.sim.SimulationManager) documentation. +For more methods and details, see the {doc}`SimulationManager API `. -### Related Tutorials +### Related Documentation -- [Basic scene creation](https://dexforce.github.io/EmbodiChain/tutorial/create_scene.html) -- [Interactive simulation with Gizmo](https://dexforce.github.io/EmbodiChain/tutorial/gizmo.html) +- {doc}`Basic scene creation ` +- {doc}`Interactive Gizmos ` - {doc}`Viser browser visualization ` diff --git a/docs/source/overview/sim/sim_rigid_object.md b/docs/source/overview/sim/sim_rigid_object.md index acda096ea..b80f267ba 100644 --- a/docs/source/overview/sim/sim_rigid_object.md +++ b/docs/source/overview/sim/sim_rigid_object.md @@ -224,7 +224,7 @@ Rigid objects integrate with sensors (cameras, contact sensors) and gizmos. You ## Related Topics -- Soft bodies: see the Soft Object documentation for deformable object interfaces. ([Soft Body Simulation Tutorial](https://dexforce.github.io/EmbodiChain/tutorial/create_softbody.html)) +- Soft bodies: see the {doc}`Soft Body Simulation Tutorial ` for deformable object interfaces. - Examples: check `scripts/tutorials/sim/create_scene.py` and `examples/sim` for more usage patterns. diff --git a/docs/source/overview/sim/sim_rigid_object_group.md b/docs/source/overview/sim/sim_rigid_object_group.md index d6d228387..e6d61f82f 100644 --- a/docs/source/overview/sim/sim_rigid_object_group.md +++ b/docs/source/overview/sim/sim_rigid_object_group.md @@ -142,8 +142,8 @@ Members in a group behave like normal `RigidObject`s: they can be observed by ca ## Related Topics - Rigid objects: See the Rigid Object overview for single-body. -- Soft bodies: Deformable objects have different observation semantics (vertex-level data). ([Soft Body Simulation Tutorial](https://dexforce.github.io/EmbodiChain/tutorial/create_softbody.html)) +- Soft bodies: Deformable objects have different observation semantics (vertex-level data); see the {doc}`Soft Body Simulation Tutorial `. - Examples: `scripts/tutorials/sim/create_rigid_object_group.py` - \ No newline at end of file + diff --git a/docs/source/overview/sim/sim_soft_object.md b/docs/source/overview/sim/sim_soft_object.md index 88b14642a..5936321d4 100644 --- a/docs/source/overview/sim/sim_soft_object.md +++ b/docs/source/overview/sim/sim_soft_object.md @@ -43,7 +43,7 @@ Soft bodies require both voxelization and physical attributes. | `mass` | `float` | `-1.0` | Total mass. If negative, density is used. | | `density` | `float` | `1000.0` | Material density in kg/m^3. | -For Soft Object tutorial, please refer to the [Soft Body Simulation](https://dexforce.github.io/EmbodiChain/tutorial/create_softbody.html). +For a runnable example, see the {doc}`Soft Body Simulation ` tutorial. ### Setup & Initialization diff --git a/docs/source/overview/sim/solvers/index.rst b/docs/source/overview/sim/solvers/index.rst index 322195af9..8dfee2630 100644 --- a/docs/source/overview/sim/solvers/index.rst +++ b/docs/source/overview/sim/solvers/index.rst @@ -67,7 +67,7 @@ space to end-effector motion. Multi-chain and closed-loop kinematics -------------------------------------- +-------------------------------------- Solvers can handle serial chains, branched kinematic trees and some closed-loop mechanisms. Closed-loop systems commonly require constraint solvers and may diff --git a/docs/source/overview/sim/solvers/pink_solver.md b/docs/source/overview/sim/solvers/pink_solver.md index c7de6d612..326d67a85 100644 --- a/docs/source/overview/sim/solvers/pink_solver.md +++ b/docs/source/overview/sim/solvers/pink_solver.md @@ -100,4 +100,4 @@ solver = PinkSolver(cfg) - [Pinocchio Library](https://github.com/stack-of-tasks/pinocchio) - [Pink Library](https://github.com/stephane-caron/pink) -- [Null Space Posture Task](https://github.com/stephane-caron/pink#null-space-posture-task) +- [Pink documentation](https://stephane-caron.github.io/pink/) diff --git a/docs/source/overview/sim/viser_visualization.md b/docs/source/overview/sim/viser_visualization.md index fe3d56ad9..efefaea50 100644 --- a/docs/source/overview/sim/viser_visualization.md +++ b/docs/source/overview/sim/viser_visualization.md @@ -163,17 +163,13 @@ included in the scene manifest and its authoritative values in each scene frame. Browser callbacks enqueue immutable scalar commands; the preview loop validates their run and scene revision before writing articulation state. -The **Articulation joints** panel uses degree sliders for bounded revolute -joints, meter sliders for bounded prismatic joints, and numeric inputs when one -or both limits are absent. Mimic joints are omitted. The controller writes both -current and target positions and clears velocity and effort before each step, -which makes the preview independent of drive configuration. Use -`--no-joint-control` to disable it. - This controller is currently specific to the Viser asset-preview path. The protocol and backend command sink are kept separate from the controller so a native DexSim GUI can reuse the simulation-side behavior later. +See {doc}`Previewing Assets ` for the complete command +workflow, panel behavior, and option reference. + ## Deformable objects Cloth and soft bodies require dynamic vertex updates and are intentionally @@ -319,5 +315,7 @@ Viser port behind an authenticated gateway. - {doc}`sim_manager` - {doc}`sim_assets` - {doc}`sim_sensor` +- {doc}`/features/interaction/gizmo` +- {doc}`/guides/preview_asset` - {doc}`/tutorial/create_scene` - {doc}`/tutorial/sensor` diff --git a/docs/source/quick_start/docs.md b/docs/source/quick_start/docs.md index 3cfb19a23..0eccebbdd 100644 --- a/docs/source/quick_start/docs.md +++ b/docs/source/quick_start/docs.md @@ -2,11 +2,23 @@ ## 1. Install the documentation dependencies +Build the docs from a source checkout in a Python 3.10 or 3.11 virtual +environment. API generation imports EmbodiChain modules, so install the project +runtime and the documentation toolchain from the repository root: + ```bash +pip install -e ".[gensim]" \ + --extra-index-url http://pyp.open3dv.site:2345/simple/ \ + --trusted-host pyp.open3dv.site \ + --extra-index-url https://download.blender.org/pypi/ pip install -r docs/requirements.txt ``` -> If you have issue like `locale.Error: unsupported locale setting`, please enter `export LC_ALL=C.UTF-8; export LANG=C.UTF-8` before build the API. +The documentation requirements are pinned so local and CI builds use the same +Sphinx toolchain. + +> If the build raises `locale.Error: unsupported locale setting`, run +> `export LC_ALL=C.UTF-8; export LANG=C.UTF-8` before rebuilding. ## 2. Build the HTML site @@ -17,7 +29,8 @@ cd docs make current-docs ``` -Then you can preview the documentation in your browser at `docs/build/html/index.html`. +This target treats warnings as errors. Preview the result at +`docs/build/html/index.html`. ### Multi-version docs (CI/production) diff --git a/docs/source/quick_start/install.md b/docs/source/quick_start/install.md index 987937695..a59224bc1 100644 --- a/docs/source/quick_start/install.md +++ b/docs/source/quick_start/install.md @@ -18,12 +18,18 @@ After installation, continue with the [Quick Start Tutorial](../tutorial/index.r |-----------|-------------| | **OS** | Linux x86_64 (Ubuntu 20.04+ recommended) | | **GPU** | NVIDIA GPU with compute capability 7.0+ | -| **NVIDIA driver** | ≥ 535 (tested on driver branches up to 580.x) | +| **NVIDIA driver** | ≥ 535 (tested on driver branches up to 595.x) | | **CUDA** | 12.x (aligned with the Docker image and `dexsim_engine` wheels) | | **Vulkan** | Host ICD/layer files for GPU rendering (see Docker notes) | | **Python** | 3.10 or 3.11 | | **Display** (optional) | X11 `DISPLAY` for interactive viewer windows | +NVIDIA drivers are backward compatible with applications built against older +CUDA toolkits. A 595-series host driver therefore works with the current CUDA +12.8 Docker image and wheels; installing a CUDA 13 toolkit on the host is not +required. See NVIDIA's [CUDA compatibility documentation](https://docs.nvidia.com/deploy/cuda-compatibility/latest/index.html) +for details. + > [!NOTE] > **PyTorch:** EmbodiChain depends on PyTorch transitively (for example via `dexsim_engine` and `pytorch_kinematics`). If you install or upgrade PyTorch separately, match the wheel to your CUDA version using the [official PyTorch install selector](https://pytorch.org/get-started/locally/). @@ -179,7 +185,15 @@ Install cuRobo separately to use EmbodiChain's CUDA-accelerated, collision-aware motion planner. cuRobo is intentionally not part of the core dependency set, and its Git source requirement cannot be included in metadata published to PyPI. Select exactly one command that matches the CUDA version -reported by `nvidia-smi`. +used by PyTorch in the active environment: + +```bash +python -c "import torch; print(torch.version.cuda)" +``` + +`nvidia-smi` reports the newest CUDA version supported by the installed driver, +which can be newer than the CUDA runtime used by PyTorch and is therefore not +the value to use when choosing the cuRobo extra. The normal EmbodiChain environment already provides PyTorch, so prefer one of the non-`torch` variants: @@ -187,13 +201,11 @@ the non-`torch` variants: ```bash # CUDA 12.x uv pip install \ - "nvidia-curobo[cu12] @ git+https://github.com/NVlabs/curobo.git@v0.8.0" \ - ${PIP_EXTRA_ARGS} + "nvidia-curobo[cu12] @ git+https://github.com/NVlabs/curobo.git@v0.8.0" # CUDA 13.x uv pip install \ - "nvidia-curobo[cu13] @ git+https://github.com/NVlabs/curobo.git@v0.8.0" \ - ${PIP_EXTRA_ARGS} + "nvidia-curobo[cu13] @ git+https://github.com/NVlabs/curobo.git@v0.8.0" ``` For a fresh environment that also needs cuRobo to select and install PyTorch, @@ -204,9 +216,7 @@ requirements work with `pip`; replace `uv pip install` with `pip install`. ```bash uv pip install \ - "nvidia-curobo[cu12] @ git+https://github.com/NVlabs/curobo.git@v0.8.0" \ - --extra-index-url http://pyp.open3dv.site:2345/simple/ \ - --trusted-host pyp.open3dv.site + "nvidia-curobo[cu12] @ git+https://github.com/NVlabs/curobo.git@v0.8.0" python -c "import curobo; print(curobo.__version__)" pytest --pyargs curobo.tests diff --git a/docs/source/resources/robot/index.rst b/docs/source/resources/robot/index.rst index f5acf2e7c..b3f68e2a2 100644 --- a/docs/source/resources/robot/index.rst +++ b/docs/source/resources/robot/index.rst @@ -1,13 +1,42 @@ Supported Robots -====================== +================ -*To be completed by adding more well-defined robot like LeRobot, Unitree H1/G1, etc.* +EmbodiChain provides configuration classes for the robot families and composed +layouts below. Each page documents construction, control parts, kinematics, and +the currently supported variants. + +.. list-table:: + :header-rows: 1 + :widths: 24 25 51 + + * - Robot + - Configuration class + - Current coverage + * - :doc:`Franka Panda ` + - ``FrankaPandaCfg`` + - Panda arm and gripper with numerical FK/IK. + * - :doc:`UR family ` + - ``URRobotCfg`` + - UR3, UR3e, UR5, UR5e, UR10, and UR10e with analytic IK. + * - :doc:`Dexforce W1 ` + - ``DexforceW1Cfg`` + - Dual 7-DOF arms, versioned assets, and configurable hands/grippers. + * - :doc:`CobotMagic ` + - ``CobotMagicCfg`` + - Fixed-base dual arms and grippers for bimanual tasks. + * - :doc:`Dual-arm composition ` + - ``DualArmRobotCfg`` + - Registry-based composition of compatible single-arm configs in multiple layouts. .. toctree:: :maxdepth: 1 + :hidden: + + franka_panda + ur_robot + dexforce_w1 + cobotmagic + dual_arm - Franka Panda - UR Family - Dexforce W1 - CobotMagic - Dual-Arm Composition +To add another model, follow :doc:`/guides/add_robot` and include its page in +this catalog and toctree. diff --git a/docs/source/resources/robot/ur_robot.md b/docs/source/resources/robot/ur_robot.md index 6b802f7bf..4056dcc3e 100644 --- a/docs/source/resources/robot/ur_robot.md +++ b/docs/source/resources/robot/ur_robot.md @@ -89,5 +89,4 @@ robot = sim.add_robot(cfg=cfg) ## See Also - :doc:`/guides/add_robot` - Adding a new robot (quick reference) -- :doc:`/tutorial/add_robot` - Adding a new robot (full tutorial) - :doc:`/overview/sim/solvers/index` - IK solver reference diff --git a/docs/source/resources/task/index.rst b/docs/source/resources/task/index.rst index 1c65e7e17..9a02a4dda 100644 --- a/docs/source/resources/task/index.rst +++ b/docs/source/resources/task/index.rst @@ -1,10 +1,80 @@ Supported Tasks -====================== +=============== -*To be completed by adding more tasks and task descriptions.* +The official task environments are bundled in the ``embodichain`` wheel under +the ``embodichain_tasks`` import package. Installing EmbodiChain registers the +environment IDs below automatically; no second package installation is needed. -.. toctree:: - :maxdepth: 1 +Run a task by passing one of its gym configuration files to the unified CLI: - Pour Water +.. code-block:: bash + embodichain run-env \ + --gym_config embodichain_tasks/configs/gym/pour_water/gym_config.json + +Use ``--preview`` to inspect a configured environment without starting a data +generation run. See :doc:`/guides/run_env` for all launch options and +:doc:`/tutorial/data_generation` for the dataset workflow. + +Environment catalog +------------------- + +.. list-table:: + :header-rows: 1 + :widths: 18 28 54 + + * - Category + - Environment ID + - Example gym config + * - Reinforcement learning + - ``CartPoleRL`` + - ``embodichain_tasks/configs/agents/rl/basic/cart_pole/gym_config.yaml`` + * - Reinforcement learning + - ``PushCubeRL`` + - ``embodichain_tasks/configs/agents/rl/push_cube/gym_config.json`` + * - Multi-segment + - ``MultiSegmentsCubePickPlace-v1`` + - ``embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json`` + * - Special + - ``SimpleTask-v1`` + - ``embodichain_tasks/configs/gym/special/simple_task_ur10.json`` + * - Special + - ``StayStillSave-v1`` + - ``embodichain_tasks/configs/gym/special/stay_still_save_ur10.json`` + * - Tableware + - ``BlocksRankingRGB-v1`` + - ``embodichain_tasks/configs/gym/blocks_ranking_rgb/cobot_magic_3cam.json`` + * - Tableware + - ``BlocksRankingSize-v1`` + - ``embodichain_tasks/configs/gym/blocks_ranking_size/cobot_magic_3cam.json`` + * - Tableware + - ``MatchObjectContainer-v1`` + - ``embodichain_tasks/configs/gym/match_object_container/cobot_magic_3cam.json`` + * - Tableware + - ``OpenDrawer-v1`` + - ``embodichain_tasks/configs/gym/open_drawer/cobot_magic_3cam.json`` + * - Tableware + - ``PlaceObjectDrawer-v1`` + - ``embodichain_tasks/configs/gym/place_object_drawer/cobot_magic_3cam.json`` + * - Tableware + - ``PourWater-v3`` + - ``embodichain_tasks/configs/gym/pour_water/gym_config.json`` + * - Tableware + - ``ScoopIce-v1`` + - ``embodichain_tasks/configs/gym/scoop_ice/gym_config.json`` + * - Tableware + - ``StackBlocksTwo-v1`` + - ``embodichain_tasks/configs/gym/stack_blocks_two/cobot_magic_3cam.json`` + * - Tableware + - ``StackCups-v1`` + - ``embodichain_tasks/configs/gym/stack_cups/cobot_magic_3cam.json`` + * - Agent variant + - ``PourWaterAgent-v3`` + - Uses the Pour Water scene together with + ``embodichain_tasks/configs/gym/agent/pour_water_agent/``. + * - Agent variant + - ``Rearrangement-v3`` / ``RearrangementAgent-v3`` + - Registered task variants; no standalone gym config is currently shipped. + +The value of ``id`` inside a gym config must match a registered environment ID. +When adding a task, update this catalog together with its runnable config. diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index f9d57c009..e2902608c 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -11,13 +11,16 @@ combines that snapshot with the latest For the complete architecture and ownership model, see :doc:`/overview/sim/atomic_actions/index`. For the capability matrix and visual demonstrations of every built-in skill, see -:doc:`/overview/sim/atomic_actions/builtin_actions`. +:doc:`/overview/sim/atomic_actions/builtin_actions`. Canonical scene identity and +snapshot/provider setup are documented in :doc:`/overview/sim/scene_registry`. -The contracts deliberately separate six concerns: +The contracts deliberately separate seven concerns: * a **goal** describes what should happen; -* an **ActionBinding** maps semantic roles such as ``primary`` or ``source`` to - names declared in the engine robot's ``control_parts`` mapping; +* a **SkillBindingContract** declares action-local participant slots, endpoint + capabilities, typed commands, and physical disjointness; +* an engine-owned **ActionBinding** contains adapter-resolved + **EndpointBinding** snapshots and immutable runtime targets for one call; * a **ControlPartCommandProfile** maps embodiment-specific meanings such as ``open``, ``grasp``, or ``ready`` to typed commands; * typed **ActionOptions** contain behavior that may vary for one skill call; @@ -26,19 +29,32 @@ The contracts deliberately separate six concerns: * a **PlanningContext** contains measured robot state, verified task state, and a versioned scene snapshot. -Binding values are keys from ``RobotCfg.control_parts``. They are not joint, -link, TCP-frame, or scene-object names. The engine validates them and resolves -their full-robot joint indices before planning. The ``end_effectors`` map names -an actuated hand/tool control part rather than an IK end frame. +Slots such as ``primary`` or ``source`` name participants only within one skill. +Each slot exposes skill-local endpoint protocols such as ``motion`` and +``grasp``. There are no global arm, hand, mobile-base, or whole-body binding +fields. A profile matches endpoint capabilities to generic robot resources and +uses an endpoint adapter to create the runtime target. -A role is an action-defined semantic participant slot, not a control part. In -``{"primary": "left_arm"}``, ``primary`` means the principal participant of -that single-participant action, while ``left_arm`` is the concrete control-part -key. It has no inherent left/right or default-arm meaning. Actions publish their -required slots through ``manipulator_roles`` and ``end_effector_roles``. When a -role such as ``primary`` occurs in both maps, the entries select the arm and -hand/tool serving the same functional participant, but the caller is still -responsible for choosing a physically compatible pair. +For advanced direct-core use, joint-backed endpoint selections are concrete +``RobotCfg.control_parts`` keys, not joint, link, TCP-frame, or scene-object +names. Build them through :meth:`~embodichain.lab.sim.atomic_actions.AtomicActionEngine.bind_control_parts`: + +.. code-block:: python + + binding = engine.bind_control_parts( + "pick_up", + { + "primary": { + "motion": "left_arm", + "grasp": "left_hand", + } + }, + ) + +The helper validates the installed skill contract, resolves joint indices and +commands, and returns the engine-owned generic binding. Profile endpoint +adapters may instead resolve locomotion, whole-body, or custom controller +targets without changing ``ActionBinding``. The engine exclusively owns the ``MotionGenerator``, shared trajectory builder, and control-part profiles. It creates and binds all built-in actions by default; @@ -47,6 +63,13 @@ step. Put invocation-varying behavior in ``ActionInvocation.skill_options``. ``register()`` remains available for custom implementations, and ``load_builtins=False`` creates an isolated or fully custom engine. +Trajectory timing is strict. A planner that returns positions must also return +per-waypoint ``dt``; ``PlanResult.duration`` is derived from it. A custom action +must pass a complete ``TimedTrajectory`` to ``build_plan``. The engine does not +repair missing timing. Action-owned interpolation reads an explicit +``PlanningContext.control_dt`` supplied by the integration, normally +``BaseEnv.step_dt``. + Choosing an engine entry point ------------------------------ @@ -65,7 +88,7 @@ Application code normally uses one of three engine entry points: - ``ActionPlan`` - Reads one context and does not project a next context * - ``engine.compile()`` - - Planning a fixed sequence whose goals are already known + - Planning a fixed sequence whose goals are known and whose plans retain joint trajectories - ``CompiledTrajectory`` - Propagates hypothetical qpos and expected effects, without observing execution * - ``engine.start()`` @@ -73,17 +96,22 @@ Application code normally uses one of three engine entry points: - ``ExecutionSession`` - ``tick()`` consumes measured context, emits commands, requests effect verification, and can replan -As a short rule: use ``plan`` for one action, ``compile`` for a static action -sequence, and ``start`` followed by ``tick`` for observed execution and error -recovery. None of these APIs steps the simulator directly. The application -sends commands returned by an execution session and supplies new observations. +As a short rule: use ``plan`` for one action, ``compile`` for a static +joint-trajectory sequence, and ``start`` followed by ``tick`` for observed +execution and error recovery. None of these APIs steps the simulator directly. +The application sends commands returned by an execution session and supplies +new observations. ``AtomicAction.plan(request, context)`` is different from ``engine.plan()``. It is the framework-owned template method called by the engine, not an additional application execution entry point. Atomic-action authors implement -the protected ``_plan()`` hook instead. Similarly, -``engine.plan_action()`` is reserved for extensions and isolated tests that -need to plan an unregistered instance. +the protected ``_plan()`` hook instead. Register custom action instances with +``engine.register()`` before using the same public planning entry points. + +This extension contract is intentionally strict: a subclass that defines +``plan()`` raises ``TypeError`` at class definition. There is no legacy adapter; +custom actions must rename that implementation to ``_plan()`` so the +framework-owned collision-scene preparation cannot be bypassed. Runnable examples ----------------- @@ -92,11 +120,14 @@ Focused examples live under ``scripts/tutorials/atomic_action``: * ``move_end_effector.py`` * ``move_joints.py`` +* ``control_dt.py`` * ``pickup.py`` * ``move_held_object.py`` * ``place.py`` * ``assemble.py`` * ``press.py`` +* ``slide.py`` +* ``twist.py`` * ``coordinated_pickment.py`` * ``coordinated_placement.py`` * ``hand_over.py`` @@ -110,10 +141,17 @@ video under ``outputs/videos``: .. code-block:: bash python scripts/tutorials/atomic_action/move_end_effector.py --headless --auto_play --device cpu + python scripts/tutorials/atomic_action/control_dt.py --headless --auto_play --device cpu python scripts/tutorials/atomic_action/pickup.py --headless --auto_play --device cpu python scripts/tutorials/atomic_action/assemble.py --headless --auto_play --device cpu python scripts/tutorials/atomic_action/hand_over.py --headless --auto_play --device cpu +``control_dt.py`` compiles the same 40-waypoint ``ik_interp`` path twice. The +positions stay identical, while changing ``PlanningContext.control_dt`` from +``2 * physics_dt`` to ``8 * physics_dt`` makes every arrival interval and the +total trajectory duration four times longer. The script checks that relationship +before replaying the fast and slow trajectories in sequence. + The ``motion_generator`` variable in the snippets below is a configured :class:`~embodichain.lab.sim.planners.MotionGenerator`; its robot, planner, device, cache, and collision world become the resources owned by the engine. @@ -146,9 +184,10 @@ engine is built: ) ``PickUp``, ``Place``, and the other manipulation skills resolve ``open`` and -``grasp`` from their bound end effector. ``MoveJoints`` resolves a string target -from its bound manipulator. Joint limits validate possible commands, but do not -define their semantic meaning; supply calibrated robot commands in production. +``grasp`` from their bound grasp endpoints. ``MoveJoints`` resolves a string +target from ``primary.motion``. Joint limits validate possible commands, but do +not define their semantic meaning; supply calibrated robot commands in +production. Planning one action ------------------- @@ -160,22 +199,27 @@ application-owned orchestration: .. code-block:: python from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, EndEffectorPoseGoal, MotionPolicy, ) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "left_arm"}}, + ) invocation = ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(xpos=target_pose), - binding=ActionBinding(manipulators={"primary": "left_arm"}), - motion_policy=MotionPolicy(sample_count=80, control_dt=1.0 / 60.0), + binding=binding, + motion_policy=MotionPolicy(sample_count=80), ) plan = engine.plan(invocation, latest_context) if plan.plan_success.all(): - trajectory = plan.trajectory.positions + command_frames = plan.commands.frames + if plan.joint_trajectory is not None: + trajectory = plan.joint_trajectory.positions diagnostics = plan.diagnostics segments = plan.segments @@ -184,22 +228,24 @@ sequence, call ``compiled.segment(action_index, name)`` to get the corresponding range in concatenated-trajectory coordinates. This is preferable to repeating a primitive's private sample-split formula in application or tutorial code. -The returned :class:`~embodichain.lab.sim.atomic_actions.ActionPlan` describes -only that invocation. Its expected effects are not committed, and ``plan`` does -not produce a projected context for a following action. Use ``compile`` when -the engine should propagate hypothetical state through a sequence. +The returned :class:`~embodichain.lab.sim.atomic_actions.ActionPlan` always owns +a transport-neutral ``commands`` sequence. Joint-planned actions may also retain +``joint_trajectory`` for feedback, inspection, and static qpos projection. The +plan describes only that invocation: expected effects are not committed, and +``plan`` does not produce a projected context for a following action. Use +``compile`` when the engine should propagate hypothetical state through a +sequence. Static compilation ------------------ Use :meth:`~embodichain.lab.sim.atomic_actions.AtomicActionEngine.compile` when -the scene is treated as fixed and all goals in a sequence are known during -planning: +the scene is treated as fixed, all goals in a sequence are known during +planning, and every action retains ``joint_trajectory``: .. code-block:: python from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, @@ -207,8 +253,11 @@ planning: ) engine = AtomicActionEngine(motion_generator) - binding = ActionBinding(manipulators={"primary": "left_arm"}) - motion_policy = MotionPolicy(sample_count=80, control_dt=1.0 / 60.0) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "left_arm"}}, + ) + motion_policy = MotionPolicy(sample_count=80) approach = ActionInvocation( skill_id="move_end_effector", @@ -236,6 +285,10 @@ planning: state. Calling it with one invocation is valid, but ``plan`` is simpler when a projected context and sequence-shaped result are unnecessary. +This is intentionally an offline joint-trajectory projection API. It rejects a +generic command plan without ``joint_trajectory``; such plans remain valid for +``plan`` and closed-loop ``start``/``tick`` execution. + Do not compile across a point where later targets depend on physical execution. The coordinated-placement tutorial, for example, compiles both pick-ups, executes them, rebuilds held-object state from measured poses, and then compiles @@ -253,7 +306,6 @@ must be resolved from the latest scene snapshot: from embodichain.lab.sim.atomic_actions import ( EndEffectorPoseGoal, RecoveryPolicy, - RigidObjectSceneProvider, SceneEntityPose, ) @@ -262,7 +314,10 @@ must be resolved from the latest scene snapshot: goal=EndEffectorPoseGoal( xpos=SceneEntityPose("moving_tray", relative_pose=tray_to_tcp) ), - binding=ActionBinding(manipulators={"primary": "left_arm"}), + binding=engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "left_arm"}}, + ), recovery_policy=RecoveryPolicy( max_replans=3, tracking_error_threshold=0.05, @@ -271,12 +326,21 @@ must be resolved from the latest scene snapshot: ) from embodichain.lab.sim.atomic_actions import ( + EndpointCommandRouter, ExecutionRunner, SimulationExecutionAdapter, TaskState, ) + from embodichain.lab.sim.skills import SceneRegistry - scene_provider = RigidObjectSceneProvider({"moving_tray": moving_tray}) + registry = SceneRegistry.from_simulation( + sim, + rigid_objects={"moving_tray": moving_tray.uid}, + ) + scene_provider = registry.make_planning_scene_provider( + motion_generator, + batch_size=robot.num_instances, + ) adapter = SimulationExecutionAdapter( sim, robot, @@ -284,8 +348,14 @@ must be resolved from the latest scene snapshot: ) task = TaskState.empty(robot.get_qpos().shape[0], robot.device) initial_context = adapter.observe(task) - session = engine.start((invocation,), initial_context) - runner = ExecutionRunner(session, adapter, adapter, clock=adapter) + initial_eligible = determine_ready_rows(initial_context) + session = engine.start( + (invocation,), + initial_context, + eligible_mask=initial_eligible, + ) + router = EndpointCommandRouter((adapter,)) + runner = ExecutionRunner(session, adapter, router, clock=adapter) result = runner.run_until_blocked() For a lightweight scene source that does not need environment correlation IDs, @@ -293,10 +363,15 @@ pass a ``scene_supplier(timestamp)`` callback instead. ``scene_provider`` and ``scene_supplier`` are mutually exclusive. The session owns planning progress and bounded recovery. The runner owns the -outer lifecycle: it requests fresh observations, schedules each command from -the :class:`~embodichain.lab.sim.atomic_actions.TimedTrajectory` time deltas, -checks controller acknowledgements, and performs cancel-then-hold on failure. -The simulation adapter advances physics instead of sleeping in wall-clock time. +outer lifecycle: it requests fresh observations, schedules each +:class:`~embodichain.lab.sim.atomic_actions.RuntimeCommandFrame` from its +``hold_duration``, checks controller acknowledgements, and performs +cancel-then-hold on failure. ``EndpointCommandRouter`` preflights the whole +frame, groups endpoint commands by exact transport ID, and aggregates their +acknowledgements. Unknown or incompatible transports are rejected before any +partial dispatch. Safe stop cancels every armed runtime target, then asks its +transport to hold from the latest observed context. The simulation adapter +advances physics instead of sleeping in wall-clock time. ``ExecutionRunnerCfg`` contains runner-level transport and scheduling settings; it is not an atomic-action option and is not replaced by invocation revision. @@ -305,27 +380,59 @@ For an application that already owns its event loop, call the non-blocking with ``is_waiting`` set has not consumed a new observation or effect result; use its ``wait_duration`` to schedule the next call. +``eligible_mask`` is an owned initial cohort, not a one-tick filter. Eligibility +can only shrink for the lifetime of the session and remains inactive across +action barriers and replans. If an application later loses a row, deactivate it +through the runner that owns scheduling: + +.. code-block:: python + + changed = runner.deactivate_rows( + lost_tracking_mask, + reason="object tracking was lost", + ) + +The operation is idempotent and the next command actively neutralizes changed +rows. Deactivating rows while an effect is pending narrows the request and +changes its ``verification_id``. Deactivating the last eligible row fails and +terminates the session. Do not call ``session.deactivate_rows()`` directly while +an ``ExecutionRunner`` owns the session because the runner must refresh its +cached effect boundary. + The complete simulation example starts with a visible cube directly in front of the robot, then applies a short horizontal force pulse so physics and friction slide it sideways during one ``PickUp`` invocation whose ``GraspGoal.grasp_xpos`` is a ``SceneEntityPose``. The session observes ``dynamic_goal_changed`` and ``replanned`` events, discards the entire stale -approach/close/lift plan, and rebuilds it from the cube's new location. The -replanned action closes the gripper, verifies the physical lift, and finishes -while holding the cube. The original and regenerated goal axes remain visible -for comparison: +approach/close/lift plan, and rebuilds it from the cube's new location while the +approach segment is active. After approach is dispatched, Pick stops monitoring +that object dependency so contact-, close-, and lift-induced movement does not +trigger a false dynamic-goal update. The replanned action closes the gripper, +verifies the physical lift, and finishes while holding the cube. The original +and regenerated goal axes remain visible for comparison: .. code-block:: bash python scripts/tutorials/atomic_action/moving_target_recovery.py --headless --auto_play --device cpu -For collision-aware execution, list pose-updatable obstacles in -``RigidObjectSceneProvider.collision_entity_ids`` and configure matching -dynamic obstacle names on a supporting planner such as cuRobo. The provider -advances per-environment collision-world revisions when an obstacle moves; -the session invalidates affected rows and the framework binds the latest poses -before replanning. Pose thresholds use the last materially published pose as -their baseline, so cumulative sub-threshold motion is eventually reported: +For collision-aware execution, register each pose-updatable obstacle with +``SceneCollisionRole.DYNAMIC`` and configure the same canonical registry IDs as +the planner's dynamic obstacle names. Derive the cuRobo object mapping with +``registry.collision_geometry_by_id()`` and construct the runtime provider with +``registry.make_planning_scene_provider(motion_generator, batch_size=...)``. +That one factory call checks that the registry's complete ``STATIC ∪ +DYNAMIC`` set exactly matches the planner's complete collision world, then +checks that the registry, provider, and planner dynamic subsets exactly match. +It also checks planner capability and shared/per-environment world mode. One +environment may infer a shared world; a multi-environment dynamic registry must choose +``SceneCollisionWorldMode.SHARED`` or ``PER_ENV`` explicitly. See +:doc:`/overview/sim/scene_registry` for the complete cuRobo mapping example. + +The provider advances per-environment collision-world revisions when an +obstacle moves; the session invalidates affected rows and the framework binds +the latest poses before replanning. Pose thresholds use the last materially +published pose as their baseline, so cumulative sub-threshold motion is +eventually reported: .. code-block:: bash @@ -355,53 +462,157 @@ control command while the action is active, submit a strictly newer revision: invocation_id=invocation.invocation_id, revision=invocation.revision + 1, ) - session.revise_current(revised) + runner.revise_current(revised) The session replans from its latest context and emits an ``invocation_revised`` event. ``skill_id`` and ``invocation_id`` must still -identify the active logical call. - -Only entities referenced through ``SceneEntityPose`` become automatic -scene-motion dependencies. A skill may query a simulation entity's live pose -when it plans, but that query alone does not cause an executing session to -replan when the entity moves. +identify the active logical call, and the replacement must preserve the +current non-empty runtime destination set and exact target address fingerprints. +Use a new invocation when changing from an arm endpoint to a base, whole-body +controller, or another controller. The runner keeps the current frame deadline, +then observes fresh state and installs the revision at that due boundary. It +rejects revision while a physical effect is awaiting verification; verify the +effect first, or cancel and start a new invocation. A manually ticked session +can call ``session.revise_current(revised, context=fresh_context)`` directly. + +Every emitted command is authorized against the binding-owned target and +physical claims. Non-empty plan frames and recovery replans keep a stable +destination set. Transports must actively neutralize inactive batch rows for +every addressed target; simply skipping those rows can leave a persistent +controller command active. + +Entities referenced through ``SceneEntityPose`` become automatic scene-motion +dependencies. Object-centric skills may additionally declare an explicit +``ObjectSemantics.entity_id`` when they ground an object pose from the same +scene snapshot; for example, ``PickUp`` automatically tracks that ID. The +legacy ``ObjectSemantics.entity`` live-pose fallback is deprecated and does not +create a scene dependency. An ``ActionPlan`` may bound each dependency with +``scene_dependency_monitor_until``. ``PickUp`` uses the exclusive end of +``approach`` as that boundary; joint tracking and collision-world revision +checks are unaffected. + +An action may give selected dependencies an exclusive waypoint cutoff through +``ActionPlan.scene_dependency_monitor_until``. A dependency is monitored while +the current waypoint index is smaller than its cutoff; ``0`` disables monitoring +from the start, and an omitted dependency remains monitored for the whole +action. Reaching the cutoff ignores every later pose change, not only motion +caused by the skill. Built-in ``PickUp`` uses ``close.start`` for the grasped +object, and ``OperateArticulation`` uses ``operate.start`` for the handle; their +physical effect monitors are authoritative after those boundaries. Task-state effects ------------------ Pick, place, handover, and coordinated skills declare attachment changes as a :class:`~embodichain.lab.sim.atomic_actions.StateDelta`. Planning does not commit -those changes. During closed-loop execution, a non-empty effect requires an -external per-environment verification mask: +those changes. During closed-loop execution, a non-empty effect requires a +correlated per-environment verification result: .. code-block:: python - def verify_effect(context, tick): - return verify_grasp_or_release(context) + import torch + + from embodichain.lab.sim.atomic_actions import EffectVerificationResult + + def verify_effect(context, request): + success_mask, failure_mask = verify_grasp_or_release(context, request.env_mask) + return EffectVerificationResult( + verification_id=request.verification_id, + success_mask=success_mask, + failure_mask=failure_mask, + invalidation_mask=failure_mask, + retry_mask=torch.zeros_like(failure_mask), + ) result = runner.run_until_blocked(effect_verifier=verify_effect) This prevents a successful trajectory plan from being mistaken for a successful -physical grasp or release. If verification is asynchronous, omit the callback; +physical grasp or release. The runner invokes this synchronous callback after a +fresh due-cycle observation and feeds its result to the session in that same +cycle. Returning all-false masks keeps the remaining rows unresolved. If +verification is asynchronous, omit the callback; ``run_until_blocked`` returns at the verification boundary and the application -can later resume with ``runner.step(effect_success=verified)`` when the next -cycle is due, or call ``run_until_blocked(effect_verifier=...)`` again. The -runner remembers the pending boundary even though the session emits its event -only once. The durable state is ``tick.pending_effect`` (an -``EffectVerificationRequest``), not the presence of that one-time event. +can later resume from the *current* pending request: + +.. code-block:: python + + request = runner.session.pending_effect + assert request is not None + success_mask, failure_mask = await_effect_observation(request.env_mask) + verified = EffectVerificationResult( + verification_id=request.verification_id, + success_mask=success_mask, + failure_mask=failure_mask, + invalidation_mask=failure_mask, + retry_mask=torch.zeros_like(failure_mask), + ) + resumed = runner.step(effect_result=verified) + if resumed.is_waiting: + schedule_after(resumed.wait_duration) + # This call did not consume ``verified``. Re-read the current request + # and submit a result for that ID again at the due cycle. + +Alternatively, call ``run_until_blocked(effect_verifier=...)`` again. Success +and failure masks must be disjoint subsets of the request mask; rows in neither +mask remain unresolved. A result must reuse the current request's +``verification_id``. Deactivation, partial resolution, or retry can replace the +request, so re-read it before delayed submission and re-verify if its ID or mask +changed. ``request.deadline`` uses the robot-observation timestamp domain; +``RecoveryPolicy.action_timeout`` covers both trajectory execution and the +terminal effect wait. A result submitted after timeout cannot satisfy the new +retry attempt because its old ID is invalid. The runner remembers the pending +boundary even though the session emits its event only once. The durable state is +``tick.pending_effect`` (an ``EffectVerificationRequest``), not the presence of +that one-time event. ``invalidation_mask`` and ``retry_mask`` must both be +subsets of ``failure_mask``. Invalidation selects rows for the request's +core-owned, removal-only ``failure_invalidation`` delta; a verifier cannot +publish arbitrary replacement state. Set a retry row only when replaying the +same invocation remains physically valid. Other failed rows enter external +recovery after selected invalidation. Unresolved evidence at the action +deadline is reconciled fail-closed when covered verified state is still active. + +Trajectory-segment effect gates +------------------------------- + +An invocation may declare a +:class:`~embodichain.lab.sim.atomic_actions.PhaseEffectGateRequirement` for a +named, non-initial trajectory segment. The execution session then exposes a +:class:`~embodichain.lab.sim.atomic_actions.PhaseEffectGateRequest` immediately +before the first frame of that segment. Curated semantic calls install these +automatically: Pick gates ``lift`` on destination attachment, Place gates +``retract`` on source detachment, and HandOver gates source ``release`` on +destination attachment. + +Supply ``phase_effect_gate_verifier(context, request)`` to ``runner.step()`` or +``runner.run_until_blocked()``. It runs on a fresh due-cycle observation and +returns a correlated +:class:`~embodichain.lab.sim.atomic_actions.PhaseEffectGateResult`. If neither +the success nor failure mask selects every remaining active row, the session +keeps the whole cohort at the boundary and resends the command immediately +before the gated segment. This preserves a close/open command and its physical +preload; it is not an observed-position hold. + +Gate success only permits the next command and does not update ``TaskState``. +The terminal effect verifier still owns the semantic commit. A contradictory +row may consume the enclosing action's retry budget; a row outside the result's +``retry_mask`` requires external recovery. The gate shares the action timeout, +and each consumed observation replaces its request ID. Without a gate verifier, +``run_until_blocked()`` returns the pending boundary for asynchronous handling. Adding an action ---------------- -Define an action-owned frozen goal dataclass with a stable ``goal_kind``. Then -define typed runtime options when needed, implement the protected +Define an action-owned frozen goal dataclass. Then define typed runtime options +when needed, implement the protected ``_plan(request, context)`` hook, and declare the stable skill metadata. Do not override the inherited public ``plan()`` method because it binds the latest -collision scene first. +collision scene first. Legacy custom actions that implemented ``plan()`` must +rename it to ``_plan()``; defining ``plan()`` is rejected immediately. Return scalar or per-environment planner success through ``build_plan``. The framework normalizes the mask and holds failed rows at the observed qpos, so a -new action should not reproduce that masking itself. +new action should not reproduce that masking itself. ``build_plan`` accepts +only a ``TimedTrajectory``; an untimed position tensor raises immediately. A minimal implementation looks like: @@ -410,9 +621,24 @@ A minimal implementation looks like: from dataclasses import dataclass from typing import ClassVar + import torch + + from embodichain.lab.sim.atomic_actions import ( + CARTESIAN_POSE_CAPABILITY, + ActionOptions, + ActionPlan, + AtomicAction, + JointPositionTarget, + PlanningContext, + ResolvedActionRequest, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, + TimedTrajectory, + ) + @dataclass(frozen=True, slots=True) class PushGoal: - goal_kind: ClassVar[str] = "push" contact_pose: torch.Tensor @dataclass(frozen=True, slots=True) @@ -423,7 +649,21 @@ A minimal implementation looks like: skill_id: ClassVar[str] = "push" GoalType: ClassVar[type] = PushGoal OptionsType: ClassVar[type] = PushOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset( + {CARTESIAN_POSE_CAPABILITY} + ), + ), + ), + ), + ), + ) def __init__(self, default_options: PushOptions | None = None) -> None: super().__init__(default_options) @@ -435,15 +675,31 @@ A minimal implementation looks like: ) -> ActionPlan: goal = self.require_goal(request) options = request.skill_options - # Resolve the bound resource, plan from context.robot.qpos, and - # return a full-robot TimedTrajectory or position tensor. + motion = request.binding.endpoint("primary", "motion") + motion_target = motion.require_target(JointPositionTarget) + # Plan from context.robot.qpos using motion_target.joint_ids and + # produce full_robot_positions. + # The joint helper lowers the result into RuntimeCommandFrame values + # and retains the trajectory for joint-position feedback. + trajectory = TimedTrajectory.from_uniform_step( + full_robot_positions, + env_ids=context.env_ids, + step_dt=context.require_control_dt(), + ) return self.build_plan( request, context, success=success_mask, - trajectory=full_robot_positions, + trajectory=trajectory, ) +For a non-joint endpoint, define a typed ``RuntimeEndpointTarget`` and matching +``RuntimeCommandPayload``, have the profile endpoint adapter produce that +target, and call ``build_command_plan(commands=TimedCommandSequence(...))``. +Register the matching ``EndpointCommandTransport`` with the runner's router. +The skill contract, resource graph, binding, runner, and recovery model do not +gain controller-specific fields. + Do not step simulation, mutate ``PlanningContext``, commit ``StateDelta``, or expose planner-specific configuration through the goal. See the in-repository ``add-atomic-action`` skill for the complete checklist. diff --git a/docs/source/tutorial/gizmo.rst b/docs/source/tutorial/gizmo.rst index f81ca8919..f195c46ee 100644 --- a/docs/source/tutorial/gizmo.rst +++ b/docs/source/tutorial/gizmo.rst @@ -7,6 +7,9 @@ Interactive Robot Control with Gizmo This tutorial demonstrates how to use the Gizmo class for interactive robot manipulation in SimulationManager. You'll learn how to create a gizmo attached to a robot's end-effector and use it for real-time inverse kinematics (IK) control, allowing intuitive manipulation of robot poses through visual interaction. +For the cross-frontend capability summary, supported targets, lifecycle rules, +and security boundary, see :doc:`/features/interaction/gizmo`. + The Code ~~~~~~~~ @@ -71,7 +74,7 @@ First, we configure a UR10 robot with an IK solver for end-effector control: .. literalinclude:: ../../../scripts/tutorials/sim/gizmo_robot.py :language: python - :start-at: # Create UR10 robot configuration + :start-at: # Create UR10 robot :end-at: robot = sim.add_robot(cfg=robot_cfg) Key components of the robot configuration: diff --git a/docs/source/tutorial/index.rst b/docs/source/tutorial/index.rst index fa19a380a..10b76881f 100644 --- a/docs/source/tutorial/index.rst +++ b/docs/source/tutorial/index.rst @@ -20,19 +20,20 @@ Follow the tutorials in this order for the best learning experience: 7. :doc:`sensor` — Add cameras and capture RGB/depth/segmentation data. 8. :doc:`solver` — Configure IK solvers for end-effector control. 9. :doc:`motion_gen` — Generate smooth trajectories with motion planners. -10. :doc:`atomic_actions` — Use built-in action primitives (move, move joints, pick, move held object, place). -11. :doc:`gizmo` — Interactively control robots with on-screen gizmos. +10. :doc:`robot_articulation` — Plan contact-rich motion to open a passive drawer and push it halfway back. +11. :doc:`atomic_actions` — Use built-in action primitives (move, move joints, pick, move held object, place). +12. :doc:`gizmo` — Interactively control robots with on-screen gizmos. **Phase 2: Environments** -12. :doc:`basic_env` — Create a simple Gymnasium environment with ``BaseEnv``. Prerequisite: Phase 1 basics. -13. :doc:`modular_env` — Build a config-driven environment with ``EmbodiedEnv``, managers, and randomization. Prerequisite: :doc:`basic_env`. -14. :doc:`data_generation` — Generate expert demonstration datasets for imitation learning. Prerequisite: :doc:`modular_env`. -15. :doc:`rl` — Train RL agents with PPO or GRPO. Prerequisite: :doc:`basic_env`. +13. :doc:`basic_env` — Create a simple Gymnasium environment with ``BaseEnv``. Prerequisite: Phase 1 basics. +14. :doc:`modular_env` — Build a config-driven environment with ``EmbodiedEnv``, managers, and randomization. Prerequisite: :doc:`basic_env`. +15. :doc:`data_generation` — Generate expert demonstration datasets for imitation learning. Prerequisite: :doc:`modular_env`. +16. :doc:`rl` — Train RL agents with PPO or GRPO. Prerequisite: :doc:`basic_env`. **Phase 3: Extending the Framework** -16. :doc:`add_robot` — Add a new robot model to EmbodiChain. +17. :doc:`/guides/add_robot` — Add a new robot model to EmbodiChain. .. toctree:: :maxdepth: 1 @@ -45,10 +46,10 @@ Follow the tutorials in this order for the best learning experience: rigid_constraint articulation robot - add_robot solver sensor motion_gen + robot_articulation atomic_actions gizmo basic_env diff --git a/docs/source/tutorial/modular_env.rst b/docs/source/tutorial/modular_env.rst index f79ee6d36..d16a06f9d 100644 --- a/docs/source/tutorial/modular_env.rst +++ b/docs/source/tutorial/modular_env.rst @@ -173,7 +173,7 @@ The actual environment class is remarkably simple due to the configuration-drive .. literalinclude:: ../../../scripts/tutorials/gym/modular_env.py :language: python - :start-at: @register_env("ModularEnv-v1", override=True) + :start-at: @register_env("ModularEnv-v1", max_episode_steps=100, override=True) :end-at: super().__init__(cfg, **kwargs) The :class:`envs.EmbodiedEnv` base class automatically: diff --git a/docs/source/tutorial/motion_gen.rst b/docs/source/tutorial/motion_gen.rst index 54566c009..9d550c454 100644 --- a/docs/source/tutorial/motion_gen.rst +++ b/docs/source/tutorial/motion_gen.rst @@ -107,6 +107,7 @@ API Reference motion_opts = MotionGenOptions( strategy="motion_gen", # "motion_gen" or "ik_interp" sample_count=None, # Optional normalized output length + interpolation_dt=None, # Required for deterministic interpolation plan_opts=ToppraPlanOptions(...), # Options for the underlying planner control_part=arm_name, # Robot part to control (e.g., 'left_arm') is_interpolate=False, # Whether to pre-interpolate trajectory @@ -126,8 +127,10 @@ API Reference options: MotionGenOptions | None = None, ) -> PlanResult -- ``strategy="motion_gen"`` delegates to the configured backend; ``strategy="ik_interp"`` performs deterministic waypoint IK and joint interpolation. -- Returns a normalized, environment-batched ``PlanResult``. +- ``strategy="motion_gen"`` delegates to the configured backend; ``strategy="ik_interp"`` performs deterministic waypoint IK and joint interpolation and requires ``interpolation_dt``. +- Returns a normalized, environment-batched ``PlanResult`` with explicit ``dt`` + and derived ``duration`` whenever positions are present. Missing timing raises + immediately. - Uses ``target_states`` (list of PlanState) and ``options`` (MotionGenOptions) instead of individual parameters. **interpolate_trajectory** @@ -166,7 +169,7 @@ API Reference - (Reserved) Plan trajectory with collision checking (not yet implemented). Notes & Best Practices -~~~~~~~~~~~~~~~~~~~~~ +~~~~~~~~~~~~~~~~~~~~~~ - Only collision-free planning is currently supported; collision checking is a placeholder. - Input/outputs are numpy arrays or torch tensors; ensure type consistency. diff --git a/docs/source/tutorial/rl.rst b/docs/source/tutorial/rl.rst index 54ce4590f..8d71b20b6 100644 --- a/docs/source/tutorial/rl.rst +++ b/docs/source/tutorial/rl.rst @@ -82,7 +82,7 @@ APG and PPO. Launch either config with the same CLI: embodichain train-rl --config embodichain_tasks/configs/agents/rl/basic/point_mass/train_ppo.yaml Configuration Sections ---------------------- +---------------------- Runtime Settings ^^^^^^^^^^^^^^^^ @@ -134,7 +134,7 @@ Example: } Policy Configuration -^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^ The ``policy`` section defines the neural network policy: @@ -266,7 +266,7 @@ All outputs are written to ``./outputs/_/``: - **checkpoints/**: Model checkpoints Training Process -~~~~~~~~~~~~~~~ +~~~~~~~~~~~~~~~~ The training process follows this sequence: @@ -326,7 +326,7 @@ Available Algorithms - **GRPO**: Group Relative Policy Optimization (no Critic, step-wise returns, masked group normalization). Use ``actor_only`` policy. Set ``kl_coef=0`` for from-scratch training (CartPole, dense reward); ``kl_coef=0.02`` for VLA/LLM fine-tuning. Adding a New Algorithm ---------------------- +---------------------- To add a new algorithm: diff --git a/docs/source/tutorial/robot.rst b/docs/source/tutorial/robot.rst index c3a54ab56..cd3f277ac 100644 --- a/docs/source/tutorial/robot.rst +++ b/docs/source/tutorial/robot.rst @@ -1,7 +1,7 @@ .. _tutorial_simulate_robot: Simulating a Robot -================ +================== .. currentmodule:: embodichain.lab.sim @@ -140,4 +140,4 @@ After mastering basic robot simulation, you can explore: - Sensor integration (cameras, force sensors) - Robot-object interaction scenarios -This tutorial provides the foundation for creating sophisticated robotic simulation scenarios with SimulationManager. \ No newline at end of file +This tutorial provides the foundation for creating sophisticated robotic simulation scenarios with SimulationManager. diff --git a/docs/source/tutorial/robot_articulation.rst b/docs/source/tutorial/robot_articulation.rst new file mode 100644 index 000000000..dd49cf7df --- /dev/null +++ b/docs/source/tutorial/robot_articulation.rst @@ -0,0 +1,301 @@ +.. _tutorial_robot_articulation: + +Opening a Drawer with Contact-Rich Motion +========================================= + +This tutorial combines a driven Franka Panda with a passive drawer. The robot +approaches a handle, pulls the drawer open through gripper contact, and then +pushes it back to half of the measured opening. Arm motion is generated with +``MotionGenerator``; the drawer joint is never commanded directly. + +.. raw:: html + +
+ +
+ Franka approaches the handle, pulls the passive drawer open, and pushes + it halfway back using three generated arm trajectories. +
+
+ +:download:`Download the MP4 video <../_static/tutorials/open_drawer.mp4>`. + +Before starting, it helps to be familiar with :doc:`robot`, +:doc:`articulation`, and :doc:`motion_gen`. + + +Learning objectives +~~~~~~~~~~~~~~~~~~~ + +After completing this tutorial, you should understand how to: + +- derive robot targets from a moving articulation link rather than fixed world + coordinates; +- convert Cartesian task poses into joint waypoints and time-parameterized + trajectories; +- separate planning, robot control, and physics interaction; +- use measured articulation state to plan the next contact phase; +- verify task success from object state instead of commanded robot motion. + +The complete example is ``scripts/tutorials/sim/open_drawer.py``. + +.. dropdown:: Complete open_drawer.py + :icon: code + + .. literalinclude:: ../../../scripts/tutorials/sim/open_drawer.py + :language: python + :linenos: + + +Understand what is controlled +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The robot and drawer are both articulations, but they play different roles: + +.. list-table:: Control boundary + :header-rows: 1 + :widths: 22 31 47 + + * - Entity + - Command + - Effect + * - Franka arm + - Arm joint-position targets + - Tracks the generated trajectory through its joint drives. + * - Franka gripper + - Finger joint-position targets + - Creates and maintains contact with the handle. + * - Drawer + - No joint target + - Its passive prismatic joint moves only in response to contact forces. + +The drawer uses ``drive_type="none"`` and a fixed base. Fixing the base does +not lock the slide joint; it only prevents the cabinet body from moving. Contact +friction is increased so the fingertips can retain the narrow handle during +the pull. + +The full data flow is: + +.. code-block:: text + + handle link pose + ↓ + Cartesian TCP waypoints + ↓ inverse kinematics + arm joint waypoints + ↓ MotionGenerator + TOPPRA + time-sampled joint trajectory + ↓ position drives + physics updates + gripper contact moves the passive drawer + +This distinction is important: ``MotionGenerator`` generates robot motion. It +does not generate a drawer trajectory or directly change drawer state. + + +Build targets in the handle frame +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The drawer URDF provides a link named ``handle_xpos``. Treating this link as the +task frame keeps all targets attached to the drawer when it moves. + +The asset's handle frame already points TCP +Z along the approach axis. The +example post-multiplies its orientation by a 90-degree local-Z rotation: + +.. math:: + + R_{grasp} = R_{handle} R_z(\pi / 2) + +Post-multiplication matters here: it rolls the gripper in the handle frame. The +TCP Z axis and approach direction remain unchanged, while the finger-closing +direction rotates to grip the handle vertically. A world-frame rotation would +also change the approach direction. + +.. literalinclude:: ../../../scripts/tutorials/sim/open_drawer.py + :language: python + :start-at: def get_handle_grasp_pose( + :end-before: def open_drawer( + +For this asset, the three translations are: + +.. code-block:: text + + pre-grasp = handle position - TCP_Z × 0.10 m + pull = live handle position - TCP_Z × 0.16 m + push = live handle position + TCP_Z × half the measured opening + +The signs come from the bundled handle frame convention. For another asset, +inspect its task-frame axes before reusing them. The handle pose is read again +before the pull and push because previous contact phases may have moved the +drawer. + + +Turn task poses into trajectories +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Trajectory generation is a three-stage process: + +1. ``robot.compute_ik`` converts each sparse TCP pose into an arm joint + waypoint. The previous IK result seeds the next solve, which encourages a + continuous solution instead of switching kinematic branches. +2. ``MotionGenerator`` passes the joint waypoints to TOPPRA, applying velocity + and acceleration constraints and sampling a smooth trajectory. +3. The script sends each sample to the arm position drives and advances + physics. Generating a trajectory alone does not move the robot. + +The tensors stay batched throughout this process. For ``B`` environments, the +generated arm positions have shape ``(B, samples, arm_dof)``. Consequently, +each environment can use a different measured drawer opening while sharing the +same planning code. + +.. attention:: + + TOPPRA time-parameterizes the supplied path but does not collision-check it. + Keep the pre-grasp outside the object, inspect the motion in the viewer, and + add collision-aware planning when obstacles make a straight approach unsafe. + + +Plan contact phases separately +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The example uses three arm trajectories instead of planning the entire task at +once: + +1. **Approach:** move through the pre-grasp and handle poses with the gripper + open. +2. **Pull:** close the fingers, let contact settle, then move 16 cm along TCP + -Z. +3. **Push:** measure the achieved drawer opening and move half that distance + back along TCP +Z while keeping the gripper closed. + +Splitting the task is necessary because closing the gripper changes the contact +state, and the push target is not known until the pull has physically executed. +This is phase-level feedback: each trajectory is played open-loop, but the next +phase is planned from newly measured simulator state. + +The push target uses the achieved opening rather than the requested 16 cm: + +.. literalinclude:: ../../../scripts/tutorials/sim/open_drawer.py + :language: python + :start-at: # Push the drawer back by half of its measured opening. + :end-at: push_pose[:, :3, 3] += pushed_handle_pose[:, :3, 2] * push_distance.unsqueeze(-1) + +``pulled_opening`` is cloned before further simulation updates so the 50% +target remains fixed while the push executes. + + +Synchronize execution and verify the object +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The approach trajectory is generated before execution starts. By default the +script then pauses at the terminal: + +.. code-block:: text + + [READY]: Trajectory planned. Press Enter to start execution... + +Pressing Enter starts the complete approach, grasp, pull, and push sequence. +This breakpoint is useful for checking the initial scene and robot state before +any arm target is applied. Use ``--auto-start`` only for unattended runs. + +Success is evaluated from ``drawer.get_qpos()`` rather than the final TCP pose. +The script checks two facts: + +- the pull opened every drawer by at least 10 cm; +- the push finished within 2 cm of half the opening actually achieved by that + environment. + +This catches contact failures that a robot-only trajectory check would miss. + + +Run the tutorial +~~~~~~~~~~~~~~~~ + +From the repository root, run with the native viewer: + +.. code-block:: bash + + python scripts/tutorials/sim/open_drawer.py + +For a non-interactive CPU run: + +.. code-block:: bash + + python scripts/tutorials/sim/open_drawer.py \ + --headless \ + --device cpu \ + --hold-steps 0 \ + --auto-start + +CUDA physics and multiple environments use the common launcher arguments: + +.. code-block:: bash + + python scripts/tutorials/sim/open_drawer.py \ + --headless \ + --device cuda \ + --num_envs 4 \ + --auto-start + +The embedded video was recorded directly from a fixed camera in headless mode. +You can reproduce it with: + +.. code-block:: bash + + python scripts/tutorials/sim/open_drawer.py \ + --headless \ + --device cuda \ + --hold-steps 100 \ + --auto-start \ + --record-fps 30 \ + --record-save-path outputs/videos/open_drawer.mp4 + +Typical output is similar to: + +.. code-block:: text + + [INFO]: Drawer opening after pull (m): [0.1606] + [INFO]: Drawer opening after half push (m): [0.0811] + +Exact values vary slightly because the drawer is moved through simulated +contact. ``--hold-steps`` controls how long the final pose remains visible. + + +Diagnose common failures +~~~~~~~~~~~~~~~~~~~~~~~~ + +.. list-table:: + :header-rows: 1 + :widths: 28 72 + + * - Symptom + - What to inspect + * - IK fails before execution + - Check handle-frame orientation, reachability, and the seeded arm + configuration. Shorten the approach distance when necessary. + * - The arm moves but the drawer does not + - Confirm the drawer drive is passive, the fingertips close around the + handle, and the contact materials provide enough friction. + * - Pull succeeds but the push misses halfway + - Re-read the handle pose after pulling and calculate the push from measured + drawer position, not the requested pull distance. + * - The arm intersects the cabinet + - Add safe Cartesian waypoints or use a collision-aware planner; TOPPRA + alone does not change the geometric path. + + +Adapt the pattern +~~~~~~~~~~~~~~~~~ + +For another prismatic mechanism, provide a task frame whose approach axis and +joint axis have known directions, then adjust the signed translation distances. + +A revolute door needs a different geometric path: sample handle poses along an +arc around the hinge, keep the gripper orientation consistent with the door, +solve the poses sequentially with seeded IK, and pass those joint waypoints to +``MotionGenerator``. The remaining structure—contact transition, state +measurement, replanning, and object-state validation—stays the same. diff --git a/docs/source/tutorial/solver.rst b/docs/source/tutorial/solver.rst index 61300096f..10fad00cd 100644 --- a/docs/source/tutorial/solver.rst +++ b/docs/source/tutorial/solver.rst @@ -106,7 +106,7 @@ Configuration - Use `cfg.init_solver()` to instantiate the solver, or assign to `robot_cfg.solver_cfg` for automatic integration. Notes & Best Practices -~~~~~~~~~~~~~~~~~~~~~ +~~~~~~~~~~~~~~~~~~~~~~ - Always ensure URDF and joint/link names match your robot model. - For IK, providing a good `qpos_seed` improves convergence and solution quality. - Use `set_iteration_params` (if available) to tune solver performance for your application. @@ -115,4 +115,4 @@ Notes & Best Practices See Also ~~~~~~~~ - :ref:`tutorial_motion_generator` — Motion Generator -- :ref:`tutorial_basic_env` — Basic Environment Setup +- :ref:`tutorial_create_basic_env` — Basic Environment Setup diff --git a/embodichain/agents/__init__.py b/embodichain/agents/__init__.py new file mode 100644 index 000000000..071c9ac48 --- /dev/null +++ b/embodichain/agents/__init__.py @@ -0,0 +1,21 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Agent-facing frontends built on EmbodiChain's typed runtime contracts.""" + +from __future__ import annotations + +__all__: list[str] = [] diff --git a/embodichain/agents/mllm/__init__.py b/embodichain/agents/mllm/__init__.py new file mode 100644 index 000000000..607c022d9 --- /dev/null +++ b/embodichain/agents/mllm/__init__.py @@ -0,0 +1,29 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Multimodal-model frontends for typed EmbodiChain agent contracts.""" + +from __future__ import annotations + +from .expert_program import ( + compile_mllm_expert_program, + decode_mllm_expert_program, +) + +__all__ = [ + "compile_mllm_expert_program", + "decode_mllm_expert_program", +] diff --git a/embodichain/agents/mllm/expert_program.py b/embodichain/agents/mllm/expert_program.py new file mode 100644 index 000000000..a54304d78 --- /dev/null +++ b/embodichain/agents/mllm/expert_program.py @@ -0,0 +1,260 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Strict MLLM frontend for declarative Expert Program JSON responses.""" + +from __future__ import annotations + +from collections.abc import Iterator + +from embodichain.lab.gym.envs.expert_program.cfg import ( + EXPERT_PROGRAM_SCHEMA_VERSION, + ExpertProgramCfg, + ExpertProgramIntegrationCfg, + HandOverCfg, + InvokeCfg, + OperateArticulationCfg, + PickCfg, + PlaceCfg, + ProgramNodeCfg, + RepeatCfg, + SegmentCfg, + SemanticCallCfg, + SequenceCfg, +) +from embodichain.lab.gym.envs.expert_program.compiler import CompiledProgram +from embodichain.lab.gym.envs.expert_program.decoder import ( + ConfigPath, + ExpertProgramDecodeError, + ExpertProgramValidationContext, + decode_expert_program, + validate_expert_program, +) +from embodichain.lab.gym.envs.expert_program.environment import ( + ExpertProgramEnvironmentAdapter, +) +from embodichain.lab.gym.envs.expert_program.loader import ( + MAX_EXPERT_PROGRAM_BYTES, + parse_expert_program_json, +) + +__all__ = [ + "compile_mllm_expert_program", + "decode_mllm_expert_program", +] + +_CURATED_CALL_TYPES = ( + PickCfg, + PlaceCfg, + HandOverCfg, + OperateArticulationCfg, +) + + +def _iter_calls( + node: ProgramNodeCfg, + *, + path: ConfigPath, +) -> Iterator[tuple[SemanticCallCfg, ConfigPath]]: + """Yield every semantic call and its decoder-compatible source path.""" + if type(node) is InvokeCfg: + yield node.call, (*path, "call") + return + if type(node) is SequenceCfg: + for index, child in enumerate(node.items): + yield from _iter_calls(child, path=(*path, "items", index)) + return + if type(node) is RepeatCfg: + yield from _iter_calls(node.body, path=(*path, "body")) + return + if type(node) is SegmentCfg: + yield from _iter_calls(node.steps, path=(*path, "steps")) + return + raise ExpertProgramDecodeError( + "mllm_program_node_not_allowed", + (*path, "kind"), + "The MLLM frontend permits only Version 1 sequential program nodes.", + ) + + +def _value_at_path(value: object, path: ConfigPath) -> object: + """Return a raw decoded JSON value at one already validated config path.""" + current = value + for part in path: + if type(part) is int: + if type(current) is not list or not 0 <= part < len(current): + raise ExpertProgramDecodeError( + "mllm_payload_mismatch", + path, + "Decoded model payload no longer matches the canonical program.", + ) + current = current[part] + else: + if type(current) is not dict or part not in current: + raise ExpertProgramDecodeError( + "mllm_payload_mismatch", + path, + "Decoded model payload no longer matches the canonical program.", + ) + current = current[part] + return current + + +def _validate_mllm_policy( + config: ExpertProgramCfg, + *, + raw_payload: dict[str, object], +) -> None: + """Apply the narrow agent-facing policy after canonical decoding.""" + if config.schema_version != EXPERT_PROGRAM_SCHEMA_VERSION: + raise ExpertProgramDecodeError( + "mllm_schema_version_not_allowed", + ("schema_version",), + "The MLLM frontend permits only Expert Program schema Version 1.", + ) + for call, path in _iter_calls(config.program, path=("program",)): + if type(call) not in _CURATED_CALL_TYPES: + raise ExpertProgramDecodeError( + "mllm_call_not_allowed", + (*path, "kind"), + "The MLLM frontend permits only curated pick, place, hand_over, " + "and operate_articulation calls.", + ) + raw_call = _value_at_path(raw_payload, path) + if type(raw_call) is not dict: + raise ExpertProgramDecodeError( + "mllm_payload_mismatch", + path, + "Decoded model payload no longer matches the canonical program.", + ) + raw_resources = raw_call.get("resources", {}) + if type(raw_resources) is dict and raw_resources: + raise ExpertProgramDecodeError( + "mllm_resource_override_not_allowed", + (*path, "resources"), + "MLLM responses cannot override robot resource bindings.", + ) + if type(call) is HandOverCfg and call.receiver is not None: + raise ExpertProgramDecodeError( + "mllm_resource_override_not_allowed", + (*path, "receiver"), + "MLLM responses cannot select a hand-over receiver resource.", + ) + if type(call) is OperateArticulationCfg and call.target is None: + raise ExpertProgramDecodeError( + "mllm_articulation_target_not_allowed", + (*path, "target_position"), + "MLLM articulation calls must select a host-declared named target.", + ) + if call.resources: + raise ExpertProgramDecodeError( + "mllm_resource_override_not_allowed", + (*path, "resources"), + "MLLM responses cannot override robot resource bindings.", + ) + + +def decode_mllm_expert_program( + response: str, + *, + integration: ExpertProgramIntegrationCfg, + validation_context: ExpertProgramValidationContext | None = None, + max_bytes: int = MAX_EXPERT_PROGRAM_BYTES, +) -> ExpertProgramCfg: + """Decode one untrusted model response into the canonical program config. + + The model response is a single plain JSON object containing + ``schema_version``, ``program_id``, ``targets``, and ``program``. The trusted + host supplies ``integration``; a response attempting to select its own + integration is rejected rather than silently overwritten. Version 1 curated + calls are the only admitted semantic surface, and robot resource overrides + are forbidden. + + Args: + response: Untrusted model response containing one plain JSON document. + integration: Host-owned scene, robot-profile, and runtime-preset choice. + validation_context: Optional provider-free static reference validator. + max_bytes: Maximum UTF-8 encoded response size. + + Returns: + An owned canonical :class:`ExpertProgramCfg`. + + Raises: + TypeError: If ``integration`` is not an exact integration config. + ExpertProgramDecodeError: If JSON, schema, or MLLM policy validation + fails. + """ + if type(integration) is not ExpertProgramIntegrationCfg: + raise TypeError("integration must be exactly ExpertProgramIntegrationCfg.") + data = parse_expert_program_json(response, max_bytes=max_bytes) + if "integration" in data: + raise ExpertProgramDecodeError( + "model_controlled_integration", + ("integration",), + "MLLM responses cannot select an integration; the host injects it.", + ) + payload = dict(data) + payload["integration"] = { + "robot_profile": integration.robot_profile, + "scene_registry": integration.scene_registry, + "runtime_preset": integration.runtime_preset, + } + config = decode_expert_program(payload) + _validate_mllm_policy(config, raw_payload=payload) + if validation_context is not None: + validate_expert_program(config, validation_context) + return config + + +def compile_mllm_expert_program( + response: str, + *, + adapter: ExpertProgramEnvironmentAdapter, + integration: ExpertProgramIntegrationCfg, + validation_context: ExpertProgramValidationContext | None = None, + max_bytes: int = MAX_EXPERT_PROGRAM_BYTES, +) -> CompiledProgram: + """Decode and compile a model response through the existing environment path. + + This function introduces no MLLM-specific compiler. It delegates the owned + config to :meth:`ExpertProgramEnvironmentAdapter.compile`, which performs the + canonical scene resolution and Expert Program lowering used by every other + frontend. + + Args: + response: Untrusted model response containing one plain JSON document. + adapter: Existing trusted Expert Program environment adapter. + integration: Host-owned scene, robot-profile, and runtime-preset choice. + validation_context: Optional provider-free static reference validator. + max_bytes: Maximum UTF-8 encoded response size. + + Returns: + Provider-free program produced by the existing Expert Program compiler. + + Raises: + TypeError: If ``adapter`` or ``integration`` has the wrong exact type. + ExpertProgramDecodeError: If JSON, schema, or MLLM policy validation + fails. + """ + if type(adapter) is not ExpertProgramEnvironmentAdapter: + raise TypeError("adapter must be exactly ExpertProgramEnvironmentAdapter.") + config = decode_mllm_expert_program( + response, + integration=integration, + validation_context=validation_context, + max_bytes=max_bytes, + ) + return adapter.compile(config) diff --git a/embodichain/data/assets/obj_assets.py b/embodichain/data/assets/obj_assets.py index 939464720..a1b77d023 100644 --- a/embodichain/data/assets/obj_assets.py +++ b/embodichain/data/assets/obj_assets.py @@ -240,3 +240,59 @@ def __init__(self, data_root: str = None): path = EMBODICHAIN_DEFAULT_DATA_ROOT if data_root is None else data_root super().__init__(prefix, data_descriptor, path) + + +class MicrowaveOven(EmbodiChainDataset): + """get_data_path("MicrowaveOven/microwave_oven.urdf")""" + + def __init__(self, data_root: str = None): + data_descriptor = o3d.data.DataDescriptor( + os.path.join(EMBODICHAIN_DOWNLOAD_PREFIX, obj_assets, "MicrowaveOven.zip"), + "5672da2d5a888a12469d6277636646b0", + ) + prefix = type(self).__name__ + path = EMBODICHAIN_DEFAULT_DATA_ROOT if data_root is None else data_root + + super().__init__(prefix, data_descriptor, path) + + +class PlasticTray(EmbodiChainDataset): + """get_data_path("PlasticTray/plastic_tray.glb")""" + + def __init__(self, data_root: str = None): + data_descriptor = o3d.data.DataDescriptor( + os.path.join(EMBODICHAIN_DOWNLOAD_PREFIX, obj_assets, "PlasticTray.zip"), + "66f1f8a507052f9e33be5433fa2a2667", + ) + prefix = type(self).__name__ + path = EMBODICHAIN_DEFAULT_DATA_ROOT if data_root is None else data_root + + super().__init__(prefix, data_descriptor, path) + + +class WaterBasin(EmbodiChainDataset): + """get_data_path("WaterBasin/water_basin.glb")""" + + def __init__(self, data_root: str = None): + data_descriptor = o3d.data.DataDescriptor( + os.path.join(EMBODICHAIN_DOWNLOAD_PREFIX, obj_assets, "WaterBasin.zip"), + "9ae41630f6f52dccd7b95ab21b6ba989", + ) + prefix = type(self).__name__ + path = EMBODICHAIN_DEFAULT_DATA_ROOT if data_root is None else data_root + + super().__init__(prefix, data_descriptor, path) + + +class Drawer(EmbodiChainDataset): + """get_data_path("Drawer/model_split_links_with_inertials.urdf")""" + + def __init__(self, data_root: str = None): + data_descriptor = o3d.data.DataDescriptor( + os.path.join(EMBODICHAIN_DOWNLOAD_PREFIX, obj_assets, "Drawer.zip"), + "eba30c852074388c2e5b634b1ae37572", + ) + prefix = type(self).__name__ + path = EMBODICHAIN_DEFAULT_DATA_ROOT if data_root is None else data_root + + super().__init__(prefix, data_descriptor, path) diff --git a/embodichain/lab/gym/envs/__init__.py b/embodichain/lab/gym/envs/__init__.py index 14c7e98bf..af28a8dc8 100644 --- a/embodichain/lab/gym/envs/__init__.py +++ b/embodichain/lab/gym/envs/__init__.py @@ -21,10 +21,9 @@ from .base_env import * from .demo import * from .embodied_env import * +from .settling import * from .wrapper import * -# Official task environments live in the bundled ``embodichain_tasks`` import -# package (alongside any third-party ``embodichain.tasks`` entry points). -# They are no longer re-exported here so that importing the core envs package -# stays warning-free. Direct imports from ``embodichain.lab.gym.envs.tasks`` -# still work via the deprecation shim in ``tasks/__init__.py``. +# Official task environments live in the bundled ``embodichain_tasks`` package +# and register through the same ``embodichain.tasks`` entry-point mechanism as +# third-party task packages. diff --git a/embodichain/lab/gym/envs/base_env.py b/embodichain/lab/gym/envs/base_env.py index 9e5d78e2e..8e0c6207b 100644 --- a/embodichain/lab/gym/envs/base_env.py +++ b/embodichain/lab/gym/envs/base_env.py @@ -17,7 +17,8 @@ from __future__ import annotations import math -from numbers import Integral +from collections.abc import Mapping +from numbers import Integral, Real import torch import numpy as np @@ -124,6 +125,11 @@ class BaseEnv(gym.Env): single_action_space: gym.spaces.Space = None single_observation_space: gym.spaces.Space = None + # EmbodiedEnv defers the summary until all managers and recording buffers + # have been initialized. + _defer_initialization_summary: bool = False + _initialization_summary_label_width: int = 22 + def __init__( self, cfg: EnvCfg, @@ -188,13 +194,110 @@ def __init__( self._init_raw_obs: Dict = self.get_obs(**kwargs) - logger.log_info("[INFO]: Initialized environment:") - logger.log_info(f"\tEnvironment device : {self.sim.device}") - logger.log_info(f"\tNumber of environments: {self._num_envs}") - logger.log_info(f"\tEnvironment seed : {self.cfg.seed}") - logger.log_info(f"\tPhysics dt : {self.physics_dt}") - logger.log_info(f"\tEnvironment dt : {self.step_dt}") - logger.log_info(f"\tControl frequency : {self.control_frequency} Hz") + if not self._defer_initialization_summary: + self._log_initialization_summary() + + def _log_initialization_summary(self) -> None: + """Log the environment initialization summary without log prefixes.""" + logger.log_info("\n".join(self._initialization_summary_lines()), prefix=False) + + def _initialization_summary_lines(self) -> list[str]: + """Build a compact, structured summary of the initialized environment.""" + robot_description = type(self.robot).__name__ + robot_uid = getattr(self.robot, "uid", None) + if robot_uid: + robot_description = f"{robot_description} (uid={robot_uid})" + + sensor_names = [str(name) for name in self.sensors] + sensor_description = ( + f"{len(sensor_names)} ({', '.join(sensor_names)})" + if sensor_names + else "none" + ) + episode_limit = ( + f"{self.cfg.max_episode_steps} control steps" + if self.cfg.max_episode_steps > 0 + else "unlimited" + ) + + lines = [ + f"╭─ Environment initialized: {type(self).__name__}", + "├─ Runtime", + self._format_initialization_summary_row("Config", type(self.cfg).__name__), + self._format_initialization_summary_row("Device", self.device), + self._format_initialization_summary_row( + "Parallel environments", self.num_envs + ), + self._format_initialization_summary_row( + "Seed", self.cfg.seed if self.cfg.seed is not None else "not set" + ), + self._format_initialization_summary_row( + "Headless", str(bool(self.sim_cfg.headless)).lower() + ), + self._format_initialization_summary_row("Robot", robot_description), + self._format_initialization_summary_row("Sensors", sensor_description), + "├─ Timing", + self._format_initialization_summary_row( + "Physics", + f"{self.physics_dt:g} s ({self.physics_frequency:g} Hz)", + ), + self._format_initialization_summary_row( + "Control", + f"{self.step_dt:g} s ({self.control_frequency:g} Hz, " + f"{self.cfg.sim_steps_per_control} physics steps)", + ), + self._format_initialization_summary_row("Episode limit", episode_limit), + ] + + summary_metadata = [ + (name, value) + for name, value in self.metadata.items() + if name != "render_fps" + ] + if summary_metadata: + lines.append("├─ Metadata") + for name, value in sorted(summary_metadata, key=lambda item: str(item[0])): + lines.append( + self._format_initialization_summary_row( + str(name), self._format_initialization_metadata_value(value) + ) + ) + + lines.extend(self._extra_initialization_summary_lines()) + lines.append("╰─ Ready") + return lines + + def _extra_initialization_summary_lines(self) -> list[str]: + """Return subclass-specific initialization summary lines.""" + return [] + + @classmethod + def _format_initialization_summary_row( + cls, label: str, value: object, indent: int = 0 + ) -> str: + """Format an aligned key-value row inside the initialization tree.""" + label_width = max(1, cls._initialization_summary_label_width - 2 * indent) + return f"│ {' ' * indent}{label:<{label_width}} {value}" + + @staticmethod + def _format_initialization_metadata_value(value: object) -> str: + """Format metadata without expanding large nested structures.""" + if value is None: + return "none" + if isinstance(value, bool): + return str(value).lower() + if isinstance(value, Real): + return f"{value:g}" + if isinstance(value, str): + return value if len(value) <= 80 else f"{value[:77]}..." + if isinstance(value, Mapping): + keys = ", ".join(sorted(str(key) for key in value)) + noun = "key" if len(value) == 1 else "keys" + return f"{len(value)} {noun}" + (f" ({keys})" if keys else "") + if isinstance(value, (list, tuple, set, frozenset)): + noun = "item" if len(value) == 1 else "items" + return f"{len(value)} {noun}" + return type(value).__name__ def _configure_timing(self) -> None: """Validate and expose the environment's simulation-derived timing.""" diff --git a/embodichain/lab/gym/envs/demo.py b/embodichain/lab/gym/envs/demo.py index 695500a8d..dc3be1ef6 100644 --- a/embodichain/lab/gym/envs/demo.py +++ b/embodichain/lab/gym/envs/demo.py @@ -20,9 +20,14 @@ from collections.abc import Callable, Iterable, Mapping from dataclasses import dataclass, field, replace -from typing import Any +import math +from types import MappingProxyType +from typing import Any, Literal import torch +from tensordict import TensorDict + +from embodichain.lab.sim.types import EnvAction __all__ = [ "DEMO_ANNOTATION_KEYS", @@ -30,6 +35,7 @@ "DemoEpisodeResult", "DemoSegment", "DemoSegmentResult", + "ProcessedEnvAction", "execute_demo_episode", "resolve_demo_segments", ] @@ -50,6 +56,69 @@ """Per-frame annotation keys stored in expert rollout buffers.""" +def _json_safe_copy(value: Any, *, field_name: str) -> Any: + """Return an owned JSON value without implicit type coercion.""" + if value is None or type(value) in {bool, int, str}: + return value + if type(value) is float: + if not math.isfinite(value): + raise ValueError(f"{field_name} contains a non-finite float.") + return value + if isinstance(value, Mapping): + result: dict[str, Any] = {} + for key, item in value.items(): + if type(key) is not str or not key or key != key.strip(): + raise ValueError( + f"{field_name} mapping keys must be non-empty strings " + "without outer whitespace." + ) + result[key] = _json_safe_copy( + item, + field_name=f"{field_name}.{key}", + ) + return result + if isinstance(value, (list, tuple)): + return [ + _json_safe_copy(item, field_name=f"{field_name}[{index}]") + for index, item in enumerate(value) + ] + raise TypeError(f"{field_name} contains non-JSON value {type(value).__name__}.") + + +@dataclass(frozen=True, slots=True, eq=False) +class ProcessedEnvAction: + """Owned controller-ready action that must still pass through ``env.step``. + + Semantic runtimes and demonstration bridges may already have produced the + action-manager output (for example, a full joint-position command assembled + from typed runtime endpoints). Wrapping it prevents the environment from + applying the pre-action transform a second time while retaining the normal + simulation, manager, recorder, reward, and dataset step lifecycle. + + Args: + value: Controller-ready tensor or ``TensorDict``. + metadata: JSON-compatible provenance attached by the producer. The + environment does not interpret this mapping. + """ + + value: EnvAction + metadata: Mapping[str, Any] = field(default_factory=dict) + + def __post_init__(self) -> None: + if not isinstance(self.value, (torch.Tensor, TensorDict)): + raise TypeError("value must be a torch.Tensor or TensorDict.") + if not isinstance(self.metadata, Mapping): + raise TypeError("metadata must be a mapping.") + owned_value = self.value.clone() + owned_metadata = _json_safe_copy(self.metadata, field_name="metadata") + object.__setattr__(self, "value", owned_value) + object.__setattr__(self, "metadata", MappingProxyType(owned_metadata)) + + def snapshot(self) -> ProcessedEnvAction: + """Return an independently owned processed-action envelope.""" + return ProcessedEnvAction(value=self.value, metadata=self.metadata) + + @dataclass(frozen=True) class DemoSegment: """One semantic subtask inside a demonstration episode. @@ -69,6 +138,16 @@ class DemoSegment: parallel environment (or one scalar broadcast to every environment). Gym ``terminated`` and ``truncated`` remain episode-level signals; use this callback for subtask-level validation. + abort_actions: Optional callback invoked when the executor stops after + retrieving an action but before exhausting the iterable. It receives + a reason and ``last_action_consumed`` flag, and must return any + emergency controller actions that still need ordinary ``env.step`` + consumption. This is the explicit cancellation handshake for lazy + runtimes whose command acknowledgements only mean locally buffered. + failure_policy: ``"batch_abort"`` preserves legacy batch-atomic + behavior. ``"row_independent"`` permanently freezes only failed + environment rows while peers continue through the shared segment + and later lazy segments. """ actions: Iterable[Any] @@ -77,6 +156,20 @@ class DemoSegment: instruction: str | None = None metadata: Mapping[str, Any] = field(default_factory=dict) validator: Callable[[], Any] | None = field(default=None, repr=False, compare=False) + abort_actions: Callable[..., Iterable[Any]] | None = field( + default=None, + repr=False, + compare=False, + ) + failure_policy: Literal["batch_abort", "row_independent"] = "batch_abort" + + def __post_init__(self) -> None: + if self.abort_actions is not None and not callable(self.abort_actions): + raise TypeError("abort_actions must be callable or None.") + if self.failure_policy not in {"batch_abort", "row_independent"}: + raise ValueError( + "failure_policy must be 'batch_abort' or 'row_independent'." + ) @dataclass(frozen=True) @@ -118,6 +211,15 @@ class DemoSegmentResult: successes: tuple[bool, ...] = () failure_reasons: tuple[str | None, ...] = () + def __post_init__(self) -> None: + if not isinstance(self.metadata, Mapping): + raise TypeError("metadata must be a mapping.") + owned_metadata = _json_safe_copy( + self.metadata, + field_name="segment result metadata", + ) + object.__setattr__(self, "metadata", MappingProxyType(owned_metadata)) + def to_metadata(self, env_id: int | None = None) -> dict[str, Any]: """Return a JSON-compatible aggregate or per-environment representation. @@ -133,7 +235,10 @@ def to_metadata(self, env_id: int | None = None) -> dict[str, Any]: "name": self.name, "target_uid": self.target_uid, "instruction": self.instruction, - "metadata": dict(self.metadata), + "metadata": _json_safe_copy( + self.metadata, + field_name="segment result metadata", + ), } if env_id is not None and self.start_steps: metadata.update( @@ -269,6 +374,28 @@ def _as_bool_tuple(value: Any, num_envs: int) -> tuple[bool, ...]: return tuple(bool(item) for item in tensor.tolist()) +def _has_terminal_runtime_failure_trace(segment: DemoSegment) -> bool: + """Return whether a lazy segment recorded a canonical failed runtime. + + Expert-program action iterables may terminate before yielding a controller + command when planning fails. Their bridge finalizes the runtime trace while + exhausting the iterable and exposes a validator that commits row-local + failure. This marker distinguishes that outcome from an ordinary empty + ``DemoSegment``, whose existing ``empty_segment`` guard remains unchanged. + """ + runtime = segment.metadata.get("runtime") + if not isinstance(runtime, Mapping): + return False + return ( + runtime.get("kind") + in { + "skill_result", + "parallel_skill_result", + } + and runtime.get("status") == "failed" + ) + + def _dataset_instruction(env: Any) -> str: """Return the dataset-level instruction used for legacy demo segments.""" metadata = getattr(_env_target(env), "metadata", {}) @@ -448,7 +575,20 @@ def publish_active_mask() -> None: f"{segment.name}", ) - for action in actions: + action_iterator = iter(actions) + last_action_consumed: bool | None = None + action_error: Exception | None = None + while True: + try: + action = next(action_iterator) + except StopIteration: + break + except Exception as exc: + action_error = exc + actions_exhausted = False + segment_reason = "action_generation_failed" + break + last_action_consumed = False if should_stop is not None and should_stop(): actions_exhausted = False fatal_reason = "interrupted" @@ -461,18 +601,32 @@ def publish_active_mask() -> None: publish_active_mask() break - if normalize_action is not None: - action = normalize_action(action) - if not all(active): - if mask_action is None: - raise RuntimeError( - "A vector demo environment completed asynchronously but " - "does not implement _mask_demo_action(action, active_mask)." - ) - action = mask_action(action, tuple(active)) + try: + if normalize_action is not None: + action = normalize_action(action) + if not all(active): + if mask_action is None: + raise RuntimeError( + "A vector demo environment completed asynchronously " + "but does not implement " + "_mask_demo_action(action, active_mask)." + ) + action = mask_action(action, tuple(active)) + except Exception as exc: + action_error = exc + actions_exhausted = False + segment_reason = "action_processing_failed" + break active_before_step = tuple(active) - _, _, terminated_value, truncated_value, info = env.step(action) + try: + _, _, terminated_value, truncated_value, info = env.step(action) + except Exception as exc: + action_error = exc + actions_exhausted = False + segment_reason = "action_execution_failed" + break + last_action_consumed = True action_count += 1 last_info = info for env_id, was_active in enumerate(active_before_step): @@ -529,16 +683,21 @@ def publish_active_mask() -> None: step_failed = True if step_failed: - actions_exhausted = False - fatal_reason = "truncated" if active_step_truncated else "failure" - segment_reason = fatal_reason - for env_id, is_active in enumerate(active): - if is_active: - terminal_reasons[env_id] = "batch_aborted" - segment_failure_reasons[env_id] = "batch_aborted" - active[env_id] = False + if segment.failure_policy == "batch_abort": + actions_exhausted = False + fatal_reason = ( + "truncated" if active_step_truncated else "failure" + ) + segment_reason = fatal_reason + for env_id, is_active in enumerate(active): + if is_active: + terminal_reasons[env_id] = "batch_aborted" + segment_failure_reasons[env_id] = "batch_aborted" + active[env_id] = False publish_active_mask() - break + if segment.failure_policy == "batch_abort" or not any(active): + actions_exhausted = False + break publish_active_mask() if not any(active): @@ -558,7 +717,78 @@ def publish_active_mask() -> None: publish_active_mask() break - if action_count == 0 and segment_reason is None: + if not actions_exhausted: + if segment.abort_actions is not None: + reason = ( + segment_reason + or fatal_reason + or "demo segment execution stopped before exhaustion" + ) + try: + emergency_actions = segment.abort_actions( + reason, + last_action_consumed=bool(last_action_consumed), + ) + if isinstance(emergency_actions, (str, bytes)): + raise TypeError( + "abort_actions must return an iterable of actions." + ) + emergency_iterator = iter(emergency_actions) + try: + for emergency_action in emergency_iterator: + if normalize_action is not None: + emergency_action = normalize_action( + emergency_action + ) + try: + _, _, _, _, emergency_info = env.step( + emergency_action + ) + except Exception as exc: + raise RuntimeError( + "Emergency demo safe-stop action failed " + "during env.step()." + ) from exc + action_count += 1 + last_info = emergency_info + for env_id, is_participant in enumerate(participants): + if is_participant: + lengths[env_id] += 1 + finally: + close_emergency = getattr( + emergency_iterator, + "close", + None, + ) + if callable(close_emergency): + close_emergency() + finally: + close_actions = getattr(action_iterator, "close", None) + if callable(close_actions): + close_actions() + else: + close_actions = getattr(action_iterator, "close", None) + if callable(close_actions): + close_actions() + + if action_error is not None: + raise RuntimeError( + "Demo action generation, processing, or execution failed " + "after an emergency safe-stop attempt." + ) from action_error + + traced_terminal_runtime_failure = ( + action_count == 0 + and actions_exhausted + and segment_reason is None + and segment.validator is not None + and _has_terminal_runtime_failure_trace(segment) + ) + if ( + action_count == 0 + and segment_reason is None + and not traced_terminal_runtime_failure + ): fatal_reason = "empty_segment" segment_reason = fatal_reason for env_id, is_participant in enumerate(participants): @@ -588,15 +818,25 @@ def publish_active_mask() -> None: terminal_reasons[env_id] = "segment_validation_failed" if validation_failed: - fatal_reason = "segment_validation_failed" - segment_reason = fatal_reason - for env_id, is_active in enumerate(active): - if is_active: - if segment_failure_reasons[env_id] is None: - segment_failure_reasons[env_id] = "batch_aborted" - terminal_reasons[env_id] = "batch_aborted" - segment_successes[env_id] = False - active[env_id] = False + if segment.failure_policy == "batch_abort": + fatal_reason = "segment_validation_failed" + segment_reason = fatal_reason + for env_id, is_active in enumerate(active): + if is_active: + if segment_failure_reasons[env_id] is None: + segment_failure_reasons[env_id] = "batch_aborted" + terminal_reasons[env_id] = "batch_aborted" + segment_successes[env_id] = False + active[env_id] = False + else: + for env_id, is_active in enumerate(active): + if ( + is_active + and segment_failure_reasons[env_id] + == "segment_validation_failed" + ): + segment_successes[env_id] = False + active[env_id] = False publish_active_mask() participant_ids = [ diff --git a/embodichain/lab/gym/envs/embodied_env.py b/embodichain/lab/gym/envs/embodied_env.py index 51b2fa966..db40d02b7 100644 --- a/embodichain/lab/gym/envs/embodied_env.py +++ b/embodichain/lab/gym/envs/embodied_env.py @@ -27,7 +27,17 @@ import gymnasium as gym from dataclasses import MISSING -from typing import Dict, Union, Sequence, Tuple, Any, Iterable, List, Optional +from typing import ( + TYPE_CHECKING, + Dict, + Union, + Sequence, + Tuple, + Any, + Iterable, + List, + Optional, +) from tensordict import TensorDict from embodichain.lab.sim.cfg import ( @@ -52,6 +62,7 @@ DemoEpisodeResult, DemoSegment, DemoSegmentResult, + ProcessedEnvAction, ) from embodichain.lab.gym.envs.managers import ( EventManager, @@ -70,6 +81,13 @@ from embodichain.data import get_data_path from embodichain.data.constants import EMBODICHAIN_DEFAULT_DATA_ROOT +if TYPE_CHECKING: + from embodichain.lab.gym.envs.expert_program import ( + CompiledProgram, + ExpertProgramCfg, + ) + from embodichain.lab.gym.envs.expert_program.bridge import AtomicDemoBridge + __all__ = ["EmbodiedEnvCfg", "EmbodiedEnv"] @@ -240,6 +258,14 @@ class EnvLightCfg: """If True (and record_trajectory is True), auto-save each env's trajectory to ``trajectory_save_dir`` at episode end and on close().""" + expert_program: ExpertProgramCfg | None = None + """Optional declarative Expert Program used to generate demo segments. + + The program remains inert until :meth:`EmbodiedEnv.create_demo_segments` + requests an explicit environment compiler and bridge through the dedicated + hooks. No live provider, planner, or callable is stored in this config. + """ + @register_env("EmbodiedEnv-v1") class EmbodiedEnv(BaseEnv): @@ -266,6 +292,15 @@ class EmbodiedEnv(BaseEnv): - affordance_datas: The affordance data that can be used to store the intermediate results or information """ + _defer_initialization_summary: bool = True + _manager_summary_fields: tuple[tuple[str, str], ...] = ( + ("EventManager", "event_manager"), + ("ObservationManager", "observation_manager"), + ("RewardManager", "reward_manager"), + ("ActionManager", "action_manager"), + ("DatasetManager", "dataset_manager"), + ) + @classmethod def __init_subclass__(cls, **kwargs): """Automatically wrap subclass demo-action builders with shape checks. @@ -389,6 +424,87 @@ def __init__(self, cfg: EmbodiedEnvCfg, **kwargs): all_env_ids = torch.arange(self.num_envs, device=self.device) self._seed_recording_state(self._init_raw_obs, all_env_ids) + self._log_initialization_summary() + + def _extra_initialization_summary_lines(self) -> list[str]: + """Build manager and functor details for the initialization summary.""" + manager_summaries: list[tuple[str, list[tuple[str, list[str]]] | None, int]] = ( + [] + ) + active_manager_count = 0 + total_functor_count = 0 + + for manager_name, attribute_name in self._manager_summary_fields: + manager = getattr(self, attribute_name, None) + if manager is None: + manager_summaries.append((manager_name, None, 0)) + continue + + groups = self._manager_functor_groups(manager_name, manager) + functor_count = sum(len(names) for _, names in groups) + manager_summaries.append((manager_name, groups, functor_count)) + active_manager_count += 1 + total_functor_count += functor_count + + functor_noun = "functor" if total_functor_count == 1 else "functors" + lines = [ + f"├─ Managers ({active_manager_count}/{len(manager_summaries)} active, " + f"{total_functor_count} {functor_noun})" + ] + for manager_name, groups, functor_count in manager_summaries: + if groups is None: + lines.append( + self._format_initialization_summary_row(manager_name, "disabled") + ) + continue + + manager_functor_noun = "functor" if functor_count == 1 else "functors" + lines.append( + self._format_initialization_summary_row( + manager_name, f"{functor_count} {manager_functor_noun}" + ) + ) + for mode, names in groups: + lines.append( + self._format_initialization_summary_row( + mode, ", ".join(names), indent=1 + ) + ) + return lines + + @staticmethod + def _manager_functor_groups( + manager_name: str, manager: object + ) -> list[tuple[str, list[str]]]: + """Normalize a manager's active functors into display groups.""" + active_functors = manager.active_functors + if isinstance(active_functors, Mapping): + return [ + (str(mode), [str(name) for name in names]) + for mode, names in active_functors.items() + ] + + names = [str(name) for name in active_functors] + if manager_name != "ActionManager": + return [("terms", names)] if names else [] + + get_terms_by_mode = getattr(manager, "get_terms_by_mode", None) + if not callable(get_terms_by_mode): + return [("terms", names)] if names else [] + + groups: list[tuple[str, list[str]]] = [] + grouped_names: set[str] = set() + for mode in ("pre", "post"): + mode_names = [str(name) for name, _ in get_terms_by_mode(mode)] + if mode_names: + groups.append((mode, mode_names)) + grouped_names.update(mode_names) + + remaining_names = [name for name in names if name not in grouped_names] + if remaining_names: + groups.append(("terms", remaining_names)) + return groups + def reset( self, seed: int | None = None, options: dict | None = None ) -> tuple[EnvObs, Dict]: @@ -1248,14 +1364,30 @@ def _write_rl_rollout_step( : self.num_envs, self.current_rollout_step ].copy_(truncateds.to(buffer_device), non_blocking=True) - def _normalize_demo_action(self, action: EnvAction) -> EnvAction: - """Normalize one legacy or segment action to the environment action space.""" + def _normalize_demo_action( + self, action: EnvAction | ProcessedEnvAction + ) -> EnvAction | ProcessedEnvAction: + """Normalize one raw action or preserve a controller-ready envelope.""" + if isinstance(action, ProcessedEnvAction): + value = action.value + if value.ndim == 0: + raise ValueError( + "Processed demo actions must have a leading environment " + "dimension." + ) + if value.shape[0] != self.num_envs: + raise ValueError( + "Processed demo action batch size must match num_envs." + ) + return action.snapshot() expected_dim = int(np.prod(self.single_action_space.shape)) return self._normalize_demo_action_tensor(action, expected_dim) def _mask_demo_action( - self, action: EnvAction, active_mask: Sequence[bool] - ) -> EnvAction: + self, + action: EnvAction | ProcessedEnvAction, + active_mask: Sequence[bool], + ) -> EnvAction | ProcessedEnvAction: """Accept an asynchronously completed vector-demo action. Raw actions may still require :class:`ActionManager` preprocessing, so @@ -1548,15 +1680,18 @@ def evaluate(self, **kwargs) -> Dict[str, Any]: eval_dict[key] = value return eval_dict - def _preprocess_action(self, action: EnvAction) -> EnvAction: - """Delegate to ActionManager when configured; stash raw action for trajectory.""" + def _preprocess_action(self, action: EnvAction | ProcessedEnvAction) -> EnvAction: + """Apply raw preprocessing once and stash the executed controller action.""" + is_processed = isinstance(action, ProcessedEnvAction) + if is_processed: + action = action.value if self._traj_buffer is not None: self._traj_raw_action = ( action.clone() if hasattr(action, "clone") else action ) - if self.action_manager is not None: + if self.action_manager is not None and not is_processed: action = self.action_manager.process_action(action, mode="pre") - else: + elif not is_processed: action = super()._preprocess_action(action) if getattr(self, "_demo_no_auto_reset", False): action = self._mask_processed_demo_action(action) @@ -1763,13 +1898,65 @@ def create_demo_action_list(self, *args, **kwargs) -> Sequence[EnvAction] | None "The method 'create_demo_action_list' must be implemented in subclasses." ) + def compile_expert_program( + self, + program: ExpertProgramCfg, + ) -> CompiledProgram: + """Compile a configured Expert Program through explicit scene providers. + + Declarative environments override this hook to supply their authoritative + scene registry/resolver to :class:`ExpertProgramCompiler`. Keeping the + provider boundary explicit prevents the base environment from inferring + identities or scanning mutable simulator internals. + + Args: + program: Strict Expert Program configuration attached to ``cfg``. + + Returns: + Provider-free compiled program ready for runtime assembly. + + Raises: + NotImplementedError: If an environment enables ``expert_program`` + without supplying the compiler/provider integration. + """ + raise NotImplementedError( + "An environment with cfg.expert_program must implement " + "compile_expert_program() using an explicit scene resolver." + ) + + def create_expert_program_bridge( + self, + program: CompiledProgram, + ) -> AtomicDemoBridge: + """Create the Gym demo bridge through explicit runtime-port factories. + + Declarative environments override this hook to assemble ``SkillRuntime`` + and Gym-aware command/clock/post-policy/validator ports. The returned + bridge must emit commands through normal ``env.step()`` processing. + + Args: + program: Compiled provider-free Expert Program. + + Returns: + Atomic demo bridge whose segments are consumed lazily. + + Raises: + NotImplementedError: If no explicit runtime factory is available. + """ + raise NotImplementedError( + "An environment with cfg.expert_program must implement " + "create_expert_program_bridge() using explicit runtime ports." + ) + def create_demo_segments(self, *args, **kwargs) -> Iterable[DemoSegment] | None: """Create the semantic segments that make up one task episode. - The default adapter preserves existing tasks by wrapping their single - ``create_demo_action_list`` result in one segment. Multi-object tasks - should override this method and may return a lazy generator so each - segment can be planned from the scene state left by the previous one. + When ``cfg.expert_program`` is configured, the environment compiles it + through an explicit scene-provider hook and creates an atomic demo bridge + through an explicit runtime-port factory hook. Otherwise, the default + adapter wraps ``create_demo_action_list`` in one segment. Multi-object + tasks may return a lazy generator so each segment can be planned from + the scene state left by the previous one. Args: *args: Positional arguments forwarded to the legacy planner. @@ -1778,6 +1965,12 @@ def create_demo_segments(self, *args, **kwargs) -> Iterable[DemoSegment] | None: Returns: Segment sequence, or ``None`` when planning fails. """ + expert_program = getattr(getattr(self, "cfg", None), "expert_program", None) + if expert_program is not None: + compiled_program = self.compile_expert_program(expert_program) + bridge = self.create_expert_program_bridge(compiled_program) + return bridge.iter_segments() + actions = self.create_demo_action_list(*args, **kwargs) if actions is None: return None diff --git a/embodichain/lab/gym/envs/expert_program/__init__.py b/embodichain/lab/gym/envs/expert_program/__init__.py new file mode 100644 index 000000000..e22b51b0a --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/__init__.py @@ -0,0 +1,295 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Versioned declarative Expert Program schema, compiler, and runtime types.""" + +from __future__ import annotations + +from .cfg import ( + EXPERT_PROGRAM_SCHEMA_VERSION, + EXPERT_PROGRAM_SCHEMA_VERSION_V2, + MAX_DECLARATIVE_DEPTH, + MAX_DECLARATIVE_NODES, + MAX_EXPANDED_CALLS, + MAX_PROGRAM_DEPTH, + MAX_PROGRAM_NODES, + MAX_REPEAT_COUNT, + SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS, + BarrierCfg, + CyclicPoseTargetCfg, + DeclarativeCfgValue, + ExpertProgramCfg, + ExpertProgramIntegrationCfg, + HandOverCfg, + InvokeCfg, + ObjectNearTargetValidatorCfg, + OperateArticulationCfg, + ParallelCfg, + PickCfg, + PlaceCfg, + PoseCfg, + PostPolicyCfg, + ProgramNodeCfg, + RegisteredSemanticCallCfg, + RepeatCfg, + SegmentCfg, + SemanticCallCfg, + SequenceCfg, + TargetCfg, + TargetRefCfg, + ValidatorCfg, + WaitStablePostCfg, +) +from .decoder import ( + ConfigPath, + ConfigPathPart, + ExpertProgramConfigError, + ExpertProgramDecodeError, + ExpertProgramValidationContext, + ExpertProgramValidationError, + SceneReferenceRole, + decode_expert_program, + decode_semantic_call, + encode_semantic_call, + render_config_path, + validate_expert_program, +) +from .loader import ( + MAX_EXPERT_PROGRAM_BYTES, + load_expert_program, + loads_expert_program_json, + parse_expert_program_json, +) +from .bridge import ( + AcceptedRuntimeCommandObserver, + AtomicDemoBridge, + BufferedGymCommandSink, + DemoBridgeError, + EnvironmentStepClock, + EnvironmentStepTimingError, + GymPlanningObservationProvider, + RuntimeCommandFrameEncoder, + RuntimeTransportActionEncoder, + SegmentPostPolicyMetadataPort, + SegmentPostPolicyPort, + SegmentPostPolicyResultPort, + SegmentValidatorMetadataPort, + SegmentValidatorPort, + UnsupportedRuntimeTransportError, +) +from .compiler import ( + CompiledBarrier, + CompiledParallelBlock, + CompiledParallelBranch, + CompiledPostPolicy, + CompiledProgram, + CompiledProgramAnalysis, + CompiledProgramCall, + CompiledProgramSegment, + CompiledProgramValidator, + CompiledRepeatFrame, + CompiledTargetSelection, + ExpertProgramCompileError, + ExpertProgramCompiler, + ExpertProgramSceneResolver, + MaterializedCompiledProgram, + SceneRegistryProgramResolver, +) +from .environment import ( + AcceptedRuntimeCommandObserverFactory, + ExpertProgramEnvironmentAdapter, + ExpertProgramEnvironmentFactory, + ExpertProgramEnvironmentMixin, + ExpertProgramRuntimeAssembly, + PlanningObservationPort, + SkillRuntimeAssemblyPort, +) +from .simulation import ( + AntipodalGraspAffordanceBinding, + ArticulationOperationAffordanceBinding, + ArticulationOperationTargetBinding, + ContainerAffordanceBinding, + ControlPartCommandPreset, + ControlPartEndpointBinding, + ControlPartResourceBinding, + RobotResourceBinding, + SimulationArticulationBinding, + SimulationArticulationLinkBinding, + SimulationRigidObjectBinding, + SimulationResourceEndpointBinding, + SimulationRobotResourceBinding, + SimulationRobotSkillProfileBinding, + SimulationSceneBinding, + SupportSurfaceAffordanceBinding, +) +from .catalog import ( + ExpertProgramIntegrationCatalog, + IntegrationFingerprintMismatch, + SimulationExpertProgramRegistration, +) +from .extensions import ( + EndpointAdapterDeclaration, + ParallelCommandSafetyValidatorFactory, + ParallelSafetyDeclaration, + RuntimeTransportDeclaration, + StandardExtensionDeclarations, + VersionedKey, +) +from .simulation_environment import ( + ControlCommandStateEvidenceTracker, + MotionGeneratorFactory, + SharedTickSceneProvider, + SimulationExpertProgramEnvironment, + SimulationExpertProgramFactory, + SimulationPlanningObservationProvider, + create_simulation_expert_program_adapter, +) +from .simulation_handover import ConfiguredHandOverPoseProvider +from .simulation_parallel_safety import ( + CuroboParallelCommandSafetyValidator, + CuroboParallelSafetyValidatorFactory, +) +from .simulation_policies import ( + SimulationSegmentPolicyPort, + default_simulation_settle_presets, +) + +__all__ = [ + "AcceptedRuntimeCommandObserver", + "AcceptedRuntimeCommandObserverFactory", + "AntipodalGraspAffordanceBinding", + "ArticulationOperationAffordanceBinding", + "ArticulationOperationTargetBinding", + "AtomicDemoBridge", + "BarrierCfg", + "BufferedGymCommandSink", + "ConfigPath", + "ConfigPathPart", + "CompiledBarrier", + "CompiledParallelBlock", + "CompiledParallelBranch", + "CompiledPostPolicy", + "CompiledProgram", + "CompiledProgramAnalysis", + "CompiledProgramCall", + "CompiledProgramSegment", + "CompiledProgramValidator", + "CompiledRepeatFrame", + "CompiledTargetSelection", + "ContainerAffordanceBinding", + "ControlCommandStateEvidenceTracker", + "ControlPartCommandPreset", + "ControlPartEndpointBinding", + "ControlPartResourceBinding", + "ConfiguredHandOverPoseProvider", + "CyclicPoseTargetCfg", + "CuroboParallelCommandSafetyValidator", + "CuroboParallelSafetyValidatorFactory", + "DeclarativeCfgValue", + "DemoBridgeError", + "EXPERT_PROGRAM_SCHEMA_VERSION", + "EXPERT_PROGRAM_SCHEMA_VERSION_V2", + "EnvironmentStepClock", + "EnvironmentStepTimingError", + "EndpointAdapterDeclaration", + "ExpertProgramCfg", + "ExpertProgramCompileError", + "ExpertProgramCompiler", + "ExpertProgramConfigError", + "ExpertProgramDecodeError", + "ExpertProgramEnvironmentAdapter", + "ExpertProgramEnvironmentFactory", + "ExpertProgramEnvironmentMixin", + "ExpertProgramIntegrationCfg", + "ExpertProgramIntegrationCatalog", + "ExpertProgramRuntimeAssembly", + "ExpertProgramSceneResolver", + "ExpertProgramValidationContext", + "ExpertProgramValidationError", + "HandOverCfg", + "GymPlanningObservationProvider", + "InvokeCfg", + "IntegrationFingerprintMismatch", + "MAX_DECLARATIVE_DEPTH", + "MAX_DECLARATIVE_NODES", + "MAX_EXPANDED_CALLS", + "MAX_EXPERT_PROGRAM_BYTES", + "MAX_PROGRAM_DEPTH", + "MAX_PROGRAM_NODES", + "MAX_REPEAT_COUNT", + "MaterializedCompiledProgram", + "MotionGeneratorFactory", + "ObjectNearTargetValidatorCfg", + "OperateArticulationCfg", + "ParallelCfg", + "ParallelCommandSafetyValidatorFactory", + "ParallelSafetyDeclaration", + "PickCfg", + "PlaceCfg", + "PlanningObservationPort", + "PoseCfg", + "PostPolicyCfg", + "ProgramNodeCfg", + "RegisteredSemanticCallCfg", + "RepeatCfg", + "RobotResourceBinding", + "RuntimeCommandFrameEncoder", + "RuntimeTransportDeclaration", + "RuntimeTransportActionEncoder", + "SceneReferenceRole", + "SceneRegistryProgramResolver", + "SegmentPostPolicyMetadataPort", + "SegmentPostPolicyPort", + "SegmentPostPolicyResultPort", + "SegmentCfg", + "SegmentValidatorMetadataPort", + "SegmentValidatorPort", + "SemanticCallCfg", + "SequenceCfg", + "SharedTickSceneProvider", + "SkillRuntimeAssemblyPort", + "SimulationArticulationBinding", + "SimulationArticulationLinkBinding", + "SimulationExpertProgramEnvironment", + "SimulationExpertProgramFactory", + "SimulationExpertProgramRegistration", + "SimulationPlanningObservationProvider", + "SimulationRigidObjectBinding", + "SimulationResourceEndpointBinding", + "SimulationRobotResourceBinding", + "SimulationRobotSkillProfileBinding", + "SimulationSceneBinding", + "SimulationSegmentPolicyPort", + "StandardExtensionDeclarations", + "SupportSurfaceAffordanceBinding", + "SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS", + "TargetCfg", + "TargetRefCfg", + "UnsupportedRuntimeTransportError", + "ValidatorCfg", + "VersionedKey", + "WaitStablePostCfg", + "create_simulation_expert_program_adapter", + "default_simulation_settle_presets", + "decode_expert_program", + "decode_semantic_call", + "encode_semantic_call", + "load_expert_program", + "loads_expert_program_json", + "parse_expert_program_json", + "render_config_path", + "validate_expert_program", +] diff --git a/embodichain/lab/gym/envs/expert_program/bridge.py b/embodichain/lab/gym/envs/expert_program/bridge.py new file mode 100644 index 000000000..aeb2506ab --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/bridge.py @@ -0,0 +1,1689 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Gym ports and a lazy demo adapter for compiled Expert Programs. + +This module deliberately stops at the Gym action boundary. It never calls +``env.step`` and never updates a simulator directly. The demo executor owns +the environment step; when it asks the action generator for the next value, +the bridge treats the previously yielded value as consumed and advances the +environment-backed execution clock by exactly one step. +""" + +from __future__ import annotations + +from collections import deque +from collections.abc import Callable, Iterable, Iterator, Mapping +from copy import deepcopy +from dataclasses import dataclass, field +import math +from typing import Any, ClassVar, Protocol, runtime_checkable + +import torch + +from embodichain.lab.gym.envs.demo import DemoSegment, ProcessedEnvAction +from embodichain.lab.sim.atomic_actions.bindings import ( + JointPositionTarget, + RuntimeEndpointTarget, +) +from embodichain.lab.sim.atomic_actions.runner import ( + CommandAcknowledgement, + ExecutionClock, + ExecutionRunnerCfg, +) +from embodichain.lab.sim.atomic_actions.runtime_commands import ( + EndpointCommand, + JointPositionPayload, + RuntimeCommandFrame, + RuntimeCommandPayload, +) +from embodichain.lab.sim.atomic_actions.state import PlanningContext, TaskState +from embodichain.lab.sim.skills.parallel import ParallelTimingPolicy +from embodichain.lab.sim.skills.parallel_runtime import ( + ParallelCommandSafetyValidator, + ParallelSkillResult, + ParallelSkillRuntime, +) +from embodichain.lab.sim.skills.runtime import SkillResult, SkillRuntime, SkillStatus +from embodichain.lab.sim.types import EnvAction + +_SAFE_HOLD_ACTION_KINDS = frozenset( + {"runtime_safe_hold", "runtime_wait_hold", "runtime_abort_safe_hold"} +) + + +class DemoBridgeError(RuntimeError): + """Base error raised by the Expert Program Gym bridge.""" + + +class EnvironmentStepTimingError(DemoBridgeError, ValueError): + """Raised when runtime timing cannot be represented on the Gym step grid.""" + + +class UnsupportedRuntimeTransportError(DemoBridgeError, LookupError): + """Raised when a command frame names an unregistered transport.""" + + +def _validate_identifier(value: str, *, field_name: str) -> str: + """Validate and return one strict identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +def _validate_timeout(timeout: float) -> None: + """Validate a runner-supplied acknowledgement timeout.""" + if not isinstance(timeout, (int, float)) or isinstance(timeout, bool): + raise TypeError("timeout must be a real number.") + if not math.isfinite(float(timeout)) or float(timeout) <= 0.0: + raise ValueError("timeout must be finite and positive.") + + +@runtime_checkable +class CurrentQposProvider(Protocol): + """Source of full robot positions aligned to explicit environment IDs.""" + + def current_qpos(self, env_ids: torch.Tensor) -> torch.Tensor: + """Return ``(batch_size, robot_dof)`` positions for ``env_ids``.""" + + +@runtime_checkable +class RuntimeTransportActionEncoder(Protocol): + """Extensible lowering boundary for one runtime transport kind. + + An encoder receives the action produced by earlier registered transports + and returns the next owned action value. This permits a future transport + to promote the built-in tensor action to a ``TensorDict`` when the Gym + action manager exposes a structured controller boundary. + """ + + transport_id: ClassVar[str] + """Exact runtime transport ID handled by this encoder.""" + + target_types: ClassVar[tuple[type[RuntimeEndpointTarget], ...]] + """Exact runtime-target types accepted by this encoder.""" + + payload_types: ClassVar[tuple[type[RuntimeCommandPayload], ...]] + """Exact runtime-payload types accepted by this encoder.""" + + def encode( + self, + command: EndpointCommand, + *, + base_action: EnvAction, + active_mask: torch.Tensor, + ) -> EnvAction: + """Merge one addressed command into ``base_action``.""" + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + base_action: EnvAction, + context: PlanningContext, + ) -> EnvAction: + """Merge this transport's self-proven safe hold into ``base_action``. + + The transport remains authoritative for neutralizing its own controller; + parallel command validation does not replace this transport-specific hold + contract. + """ + + +@runtime_checkable +class AcceptedRuntimeCommandObserver(Protocol): + """Transactional observer of commands accepted by the buffered Gym sink. + + Implementations may maintain runtime-local evidence state, but must not + control the robot or advance the environment. ``accepted`` is called only + after a complete frame was encoded and appended to the local buffer. + Cancellation and discard notifications are fail-closed reset boundaries. + """ + + def accepted(self, command: RuntimeCommandFrame) -> None: + """Record one independently owned accepted command frame.""" + + def cancelled(self, targets: tuple[RuntimeEndpointTarget, ...]) -> None: + """Clear state owned by the cancelled endpoint targets.""" + + def discarded(self) -> None: + """Clear every runtime-local state value after a buffer discard.""" + + +@runtime_checkable +class CompiledProgramPort(Protocol): + """Minimal provider-free compiled-program surface consumed by the bridge.""" + + schema_version: int + program_id: str + + def iter_segments(self) -> Iterator[Any]: + """Lazily yield compiled logical segments.""" + + def sequential_execution_analysis(self, segment_index: int) -> Any: + """Return current prefix plus downstream calls up to the next barrier.""" + + +@runtime_checkable +class SequentialSkillRuntimePort(Protocol): + """Nonblocking semantic runtime surface used by sequential segments.""" + + @property + def result(self) -> SkillResult: + """Return the current immutable runtime result.""" + + @property + def status(self) -> SkillStatus: + """Return the current runtime status.""" + + def start( + self, + *calls: Any, + workflow_id: str = "semantic_workflow", + eligible_mask: torch.Tensor | None = None, + execution_prefix_length: int | None = None, + ) -> SkillResult: + """Start one semantic workflow without blocking on motion.""" + + def step(self) -> SkillResult: + """Advance the workflow by at most one due runner cycle.""" + + def cancel(self, reason: str) -> SkillResult: + """Cancel one running workflow through the runner's safe-stop path.""" + + def adopt_verified_task_state(self, task_state: TaskState) -> SkillResult: + """Install state merged at an independent parallel barrier.""" + + +@runtime_checkable +class SegmentPostPolicyPort(Protocol): + """Environment-aware program post-policy boundary. + + Implementations may observe the environment after each resumed yield, but + must return every controller action to this iterable. The bridge then + routes those values through the ordinary demo executor and ``env.step``. + """ + + def validate_policy( + self, + policy: Any, + *, + segment: Any, + ) -> None: + """Validate one compiled policy without live observation or action.""" + + def actions( + self, + policy: Any, + *, + segment: Any, + active_mask: torch.Tensor, + ) -> Iterable[Any]: + """Yield holds until ``policy`` completes for the active rows only.""" + + +@runtime_checkable +class SegmentPostPolicyMetadataPort(Protocol): + """Optional result trace supplied by a segment post-policy port.""" + + def post_policy_metadata( + self, + policy: Any, + *, + segment: Any, + ) -> Mapping[str, Any]: + """Return JSON-safe metadata after one policy has run.""" + + +@runtime_checkable +class SegmentPostPolicyResultPort(Protocol): + """Optional row-local success result supplied by a post-policy port.""" + + def post_policy_result(self, policy: Any, *, segment: Any) -> Any: + """Return one boolean or one boolean per environment row.""" + + +@runtime_checkable +class SegmentValidatorPort(Protocol): + """Environment-aware boundary for compiled program validators.""" + + def validate_validator( + self, + validator: Any, + *, + segment: Any, + ) -> None: + """Validate one compiled validator without observing the environment.""" + + def validate(self, validator: Any, *, segment: Any) -> Any: + """Return one boolean or one boolean per environment row.""" + + +@runtime_checkable +class SegmentValidatorMetadataPort(Protocol): + """Optional result trace supplied by a segment validator port.""" + + def validator_metadata( + self, + validator: Any, + *, + segment: Any, + ) -> Mapping[str, Any]: + """Return JSON-safe metadata after one validator has run.""" + + +class GymPlanningObservationProvider: + """Callback-backed observation port that also exposes the latest qpos. + + Args: + capture: Callback accepting verified :class:`TaskState` and returning + one fresh :class:`PlanningContext` from the Gym environment. + + The callback is intentionally explicit: environment-specific scene, + simulator, and registry access remains in environment integration code. + """ + + def __init__(self, capture: Callable[[TaskState], PlanningContext]) -> None: + if not callable(capture): + raise TypeError("capture must be callable.") + self._capture = capture + self._latest: PlanningContext | None = None + + @property + def latest(self) -> PlanningContext | None: + """Return the latest immutable planning context, if one was captured.""" + return self._latest + + def observe(self, task_state: TaskState) -> PlanningContext: + """Capture and retain one fresh planning context.""" + if not isinstance(task_state, TaskState): + raise TypeError("task_state must be a TaskState.") + context = self._capture(task_state) + if not isinstance(context, PlanningContext): + raise TypeError("capture must return a PlanningContext.") + self._latest = context + return context + + def current_qpos(self, env_ids: torch.Tensor) -> torch.Tensor: + """Return latest full qpos rows in the requested stable-ID order.""" + context = self._latest + if context is None: + raise RuntimeError("No planning context has been observed yet.") + if not isinstance(env_ids, torch.Tensor): + raise TypeError("env_ids must be a torch.Tensor.") + if env_ids.dtype != torch.long or env_ids.dim() != 1 or env_ids.numel() == 0: + raise ValueError("env_ids must be a non-empty one-dimensional long tensor.") + if env_ids.device != context.env_ids.device: + raise ValueError("env_ids must share the latest context device.") + if torch.unique(env_ids).numel() != env_ids.numel(): + raise ValueError("env_ids must be unique.") + + row_by_id = { + int(env_id): row + for row, env_id in enumerate(context.env_ids.detach().cpu().tolist()) + } + try: + rows = [ + row_by_id[int(env_id)] for env_id in env_ids.detach().cpu().tolist() + ] + except KeyError as exc: + raise ValueError( + f"Environment ID {int(exc.args[0])} is absent from the latest context." + ) from exc + return context.robot.qpos[rows].clone() + + +class EnvironmentStepClock(ExecutionClock): + """Monotonic execution clock advanced only by explicit Gym steps. + + ``sleep`` intentionally raises. Calling synchronous ``SkillRuntime.run`` + with this clock would otherwise advance execution without an environment + transition. Demo integrations must use the nonblocking ``start``/``step`` + path and call :meth:`advance_after_env_step` only after a yielded action was + passed to ``env.step``. + """ + + def __init__(self, step_dt: float, *, initial_step: int = 0) -> None: + if not isinstance(step_dt, (int, float)) or isinstance(step_dt, bool): + raise TypeError("step_dt must be a real number.") + if not math.isfinite(float(step_dt)) or float(step_dt) <= 0.0: + raise ValueError("step_dt must be finite and positive.") + if type(initial_step) is not int or initial_step < 0: + raise ValueError("initial_step must be a non-negative integer.") + self._step_dt = float(step_dt) + self._step_index = initial_step + + @property + def step_dt(self) -> float: + """Return the authoritative Gym control cadence.""" + return self._step_dt + + @property + def step_index(self) -> int: + """Return the number of explicitly acknowledged environment steps.""" + return self._step_index + + def now(self) -> float: + """Return deterministic environment time in seconds.""" + return self._step_index * self._step_dt + + def sleep(self, duration: float) -> None: + """Reject implicit waiting that is not backed by ``env.step``.""" + self.steps_for_duration(duration, field_name="sleep duration") + raise RuntimeError( + "EnvironmentStepClock cannot sleep or advance implicitly; use the " + "nonblocking runtime and advance_after_env_step() after env.step()." + ) + + def steps_for_duration( + self, + duration: float, + *, + field_name: str = "duration", + ) -> int: + """Return an exact integer-grid representation of ``duration``. + + Float32 command tensors receive a small ratio-space tolerance, but an + incompatible cadence is never rounded or resampled. + """ + if not isinstance(duration, (int, float)) or isinstance(duration, bool): + raise TypeError(f"{field_name} must be a real number.") + duration = float(duration) + if not math.isfinite(duration) or duration < 0.0: + raise ValueError(f"{field_name} must be finite and non-negative.") + ratio = duration / self._step_dt + nearest = round(ratio) + tolerance = max(1.0e-6, abs(ratio) * 1.0e-6) + if not math.isclose(ratio, nearest, rel_tol=0.0, abs_tol=tolerance): + raise EnvironmentStepTimingError( + f"{field_name}={duration:.9g}s is not an integer multiple of " + f"step_dt={self._step_dt:.9g}s; explicit resampling is not supported." + ) + return int(nearest) + + def validate_frame(self, frame: RuntimeCommandFrame) -> None: + """Validate every row's command hold duration against the step grid.""" + if not isinstance(frame, RuntimeCommandFrame): + raise TypeError("frame must be a RuntimeCommandFrame.") + for row, duration in enumerate(frame.hold_duration.detach().cpu().tolist()): + self.steps_for_duration( + float(duration), + field_name=f"RuntimeCommandFrame.hold_duration[{row}]", + ) + + def advance_after_env_step(self, steps: int = 1) -> None: + """Advance time after ``steps`` completed Gym environment transitions.""" + if type(steps) is not int or steps <= 0: + raise ValueError("steps must be a positive integer.") + self._step_index += steps + + +class JointPositionGymTransportEncoder: + """Built-in ``robot.joint_position`` to full-qpos action encoder.""" + + transport_id: ClassVar[str] = JointPositionTarget.TRANSPORT_ID + target_types: ClassVar[tuple[type[RuntimeEndpointTarget], ...]] = ( + JointPositionTarget, + ) + payload_types: ClassVar[tuple[type[RuntimeCommandPayload], ...]] = ( + JointPositionPayload, + ) + + def encode( + self, + command: EndpointCommand, + *, + base_action: EnvAction, + active_mask: torch.Tensor, + ) -> EnvAction: + """Write addressed joints while holding every other qpos column.""" + if not isinstance(command.target, JointPositionTarget): + raise TypeError("Joint-position transport requires JointPositionTarget.") + if not isinstance(command.payload, JointPositionPayload): + raise TypeError("Joint-position transport requires JointPositionPayload.") + if not isinstance(base_action, torch.Tensor): + raise TypeError( + "The built-in joint-position encoder requires a tensor base action; " + "register structured transports after it or provide a compatible " + "custom composition encoder." + ) + if base_action.dim() != 2 or base_action.shape[0] != command.batch_size: + raise ValueError( + "The full-qpos base action must have shape (batch_size, robot_dof)." + ) + if active_mask.dtype != torch.bool or active_mask.shape != ( + command.batch_size, + ): + raise ValueError("active_mask must be bool with one value per command row.") + if active_mask.device != base_action.device: + raise ValueError("active_mask and base_action must share a device.") + joint_ids = command.target.joint_ids + if max(joint_ids) >= base_action.shape[1]: + raise ValueError( + f"Joint ID {max(joint_ids)} exceeds full qpos width " + f"{base_action.shape[1]}." + ) + positions = command.payload.positions + if positions.device != base_action.device: + raise ValueError("Joint payload and base action must share a device.") + if not base_action.is_floating_point(): + raise TypeError("The full-qpos base action must be floating point.") + + action = base_action.clone() + columns = torch.tensor(joint_ids, dtype=torch.long, device=action.device) + selected = action.index_select(1, columns) + selected[active_mask] = positions[active_mask].to(dtype=action.dtype) + action[:, columns] = selected + return action + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + base_action: EnvAction, + context: PlanningContext, + ) -> EnvAction: + """Keep observed full qpos unchanged for addressed joint targets.""" + del context + if not all(isinstance(target, JointPositionTarget) for target in targets): + raise TypeError("Joint-position hold received an incompatible target.") + return base_action.clone() + + +class RuntimeCommandFrameEncoder: + """Encode transport-neutral command frames to controller-ready Gym actions. + + Args: + qpos_provider: Full-qpos source aligned to a frame's explicit ``env_ids``. + transports: Optional additional transport encoders. The built-in + joint-position encoder precedes them when enabled. + include_joint_position: Whether to install the built-in joint-position + encoder. Standard assemblies disable it when their exact profile uses + only custom endpoint transports. + """ + + def __init__( + self, + qpos_provider: CurrentQposProvider, + *, + transports: Iterable[RuntimeTransportActionEncoder] = (), + include_joint_position: bool = True, + ) -> None: + if not isinstance(qpos_provider, CurrentQposProvider): + raise TypeError("qpos_provider must implement CurrentQposProvider.") + if type(include_joint_position) is not bool: + raise TypeError("include_joint_position must be a bool.") + self._qpos_provider = qpos_provider + self._transports: dict[str, RuntimeTransportActionEncoder] = {} + self._frozen = False + if include_joint_position: + self.register_transport(JointPositionGymTransportEncoder()) + for transport in transports: + self.register_transport(transport) + + @property + def transport_ids(self) -> tuple[str, ...]: + """Return registered transport IDs in deterministic encoding order.""" + return tuple(self._transports) + + @property + def is_frozen(self) -> bool: + """Return whether runtime transport registration is permanently closed.""" + return self._frozen + + def freeze(self) -> None: + """Permanently close transport registration for a standard assembly.""" + self._frozen = True + + def register_transport( + self, + transport: RuntimeTransportActionEncoder, + *, + replace: bool = False, + ) -> None: + """Register one shared transport-to-Gym action encoder.""" + if self._frozen: + raise RuntimeError( + "Runtime transport registration is frozen for this command encoder." + ) + if not isinstance(transport, RuntimeTransportActionEncoder): + raise TypeError("transport must implement RuntimeTransportActionEncoder.") + transport_type = type(transport) + transport_id = _validate_identifier( + getattr(transport_type, "transport_id", None), + field_name="RuntimeTransportActionEncoder.transport_id", + ) + self._validate_declared_types( + getattr(transport_type, "target_types", None), + base_type=RuntimeEndpointTarget, + field_name="RuntimeTransportActionEncoder.target_types", + ) + self._validate_declared_types( + getattr(transport_type, "payload_types", None), + base_type=RuntimeCommandPayload, + field_name="RuntimeTransportActionEncoder.payload_types", + ) + if type(replace) is not bool: + raise TypeError("replace must be a bool.") + if transport_id in self._transports and not replace: + raise ValueError(f"Transport {transport_id!r} is already registered.") + self._transports[transport_id] = transport + + @staticmethod + def _validate_declared_types( + values: object, + *, + base_type: type[object], + field_name: str, + ) -> None: + """Validate one non-empty exact tuple of supported runtime types.""" + if type(values) is not tuple or not values: + raise TypeError(f"{field_name} must be a non-empty exact tuple.") + if not all( + isinstance(value, type) and issubclass(value, base_type) for value in values + ): + raise TypeError( + f"{field_name} must contain {base_type.__name__} subclasses." + ) + if len(set(values)) != len(values): + raise ValueError(f"{field_name} must not contain duplicate types.") + + @staticmethod + def _validate_command_types( + transport: RuntimeTransportActionEncoder, + command: EndpointCommand, + ) -> None: + """Require exact target and payload coverage before transport routing.""" + transport_type = type(transport) + if type(command.target) not in transport_type.target_types: + raise TypeError( + f"Transport {transport_type.transport_id!r} does not declare exact " + f"target type {type(command.target).__name__}." + ) + if type(command.payload) not in transport_type.payload_types: + raise TypeError( + f"Transport {transport_type.transport_id!r} does not declare exact " + f"payload type {type(command.payload).__name__}." + ) + + @staticmethod + def _validate_hold_target_types( + transport: RuntimeTransportActionEncoder, + targets: Iterable[RuntimeEndpointTarget], + ) -> None: + """Require exact target coverage before safe-hold routing.""" + transport_type = type(transport) + for target in targets: + if type(target) not in transport_type.target_types: + raise TypeError( + f"Transport {transport_type.transport_id!r} does not declare " + f"exact hold target type {type(target).__name__}." + ) + + def _base_qpos(self, env_ids: torch.Tensor) -> torch.Tensor: + """Capture and validate one owned full-qpos hold action.""" + qpos = self._qpos_provider.current_qpos(env_ids) + if not isinstance(qpos, torch.Tensor): + raise TypeError("CurrentQposProvider.current_qpos() must return a tensor.") + if qpos.dim() != 2 or qpos.shape[0] != env_ids.shape[0] or qpos.shape[1] == 0: + raise ValueError( + "Current qpos must have shape (batch_size, robot_dof) with non-zero DOF." + ) + if qpos.device != env_ids.device: + raise ValueError("Current qpos and env_ids must share a device.") + if not qpos.is_floating_point() or not torch.isfinite(qpos).all().item(): + raise ValueError("Current qpos must contain finite floating-point values.") + return qpos.clone() + + def encode(self, frame: RuntimeCommandFrame) -> EnvAction: + """Encode one frame on top of a fresh full-qpos hold action.""" + if not isinstance(frame, RuntimeCommandFrame): + raise TypeError("frame must be a RuntimeCommandFrame.") + action: EnvAction = self._base_qpos(frame.env_ids) + by_transport: dict[str, list[EndpointCommand]] = {} + for command in frame.commands: + transport = self._transports.get(command.transport_id) + if transport is None: + raise UnsupportedRuntimeTransportError( + f"No Gym action encoder is registered for runtime transport " + f"{command.transport_id!r}." + ) + self._validate_command_types(transport, command) + by_transport.setdefault(command.transport_id, []).append(command) + for transport_id, transport in self._transports.items(): + for command in by_transport.get(transport_id, ()): + action = transport.encode( + command, + base_action=action, + active_mask=frame.active_mask, + ) + return action + + def encode_hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, + ) -> EnvAction: + """Encode an observed-position safe hold for addressed transports.""" + if not isinstance(context, PlanningContext): + raise TypeError("context must be a PlanningContext.") + action: EnvAction = context.robot.qpos.clone() + by_transport: dict[str, list[RuntimeEndpointTarget]] = {} + for target in targets: + if not isinstance(target, RuntimeEndpointTarget): + raise TypeError("targets must contain RuntimeEndpointTarget values.") + by_transport.setdefault(target.transport_id, []).append(target) + for transport_id, grouped in by_transport.items(): + transport = self._transports.get(transport_id) + if transport is None: + raise UnsupportedRuntimeTransportError( + f"No Gym action encoder is registered for runtime transport " + f"{transport_id!r}." + ) + self._validate_hold_target_types(transport, grouped) + for transport_id, transport in self._transports.items(): + grouped = by_transport.get(transport_id) + if grouped is None: + continue + action = transport.hold( + tuple(grouped), + base_action=action, + context=context, + ) + return action + + def encode_idle_hold(self, env_ids: torch.Tensor) -> EnvAction: + """Return a fresh full-qpos hold when no transport was armed yet.""" + return self._base_qpos(env_ids) + + +@dataclass(frozen=True, slots=True) +class _BufferedAction: + """One owned action plus command-boundary provenance.""" + + action: ProcessedEnvAction + + def snapshot(self) -> _BufferedAction: + """Return one independently owned buffered action.""" + return _BufferedAction(self.action.snapshot()) + + +class BufferedGymCommandSink: + """Runner command sink that buffers actions for the Gym demo generator. + + Acceptance means the command was validated and copied into the local + buffer; it does not claim that an environment transition already occurred. + """ + + def __init__( + self, + encoder: RuntimeCommandFrameEncoder, + clock: EnvironmentStepClock, + *, + accepted_command_observer: AcceptedRuntimeCommandObserver | None = None, + ) -> None: + if not isinstance(encoder, RuntimeCommandFrameEncoder): + raise TypeError("encoder must be a RuntimeCommandFrameEncoder.") + if not isinstance(clock, EnvironmentStepClock): + raise TypeError("clock must be an EnvironmentStepClock.") + if accepted_command_observer is not None and not isinstance( + accepted_command_observer, + AcceptedRuntimeCommandObserver, + ): + raise TypeError( + "accepted_command_observer must implement " + "AcceptedRuntimeCommandObserver or be None." + ) + self._encoder = encoder + self._clock = clock + self._accepted_command_observer = accepted_command_observer + self._pending: deque[_BufferedAction] = deque() + self._last_emitted: ProcessedEnvAction | None = None + self._accepted_action_count = 0 + + @property + def clock(self) -> EnvironmentStepClock: + """Return the exact environment-step clock used for timing checks.""" + return self._clock + + @property + def pending_count(self) -> int: + """Return the number of accepted actions not yet yielded to Gym.""" + return len(self._pending) + + @property + def accepted_action_count(self) -> int: + """Return the monotonic count of actions accepted by this sink.""" + return self._accepted_action_count + + def send( + self, + command: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Validate, encode, and buffer one runtime command frame.""" + _validate_timeout(timeout) + if not isinstance(command, RuntimeCommandFrame): + raise TypeError("command must be a RuntimeCommandFrame.") + self._clock.validate_frame(command) + action = self._encoder.encode(command) + metadata = { + "bridge_action_kind": "runtime_command", + "runtime_destinations": [ + [item.transport_id, item.target.target_id] for item in command.commands + ], + "active_mask": command.active_mask.detach().cpu().tolist(), + "hold_duration": command.hold_duration.detach().cpu().tolist(), + } + self._pending.append( + _BufferedAction(ProcessedEnvAction(value=action, metadata=metadata)) + ) + observer = self._accepted_command_observer + if observer is not None: + try: + observer.accepted(command.snapshot()) + except Exception: + self._pending.clear() + self._discard_observer_state() + raise + self._accepted_action_count += 1 + return CommandAcknowledgement.accepted_ack("Buffered for the Gym step loop.") + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Buffer one observed-position safe hold action.""" + _validate_timeout(timeout) + action = self._encoder.encode_hold(tuple(targets), context) + metadata = { + "bridge_action_kind": "runtime_safe_hold", + "runtime_destinations": [ + [target.transport_id, target.target_id] for target in targets + ], + } + self._pending.append( + _BufferedAction(ProcessedEnvAction(value=action, metadata=metadata)) + ) + self._accepted_action_count += 1 + return CommandAcknowledgement.accepted_ack("Safe hold buffered for Gym.") + + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + """Discard accepted-but-not-yielded frames before a safe-stop hold.""" + _validate_timeout(timeout) + if not all(isinstance(target, RuntimeEndpointTarget) for target in targets): + raise TypeError("targets must contain RuntimeEndpointTarget values.") + self._pending.clear() + observer = self._accepted_command_observer + if observer is not None: + try: + observer.cancelled(tuple(target.snapshot() for target in targets)) + except Exception: + self._discard_observer_state() + raise + return CommandAcknowledgement.accepted_ack("Buffered commands cancelled.") + + def discard_pending(self) -> None: + """Discard actions that were accepted locally but never yielded.""" + self._pending.clear() + self._discard_observer_state() + + def drain_safe_stop_action( + self, + *, + fallback: ProcessedEnvAction | None = None, + ) -> ProcessedEnvAction | None: + """Select one buffered safe hold and discard every other local action. + + This method is used only by the demo abort handshake. A runtime + acknowledgement proves local buffering, not ``env.step`` consumption; + therefore an interrupted generator must explicitly surface the final + safe hold to the executor while dropping stale motion commands. + """ + candidates: list[ProcessedEnvAction] = [] + for candidate in (self._last_emitted, fallback): + if ( + candidate is not None + and candidate.metadata.get("bridge_action_kind") + in _SAFE_HOLD_ACTION_KINDS + ): + candidates.append(candidate.snapshot()) + while self._pending: + candidate = self._pending.popleft().action + if candidate.metadata.get("bridge_action_kind") in _SAFE_HOLD_ACTION_KINDS: + candidates.append(candidate.snapshot()) + self._discard_observer_state() + return None if not candidates else candidates[-1].snapshot() + + def _discard_observer_state(self) -> None: + """Reset observer state after any fail-closed local discard.""" + observer = self._accepted_command_observer + if observer is not None: + observer.discarded() + + def pop(self) -> ProcessedEnvAction: + """Pop the next accepted action and remember it as the active hold.""" + if not self._pending: + raise RuntimeError("No buffered Gym command is available.") + action = self._pending.popleft().action.snapshot() + self._last_emitted = action.snapshot() + return action + + def wait_hold(self, env_ids: torch.Tensor) -> ProcessedEnvAction: + """Return an owned hold action for one runtime waiting step.""" + if self._last_emitted is None: + value = self._encoder.encode_idle_hold(env_ids) + else: + value = self._last_emitted.value + return ProcessedEnvAction( + value=value, + metadata={"bridge_action_kind": "runtime_wait_hold"}, + ) + + +@dataclass(slots=True) +class _SegmentLifecycle: + """Mutable state shared by one lazy action generator and validator.""" + + complete: bool = False + result: SkillResult | ParallelSkillResult | None = None + validation: torch.Tensor | None = None + runtime: SequentialSkillRuntimePort | ParallelSkillRuntime | None = None + pending_action: ProcessedEnvAction | None = None + actions_started: bool = False + sink_acceptance_baseline: int | None = None + yielded_action_count: int = 0 + abort_started: bool = False + abort_complete: bool = False + metadata: dict[str, Any] = field(default_factory=dict) + post_policy_success: torch.Tensor | None = None + + +def _validate_runtime_result( + result: SkillResult | ParallelSkillResult, +) -> SkillResult | ParallelSkillResult: + """Validate one exact sequential or parallel runtime boundary.""" + if not isinstance(result, (SkillResult, ParallelSkillResult)): + raise TypeError( + "Runtime methods must return SkillResult or ParallelSkillResult values." + ) + return result + + +def _normalize_validation( + value: Any, + *, + batch_size: int, + device: torch.device, +) -> torch.Tensor: + """Normalize one validator output to an owned row-local boolean tensor.""" + tensor = torch.as_tensor(value, dtype=torch.bool, device=device).reshape(-1) + if tensor.numel() == 1 and batch_size > 1: + tensor = tensor.repeat(batch_size) + if tensor.numel() != batch_size: + raise ValueError( + f"Segment validator returned {tensor.numel()} flags, expected " + f"{batch_size}." + ) + return tensor.clone() + + +def _json_safe_copy(value: Any, *, field_name: str) -> Any: + """Return an owned JSON value while rejecting lossy coercions.""" + if value is None or type(value) in {bool, int, str}: + return value + if type(value) is float: + if not math.isfinite(value): + raise ValueError(f"{field_name} contains a non-finite float.") + return value + if isinstance(value, Mapping): + result: dict[str, Any] = {} + for key, item in value.items(): + if type(key) is not str or not key or key != key.strip(): + raise ValueError( + f"{field_name} mapping keys must be non-empty strings " + "without outer whitespace." + ) + result[key] = _json_safe_copy( + item, + field_name=f"{field_name}.{key}", + ) + return result + if isinstance(value, (list, tuple)): + return [ + _json_safe_copy(item, field_name=f"{field_name}[{index}]") + for index, item in enumerate(value) + ] + raise TypeError(f"{field_name} contains non-JSON value {type(value).__name__}.") + + +def _runtime_result_metadata( + result: SkillResult | ParallelSkillResult, +) -> dict[str, Any]: + """Snapshot one core runtime result through its canonical serializer.""" + serializer = getattr(result, "to_metadata", None) + if not callable(serializer): + raise TypeError( + f"{type(result).__name__} must provide to_metadata() for demo tracing." + ) + metadata = _json_safe_copy(serializer(), field_name="runtime result metadata") + if not isinstance(metadata, dict): + raise TypeError("Runtime result to_metadata() must return a mapping.") + return metadata + + +class AtomicDemoBridge: + """Adapt sequential compiled program segments to lazy Gym demonstrations. + + Args: + program: Provider-free compiled Expert Program. + runtime: Nonblocking semantic :class:`SkillRuntime` surface. + command_sink: The same buffered sink installed in ``runtime``. + clock: The same environment-step clock installed in ``runtime``. + post_policy_port: Optional environment-aware post-policy executor. + validator_port: Optional environment-aware validator executor. + runner_cfg: Runner transport policy selected by the runtime preset. + parallel_safety_validator: Optional authoritative physical-safety gate + required before any parallel branch can start. + + Schema-v2 parallel blocks retain their branch lanes and explicit barrier. + They are lowered through :class:`ParallelSkillRuntime`; they are never + flattened into a sequential semantic-call list. + """ + + def __init__( + self, + program: CompiledProgramPort, + runtime: SequentialSkillRuntimePort, + command_sink: BufferedGymCommandSink, + clock: EnvironmentStepClock, + *, + post_policy_port: SegmentPostPolicyPort | None = None, + validator_port: SegmentValidatorPort | None = None, + runner_cfg: ExecutionRunnerCfg | None = None, + parallel_safety_validator: ParallelCommandSafetyValidator | None = None, + ) -> None: + if not isinstance(program, CompiledProgramPort): + raise TypeError("program must implement CompiledProgramPort.") + _validate_identifier(program.program_id, field_name="program.program_id") + if type(program.schema_version) is not int or program.schema_version < 1: + raise ValueError("program.schema_version must be a positive integer.") + if not isinstance(runtime, SequentialSkillRuntimePort): + raise TypeError("runtime must implement SequentialSkillRuntimePort.") + if not isinstance(command_sink, BufferedGymCommandSink): + raise TypeError("command_sink must be a BufferedGymCommandSink.") + if not isinstance(clock, EnvironmentStepClock): + raise TypeError("clock must be an EnvironmentStepClock.") + if command_sink.clock is not clock: + raise ValueError("command_sink and bridge must share the exact clock.") + if post_policy_port is not None and not isinstance( + post_policy_port, SegmentPostPolicyPort + ): + raise TypeError("post_policy_port must implement SegmentPostPolicyPort.") + if validator_port is not None and not isinstance( + validator_port, SegmentValidatorPort + ): + raise TypeError("validator_port must implement SegmentValidatorPort.") + if runner_cfg is not None and not isinstance(runner_cfg, ExecutionRunnerCfg): + raise TypeError("runner_cfg must be an ExecutionRunnerCfg or None.") + if parallel_safety_validator is not None and not isinstance( + parallel_safety_validator, ParallelCommandSafetyValidator + ): + raise TypeError( + "parallel_safety_validator must implement " + "ParallelCommandSafetyValidator." + ) + self._program = program + self._runtime = runtime + self._sink = command_sink + self._clock = clock + self._post_policy_port = post_policy_port + self._validator_port = validator_port + self._runner_cfg = deepcopy(runner_cfg or ExecutionRunnerCfg()) + self._parallel_safety_validator = parallel_safety_validator + self._active_segment_id: str | None = None + self._eligible_mask: torch.Tensor | None = None + + @property + def clock(self) -> EnvironmentStepClock: + """Return the environment-step clock used by this bridge.""" + return self._clock + + def iter_segments(self) -> Iterator[DemoSegment]: + """Lazily adapt compiled program segments to ``DemoSegment`` values. + + Consumers must exhaust each segment's actions and invoke its validator + before requesting the next segment. Skipping either lifecycle boundary + raises :class:`DemoBridgeError` instead of silently carrying stale row + eligibility into downstream execution. + """ + for segment in self._program.iter_segments(): + metadata = self._segment_metadata(segment) + lifecycle = _SegmentLifecycle(metadata=metadata) + validator = self._segment_validator(segment, lifecycle) + yield DemoSegment( + actions=self._segment_actions(segment, lifecycle), + name=segment.name, + metadata=metadata, + validator=validator, + abort_actions=self._segment_abort_actions(segment, lifecycle), + failure_policy="row_independent", + ) + self._require_consumed_segment_lifecycle(segment, lifecycle) + + def __iter__(self) -> Iterator[DemoSegment]: + """Delegate iteration to :meth:`iter_segments`.""" + return self.iter_segments() + + @staticmethod + def _require_consumed_segment_lifecycle( + segment: Any, + lifecycle: _SegmentLifecycle, + ) -> None: + """Reject advancing past a segment with an unconsumed lifecycle. + + The public demo executor exhausts ``actions`` and then invokes the + segment validator before requesting the next lazy segment. Direct + bridge consumers must preserve the same ordering because validation is + also the commit point for runtime and post-policy row eligibility. + """ + if not lifecycle.complete: + raise DemoBridgeError( + f"Segment {segment.segment_id!r} actions must be exhausted before " + "requesting the next compiled segment." + ) + if lifecycle.validation is None: + raise DemoBridgeError( + f"Segment {segment.segment_id!r} validator must be called after " + "its actions are exhausted and before requesting the next " + "compiled segment." + ) + + def _segment_metadata(self, segment: Any) -> dict[str, Any]: + """Build mutable JSON-safe metadata completed at lifecycle boundaries.""" + return { + "expert_program_schema_version": self._program.schema_version, + "expert_program_id": self._program.program_id, + "program_segment_id": segment.segment_id, + "program_segment_index": segment.segment_index, + "program_segment_source_path": list(segment.source_path), + "program_segment_implicit": bool(segment.implicit), + "semantic_call_indices": [call.call_index for call in segment.calls], + "post_policy_count": len(segment.post_policies), + "validator_count": len(segment.validators), + "parallel": getattr(segment, "parallel_block", None) is not None, + "runtime": None, + "post_policies": [], + "validation": None, + } + + @staticmethod + def _record_runtime_result( + lifecycle: _SegmentLifecycle, + result: SkillResult | ParallelSkillResult, + ) -> None: + """Snapshot one runtime boundary into its owning segment metadata.""" + lifecycle.result = result + lifecycle.metadata["runtime"] = _runtime_result_metadata(result) + + def _decorate_action( + self, + action: Any, + *, + segment: Any, + result: SkillResult | ParallelSkillResult, + action_kind: str | None = None, + ) -> ProcessedEnvAction: + """Own one action and attach stable program/runtime provenance.""" + if isinstance(action, ProcessedEnvAction): + value = action.value + metadata = dict(action.metadata) + else: + value = action + metadata = {} + if action_kind is not None: + metadata["bridge_action_kind"] = action_kind + metadata.update( + { + "expert_program_id": self._program.program_id, + "program_segment_id": segment.segment_id, + "program_segment_index": segment.segment_index, + "environment_step": self._clock.step_index, + "runtime_status": result.status.value, + "runtime_call_index": getattr(result, "current_call_index", None), + } + ) + return ProcessedEnvAction(value=value, metadata=metadata) + + def _yield_and_advance( + self, + action: ProcessedEnvAction, + lifecycle: _SegmentLifecycle, + ) -> Iterator[ProcessedEnvAction]: + """Yield once and advance only after explicit consumption acknowledgement.""" + if lifecycle.pending_action is not None: + raise RuntimeError("A prior demo action is still awaiting acknowledgement.") + lifecycle.pending_action = action.snapshot() + lifecycle.yielded_action_count += 1 + yield action + if lifecycle.pending_action is not None: + self._clock.advance_after_env_step() + lifecycle.pending_action = None + + def _segment_actions( + self, + segment: Any, + lifecycle: _SegmentLifecycle, + ) -> Iterator[ProcessedEnvAction]: + """Drive one semantic segment without bypassing the Gym step loop.""" + segment_id = segment.segment_id + lifecycle.actions_started = True + lifecycle.sink_acceptance_baseline = self._sink.accepted_action_count + if self._active_segment_id is not None: + raise RuntimeError( + f"Segment {self._active_segment_id!r} is still active; exhaust or " + "close it before starting another lazy segment." + ) + self._active_segment_id = segment_id + result: SkillResult | ParallelSkillResult | None = None + segment_runtime: SequentialSkillRuntimePort | ParallelSkillRuntime = ( + self._runtime + ) + is_parallel = getattr(segment, "parallel_block", None) is not None + try: + if is_parallel: + segment_runtime = self._parallel_runtime(segment) + lifecycle.runtime = segment_runtime + result = _validate_runtime_result( + segment_runtime.start( + workflow_id=f"{self._program.program_id}/{segment_id}", + eligible_mask=self._eligible_mask, + ) + ) + else: + lifecycle.runtime = segment_runtime + analysis = self._program.sequential_execution_analysis( + segment.segment_index + ) + calls = tuple(analysis.calls) + if not calls: + raise DemoBridgeError( + f"Compiled segment {segment_id!r} contains no semantic calls." + ) + execution_prefix_length = analysis.execution_prefix_length + if execution_prefix_length != len(segment.calls): + raise DemoBridgeError( + f"Compiled segment {segment_id!r} analysis prefix length " + "does not match its owned semantic calls." + ) + result = _validate_runtime_result( + segment_runtime.start( + calls, + workflow_id=f"{self._program.program_id}/{segment_id}", + eligible_mask=self._eligible_mask, + execution_prefix_length=execution_prefix_length, + ) + ) + + while True: + emitted = False + while self._sink.pending_count: + action = self._decorate_action( + self._sink.pop(), + segment=segment, + result=result, + ) + yield from self._yield_and_advance(action, lifecycle) + emitted = True + + if emitted and not result.terminal: + # The result's wait duration was measured before the action + # just consumed by Gym. Refresh it against the advanced + # environment clock before deciding whether another hold is due. + result = _validate_runtime_result(segment_runtime.step()) + continue + + if result.terminal: + break + + if result.wait_duration > 0.0: + self._clock.steps_for_duration( + result.wait_duration, + field_name="SkillResult.wait_duration", + ) + hold = self._decorate_action( + self._sink.wait_hold(result.env_ids), + segment=segment, + result=result, + action_kind="runtime_wait_hold", + ) + yield from self._yield_and_advance(hold, lifecycle) + + result = _validate_runtime_result(segment_runtime.step()) + + self._record_runtime_result(lifecycle, result) + self._retain_eligible_rows(result.success_mask) + if is_parallel: + self._runtime.adopt_verified_task_state(result.task_state) + if result.status is SkillStatus.COMPLETED: + yield from self._post_policy_actions(segment, result, lifecycle) + lifecycle.complete = True + finally: + if not lifecycle.abort_started and lifecycle.pending_action is not None: + if result is not None and not result.terminal: + segment_runtime.cancel( + f"Demo segment {segment_id!r} action iteration stopped early." + ) + raise DemoBridgeError( + f"Demo segment {segment_id!r} was closed with an unacknowledged " + "action. Consume DemoSegment.abort_actions through env.step() " + "before closing the action iterator." + ) + self._active_segment_id = None + + def _segment_abort_actions( + self, + segment: Any, + lifecycle: _SegmentLifecycle, + ) -> Callable[..., Iterator[ProcessedEnvAction]]: + """Create the explicit executor-to-runtime cancellation handshake.""" + + def abort( + reason: str, + *, + last_action_consumed: bool, + ) -> Iterator[ProcessedEnvAction]: + return self._abort_segment( + segment, + lifecycle, + reason=reason, + last_action_consumed=last_action_consumed, + ) + + return abort + + def _abort_segment( + self, + segment: Any, + lifecycle: _SegmentLifecycle, + *, + reason: str, + last_action_consumed: bool, + ) -> Iterator[ProcessedEnvAction]: + """Abort one segment, surfacing a safe hold only after controller activity.""" + if type(reason) is not str or not reason: + raise ValueError("abort reason must be a non-empty string.") + if type(last_action_consumed) is not bool: + raise TypeError("last_action_consumed must be a bool.") + if lifecycle.abort_started: + raise RuntimeError( + f"Segment {segment.segment_id!r} abort handshake already started." + ) + if not lifecycle.actions_started: + raise RuntimeError( + f"Segment {segment.segment_id!r} has no started action iteration " + "to abort." + ) + baseline = lifecycle.sink_acceptance_baseline + if baseline is None: + raise RuntimeError( + f"Segment {segment.segment_id!r} has no sink lifecycle baseline." + ) + controller_activity_started = ( + lifecycle.yielded_action_count > 0 + or lifecycle.pending_action is not None + or self._sink.accepted_action_count > baseline + ) + if not controller_activity_started: + # Runtime construction and preflight are deliberately observation- and + # command-free. If either fails before the first accepted or yielded + # action, there is no physical controller state to safe-stop. Mark the + # handshake complete without touching the partially constructed runtime + # so the original action-generation exception remains authoritative. + lifecycle.abort_started = True + lifecycle.abort_complete = True + return + runtime = lifecycle.runtime + pending = lifecycle.pending_action + if runtime is None: + raise DemoBridgeError( + f"Segment {segment.segment_id!r} accepted or yielded a controller " + "action without retaining a runtime capable of strict safe-stop." + ) + lifecycle.abort_started = True + if pending is not None: + pending = pending.snapshot() + if pending is not None and last_action_consumed: + self._clock.advance_after_env_step() + lifecycle.pending_action = None + + result = _validate_runtime_result(runtime.result) + if not result.terminal: + result = _validate_runtime_result(runtime.cancel(reason)) + self._record_runtime_result(lifecycle, result) + + pending_kind = ( + None if pending is None else pending.metadata.get("bridge_action_kind") + ) + if ( + pending is not None + and last_action_consumed + and pending_kind in _SAFE_HOLD_ACTION_KINDS + ): + self._sink.discard_pending() + lifecycle.abort_complete = True + return + + safe_action = self._sink.drain_safe_stop_action( + fallback=None if last_action_consumed else pending, + ) + if safe_action is None: + raise DemoBridgeError( + f"Segment {segment.segment_id!r} stopped before exhaustion, but " + "no controller safe-hold action was available for env.step()." + ) + processed = self._decorate_action( + safe_action, + segment=segment, + result=result, + action_kind="runtime_abort_safe_hold", + ) + yield processed + self._clock.advance_after_env_step() + lifecycle.abort_complete = True + + def _parallel_runtime(self, segment: Any) -> ParallelSkillRuntime: + """Build one one-shot coordinator from a compiled explicit barrier.""" + if self._parallel_safety_validator is None: + raise DemoBridgeError( + f"Parallel segment {segment.segment_id!r} requires an explicit " + "ParallelCommandSafetyValidator; resource claims alone do not " + "establish physical collision safety." + ) + if not isinstance(self._runtime, SkillRuntime): + # Production integration always supplies SkillRuntime. Keeping the + # sequential protocol permits lightweight tests and alternate + # frontends, but the canonical parallel factory requires forkable + # runtime internals by design. + raise TypeError( + "Parallel compiled segments require a concrete SkillRuntime " + "template." + ) + block = segment.parallel_block + branches = tuple(block.branches) + if len(branches) < 2: + raise DemoBridgeError( + f"Parallel segment {segment.segment_id!r} requires at least two " + "compiled branches." + ) + branch_calls = { + f"branch_{branch.branch_index}": tuple( + compiled.call for compiled in branch.calls + ) + for branch in branches + } + branch_paths = { + f"branch_{branch.branch_index}": tuple( + getattr(branch, "source_path", segment.source_path) + ) + for branch in branches + } + if any(not calls for calls in branch_calls.values()): + raise DemoBridgeError( + f"Parallel segment {segment.segment_id!r} contains an empty branch." + ) + barrier = block.barrier + return ParallelSkillRuntime.from_template( + self._runtime, + branch_calls, + self._sink, + ParallelTimingPolicy(self._clock.step_dt), + self._parallel_safety_validator, + timeout_steps=barrier.timeout_steps, + failure_policy=barrier.failure_policy, + runner_cfg=self._runner_cfg, + workflow_id=( + f"{self._program.program_id}/{segment.segment_id}:parallel_analysis" + ), + branch_paths=branch_paths, + ) + + def _retain_eligible_rows(self, accepted: torch.Tensor) -> None: + """Permanently remove failed rows before a later lazy segment starts.""" + if not isinstance(accepted, torch.Tensor): + raise TypeError("accepted must be a torch.Tensor.") + if accepted.dtype != torch.bool or accepted.dim() != 1: + raise ValueError("accepted must be a one-dimensional bool tensor.") + if self._eligible_mask is None: + self._eligible_mask = torch.ones_like(accepted) + elif ( + self._eligible_mask.shape != accepted.shape + or self._eligible_mask.device != accepted.device + ): + raise ValueError("Environment rows changed across program segments.") + self._eligible_mask &= accepted + + def _post_policy_actions( + self, + segment: Any, + result: SkillResult | ParallelSkillResult, + lifecycle: _SegmentLifecycle, + ) -> Iterator[ProcessedEnvAction]: + """Route environment-aware post-policy actions through the same generator.""" + policies = tuple(segment.post_policies) + if policies and self._post_policy_port is None: + raise DemoBridgeError( + f"Segment {segment.segment_id!r} declares post-policies, but no " + "SegmentPostPolicyPort was installed." + ) + traces = lifecycle.metadata["post_policies"] + if not isinstance(traces, list): + raise TypeError("Segment post-policy metadata storage must be a list.") + for policy_index, policy in enumerate(policies): + assert self._post_policy_port is not None + active_mask = ( + result.success_mask.clone() + if self._eligible_mask is None + else self._eligible_mask.clone() + ) + if lifecycle.post_policy_success is not None: + active_mask &= lifecycle.post_policy_success + actions = self._post_policy_port.actions( + policy, + segment=segment, + active_mask=active_mask, + ) + if isinstance(actions, (str, bytes)): + raise TypeError("Post-policy actions must be an iterable of actions.") + action_iterator = iter(actions) + iteration_error: BaseException | None = None + try: + for action in action_iterator: + processed = self._decorate_action( + action, + segment=segment, + result=result, + action_kind="program_post_policy", + ) + yield from self._yield_and_advance(processed, lifecycle) + except BaseException as exc: + iteration_error = exc + raise + finally: + close = getattr(action_iterator, "close", None) + if callable(close): + close() + cfg = getattr(policy, "cfg", None) + trace: dict[str, Any] = { + "policy_index": policy_index, + "kind": getattr(cfg, "kind", type(policy).__name__), + "source_path": list(getattr(policy, "source_path", ())), + "result_mask": result.success_mask.detach().cpu().tolist(), + "result": None, + } + port = self._post_policy_port + policy_success = active_mask.clone() + if isinstance(port, SegmentPostPolicyResultPort): + try: + policy_success &= _normalize_validation( + port.post_policy_result(policy, segment=segment), + batch_size=result.env_ids.numel(), + device=result.env_ids.device, + ) + except Exception: + if iteration_error is None: + raise + if lifecycle.post_policy_success is None: + lifecycle.post_policy_success = policy_success.clone() + else: + lifecycle.post_policy_success &= policy_success + trace["result_mask"] = policy_success.detach().cpu().tolist() + if isinstance(port, SegmentPostPolicyMetadataPort): + try: + trace["result"] = port.post_policy_metadata( + policy, + segment=segment, + ) + except Exception: + if iteration_error is None: + raise + traces.append( + _json_safe_copy( + trace, + field_name=f"post-policy {policy_index} metadata", + ) + ) + + def _segment_validator( + self, + segment: Any, + lifecycle: _SegmentLifecycle, + ) -> Callable[[], torch.Tensor]: + """Create a demo-boundary validator including runtime row success.""" + + def validate() -> torch.Tensor: + if not lifecycle.complete or lifecycle.result is None: + raise RuntimeError( + f"Segment {segment.segment_id!r} cannot be validated before its " + "action iterable is exhausted." + ) + if lifecycle.validation is not None: + return lifecycle.validation.clone() + result = lifecycle.result + accepted = result.success_mask.clone() + runtime_success = result.success_mask.clone() + eligible_before = ( + torch.ones_like(accepted) + if self._eligible_mask is None + else self._eligible_mask.clone() + ) + if self._eligible_mask is not None: + accepted &= self._eligible_mask + if lifecycle.post_policy_success is not None: + accepted &= lifecycle.post_policy_success + validators = tuple(segment.validators) + if validators and self._validator_port is None: + raise DemoBridgeError( + f"Segment {segment.segment_id!r} declares validators, but no " + "SegmentValidatorPort was installed." + ) + validator_traces: list[dict[str, Any]] = [] + for validator_index, validator in enumerate(validators): + assert self._validator_port is not None + value = self._validator_port.validate(validator, segment=segment) + validator_result = _normalize_validation( + value, + batch_size=result.env_ids.numel(), + device=result.env_ids.device, + ) + accepted &= validator_result + cfg = getattr(validator, "cfg", None) + trace: dict[str, Any] = { + "validator_index": validator_index, + "kind": getattr(cfg, "kind", type(validator).__name__), + "source_path": list(getattr(validator, "source_path", ())), + "result_mask": validator_result.detach().cpu().tolist(), + "result": None, + } + port = self._validator_port + if isinstance(port, SegmentValidatorMetadataPort): + trace["result"] = port.validator_metadata( + validator, + segment=segment, + ) + validator_traces.append( + _json_safe_copy( + trace, + field_name=f"validator {validator_index} metadata", + ) + ) + lifecycle.metadata["validation"] = _json_safe_copy( + { + "env_ids": result.env_ids.detach().cpu().tolist(), + "runtime_success_mask": runtime_success.detach().cpu().tolist(), + "eligible_mask_before_validation": eligible_before.detach() + .cpu() + .tolist(), + "post_policy_success_mask": ( + None + if lifecycle.post_policy_success is None + else lifecycle.post_policy_success.detach().cpu().tolist() + ), + "validators": validator_traces, + "accepted_mask": accepted.detach().cpu().tolist(), + }, + field_name="segment validation metadata", + ) + self._retain_eligible_rows(accepted) + lifecycle.validation = accepted.clone() + return accepted.clone() + + return validate + + +__all__ = [ + "AcceptedRuntimeCommandObserver", + "AtomicDemoBridge", + "BufferedGymCommandSink", + "CompiledProgramPort", + "CurrentQposProvider", + "DemoBridgeError", + "EnvironmentStepClock", + "EnvironmentStepTimingError", + "GymPlanningObservationProvider", + "JointPositionGymTransportEncoder", + "ParallelCommandSafetyValidator", + "RuntimeCommandFrameEncoder", + "RuntimeTransportActionEncoder", + "SegmentPostPolicyMetadataPort", + "SegmentPostPolicyPort", + "SegmentPostPolicyResultPort", + "SegmentValidatorMetadataPort", + "SegmentValidatorPort", + "SequentialSkillRuntimePort", + "UnsupportedRuntimeTransportError", +] diff --git a/embodichain/lab/gym/envs/expert_program/catalog.py b/embodichain/lab/gym/envs/expert_program/catalog.py new file mode 100644 index 000000000..29cc5276f --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/catalog.py @@ -0,0 +1,1710 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Immutable task-registration catalog for declarative Expert Programs.""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import dataclass, field, fields, is_dataclass, replace +from enum import Enum +import hashlib +import json +import math +from _thread import LockType +from threading import Lock +from types import MappingProxyType +import torch + +from embodichain.lab.gym.envs.settling import DynamicSettleMonitorCfg +from embodichain.lab.sim.atomic_actions import ( + Affordance, + ArticulationOperationAffordance, + AtomicActionEngine, + EndpointTrackingFeedbackAddress, + GRASP_CAPABILITY, + JOINT_POSITION_CHANNEL, + SkillDescriptor, +) +from embodichain.lab.sim.atomic_actions.primitives import BUILTIN_ACTION_TYPES +from embodichain.lab.sim.atomic_actions.tracking import ( + FeedbackTerminalAcceptance, + TrackingRuntime, +) +from embodichain.lab.sim.skills import ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + ControlPartEndpoint, + ControlPartEvidenceAddress, + FORCE_EFFECT_CHANNEL, + JOINT_STATE_EFFECT_CHANNEL, + PLACE_IN_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, + POSE_RELATION_EFFECT_CHANNEL, + BoundRobotSkillProfile, + ContainerRelationTargetGrounder, + HandOverPoseProvider, + OperateArticulation, + Place, + RelationTargetGrounder, + RobotResource, + RobotSkillProfile, + RegisteredSemanticCall, + ResourceEndpoint, + ResourceEndpointAdapter, + SceneAffordanceRef, + SceneArticulationRef, + SceneEntityRef, + SceneEntityManifest, + SceneManifest, + SceneObjectRef, + SceneRegistry, + SemanticCallCatalog, + SemanticCallDescriptor, + SemanticIntegrationManifest, + SemanticValidationError, + SkillPolicyPreset, + SupportSurfaceRelationTargetGrounder, + builtin_semantic_call_catalog, +) +from embodichain.lab.sim.skills.effects import ( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + CompositeEffectMonitorFactory, + EffectMonitorRegistry, +) +from embodichain.lab.sim.skills.parallel_runtime import ( + ParallelCommandSafetyValidator, +) + +from .cfg import ( + ExpertProgramCfg, + ExpertProgramIntegrationCfg, + OperateArticulationCfg, + PostPolicyCfg, + RegisteredSemanticCallCfg, + SemanticCallCfg, + ValidatorCfg, +) +from .compiler import ( + CompiledProgram, + ExpertProgramCompileError, + ExpertProgramCompiler, + ExpertProgramSceneResolver, +) +from .decoder import ( + ConfigPath, + ExpertProgramValidationError, + SceneReferenceRole, +) +from .bridge import RuntimeTransportActionEncoder +from .extensions import ( + EndpointAdapterDeclaration, + ParallelCommandSafetyValidatorFactory, + ParallelSafetyDeclaration, + RuntimeTransportDeclaration, + StandardExtensionDeclarations, + build_standard_extension_declarations, + validate_immutable_extension_declaration, +) +from .simulation import SimulationRobotSkillProfileBinding, SimulationSceneBinding +from .simulation_policies import default_simulation_settle_presets + +_CATALOG_FINGERPRINT_SCHEMA_VERSION = 2 +_POST_POLICY_KINDS = frozenset({"wait_stable"}) +_VALIDATOR_KINDS = frozenset({"object_near_target"}) + + +class IntegrationFingerprintMismatch(RuntimeError): + """Raised when a live integration no longer matches its registration.""" + + +def _qualified_name(value: type[object] | object) -> str: + """Return a stable fully-qualified type name.""" + value_type = value if isinstance(value, type) else type(value) + return f"{value_type.__module__}.{value_type.__qualname__}" + + +def _canonical_value(value: object) -> object: + """Convert provider-free declarations to deterministic JSON values.""" + if value is None or type(value) in (bool, int, str): + return value + if type(value) is float: + if not math.isfinite(value): + raise ValueError("Fingerprint metadata cannot contain non-finite floats.") + return value + if isinstance(value, Enum): + return { + "type": _qualified_name(value), + "value": _canonical_value(value.value), + } + if isinstance(value, type): + return {"type": _qualified_name(value)} + if isinstance(value, torch.Tensor): + tensor = value.detach().cpu() + return { + "tensor_dtype": str(tensor.dtype), + "tensor_shape": list(tensor.shape), + "tensor_value": tensor.tolist(), + } + if isinstance(value, Mapping): + normalized: dict[str, object] = {} + for key, nested in value.items(): + if type(key) is not str: + raise TypeError("Fingerprint mapping keys must be exact strings.") + normalized[key] = _canonical_value(nested) + return {key: normalized[key] for key in sorted(normalized)} + if isinstance(value, (tuple, list)): + return [_canonical_value(nested) for nested in value] + if isinstance(value, (set, frozenset)): + normalized = [_canonical_value(nested) for nested in value] + return sorted( + normalized, + key=lambda item: json.dumps( + item, + sort_keys=True, + separators=(",", ":"), + ensure_ascii=False, + ), + ) + if is_dataclass(value): + metadata = { + data_field.name: _canonical_value(getattr(value, data_field.name)) + for data_field in fields(value) + } + return {"type": _qualified_name(value), "fields": metadata} + raise TypeError( + "Registration fingerprint metadata contains unsupported value type " + f"{_qualified_name(value)!r}. Values must be complete declarative data; " + "live or opaque objects cannot be fingerprinted by type alone." + ) + + +def _provider_fingerprint_declaration(provider: object) -> object: + """Return the complete canonical declaration for one validated provider.""" + if is_dataclass(provider): + return provider + return {"provider_type": _qualified_name(provider)} + + +def _canonical_json(value: object) -> str: + """Encode one declaration using the versioned canonical JSON form.""" + return json.dumps( + _canonical_value(value), + sort_keys=True, + separators=(",", ":"), + ensure_ascii=False, + allow_nan=False, + ) + + +def _digest(payload: object) -> str: + """Return the SHA-256 digest for one canonical declaration payload.""" + return hashlib.sha256(_canonical_json(payload).encode("utf-8")).hexdigest() + + +def _snapshot_settle_presets( + values: Mapping[str, DynamicSettleMonitorCfg], +) -> Mapping[str, DynamicSettleMonitorCfg]: + """Own one strict named settle-preset table.""" + if not isinstance(values, Mapping) or not values: + raise ValueError("settle_presets must be a non-empty mapping.") + normalized: dict[str, DynamicSettleMonitorCfg] = {} + for preset_id, preset in values.items(): + if ( + type(preset_id) is not str + or not preset_id + or preset_id != preset_id.strip() + ): + raise ValueError( + "Settle preset IDs must be non-empty strings without outer " + "whitespace." + ) + if not isinstance(preset, DynamicSettleMonitorCfg): + raise TypeError( + "settle_presets values must be DynamicSettleMonitorCfg values." + ) + normalized[preset_id] = preset.snapshot() + return MappingProxyType(normalized) + + +def _exact_identifier(value: object, *, field_name: str) -> str: + """Validate one exact catalog identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +def _relation_grounder_key( + grounder: RelationTargetGrounder, +) -> tuple[str, type[Affordance], str]: + """Return the compiler-compatible exact key for one relation grounder.""" + grounder_type = type(grounder) + capability = _exact_identifier( + getattr(grounder_type, "capability", None), + field_name="RelationTargetGrounder.capability", + ) + affordance_type = getattr(grounder_type, "affordance_type", None) + if not isinstance(affordance_type, type) or not issubclass( + affordance_type, + Affordance, + ): + raise TypeError( + "RelationTargetGrounder.affordance_type must be an Affordance subclass." + ) + revision = _exact_identifier( + getattr(grounder_type, "affordance_revision", None), + field_name="RelationTargetGrounder.affordance_revision", + ) + return capability, affordance_type, revision + + +def _relation_grounder_order_key( + grounder: RelationTargetGrounder, +) -> tuple[str, str, str]: + """Return one totally ordered rendering of a relation-grounder key.""" + capability, affordance_type, revision = _relation_grounder_key(grounder) + return capability, _qualified_name(affordance_type), revision + + +def _snapshot_relation_grounders( + values: tuple[RelationTargetGrounder, ...], +) -> tuple[RelationTargetGrounder, ...]: + """Validate and own one immutable relation-grounder tuple.""" + if type(values) is not tuple: + raise TypeError("relation_grounders must be an exact tuple.") + seen: set[tuple[str, type[Affordance], str]] = set() + for grounder in values: + if not isinstance(grounder, RelationTargetGrounder): + raise TypeError( + "relation_grounders must contain RelationTargetGrounder instances." + ) + validate_immutable_extension_declaration( + grounder, + field_name="relation_grounders", + ) + key = _relation_grounder_key(grounder) + if key in seen: + raise ValueError(f"Duplicate relation grounder key {key!r}.") + seen.add(key) + return tuple(values) + + +def _builtin_relation_grounders( + scene_binding: SimulationSceneBinding, +) -> tuple[RelationTargetGrounder, ...]: + """Install standard grounders for declared production relation bindings.""" + values: list[RelationTargetGrounder] = [] + if scene_binding.support_surfaces: + values.append(SupportSurfaceRelationTargetGrounder()) + if scene_binding.containers: + values.append(ContainerRelationTargetGrounder()) + return tuple(values) + + +def _snapshot_relation_grounder_keys( + values: frozenset[tuple[str, type[Affordance], str]], +) -> frozenset[tuple[str, type[Affordance], str]]: + """Validate immutable provider-free relation-grounder lookup keys.""" + if type(values) is not frozenset: + raise TypeError("relation_grounder_keys must be an exact frozenset.") + normalized: set[tuple[str, type[Affordance], str]] = set() + for key in values: + if type(key) is not tuple or len(key) != 3: + raise TypeError("relation_grounder_keys must contain exact 3-tuple values.") + capability, affordance_type, revision = key + _exact_identifier(capability, field_name="relation grounder capability") + if not isinstance(affordance_type, type) or not issubclass( + affordance_type, + Affordance, + ): + raise TypeError( + "relation grounder affordance types must be Affordance subclasses." + ) + _exact_identifier(revision, field_name="relation grounder revision") + normalized.add((capability, affordance_type, revision)) + return frozenset(normalized) + + +def _handover_pose_provider_id(provider: HandOverPoseProvider) -> str: + """Return the compiler-compatible class ID for one hand-over provider.""" + return _exact_identifier( + getattr(type(provider), "provider_id", None), + field_name="HandOverPoseProvider.provider_id", + ) + + +def _snapshot_handover_pose_providers( + values: tuple[HandOverPoseProvider, ...], +) -> tuple[HandOverPoseProvider, ...]: + """Validate and own one immutable hand-over-provider tuple.""" + if type(values) is not tuple: + raise TypeError("handover_pose_providers must be an exact tuple.") + seen: set[str] = set() + for provider in values: + if not isinstance(provider, HandOverPoseProvider): + raise TypeError( + "handover_pose_providers must contain HandOverPoseProvider instances." + ) + validate_immutable_extension_declaration( + provider, + field_name="handover_pose_providers", + ) + provider_id = _handover_pose_provider_id(provider) + if provider_id in seen: + raise ValueError(f"Duplicate handover pose provider {provider_id!r}.") + seen.add(provider_id) + return tuple(values) + + +def _validate_standard_call_catalog(call_catalog: SemanticCallCatalog) -> None: + """Reject semantic lowerer extensions from the standard registration path.""" + builtins = builtin_semantic_call_catalog().descriptors + for descriptor in call_catalog.descriptors.values(): + if descriptor.spec_type is RegisteredSemanticCall: + raise ValueError( + f"Registered semantic call {descriptor.call_id!r} is not " + "supported by the standard simulation registration; only " + "curated semantic calls may be registered." + ) + expected = builtins.get(descriptor.call_id) + if expected != descriptor: + raise ValueError( + f"Semantic call {descriptor.call_id!r} does not match its exact " + "curated descriptor." + ) + + +def _validate_standard_effect_monitors(profile: RobotSkillProfile) -> None: + """Require every preset to use the exact built-in effect-monitor factory.""" + registry = EffectMonitorRegistry((CompositeEffectMonitorFactory(),)) + builtin_key = ( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + ) + for preset_id, preset in profile.presets.items(): + for semantic_id, monitor_ref in preset.effect_monitors.items(): + key = monitor_ref.monitor_id, monitor_ref.revision + if key != builtin_key: + raise ValueError( + f"Preset {preset_id!r} semantic call {semantic_id!r} selects " + f"non-built-in effect monitor {key!r}; the standard " + "simulation registration supports only {builtin_key!r}." + ) + try: + registry.validate_ref(monitor_ref) + except (KeyError, TypeError, ValueError) as exc: + raise ValueError( + f"Preset {preset_id!r} semantic call {semantic_id!r} has an " + "invalid built-in effect-monitor declaration." + ) from exc + + +def _validate_standard_tracking_metrics(profile: RobotSkillProfile) -> None: + """Resolve every reachable metric through the exact built-in evaluator table.""" + evaluators = TrackingRuntime.with_builtins().evaluators + for preset_id, preset in profile.presets.items(): + policy = preset.tracking_policy + metric_groups = [] + if policy.in_flight is not None: + metric_groups.append(("in_flight", policy.in_flight.metrics)) + if isinstance(policy.terminal, FeedbackTerminalAcceptance): + metric_groups.append(("terminal", policy.terminal.metrics)) + for phase, metrics in metric_groups: + for metric in metrics: + try: + evaluators.resolve(metric) + except (KeyError, TypeError, ValueError) as exc: + key = metric.metric_id, metric.revision, _qualified_name(metric) + raise ValueError( + f"Preset {preset_id!r} {phase} tracking metric {key!r} " + "has no exact built-in evaluator in the standard " + "simulation registration." + ) from exc + + +def _declared_articulation_operation_targets( + scene_binding: SimulationSceneBinding, +) -> dict[str, frozenset[str]]: + """Derive named operation-target IDs from the task-owned scene binding.""" + return { + binding.entity_id: frozenset(binding.semantic_targets) + for binding in scene_binding.articulation_operations + } + + +def _snapshot_articulation_operation_targets( + values: Mapping[str, frozenset[str]], + *, + scene: SceneManifest, +) -> Mapping[str, frozenset[str]]: + """Own and cross-check provider-free named articulation targets.""" + if not isinstance(values, Mapping): + raise TypeError("articulation_operation_targets must be a mapping.") + normalized: dict[str, frozenset[str]] = {} + for affordance_id, target_ids in values.items(): + _exact_identifier( + affordance_id, + field_name="articulation operation affordance IDs", + ) + if type(target_ids) is not frozenset: + raise TypeError( + "articulation_operation_targets values must be exact frozensets." + ) + for target_id in target_ids: + _exact_identifier( + target_id, + field_name="articulation operation target IDs", + ) + entry = scene.lookup( + affordance_id, + expected_type=SceneAffordanceRef, + path=("articulation_operation_targets", affordance_id), + ) + if entry.ref.entity_id != affordance_id: + raise ValueError( + "articulation_operation_targets keys must use canonical " + "affordance IDs." + ) + if ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY + not in entry.affordance_capabilities + or entry.affordance_payload_type is not ArticulationOperationAffordance + ): + raise TypeError( + f"Scene affordance {affordance_id!r} is not an articulation " + "operation affordance." + ) + normalized[affordance_id] = frozenset(target_ids) + + declared_affordance_ids = { + entry.ref.entity_id + for entry in scene.entries + if type(entry.ref) is SceneAffordanceRef + and ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY + in entry.affordance_capabilities + } + if set(normalized) != declared_affordance_ids: + raise ValueError( + "articulation_operation_targets must cover every declared operation " + f"affordance exactly; expected {sorted(declared_affordance_ids)}, got " + f"{sorted(normalized)}." + ) + return MappingProxyType(normalized) + + +class _SceneManifestProgramResolver: + """Resolve compiler references from an immutable :class:`SceneManifest`.""" + + def __init__(self, scene: SceneManifest) -> None: + if type(scene) is not SceneManifest: + raise TypeError("scene must be exactly SceneManifest.") + self._scene = scene + + def resolve( + self, + reference: str, + *, + expected_types: tuple[type[SceneEntityRef], ...], + path: ConfigPath, + ) -> SceneEntityRef: + """Resolve one reference without retaining a live registry.""" + if ( + type(expected_types) is not tuple + or not expected_types + or not all( + isinstance(expected_type, type) + and issubclass(expected_type, SceneEntityRef) + for expected_type in expected_types + ) + ): + raise TypeError( + "expected_types must be a non-empty tuple of scene-ref types." + ) + try: + resolved = self._scene.resolve(reference, path=path) + except (KeyError, TypeError, ValueError) as exc: + raise ExpertProgramCompileError( + "unknown_scene_reference", + path, + str(exc), + ) from exc + if type(resolved) not in expected_types: + raise ExpertProgramCompileError( + "scene_reference_type_mismatch", + path, + f"Scene reference {reference!r} resolves to " + f"{type(resolved).__name__}, expected one of " + f"{tuple(value.__name__ for value in expected_types)}.", + ) + return type(resolved)(resolved.entity_id) + + +def _planner_scene_entry(entry: SceneEntityManifest) -> dict[str, object]: + """Project one provider-free scene entry for task planning.""" + return { + "id": entry.ref.entity_id, + "ref_type": type(entry.ref).__name__, + "aliases": list(entry.aliases), + "parent": None if entry.parent is None else entry.parent.entity_id, + "native_name": entry.native_name, + "dynamics": entry.dynamics.value, + "collision_role": entry.collision_role.value, + "semantic_type": entry.semantic_type, + "affordance_capabilities": sorted(entry.affordance_capabilities), + "default_affordances": { + capability: reference.entity_id + for capability, reference in sorted(entry.default_affordances.items()) + }, + "affordance_payload_type": ( + None + if entry.affordance_payload_type is None + else _qualified_name(entry.affordance_payload_type) + ), + "affordance_revision": entry.affordance_revision, + "relative_pose": ( + None if entry.relative_pose is None else list(entry.relative_pose) + ), + } + + +def _planner_call_descriptor( + descriptor: SemanticCallDescriptor, +) -> dict[str, object]: + """Project one semantic call and its robot-independent resource contract.""" + contract = descriptor.binding_contract + return { + "call_id": descriptor.call_id, + "schema_version": descriptor.schema_version, + "skill_id": descriptor.skill_id, + "config_type": _qualified_name(descriptor.spec_type), + "slots": [ + { + "slot_id": slot.slot_id, + "endpoints": [ + { + "endpoint_id": endpoint.endpoint_id, + "capabilities": sorted(endpoint.capabilities), + "required_commands": { + command_id: _qualified_name(command_type) + for command_id, command_type in sorted( + endpoint.required_commands.items() + ) + }, + } + for endpoint in slot.endpoints + ], + "constraints": [ + _canonical_value(constraint) for constraint in slot.constraints + ], + } + for slot in contract.slots + ], + "constraints": [ + _canonical_value(constraint) for constraint in contract.constraints + ], + } + + +def _planner_robot_resource(resource: RobotResource) -> dict[str, object]: + """Project one robot resource without live binding or controller state.""" + return { + "resource_id": resource.resource_id, + "members": list(resource.members), + "endpoints": [ + { + "endpoint_id": endpoint_id, + "endpoint_type": _qualified_name(endpoint), + "capabilities": sorted(endpoint.capabilities), + "declaration": _canonical_value(endpoint), + } + for endpoint_id, endpoint in sorted(resource.endpoints.items()) + ], + } + + +@dataclass(frozen=True, slots=True) +class ExpertProgramIntegrationCatalog: + """Provider-free integration directory owned by one task registration.""" + + scene_registry_id: str + robot_profile_id: str + scene: SceneManifest + robot_profile: RobotSkillProfile + call_catalog: SemanticCallCatalog + relation_grounder_keys: frozenset[tuple[str, type[Affordance], str]] + articulation_operation_targets: Mapping[str, frozenset[str]] + settle_preset_ids: frozenset[str] + endpoint_adapter_declarations: Mapping[ + type[ResourceEndpoint], EndpointAdapterDeclaration + ] + runtime_transport_declarations: tuple[RuntimeTransportDeclaration, ...] + parallel_safety_declaration: ParallelSafetyDeclaration | None + fingerprint: str + _required_skills: Mapping[str, SkillDescriptor] = field( + repr=False, + compare=False, + ) + + def __post_init__(self) -> None: + for field_name in ("scene_registry_id", "robot_profile_id"): + value = getattr(self, field_name) + if type(value) is not str or not value or value != value.strip(): + raise ValueError(f"{field_name} must be an exact identifier.") + if type(self.scene) is not SceneManifest: + raise TypeError("scene must be exactly SceneManifest.") + if type(self.robot_profile) is not RobotSkillProfile: + raise TypeError("robot_profile must be exactly RobotSkillProfile.") + if type(self.call_catalog) is not SemanticCallCatalog: + raise TypeError("call_catalog must be exactly SemanticCallCatalog.") + object.__setattr__( + self, + "relation_grounder_keys", + _snapshot_relation_grounder_keys(self.relation_grounder_keys), + ) + object.__setattr__( + self, + "articulation_operation_targets", + _snapshot_articulation_operation_targets( + self.articulation_operation_targets, + scene=self.scene, + ), + ) + extensions = StandardExtensionDeclarations( + endpoint_adapters=self.endpoint_adapter_declarations, + runtime_transports=self.runtime_transport_declarations, + parallel_safety=self.parallel_safety_declaration, + ) + profile_endpoint_types = frozenset( + type(endpoint) + for resource in self.robot_profile.resources.values() + for endpoint in resource.endpoints.values() + ) + if profile_endpoint_types != frozenset(extensions.endpoint_adapters): + raise ValueError( + "endpoint_adapter_declarations must cover every exact robot " + "profile endpoint type and no others." + ) + object.__setattr__( + self, + "endpoint_adapter_declarations", + extensions.endpoint_adapters, + ) + object.__setattr__( + self, + "runtime_transport_declarations", + extensions.runtime_transports, + ) + object.__setattr__( + self, + "parallel_safety_declaration", + extensions.parallel_safety, + ) + if self.robot_profile.profile_id != self.robot_profile_id: + raise ValueError("robot_profile_id must match robot_profile.profile_id.") + preset_ids = frozenset(self.settle_preset_ids) + if not preset_ids: + raise ValueError("settle_preset_ids must not be empty.") + object.__setattr__(self, "settle_preset_ids", preset_ids) + if ( + type(self.fingerprint) is not str + or len(self.fingerprint) != 64 + or any( + character not in "0123456789abcdef" for character in self.fingerprint + ) + ): + raise ValueError("fingerprint must be a lowercase SHA-256 digest.") + object.__setattr__( + self, + "_required_skills", + MappingProxyType(dict(self._required_skills)), + ) + + def planner_projection(self) -> dict[str, object]: + """Return a deterministic JSON-safe planner view of this integration. + + The projection is derived exclusively from the canonical catalog and + carries its exact fingerprint. It contains no providers, live handles, + bound resource claims, motion policy, or executable implementation. + """ + profile = self.robot_profile + return { + "schema_version": "semantic_integration_planner_projection/v1", + "integration_fingerprint": self.fingerprint, + "scene_registry_id": self.scene_registry_id, + "robot_profile_id": self.robot_profile_id, + "scene": { + "collision_world_mode": ( + None + if self.scene.collision_world_mode is None + else self.scene.collision_world_mode.value + ), + "entities": [ + _planner_scene_entry(entry) for entry in self.scene.entries + ], + }, + "semantic_calls": [ + _planner_call_descriptor(descriptor) + for descriptor in sorted( + self.call_catalog.descriptors.values(), + key=lambda value: value.call_id, + ) + ], + "robot": { + "resources": [ + _planner_robot_resource(resource) + for resource in sorted( + profile.resources.values(), + key=lambda value: value.resource_id, + ) + ], + "defaults": { + skill_id: dict(binding.resources) + for skill_id, binding in sorted(profile.defaults.items()) + }, + "preset_ids": sorted(profile.presets), + "default_preset": profile.default_preset, + "skill_presets": dict(sorted(profile.skill_presets.items())), + "grounding_providers": dict( + sorted(profile.grounding_providers.items()) + ), + }, + "providers": { + "relation_grounders": [ + { + "capability": capability, + "affordance_type": _qualified_name(affordance_type), + "revision": revision, + } + for capability, affordance_type, revision in sorted( + self.relation_grounder_keys, + key=lambda value: ( + value[0], + _qualified_name(value[1]), + value[2], + ), + ) + ], + "articulation_operation_targets": { + entity_id: sorted(targets) + for entity_id, targets in sorted( + self.articulation_operation_targets.items() + ) + }, + "settle_preset_ids": sorted(self.settle_preset_ids), + "endpoint_adapters": [ + _canonical_value(declaration) + for declaration in sorted( + self.endpoint_adapter_declarations.values(), + key=lambda value: value.adapter_id, + ) + ], + "runtime_transports": [ + _canonical_value(declaration) + for declaration in self.runtime_transport_declarations + ], + "parallel_safety": ( + None + if self.parallel_safety_declaration is None + else _canonical_value(self.parallel_safety_declaration) + ), + }, + } + + def validate_integration( + self, + integration: ExpertProgramIntegrationCfg, + *, + path: ConfigPath, + ) -> None: + """Validate exact scene, profile, and runtime-preset selection.""" + del path + if integration.scene_registry != self.scene_registry_id: + raise ValueError( + f"Expected scene_registry {self.scene_registry_id!r}, got " + f"{integration.scene_registry!r}." + ) + if integration.robot_profile != self.robot_profile_id: + raise ValueError( + f"Expected robot_profile {self.robot_profile_id!r}, got " + f"{integration.robot_profile!r}." + ) + if integration.runtime_preset not in self.robot_profile.presets: + raise KeyError( + f"Unknown runtime preset {integration.runtime_preset!r}; available " + f"presets are {sorted(self.robot_profile.presets)}." + ) + + def validate_semantic_call( + self, + call: SemanticCallCfg, + *, + path: ConfigPath, + ) -> None: + """Validate semantic-call catalog and payload revision references.""" + call_id = call.call_id if type(call) is RegisteredSemanticCallCfg else call.kind + descriptor = self.call_catalog.discover(call_id) + if type(call) is RegisteredSemanticCallCfg and ( + call.schema_version != descriptor.schema_version + ): + raise ValueError( + f"Semantic call {call_id!r} requires schema_version " + f"{descriptor.schema_version}, got {call.schema_version}." + ) + if type(call) is OperateArticulationCfg and call.target is not None: + self._validate_articulation_operation_target( + articulation=call.articulation, + handle=call.handle, + target=call.target, + path=path, + ) + + def _validate_articulation_operation_target( + self, + *, + articulation: str | SceneArticulationRef, + handle: str | SceneAffordanceRef | None, + target: str, + path: ConfigPath, + ) -> None: + """Resolve one operation affordance and validate its named target.""" + try: + articulation_ref = self.scene.resolve( + articulation, + expected_type=SceneArticulationRef, + path=(*path, "articulation"), + ) + except SemanticValidationError as exc: + raise ExpertProgramValidationError( + exc.diagnostic.code, + exc.diagnostic.path, + exc.diagnostic.message, + ) from exc + try: + affordance = self.scene.resolve_affordance( + articulation_ref, + capability=ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + explicit=handle, + path=(*path, "handle"), + ) + except SemanticValidationError as exc: + raise ExpertProgramValidationError( + exc.diagnostic.code, + exc.diagnostic.path, + exc.diagnostic.message, + ) from exc + target_ids = self.articulation_operation_targets.get(affordance.entity_id) + if target_ids is None: + raise ExpertProgramValidationError( + "missing_articulation_operation_targets", + (*path, "handle"), + f"Operation affordance {affordance.entity_id!r} has no static " + "named-target declaration.", + ) + if target not in target_ids: + raise ExpertProgramValidationError( + "unknown_articulation_operation_target", + (*path, "target"), + f"Unknown target {target!r} for operation affordance " + f"{affordance.entity_id!r}; available targets are " + f"{sorted(target_ids)}.", + ) + + def _validate_place_relation_grounder( + self, + call: Place, + *, + affordance: SceneAffordanceRef, + path: ConfigPath, + ) -> None: + """Require the exact linked relation-affordance grounder pre-sim.""" + if call.on is not None: + capability = PLACE_ON_AFFORDANCE_CAPABILITY + relation_field = "on" + elif call.inside is not None: + capability = PLACE_IN_AFFORDANCE_CAPABILITY + relation_field = "inside" + else: + return + entry = self.scene.lookup( + affordance, + expected_type=SceneAffordanceRef, + path=(*path, relation_field), + ) + payload_type = entry.affordance_payload_type + revision = entry.affordance_revision + if payload_type is None or revision is None: + raise ExpertProgramValidationError( + "incomplete_relation_affordance_declaration", + (*path, relation_field), + f"Relation affordance {affordance.entity_id!r} must declare an " + "exact payload type and revision.", + ) + key = (capability, payload_type, revision) + if key not in self.relation_grounder_keys: + rendered_key = ( + capability, + _qualified_name(payload_type), + revision, + ) + raise ExpertProgramValidationError( + "relation_grounder_not_registered", + (*path, relation_field), + f"No task-registration relation grounder matches linked " + f"affordance {affordance.entity_id!r} with key {rendered_key!r}.", + ) + + def validate_scene_reference( + self, + reference: str, + *, + role: SceneReferenceRole, + path: ConfigPath, + ) -> None: + """Validate one typed scene reference against its declared role.""" + expected: dict[str, tuple[type[SceneEntityRef], ...]] = { + "entity": (SceneEntityRef,), + "object": (SceneObjectRef,), + "articulation": (SceneArticulationRef,), + "affordance": (SceneAffordanceRef,), + "object_or_affordance": (SceneObjectRef, SceneAffordanceRef), + } + expected_types = expected.get(role) + if expected_types is None: + raise ValueError(f"Unsupported scene reference role {role!r}.") + resolved = self.scene.resolve(reference, path=path) + if not isinstance(resolved, expected_types): + raise TypeError( + f"Scene reference {reference!r} is {type(resolved).__name__}, " + f"not one of {tuple(value.__name__ for value in expected_types)}." + ) + + def validate_post_policy( + self, + policy: PostPolicyCfg, + *, + path: ConfigPath, + ) -> None: + """Validate one registered post-policy kind and named preset.""" + del path + if policy.kind not in _POST_POLICY_KINDS: + raise KeyError( + f"Unknown post-policy kind {policy.kind!r}; available kinds are " + f"{sorted(_POST_POLICY_KINDS)}." + ) + if policy.preset not in self.settle_preset_ids: + raise KeyError( + f"Unknown settle preset {policy.preset!r}; available presets are " + f"{sorted(self.settle_preset_ids)}." + ) + + def validate_validator( + self, + validator: ValidatorCfg, + *, + path: ConfigPath, + ) -> None: + """Validate one registered segment-validator kind.""" + del path + if validator.kind not in _VALIDATOR_KINDS: + raise KeyError( + f"Unknown validator kind {validator.kind!r}; available kinds are " + f"{sorted(_VALIDATOR_KINDS)}." + ) + + def preflight(self, program: ExpertProgramCfg) -> CompiledProgram: + """Compile and statically link every expanded semantic call.""" + self.validate_integration(program.integration, path=("integration",)) + resolver: ExpertProgramSceneResolver = _SceneManifestProgramResolver(self.scene) + compiled = ExpertProgramCompiler(resolver).compile(program) + manifest = SemanticIntegrationManifest( + scene=self.scene, + robot_profile=self.robot_profile, + call_catalog=self.call_catalog, + runtime_preset=program.integration.runtime_preset, + ) + for segment in compiled.iter_segments(): + if ( + segment.parallel_block is not None + and self.parallel_safety_declaration is None + ): + raise ExpertProgramValidationError( + "parallel_safety_factory_not_registered", + segment.parallel_block.source_path, + "Parallel execution requires a task-registration-owned " + "physical safety-validator factory.", + ) + for call in segment.calls: + if ( + type(call.call) is OperateArticulation + and call.call.target is not None + ): + self._validate_articulation_operation_target( + articulation=call.call.articulation, + handle=call.call.handle, + target=call.call.target, + path=call.source_path, + ) + linked = manifest.link_call(call.call, path=call.source_path) + if type(linked.call) is Place and linked.call.at is None: + destination = linked.affordances.get("destination") + if destination is None: + raise AssertionError( + "Linked relation Place call lacks a destination " + "affordance." + ) + self._validate_place_relation_grounder( + linked.call, + affordance=destination, + path=call.source_path, + ) + return compiled + + def validate_engine(self, engine: AtomicActionEngine) -> None: + """Require the live engine to expose every statically selected skill.""" + if not isinstance(engine, AtomicActionEngine): + raise TypeError("engine must be an AtomicActionEngine.") + for skill_id, expected in self._required_skills.items(): + actual = engine.skills.get(skill_id) + if actual != expected: + raise IntegrationFingerprintMismatch( + f"Live skill {skill_id!r} differs from the registered " + "semantic target descriptor." + ) + bound_profile = engine.skill_profile + if type(bound_profile) is not BoundRobotSkillProfile: + raise IntegrationFingerprintMismatch( + "The standard live engine must own one exact bound robot profile." + ) + self.validate_bound_endpoint_extensions(bound_profile) + + def validate_bound_endpoint_extensions( + self, + bound_profile: BoundRobotSkillProfile, + ) -> None: + """Match every live resolved endpoint to its fingerprinted declaration.""" + if type(bound_profile) is not BoundRobotSkillProfile: + raise TypeError("bound_profile must be exactly BoundRobotSkillProfile.") + if bound_profile.profile_id != self.robot_profile_id: + raise IntegrationFingerprintMismatch( + "The bound robot profile ID differs from the registered profile." + ) + + transport_owner_by_target_type = { + target_type: transport + for transport in self.runtime_transport_declarations + for target_type in transport.target_types + } + expected_resource_ids = frozenset(self.robot_profile.resources) + live_resource_ids = frozenset(bound_profile.resources) + if live_resource_ids != expected_resource_ids: + raise IntegrationFingerprintMismatch( + "Bound robot resource IDs differ from the registered profile; " + f"expected {sorted(expected_resource_ids)}, " + f"got {sorted(live_resource_ids)}." + ) + for resource_id, resource in bound_profile.resources.items(): + expected_resource = self.robot_profile.resources[resource_id] + if resource.resource_id != expected_resource.resource_id: + raise IntegrationFingerprintMismatch( + f"Bound resource {resource_id!r} declaration ID differs from " + "the registered profile." + ) + if resource.members != expected_resource.members: + raise IntegrationFingerprintMismatch( + f"Bound resource {resource_id!r} members differ from the " + "registered profile." + ) + expected_endpoint_ids = frozenset(expected_resource.endpoints) + live_endpoint_ids = frozenset(resource.endpoints) + if live_endpoint_ids != expected_endpoint_ids: + raise IntegrationFingerprintMismatch( + f"Bound resource {resource_id!r} endpoint IDs differ from the " + f"registered profile; expected {sorted(expected_endpoint_ids)}, " + f"got {sorted(live_endpoint_ids)}." + ) + for endpoint_id, endpoint in resource.endpoints.items(): + location = f"{resource_id}.{endpoint_id}" + expected_endpoint = expected_resource.endpoints[endpoint_id] + if type(endpoint.endpoint) is not type(expected_endpoint) or ( + _canonical_json(endpoint.endpoint) + != _canonical_json(expected_endpoint) + ): + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} declaration differs from the " + "registered robot profile." + ) + endpoint_type = type(endpoint.endpoint) + declaration = self.endpoint_adapter_declarations.get(endpoint_type) + if declaration is None: + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} has undeclared exact type " + f"{_qualified_name(endpoint_type)!r}." + ) + if endpoint.adapter_id != declaration.adapter_id: + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} adapter ID " + f"{endpoint.adapter_id!r} differs from registered " + f"{declaration.adapter_id!r}." + ) + + target = endpoint.runtime_target + target_type = type(target) + if target_type not in declaration.runtime_target_types: + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} resolved undeclared exact " + f"runtime target type {_qualified_name(target_type)!r}." + ) + owner = transport_owner_by_target_type.get(target_type) + if owner is None or owner.transport_id not in ( + declaration.runtime_transport_ids + ): + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} target type has no registered " + "adapter transport owner." + ) + if target.transport_id != owner.transport_id: + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} live transport " + f"{target.transport_id!r} differs from target type owner " + f"{owner.transport_id!r}." + ) + + feedback_keys = frozenset( + (binding.source.provider_id, binding.source.revision) + for binding in endpoint.tracking_channels.values() + ) + if feedback_keys != declaration.tracking_feedback_source_keys: + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} tracking-feedback routes " + "differ from its registered adapter declaration." + ) + projector_keys = frozenset( + (binding.projector.projector_id, binding.projector.revision) + for binding in endpoint.tracking_channels.values() + ) + if projector_keys != declaration.tracking_projector_keys: + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} tracking-projector routes " + "differ from its registered adapter declaration." + ) + evidence_keys = frozenset( + (source.provider_id, source.revision) + for source in endpoint.effect_sources.values() + ) + if evidence_keys != declaration.effect_evidence_source_keys: + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} effect-evidence routes " + "differ from its registered adapter declaration." + ) + if endpoint_type is ControlPartEndpoint: + control_part = endpoint.endpoint.control_part + if getattr(target, "control_part", None) != control_part: + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} runtime target addresses " + "a different control part." + ) + if frozenset(endpoint.tracking_channels) != frozenset( + {JOINT_POSITION_CHANNEL} + ): + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} must expose exactly the " + "built-in joint-position tracking channel." + ) + tracking = endpoint.tracking_channels[JOINT_POSITION_CHANNEL] + feedback_address = tracking.source.address + if type(feedback_address) is not EndpointTrackingFeedbackAddress: + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} must use the exact " + "built-in endpoint tracking address." + ) + if ( + feedback_address.channel_id != JOINT_POSITION_CHANNEL + or type(feedback_address.target) is not target_type + or _canonical_json(feedback_address.target) + != _canonical_json(target) + ): + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} tracking address differs " + "from its runtime target or channel." + ) + + expected_effect_channels = { + POSE_RELATION_EFFECT_CHANNEL, + JOINT_STATE_EFFECT_CHANNEL, + } + if GRASP_CAPABILITY in endpoint.endpoint.capabilities: + expected_effect_channels.update( + { + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + FORCE_EFFECT_CHANNEL, + } + ) + if frozenset(endpoint.effect_sources) != frozenset( + expected_effect_channels + ): + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} effect-evidence channels " + "differ from the exact built-in control-part routes." + ) + for channel, source in endpoint.effect_sources.items(): + address = source.address + if ( + type(address) is not ControlPartEvidenceAddress + or address.control_part != control_part + or address.channel != channel + ): + raise IntegrationFingerprintMismatch( + f"Bound endpoint {location!r} effect-evidence " + f"address for channel {channel!r} differs from its " + "control part or channel." + ) + elif endpoint.tracking_channels or endpoint.effect_sources: + raise IntegrationFingerprintMismatch( + f"Bound custom endpoint {location!r} exposes closed-loop " + "routes forbidden by the C1 standard runtime." + ) + + +def _profile_with_step_dt( + profile: RobotSkillProfile, + *, + step_dt: float, +) -> RobotSkillProfile: + """Return the registration profile with runner cadence aligned to Gym.""" + return replace( + profile, + presets={ + preset_id: SkillPolicyPreset( + preset_id=preset.preset_id, + schema_version=preset.schema_version, + motion_policy=preset.motion_policy, + tracking_policy=preset.tracking_policy, + recovery_policy=preset.recovery_policy, + workflow_recovery_policy=preset.workflow_recovery_policy, + runner_cfg=replace( + preset.runner_cfg, + minimum_cycle_time=step_dt, + ), + effect_monitors=preset.effect_monitors, + action_option_templates=preset.action_option_templates, + required_planner=preset.required_planner, + ) + for preset_id, preset in profile.presets.items() + }, + ) + + +def _registration_payload( + *, + scene_binding: SimulationSceneBinding, + scene: SceneManifest, + articulation_operation_targets: Mapping[str, frozenset[str]], + robot_profile_binding: SimulationRobotSkillProfileBinding, + robot_profile: RobotSkillProfile, + call_catalog: SemanticCallCatalog, + settle_presets: Mapping[str, DynamicSettleMonitorCfg], + relation_grounder_keys: frozenset[tuple[str, type[Affordance], str]], + relation_grounders: tuple[RelationTargetGrounder, ...], + handover_pose_providers: tuple[HandOverPoseProvider, ...], + extensions: StandardExtensionDeclarations, + endpoint_adapters: tuple[ResourceEndpointAdapter, ...], + runtime_transports: tuple[RuntimeTransportActionEncoder, ...], + parallel_safety_factory: ParallelCommandSafetyValidatorFactory | None, +) -> dict[str, object]: + """Build the versioned canonical fingerprint payload.""" + return { + "schema_version": _CATALOG_FINGERPRINT_SCHEMA_VERSION, + "scene_binding": scene_binding, + "scene_manifest": scene.entries, + "articulation_operation_targets": articulation_operation_targets, + "robot_profile_binding": robot_profile_binding, + "robot_profile": robot_profile, + "call_descriptors": tuple( + sorted( + call_catalog.descriptors.values(), + key=lambda descriptor: descriptor.call_id, + ) + ), + "relation_grounder_keys": relation_grounder_keys, + "relation_grounders": tuple( + { + "key": _relation_grounder_key(grounder), + "provider": _provider_fingerprint_declaration(grounder), + } + for grounder in sorted( + relation_grounders, + key=_relation_grounder_order_key, + ) + ), + "handover_pose_providers": tuple( + { + "provider_id": _handover_pose_provider_id(provider), + "provider": _provider_fingerprint_declaration(provider), + } + for provider in sorted( + handover_pose_providers, + key=_handover_pose_provider_id, + ) + ), + "standard_extensions": { + "endpoint_adapters": tuple( + sorted( + extensions.endpoint_adapters.values(), + key=lambda declaration: declaration.adapter_id, + ) + ), + "runtime_transports": extensions.runtime_transports, + "parallel_safety": extensions.parallel_safety, + }, + "endpoint_adapters": tuple( + { + "declaration": extensions.endpoint_adapters[ + getattr(type(adapter), "endpoint_type") + ], + "provider": _provider_fingerprint_declaration(adapter), + } + for adapter in sorted( + endpoint_adapters, + key=lambda value: getattr(type(value), "adapter_id"), + ) + ), + "runtime_transports": tuple( + { + "declaration": next( + declaration + for declaration in extensions.runtime_transports + if declaration.transport_id + == getattr(type(transport), "transport_id") + ), + "provider": _provider_fingerprint_declaration(transport), + } + for transport in runtime_transports + ), + "parallel_safety_factory": ( + None + if parallel_safety_factory is None + else { + "declaration": extensions.parallel_safety, + "provider": _provider_fingerprint_declaration(parallel_safety_factory), + } + ), + "post_policy_kinds": _POST_POLICY_KINDS, + "settle_presets": settle_presets, + "validator_kinds": _VALIDATOR_KINDS, + } + + +@dataclass(frozen=True, slots=True) +class SimulationExpertProgramRegistration: + """Exact immutable task-owned simulation integration registration.""" + + scene_binding: SimulationSceneBinding + robot_profile_binding: SimulationRobotSkillProfileBinding + call_catalog: SemanticCallCatalog = field( + default_factory=builtin_semantic_call_catalog + ) + settle_presets: Mapping[str, DynamicSettleMonitorCfg] = field( + default_factory=default_simulation_settle_presets + ) + relation_grounders: tuple[RelationTargetGrounder, ...] = () + handover_pose_providers: tuple[HandOverPoseProvider, ...] = () + endpoint_adapters: tuple[ResourceEndpointAdapter, ...] = () + runtime_transports: tuple[RuntimeTransportActionEncoder, ...] = () + parallel_safety_factory: ParallelCommandSafetyValidatorFactory | None = None + catalog: ExpertProgramIntegrationCatalog = field(init=False) + _parallel_safety_validator_history: list[ParallelCommandSafetyValidator] = field( + init=False, + repr=False, + compare=False, + ) + _parallel_safety_validator_lock: LockType = field( + init=False, + repr=False, + compare=False, + ) + + def __post_init__(self) -> None: + if type(self.scene_binding) is not SimulationSceneBinding: + raise TypeError("scene_binding must be exactly SimulationSceneBinding.") + if type(self.robot_profile_binding) is not SimulationRobotSkillProfileBinding: + raise TypeError( + "robot_profile_binding must be exactly " + "SimulationRobotSkillProfileBinding." + ) + if type(self.call_catalog) is not SemanticCallCatalog: + raise TypeError("call_catalog must be exactly SemanticCallCatalog.") + _validate_standard_call_catalog(self.call_catalog) + settle_presets = _snapshot_settle_presets(self.settle_presets) + object.__setattr__(self, "settle_presets", settle_presets) + configured_relation_grounders = _snapshot_relation_grounders( + self.relation_grounders + ) + relation_grounders = _snapshot_relation_grounders( + ( + *_builtin_relation_grounders(self.scene_binding), + *configured_relation_grounders, + ) + ) + object.__setattr__(self, "relation_grounders", relation_grounders) + relation_grounder_keys = frozenset( + _relation_grounder_key(grounder) for grounder in relation_grounders + ) + handover_pose_providers = _snapshot_handover_pose_providers( + self.handover_pose_providers + ) + object.__setattr__( + self, + "handover_pose_providers", + handover_pose_providers, + ) + + scene = self.scene_binding.declare() + articulation_operation_targets = _declared_articulation_operation_targets( + self.scene_binding + ) + profile = self.robot_profile_binding.declare() + _validate_standard_effect_monitors(profile) + _validate_standard_tracking_metrics(profile) + extensions = build_standard_extension_declarations( + profile=profile, + endpoint_adapters=self.endpoint_adapters, + runtime_transports=self.runtime_transports, + parallel_safety_factory=self.parallel_safety_factory, + ) + selected_handover_provider = profile.grounding_providers.get("hand_over") + registered_handover_provider_ids = { + _handover_pose_provider_id(provider) for provider in handover_pose_providers + } + if ( + selected_handover_provider is not None + and selected_handover_provider not in registered_handover_provider_ids + ): + raise ValueError( + "Robot profile selects handover pose provider " + f"{selected_handover_provider!r}, but the task registration did " + "not install it." + ) + builtin_skills = { + descriptor.skill_id: descriptor + for action_type in BUILTIN_ACTION_TYPES + if (descriptor := action_type.descriptor()).agent_visible + and descriptor.binding_contract is not None + } + required_skills: dict[str, SkillDescriptor] = {} + for descriptor in self.call_catalog.descriptors.values(): + target = descriptor.target_descriptor + installed = builtin_skills.get(descriptor.skill_id) + if target is None or installed != target: + raise ValueError( + f"Semantic call {descriptor.call_id!r} targets skill " + f"{descriptor.skill_id!r}, which is not installed by the " + "standard simulation factory." + ) + required_skills[descriptor.skill_id] = target + + fingerprint = _digest( + _registration_payload( + scene_binding=self.scene_binding, + scene=scene, + articulation_operation_targets=articulation_operation_targets, + robot_profile_binding=self.robot_profile_binding, + robot_profile=profile, + call_catalog=self.call_catalog, + settle_presets=settle_presets, + relation_grounder_keys=relation_grounder_keys, + relation_grounders=relation_grounders, + handover_pose_providers=handover_pose_providers, + extensions=extensions, + endpoint_adapters=self.endpoint_adapters, + runtime_transports=self.runtime_transports, + parallel_safety_factory=self.parallel_safety_factory, + ) + ) + object.__setattr__( + self, + "catalog", + ExpertProgramIntegrationCatalog( + scene_registry_id=self.scene_binding.registry_id, + robot_profile_id=self.robot_profile_binding.profile_id, + scene=scene, + robot_profile=profile, + call_catalog=self.call_catalog, + relation_grounder_keys=relation_grounder_keys, + articulation_operation_targets=articulation_operation_targets, + settle_preset_ids=frozenset(settle_presets), + endpoint_adapter_declarations=extensions.endpoint_adapters, + runtime_transport_declarations=extensions.runtime_transports, + parallel_safety_declaration=extensions.parallel_safety, + fingerprint=fingerprint, + _required_skills=required_skills, + ), + ) + object.__setattr__(self, "_parallel_safety_validator_history", []) + object.__setattr__(self, "_parallel_safety_validator_lock", Lock()) + + @property + def fingerprint(self) -> str: + """Return the canonical registration fingerprint.""" + return self.catalog.fingerprint + + def assert_unchanged(self) -> None: + """Reject nested declaration drift before live component creation.""" + scene = self.scene_binding.declare() + articulation_operation_targets = _declared_articulation_operation_targets( + self.scene_binding + ) + profile = self.robot_profile_binding.declare() + try: + _validate_standard_call_catalog(self.call_catalog) + _validate_standard_effect_monitors(profile) + _validate_standard_tracking_metrics(profile) + extensions = build_standard_extension_declarations( + profile=profile, + endpoint_adapters=self.endpoint_adapters, + runtime_transports=self.runtime_transports, + parallel_safety_factory=self.parallel_safety_factory, + ) + relation_grounders = _snapshot_relation_grounders(self.relation_grounders) + relation_grounder_keys = frozenset( + _relation_grounder_key(grounder) for grounder in relation_grounders + ) + handover_pose_providers = _snapshot_handover_pose_providers( + self.handover_pose_providers + ) + current = _digest( + _registration_payload( + scene_binding=self.scene_binding, + scene=scene, + articulation_operation_targets=(articulation_operation_targets), + robot_profile_binding=self.robot_profile_binding, + robot_profile=profile, + call_catalog=self.call_catalog, + settle_presets=self.settle_presets, + relation_grounder_keys=relation_grounder_keys, + relation_grounders=relation_grounders, + handover_pose_providers=handover_pose_providers, + extensions=extensions, + endpoint_adapters=self.endpoint_adapters, + runtime_transports=self.runtime_transports, + parallel_safety_factory=self.parallel_safety_factory, + ) + ) + except (TypeError, ValueError) as exc: + raise IntegrationFingerprintMismatch( + "Expert Program integration provider declaration changed after " + "task registration." + ) from exc + if current != self.fingerprint: + raise IntegrationFingerprintMismatch( + "Expert Program integration declaration changed after task " + "registration." + ) + + @property + def endpoint_adapter_map( + self, + ) -> Mapping[type[ResourceEndpoint], ResourceEndpointAdapter]: + """Return custom live adapters keyed by their exact endpoint type.""" + return MappingProxyType( + { + getattr(type(adapter), "endpoint_type"): adapter + for adapter in self.endpoint_adapters + } + ) + + def create_parallel_safety_validator( + self, + *, + simulation: object, + robot: object, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + ) -> ParallelCommandSafetyValidator | None: + """Create and strictly validate the registration-owned live safety gate.""" + self.assert_unchanged() + factory = self.parallel_safety_factory + if factory is None: + return None + if type(scene_registry) is not SceneRegistry: + raise TypeError("scene_registry must be exactly SceneRegistry.") + if not isinstance(engine, AtomicActionEngine): + raise TypeError("engine must be an AtomicActionEngine.") + if engine.robot is not robot: + raise ValueError("engine and factory must reference the exact same robot.") + with self._parallel_safety_validator_lock: + validator = factory.create( + simulation=simulation, + robot=robot, + scene_registry=scene_registry, + engine=engine, + ) + if not isinstance(validator, ParallelCommandSafetyValidator): + raise TypeError( + "parallel_safety_factory.create() must return a " + "ParallelCommandSafetyValidator." + ) + if any( + validator is previous + for previous in self._parallel_safety_validator_history + ): + raise ValueError( + "ParallelCommandSafetyValidatorFactory.create() must return a " + "fresh validator for every runtime assembly owned by this " + "registration." + ) + self._parallel_safety_validator_history.append(validator) + return validator + + def validate_scene_registry(self, registry: SceneRegistry) -> None: + """Validate a live registry against the registered scene declaration.""" + self.assert_unchanged() + self.catalog.scene.validate_registry(registry) + + def validate_engine(self, engine: AtomicActionEngine) -> None: + """Validate live skills and resolved endpoints against this registration.""" + self.assert_unchanged() + self.catalog.validate_engine(engine) + + def validate_robot_profile( + self, + profile: RobotSkillProfile, + *, + step_dt: float, + ) -> None: + """Validate a live profile against its provider-free declaration.""" + self.assert_unchanged() + if type(profile) is not RobotSkillProfile: + raise TypeError("profile must be exactly RobotSkillProfile.") + expected = _profile_with_step_dt( + self.catalog.robot_profile, + step_dt=step_dt, + ) + if _canonical_json(profile) != _canonical_json(expected): + raise IntegrationFingerprintMismatch( + "Live robot skill profile differs from the registered declaration." + ) + + +__all__ = [ + "ExpertProgramIntegrationCatalog", + "IntegrationFingerprintMismatch", + "SimulationExpertProgramRegistration", +] diff --git a/embodichain/lab/gym/envs/expert_program/cfg.py b/embodichain/lab/gym/envs/expert_program/cfg.py new file mode 100644 index 000000000..04959ce1d --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/cfg.py @@ -0,0 +1,848 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Typed configuration values for declarative Expert Programs.""" + +from __future__ import annotations + +import math +import re +from dataclasses import MISSING, field +from typing import TypeAlias + +from embodichain.utils import configclass + +EXPERT_PROGRAM_SCHEMA_VERSION = 1 +"""Stable sequential Expert Program schema version.""" + +EXPERT_PROGRAM_SCHEMA_VERSION_V2 = 2 +"""Schema version adding deterministic ``Parallel`` and ``Barrier`` nodes.""" + +SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS = ( + EXPERT_PROGRAM_SCHEMA_VERSION, + EXPERT_PROGRAM_SCHEMA_VERSION_V2, +) +"""Exact schema revisions accepted by the strict decoder.""" + +MAX_REPEAT_COUNT = 1_000 +"""Maximum repeat count accepted by one Expert Program repeat node.""" + +MAX_EXPANDED_CALLS = 10_000 +"""Maximum statically expanded semantic calls in one Expert Program.""" + +MAX_PROGRAM_DEPTH = 64 +"""Maximum nesting depth of a supported Expert Program AST.""" + +MAX_PROGRAM_NODES = 10_000 +"""Maximum number of stored nodes in a supported Expert Program AST.""" + +MAX_DECLARATIVE_DEPTH = 32 +"""Maximum nesting depth of a registered-call declarative payload.""" + +MAX_DECLARATIVE_NODES = 10_000 +"""Maximum number of values in a registered-call declarative payload.""" + +_REGISTERED_CALL_ID_PATTERN = re.compile(r"[a-z][a-z0-9_]*(?:\.[a-z][a-z0-9_]*)+") +_ENV_TRAVERSAL_PATTERN = re.compile( + r"(?:\$?(?:env|environment)(?:\.[A-Za-z_][A-Za-z0-9_]*)+|" + r"\$\{(?:env|environment)(?:\.[A-Za-z_][A-Za-z0-9_]*)+\})" +) +_FORBIDDEN_DECLARATIVE_KEYS = frozenset( + { + "__import__", + "attribute_path", + "callable", + "environment_path", + "env_path", + "eval", + "exec", + "expression", + "getattr", + "import", + "module", + "python", + } +) + +DeclarativeCfgValue: TypeAlias = ( + None + | bool + | int + | float + | str + | tuple["DeclarativeCfgValue", ...] + | dict[str, "DeclarativeCfgValue"] +) +"""Executable-free value accepted by a registered semantic call config.""" + + +def _validate_identifier(value: object, *, field_name: str) -> str: + """Return one exact non-empty identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +def _validate_kind(value: object, *, expected: str, field_name: str) -> None: + """Require one exact discriminator value.""" + if type(value) is not str or value != expected: + raise ValueError(f"{field_name} must be exactly {expected!r}.") + + +def _validate_number(value: object, *, field_name: str) -> float: + """Return one finite number while rejecting bool values.""" + if type(value) not in (int, float): + raise TypeError(f"{field_name} must be an int or float.") + try: + normalized = float(value) + except OverflowError as error: + raise ValueError(f"{field_name} must be finite.") from error + if not math.isfinite(normalized): + raise ValueError(f"{field_name} must be finite.") + return normalized + + +def _validate_resources(value: object, *, field_name: str) -> dict[str, str]: + """Own one strict slot-to-resource mapping.""" + if type(value) is not dict: + raise TypeError(f"{field_name} must be an exact dict.") + resources: dict[str, str] = {} + for slot_id, resource_id in value.items(): + resources[ + _validate_identifier(slot_id, field_name=f"{field_name} slot IDs") + ] = _validate_identifier( + resource_id, + field_name=f"{field_name} resource IDs", + ) + return resources + + +def _validate_declarative_string(value: str, *, path: str) -> str: + """Reject strings that request executable or environment traversal behavior.""" + stripped = value.strip() + lowered = stripped.lower() + forbidden_prefixes = ( + "__import__(", + "eval(", + "exec(", + "import ", + "from ", + ) + if lowered.startswith(forbidden_prefixes): + raise ValueError(f"{path} contains an executable import/eval expression.") + if _ENV_TRAVERSAL_PATTERN.fullmatch(stripped) is not None: + raise ValueError(f"{path} contains dotted environment attribute traversal.") + return value + + +def _snapshot_declarative_value( + value: object, + *, + path: str, + _active: set[int] | None = None, + _budget: list[int] | None = None, + _depth: int = 0, +) -> DeclarativeCfgValue: + """Validate and own one bounded executable-free declarative value.""" + active = set() if _active is None else _active + budget = [MAX_DECLARATIVE_NODES] if _budget is None else _budget + if _depth > MAX_DECLARATIVE_DEPTH: + raise ValueError( + f"{path} exceeds declarative depth limit {MAX_DECLARATIVE_DEPTH}." + ) + budget[0] -= 1 + if budget[0] < 0: + raise ValueError( + f"{path} exceeds declarative node limit {MAX_DECLARATIVE_NODES}." + ) + if value is None or type(value) in (bool, int): + return value # type: ignore[return-value] + if type(value) is float: + if not math.isfinite(value): + raise ValueError(f"{path} contains a non-finite float.") + return value + if type(value) is str: + return _validate_declarative_string(value, path=path) + if type(value) in (list, tuple): + identity = id(value) + if identity in active: + raise ValueError(f"{path} contains a cyclic sequence.") + active.add(identity) + try: + return tuple( + _snapshot_declarative_value( + item, + path=f"{path}[{index}]", + _active=active, + _budget=budget, + _depth=_depth + 1, + ) + for index, item in enumerate(value) + ) + finally: + active.remove(identity) + if type(value) is dict: + identity = id(value) + if identity in active: + raise ValueError(f"{path} contains a cyclic mapping.") + active.add(identity) + try: + result: dict[str, DeclarativeCfgValue] = {} + for key, item in value.items(): + if type(key) is not str: + raise TypeError(f"{path} keys must be exact strings.") + if key.lower() in _FORBIDDEN_DECLARATIVE_KEYS: + raise ValueError( + f"{path}.{key} requests forbidden executable behavior." + ) + result[key] = _snapshot_declarative_value( + item, + path=f"{path}.{key}", + _active=active, + _budget=budget, + _depth=_depth + 1, + ) + return result + finally: + active.remove(identity) + raise TypeError( + f"{path} contains non-declarative {type(value).__name__}; callables, " + "classes, modules, tensors, and live objects are not allowed." + ) + + +@configclass +class ExpertProgramIntegrationCfg: + """Static integration references selected by one Expert Program.""" + + robot_profile: str = MISSING + scene_registry: str = MISSING + runtime_preset: str = MISSING + + def __post_init__(self) -> None: + """Validate stable integration identifiers.""" + _validate_identifier(self.robot_profile, field_name="robot_profile") + _validate_identifier(self.scene_registry, field_name="scene_registry") + _validate_identifier(self.runtime_preset, field_name="runtime_preset") + + +@configclass +class PoseCfg: + """One declarative Cartesian pose using a WXYZ quaternion.""" + + position: tuple[float, float, float] = MISSING + quaternion_wxyz: tuple[float, float, float, float] = MISSING + + def __post_init__(self) -> None: + """Validate pose shape, finiteness, and quaternion magnitude.""" + if type(self.position) not in (list, tuple) or len(self.position) != 3: + raise ValueError("position must contain exactly three numbers.") + if ( + type(self.quaternion_wxyz) not in (list, tuple) + or len(self.quaternion_wxyz) != 4 + ): + raise ValueError("quaternion_wxyz must contain exactly four numbers.") + position = tuple( + _validate_number(value, field_name=f"position[{index}]") + for index, value in enumerate(self.position) + ) + quaternion = tuple( + _validate_number(value, field_name=f"quaternion_wxyz[{index}]") + for index, value in enumerate(self.quaternion_wxyz) + ) + norm = math.sqrt(sum(value * value for value in quaternion)) + if norm <= 1.0e-12: + raise ValueError("quaternion_wxyz must have non-zero magnitude.") + self.position = position # type: ignore[assignment] + self.quaternion_wxyz = quaternion # type: ignore[assignment] + + +@configclass +class TargetRefCfg: + """Reference to one top-level typed target provider.""" + + target: str = MISSING + kind: str = "target_ref" + + def __post_init__(self) -> None: + """Validate the target identifier and discriminator.""" + _validate_identifier(self.target, field_name="target") + _validate_kind(self.kind, expected="target_ref", field_name="kind") + + +@configclass +class CyclicPoseTargetCfg: + """Finite pose values selected cyclically by the enclosing repeat index.""" + + values: tuple[PoseCfg, ...] = MISSING + kind: str = "cyclic_pose" + + def __post_init__(self) -> None: + """Validate a non-empty owned pose sequence.""" + _validate_kind(self.kind, expected="cyclic_pose", field_name="kind") + if type(self.values) not in (list, tuple) or not self.values: + raise ValueError("values must contain at least one PoseCfg.") + values = tuple(self.values) + if not all(type(value) is PoseCfg for value in values): + raise TypeError("values must contain exact PoseCfg values.") + self.values = values # type: ignore[assignment] + + +TargetCfg: TypeAlias = CyclicPoseTargetCfg + + +@configclass +class PickCfg: + """Declarative request to acquire one registered object.""" + + object: str = MISSING + grasp: str | None = None + resources: dict[str, str] = field(default_factory=dict) + kind: str = "pick" + + def __post_init__(self) -> None: + """Validate object, optional affordance, resources, and kind.""" + _validate_identifier(self.object, field_name="object") + if self.grasp is not None: + _validate_identifier(self.grasp, field_name="grasp") + self.resources = _validate_resources(self.resources, field_name="resources") + _validate_kind(self.kind, expected="pick", field_name="kind") + + +@configclass +class PlaceCfg: + """Declarative request to place one held object at one destination.""" + + object: str = MISSING + at: TargetRefCfg | None = None + on: str | None = None + inside: str | None = None + resources: dict[str, str] = field(default_factory=dict) + kind: str = "place" + + def __post_init__(self) -> None: + """Require exactly one typed destination.""" + _validate_identifier(self.object, field_name="object") + selected = sum(value is not None for value in (self.at, self.on, self.inside)) + if selected != 1: + raise ValueError("Place requires exactly one of at, on, or inside.") + if self.at is not None and type(self.at) is not TargetRefCfg: + raise TypeError("at must be exactly TargetRefCfg or None.") + if self.on is not None: + _validate_identifier(self.on, field_name="on") + if self.inside is not None: + _validate_identifier(self.inside, field_name="inside") + self.resources = _validate_resources(self.resources, field_name="resources") + _validate_kind(self.kind, expected="place", field_name="kind") + + +@configclass +class HandOverCfg: + """Declarative request to transfer one held object between resources.""" + + object: str = MISSING + final_target: TargetRefCfg | None = None + resources: dict[str, str] = field(default_factory=dict) + kind: str = "hand_over" + + def __post_init__(self) -> None: + """Validate object, destination resource, and optional target.""" + _validate_identifier(self.object, field_name="object") + if ( + self.final_target is not None + and type(self.final_target) is not TargetRefCfg + ): + raise TypeError("final_target must be exactly TargetRefCfg or None.") + self.resources = _validate_resources(self.resources, field_name="resources") + _validate_kind(self.kind, expected="hand_over", field_name="kind") + + +@configclass +class OperateArticulationCfg: + """Declarative request to operate one articulated joint through a handle.""" + + articulation: str = MISSING + handle: str | None = None + target: str | None = None + target_position: float | None = None + target_displacement: float | None = None + resources: dict[str, str] = field(default_factory=dict) + kind: str = "operate_articulation" + + def __post_init__(self) -> None: + """Require one named target or one complete explicit target pair.""" + _validate_identifier(self.articulation, field_name="articulation") + if self.handle is not None: + _validate_identifier(self.handle, field_name="handle") + named = self.target is not None + explicit_position = self.target_position is not None + explicit_displacement = self.target_displacement is not None + if named: + _validate_identifier(self.target, field_name="target") + if explicit_position or explicit_displacement: + raise ValueError( + "target is mutually exclusive with target_position and " + "target_displacement." + ) + elif not (explicit_position and explicit_displacement): + raise ValueError( + "OperateArticulation requires either target or the explicit " + "target_position and target_displacement pair." + ) + else: + self.target_position = _validate_number( + self.target_position, + field_name="target_position", + ) + self.target_displacement = _validate_number( + self.target_displacement, + field_name="target_displacement", + ) + self.resources = _validate_resources(self.resources, field_name="resources") + _validate_kind( + self.kind, + expected="operate_articulation", + field_name="kind", + ) + + +@configclass +class RegisteredSemanticCallCfg: + """Safe declarative payload for one catalog-registered semantic call.""" + + call_id: str = MISSING + schema_version: int = EXPERT_PROGRAM_SCHEMA_VERSION + arguments: dict[str, DeclarativeCfgValue] = field(default_factory=dict) + resources: dict[str, str] = field(default_factory=dict) + kind: str = "registered" + + def __post_init__(self) -> None: + """Validate versioned ID and recursively executable-free arguments.""" + _validate_identifier(self.call_id, field_name="call_id") + if _REGISTERED_CALL_ID_PATTERN.fullmatch(self.call_id) is None: + raise ValueError( + "call_id must contain two or more lowercase identifier segments " + "separated by single dots." + ) + if type(self.schema_version) is not int or self.schema_version != 1: + raise ValueError("Registered call schema_version must be exactly 1.") + if type(self.arguments) is not dict: + raise TypeError("arguments must be an exact dict.") + arguments = _snapshot_declarative_value( + self.arguments, + path="arguments", + ) + assert type(arguments) is dict + self.arguments = arguments + self.resources = _validate_resources(self.resources, field_name="resources") + _validate_kind(self.kind, expected="registered", field_name="kind") + + +SemanticCallCfg: TypeAlias = ( + PickCfg + | PlaceCfg + | HandOverCfg + | OperateArticulationCfg + | RegisteredSemanticCallCfg +) + + +@configclass +class WaitStablePostCfg: + """Wait for one registered entity to satisfy a named stability preset.""" + + entity: str = MISSING + preset: str = "rigid_object" + kind: str = "wait_stable" + + def __post_init__(self) -> None: + """Validate entity, preset, and discriminator.""" + _validate_identifier(self.entity, field_name="entity") + _validate_identifier(self.preset, field_name="preset") + _validate_kind(self.kind, expected="wait_stable", field_name="kind") + + +PostPolicyCfg: TypeAlias = WaitStablePostCfg + + +@configclass +class ObjectNearTargetValidatorCfg: + """Validate an object's position against one resolved target.""" + + object: str = MISSING + target: str = MISSING + position_tolerance: float = 0.03 + kind: str = "object_near_target" + + def __post_init__(self) -> None: + """Validate reference IDs and a positive finite tolerance.""" + _validate_identifier(self.object, field_name="object") + _validate_identifier(self.target, field_name="target") + tolerance = _validate_number( + self.position_tolerance, + field_name="position_tolerance", + ) + if tolerance <= 0.0: + raise ValueError("position_tolerance must be positive.") + self.position_tolerance = tolerance + _validate_kind( + self.kind, + expected="object_near_target", + field_name="kind", + ) + + +ValidatorCfg: TypeAlias = ObjectNearTargetValidatorCfg + + +@configclass +class InvokeCfg: + """Invoke exactly one semantic call at the current program boundary.""" + + call: SemanticCallCfg = MISSING + kind: str = "invoke" + + def __post_init__(self) -> None: + """Validate the semantic-call union and discriminator.""" + if type(self.call) not in _SEMANTIC_CALL_TYPES: + raise TypeError("call must be an exact SemanticCallCfg value.") + _validate_kind(self.kind, expected="invoke", field_name="kind") + + +@configclass +class BarrierCfg: + """Explicit synchronization boundary owned by one parallel node.""" + + name: str = "join" + timeout_steps: int = 1_000 + failure_policy: str = "fail_fast" + kind: str = "barrier" + + def __post_init__(self) -> None: + """Validate deterministic timeout and cancellation semantics.""" + _validate_kind(self.kind, expected="barrier", field_name="kind") + _validate_identifier(self.name, field_name="name") + if type(self.timeout_steps) is not int or self.timeout_steps <= 0: + raise ValueError("timeout_steps must be a positive integer.") + if self.failure_policy != "fail_fast": + raise ValueError("failure_policy must be exactly 'fail_fast'.") + + +@configclass +class SequenceCfg: + """Execute one non-empty ordered tuple of program nodes.""" + + items: tuple[ProgramNodeCfg, ...] = MISSING + kind: str = "sequence" + + def __post_init__(self) -> None: + """Validate ordered child nodes and discriminator.""" + _validate_kind(self.kind, expected="sequence", field_name="kind") + if type(self.items) not in (list, tuple) or not self.items: + raise ValueError("items must contain at least one program node.") + items = tuple(self.items) + if not all(type(item) in _PROGRAM_NODE_TYPES for item in items): + raise TypeError("items must contain exact ProgramNodeCfg values.") + self.items = items # type: ignore[assignment] + + +@configclass +class RepeatCfg: + """Repeat one child node a finite validated number of times.""" + + count: int = MISSING + body: ProgramNodeCfg = MISSING + kind: str = "repeat" + + def __post_init__(self) -> None: + """Validate a bounded positive repeat and its child node.""" + if type(self.count) is not int or not 1 <= self.count <= MAX_REPEAT_COUNT: + raise ValueError(f"count must be an integer in [1, {MAX_REPEAT_COUNT}].") + if type(self.body) not in _PROGRAM_NODE_TYPES: + raise TypeError("body must be an exact ProgramNodeCfg value.") + _validate_kind(self.kind, expected="repeat", field_name="kind") + + +@configclass +class SegmentCfg: + """Logical program transaction with post-policies and validators.""" + + name: str = MISSING + steps: ProgramNodeCfg = MISSING + post: tuple[PostPolicyCfg, ...] = field(default_factory=tuple) + validators: tuple[ValidatorCfg, ...] = field(default_factory=tuple) + kind: str = "segment" + + def __post_init__(self) -> None: + """Validate the segment boundary and its declarative hooks.""" + _validate_identifier(self.name, field_name="name") + if type(self.steps) not in _PROGRAM_NODE_TYPES: + raise TypeError("steps must be an exact ProgramNodeCfg value.") + if type(self.post) not in (list, tuple): + raise TypeError("post must be a list or tuple.") + if type(self.validators) not in (list, tuple): + raise TypeError("validators must be a list or tuple.") + post = tuple(self.post) + validators = tuple(self.validators) + if not all(type(value) in _POST_POLICY_TYPES for value in post): + raise TypeError("post must contain exact PostPolicyCfg values.") + if not all(type(value) in _VALIDATOR_TYPES for value in validators): + raise TypeError("validators must contain exact ValidatorCfg values.") + self.post = post # type: ignore[assignment] + self.validators = validators # type: ignore[assignment] + _validate_kind(self.kind, expected="segment", field_name="kind") + + +@configclass +class ParallelCfg: + """Execute two or more branches concurrently and join at one barrier.""" + + branches: tuple[ProgramNodeCfg, ...] = MISSING + barrier: BarrierCfg = MISSING + kind: str = "parallel" + + def __post_init__(self) -> None: + """Validate branch ownership and an explicit synchronization node.""" + _validate_kind(self.kind, expected="parallel", field_name="kind") + if type(self.branches) not in (list, tuple) or len(self.branches) < 2: + raise ValueError("branches must contain at least two program nodes.") + branches = tuple(self.branches) + if not all(type(branch) in _PROGRAM_NODE_TYPES for branch in branches): + raise TypeError("branches must contain exact ProgramNodeCfg values.") + if any(type(branch) in (ParallelCfg, BarrierCfg) for branch in branches): + raise ValueError( + "Nested Parallel and standalone Barrier branches are forbidden." + ) + if type(self.barrier) is not BarrierCfg: + raise TypeError("barrier must be exactly BarrierCfg.") + self.branches = branches # type: ignore[assignment] + + +ProgramNodeCfg: TypeAlias = ( + SequenceCfg | RepeatCfg | SegmentCfg | InvokeCfg | ParallelCfg | BarrierCfg +) + +_SEMANTIC_CALL_TYPES = ( + PickCfg, + PlaceCfg, + HandOverCfg, + OperateArticulationCfg, + RegisteredSemanticCallCfg, +) +_POST_POLICY_TYPES = (WaitStablePostCfg,) +_VALIDATOR_TYPES = (ObjectNearTargetValidatorCfg,) +_PROGRAM_NODE_TYPES = ( + SequenceCfg, + RepeatCfg, + SegmentCfg, + InvokeCfg, + ParallelCfg, + BarrierCfg, +) + + +def _validate_target_reference(target: str, targets: dict[str, TargetCfg]) -> None: + """Require one target reference to exist in the top-level registry.""" + if target not in targets: + raise ValueError(f"Unknown target reference {target!r}.") + + +def _validate_program( + node: ProgramNodeCfg, + *, + targets: dict[str, TargetCfg], + depth: int, + budget: list[int], + schema_version: int, + inside_parallel: bool = False, +) -> int: + """Validate references and return the statically expanded call count.""" + if depth > MAX_PROGRAM_DEPTH: + raise ValueError(f"Program exceeds depth limit {MAX_PROGRAM_DEPTH}.") + budget[0] -= 1 + if budget[0] < 0: + raise ValueError(f"Program exceeds node limit {MAX_PROGRAM_NODES}.") + if type(node) is InvokeCfg: + call = node.call + if type(call) is PlaceCfg and call.at is not None: + _validate_target_reference(call.at.target, targets) + if type(call) is HandOverCfg and call.final_target is not None: + _validate_target_reference(call.final_target.target, targets) + return 1 + if type(node) is BarrierCfg: + if schema_version < EXPERT_PROGRAM_SCHEMA_VERSION_V2: + raise ValueError("Barrier requires Expert Program schema version 2.") + if not inside_parallel: + raise ValueError("Barrier nodes may only be owned by Parallel.") + return 0 + if type(node) is SequenceCfg: + expanded = sum( + _validate_program( + child, + targets=targets, + depth=depth + 1, + budget=budget, + schema_version=schema_version, + inside_parallel=inside_parallel, + ) + for child in node.items + ) + elif type(node) is RepeatCfg: + expanded = node.count * _validate_program( + node.body, + targets=targets, + depth=depth + 1, + budget=budget, + schema_version=schema_version, + inside_parallel=inside_parallel, + ) + elif type(node) is SegmentCfg: + if inside_parallel: + raise ValueError( + "Parallel branches may contain only Invoke, Sequence, and Repeat " + "nodes; wrap the Parallel node in one Segment instead." + ) + for validator in node.validators: + _validate_target_reference(validator.target, targets) + expanded = _validate_program( + node.steps, + targets=targets, + depth=depth + 1, + budget=budget, + schema_version=schema_version, + inside_parallel=inside_parallel, + ) + elif type(node) is ParallelCfg: + if schema_version < EXPERT_PROGRAM_SCHEMA_VERSION_V2: + raise ValueError("Parallel requires Expert Program schema version 2.") + if inside_parallel: + raise ValueError("Nested Parallel nodes are forbidden in schema version 2.") + branch_counts = tuple( + _validate_program( + branch, + targets=targets, + depth=depth + 1, + budget=budget, + schema_version=schema_version, + inside_parallel=True, + ) + for branch in node.branches + ) + if any(count <= 0 for count in branch_counts): + raise ValueError("Every Parallel branch must contain a semantic call.") + _validate_program( + node.barrier, + targets=targets, + depth=depth + 1, + budget=budget, + schema_version=schema_version, + inside_parallel=True, + ) + expanded = sum(branch_counts) + else: # pragma: no cover - exact construction prevents this branch + raise TypeError("program must contain exact ProgramNodeCfg values.") + if expanded > MAX_EXPANDED_CALLS: + raise ValueError( + f"Program expands to more than {MAX_EXPANDED_CALLS} semantic calls." + ) + return expanded + + +@configclass +class ExpertProgramCfg: + """Strict, versioned, executable-free Expert Program configuration.""" + + schema_version: int = MISSING + program_id: str = MISSING + integration: ExpertProgramIntegrationCfg = MISSING + program: ProgramNodeCfg = MISSING + targets: dict[str, TargetCfg] = field(default_factory=dict) + + def __post_init__(self) -> None: + """Validate the complete static configuration and target graph.""" + if ( + type(self.schema_version) is not int + or self.schema_version not in SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS + ): + raise ValueError( + "schema_version must be one of " + f"{SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS}." + ) + _validate_identifier(self.program_id, field_name="program_id") + if type(self.integration) is not ExpertProgramIntegrationCfg: + raise TypeError("integration must be ExpertProgramIntegrationCfg.") + if type(self.targets) is not dict: + raise TypeError("targets must be an exact dict.") + targets: dict[str, TargetCfg] = {} + for target_id, target in self.targets.items(): + normalized_id = _validate_identifier( + target_id, + field_name="target IDs", + ) + if type(target) is not CyclicPoseTargetCfg: + raise TypeError("targets must contain exact TargetCfg values.") + targets[normalized_id] = target + if type(self.program) not in _PROGRAM_NODE_TYPES: + raise TypeError("program must be an exact ProgramNodeCfg value.") + expanded = _validate_program( + self.program, + targets=targets, + depth=0, + budget=[MAX_PROGRAM_NODES], + schema_version=self.schema_version, + ) + if expanded <= 0: + raise ValueError("program must contain at least one semantic call.") + self.targets = targets + + +__all__ = [ + "BarrierCfg", + "CyclicPoseTargetCfg", + "DeclarativeCfgValue", + "EXPERT_PROGRAM_SCHEMA_VERSION", + "EXPERT_PROGRAM_SCHEMA_VERSION_V2", + "ExpertProgramCfg", + "ExpertProgramIntegrationCfg", + "HandOverCfg", + "InvokeCfg", + "MAX_DECLARATIVE_DEPTH", + "MAX_DECLARATIVE_NODES", + "MAX_EXPANDED_CALLS", + "MAX_PROGRAM_DEPTH", + "MAX_PROGRAM_NODES", + "MAX_REPEAT_COUNT", + "ObjectNearTargetValidatorCfg", + "OperateArticulationCfg", + "ParallelCfg", + "PickCfg", + "PlaceCfg", + "PoseCfg", + "PostPolicyCfg", + "ProgramNodeCfg", + "RegisteredSemanticCallCfg", + "RepeatCfg", + "SegmentCfg", + "SemanticCallCfg", + "SequenceCfg", + "SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS", + "TargetCfg", + "TargetRefCfg", + "ValidatorCfg", + "WaitStablePostCfg", +] diff --git a/embodichain/lab/gym/envs/expert_program/compiler.py b/embodichain/lab/gym/envs/expert_program/compiler.py new file mode 100644 index 000000000..ca01f6663 --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/compiler.py @@ -0,0 +1,1908 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Provider-free compilation and lazy expansion of Expert Program ASTs.""" + +from __future__ import annotations + +from collections.abc import Iterator, Mapping +from dataclasses import dataclass, field +from types import MappingProxyType +from typing import Protocol, runtime_checkable + +from embodichain.lab.sim.skills.calls import ( + DeclarativeValue, + HandOver, + OperateArticulation, + Pick, + Place, + RegisteredSemanticCall, + SemanticCallSpec, + SemanticPose, +) +from embodichain.lab.sim.skills.scene import ( + SceneAffordanceRef, + SceneArticulationRef, + SceneEntityRef, + SceneLinkRef, + SceneObjectRef, + SceneRegistry, +) + +from .cfg import ( + EXPERT_PROGRAM_SCHEMA_VERSION, + EXPERT_PROGRAM_SCHEMA_VERSION_V2, + MAX_EXPANDED_CALLS, + MAX_REPEAT_COUNT, + SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS, + BarrierCfg, + CyclicPoseTargetCfg, + ExpertProgramCfg, + ExpertProgramIntegrationCfg, + HandOverCfg, + InvokeCfg, + ObjectNearTargetValidatorCfg, + OperateArticulationCfg, + ParallelCfg, + PickCfg, + PlaceCfg, + PoseCfg, + ProgramNodeCfg, + RegisteredSemanticCallCfg, + RepeatCfg, + SegmentCfg, + SequenceCfg, + TargetRefCfg, + WaitStablePostCfg, +) +from .decoder import ConfigPath, ExpertProgramConfigError, render_config_path + +_SEMANTIC_CALL_TYPES = ( + Pick, + Place, + HandOver, + OperateArticulation, + RegisteredSemanticCall, +) +_SCENE_REF_TYPES = ( + SceneEntityRef, + SceneObjectRef, + SceneArticulationRef, + SceneLinkRef, + SceneAffordanceRef, +) + + +class ExpertProgramCompileError(ExpertProgramConfigError): + """Raised when a validated AST cannot lower to canonical semantic calls.""" + + +@runtime_checkable +class ExpertProgramSceneResolver(Protocol): + """Provider-free typed resolver for canonical static scene references.""" + + def resolve( + self, + reference: str, + *, + expected_types: tuple[type[SceneEntityRef], ...], + path: ConfigPath, + ) -> SceneEntityRef: + """Resolve one canonical or aliased ID without observing scene state.""" + + +def _copy_scene_ref(reference: SceneEntityRef) -> SceneEntityRef: + """Return one independent exact typed scene reference.""" + if type(reference) not in _SCENE_REF_TYPES: + raise TypeError(f"Unsupported scene reference {type(reference).__name__}.") + return type(reference)(reference.entity_id) + + +class SceneRegistryProgramResolver: + """Provider-free static resolver snapshotted from one SceneRegistry. + + The resolver copies only canonical typed references and aliases. It does not + retain registrations, state providers, geometry providers, or the registry + itself, so compilation cannot observe dynamic scene state. + """ + + def __init__(self, registry: SceneRegistry) -> None: + """Snapshot the registry's static identity table. + + Args: + registry: Authoritative registry used only for static identity data. + """ + if type(registry) is not SceneRegistry: + raise TypeError("registry must be exactly SceneRegistry.") + references = { + reference.entity_id: _copy_scene_ref(reference) + for reference in registry.entity_refs + } + self._references = MappingProxyType(references) + self._aliases = MappingProxyType(dict(registry.aliases)) + + @property + def canonical_references(self) -> Mapping[str, SceneEntityRef]: + """Return an independent canonical typed-reference mapping.""" + return MappingProxyType( + { + entity_id: _copy_scene_ref(reference) + for entity_id, reference in self._references.items() + } + ) + + def resolve( + self, + reference: str, + *, + expected_types: tuple[type[SceneEntityRef], ...], + path: ConfigPath, + ) -> SceneEntityRef: + """Resolve one ID or alias through the snapshotted type table.""" + if ( + type(reference) is not str + or not reference + or reference != reference.strip() + ): + raise ExpertProgramCompileError( + "invalid_scene_reference", + path, + "Scene references must be non-empty strings without outer whitespace.", + ) + if ( + type(expected_types) is not tuple + or not expected_types + or not all( + isinstance(expected_type, type) + and issubclass(expected_type, SceneEntityRef) + for expected_type in expected_types + ) + ): + raise TypeError( + "expected_types must be a non-empty tuple of SceneEntityRef types." + ) + canonical_id = self._aliases.get(reference, reference) + resolved = self._references.get(canonical_id) + if resolved is None: + raise ExpertProgramCompileError( + "unknown_scene_reference", + path, + f"Unknown scene reference {reference!r}.", + ) + if type(resolved) not in expected_types: + expected_names = tuple(value.__name__ for value in expected_types) + raise ExpertProgramCompileError( + "scene_reference_type_mismatch", + path, + f"Scene reference {reference!r} resolves to " + f"{type(resolved).__name__}, expected one of {expected_names}.", + ) + return _copy_scene_ref(resolved) + + +@dataclass(frozen=True, slots=True) +class CompiledRepeatFrame: + """One lexical repeat occurrence in a compiled call or segment path.""" + + path: ConfigPath + iteration_index: int + count: int + + def __post_init__(self) -> None: + if type(self.path) is not tuple: + raise TypeError("path must be a ConfigPath tuple.") + if type(self.iteration_index) is not int or not 0 <= self.iteration_index: + raise ValueError("iteration_index must be a non-negative integer.") + if type(self.count) is not int or self.count <= 0: + raise ValueError("count must be a positive integer.") + if self.iteration_index >= self.count: + raise ValueError("iteration_index must be smaller than count.") + + +@dataclass(frozen=True, slots=True) +class CompiledTargetSelection: + """Deterministic cyclic-target selection metadata for one occurrence.""" + + target_id: str + value_index: int + repeat_path: ConfigPath | None + repeat_iteration_index: int | None + + def __post_init__(self) -> None: + if type(self.target_id) is not str or not self.target_id: + raise ValueError("target_id must be a non-empty string.") + if type(self.value_index) is not int or self.value_index < 0: + raise ValueError("value_index must be a non-negative integer.") + if (self.repeat_path is None) != (self.repeat_iteration_index is None): + raise ValueError( + "repeat_path and repeat_iteration_index must both be set or unset." + ) + if self.repeat_path is not None and type(self.repeat_path) is not tuple: + raise TypeError("repeat_path must be a ConfigPath tuple or None.") + if self.repeat_iteration_index is not None and ( + type(self.repeat_iteration_index) is not int + or self.repeat_iteration_index < 0 + ): + raise ValueError("repeat_iteration_index must be non-negative or None.") + + +def _snapshot_semantic_call(call: SemanticCallSpec) -> SemanticCallSpec: + """Return one independently owned exact semantic-call value.""" + if type(call) is Pick: + return Pick( + object=_copy_scene_ref(call.object), + grasp=(None if call.grasp is None else _copy_scene_ref(call.grasp)), + resources=dict(call.resources), + ) + if type(call) is Place: + return Place( + object=_copy_scene_ref(call.object), + at=None if call.at is None else call.at.snapshot(), + on=None if call.on is None else _copy_scene_ref(call.on), + inside=None if call.inside is None else _copy_scene_ref(call.inside), + resources=dict(call.resources), + ) + if type(call) is HandOver: + return HandOver( + object=_copy_scene_ref(call.object), + final_target=( + None if call.final_target is None else call.final_target.snapshot() + ), + resources=dict(call.resources), + ) + if type(call) is OperateArticulation: + return OperateArticulation( + articulation=_copy_scene_ref(call.articulation), + handle=(None if call.handle is None else _copy_scene_ref(call.handle)), + target=call.target, + target_position=call.target_position, + target_displacement=call.target_displacement, + resources=dict(call.resources), + ) + if type(call) is RegisteredSemanticCall: + return RegisteredSemanticCall( + call_id=call.call_id, + arguments=call.arguments, + resources=dict(call.resources), + ) + raise TypeError("call must be an exact supported SemanticCallSpec value.") + + +@dataclass(frozen=True, slots=True) +class CompiledProgramCall: + """One owned semantic call occurrence emitted by lazy program expansion.""" + + call_index: int + segment_call_index: int + call: SemanticCallSpec + source_path: ConfigPath + repeat_frames: tuple[CompiledRepeatFrame, ...] = () + target_selections: tuple[CompiledTargetSelection, ...] = () + + def __post_init__(self) -> None: + if type(self.call_index) is not int or self.call_index < 0: + raise ValueError("call_index must be a non-negative integer.") + if type(self.segment_call_index) is not int or self.segment_call_index < 0: + raise ValueError("segment_call_index must be a non-negative integer.") + if type(self.call) not in _SEMANTIC_CALL_TYPES: + raise TypeError("call must be an exact supported SemanticCallSpec value.") + if type(self.source_path) is not tuple: + raise TypeError("source_path must be a ConfigPath tuple.") + frames = tuple(self.repeat_frames) + selections = tuple(self.target_selections) + if not all(type(frame) is CompiledRepeatFrame for frame in frames): + raise TypeError("repeat_frames must contain CompiledRepeatFrame values.") + if not all( + type(selection) is CompiledTargetSelection for selection in selections + ): + raise TypeError( + "target_selections must contain CompiledTargetSelection values." + ) + object.__setattr__(self, "call", _snapshot_semantic_call(self.call)) + object.__setattr__(self, "repeat_frames", frames) + object.__setattr__(self, "target_selections", selections) + + +@dataclass(frozen=True, slots=True) +class CompiledPostPolicy: + """Owned post-policy config plus its canonical scene entity and source path.""" + + cfg: WaitStablePostCfg + entity: SceneEntityRef + source_path: ConfigPath + + def __post_init__(self) -> None: + if type(self.cfg) is not WaitStablePostCfg: + raise TypeError("cfg must be exactly WaitStablePostCfg.") + if type(self.entity) not in _SCENE_REF_TYPES: + raise TypeError("entity must be an exact SceneEntityRef value.") + if type(self.source_path) is not tuple: + raise TypeError("source_path must be a ConfigPath tuple.") + object.__setattr__( + self, + "cfg", + WaitStablePostCfg( + entity=self.cfg.entity, + preset=self.cfg.preset, + kind=self.cfg.kind, + ), + ) + object.__setattr__(self, "entity", _copy_scene_ref(self.entity)) + + +@dataclass(frozen=True, slots=True) +class CompiledProgramValidator: + """Owned validator config with canonical object and resolved target pose.""" + + cfg: ObjectNearTargetValidatorCfg + object: SceneObjectRef + target_pose: SemanticPose + target_selection: CompiledTargetSelection + source_path: ConfigPath + + def __post_init__(self) -> None: + if type(self.cfg) is not ObjectNearTargetValidatorCfg: + raise TypeError("cfg must be exactly ObjectNearTargetValidatorCfg.") + if type(self.object) is not SceneObjectRef: + raise TypeError("object must be exactly SceneObjectRef.") + if type(self.target_pose) is not SemanticPose: + raise TypeError("target_pose must be exactly SemanticPose.") + if type(self.target_selection) is not CompiledTargetSelection: + raise TypeError("target_selection must be exactly CompiledTargetSelection.") + if type(self.source_path) is not tuple: + raise TypeError("source_path must be a ConfigPath tuple.") + object.__setattr__( + self, + "cfg", + ObjectNearTargetValidatorCfg( + object=self.cfg.object, + target=self.cfg.target, + position_tolerance=self.cfg.position_tolerance, + kind=self.cfg.kind, + ), + ) + object.__setattr__(self, "object", _copy_scene_ref(self.object)) + object.__setattr__(self, "target_pose", self.target_pose.snapshot()) + + +@dataclass(frozen=True, slots=True) +class CompiledBarrier: + """Explicit schema-v2 join semantics for one compiled parallel block.""" + + name: str + timeout_steps: int + failure_policy: str + source_path: ConfigPath + + def __post_init__(self) -> None: + if type(self.name) is not str or not self.name: + raise ValueError("barrier name must be non-empty.") + if type(self.timeout_steps) is not int or self.timeout_steps <= 0: + raise ValueError("barrier timeout_steps must be positive.") + if self.failure_policy != "fail_fast": + raise ValueError("barrier failure_policy must be 'fail_fast'.") + if type(self.source_path) is not tuple: + raise TypeError("barrier source_path must be a ConfigPath tuple.") + + +@dataclass(frozen=True, slots=True) +class CompiledParallelBranch: + """One ordered semantic-call lane inside a parallel block.""" + + branch_index: int + calls: tuple[CompiledProgramCall, ...] + source_path: ConfigPath + + def __post_init__(self) -> None: + if type(self.branch_index) is not int or self.branch_index < 0: + raise ValueError("branch_index must be non-negative.") + calls = tuple(self.calls) + if not calls or not all(type(call) is CompiledProgramCall for call in calls): + raise TypeError("parallel branch calls must be non-empty compiled calls.") + if type(self.source_path) is not tuple: + raise TypeError("parallel branch source_path must be a ConfigPath tuple.") + object.__setattr__(self, "calls", calls) + + +@dataclass(frozen=True, slots=True) +class CompiledParallelBlock: + """Two or more call lanes joined by an explicit deterministic barrier.""" + + branches: tuple[CompiledParallelBranch, ...] + barrier: CompiledBarrier + source_path: ConfigPath + + def __post_init__(self) -> None: + branches = tuple(self.branches) + if len(branches) < 2 or not all( + type(branch) is CompiledParallelBranch for branch in branches + ): + raise TypeError("parallel blocks require at least two compiled branches.") + if tuple(branch.branch_index for branch in branches) != tuple( + range(len(branches)) + ): + raise ValueError("parallel branch indices must be contiguous from zero.") + if type(self.barrier) is not CompiledBarrier: + raise TypeError("barrier must be exactly CompiledBarrier.") + if type(self.source_path) is not tuple: + raise TypeError("parallel source_path must be a ConfigPath tuple.") + object.__setattr__(self, "branches", branches) + + +@dataclass(frozen=True, slots=True) +class CompiledProgramSegment: + """One independent explicit or implicit logical program segment.""" + + segment_index: int + segment_id: str + name: str + calls: tuple[CompiledProgramCall, ...] + source_path: ConfigPath + repeat_frames: tuple[CompiledRepeatFrame, ...] = () + post_policies: tuple[CompiledPostPolicy, ...] = () + validators: tuple[CompiledProgramValidator, ...] = () + parallel_block: CompiledParallelBlock | None = None + implicit: bool = False + + def __post_init__(self) -> None: + if type(self.segment_index) is not int or self.segment_index < 0: + raise ValueError("segment_index must be a non-negative integer.") + for field_name in ("segment_id", "name"): + value = getattr(self, field_name) + if type(value) is not str or not value: + raise ValueError(f"{field_name} must be a non-empty string.") + calls = tuple(self.calls) + if not calls or not all(type(call) is CompiledProgramCall for call in calls): + raise TypeError( + "calls must contain at least one exact CompiledProgramCall." + ) + if tuple(call.segment_call_index for call in calls) != tuple(range(len(calls))): + raise ValueError("segment call indices must be contiguous from zero.") + if type(self.source_path) is not tuple: + raise TypeError("source_path must be a ConfigPath tuple.") + frames = tuple(self.repeat_frames) + post = tuple(self.post_policies) + validators = tuple(self.validators) + if not all(type(frame) is CompiledRepeatFrame for frame in frames): + raise TypeError("repeat_frames must contain CompiledRepeatFrame values.") + if not all(type(value) is CompiledPostPolicy for value in post): + raise TypeError("post_policies must contain CompiledPostPolicy values.") + if not all(type(value) is CompiledProgramValidator for value in validators): + raise TypeError("validators must contain CompiledProgramValidator values.") + if type(self.implicit) is not bool: + raise TypeError("implicit must be a bool.") + if self.implicit and (post or validators): + raise ValueError( + "Implicit segments cannot own post-policies or validators." + ) + if self.parallel_block is not None: + if type(self.parallel_block) is not CompiledParallelBlock: + raise TypeError("parallel_block must be CompiledParallelBlock or None.") + flattened = tuple( + call for branch in self.parallel_block.branches for call in branch.calls + ) + if flattened != calls: + raise ValueError( + "segment calls must equal parallel branch calls in branch order." + ) + object.__setattr__(self, "calls", calls) + object.__setattr__(self, "repeat_frames", frames) + object.__setattr__(self, "post_policies", post) + object.__setattr__(self, "validators", validators) + + +@dataclass(frozen=True, slots=True) +class CompiledProgramAnalysis: + """One owned canonical semantic-analysis window for a compiled program. + + ``execution_prefix_length`` separates calls that the current segment owns + from downstream calls included only for static state-flow and target + look-ahead. Preflight analyses set the prefix to the complete window. + """ + + analysis_id: str + kind: str + calls: tuple[SemanticCallSpec, ...] + source_path: ConfigPath + segment_indices: tuple[int, ...] + execution_prefix_length: int + + def __post_init__(self) -> None: + if type(self.analysis_id) is not str or not self.analysis_id: + raise ValueError("analysis_id must be a non-empty string.") + if self.kind not in { + "sequential_stretch", + "parallel_branch", + "sequential_suffix", + }: + raise ValueError("kind must identify a supported program analysis.") + calls = tuple(self.calls) + if not calls or not all(type(call) in _SEMANTIC_CALL_TYPES for call in calls): + raise TypeError("calls must contain supported semantic call values.") + if type(self.source_path) is not tuple: + raise TypeError("source_path must be a ConfigPath tuple.") + indices = tuple(self.segment_indices) + if not indices or any(type(index) is not int or index < 0 for index in indices): + raise ValueError("segment_indices must contain non-negative integers.") + if len(set(indices)) != len(indices) or tuple(sorted(indices)) != indices: + raise ValueError("segment_indices must be unique and ordered.") + if type( + self.execution_prefix_length + ) is not int or not 1 <= self.execution_prefix_length <= len(calls): + raise ValueError( + "execution_prefix_length must select a non-empty prefix of calls." + ) + object.__setattr__( + self, + "calls", + tuple(_snapshot_semantic_call(call) for call in calls), + ) + object.__setattr__(self, "segment_indices", indices) + + +@dataclass(frozen=True, slots=True) +class _CallTemplate: + kind: str + source_path: ConfigPath + object: SceneObjectRef | None = None + grasp: SceneAffordanceRef | None = None + at_target_id: str | None = None + on: SceneObjectRef | SceneAffordanceRef | None = None + inside: SceneObjectRef | SceneAffordanceRef | None = None + final_target_id: str | None = None + articulation: SceneArticulationRef | None = None + handle: SceneAffordanceRef | None = None + articulation_target: str | None = None + target_position: float | None = None + target_displacement: float | None = None + call_id: str | None = None + arguments: Mapping[str, DeclarativeValue] | None = None + resources: tuple[tuple[str, str], ...] = () + + +@dataclass(frozen=True, slots=True) +class _InvokeTemplate: + call: _CallTemplate + source_path: ConfigPath + + +@dataclass(frozen=True, slots=True) +class _SequenceTemplate: + items: tuple[_NodeTemplate, ...] + source_path: ConfigPath + + +@dataclass(frozen=True, slots=True) +class _RepeatTemplate: + count: int + body: _NodeTemplate + source_path: ConfigPath + + +@dataclass(frozen=True, slots=True) +class _BarrierTemplate: + name: str + timeout_steps: int + failure_policy: str + source_path: ConfigPath + + +@dataclass(frozen=True, slots=True) +class _ParallelTemplate: + branches: tuple[_NodeTemplate, ...] + barrier: _BarrierTemplate + source_path: ConfigPath + + +@dataclass(frozen=True, slots=True) +class _PostTemplate: + cfg: WaitStablePostCfg + entity: SceneEntityRef + source_path: ConfigPath + + +@dataclass(frozen=True, slots=True) +class _ValidatorTemplate: + cfg: ObjectNearTargetValidatorCfg + object: SceneObjectRef + target_id: str + source_path: ConfigPath + + +@dataclass(frozen=True, slots=True) +class _SegmentTemplate: + name: str + steps: _NodeTemplate + post: tuple[_PostTemplate, ...] + validators: tuple[_ValidatorTemplate, ...] + source_path: ConfigPath + + +_NodeTemplate = ( + _InvokeTemplate + | _SequenceTemplate + | _RepeatTemplate + | _SegmentTemplate + | _ParallelTemplate + | _BarrierTemplate +) + + +def _contains_parallel(template: _NodeTemplate) -> bool: + """Return whether a compiled subtree owns a parallel block.""" + if type(template) is _ParallelTemplate: + return True + if type(template) is _SequenceTemplate: + return any(_contains_parallel(child) for child in template.items) + if type(template) is _RepeatTemplate: + return _contains_parallel(template.body) + if type(template) is _SegmentTemplate: + return _contains_parallel(template.steps) + return False + + +@dataclass(slots=True) +class _ExpansionState: + segment_index: int = 0 + call_index: int = 0 + + +def _resolve_target( + target_id: str, + *, + targets: Mapping[str, tuple[SemanticPose, ...]], + repeat_frames: tuple[CompiledRepeatFrame, ...], +) -> tuple[SemanticPose, CompiledTargetSelection]: + """Select one cyclic target from the nearest lexical repeat frame.""" + values = targets[target_id] + repeat = repeat_frames[-1] if repeat_frames else None + value_index = 0 if repeat is None else repeat.iteration_index % len(values) + selection = CompiledTargetSelection( + target_id=target_id, + value_index=value_index, + repeat_path=None if repeat is None else repeat.path, + repeat_iteration_index=None if repeat is None else repeat.iteration_index, + ) + return values[value_index].snapshot(), selection + + +def _instantiate_call( + template: _CallTemplate, + *, + call_index: int, + segment_call_index: int, + targets: Mapping[str, tuple[SemanticPose, ...]], + repeat_frames: tuple[CompiledRepeatFrame, ...], +) -> CompiledProgramCall: + """Instantiate one semantic call occurrence from static templates.""" + resources = dict(template.resources) + selections: list[CompiledTargetSelection] = [] + if template.kind == "pick": + assert template.object is not None + call: SemanticCallSpec = Pick( + object=_copy_scene_ref(template.object), + grasp=(None if template.grasp is None else _copy_scene_ref(template.grasp)), + resources=resources, + ) + elif template.kind == "place": + assert template.object is not None + at: SemanticPose | None = None + if template.at_target_id is not None: + at, selection = _resolve_target( + template.at_target_id, + targets=targets, + repeat_frames=repeat_frames, + ) + selections.append(selection) + call = Place( + object=_copy_scene_ref(template.object), + at=at, + on=None if template.on is None else _copy_scene_ref(template.on), + inside=( + None if template.inside is None else _copy_scene_ref(template.inside) + ), + resources=resources, + ) + elif template.kind == "hand_over": + assert template.object is not None + final_target: SemanticPose | None = None + if template.final_target_id is not None: + final_target, selection = _resolve_target( + template.final_target_id, + targets=targets, + repeat_frames=repeat_frames, + ) + selections.append(selection) + call = HandOver( + object=_copy_scene_ref(template.object), + final_target=final_target, + resources=resources, + ) + elif template.kind == "operate_articulation": + assert template.articulation is not None + call = OperateArticulation( + articulation=_copy_scene_ref(template.articulation), + handle=( + None if template.handle is None else _copy_scene_ref(template.handle) + ), + target=template.articulation_target, + target_position=template.target_position, + target_displacement=template.target_displacement, + resources=resources, + ) + elif template.kind == "registered": + assert template.call_id is not None and template.arguments is not None + call = RegisteredSemanticCall( + call_id=template.call_id, + arguments=template.arguments, + resources=resources, + ) + else: # pragma: no cover - compiler-owned templates prevent this + raise AssertionError(f"Unknown call template {template.kind!r}.") + return CompiledProgramCall( + call_index=call_index, + segment_call_index=segment_call_index, + call=call, + source_path=template.source_path, + repeat_frames=repeat_frames, + target_selections=tuple(selections), + ) + + +def _iter_call_templates( + template: _NodeTemplate, + *, + repeat_frames: tuple[CompiledRepeatFrame, ...], +) -> Iterator[tuple[_CallTemplate, tuple[CompiledRepeatFrame, ...]]]: + """Expand call templates inside one explicit segment without segment splits.""" + if type(template) is _InvokeTemplate: + yield template.call, repeat_frames + elif type(template) is _SequenceTemplate: + for child in template.items: + yield from _iter_call_templates(child, repeat_frames=repeat_frames) + elif type(template) is _RepeatTemplate: + for iteration_index in range(template.count): + frame = CompiledRepeatFrame( + path=template.source_path, + iteration_index=iteration_index, + count=template.count, + ) + yield from _iter_call_templates( + template.body, + repeat_frames=(*repeat_frames, frame), + ) + else: # pragma: no cover - nested segments are rejected during compilation + raise AssertionError("A nested segment reached call-only expansion.") + + +def _instantiate_parallel_block( + template: _ParallelTemplate, + *, + targets: Mapping[str, tuple[SemanticPose, ...]], + repeat_frames: tuple[CompiledRepeatFrame, ...], + state: _ExpansionState, +) -> tuple[CompiledParallelBlock, tuple[CompiledProgramCall, ...]]: + """Instantiate branch-local call order without serializing branch semantics.""" + branches: list[CompiledParallelBranch] = [] + flattened: list[CompiledProgramCall] = [] + segment_call_index = 0 + for branch_index, branch_template in enumerate(template.branches): + calls: list[CompiledProgramCall] = [] + for call_template, call_repeat_frames in _iter_call_templates( + branch_template, + repeat_frames=repeat_frames, + ): + call = _instantiate_call( + call_template, + call_index=state.call_index, + segment_call_index=segment_call_index, + targets=targets, + repeat_frames=call_repeat_frames, + ) + calls.append(call) + flattened.append(call) + state.call_index += 1 + segment_call_index += 1 + branches.append( + CompiledParallelBranch( + branch_index=branch_index, + calls=tuple(calls), + source_path=template.branches[branch_index].source_path, + ) + ) + barrier = CompiledBarrier( + name=template.barrier.name, + timeout_steps=template.barrier.timeout_steps, + failure_policy=template.barrier.failure_policy, + source_path=template.barrier.source_path, + ) + return ( + CompiledParallelBlock( + branches=tuple(branches), + barrier=barrier, + source_path=template.source_path, + ), + tuple(flattened), + ) + + +def _segment_identity( + program_id: str, + *, + source_path: ConfigPath, + repeat_frames: tuple[CompiledRepeatFrame, ...], + implicit: bool, +) -> str: + """Build one deterministic segment identity from lexical occurrence data.""" + repeat_suffix = "".join( + f"@{render_config_path(frame.path)}[{frame.iteration_index}]" + for frame in repeat_frames + ) + boundary = "implicit" if implicit else "segment" + return f"{program_id}:{boundary}:{render_config_path(source_path)}{repeat_suffix}" + + +def _iter_segments( + template: _NodeTemplate, + *, + program_id: str, + targets: Mapping[str, tuple[SemanticPose, ...]], + repeat_frames: tuple[CompiledRepeatFrame, ...], + state: _ExpansionState, +) -> Iterator[CompiledProgramSegment]: + """Lazily expand outer program structure into independent segments.""" + if type(template) is _SequenceTemplate: + for child in template.items: + yield from _iter_segments( + child, + program_id=program_id, + targets=targets, + repeat_frames=repeat_frames, + state=state, + ) + return + if type(template) is _RepeatTemplate: + for iteration_index in range(template.count): + frame = CompiledRepeatFrame( + path=template.source_path, + iteration_index=iteration_index, + count=template.count, + ) + yield from _iter_segments( + template.body, + program_id=program_id, + targets=targets, + repeat_frames=(*repeat_frames, frame), + state=state, + ) + return + if type(template) is _InvokeTemplate: + call = _instantiate_call( + template.call, + call_index=state.call_index, + segment_call_index=0, + targets=targets, + repeat_frames=repeat_frames, + ) + state.call_index += 1 + segment = CompiledProgramSegment( + segment_index=state.segment_index, + segment_id=_segment_identity( + program_id, + source_path=template.source_path, + repeat_frames=repeat_frames, + implicit=True, + ), + name=f"invoke:{call.call.semantic_id}", + calls=(call,), + source_path=template.source_path, + repeat_frames=repeat_frames, + implicit=True, + ) + state.segment_index += 1 + yield segment + return + + if type(template) is _ParallelTemplate: + parallel_block, calls = _instantiate_parallel_block( + template, + targets=targets, + repeat_frames=repeat_frames, + state=state, + ) + segment = CompiledProgramSegment( + segment_index=state.segment_index, + segment_id=_segment_identity( + program_id, + source_path=template.source_path, + repeat_frames=repeat_frames, + implicit=True, + ), + name=f"parallel:{parallel_block.barrier.name}", + calls=calls, + source_path=template.source_path, + repeat_frames=repeat_frames, + parallel_block=parallel_block, + implicit=True, + ) + state.segment_index += 1 + yield segment + return + + assert type(template) is _SegmentTemplate + parallel_block: CompiledParallelBlock | None = None + if type(template.steps) is _ParallelTemplate: + parallel_block, instantiated_calls = _instantiate_parallel_block( + template.steps, + targets=targets, + repeat_frames=repeat_frames, + state=state, + ) + calls = list(instantiated_calls) + else: + calls = [] + for segment_call_index, (call_template, call_repeat_frames) in enumerate( + _iter_call_templates(template.steps, repeat_frames=repeat_frames) + ): + calls.append( + _instantiate_call( + call_template, + call_index=state.call_index, + segment_call_index=segment_call_index, + targets=targets, + repeat_frames=call_repeat_frames, + ) + ) + state.call_index += 1 + post_policies = tuple( + CompiledPostPolicy( + cfg=post.cfg, + entity=post.entity, + source_path=post.source_path, + ) + for post in template.post + ) + validators: list[CompiledProgramValidator] = [] + for validator in template.validators: + target_pose, selection = _resolve_target( + validator.target_id, + targets=targets, + repeat_frames=repeat_frames, + ) + validators.append( + CompiledProgramValidator( + cfg=validator.cfg, + object=validator.object, + target_pose=target_pose, + target_selection=selection, + source_path=validator.source_path, + ) + ) + segment = CompiledProgramSegment( + segment_index=state.segment_index, + segment_id=_segment_identity( + program_id, + source_path=template.source_path, + repeat_frames=repeat_frames, + implicit=False, + ), + name=template.name, + calls=tuple(calls), + source_path=template.source_path, + repeat_frames=repeat_frames, + post_policies=post_policies, + validators=tuple(validators), + parallel_block=parallel_block, + implicit=False, + ) + state.segment_index += 1 + yield segment + + +@dataclass(frozen=True, slots=True, init=False) +class CompiledProgram: + """Owned provider-free program template with lazy deterministic expansion.""" + + schema_version: int + program_id: str + _integration: ExpertProgramIntegrationCfg = field(repr=False, compare=False) + _targets: Mapping[str, tuple[SemanticPose, ...]] = field( + repr=False, + compare=False, + ) + _root: _NodeTemplate = field(repr=False, compare=False) + + def __init__(self, *args: object, **kwargs: object) -> None: + """Reject construction outside :class:`ExpertProgramCompiler`.""" + del args, kwargs + raise TypeError("CompiledProgram values are created by ExpertProgramCompiler.") + + @classmethod + def _create( + cls, + *, + schema_version: int, + program_id: str, + integration: ExpertProgramIntegrationCfg, + targets: Mapping[str, tuple[SemanticPose, ...]], + root: _NodeTemplate, + ) -> CompiledProgram: + """Create one compiler-owned lazy program template.""" + instance = object.__new__(cls) + object.__setattr__(instance, "schema_version", schema_version) + object.__setattr__(instance, "program_id", program_id) + object.__setattr__(instance, "_integration", integration) + object.__setattr__( + instance, + "_targets", + MappingProxyType( + { + target_id: tuple(pose.snapshot() for pose in values) + for target_id, values in targets.items() + } + ), + ) + object.__setattr__(instance, "_root", root) + return instance + + @property + def integration(self) -> ExpertProgramIntegrationCfg: + """Return an independent integration-selection snapshot.""" + return ExpertProgramIntegrationCfg( + robot_profile=self._integration.robot_profile, + scene_registry=self._integration.scene_registry, + runtime_preset=self._integration.runtime_preset, + ) + + @property + def targets(self) -> Mapping[str, tuple[SemanticPose, ...]]: + """Return independent static target-pose snapshots.""" + return MappingProxyType( + { + target_id: tuple(pose.snapshot() for pose in values) + for target_id, values in self._targets.items() + } + ) + + def iter_segments(self) -> Iterator[CompiledProgramSegment]: + """Lazily expand a fresh deterministic segment stream.""" + return _iter_segments( + self._root, + program_id=self.program_id, + targets=self._targets, + repeat_frames=(), + state=_ExpansionState(), + ) + + def materialize(self) -> MaterializedCompiledProgram: + """Expand the bounded provider-free segment stream exactly once. + + Materialization never observes a scene provider. It also re-enforces + the public expanded-call bound so a configuration mutated after its + initial validation cannot create an unbounded bridge-preflight pass. + + Returns: + Immutable materialized program with deterministic analysis windows. + + Raises: + ExpertProgramCompileError: If expansion exceeds the configured + semantic-call bound. + """ + segments: list[CompiledProgramSegment] = [] + expanded_calls = 0 + for segment in self.iter_segments(): + expanded_calls += len(segment.calls) + if expanded_calls > MAX_EXPANDED_CALLS: + raise ExpertProgramCompileError( + "expanded_call_limit", + segment.source_path, + "Program materialization exceeds the static limit of " + f"{MAX_EXPANDED_CALLS} semantic calls.", + ) + segments.append(segment) + return MaterializedCompiledProgram._create( + schema_version=self.schema_version, + program_id=self.program_id, + integration=self._integration, + segments=tuple(segments), + ) + + def __iter__(self) -> Iterator[CompiledProgramSegment]: + return self.iter_segments() + + +@dataclass(frozen=True, slots=True, init=False) +class MaterializedCompiledProgram: + """Bounded provider-free segment snapshot used by preflight and execution.""" + + schema_version: int + program_id: str + _integration: ExpertProgramIntegrationCfg = field(repr=False, compare=False) + _segments: tuple[CompiledProgramSegment, ...] = field( + repr=False, + compare=False, + ) + + def __init__(self, *args: object, **kwargs: object) -> None: + """Reject construction outside :meth:`CompiledProgram.materialize`.""" + del args, kwargs + raise TypeError( + "MaterializedCompiledProgram values are created by " + "CompiledProgram.materialize()." + ) + + @classmethod + def _create( + cls, + *, + schema_version: int, + program_id: str, + integration: ExpertProgramIntegrationCfg, + segments: tuple[CompiledProgramSegment, ...], + ) -> MaterializedCompiledProgram: + """Create one compiler-owned materialized program.""" + if type(schema_version) is not int or schema_version < 1: + raise ValueError("schema_version must be a positive integer.") + if type(program_id) is not str or not program_id: + raise ValueError("program_id must be a non-empty string.") + if type(integration) is not ExpertProgramIntegrationCfg: + raise TypeError("integration must be ExpertProgramIntegrationCfg.") + values = tuple(segments) + if not values or not all( + type(segment) is CompiledProgramSegment for segment in values + ): + raise TypeError( + "segments must contain at least one CompiledProgramSegment." + ) + if tuple(segment.segment_index for segment in values) != tuple( + range(len(values)) + ): + raise ValueError("Materialized segment indices must be contiguous.") + flattened_calls = tuple(call for segment in values for call in segment.calls) + if len(flattened_calls) > MAX_EXPANDED_CALLS: + raise ValueError( + f"Materialized program exceeds {MAX_EXPANDED_CALLS} calls." + ) + if tuple(call.call_index for call in flattened_calls) != tuple( + range(len(flattened_calls)) + ): + raise ValueError("Materialized call indices must be contiguous.") + + instance = object.__new__(cls) + object.__setattr__(instance, "schema_version", schema_version) + object.__setattr__(instance, "program_id", program_id) + object.__setattr__( + instance, + "_integration", + ExpertProgramIntegrationCfg( + robot_profile=integration.robot_profile, + scene_registry=integration.scene_registry, + runtime_preset=integration.runtime_preset, + ), + ) + object.__setattr__(instance, "_segments", values) + return instance + + @property + def integration(self) -> ExpertProgramIntegrationCfg: + """Return an independent integration-selection snapshot.""" + return ExpertProgramIntegrationCfg( + robot_profile=self._integration.robot_profile, + scene_registry=self._integration.scene_registry, + runtime_preset=self._integration.runtime_preset, + ) + + @property + def segment_count(self) -> int: + """Return the number of materialized logical segments.""" + return len(self._segments) + + def iter_segments(self) -> Iterator[CompiledProgramSegment]: + """Iterate the already materialized provider-free segments.""" + return iter(self._segments) + + def preflight_analyses(self) -> tuple[CompiledProgramAnalysis, ...]: + """Return full-program analyses split only at parallel barriers. + + Consecutive sequential segments form one static workflow, preserving + their object-state flow and cross-segment target look-ahead. Each + parallel branch is analyzed independently; no state or target inference + crosses the barrier in either direction. + """ + analyses: list[CompiledProgramAnalysis] = [] + stretch: list[CompiledProgramSegment] = [] + + def flush_stretch() -> None: + if not stretch: + return + indices = tuple(segment.segment_index for segment in stretch) + calls = tuple(call.call for segment in stretch for call in segment.calls) + analyses.append( + CompiledProgramAnalysis( + analysis_id=( + f"{self.program_id}:preflight:sequential:" + f"{indices[0]}-{indices[-1]}" + ), + kind="sequential_stretch", + calls=calls, + source_path=stretch[0].source_path, + segment_indices=indices, + execution_prefix_length=len(calls), + ) + ) + stretch.clear() + + for segment in self._segments: + block = segment.parallel_block + if block is None: + stretch.append(segment) + continue + flush_stretch() + for branch in block.branches: + calls = tuple(call.call for call in branch.calls) + analyses.append( + CompiledProgramAnalysis( + analysis_id=( + f"{self.program_id}:preflight:parallel:" + f"{segment.segment_index}:{branch.branch_index}" + ), + kind="parallel_branch", + calls=calls, + source_path=branch.source_path, + segment_indices=(segment.segment_index,), + execution_prefix_length=len(calls), + ) + ) + flush_stretch() + return tuple(analyses) + + def sequential_execution_analysis( + self, + segment_index: int, + ) -> CompiledProgramAnalysis: + """Return current-segment prefix plus downstream sequential look-ahead. + + Args: + segment_index: Index of the sequential segment about to execute. + + Returns: + Analysis beginning at the selected segment and ending immediately + before the next parallel barrier or the end of the program. + + Raises: + IndexError: If ``segment_index`` is outside this program. + ValueError: If the selected segment is itself parallel. + """ + if type(segment_index) is not int: + raise TypeError("segment_index must be an integer.") + if not 0 <= segment_index < len(self._segments): + raise IndexError(f"segment_index {segment_index!r} is outside the program.") + current = self._segments[segment_index] + if current.parallel_block is not None: + raise ValueError("Parallel segments do not have sequential look-ahead.") + window: list[CompiledProgramSegment] = [] + for segment in self._segments[segment_index:]: + if segment.parallel_block is not None: + break + window.append(segment) + calls = tuple(call.call for segment in window for call in segment.calls) + indices = tuple(segment.segment_index for segment in window) + return CompiledProgramAnalysis( + analysis_id=( + f"{self.program_id}:execution:sequential:" f"{indices[0]}-{indices[-1]}" + ), + kind="sequential_suffix", + calls=calls, + source_path=current.source_path, + segment_indices=indices, + execution_prefix_length=len(current.calls), + ) + + def __iter__(self) -> Iterator[CompiledProgramSegment]: + return self.iter_segments() + + +class ExpertProgramCompiler: + """Compile validated Expert Program ASTs through one typed resolver.""" + + def __init__(self, scene_resolver: ExpertProgramSceneResolver) -> None: + """Create one provider-free compiler. + + Args: + scene_resolver: Static typed scene identity resolver. + """ + if not isinstance(scene_resolver, ExpertProgramSceneResolver): + raise TypeError("scene_resolver must implement ExpertProgramSceneResolver.") + self._scene_resolver = scene_resolver + + @classmethod + def from_scene_registry(cls, registry: SceneRegistry) -> ExpertProgramCompiler: + """Create a compiler from a provider-free SceneRegistry identity snapshot.""" + return cls(SceneRegistryProgramResolver(registry)) + + def _resolve_scene( + self, + reference: str, + *, + expected_types: tuple[type[SceneEntityRef], ...], + path: ConfigPath, + ) -> SceneEntityRef: + """Resolve and validate one exact typed canonical scene reference.""" + try: + resolved = self._scene_resolver.resolve( + reference, + expected_types=expected_types, + path=path, + ) + except ExpertProgramConfigError: + raise + except (KeyError, TypeError, ValueError) as exc: + raise ExpertProgramCompileError( + "scene_resolution_failed", + path, + str(exc), + ) from exc + if type(resolved) not in expected_types: + raise ExpertProgramCompileError( + "scene_resolver_contract_violation", + path, + "Scene resolver returned an incompatible typed reference.", + ) + return _copy_scene_ref(resolved) + + @staticmethod + def _target_id( + reference: TargetRefCfg, + *, + targets: Mapping[str, tuple[SemanticPose, ...]], + path: ConfigPath, + ) -> str: + """Resolve one statically registered target ID.""" + if type(reference) is not TargetRefCfg or reference.kind != "target_ref": + raise ExpertProgramCompileError( + "invalid_target_reference", + path, + "Expected an exact target_ref configuration.", + ) + if reference.target not in targets: + raise ExpertProgramCompileError( + "unknown_target", + (*path, "target"), + f"Unknown target {reference.target!r}.", + ) + return reference.target + + def _compile_call( + self, + cfg: object, + *, + targets: Mapping[str, tuple[SemanticPose, ...]], + path: ConfigPath, + ) -> _CallTemplate: + """Lower one config call into a provider-free canonical template.""" + if type(cfg) is PickCfg: + if cfg.kind != "pick": + raise ExpertProgramCompileError( + "invalid_discriminator", (*path, "kind"), "Expected 'pick'." + ) + object_ref = self._resolve_scene( + cfg.object, + expected_types=(SceneObjectRef,), + path=(*path, "object"), + ) + grasp_ref = ( + None + if cfg.grasp is None + else self._resolve_scene( + cfg.grasp, + expected_types=(SceneAffordanceRef,), + path=(*path, "grasp"), + ) + ) + return _CallTemplate( + kind="pick", + source_path=path, + object=object_ref, + grasp=grasp_ref, + resources=tuple(sorted(cfg.resources.items())), + ) + if type(cfg) is PlaceCfg: + if cfg.kind != "place": + raise ExpertProgramCompileError( + "invalid_discriminator", (*path, "kind"), "Expected 'place'." + ) + object_ref = self._resolve_scene( + cfg.object, + expected_types=(SceneObjectRef,), + path=(*path, "object"), + ) + at_target_id = ( + None + if cfg.at is None + else self._target_id( + cfg.at, + targets=targets, + path=(*path, "at"), + ) + ) + on = ( + None + if cfg.on is None + else self._resolve_scene( + cfg.on, + expected_types=(SceneObjectRef, SceneAffordanceRef), + path=(*path, "on"), + ) + ) + inside = ( + None + if cfg.inside is None + else self._resolve_scene( + cfg.inside, + expected_types=(SceneObjectRef, SceneAffordanceRef), + path=(*path, "inside"), + ) + ) + return _CallTemplate( + kind="place", + source_path=path, + object=object_ref, + at_target_id=at_target_id, + on=on, + inside=inside, + resources=tuple(sorted(cfg.resources.items())), + ) + if type(cfg) is HandOverCfg: + if cfg.kind != "hand_over": + raise ExpertProgramCompileError( + "invalid_discriminator", + (*path, "kind"), + "Expected 'hand_over'.", + ) + object_ref = self._resolve_scene( + cfg.object, + expected_types=(SceneObjectRef,), + path=(*path, "object"), + ) + final_target_id = ( + None + if cfg.final_target is None + else self._target_id( + cfg.final_target, + targets=targets, + path=(*path, "final_target"), + ) + ) + return _CallTemplate( + kind="hand_over", + source_path=path, + object=object_ref, + final_target_id=final_target_id, + resources=tuple(sorted(cfg.resources.items())), + ) + if type(cfg) is OperateArticulationCfg: + if cfg.kind != "operate_articulation": + raise ExpertProgramCompileError( + "invalid_discriminator", + (*path, "kind"), + "Expected 'operate_articulation'.", + ) + articulation = self._resolve_scene( + cfg.articulation, + expected_types=(SceneArticulationRef,), + path=(*path, "articulation"), + ) + handle = ( + None + if cfg.handle is None + else self._resolve_scene( + cfg.handle, + expected_types=(SceneAffordanceRef,), + path=(*path, "handle"), + ) + ) + snapshot = OperateArticulation( + articulation=articulation, + handle=handle, + target=cfg.target, + target_position=cfg.target_position, + target_displacement=cfg.target_displacement, + resources=cfg.resources, + ) + return _CallTemplate( + kind="operate_articulation", + source_path=path, + articulation=snapshot.articulation, + handle=snapshot.handle, + articulation_target=snapshot.target, + target_position=snapshot.target_position, + target_displacement=snapshot.target_displacement, + resources=tuple(sorted(snapshot.resources.items())), + ) + if type(cfg) is RegisteredSemanticCallCfg: + if cfg.kind != "registered": + raise ExpertProgramCompileError( + "invalid_discriminator", + (*path, "kind"), + "Expected 'registered'.", + ) + if cfg.schema_version != EXPERT_PROGRAM_SCHEMA_VERSION: + raise ExpertProgramCompileError( + "unsupported_registered_schema", + (*path, "schema_version"), + "Registered call schema_version must be exactly 1.", + ) + snapshot = RegisteredSemanticCall( + call_id=cfg.call_id, + arguments=cfg.arguments, + resources=cfg.resources, + ) + return _CallTemplate( + kind="registered", + source_path=path, + call_id=snapshot.call_id, + arguments=snapshot.arguments, + resources=tuple(sorted(snapshot.resources.items())), + ) + raise ExpertProgramCompileError( + "unsupported_call", + path, + f"Unsupported semantic call config {type(cfg).__name__}.", + ) + + def _compile_node( + self, + node: ProgramNodeCfg, + *, + targets: Mapping[str, tuple[SemanticPose, ...]], + path: ConfigPath, + inside_segment: bool, + inside_parallel: bool, + ) -> _NodeTemplate: + """Compile static AST structure without expanding repeats.""" + if type(node) is InvokeCfg: + if node.kind != "invoke": + raise ExpertProgramCompileError( + "invalid_discriminator", (*path, "kind"), "Expected 'invoke'." + ) + return _InvokeTemplate( + call=self._compile_call( + node.call, + targets=targets, + path=(*path, "call"), + ), + source_path=path, + ) + if type(node) is SequenceCfg: + if node.kind != "sequence": + raise ExpertProgramCompileError( + "invalid_discriminator", + (*path, "kind"), + "Expected 'sequence'.", + ) + if not node.items: + raise ExpertProgramCompileError( + "empty_sequence", + (*path, "items"), + "Sequence items must contain at least one program node.", + ) + return _SequenceTemplate( + items=tuple( + self._compile_node( + child, + targets=targets, + path=(*path, "items", index), + inside_segment=inside_segment, + inside_parallel=inside_parallel, + ) + for index, child in enumerate(node.items) + ), + source_path=path, + ) + if type(node) is RepeatCfg: + if node.kind != "repeat": + raise ExpertProgramCompileError( + "invalid_discriminator", (*path, "kind"), "Expected 'repeat'." + ) + if type(node.count) is not int or not 1 <= node.count <= MAX_REPEAT_COUNT: + raise ExpertProgramCompileError( + "invalid_repeat_count", + (*path, "count"), + f"Repeat count must be an integer in [1, {MAX_REPEAT_COUNT}].", + ) + return _RepeatTemplate( + count=node.count, + body=self._compile_node( + node.body, + targets=targets, + path=(*path, "body"), + inside_segment=inside_segment, + inside_parallel=inside_parallel, + ), + source_path=path, + ) + if type(node) is SegmentCfg: + if inside_parallel: + raise ExpertProgramCompileError( + "segment_inside_parallel", + path, + "Parallel branches may contain only Invoke, Sequence, and " + "Repeat nodes; wrap the Parallel node in one Segment instead.", + ) + if inside_segment: + raise ExpertProgramCompileError( + "nested_segment", + path, + "Nested Segment nodes are ambiguous and forbidden.", + ) + if node.kind != "segment": + raise ExpertProgramCompileError( + "invalid_discriminator", (*path, "kind"), "Expected 'segment'." + ) + post: list[_PostTemplate] = [] + for index, cfg in enumerate(node.post): + post_path = (*path, "post", index) + if type(cfg) is not WaitStablePostCfg or cfg.kind != "wait_stable": + raise ExpertProgramCompileError( + "unsupported_post_policy", + post_path, + "Supported schemas accept only exact wait_stable post policies.", + ) + entity = self._resolve_scene( + cfg.entity, + expected_types=_SCENE_REF_TYPES, + path=(*post_path, "entity"), + ) + post.append( + _PostTemplate( + cfg=WaitStablePostCfg( + entity=cfg.entity, + preset=cfg.preset, + kind=cfg.kind, + ), + entity=entity, + source_path=post_path, + ) + ) + validators: list[_ValidatorTemplate] = [] + for index, cfg in enumerate(node.validators): + validator_path = (*path, "validators", index) + if ( + type(cfg) is not ObjectNearTargetValidatorCfg + or cfg.kind != "object_near_target" + ): + raise ExpertProgramCompileError( + "unsupported_validator", + validator_path, + "Supported schemas accept only exact object_near_target " + "validators.", + ) + if cfg.target not in targets: + raise ExpertProgramCompileError( + "unknown_target", + (*validator_path, "target"), + f"Unknown target {cfg.target!r}.", + ) + object_ref = self._resolve_scene( + cfg.object, + expected_types=(SceneObjectRef,), + path=(*validator_path, "object"), + ) + validators.append( + _ValidatorTemplate( + cfg=ObjectNearTargetValidatorCfg( + object=cfg.object, + target=cfg.target, + position_tolerance=cfg.position_tolerance, + kind=cfg.kind, + ), + object=object_ref, + target_id=cfg.target, + source_path=validator_path, + ) + ) + steps = self._compile_node( + node.steps, + targets=targets, + path=(*path, "steps"), + inside_segment=True, + inside_parallel=False, + ) + if type(steps) is not _ParallelTemplate and _contains_parallel(steps): + raise ExpertProgramCompileError( + "mixed_parallel_segment", + (*path, "steps"), + "A Segment may contain either a call-only program or one direct " + "Parallel node, not a mixed sequential/parallel tree.", + ) + return _SegmentTemplate( + name=node.name, + steps=steps, + post=tuple(post), + validators=tuple(validators), + source_path=path, + ) + if type(node) is ParallelCfg: + if inside_parallel: + raise ExpertProgramCompileError( + "nested_parallel", + path, + "Nested Parallel nodes are forbidden in schema version 2.", + ) + if node.kind != "parallel": + raise ExpertProgramCompileError( + "invalid_discriminator", + (*path, "kind"), + "Expected 'parallel'.", + ) + if len(node.branches) < 2: + raise ExpertProgramCompileError( + "parallel_branch_count", + (*path, "branches"), + "Parallel requires at least two branches.", + ) + if type(node.barrier) is not BarrierCfg: + raise ExpertProgramCompileError( + "parallel_barrier_required", + (*path, "barrier"), + "Parallel.barrier must be an exact BarrierCfg.", + ) + branches = tuple( + self._compile_node( + branch, + targets=targets, + path=(*path, "branches", index), + inside_segment=inside_segment, + inside_parallel=True, + ) + for index, branch in enumerate(node.branches) + ) + if any(_contains_parallel(branch) for branch in branches): + raise ExpertProgramCompileError( + "nested_parallel", + (*path, "branches"), + "Nested Parallel nodes are forbidden in schema version 2.", + ) + barrier = node.barrier + if barrier.kind != "barrier": + raise ExpertProgramCompileError( + "invalid_discriminator", + (*path, "barrier", "kind"), + "Expected 'barrier'.", + ) + if barrier.failure_policy != "fail_fast": + raise ExpertProgramCompileError( + "unsupported_failure_policy", + (*path, "barrier", "failure_policy"), + "Barrier failure_policy must be exactly 'fail_fast'.", + ) + return _ParallelTemplate( + branches=branches, + barrier=_BarrierTemplate( + name=barrier.name, + timeout_steps=barrier.timeout_steps, + failure_policy=barrier.failure_policy, + source_path=(*path, "barrier"), + ), + source_path=path, + ) + if type(node) is BarrierCfg: + raise ExpertProgramCompileError( + "standalone_barrier", + path, + "Barrier nodes may only be owned by Parallel.", + ) + raise ExpertProgramCompileError( + "unsupported_program_node", + path, + f"Unsupported program node {type(node).__name__}.", + ) + + @staticmethod + def _compile_targets( + targets: Mapping[str, CyclicPoseTargetCfg], + ) -> Mapping[str, tuple[SemanticPose, ...]]: + """Compile static pose providers without selecting repeat values.""" + compiled: dict[str, tuple[SemanticPose, ...]] = {} + for target_id, target in targets.items(): + path = ("targets", target_id) + if type(target) is not CyclicPoseTargetCfg or target.kind != "cyclic_pose": + raise ExpertProgramCompileError( + "unsupported_target", + path, + "Supported schemas accept only exact cyclic_pose targets.", + ) + poses: list[SemanticPose] = [] + if not target.values: + raise ExpertProgramCompileError( + "empty_target_values", + (*path, "values"), + "Cyclic target values must contain at least one pose.", + ) + for index, pose in enumerate(target.values): + if type(pose) is not PoseCfg: + raise ExpertProgramCompileError( + "invalid_pose", + (*path, "values", index), + "Target values must be exact PoseCfg values.", + ) + poses.append(SemanticPose(pose.position, pose.quaternion_wxyz)) + compiled[target_id] = tuple(poses) + return MappingProxyType(compiled) + + def compile(self, config: ExpertProgramCfg) -> CompiledProgram: + """Compile one validated AST into a provider-free lazy program. + + Args: + config: Strict, supported-version Expert Program configuration. + + Returns: + Owned static templates whose iteration resolves repeat-local targets + and emits independent logical segments. + + Raises: + ExpertProgramCompileError: If typed scene resolution or AST lowering + fails. + """ + if type(config) is not ExpertProgramCfg: + raise TypeError("config must be exactly ExpertProgramCfg.") + if config.schema_version not in SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS: + raise ExpertProgramCompileError( + "unsupported_schema_version", + ("schema_version",), + "Supported Expert Program schema versions are " + f"{SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS}.", + ) + targets = self._compile_targets(config.targets) + root = self._compile_node( + config.program, + targets=targets, + path=("program",), + inside_segment=False, + inside_parallel=False, + ) + integration = ExpertProgramIntegrationCfg( + robot_profile=config.integration.robot_profile, + scene_registry=config.integration.scene_registry, + runtime_preset=config.integration.runtime_preset, + ) + return CompiledProgram._create( + schema_version=config.schema_version, + program_id=config.program_id, + integration=integration, + targets=targets, + root=root, + ) + + +__all__ = [ + "CompiledBarrier", + "CompiledParallelBlock", + "CompiledParallelBranch", + "CompiledPostPolicy", + "CompiledProgram", + "CompiledProgramAnalysis", + "CompiledProgramCall", + "CompiledProgramSegment", + "CompiledProgramValidator", + "CompiledRepeatFrame", + "CompiledTargetSelection", + "ExpertProgramCompileError", + "ExpertProgramCompiler", + "ExpertProgramSceneResolver", + "MaterializedCompiledProgram", + "SceneRegistryProgramResolver", +] diff --git a/embodichain/lab/gym/envs/expert_program/decoder.py b/embodichain/lab/gym/envs/expert_program/decoder.py new file mode 100644 index 000000000..4b7e78b4e --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/decoder.py @@ -0,0 +1,1353 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Strict JSON/YAML-value decoder for Expert Program schema versions 1 and 2.""" + +from __future__ import annotations + +import math +import re +from collections.abc import Callable, Mapping +from copy import deepcopy +from typing import Literal, Protocol, TypeAlias, runtime_checkable + +from .cfg import ( + BarrierCfg, + EXPERT_PROGRAM_SCHEMA_VERSION, + EXPERT_PROGRAM_SCHEMA_VERSION_V2, + MAX_PROGRAM_DEPTH, + MAX_REPEAT_COUNT, + CyclicPoseTargetCfg, + ExpertProgramCfg, + ExpertProgramIntegrationCfg, + HandOverCfg, + InvokeCfg, + ObjectNearTargetValidatorCfg, + OperateArticulationCfg, + ParallelCfg, + PickCfg, + PlaceCfg, + PoseCfg, + PostPolicyCfg, + ProgramNodeCfg, + RegisteredSemanticCallCfg, + RepeatCfg, + SegmentCfg, + SemanticCallCfg, + SequenceCfg, + SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS, + TargetCfg, + TargetRefCfg, + ValidatorCfg, + WaitStablePostCfg, +) + +ConfigPathPart: TypeAlias = str | int +ConfigPath: TypeAlias = tuple[ConfigPathPart, ...] +SceneReferenceRole: TypeAlias = Literal[ + "entity", + "object", + "articulation", + "affordance", + "object_or_affordance", +] + +_MAX_INPUT_DEPTH = 128 +_MAX_INPUT_NODES = 100_000 +_ENV_TRAVERSAL_PATTERN = re.compile( + r"(?:\$?(?:env|environment)(?:\.[A-Za-z_][A-Za-z0-9_]*)+|" + r"\$\{(?:env|environment)(?:\.[A-Za-z_][A-Za-z0-9_]*)+\})" +) +_FORBIDDEN_KEYS = frozenset( + { + "__import__", + "attribute_path", + "callable", + "environment_path", + "env_path", + "eval", + "exec", + "expression", + "getattr", + "import", + "module", + "python", + } +) + + +def render_config_path(path: ConfigPath) -> str: + """Render one configuration path using JSONPath-like notation. + + Args: + path: Tuple of mapping keys and sequence indices. + + Returns: + Stable human-readable path beginning at ``$``. + """ + rendered = "$" + for part in path: + if type(part) is int: + rendered += f"[{part}]" + elif re.fullmatch(r"[A-Za-z_][A-Za-z0-9_]*", part) is not None: + rendered += f".{part}" + else: + rendered += f"[{part!r}]" + return rendered + + +class ExpertProgramConfigError(ValueError): + """Base pathful diagnostic for Expert Program configuration failures.""" + + def __init__(self, code: str, path: ConfigPath, message: str) -> None: + """Create one stable pathful diagnostic. + + Args: + code: Machine-readable failure code. + path: Exact configuration location. + message: Human-readable explanation. + """ + self.code = code + self.path = tuple(path) + self.message = message + super().__init__(f"{render_config_path(self.path)}: {message} [{code}]") + + +class ExpertProgramDecodeError(ExpertProgramConfigError): + """Raised when untrusted data does not match a supported strict schema.""" + + +class ExpertProgramValidationError(ExpertProgramConfigError): + """Raised when an explicit static integration context rejects a reference.""" + + +@runtime_checkable +class ExpertProgramValidationContext(Protocol): + """Provider-free static validation boundary for external references. + + Implementations may resolve profile, scene, preset, catalog, affordance, and + resource IDs, but must not observe simulation state, construct planners, or + execute calls. + """ + + def validate_integration( + self, + integration: ExpertProgramIntegrationCfg, + *, + path: ConfigPath, + ) -> None: + """Validate integration references at ``path``.""" + + def validate_semantic_call( + self, + call: SemanticCallCfg, + *, + path: ConfigPath, + ) -> None: + """Validate catalog identity, schema revision, and resource overrides.""" + + def validate_scene_reference( + self, + reference: str, + *, + role: SceneReferenceRole, + path: ConfigPath, + ) -> None: + """Validate one canonical scene reference with its semantic role.""" + + def validate_post_policy( + self, + policy: PostPolicyCfg, + *, + path: ConfigPath, + ) -> None: + """Validate a post-policy kind and its named preset.""" + + def validate_validator( + self, + validator: ValidatorCfg, + *, + path: ConfigPath, + ) -> None: + """Validate one registered segment-validator contract.""" + + +def _error(code: str, path: ConfigPath, message: str) -> ExpertProgramDecodeError: + """Build one decoder diagnostic.""" + return ExpertProgramDecodeError(code, path, message) + + +def _clone_untrusted_value( + value: object, + *, + path: ConfigPath, + active: set[int], + budget: list[int], + depth: int, +) -> object: + """Own and validate one bounded JSON-compatible value tree.""" + if depth > _MAX_INPUT_DEPTH: + raise _error( + "input_too_deep", + path, + f"Input exceeds nesting depth limit {_MAX_INPUT_DEPTH}.", + ) + budget[0] -= 1 + if budget[0] < 0: + raise _error( + "input_too_large", + path, + f"Input exceeds node limit {_MAX_INPUT_NODES}.", + ) + if value is None or type(value) in (bool, int): + return value + if type(value) is float: + if not math.isfinite(value): + raise _error("non_finite_number", path, "Floats must be finite.") + return value + if type(value) is str: + stripped = value.strip() + lowered = stripped.lower() + if lowered.startswith(("__import__(", "eval(", "exec(", "import ", "from ")): + raise _error( + "executable_expression", + path, + "Imports, eval, exec, and executable expressions are forbidden.", + ) + if _ENV_TRAVERSAL_PATTERN.fullmatch(stripped) is not None: + raise _error( + "environment_traversal", + path, + "Dotted environment attribute traversal is forbidden.", + ) + return value + if type(value) is list: + identity = id(value) + if identity in active: + raise _error("cyclic_input", path, "Input contains a cyclic list.") + active.add(identity) + try: + return [ + _clone_untrusted_value( + item, + path=(*path, index), + active=active, + budget=budget, + depth=depth + 1, + ) + for index, item in enumerate(value) + ] + finally: + active.remove(identity) + if type(value) is dict: + identity = id(value) + if identity in active: + raise _error("cyclic_input", path, "Input contains a cyclic mapping.") + active.add(identity) + try: + result: dict[str, object] = {} + for key, item in value.items(): + if type(key) is not str: + raise _error( + "invalid_mapping_key", + path, + "Mapping keys must be exact strings.", + ) + if key.lower() in _FORBIDDEN_KEYS: + raise _error( + "forbidden_construct", + (*path, key), + f"Field {key!r} requests executable or traversal behavior.", + ) + result[key] = _clone_untrusted_value( + item, + path=(*path, key), + active=active, + budget=budget, + depth=depth + 1, + ) + return result + finally: + active.remove(identity) + raise _error( + "non_declarative_value", + path, + f"{type(value).__name__} is not JSON-compatible declarative data; " + "callables, classes, modules, tensors, and live objects are forbidden.", + ) + + +def _expect_mapping(value: object, *, path: ConfigPath) -> dict[str, object]: + """Require one exact mapping.""" + if type(value) is not dict: + raise _error("expected_mapping", path, "Expected an object mapping.") + return value + + +def _expect_list(value: object, *, path: ConfigPath) -> list[object]: + """Require one exact JSON list.""" + if type(value) is not list: + raise _error("expected_list", path, "Expected a list.") + return value + + +def _validate_fields( + value: dict[str, object], + *, + allowed: frozenset[str], + required: frozenset[str], + path: ConfigPath, +) -> None: + """Reject unknown fields and report the first missing required field.""" + unknown = sorted(set(value).difference(allowed)) + if unknown: + field_name = unknown[0] + raise _error( + "unknown_field", + (*path, field_name), + f"Unknown field {field_name!r}; allowed fields are {sorted(allowed)}.", + ) + missing = sorted(required.difference(value)) + if missing: + field_name = missing[0] + raise _error( + "missing_field", + (*path, field_name), + f"Missing required field {field_name!r}.", + ) + + +def _expect_identifier(value: object, *, path: ConfigPath) -> str: + """Require one exact non-empty identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise _error( + "invalid_identifier", + path, + "Expected a non-empty string without outer whitespace.", + ) + return value + + +def _expect_discriminator( + value: dict[str, object], + *, + path: ConfigPath, + supported: tuple[str, ...], +) -> str: + """Read one required exact string discriminator.""" + if "kind" not in value: + raise _error( + "missing_discriminator", + (*path, "kind"), + "Missing required discriminator 'kind'.", + ) + kind = value["kind"] + if type(kind) is not str or kind not in supported: + raise _error( + "unknown_discriminator", + (*path, "kind"), + f"Unsupported discriminator {kind!r}; expected one of {supported}.", + ) + return kind + + +def _decode_resources(value: object, *, path: ConfigPath) -> dict[str, str]: + """Decode one strict slot-to-resource mapping.""" + mapping = _expect_mapping(value, path=path) + return { + _expect_identifier(slot_id, path=(*path, slot_id)): _expect_identifier( + resource_id, + path=(*path, slot_id), + ) + for slot_id, resource_id in mapping.items() + } + + +def _construct( + constructor: Callable[..., object], + *, + path: ConfigPath, + **kwargs: object, +) -> object: + """Construct one config value and wrap invariant failures pathfully.""" + try: + return constructor(**kwargs) + except ExpertProgramConfigError: + raise + except (TypeError, ValueError) as exc: + raise _error("invalid_value", path, str(exc)) from exc + + +def _decode_pose(value: object, *, path: ConfigPath) -> PoseCfg: + """Decode one finite pose value.""" + mapping = _expect_mapping(value, path=path) + _validate_fields( + mapping, + allowed=frozenset({"position", "quaternion_wxyz"}), + required=frozenset({"position", "quaternion_wxyz"}), + path=path, + ) + position_values = _expect_list(mapping["position"], path=(*path, "position")) + quaternion_values = _expect_list( + mapping["quaternion_wxyz"], + path=(*path, "quaternion_wxyz"), + ) + if len(position_values) != 3: + raise _error( + "invalid_pose_shape", + (*path, "position"), + "position must contain exactly three numbers.", + ) + if len(quaternion_values) != 4: + raise _error( + "invalid_pose_shape", + (*path, "quaternion_wxyz"), + "quaternion_wxyz must contain exactly four numbers.", + ) + for name, values in ( + ("position", position_values), + ("quaternion_wxyz", quaternion_values), + ): + for index, number in enumerate(values): + if type(number) not in (int, float): + raise _error( + "invalid_number", + (*path, name, index), + "Pose components must be finite numbers, not bool values.", + ) + return _construct( + PoseCfg, + path=path, + position=tuple(position_values), + quaternion_wxyz=tuple(quaternion_values), + ) # type: ignore[return-value] + + +def _decode_target(value: object, *, path: ConfigPath) -> TargetCfg: + """Decode one target provider shared by the supported schema versions.""" + mapping = _expect_mapping(value, path=path) + kind = _expect_discriminator( + mapping, + path=path, + supported=("cyclic_pose",), + ) + assert kind == "cyclic_pose" + _validate_fields( + mapping, + allowed=frozenset({"kind", "values"}), + required=frozenset({"kind", "values"}), + path=path, + ) + values = tuple( + _decode_pose(item, path=(*path, "values", index)) + for index, item in enumerate( + _expect_list(mapping["values"], path=(*path, "values")) + ) + ) + return _construct( + CyclicPoseTargetCfg, + path=path, + kind=kind, + values=values, + ) # type: ignore[return-value] + + +def _decode_target_ref( + value: object, + *, + path: ConfigPath, + target_ids: frozenset[str], +) -> TargetRefCfg: + """Decode and statically resolve one target reference.""" + mapping = _expect_mapping(value, path=path) + kind = _expect_discriminator(mapping, path=path, supported=("target_ref",)) + _validate_fields( + mapping, + allowed=frozenset({"kind", "target"}), + required=frozenset({"kind", "target"}), + path=path, + ) + target = _expect_identifier(mapping["target"], path=(*path, "target")) + if target not in target_ids: + raise _error( + "unknown_target", + (*path, "target"), + f"Unknown target {target!r}; available targets are {sorted(target_ids)}.", + ) + return _construct( + TargetRefCfg, + path=path, + kind=kind, + target=target, + ) # type: ignore[return-value] + + +def _decode_call( + value: object, + *, + path: ConfigPath, + target_ids: frozenset[str], +) -> SemanticCallCfg: + """Decode one discriminated semantic call.""" + mapping = _expect_mapping(value, path=path) + kind = _expect_discriminator( + mapping, + path=path, + supported=( + "pick", + "place", + "hand_over", + "operate_articulation", + "registered", + ), + ) + resources = _decode_resources( + mapping.get("resources", {}), + path=(*path, "resources"), + ) + if kind == "pick": + _validate_fields( + mapping, + allowed=frozenset({"kind", "object", "grasp", "resources"}), + required=frozenset({"kind", "object"}), + path=path, + ) + grasp = mapping.get("grasp") + if grasp is not None: + grasp = _expect_identifier(grasp, path=(*path, "grasp")) + return _construct( + PickCfg, + path=path, + kind=kind, + object=_expect_identifier(mapping["object"], path=(*path, "object")), + grasp=grasp, + resources=resources, + ) # type: ignore[return-value] + if kind == "place": + _validate_fields( + mapping, + allowed=frozenset({"kind", "object", "at", "on", "inside", "resources"}), + required=frozenset({"kind", "object"}), + path=path, + ) + at = ( + None + if mapping.get("at") is None + else _decode_target_ref( + mapping["at"], + path=(*path, "at"), + target_ids=target_ids, + ) + ) + on = mapping.get("on") + inside = mapping.get("inside") + if on is not None: + on = _expect_identifier(on, path=(*path, "on")) + if inside is not None: + inside = _expect_identifier(inside, path=(*path, "inside")) + return _construct( + PlaceCfg, + path=path, + kind=kind, + object=_expect_identifier(mapping["object"], path=(*path, "object")), + at=at, + on=on, + inside=inside, + resources=resources, + ) # type: ignore[return-value] + if kind == "hand_over": + _validate_fields( + mapping, + allowed=frozenset({"kind", "object", "final_target", "resources"}), + required=frozenset({"kind", "object"}), + path=path, + ) + final_target = ( + None + if mapping.get("final_target") is None + else _decode_target_ref( + mapping["final_target"], + path=(*path, "final_target"), + target_ids=target_ids, + ) + ) + return _construct( + HandOverCfg, + path=path, + kind=kind, + object=_expect_identifier(mapping["object"], path=(*path, "object")), + final_target=final_target, + resources=resources, + ) # type: ignore[return-value] + if kind == "operate_articulation": + _validate_fields( + mapping, + allowed=frozenset( + { + "kind", + "articulation", + "handle", + "target", + "target_position", + "target_displacement", + "resources", + } + ), + required=frozenset({"kind", "articulation"}), + path=path, + ) + handle = mapping.get("handle") + target = mapping.get("target") + if handle is not None: + handle = _expect_identifier(handle, path=(*path, "handle")) + if target is not None: + target = _expect_identifier(target, path=(*path, "target")) + target_position = mapping.get("target_position") + target_displacement = mapping.get("target_displacement") + for field_name, value in ( + ("target_position", target_position), + ("target_displacement", target_displacement), + ): + if value is not None and type(value) not in (int, float): + raise _error( + "invalid_number", + (*path, field_name), + f"{field_name} must be a finite number, not bool.", + ) + named = target is not None + explicit_position = target_position is not None + explicit_displacement = target_displacement is not None + if named and (explicit_position or explicit_displacement): + raise _error( + "conflicting_articulation_target", + path, + "target is mutually exclusive with target_position and " + "target_displacement.", + ) + if not named and not (explicit_position and explicit_displacement): + raise _error( + "incomplete_articulation_target", + path, + "Specify target or both target_position and target_displacement.", + ) + return _construct( + OperateArticulationCfg, + path=path, + kind=kind, + articulation=_expect_identifier( + mapping["articulation"], + path=(*path, "articulation"), + ), + handle=handle, + target=target, + target_position=target_position, + target_displacement=target_displacement, + resources=resources, + ) # type: ignore[return-value] + + _validate_fields( + mapping, + allowed=frozenset( + {"kind", "call_id", "schema_version", "arguments", "resources"} + ), + required=frozenset({"kind", "call_id", "schema_version"}), + path=path, + ) + arguments = _expect_mapping( + mapping.get("arguments", {}), + path=(*path, "arguments"), + ) + schema_version = mapping["schema_version"] + if type(schema_version) is not int or schema_version != 1: + raise _error( + "invalid_schema_version", + (*path, "schema_version"), + "Registered call schema_version must be exactly 1.", + ) + return _construct( + RegisteredSemanticCallCfg, + path=path, + kind=kind, + call_id=_expect_identifier(mapping["call_id"], path=(*path, "call_id")), + schema_version=schema_version, + arguments=arguments, + resources=resources, + ) # type: ignore[return-value] + + +def _decode_post_policy(value: object, *, path: ConfigPath) -> PostPolicyCfg: + """Decode one segment post-policy shared by the supported schemas.""" + mapping = _expect_mapping(value, path=path) + kind = _expect_discriminator(mapping, path=path, supported=("wait_stable",)) + _validate_fields( + mapping, + allowed=frozenset({"kind", "entity", "preset"}), + required=frozenset({"kind", "entity"}), + path=path, + ) + return _construct( + WaitStablePostCfg, + path=path, + kind=kind, + entity=_expect_identifier(mapping["entity"], path=(*path, "entity")), + preset=_expect_identifier( + mapping.get("preset", "rigid_object"), + path=(*path, "preset"), + ), + ) # type: ignore[return-value] + + +def _decode_validator( + value: object, + *, + path: ConfigPath, + target_ids: frozenset[str], +) -> ValidatorCfg: + """Decode one segment validator shared by the supported schemas.""" + mapping = _expect_mapping(value, path=path) + kind = _expect_discriminator( + mapping, + path=path, + supported=("object_near_target",), + ) + _validate_fields( + mapping, + allowed=frozenset({"kind", "object", "target", "position_tolerance"}), + required=frozenset({"kind", "object", "target"}), + path=path, + ) + target = _expect_identifier(mapping["target"], path=(*path, "target")) + if target not in target_ids: + raise _error( + "unknown_target", + (*path, "target"), + f"Unknown target {target!r}; available targets are {sorted(target_ids)}.", + ) + tolerance = mapping.get("position_tolerance", 0.03) + if type(tolerance) not in (int, float): + raise _error( + "invalid_number", + (*path, "position_tolerance"), + "position_tolerance must be a finite number, not bool.", + ) + return _construct( + ObjectNearTargetValidatorCfg, + path=path, + kind=kind, + object=_expect_identifier(mapping["object"], path=(*path, "object")), + target=target, + position_tolerance=tolerance, + ) # type: ignore[return-value] + + +def _decode_program_node( + value: object, + *, + path: ConfigPath, + target_ids: frozenset[str], + depth: int, + schema_version: int, +) -> ProgramNodeCfg: + """Recursively decode one bounded versioned program node.""" + if depth > MAX_PROGRAM_DEPTH: + raise _error( + "program_too_deep", + path, + "Program AST exceeds the configured nesting depth.", + ) + mapping = _expect_mapping(value, path=path) + supported_kinds = ["sequence", "repeat", "segment", "invoke"] + if schema_version >= EXPERT_PROGRAM_SCHEMA_VERSION_V2: + supported_kinds.extend(("parallel", "barrier")) + kind = _expect_discriminator( + mapping, + path=path, + supported=tuple(supported_kinds), + ) + if kind == "sequence": + _validate_fields( + mapping, + allowed=frozenset({"kind", "items"}), + required=frozenset({"kind", "items"}), + path=path, + ) + items = tuple( + _decode_program_node( + item, + path=(*path, "items", index), + target_ids=target_ids, + depth=depth + 1, + schema_version=schema_version, + ) + for index, item in enumerate( + _expect_list(mapping["items"], path=(*path, "items")) + ) + ) + return _construct( + SequenceCfg, + path=path, + kind=kind, + items=items, + ) # type: ignore[return-value] + if kind == "repeat": + _validate_fields( + mapping, + allowed=frozenset({"kind", "count", "body"}), + required=frozenset({"kind", "count", "body"}), + path=path, + ) + count = mapping["count"] + if type(count) is not int or not 1 <= count <= MAX_REPEAT_COUNT: + raise _error( + "invalid_repeat_count", + (*path, "count"), + f"Repeat count must be an integer in [1, {MAX_REPEAT_COUNT}].", + ) + return _construct( + RepeatCfg, + path=path, + kind=kind, + count=count, + body=_decode_program_node( + mapping["body"], + path=(*path, "body"), + target_ids=target_ids, + depth=depth + 1, + schema_version=schema_version, + ), + ) # type: ignore[return-value] + if kind == "segment": + _validate_fields( + mapping, + allowed=frozenset({"kind", "name", "steps", "post", "validators"}), + required=frozenset({"kind", "name", "steps"}), + path=path, + ) + post = tuple( + _decode_post_policy(item, path=(*path, "post", index)) + for index, item in enumerate( + _expect_list(mapping.get("post", []), path=(*path, "post")) + ) + ) + validators = tuple( + _decode_validator( + item, + path=(*path, "validators", index), + target_ids=target_ids, + ) + for index, item in enumerate( + _expect_list( + mapping.get("validators", []), + path=(*path, "validators"), + ) + ) + ) + return _construct( + SegmentCfg, + path=path, + kind=kind, + name=_expect_identifier(mapping["name"], path=(*path, "name")), + steps=_decode_program_node( + mapping["steps"], + path=(*path, "steps"), + target_ids=target_ids, + depth=depth + 1, + schema_version=schema_version, + ), + post=post, + validators=validators, + ) # type: ignore[return-value] + + if kind == "parallel": + _validate_fields( + mapping, + allowed=frozenset({"kind", "branches", "barrier"}), + required=frozenset({"kind", "branches", "barrier"}), + path=path, + ) + branches_values = _expect_list( + mapping["branches"], + path=(*path, "branches"), + ) + if len(branches_values) < 2: + raise _error( + "parallel_branch_count", + (*path, "branches"), + "Parallel requires at least two branches.", + ) + barrier = _decode_program_node( + mapping["barrier"], + path=(*path, "barrier"), + target_ids=target_ids, + depth=depth + 1, + schema_version=schema_version, + ) + if type(barrier) is not BarrierCfg: + raise _error( + "parallel_barrier_required", + (*path, "barrier"), + "Parallel.barrier must be an explicit barrier node.", + ) + return _construct( + ParallelCfg, + path=path, + kind=kind, + branches=tuple( + _decode_program_node( + branch, + path=(*path, "branches", index), + target_ids=target_ids, + depth=depth + 1, + schema_version=schema_version, + ) + for index, branch in enumerate(branches_values) + ), + barrier=barrier, + ) # type: ignore[return-value] + if kind == "barrier": + _validate_fields( + mapping, + allowed=frozenset({"kind", "name", "timeout_steps", "failure_policy"}), + required=frozenset({"kind", "name"}), + path=path, + ) + timeout_steps = mapping.get("timeout_steps", 1_000) + if type(timeout_steps) is not int or timeout_steps <= 0: + raise _error( + "invalid_barrier_timeout", + (*path, "timeout_steps"), + "Barrier timeout_steps must be a positive integer.", + ) + failure_policy = mapping.get("failure_policy", "fail_fast") + if failure_policy != "fail_fast": + raise _error( + "unsupported_failure_policy", + (*path, "failure_policy"), + "Barrier failure_policy must be exactly 'fail_fast'.", + ) + return _construct( + BarrierCfg, + path=path, + kind=kind, + name=_expect_identifier(mapping["name"], path=(*path, "name")), + timeout_steps=timeout_steps, + failure_policy=failure_policy, + ) # type: ignore[return-value] + + _validate_fields( + mapping, + allowed=frozenset({"kind", "call"}), + required=frozenset({"kind", "call"}), + path=path, + ) + return _construct( + InvokeCfg, + path=path, + kind=kind, + call=_decode_call( + mapping["call"], + path=(*path, "call"), + target_ids=target_ids, + ), + ) # type: ignore[return-value] + + +def _walk_program( + node: ProgramNodeCfg, + *, + path: ConfigPath, +) -> list[tuple[ProgramNodeCfg, ConfigPath]]: + """Return deterministic node/path pairs for static context validation.""" + values = [(node, path)] + if type(node) is SequenceCfg: + for index, child in enumerate(node.items): + values.extend(_walk_program(child, path=(*path, "items", index))) + elif type(node) is RepeatCfg: + values.extend(_walk_program(node.body, path=(*path, "body"))) + elif type(node) is SegmentCfg: + values.extend(_walk_program(node.steps, path=(*path, "steps"))) + elif type(node) is ParallelCfg: + for index, branch in enumerate(node.branches): + values.extend(_walk_program(branch, path=(*path, "branches", index))) + values.extend(_walk_program(node.barrier, path=(*path, "barrier"))) + return values + + +def _call_context( + callback: Callable[..., None], + *args: object, + path: ConfigPath, + **kwargs: object, +) -> None: + """Call one static validation hook and preserve pathful failures.""" + try: + callback(*args, path=path, **kwargs) + except ExpertProgramConfigError: + raise + except (KeyError, TypeError, ValueError) as exc: + raise ExpertProgramValidationError( + "reference_validation_failed", + path, + str(exc), + ) from exc + + +def _validate_decoded_semantic_call( + call: SemanticCallCfg, + context: ExpertProgramValidationContext, + *, + path: ConfigPath, +) -> None: + """Validate one decoded call against provider-free catalog and scene ports.""" + _call_context(context.validate_semantic_call, call, path=path) + if type(call) in (PickCfg, PlaceCfg, HandOverCfg): + _call_context( + context.validate_scene_reference, + call.object, + role="object", + path=(*path, "object"), + ) + if type(call) is PickCfg and call.grasp is not None: + _call_context( + context.validate_scene_reference, + call.grasp, + role="affordance", + path=(*path, "grasp"), + ) + if type(call) is PlaceCfg: + for field_name in ("on", "inside"): + reference = getattr(call, field_name) + if reference is not None: + _call_context( + context.validate_scene_reference, + reference, + role="object_or_affordance", + path=(*path, field_name), + ) + if type(call) is OperateArticulationCfg: + _call_context( + context.validate_scene_reference, + call.articulation, + role="articulation", + path=(*path, "articulation"), + ) + if call.handle is not None: + _call_context( + context.validate_scene_reference, + call.handle, + role="affordance", + path=(*path, "handle"), + ) + + +def decode_semantic_call( + data: object, + *, + target_ids: frozenset[str] = frozenset(), + validation_context: ExpertProgramValidationContext | None = None, + path: ConfigPath = ("call",), +) -> SemanticCallCfg: + """Decode one untrusted canonical semantic-call payload. + + Args: + data: Exact JSON-compatible call mapping produced by a trusted parser. + target_ids: Target IDs available to ``at`` or ``final_target`` refs. + validation_context: Optional provider-free catalog and scene validator. + path: Diagnostic root used when the call is embedded in another schema. + + Returns: + Fully owned and strictly validated canonical semantic-call config. + + Raises: + ExpertProgramDecodeError: If the payload violates the call schema. + ExpertProgramValidationError: If provider-free validation rejects it. + """ + if type(target_ids) is not frozenset or not all( + type(target_id) is str and target_id and target_id == target_id.strip() + for target_id in target_ids + ): + raise TypeError("target_ids must be a frozenset of exact identifiers.") + if type(path) is not tuple: + raise TypeError("path must be a ConfigPath tuple.") + owned = _clone_untrusted_value( + data, + path=path, + active=set(), + budget=[_MAX_INPUT_NODES], + depth=0, + ) + call = _decode_call(owned, path=path, target_ids=target_ids) + if validation_context is not None: + if not isinstance(validation_context, ExpertProgramValidationContext): + raise TypeError( + "validation_context must implement " "ExpertProgramValidationContext." + ) + _validate_decoded_semantic_call(call, validation_context, path=path) + return call + + +def encode_semantic_call(call: SemanticCallCfg) -> dict[str, object]: + """Encode one canonical semantic-call config as owned JSON-safe values. + + Args: + call: Exact supported semantic-call config. + + Returns: + Deterministic mapping accepted by :func:`decode_semantic_call`. + """ + if type(call) not in ( + PickCfg, + PlaceCfg, + HandOverCfg, + OperateArticulationCfg, + RegisteredSemanticCallCfg, + ): + raise TypeError("call must be an exact SemanticCallCfg value.") + result: dict[str, object] = {"kind": call.kind} + if type(call) is PickCfg: + result["object"] = call.object + if call.grasp is not None: + result["grasp"] = call.grasp + elif type(call) is PlaceCfg: + result["object"] = call.object + if call.at is not None: + result["at"] = {"kind": call.at.kind, "target": call.at.target} + if call.on is not None: + result["on"] = call.on + if call.inside is not None: + result["inside"] = call.inside + elif type(call) is HandOverCfg: + result["object"] = call.object + if call.final_target is not None: + result["final_target"] = { + "kind": call.final_target.kind, + "target": call.final_target.target, + } + elif type(call) is OperateArticulationCfg: + result["articulation"] = call.articulation + if call.handle is not None: + result["handle"] = call.handle + if call.target is not None: + result["target"] = call.target + else: + result["target_position"] = call.target_position + result["target_displacement"] = call.target_displacement + else: + assert type(call) is RegisteredSemanticCallCfg + result["call_id"] = call.call_id + result["schema_version"] = call.schema_version + result["arguments"] = _encode_declarative_value(call.arguments) + if call.resources: + result["resources"] = dict(call.resources) + return result + + +def _encode_declarative_value(value: object) -> object: + """Convert an owned config snapshot back to exact JSON-compatible values.""" + if value is None or type(value) in (bool, int, float, str): + return value + if isinstance(value, Mapping): + return { + str(key): _encode_declarative_value(nested) for key, nested in value.items() + } + if isinstance(value, (tuple, list)): + return [_encode_declarative_value(nested) for nested in value] + return deepcopy(value) + + +def validate_expert_program( + config: ExpertProgramCfg, + context: ExpertProgramValidationContext, +) -> None: + """Resolve external references without observing or executing an environment. + + Args: + config: Fully decoded and internally validated Expert Program. + context: Provider-free static integration/catalog/scene validator. + + Raises: + TypeError: If either argument has the wrong contract. + ExpertProgramValidationError: If an external reference is unavailable. + """ + if type(config) is not ExpertProgramCfg: + raise TypeError("config must be exactly ExpertProgramCfg.") + if not isinstance(context, ExpertProgramValidationContext): + raise TypeError( + "context must implement ExpertProgramValidationContext exactly." + ) + _call_context( + context.validate_integration, + config.integration, + path=("integration",), + ) + for node, path in _walk_program(config.program, path=("program",)): + if type(node) is InvokeCfg: + call = node.call + call_path = (*path, "call") + _validate_decoded_semantic_call(call, context, path=call_path) + elif type(node) is SegmentCfg: + for index, post in enumerate(node.post): + post_path = (*path, "post", index) + _call_context( + context.validate_post_policy, + post, + path=post_path, + ) + _call_context( + context.validate_scene_reference, + post.entity, + role="entity", + path=(*post_path, "entity"), + ) + for index, validator in enumerate(node.validators): + validator_path = (*path, "validators", index) + _call_context( + context.validate_validator, + validator, + path=validator_path, + ) + _call_context( + context.validate_scene_reference, + validator.object, + role="object", + path=(*validator_path, "object"), + ) + + +def decode_expert_program( + data: object, + *, + validation_context: ExpertProgramValidationContext | None = None, +) -> ExpertProgramCfg: + """Decode untrusted JSON/YAML-shaped values into strict versioned config. + + Schema versions 1 and 2 are supported. Version 2 adds deterministic + parallel blocks with explicit barriers while preserving the Version 1 + sequential nodes and semantic calls. + + Args: + data: Exact JSON-compatible mapping produced by a trusted parser. + validation_context: Optional provider-free static reference validator. + + Returns: + Fully owned and internally validated Expert Program configuration. + + Raises: + ExpertProgramDecodeError: If data is unsafe or violates the schema. + ExpertProgramValidationError: If an explicit context rejects a reference. + """ + owned = _clone_untrusted_value( + data, + path=(), + active=set(), + budget=[_MAX_INPUT_NODES], + depth=0, + ) + mapping = _expect_mapping(owned, path=()) + _validate_fields( + mapping, + allowed=frozenset( + {"schema_version", "program_id", "integration", "targets", "program"} + ), + required=frozenset( + {"schema_version", "program_id", "integration", "targets", "program"} + ), + path=(), + ) + schema_version = mapping["schema_version"] + if ( + type(schema_version) is not int + or schema_version not in SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS + ): + raise _error( + "unsupported_schema_version", + ("schema_version",), + "Supported schema versions are " + f"{list(SUPPORTED_EXPERT_PROGRAM_SCHEMA_VERSIONS)}.", + ) + + integration_mapping = _expect_mapping( + mapping["integration"], + path=("integration",), + ) + _validate_fields( + integration_mapping, + allowed=frozenset({"robot_profile", "scene_registry", "runtime_preset"}), + required=frozenset({"robot_profile", "scene_registry", "runtime_preset"}), + path=("integration",), + ) + integration = _construct( + ExpertProgramIntegrationCfg, + path=("integration",), + robot_profile=_expect_identifier( + integration_mapping["robot_profile"], + path=("integration", "robot_profile"), + ), + scene_registry=_expect_identifier( + integration_mapping["scene_registry"], + path=("integration", "scene_registry"), + ), + runtime_preset=_expect_identifier( + integration_mapping["runtime_preset"], + path=("integration", "runtime_preset"), + ), + ) + + target_mapping = _expect_mapping(mapping["targets"], path=("targets",)) + targets: dict[str, TargetCfg] = {} + for target_id, target_value in target_mapping.items(): + normalized_id = _expect_identifier(target_id, path=("targets", target_id)) + targets[normalized_id] = _decode_target( + target_value, + path=("targets", normalized_id), + ) + target_ids = frozenset(targets) + program = _decode_program_node( + mapping["program"], + path=("program",), + target_ids=target_ids, + depth=0, + schema_version=schema_version, + ) + config = _construct( + ExpertProgramCfg, + path=(), + schema_version=schema_version, + program_id=_expect_identifier(mapping["program_id"], path=("program_id",)), + integration=integration, + targets=targets, + program=program, + ) + assert type(config) is ExpertProgramCfg + if validation_context is not None: + validate_expert_program(config, validation_context) + return config + + +__all__ = [ + "ConfigPath", + "ConfigPathPart", + "ExpertProgramConfigError", + "ExpertProgramDecodeError", + "ExpertProgramValidationContext", + "ExpertProgramValidationError", + "SceneReferenceRole", + "decode_expert_program", + "decode_semantic_call", + "encode_semantic_call", + "render_config_path", + "validate_expert_program", +] diff --git a/embodichain/lab/gym/envs/expert_program/environment.py b/embodichain/lab/gym/envs/expert_program/environment.py new file mode 100644 index 000000000..2f690a5ca --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/environment.py @@ -0,0 +1,1127 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Explicit production assembly for environment-backed Expert Programs. + +The adapter in this module is deliberately strict. It does not scan a +simulation, infer robot resources, or manufacture task semantics from naming +conventions. An environment supplies one typed factory that owns all live +provider choices; the adapter validates those declarations and wires the +shared semantic compiler, runtime, and Gym bridge. +""" + +from __future__ import annotations + +from collections.abc import Iterable, Mapping +from copy import deepcopy +from dataclasses import dataclass +import math +from typing import Protocol, runtime_checkable + +from embodichain.lab.sim.atomic_actions.engine import AtomicActionEngine +from embodichain.lab.sim.atomic_actions.runner import ( + ExecutionRunnerCfg, + ObservationProvider, +) +from embodichain.lab.sim.skills.calls import ( + SemanticCallCatalog, + SemanticCallSpec, + builtin_semantic_call_catalog, +) +from embodichain.lab.sim.skills.compiler import ( + HandOverPoseProvider, + RegisteredSemanticLowerer, + RelationTargetGrounder, + SemanticSkillCompiler, +) +from embodichain.lab.sim.skills.effects import EffectMonitorRegistry +from embodichain.lab.sim.skills.evidence import ( + EffectEvidenceCollector, + EffectEvidenceProvider, + EffectEvidenceProviderRegistry, +) +from embodichain.lab.sim.skills.integration import ( + SceneManifest, + SemanticIntegrationManifest, +) +from embodichain.lab.sim.skills.parallel_runtime import ( + ParallelCommandSafetyValidator, + analyze_parallel_branches, +) +from embodichain.lab.sim.skills.profiles import ( + BoundRobotSkillProfile, + ResourceEndpoint, + ResourceEndpointAdapter, + RobotSkillProfile, +) +from embodichain.lab.sim.skills.runtime import SkillRuntime +from embodichain.lab.sim.skills.scene import SceneRegistry + +from .bridge import ( + AcceptedRuntimeCommandObserver, + AtomicDemoBridge, + BufferedGymCommandSink, + CurrentQposProvider, + DemoBridgeError, + EnvironmentStepClock, + JointPositionGymTransportEncoder, + RuntimeCommandFrameEncoder, + RuntimeTransportActionEncoder, + SegmentPostPolicyPort, + SegmentValidatorPort, +) +from .catalog import ( + ExpertProgramIntegrationCatalog, + IntegrationFingerprintMismatch, + SimulationExpertProgramRegistration, +) +from .cfg import ExpertProgramCfg, ExpertProgramIntegrationCfg +from .compiler import ( + CompiledProgram, + ExpertProgramCompiler, + MaterializedCompiledProgram, +) + + +def _validate_identifier(value: str, *, field_name: str) -> str: + """Validate one stable integration identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +@runtime_checkable +class PlanningObservationPort( + ObservationProvider, + CurrentQposProvider, + Protocol, +): + """Combined observation and full-qpos port required by the Gym runtime.""" + + +@runtime_checkable +class ExpertProgramEnvironmentFactory(Protocol): + """Environment-owned factories for one explicit semantic integration. + + Implementations normally live in reusable robot/task integration modules, + not in individual task motion planners. Every method is passed the exact + objects selected earlier in the assembly so a factory cannot silently bind + a different scene, robot profile, or engine. + """ + + @property + def scene_registry_id(self) -> str: + """Return the configuration ID selecting this scene declaration. + + Returns: + Stable scene-registry identifier. + """ + + @property + def robot_profile_id(self) -> str: + """Return the configuration ID selecting this robot profile. + + Returns: + Stable robot-profile identifier. + """ + + def create_scene_registry(self) -> SceneRegistry: + """Create the authoritative explicitly registered live scene. + + Returns: + Fresh registry containing only explicitly selected entities. + """ + + def create_robot_skill_profile(self) -> RobotSkillProfile: + """Create the authoritative declarative robot skill profile. + + Returns: + Profile whose ID matches :attr:`robot_profile_id`. + """ + + def create_atomic_action_engine( + self, + profile: RobotSkillProfile, + ) -> AtomicActionEngine: + """Create an engine for exactly ``profile`` and its motion backend. + + Args: + profile: Profile selected and validated by the adapter. + + Returns: + Atomic engine connected to the environment's robot and planner. + """ + + def create_planning_observation_provider( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + clock: EnvironmentStepClock, + ) -> PlanningObservationPort: + """Create fresh planning observations and aligned full-qpos reads. + + Args: + scene_registry: Exact registry selected for this runtime. + engine: Exact atomic engine selected for this runtime. + clock: Shared environment-step execution clock. + + Returns: + Combined planning-observation and qpos provider. + """ + + def create_effect_evidence_providers( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + observation_provider: PlanningObservationPort, + ) -> Iterable[EffectEvidenceProvider]: + """Create exact-version providers used by semantic effect monitors. + + Args: + scene_registry: Exact registry selected for this runtime. + engine: Exact atomic engine selected for this runtime. + observation_provider: Shared planning observation provider. + + Returns: + Explicit provider set; an empty iterable is permitted. + """ + + +@runtime_checkable +class AcceptedRuntimeCommandObserverFactory(Protocol): + """Optional factory capability for runtime-local accepted-command state. + + The observer is created from the exact observation provider used by the + runtime so command-derived evidence cannot leak across bridge instances or + bind to a different simulation batch. + """ + + def create_accepted_runtime_command_observer( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + observation_provider: PlanningObservationPort, + ) -> AcceptedRuntimeCommandObserver: + """Return the observer shared by the command sink and evidence ports.""" + + +@runtime_checkable +class ParallelCommandSafetyValidatorProvider(Protocol): + """Runtime-factory capability for a fresh registration-owned safety gate.""" + + def create_parallel_command_safety_validator( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + observation_provider: PlanningObservationPort, + ) -> ParallelCommandSafetyValidator: + """Create the live gate for the exact assembled runtime components.""" + + +@runtime_checkable +class _RegistrationOwningExpertProgramFactory(Protocol): + """Internal capability exposing one exact standard registration owner.""" + + @property + def expert_program_registration(self) -> SimulationExpertProgramRegistration: + """Return the exact registration owned by this live factory.""" + + def registration_owned_segment_policy_ports( + self, + ) -> tuple[SegmentPostPolicyPort | None, SegmentValidatorPort | None]: + """Return factory-owned post-policy and validator ports.""" + + +@dataclass(frozen=True, slots=True) +class ExpertProgramRuntimeAssembly: + """Auditable result of one fresh environment runtime assembly. + + Attributes: + integration: Owned integration-selection snapshot. + scene_registry: Authoritative live scene registry. + robot_profile: Declarative robot resource profile. + manifest: Static scene/profile/call integration manifest. + engine: Bound atomic action engine. + compiler: Bound semantic skill compiler. + observation_provider: Shared planning and full-qpos provider. + evidence_collector: Exact-version semantic evidence collector. + clock: Shared environment-step clock. + command_encoder: Runtime-frame to Gym-action encoder. + command_sink: Buffered Gym command sink. + accepted_command_observer: Optional transactional command-state owner. + runner_cfg: Runner policy selected by the integration runtime preset. + parallel_safety_validator: Optional fresh registration-owned safety gate. + runtime: Nonblocking semantic skill runtime. + """ + + integration: ExpertProgramIntegrationCfg + scene_registry: SceneRegistry + robot_profile: RobotSkillProfile + manifest: SemanticIntegrationManifest + engine: AtomicActionEngine + compiler: SemanticSkillCompiler + observation_provider: PlanningObservationPort + evidence_collector: EffectEvidenceCollector + clock: EnvironmentStepClock + command_encoder: RuntimeCommandFrameEncoder + command_sink: BufferedGymCommandSink + accepted_command_observer: AcceptedRuntimeCommandObserver | None + runner_cfg: ExecutionRunnerCfg + parallel_safety_validator: ParallelCommandSafetyValidator | None + runtime: SkillRuntime + + +@runtime_checkable +class SkillRuntimeAssemblyPort(Protocol): + """Narrow factory port for frontends that need one canonical runtime.""" + + def assemble_runtime( + self, + integration: ExpertProgramIntegrationCfg, + ) -> ExpertProgramRuntimeAssembly: + """Create a fresh runtime assembly for an exact integration selection.""" + + +@dataclass(frozen=True, slots=True) +class _ExpertProgramSemanticAssembly: + """Observation-free semantic components prepared for program preflight.""" + + integration: ExpertProgramIntegrationCfg + scene_registry: SceneRegistry + robot_profile: RobotSkillProfile + manifest: SemanticIntegrationManifest + engine: AtomicActionEngine + compiler: SemanticSkillCompiler + + +class ExpertProgramEnvironmentAdapter: + """Compile and run Expert Programs through explicit environment factories. + + Args: + factory: Environment-owned live-provider and engine factory. + step_dt: Authoritative Gym control cadence in seconds. + integration_catalog: Optional immutable task-registration catalog used + for provider-free compilation. + registration: Optional exact standard task registration. When present, + every compiler/runtime extension comes exclusively from it. + call_catalog: Optional immutable semantic call catalog. The built-in + catalog is used when omitted. + endpoint_adapters: Optional custom robot endpoint adapters. + registered_lowerers: Explicit lowerers for registered semantic calls. + relation_grounders: Explicit relation-target grounding providers. + handover_pose_providers: Explicit embodiment hand-over providers. + effect_monitor_registry: Optional exact-version monitor registry. + runtime_transports: Additional runtime-command-to-Gym encoders. + runner_cfg: Optional execution-runner policy. + post_policy_port: Optional environment post-policy executor. + validator_port: Optional environment segment validator. + parallel_safety_validator: Optional authoritative parallel safety gate. + + A call to :meth:`compile` snapshots only scene identities. A call to + :meth:`assemble_runtime` creates a fresh live runtime, which makes reset and + episode ownership explicit and avoids retaining providers in compiled data. + """ + + def __init__( + self, + factory: ExpertProgramEnvironmentFactory, + *, + step_dt: float, + integration_catalog: ExpertProgramIntegrationCatalog | None = None, + registration: SimulationExpertProgramRegistration | None = None, + call_catalog: SemanticCallCatalog | None = None, + endpoint_adapters: ( + Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None + ) = None, + registered_lowerers: Iterable[RegisteredSemanticLowerer] = (), + relation_grounders: Iterable[RelationTargetGrounder] = (), + handover_pose_providers: Iterable[HandOverPoseProvider] = (), + effect_monitor_registry: EffectMonitorRegistry | None = None, + runtime_transports: Iterable[RuntimeTransportActionEncoder] = (), + runner_cfg: ExecutionRunnerCfg | None = None, + post_policy_port: SegmentPostPolicyPort | None = None, + validator_port: SegmentValidatorPort | None = None, + parallel_safety_validator: ParallelCommandSafetyValidator | None = None, + ) -> None: + if not isinstance(factory, ExpertProgramEnvironmentFactory): + raise TypeError("factory must implement ExpertProgramEnvironmentFactory.") + if not isinstance(step_dt, (int, float)) or isinstance(step_dt, bool): + raise TypeError("step_dt must be a real number.") + if not math.isfinite(float(step_dt)) or float(step_dt) <= 0.0: + raise ValueError("step_dt must be finite and positive.") + scene_registry_id = _validate_identifier( + factory.scene_registry_id, + field_name="factory.scene_registry_id", + ) + robot_profile_id = _validate_identifier( + factory.robot_profile_id, + field_name="factory.robot_profile_id", + ) + if ( + integration_catalog is not None + and type(integration_catalog) is not ExpertProgramIntegrationCatalog + ): + raise TypeError( + "integration_catalog must be exactly " + "ExpertProgramIntegrationCatalog or None." + ) + if ( + registration is not None + and type(registration) is not SimulationExpertProgramRegistration + ): + raise TypeError( + "registration must be exactly " + "SimulationExpertProgramRegistration or None." + ) + registration_owner = ( + factory + if isinstance(factory, _RegistrationOwningExpertProgramFactory) + else None + ) + if registration_owner is not None: + owned_registration = registration_owner.expert_program_registration + if type(owned_registration) is not SimulationExpertProgramRegistration: + raise TypeError( + "A registration-owning factory must expose exactly " + "SimulationExpertProgramRegistration." + ) + if registration is None: + raise ValueError( + "A registration-owning factory requires its exact registration; " + "catalog-only or unregistered adapter construction is forbidden." + ) + if registration is not owned_registration: + raise ValueError( + "registration must be the exact object owned by the factory." + ) + elif registration is not None: + raise TypeError( + "registration requires a factory that exposes exact registration " + "ownership and factory-owned segment policy ports." + ) + registered_lowerer_values = tuple(registered_lowerers) + relation_grounder_values = tuple(relation_grounders) + handover_pose_provider_values = tuple(handover_pose_providers) + runtime_transport_values = tuple(runtime_transports) + if registration is not None: + if integration_catalog is not None: + raise ValueError( + "integration_catalog cannot override an exact task registration." + ) + forbidden = { + "call_catalog": call_catalog is not None, + "endpoint_adapters": endpoint_adapters is not None, + "registered_lowerers": bool(registered_lowerer_values), + "relation_grounders": bool(relation_grounder_values), + "handover_pose_providers": bool(handover_pose_provider_values), + "effect_monitor_registry": effect_monitor_registry is not None, + "runtime_transports": bool(runtime_transport_values), + "runner_cfg": runner_cfg is not None, + "post_policy_port": post_policy_port is not None, + "validator_port": validator_port is not None, + "parallel_safety_validator": parallel_safety_validator is not None, + } + supplied = tuple(name for name, present in forbidden.items() if present) + if supplied: + raise ValueError( + "Standard task registration owns all semantic and runtime " + f"extensions; external overrides are forbidden: {supplied}." + ) + registration.assert_unchanged() + integration_catalog = registration.catalog + endpoint_adapters = dict(registration.endpoint_adapter_map) + registered_lowerer_values = () + relation_grounder_values = registration.relation_grounders + handover_pose_provider_values = registration.handover_pose_providers + effect_monitor_registry = None + runtime_transport_values = registration.runtime_transports + runner_cfg = None + parallel_safety_validator = None + assert registration_owner is not None + owned_ports = registration_owner.registration_owned_segment_policy_ports() + if type(owned_ports) is not tuple or len(owned_ports) != 2: + raise TypeError( + "registration_owned_segment_policy_ports() must return an " + "exact 2-tuple." + ) + post_policy_port, validator_port = owned_ports + if integration_catalog is not None: + if integration_catalog.scene_registry_id != scene_registry_id: + raise ValueError( + "integration_catalog scene_registry_id does not match factory." + ) + if integration_catalog.robot_profile_id != robot_profile_id: + raise ValueError( + "integration_catalog robot_profile_id does not match factory." + ) + if call_catalog is not None and ( + call_catalog is not integration_catalog.call_catalog + ): + raise ValueError( + "call_catalog cannot override the task registration catalog." + ) + selected_catalog = integration_catalog.call_catalog + else: + selected_catalog = call_catalog or builtin_semantic_call_catalog() + if type(selected_catalog) is not SemanticCallCatalog: + raise TypeError("call_catalog must be exactly SemanticCallCatalog or None.") + if endpoint_adapters is not None and not isinstance(endpoint_adapters, Mapping): + raise TypeError("endpoint_adapters must be a mapping or None.") + if runner_cfg is not None and not isinstance(runner_cfg, ExecutionRunnerCfg): + raise TypeError("runner_cfg must be an ExecutionRunnerCfg or None.") + if post_policy_port is not None and not isinstance( + post_policy_port, + SegmentPostPolicyPort, + ): + raise TypeError( + "post_policy_port must implement SegmentPostPolicyPort or be None." + ) + if validator_port is not None and not isinstance( + validator_port, + SegmentValidatorPort, + ): + raise TypeError( + "validator_port must implement SegmentValidatorPort or be None." + ) + if parallel_safety_validator is not None and not isinstance( + parallel_safety_validator, + ParallelCommandSafetyValidator, + ): + raise TypeError( + "parallel_safety_validator must implement " + "ParallelCommandSafetyValidator or be None." + ) + + self._factory = factory + self._scene_registry_id = scene_registry_id + self._robot_profile_id = robot_profile_id + self._step_dt = float(step_dt) + self._registration = registration + self._integration_catalog = integration_catalog + self._call_catalog = selected_catalog + self._endpoint_adapters = ( + None if endpoint_adapters is None else dict(endpoint_adapters) + ) + self._registered_lowerers = registered_lowerer_values + self._relation_grounders = relation_grounder_values + self._handover_pose_providers = handover_pose_provider_values + self._effect_monitor_registry = effect_monitor_registry + self._runtime_transports = runtime_transport_values + self._runner_cfg = runner_cfg + self._post_policy_port = post_policy_port + self._validator_port = validator_port + self._parallel_safety_validator = parallel_safety_validator + + @property + def scene_registry_id(self) -> str: + """Return the exact scene integration ID accepted by this adapter. + + Returns: + Stable scene-registry identifier. + """ + return self._scene_registry_id + + @property + def robot_profile_id(self) -> str: + """Return the exact robot profile ID accepted by this adapter. + + Returns: + Stable robot-profile identifier. + """ + return self._robot_profile_id + + @property + def step_dt(self) -> float: + """Return the authoritative environment-step cadence. + + Returns: + Positive control step duration in seconds. + """ + return self._step_dt + + def compile(self, program: ExpertProgramCfg) -> CompiledProgram: + """Compile one program after exact integration-selection validation. + + Args: + program: Strict declarative program configuration. + + Returns: + Provider-free lazily expanded compiled program. + """ + if type(program) is not ExpertProgramCfg: + raise TypeError("program must be exactly ExpertProgramCfg.") + self._validate_selection(program.integration) + if self._integration_catalog is not None: + return self._integration_catalog.preflight(program) + registry = self._create_scene_registry() + return ExpertProgramCompiler.from_scene_registry(registry).compile(program) + + def assemble_runtime( + self, + integration: ExpertProgramIntegrationCfg, + ) -> ExpertProgramRuntimeAssembly: + """Create a fresh fully connected semantic runtime. + + Args: + integration: Exact scene, profile, and runtime-preset selection. + + Returns: + Owned assembly containing every validated runtime boundary. + """ + semantic = self._assemble_semantic_components(integration) + return self._assemble_execution_runtime(semantic) + + def _assemble_semantic_components( + self, + integration: ExpertProgramIntegrationCfg, + ) -> _ExpertProgramSemanticAssembly: + """Bind compiler dependencies without observation or evidence ports.""" + self._validate_selection(integration) + registry = self._create_scene_registry() + current_profile_id = _validate_identifier( + self._factory.robot_profile_id, + field_name="factory.robot_profile_id", + ) + if current_profile_id != self._robot_profile_id: + raise ValueError( + "Factory robot profile declaration drifted: expected " + f"{self._robot_profile_id!r}, got {current_profile_id!r}." + ) + profile = self._factory.create_robot_skill_profile() + self._validate_registration_ownership() + if type(profile) is not RobotSkillProfile: + raise TypeError( + "create_robot_skill_profile() must return exactly RobotSkillProfile." + ) + if profile.profile_id != self._robot_profile_id: + raise ValueError( + "Factory robot profile declaration drifted: expected " + f"{self._robot_profile_id!r}, got {profile.profile_id!r}." + ) + if self._registration is not None: + self._registration.validate_robot_profile( + profile, + step_dt=self._step_dt, + ) + + engine = self._factory.create_atomic_action_engine(profile) + self._validate_registration_ownership() + if not isinstance(engine, AtomicActionEngine): + raise TypeError( + "create_atomic_action_engine() must return an AtomicActionEngine." + ) + if self._registration is not None: + bound_profile = engine.skill_profile + if type(bound_profile) is not BoundRobotSkillProfile: + raise IntegrationFingerprintMismatch( + "The standard factory engine must own one exact bound robot " + "profile." + ) + if bound_profile.source_profile is not profile: + raise IntegrationFingerprintMismatch( + "The standard factory engine is bound to a different robot " + "profile object than the adapter validated." + ) + self._registration.validate_engine(engine) + + manifest = self._create_manifest( + registry, + profile, + runtime_preset=integration.runtime_preset, + ) + bound = manifest.bind( + registry, + engine, + endpoint_adapters=self._endpoint_adapters, + ) + if self._registration is not None: + self._validate_registration_ownership() + # ``manifest.bind`` resolves endpoints again and replaces the + # engine-owned bound profile. Revalidate that second live result so a + # provider cannot pass factory construction and drift before compile. + self._registration.validate_engine(engine) + compiler = SemanticSkillCompiler( + bound, + registered_lowerers=self._registered_lowerers, + relation_grounders=self._relation_grounders, + handover_pose_providers=self._handover_pose_providers, + effect_monitor_registry=self._effect_monitor_registry, + ) + + selection = ExpertProgramIntegrationCfg( + robot_profile=integration.robot_profile, + scene_registry=integration.scene_registry, + runtime_preset=integration.runtime_preset, + ) + return _ExpertProgramSemanticAssembly( + integration=selection, + scene_registry=registry, + robot_profile=profile, + manifest=manifest, + engine=engine, + compiler=compiler, + ) + + def _assemble_execution_runtime( + self, + semantic: _ExpertProgramSemanticAssembly, + ) -> ExpertProgramRuntimeAssembly: + """Attach live observation, evidence, command, and runtime boundaries.""" + if type(semantic) is not _ExpertProgramSemanticAssembly: + raise TypeError("semantic must be exactly _ExpertProgramSemanticAssembly.") + self._validate_registration_ownership() + + clock = EnvironmentStepClock(self._step_dt) + observation_provider = self._factory.create_planning_observation_provider( + scene_registry=semantic.scene_registry, + engine=semantic.engine, + clock=clock, + ) + self._validate_registration_ownership() + if not isinstance(observation_provider, PlanningObservationPort): + raise TypeError( + "create_planning_observation_provider() must return a port " + "implementing both ObservationProvider and CurrentQposProvider." + ) + providers = self._factory.create_effect_evidence_providers( + scene_registry=semantic.scene_registry, + engine=semantic.engine, + observation_provider=observation_provider, + ) + self._validate_registration_ownership() + if isinstance(providers, (str, bytes)): + raise TypeError( + "create_effect_evidence_providers() must return an iterable of " + "EffectEvidenceProvider values." + ) + try: + provider_values = tuple(providers) + except TypeError as exc: + raise TypeError( + "create_effect_evidence_providers() must return an iterable of " + "EffectEvidenceProvider values." + ) from exc + evidence_collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry(provider_values) + ) + expected_transport_ids: tuple[str, ...] | None = None + include_joint_position = True + if self._registration is not None: + expected_transport_ids = tuple( + declaration.transport_id + for declaration in ( + self._registration.catalog.runtime_transport_declarations + ) + ) + include_joint_position = ( + JointPositionGymTransportEncoder.transport_id in expected_transport_ids + ) + command_encoder = RuntimeCommandFrameEncoder( + observation_provider, + transports=self._runtime_transports, + include_joint_position=include_joint_position, + ) + if expected_transport_ids is not None: + if command_encoder.transport_ids != expected_transport_ids: + raise IntegrationFingerprintMismatch( + "Live command encoder transport order differs from the exact " + "registration catalog." + ) + command_encoder.freeze() + accepted_command_observer: AcceptedRuntimeCommandObserver | None = None + if isinstance(self._factory, AcceptedRuntimeCommandObserverFactory): + accepted_command_observer = ( + self._factory.create_accepted_runtime_command_observer( + scene_registry=semantic.scene_registry, + engine=semantic.engine, + observation_provider=observation_provider, + ) + ) + self._validate_registration_ownership() + if not isinstance( + accepted_command_observer, + AcceptedRuntimeCommandObserver, + ): + raise TypeError( + "create_accepted_runtime_command_observer() must return an " + "AcceptedRuntimeCommandObserver." + ) + command_sink = BufferedGymCommandSink( + command_encoder, + clock, + accepted_command_observer=accepted_command_observer, + ) + try: + selected_preset = semantic.robot_profile.presets[ + semantic.integration.runtime_preset + ] + except KeyError as exc: + raise ValueError( + "The selected runtime preset is absent from the assembled robot " + "profile." + ) from exc + selected_runner_cfg = selected_preset.runner_cfg + if self._registration is None and self._runner_cfg is not None: + selected_runner_cfg = deepcopy(self._runner_cfg) + runtime = SkillRuntime.from_components( + semantic.compiler, + observation_provider, + command_sink, + evidence_collector, + clock=clock, + runner_cfg=deepcopy(selected_runner_cfg), + ) + parallel_safety_validator = self._parallel_safety_validator + if ( + self._registration is not None + and self._registration.parallel_safety_factory is not None + ): + if not isinstance( + self._factory, + ParallelCommandSafetyValidatorProvider, + ): + raise TypeError( + "A registration-owned parallel_safety_factory requires the " + "environment factory to implement " + "ParallelCommandSafetyValidatorProvider." + ) + parallel_safety_validator = ( + self._factory.create_parallel_command_safety_validator( + scene_registry=semantic.scene_registry, + engine=semantic.engine, + observation_provider=observation_provider, + ) + ) + self._validate_registration_ownership() + if not isinstance( + parallel_safety_validator, + ParallelCommandSafetyValidator, + ): + raise TypeError( + "create_parallel_command_safety_validator() must return a " + "ParallelCommandSafetyValidator." + ) + return ExpertProgramRuntimeAssembly( + integration=semantic.integration, + scene_registry=semantic.scene_registry, + robot_profile=semantic.robot_profile, + manifest=semantic.manifest, + engine=semantic.engine, + compiler=semantic.compiler, + observation_provider=observation_provider, + evidence_collector=evidence_collector, + clock=clock, + command_encoder=command_encoder, + command_sink=command_sink, + accepted_command_observer=accepted_command_observer, + runner_cfg=selected_runner_cfg, + parallel_safety_validator=parallel_safety_validator, + runtime=runtime, + ) + + def create_bridge(self, program: CompiledProgram) -> AtomicDemoBridge: + """Create a fresh Gym bridge for one provider-free compiled program. + + Args: + program: Program compiled for this adapter's exact integration IDs. + + Returns: + Lazy bridge sharing one newly assembled runtime, clock, and sink. + """ + if type(program) is not CompiledProgram: + raise TypeError("program must be exactly CompiledProgram.") + materialized = program.materialize() + self._validate_selection(materialized.integration) + self._preflight_program_surfaces(materialized) + semantic = self._assemble_semantic_components(materialized.integration) + self._preflight_program(materialized, semantic.compiler) + assembly = self._assemble_execution_runtime(semantic) + return AtomicDemoBridge( + materialized, + assembly.runtime, + assembly.command_sink, + assembly.clock, + post_policy_port=self._post_policy_port, + validator_port=self._validator_port, + runner_cfg=assembly.runner_cfg, + parallel_safety_validator=assembly.parallel_safety_validator, + ) + + def _preflight_program_surfaces( + self, + program: MaterializedCompiledProgram, + ) -> None: + """Validate every segment hook without live observation or action.""" + if type(program) is not MaterializedCompiledProgram: + raise TypeError("program must be exactly MaterializedCompiledProgram.") + for segment in program.iter_segments(): + if segment.post_policies and self._post_policy_port is None: + raise DemoBridgeError( + f"Segment {segment.segment_id!r} declares post-policies, but no " + "SegmentPostPolicyPort was installed." + ) + for policy in segment.post_policies: + assert self._post_policy_port is not None + self._post_policy_port.validate_policy(policy, segment=segment) + if segment.validators and self._validator_port is None: + raise DemoBridgeError( + f"Segment {segment.segment_id!r} declares validators, but no " + "SegmentValidatorPort was installed." + ) + for validator in segment.validators: + assert self._validator_port is not None + self._validator_port.validate_validator( + validator, + segment=segment, + ) + + def _preflight_program( + self, + program: MaterializedCompiledProgram, + compiler: SemanticSkillCompiler, + ) -> None: + """Analyze every program workflow before any physical action can run. + + Sequential stretches retain cross-segment state flow and target + look-ahead. A parallel barrier cuts that flow; each branch is checked + independently through the same canonical semantic compiler used by the + runtime. This boundary materializes no observations and starts no + execution session. + """ + if type(program) is not MaterializedCompiledProgram: + raise TypeError("program must be exactly MaterializedCompiledProgram.") + if not isinstance(compiler, SemanticSkillCompiler): + raise TypeError("compiler must be a SemanticSkillCompiler.") + analyses = program.preflight_analyses() + if any(analysis.kind == "parallel_branch" for analysis in analyses) and ( + not self._parallel_safety_is_registered + ): + raise ValueError( + "Expert Programs containing parallel blocks require an explicit " + "ParallelCommandSafetyValidator before bridge creation." + ) + + index = 0 + while index < len(analyses): + analysis = analyses[index] + if analysis.kind != "parallel_branch": + compiler.analyze( + analysis.calls, + workflow_id=analysis.analysis_id, + path=analysis.source_path, + ) + index += 1 + continue + segment_index = analysis.segment_indices[0] + branches: dict[str, tuple[SemanticCallSpec, ...]] = {} + branch_paths: dict[str, tuple[str | int, ...]] = {} + while index < len(analyses): + branch = analyses[index] + if branch.kind != "parallel_branch" or branch.segment_indices != ( + segment_index, + ): + break + branch_id = f"branch_{len(branches)}" + branches[branch_id] = branch.calls + branch_paths[branch_id] = branch.source_path + index += 1 + analyze_parallel_branches( + compiler, + branches, + workflow_id=( + f"{program.program_id}:preflight:parallel:{segment_index}" + ), + branch_paths=branch_paths, + ) + + @property + def _parallel_safety_is_registered(self) -> bool: + """Whether static assembly owns an authoritative parallel safety gate.""" + if self._registration is not None: + return self._registration.parallel_safety_factory is not None + return self._parallel_safety_validator is not None + + def _validate_selection( + self, + integration: ExpertProgramIntegrationCfg, + ) -> None: + """Reject an integration selection owned by another adapter.""" + if type(integration) is not ExpertProgramIntegrationCfg: + raise TypeError("integration must be exactly ExpertProgramIntegrationCfg.") + self._validate_registration_ownership() + current_scene_id = _validate_identifier( + self._factory.scene_registry_id, + field_name="factory.scene_registry_id", + ) + current_profile_id = _validate_identifier( + self._factory.robot_profile_id, + field_name="factory.robot_profile_id", + ) + if current_scene_id != self._scene_registry_id: + raise ValueError( + "Factory scene registry declaration drifted: expected " + f"{self._scene_registry_id!r}, got {current_scene_id!r}." + ) + if current_profile_id != self._robot_profile_id: + raise ValueError( + "Factory robot profile declaration drifted: expected " + f"{self._robot_profile_id!r}, got {current_profile_id!r}." + ) + if integration.scene_registry != self._scene_registry_id: + raise ValueError( + f"Expert Program selects scene_registry " + f"{integration.scene_registry!r}, but this environment exposes " + f"only {self._scene_registry_id!r}." + ) + if integration.robot_profile != self._robot_profile_id: + raise ValueError( + f"Expert Program selects robot_profile " + f"{integration.robot_profile!r}, but this environment exposes " + f"only {self._robot_profile_id!r}." + ) + + def _validate_registration_ownership(self) -> None: + """Reject a standard factory whose exact registration owner drifted.""" + registration = self._registration + if registration is None: + return + if not isinstance(self._factory, _RegistrationOwningExpertProgramFactory): + raise IntegrationFingerprintMismatch( + "The standard environment factory no longer exposes registration " + "ownership." + ) + current = self._factory.expert_program_registration + if type(current) is not SimulationExpertProgramRegistration: + raise IntegrationFingerprintMismatch( + "The standard environment factory no longer exposes an exact " + "SimulationExpertProgramRegistration." + ) + if current is not registration: + raise IntegrationFingerprintMismatch( + "The standard environment factory registration ownership changed " + "after adapter construction." + ) + + def _create_scene_registry(self) -> SceneRegistry: + """Create and validate one exact live scene registry.""" + current_id = _validate_identifier( + self._factory.scene_registry_id, + field_name="factory.scene_registry_id", + ) + if current_id != self._scene_registry_id: + raise ValueError( + "Factory scene registry declaration drifted: expected " + f"{self._scene_registry_id!r}, got {current_id!r}." + ) + registry = self._factory.create_scene_registry() + self._validate_registration_ownership() + if type(registry) is not SceneRegistry: + raise TypeError( + "create_scene_registry() must return exactly SceneRegistry." + ) + if self._registration is not None: + self._registration.validate_scene_registry(registry) + return registry + + def _create_manifest( + self, + registry: SceneRegistry, + profile: RobotSkillProfile, + *, + runtime_preset: str, + ) -> SemanticIntegrationManifest: + """Create one static manifest from exact selected declarations.""" + return SemanticIntegrationManifest( + scene=SceneManifest.from_registry(registry), + robot_profile=profile, + call_catalog=self._call_catalog, + runtime_preset=runtime_preset, + ) + + +class ExpertProgramEnvironmentMixin: + """Delegate environment hooks to one reusable explicit adapter. + + Environment classes place this mixin before their normal environment base + and implement only :attr:`expert_program_adapter`. Motion generation and + runtime stepping remain in shared components. + """ + + @property + def expert_program_adapter(self) -> ExpertProgramEnvironmentAdapter: + """Return the environment-owned reusable adapter. + + Returns: + Exact shared Expert Program environment adapter. + """ + raise NotImplementedError( + "Expert Program environments must expose expert_program_adapter." + ) + + def compile_expert_program( + self, + program: ExpertProgramCfg, + ) -> CompiledProgram: + """Delegate provider-free compilation to the explicit adapter. + + Args: + program: Strict declarative program configuration. + + Returns: + Provider-free compiled program. + """ + return self._checked_expert_program_adapter().compile(program) + + def create_expert_program_bridge( + self, + program: CompiledProgram, + ) -> AtomicDemoBridge: + """Delegate live runtime and Gym bridge assembly to the adapter. + + Args: + program: Provider-free compiled program. + + Returns: + Fresh lazy Gym bridge. + """ + return self._checked_expert_program_adapter().create_bridge(program) + + def _checked_expert_program_adapter(self) -> ExpertProgramEnvironmentAdapter: + """Return the exact adapter or fail before any provider is touched.""" + adapter = self.expert_program_adapter + if type(adapter) is not ExpertProgramEnvironmentAdapter: + raise TypeError( + "expert_program_adapter must be exactly " + "ExpertProgramEnvironmentAdapter." + ) + return adapter + + +__all__ = [ + "AcceptedRuntimeCommandObserverFactory", + "ExpertProgramEnvironmentAdapter", + "ExpertProgramEnvironmentFactory", + "ExpertProgramEnvironmentMixin", + "ExpertProgramRuntimeAssembly", + "PlanningObservationPort", + "SkillRuntimeAssemblyPort", +] diff --git a/embodichain/lab/gym/envs/expert_program/extensions.py b/embodichain/lab/gym/envs/expert_program/extensions.py new file mode 100644 index 000000000..f5b019d8b --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/extensions.py @@ -0,0 +1,914 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Typed standard-runtime extension declarations for Expert Programs. + +The values in this module deliberately describe extension wiring without +creating a simulator or resolving one live robot endpoint. A task +registration owns the corresponding adapter, transport, and safety-factory +instances, while its provider-free catalog owns the exact declarations below. +""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import dataclass, fields, is_dataclass +from enum import Enum +from types import MappingProxyType +from typing import ClassVar, Protocol, runtime_checkable, TYPE_CHECKING + +import torch + +from embodichain.lab.sim.atomic_actions.bindings import ( + JointPositionTarget, + RuntimeEndpointTarget, +) +from embodichain.lab.sim.atomic_actions.runtime_commands import RuntimeCommandPayload +from embodichain.lab.sim.skills.effects import ( + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, +) +from embodichain.lab.sim.skills.parallel_runtime import ( + ParallelCommandSafetyValidator, +) +from embodichain.lab.sim.skills.profiles import ( + ControlPartEndpoint, + ControlPartEndpointAdapter, + ResourceEndpoint, + ResourceEndpointAdapter, + RobotSkillProfile, +) + +from .bridge import ( + JointPositionGymTransportEncoder, + RuntimeTransportActionEncoder, +) + +if TYPE_CHECKING: + from embodichain.lab.sim.atomic_actions import AtomicActionEngine + from embodichain.lab.sim.skills import SceneRegistry + +VersionedKey = tuple[str, str] +"""Exact ``(provider_or_projector_id, revision)`` registry key.""" + +_BUILTIN_TRACKING_FEEDBACK_SOURCE_KEYS = frozenset({("planning_context.robot", "1")}) +_BUILTIN_TRACKING_PROJECTOR_KEYS = frozenset({("joint_position_payload", "1")}) +_BUILTIN_EFFECT_EVIDENCE_SOURCE_KEYS = frozenset( + { + ( + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + ) + } +) + + +def _identifier(value: object, *, field_name: str) -> str: + """Validate one exact, non-empty registration identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +def _qualified_name(value: type[object] | object) -> str: + """Return one deterministic diagnostic name.""" + value_type = value if isinstance(value, type) else type(value) + return f"{value_type.__module__}.{value_type.__qualname__}" + + +def _class_attribute(value: object, name: str, *, field_name: str) -> object: + """Read registration metadata from the provider type, never instance state.""" + owner = type(value) + if not hasattr(owner, name): + raise TypeError(f"{field_name} must be declared on {owner.__name__}.") + return getattr(owner, name) + + +def _versioned_keys(value: object, *, field_name: str) -> frozenset[VersionedKey]: + """Validate one exact immutable set of versioned registry keys.""" + if type(value) is not frozenset: + raise TypeError(f"{field_name} must be an exact frozenset.") + normalized: set[VersionedKey] = set() + for key in value: + if type(key) is not tuple or len(key) != 2: + raise TypeError(f"{field_name} must contain exact 2-tuples.") + identifier, revision = key + normalized.add( + ( + _identifier(identifier, field_name=f"{field_name} IDs"), + _identifier(revision, field_name=f"{field_name} revisions"), + ) + ) + return frozenset(normalized) + + +def _identifier_set(value: object, *, field_name: str) -> frozenset[str]: + """Validate one exact immutable set of identifiers.""" + if type(value) is not frozenset: + raise TypeError(f"{field_name} must be an exact frozenset.") + return frozenset(_identifier(item, field_name=field_name) for item in value) + + +def _type_tuple( + value: object, + *, + base_type: type[object], + field_name: str, +) -> tuple[type[object], ...]: + """Validate one non-empty exact tuple of unique exact value types.""" + if type(value) is not tuple or not value: + raise TypeError(f"{field_name} must be a non-empty exact tuple.") + normalized: list[type[object]] = [] + for item in value: + if not isinstance(item, type) or not issubclass(item, base_type): + raise TypeError( + f"{field_name} values must be {base_type.__name__} subclasses." + ) + normalized.append(item) + if len(set(normalized)) != len(normalized): + raise ValueError(f"{field_name} must not contain duplicate exact types.") + return tuple(normalized) + + +def validate_immutable_extension_declaration( + value: object, + *, + field_name: str, +) -> None: + """Accept only a deeply immutable frozen dataclass or stateless instance. + + Frozen dataclass fields may contain only immutable scalar values, types, + enums with immutable values, exact tuples, exact frozensets, and recursively + frozen dataclasses. + Mutable leaves such as mappings, lists, sets, bytearrays, and tensors are + rejected because registration-owned live extensions are shared with an + assembled runtime. A non-dataclass extension must not have instance or + slot state at all. + """ + if isinstance(value, type): + raise TypeError(f"{field_name} must contain instances, not types.") + + def validate_state( + declaration: object, + *, + path: str, + ) -> tuple[bool, tuple[str, ...]]: + """Validate declared state and return dataclass field names.""" + dataclass_declaration = is_dataclass(declaration) + dataclass_field_names: set[str] = set() + if dataclass_declaration: + params = getattr(type(declaration), "__dataclass_params__", None) + if params is None or not params.frozen: + raise TypeError( + f"{path} stateful declarations must be frozen dataclasses." + ) + dataclass_field_names.update(item.name for item in fields(declaration)) + + state_names: set[str] = set() + instance_state = getattr(declaration, "__dict__", None) + if isinstance(instance_state, Mapping): + state_names.update(instance_state) + for owner in type(declaration).__mro__: + declared_slots = getattr(owner, "__slots__", ()) + slots = ( + (declared_slots,) if isinstance(declared_slots, str) else declared_slots + ) + for slot_name in slots: + if slot_name in {"__dict__", "__weakref__"}: + continue + storage_name = ( + f"_{owner.__name__.lstrip('_')}{slot_name}" + if slot_name.startswith("__") and not slot_name.endswith("__") + else slot_name + ) + if hasattr(declaration, storage_name): + state_names.add(storage_name) + undeclared_state = ( + state_names.difference(dataclass_field_names) + if dataclass_declaration + else state_names + ) + if undeclared_state: + raise TypeError( + f"{path} contains unfingerprinted state " + f"{sorted(undeclared_state)}; Use a frozen dataclass with every " + "configuration field declared, or a stateless instance." + ) + return dataclass_declaration, tuple(sorted(dataclass_field_names)) + + def validate_nested( + nested: object, + *, + path: str, + active: set[int], + ) -> None: + """Reject every mutable or opaque leaf in one declaration graph.""" + if nested is None or type(nested) in {bool, int, float, str}: + return + if isinstance(nested, type): + return + if isinstance(nested, Enum): + validate_nested( + nested.value, + path=f"{path}.value", + active=active, + ) + return + if isinstance(nested, torch.Tensor) or type(nested) in { + list, + dict, + set, + bytearray, + }: + raise TypeError( + f"{path} must be deeply immutable; mutable value type " + f"{_qualified_name(nested)!r} is forbidden." + ) + if isinstance(nested, Mapping): + raise TypeError( + f"{path} must be deeply immutable; mapping values are forbidden." + ) + + nested_id = id(nested) + if nested_id in active: + raise TypeError(f"{path} must not contain a cyclic declaration graph.") + if type(nested) in {tuple, frozenset}: + active.add(nested_id) + try: + for index, item in enumerate(nested): + validate_nested( + item, + path=f"{path}[{index}]", + active=active, + ) + finally: + active.remove(nested_id) + return + if is_dataclass(nested) and not isinstance(nested, type): + active.add(nested_id) + try: + _, nested_field_names = validate_state(nested, path=path) + for nested_field_name in nested_field_names: + validate_nested( + getattr(nested, nested_field_name), + path=f"{path}.{nested_field_name}", + active=active, + ) + finally: + active.remove(nested_id) + return + raise TypeError( + f"{path} contains unsupported value type " + f"{_qualified_name(nested)!r}; extension declarations must be " + "complete deeply immutable data." + ) + + dataclass_declaration, dataclass_field_names = validate_state( + value, + path=field_name, + ) + if dataclass_declaration: + for dataclass_field_name in dataclass_field_names: + validate_nested( + getattr(value, dataclass_field_name), + path=f"{field_name}.{dataclass_field_name}", + active={id(value)}, + ) + + +@dataclass(frozen=True, slots=True) +class EndpointAdapterDeclaration: + """Provider-free declaration of one exact endpoint adapter.""" + + endpoint_type: type[ResourceEndpoint] + adapter_type: type[ResourceEndpointAdapter] + adapter_id: str + runtime_transport_ids: frozenset[str] + runtime_target_types: tuple[type[RuntimeEndpointTarget], ...] + tracking_feedback_source_keys: frozenset[VersionedKey] + tracking_projector_keys: frozenset[VersionedKey] + effect_evidence_source_keys: frozenset[VersionedKey] + + def __post_init__(self) -> None: + if not isinstance(self.endpoint_type, type) or not issubclass( + self.endpoint_type, ResourceEndpoint + ): + raise TypeError("endpoint_type must be a ResourceEndpoint subclass.") + if not isinstance(self.adapter_type, type) or not issubclass( + self.adapter_type, ResourceEndpointAdapter + ): + raise TypeError("adapter_type must be a ResourceEndpointAdapter subclass.") + _identifier(self.adapter_id, field_name="adapter_id") + object.__setattr__( + self, + "runtime_transport_ids", + _identifier_set( + self.runtime_transport_ids, + field_name="runtime_transport_ids", + ), + ) + if not self.runtime_transport_ids: + raise ValueError("runtime_transport_ids must not be empty.") + object.__setattr__( + self, + "runtime_target_types", + _type_tuple( + self.runtime_target_types, + base_type=RuntimeEndpointTarget, + field_name="runtime_target_types", + ), + ) + object.__setattr__( + self, + "tracking_feedback_source_keys", + _versioned_keys( + self.tracking_feedback_source_keys, + field_name="tracking_feedback_source_keys", + ), + ) + object.__setattr__( + self, + "tracking_projector_keys", + _versioned_keys( + self.tracking_projector_keys, + field_name="tracking_projector_keys", + ), + ) + object.__setattr__( + self, + "effect_evidence_source_keys", + _versioned_keys( + self.effect_evidence_source_keys, + field_name="effect_evidence_source_keys", + ), + ) + + +@dataclass(frozen=True, slots=True) +class RuntimeTransportDeclaration: + """Provider-free declaration of one ordered runtime transport encoder.""" + + transport_type: type[RuntimeTransportActionEncoder] + transport_id: str + target_types: tuple[type[RuntimeEndpointTarget], ...] + payload_types: tuple[type[RuntimeCommandPayload], ...] + + def __post_init__(self) -> None: + if not isinstance(self.transport_type, type): + raise TypeError("transport_type must be a type.") + _identifier(self.transport_id, field_name="transport_id") + object.__setattr__( + self, + "target_types", + _type_tuple( + self.target_types, + base_type=RuntimeEndpointTarget, + field_name="target_types", + ), + ) + object.__setattr__( + self, + "payload_types", + _type_tuple( + self.payload_types, + base_type=RuntimeCommandPayload, + field_name="payload_types", + ), + ) + for field_name, declared_types in ( + ("target_types", self.target_types), + ("payload_types", self.payload_types), + ): + for declared_type in declared_types: + try: + type_transport_id = declared_type.__dict__["TRANSPORT_ID"] + except KeyError as exc: + raise TypeError( + f"{field_name} value {declared_type.__name__} must declare " + "an exact ClassVar TRANSPORT_ID on that type; inherited or " + "instance-only transport IDs are forbidden." + ) from exc + _identifier( + type_transport_id, + field_name=f"{declared_type.__name__}.TRANSPORT_ID", + ) + if type_transport_id != self.transport_id: + raise ValueError( + f"{field_name} value {declared_type.__name__} declares " + f"transport {type_transport_id!r}, not " + f"{self.transport_id!r}." + ) + + +@dataclass(frozen=True, slots=True) +class ParallelSafetyDeclaration: + """Provider-free identity and transport coverage of one safety factory.""" + + factory_type: type[object] + validator_id: str + revision: str + supported_transport_ids: frozenset[str] + + def __post_init__(self) -> None: + if not isinstance(self.factory_type, type): + raise TypeError("factory_type must be a type.") + _identifier(self.validator_id, field_name="validator_id") + _identifier(self.revision, field_name="revision") + object.__setattr__( + self, + "supported_transport_ids", + _identifier_set( + self.supported_transport_ids, + field_name="supported_transport_ids", + ), + ) + if not self.supported_transport_ids: + raise ValueError("supported_transport_ids must not be empty.") + + +@runtime_checkable +class ParallelCommandSafetyValidatorFactory(Protocol): + """Registration-owned factory for one authoritative live safety gate.""" + + validator_id: ClassVar[str] + revision: ClassVar[str] + supported_transport_ids: ClassVar[frozenset[str]] + + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + ) -> ParallelCommandSafetyValidator: + """Create one live gate bound to the exact assembled runtime.""" + + +@dataclass(frozen=True, slots=True) +class StandardExtensionDeclarations: + """Cross-checked provider-free declarations for the standard factory.""" + + endpoint_adapters: Mapping[type[ResourceEndpoint], EndpointAdapterDeclaration] + runtime_transports: tuple[RuntimeTransportDeclaration, ...] + parallel_safety: ParallelSafetyDeclaration | None + + def __post_init__(self) -> None: + if not isinstance(self.endpoint_adapters, Mapping): + raise TypeError("endpoint_adapters must be a mapping.") + normalized: dict[type[ResourceEndpoint], EndpointAdapterDeclaration] = {} + for endpoint_type, declaration in self.endpoint_adapters.items(): + if type(declaration) is not EndpointAdapterDeclaration: + raise TypeError( + "endpoint_adapters values must be EndpointAdapterDeclaration " + "values." + ) + if endpoint_type is not declaration.endpoint_type: + raise ValueError( + "endpoint_adapters keys must exactly match declaration " + "endpoint_type values." + ) + normalized[endpoint_type] = declaration + object.__setattr__(self, "endpoint_adapters", MappingProxyType(normalized)) + transports = tuple(self.runtime_transports) + if not transports or not all( + type(value) is RuntimeTransportDeclaration for value in transports + ): + raise TypeError( + "runtime_transports must contain RuntimeTransportDeclaration values." + ) + object.__setattr__(self, "runtime_transports", transports) + if ( + self.parallel_safety is not None + and type(self.parallel_safety) is not ParallelSafetyDeclaration + ): + raise TypeError( + "parallel_safety must be ParallelSafetyDeclaration or None." + ) + adapter_ids = [value.adapter_id for value in normalized.values()] + if len(set(adapter_ids)) != len(adapter_ids): + raise ValueError("Endpoint adapter IDs must be unique.") + transport_ids = [value.transport_id for value in transports] + if len(set(transport_ids)) != len(transport_ids): + raise ValueError("Runtime transport IDs must be unique.") + transport_by_id = {value.transport_id: value for value in transports} + required_transport_ids = frozenset( + transport_id + for declaration in normalized.values() + for transport_id in declaration.runtime_transport_ids + ) + if required_transport_ids != frozenset(transport_by_id): + raise ValueError( + "Provider-free runtime transports must exactly cover endpoint " + f"adapter transport IDs; expected {sorted(required_transport_ids)}, " + f"got {sorted(transport_by_id)}." + ) + target_owners: dict[type[RuntimeEndpointTarget], str] = {} + for transport in transports: + for target_type in transport.target_types: + if target_type in target_owners: + raise ValueError( + f"Runtime target type {_qualified_name(target_type)!r} has " + "multiple transport owners." + ) + target_owners[target_type] = transport.transport_id + declared_target_types: set[type[RuntimeEndpointTarget]] = set() + for adapter in normalized.values(): + counts = {transport_id: 0 for transport_id in adapter.runtime_transport_ids} + for target_type in adapter.runtime_target_types: + owner = target_owners.get(target_type) + if owner is None or owner not in counts: + raise ValueError( + f"Endpoint adapter {adapter.adapter_id!r} target type " + f"{_qualified_name(target_type)!r} has no matching " + "declared transport." + ) + counts[owner] += 1 + declared_target_types.add(target_type) + unused = sorted(key for key, count in counts.items() if count == 0) + if unused: + raise ValueError( + f"Endpoint adapter {adapter.adapter_id!r} declares unused " + f"transport IDs {unused}." + ) + if declared_target_types != set(target_owners): + raise ValueError( + "Provider-free runtime target types must be covered exactly by " + "endpoint adapter declarations." + ) + _validate_builtin_routes(normalized) + if self.parallel_safety is not None and ( + self.parallel_safety.supported_transport_ids != frozenset(transport_by_id) + ): + raise ValueError( + "Parallel safety transport coverage must exactly match the " + "provider-free runtime transports." + ) + + +def declare_endpoint_adapter( + adapter: ResourceEndpointAdapter, +) -> EndpointAdapterDeclaration: + """Read one endpoint adapter's exact static extension contract.""" + if not isinstance(adapter, ResourceEndpointAdapter): + raise TypeError("endpoint adapters must be ResourceEndpointAdapter instances.") + validate_immutable_extension_declaration( + adapter, + field_name="endpoint_adapters", + ) + endpoint_type = _class_attribute( + adapter, + "endpoint_type", + field_name="ResourceEndpointAdapter.endpoint_type", + ) + if not isinstance(endpoint_type, type) or not issubclass( + endpoint_type, ResourceEndpoint + ): + raise TypeError( + "ResourceEndpointAdapter.endpoint_type must be a ResourceEndpoint " + "subclass." + ) + return EndpointAdapterDeclaration( + endpoint_type=endpoint_type, + adapter_type=type(adapter), + adapter_id=_identifier( + _class_attribute( + adapter, + "adapter_id", + field_name="ResourceEndpointAdapter.adapter_id", + ), + field_name="ResourceEndpointAdapter.adapter_id", + ), + runtime_transport_ids=_class_attribute( + adapter, + "runtime_transport_ids", + field_name="ResourceEndpointAdapter.runtime_transport_ids", + ), + runtime_target_types=_class_attribute( + adapter, + "runtime_target_types", + field_name="ResourceEndpointAdapter.runtime_target_types", + ), + tracking_feedback_source_keys=_class_attribute( + adapter, + "tracking_feedback_source_keys", + field_name="ResourceEndpointAdapter.tracking_feedback_source_keys", + ), + tracking_projector_keys=_class_attribute( + adapter, + "tracking_projector_keys", + field_name="ResourceEndpointAdapter.tracking_projector_keys", + ), + effect_evidence_source_keys=_class_attribute( + adapter, + "effect_evidence_source_keys", + field_name="ResourceEndpointAdapter.effect_evidence_source_keys", + ), + ) + + +def declare_runtime_transport( + transport: RuntimeTransportActionEncoder, +) -> RuntimeTransportDeclaration: + """Read one runtime encoder's exact static target/payload contract.""" + if not isinstance(transport, RuntimeTransportActionEncoder): + raise TypeError( + "runtime_transports must implement RuntimeTransportActionEncoder." + ) + validate_immutable_extension_declaration( + transport, + field_name="runtime_transports", + ) + return RuntimeTransportDeclaration( + transport_type=type(transport), + transport_id=_identifier( + _class_attribute( + transport, + "transport_id", + field_name="RuntimeTransportActionEncoder.transport_id", + ), + field_name="RuntimeTransportActionEncoder.transport_id", + ), + target_types=_class_attribute( + transport, + "target_types", + field_name="RuntimeTransportActionEncoder.target_types", + ), + payload_types=_class_attribute( + transport, + "payload_types", + field_name="RuntimeTransportActionEncoder.payload_types", + ), + ) + + +def declare_parallel_safety_factory( + factory: ParallelCommandSafetyValidatorFactory, +) -> ParallelSafetyDeclaration: + """Read one safety factory's exact static identity and coverage.""" + create = getattr(factory, "create", None) + if not callable(create): + raise TypeError("parallel_safety_factory must define create().") + validate_immutable_extension_declaration( + factory, + field_name="parallel_safety_factory", + ) + return ParallelSafetyDeclaration( + factory_type=type(factory), + validator_id=_identifier( + _class_attribute( + factory, + "validator_id", + field_name="ParallelCommandSafetyValidatorFactory.validator_id", + ), + field_name="ParallelCommandSafetyValidatorFactory.validator_id", + ), + revision=_identifier( + _class_attribute( + factory, + "revision", + field_name="ParallelCommandSafetyValidatorFactory.revision", + ), + field_name="ParallelCommandSafetyValidatorFactory.revision", + ), + supported_transport_ids=_class_attribute( + factory, + "supported_transport_ids", + field_name=( + "ParallelCommandSafetyValidatorFactory.supported_transport_ids" + ), + ), + ) + + +def _profile_endpoint_types( + profile: RobotSkillProfile, +) -> frozenset[type[ResourceEndpoint]]: + """Return every exact endpoint declaration type used by one profile.""" + if type(profile) is not RobotSkillProfile: + raise TypeError("profile must be exactly RobotSkillProfile.") + return frozenset( + type(endpoint) + for resource in profile.resources.values() + for endpoint in resource.endpoints.values() + ) + + +def _validate_builtin_routes( + declarations: Mapping[type[ResourceEndpoint], EndpointAdapterDeclaration], +) -> None: + """Keep C1 custom endpoints open-loop and preserve exact built-in routes.""" + for endpoint_type, declaration in declarations.items(): + if endpoint_type is ControlPartEndpoint: + if ( + declaration.tracking_feedback_source_keys + != _BUILTIN_TRACKING_FEEDBACK_SOURCE_KEYS + or declaration.tracking_projector_keys + != _BUILTIN_TRACKING_PROJECTOR_KEYS + or declaration.effect_evidence_source_keys + != _BUILTIN_EFFECT_EVIDENCE_SOURCE_KEYS + ): + raise ValueError( + "The built-in ControlPartEndpoint adapter must retain its " + "exact tracking and effect-evidence routes." + ) + continue + if ( + declaration.tracking_feedback_source_keys + or declaration.tracking_projector_keys + or declaration.effect_evidence_source_keys + ): + raise ValueError( + f"Custom endpoint adapter {declaration.adapter_id!r} must declare " + "empty tracking and effect-evidence routes; the C1 standard " + "simulation factory does not install custom closed-loop providers." + ) + + +def build_standard_extension_declarations( + *, + profile: RobotSkillProfile, + endpoint_adapters: tuple[ResourceEndpointAdapter, ...], + runtime_transports: tuple[RuntimeTransportActionEncoder, ...], + parallel_safety_factory: ParallelCommandSafetyValidatorFactory | None, +) -> StandardExtensionDeclarations: + """Cross-check standard-runtime extensions against one exact profile. + + The built-in control-part adapter and joint-position transport cannot be + overridden. They are installed first only when the profile uses a + :class:`ControlPartEndpoint`; a pure-custom profile contains only its custom + declarations. Custom adapters and transports must cover exactly the endpoint + types and transport IDs used by the registered profile; unused declarations + fail closed. + """ + if type(endpoint_adapters) is not tuple: + raise TypeError("endpoint_adapters must be an exact tuple.") + if type(runtime_transports) is not tuple: + raise TypeError("runtime_transports must be an exact tuple.") + + builtin_adapter = declare_endpoint_adapter(ControlPartEndpointAdapter()) + custom_adapters = tuple( + declare_endpoint_adapter(adapter) for adapter in endpoint_adapters + ) + adapter_declarations = (builtin_adapter, *custom_adapters) + endpoint_types = [value.endpoint_type for value in adapter_declarations] + adapter_ids = [value.adapter_id for value in adapter_declarations] + if len(set(endpoint_types)) != len(endpoint_types): + raise ValueError( + "Endpoint adapter declarations contain a duplicate exact endpoint " + "type or attempt to override the built-in ControlPartEndpoint." + ) + if len(set(adapter_ids)) != len(adapter_ids): + raise ValueError( + "Endpoint adapter declarations contain a duplicate adapter ID or " + "attempt to override a built-in adapter." + ) + installed_by_type = { + declaration.endpoint_type: declaration for declaration in adapter_declarations + } + used_endpoint_types = _profile_endpoint_types(profile) + missing_adapters = used_endpoint_types - set(installed_by_type) + unused_adapters = set(installed_by_type) - used_endpoint_types + unused_adapters.discard(ControlPartEndpoint) + if missing_adapters or unused_adapters: + raise ValueError( + "Endpoint adapter coverage must exactly match profile endpoint types; " + f"missing={sorted(_qualified_name(value) for value in missing_adapters)}, " + f"unused={sorted(_qualified_name(value) for value in unused_adapters)}." + ) + if ControlPartEndpoint not in used_endpoint_types: + installed_by_type.pop(ControlPartEndpoint) + + _validate_builtin_routes(installed_by_type) + + builtin_transport = declare_runtime_transport(JointPositionGymTransportEncoder()) + custom_transports = tuple( + declare_runtime_transport(transport) for transport in runtime_transports + ) + transport_declarations = (builtin_transport, *custom_transports) + transport_ids = [value.transport_id for value in transport_declarations] + if len(set(transport_ids)) != len(transport_ids): + raise ValueError( + "Runtime transport declarations contain a duplicate transport ID or " + "attempt to override the built-in joint-position transport." + ) + transport_by_id = { + declaration.transport_id: declaration for declaration in transport_declarations + } + required_transport_ids = frozenset( + transport_id + for declaration in installed_by_type.values() + for transport_id in declaration.runtime_transport_ids + ) + missing_transports = required_transport_ids - set(transport_by_id) + unused_transports = set(transport_by_id) - required_transport_ids + unused_transports.discard(JointPositionTarget.TRANSPORT_ID) + if missing_transports or unused_transports: + raise ValueError( + "Runtime transport coverage must exactly match endpoint adapters; " + f"missing={sorted(missing_transports)}, " + f"unused={sorted(unused_transports)}." + ) + if JointPositionTarget.TRANSPORT_ID not in required_transport_ids: + transport_declarations = custom_transports + transport_by_id.pop(JointPositionTarget.TRANSPORT_ID) + + target_owners: dict[type[RuntimeEndpointTarget], str] = {} + for transport in transport_declarations: + for target_type in transport.target_types: + previous = target_owners.get(target_type) + if previous is not None: + raise ValueError( + f"Runtime target type {_qualified_name(target_type)!r} is " + f"declared by both transports {previous!r} and " + f"{transport.transport_id!r}." + ) + target_owners[target_type] = transport.transport_id + + adapter_target_types: set[type[RuntimeEndpointTarget]] = set() + for adapter in installed_by_type.values(): + for transport_id in adapter.runtime_transport_ids: + if transport_id not in transport_by_id: + raise ValueError( + f"Endpoint adapter {adapter.adapter_id!r} requires missing " + f"transport {transport_id!r}." + ) + per_transport_counts = { + transport_id: 0 for transport_id in adapter.runtime_transport_ids + } + for target_type in adapter.runtime_target_types: + owner = target_owners.get(target_type) + if owner is None or owner not in adapter.runtime_transport_ids: + raise ValueError( + f"Endpoint adapter {adapter.adapter_id!r} target type " + f"{_qualified_name(target_type)!r} is not covered by one of " + f"its transports {sorted(adapter.runtime_transport_ids)}." + ) + per_transport_counts[owner] += 1 + adapter_target_types.add(target_type) + unused_adapter_transport_ids = sorted( + transport_id + for transport_id, count in per_transport_counts.items() + if count == 0 + ) + if unused_adapter_transport_ids: + raise ValueError( + f"Endpoint adapter {adapter.adapter_id!r} declares unused " + f"transport IDs {unused_adapter_transport_ids}." + ) + extra_transport_target_types = set(target_owners) - adapter_target_types + if extra_transport_target_types: + raise ValueError( + "Runtime transports declare target types unused by endpoint adapters: " + f"{sorted(_qualified_name(value) for value in extra_transport_target_types)}." + ) + + parallel_safety = ( + None + if parallel_safety_factory is None + else declare_parallel_safety_factory(parallel_safety_factory) + ) + if parallel_safety is not None: + installed_transport_ids = frozenset(transport_by_id) + if parallel_safety.supported_transport_ids != installed_transport_ids: + raise ValueError( + "parallel_safety_factory must support exactly the registered " + f"runtime transports; expected {sorted(installed_transport_ids)}, " + f"got {sorted(parallel_safety.supported_transport_ids)}." + ) + + return StandardExtensionDeclarations( + endpoint_adapters=installed_by_type, + runtime_transports=transport_declarations, + parallel_safety=parallel_safety, + ) + + +__all__ = [ + "EndpointAdapterDeclaration", + "ParallelCommandSafetyValidatorFactory", + "ParallelSafetyDeclaration", + "RuntimeTransportDeclaration", + "StandardExtensionDeclarations", + "VersionedKey", + "build_standard_extension_declarations", + "declare_endpoint_adapter", + "declare_parallel_safety_factory", + "declare_runtime_transport", + "validate_immutable_extension_declaration", +] diff --git a/embodichain/lab/gym/envs/expert_program/loader.py b/embodichain/lab/gym/envs/expert_program/loader.py new file mode 100644 index 000000000..f47e60940 --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/loader.py @@ -0,0 +1,337 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Safe file and strict JSON loading for declarative Expert Programs.""" + +from __future__ import annotations + +import json +import math +import os +from pathlib import Path + +import yaml + +from .cfg import ExpertProgramCfg +from .decoder import ( + ExpertProgramDecodeError, + ExpertProgramValidationContext, + decode_expert_program, +) + +__all__ = [ + "MAX_EXPERT_PROGRAM_BYTES", + "load_expert_program", + "loads_expert_program_json", + "parse_expert_program_json", +] + +MAX_EXPERT_PROGRAM_BYTES = 4 * 1024 * 1024 +"""Maximum serialized Expert Program size accepted by the file loader.""" + + +class _StrictJsonValueError(ValueError): + """Carry one stable strict-JSON failure into the public decode boundary.""" + + def __init__(self, code: str, message: str) -> None: + self.code = code + self.message = message + super().__init__(message) + + +def _reject_duplicate_json_keys( + pairs: list[tuple[str, object]], +) -> dict[str, object]: + """Build a JSON mapping while rejecting ambiguous duplicate keys.""" + mapping: dict[str, object] = {} + for key, value in pairs: + if key in mapping: + raise _StrictJsonValueError( + "duplicate_json_key", + f"Duplicate JSON key {key!r}.", + ) + mapping[key] = value + return mapping + + +def _reject_non_finite_json_constant(token: str) -> object: + """Reject the non-standard NaN and Infinity JSON constants.""" + raise _StrictJsonValueError( + "non_finite_number", + f"Non-finite JSON number {token!r} is forbidden.", + ) + + +def _parse_finite_json_float(token: str) -> float: + """Parse one JSON float while rejecting overflow to infinity.""" + value = float(token) + if not math.isfinite(value): + raise _StrictJsonValueError( + "non_finite_number", + f"JSON number {token!r} is not finite.", + ) + return value + + +def _validate_decoded_json_unicode(value: object) -> None: + """Reject decoded JSON strings that cannot be represented as UTF-8.""" + if type(value) is str: + try: + value.encode("utf-8") + except UnicodeEncodeError as error: + raise _StrictJsonValueError( + "invalid_utf8", + "Expert Program JSON contains an unpaired Unicode surrogate.", + ) from error + return + if type(value) is list: + for item in value: + _validate_decoded_json_unicode(item) + return + if type(value) is dict: + for key, item in value.items(): + _validate_decoded_json_unicode(key) + _validate_decoded_json_unicode(item) + + +def _loads_strict_json_value( + text: str, + *, + max_bytes: int = MAX_EXPERT_PROGRAM_BYTES, +) -> object: + """Parse one bounded JSON document into exact JSON-compatible values.""" + + if type(text) is not str: + raise TypeError("text must be exactly str.") + if type(max_bytes) is not int: + raise TypeError("max_bytes must be exactly int.") + if max_bytes <= 0: + raise ValueError("max_bytes must be positive.") + try: + payload = text.encode("utf-8") + except UnicodeEncodeError as error: + raise ExpertProgramDecodeError( + "invalid_utf8", + (), + "Expert Program JSON must be valid UTF-8 text.", + ) from error + if len(payload) > max_bytes: + raise ExpertProgramDecodeError( + "input_too_large", + (), + f"Expert Program JSON exceeds the {max_bytes}-byte input limit.", + ) + try: + value = json.loads( + text, + object_pairs_hook=_reject_duplicate_json_keys, + parse_constant=_reject_non_finite_json_constant, + parse_float=_parse_finite_json_float, + ) + _validate_decoded_json_unicode(value) + return value + except _StrictJsonValueError as error: + raise ExpertProgramDecodeError(error.code, (), error.message) from error + except json.JSONDecodeError as error: + raise ExpertProgramDecodeError( + "invalid_json", + (), + "Invalid Expert Program JSON at " + f"line {error.lineno}, column {error.colno}.", + ) from error + except RecursionError as error: + raise ExpertProgramDecodeError( + "input_too_deep", + (), + "Expert Program JSON exceeds the parser nesting limit.", + ) from error + except ValueError as error: + raise ExpertProgramDecodeError( + "invalid_json", + (), + "Expert Program JSON contains an invalid numeric value.", + ) from error + + +def parse_expert_program_json( + text: str, + *, + max_bytes: int = MAX_EXPERT_PROGRAM_BYTES, +) -> dict[str, object]: + """Parse one bounded Expert Program JSON object without decoding its schema. + + This parse-only boundary lets a host-controlled frontend inspect or inject + fields before calling :func:`decode_expert_program`. It rejects duplicate + keys, non-finite numbers, trailing content, invalid Unicode, excessive + nesting, oversized UTF-8 input, and non-object top-level values. It does + not validate the Expert Program schema. + + Args: + text: Untrusted JSON document text. + max_bytes: Maximum accepted UTF-8 encoded input size. + + Returns: + Exact JSON object mapping ready for explicit schema decoding. + + Raises: + TypeError: If ``text`` or ``max_bytes`` has the wrong exact type. + ValueError: If ``max_bytes`` is not positive. + ExpertProgramDecodeError: If strict JSON parsing fails. + """ + value = _loads_strict_json_value(text, max_bytes=max_bytes) + if type(value) is not dict: + raise ExpertProgramDecodeError( + "expected_mapping", + (), + "Expected an object mapping.", + ) + return value + + +def loads_expert_program_json( + text: str, + *, + validation_context: ExpertProgramValidationContext | None = None, + max_bytes: int = MAX_EXPERT_PROGRAM_BYTES, +) -> ExpertProgramCfg: + """Strictly parse and decode one untrusted Expert Program JSON document. + + The input must be one plain JSON document. Markdown fences, trailing text, + multiple documents, duplicate keys, non-finite numbers, and oversized input + are rejected before the existing Expert Program decoder is called. + + Args: + text: Untrusted JSON response text. + validation_context: Optional provider-free static reference validator. + max_bytes: Maximum UTF-8 encoded response size. + + Returns: + Fully owned and internally validated Expert Program configuration. + + Raises: + TypeError: If ``text`` or ``max_bytes`` has the wrong exact type. + ValueError: If ``max_bytes`` is not positive. + ExpertProgramDecodeError: If parsing or strict decoding fails. + """ + data = parse_expert_program_json(text, max_bytes=max_bytes) + return decode_expert_program(data, validation_context=validation_context) + + +class _UniqueKeySafeLoader(yaml.SafeLoader): + """YAML safe loader that also rejects ambiguous duplicate keys.""" + + +def _construct_unique_yaml_mapping( + loader: _UniqueKeySafeLoader, + node: yaml.MappingNode, + deep: bool = False, +) -> dict[object, object]: + """Construct one YAML mapping with unique, hashable keys.""" + loader.flatten_mapping(node) + mapping: dict[object, object] = {} + for key_node, value_node in node.value: + key = loader.construct_object(key_node, deep=deep) + try: + duplicate = key in mapping + except TypeError as error: + raise yaml.constructor.ConstructorError( + "while constructing a mapping", + node.start_mark, + "found an unhashable key", + key_node.start_mark, + ) from error + if duplicate: + raise yaml.constructor.ConstructorError( + "while constructing a mapping", + node.start_mark, + f"found duplicate key {key!r}", + key_node.start_mark, + ) + mapping[key] = loader.construct_object(value_node, deep=deep) + return mapping + + +_UniqueKeySafeLoader.add_constructor( + yaml.resolver.BaseResolver.DEFAULT_MAPPING_TAG, + _construct_unique_yaml_mapping, +) + + +def load_expert_program( + path: str | os.PathLike[str], + *, + base_dir: str | os.PathLike[str] | None = None, + validation_context: ExpertProgramValidationContext | None = None, +) -> ExpertProgramCfg: + """Safely load and strictly decode one JSON or YAML Expert Program file. + + Relative paths are resolved from ``base_dir`` when provided. Otherwise, + they retain normal :class:`pathlib.Path` semantics and therefore resolve + from the process working directory when opened. + + Args: + path: JSON, YAML, or YML file to load. + base_dir: Optional directory used to resolve a relative ``path``. + validation_context: Optional provider-free static reference validator + applied after decoding either serialized format. + + Returns: + An owned, validated Expert Program configuration. + + Raises: + FileNotFoundError: If the resolved path is not a regular file. + ValueError: If the file is too large, has an unsupported extension, or + contains ambiguous or invalid serialized data. + ExpertProgramValidationError: If ``validation_context`` rejects an + external reference. + UnicodeDecodeError: If the file is not valid UTF-8. + """ + program_path = Path(path).expanduser() + if base_dir is not None and not program_path.is_absolute(): + program_path = Path(base_dir).expanduser() / program_path + if not program_path.is_file(): + raise FileNotFoundError(f"Expert Program path is not a file: {program_path}.") + suffix = program_path.suffix.lower() + if suffix not in {".json", ".yaml", ".yml"}: + raise ValueError( + "Expert Program must use a .json, .yaml, or .yml extension; " + f"got {program_path.name!r}." + ) + + payload = program_path.read_bytes() + if len(payload) > MAX_EXPERT_PROGRAM_BYTES: + raise ExpertProgramDecodeError( + "input_too_large", + (), + "Expert Program exceeds the " + f"{MAX_EXPERT_PROGRAM_BYTES}-byte input limit.", + ) + text = payload.decode("utf-8") + if suffix == ".json": + return loads_expert_program_json( + text, + validation_context=validation_context, + ) + try: + data = yaml.load(text, Loader=_UniqueKeySafeLoader) + except yaml.YAMLError as error: + raise ValueError( + f"Invalid Expert Program YAML in {program_path}: {error}" + ) from error + return decode_expert_program( + data, + validation_context=validation_context, + ) diff --git a/embodichain/lab/gym/envs/expert_program/simulation.py b/embodichain/lab/gym/envs/expert_program/simulation.py new file mode 100644 index 000000000..8f8fd5f1e --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/simulation.py @@ -0,0 +1,1698 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Explicit simulation bindings for declarative Expert Programs. + +The values in this module bridge task-owned, executable-free declarations to +the existing :class:`SceneRegistry` and :class:`RobotSkillProfile` contracts. +They deliberately do not scan the simulation or infer semantic capabilities +from names. Every simulation entity, articulation member, control part, and +semantic command is selected explicitly and validated while the binding is +built. +""" + +from __future__ import annotations + +from collections.abc import Mapping +from copy import deepcopy +from dataclasses import dataclass, field, replace +import math +from types import MappingProxyType +from typing import Any, Protocol, runtime_checkable, TYPE_CHECKING + +import torch + +from embodichain.lab.sim.atomic_actions import ( + AntipodalAffordance, + ArticulationOperationAffordance, + ArticulationOperationTarget, + ControlPartCommandProfile, + EntityState, +) +from embodichain.lab.sim.skills.profiles import ( + ControlPartEndpoint, + ResourceEndpoint, + ResourceBinding, + RobotResource, + RobotSkillProfile, + SkillPolicyPreset, +) +from embodichain.lab.sim.skills.integration import SceneEntityManifest, SceneManifest +from embodichain.lab.sim.skills.scene import ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + ContainerAffordance, + GRASP_AFFORDANCE_CAPABILITY, + PLACEMENT_TARGET_AFFORDANCE_REVISION, + PLACE_IN_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, + SceneAffordanceRef, + SceneArticulationRef, + SceneCollisionRole, + SceneCollisionWorldMode, + SceneDynamics, + SceneEntityRef, + SceneEntityRegistration, + SceneGeometryProvider, + SceneLinkRef, + SceneObjectRef, + SceneRegistry, + SupportSurfaceAffordance, +) +from embodichain.toolkits.graspkit.pg_grasp import GraspGeneratorCfg +from embodichain.toolkits.graspkit.pg_grasp.gripper_collision_checker import ( + GripperCollisionCfg, +) + +if TYPE_CHECKING: + from embodichain.lab.sim.objects import Robot + from embodichain.lab.sim.sim_manager import SimulationManager + + +_IDENTITY_POSE = ( + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, +) + + +def _identifier(value: str, *, field_name: str) -> str: + """Return one exact non-empty identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +def _optional_identifier(value: str | None, *, field_name: str) -> str | None: + """Validate one optional identifier.""" + if value is not None: + _identifier(value, field_name=field_name) + return value + + +def _identifier_tuple( + values: tuple[str, ...], + *, + field_name: str, +) -> tuple[str, ...]: + """Own a duplicate-free tuple of exact identifiers.""" + if isinstance(values, (str, bytes)): + raise TypeError(f"{field_name} must be an iterable of identifiers.") + normalized = tuple(values) + for value in normalized: + _identifier(value, field_name=field_name) + if len(set(normalized)) != len(normalized): + raise ValueError(f"{field_name} must contain unique identifiers.") + return normalized + + +def _finite(value: float, *, field_name: str) -> float: + """Return one finite non-boolean float.""" + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise TypeError(f"{field_name} must be a finite number.") + normalized = float(value) + if not math.isfinite(normalized): + raise ValueError(f"{field_name} must be finite.") + return normalized + + +def _pose_tuple( + values: tuple[float, ...], + *, + field_name: str, +) -> tuple[float, ...]: + """Own and validate one flattened SE(3) matrix.""" + if isinstance(values, (str, bytes)): + raise TypeError(f"{field_name} must contain 16 finite numbers.") + normalized = tuple( + _finite(value, field_name=f"{field_name}[{index}]") + for index, value in enumerate(values) + ) + if len(normalized) != 16: + raise ValueError(f"{field_name} must contain exactly 16 numbers.") + pose = torch.tensor(normalized, dtype=torch.float64).reshape(4, 4) + bottom = torch.tensor((0.0, 0.0, 0.0, 1.0), dtype=torch.float64) + if not torch.allclose(pose[3], bottom, atol=1.0e-6, rtol=0.0): + raise ValueError(f"{field_name} must have bottom row [0, 0, 0, 1].") + rotation = pose[:3, :3] + if not torch.allclose( + rotation.T @ rotation, + torch.eye(3, dtype=torch.float64), + atol=1.0e-6, + rtol=0.0, + ) or not torch.isclose( + torch.linalg.det(rotation), + torch.tensor(1.0, dtype=torch.float64), + atol=1.0e-6, + rtol=0.0, + ): + raise ValueError(f"{field_name} must contain a proper SE(3) rotation.") + return normalized + + +def _pose_tensor(values: tuple[float, ...]) -> torch.Tensor: + """Materialize an owned float32 pose matrix.""" + return torch.tensor(values, dtype=torch.float32).reshape(4, 4) + + +def _validate_scene_classification( + dynamics: SceneDynamics, + collision_role: SceneCollisionRole, +) -> None: + """Validate exact scene-enum values.""" + if not isinstance(dynamics, SceneDynamics): + raise TypeError("dynamics must be a SceneDynamics value.") + if not isinstance(collision_role, SceneCollisionRole): + raise TypeError("collision_role must be a SceneCollisionRole value.") + + +@dataclass(frozen=True, slots=True) +class SimulationRigidObjectBinding: + """Explicit binding for one simulation rigid object.""" + + entity_id: str + simulation_uid: str + aliases: tuple[str, ...] = () + dynamics: SceneDynamics = SceneDynamics.UNKNOWN + collision_role: SceneCollisionRole = SceneCollisionRole.NONE + semantic_type: str | None = None + default_grasp_affordance: str | None = None + geometry_provider: SceneGeometryProvider | None = None + + def __post_init__(self) -> None: + _identifier(self.entity_id, field_name="entity_id") + _identifier(self.simulation_uid, field_name="simulation_uid") + object.__setattr__( + self, + "aliases", + _identifier_tuple(self.aliases, field_name="aliases"), + ) + _validate_scene_classification(self.dynamics, self.collision_role) + _optional_identifier(self.semantic_type, field_name="semantic_type") + _optional_identifier( + self.default_grasp_affordance, + field_name="default_grasp_affordance", + ) + + +@dataclass(frozen=True, slots=True) +class SimulationArticulationBinding: + """Explicit binding for one simulation articulation.""" + + entity_id: str + simulation_uid: str + aliases: tuple[str, ...] = () + dynamics: SceneDynamics = SceneDynamics.UNKNOWN + collision_role: SceneCollisionRole = SceneCollisionRole.NONE + semantic_type: str | None = None + default_operation_affordance: str | None = None + geometry_provider: SceneGeometryProvider | None = None + + def __post_init__(self) -> None: + _identifier(self.entity_id, field_name="entity_id") + _identifier(self.simulation_uid, field_name="simulation_uid") + object.__setattr__( + self, + "aliases", + _identifier_tuple(self.aliases, field_name="aliases"), + ) + _validate_scene_classification(self.dynamics, self.collision_role) + _optional_identifier(self.semantic_type, field_name="semantic_type") + _optional_identifier( + self.default_operation_affordance, + field_name="default_operation_affordance", + ) + + +@dataclass(frozen=True, slots=True) +class SimulationArticulationLinkBinding: + """Explicit canonical link backed by one native articulation link.""" + + entity_id: str + articulation_id: str + native_link_name: str + aliases: tuple[str, ...] = () + dynamics: SceneDynamics = SceneDynamics.UNKNOWN + semantic_type: str | None = None + + def __post_init__(self) -> None: + _identifier(self.entity_id, field_name="entity_id") + _identifier(self.articulation_id, field_name="articulation_id") + _identifier(self.native_link_name, field_name="native_link_name") + object.__setattr__( + self, + "aliases", + _identifier_tuple(self.aliases, field_name="aliases"), + ) + if not isinstance(self.dynamics, SceneDynamics): + raise TypeError("dynamics must be a SceneDynamics value.") + _optional_identifier(self.semantic_type, field_name="semantic_type") + + +@dataclass(frozen=True, slots=True) +class AntipodalGraspAffordanceBinding: + """Build one antipodal grasp affordance from a selected rigid-object mesh.""" + + entity_id: str + object_id: str + native_name: str + revision: str + aliases: tuple[str, ...] = () + relative_pose: tuple[float, ...] = _IDENTITY_POSE + mesh_env_id: int = 0 + generator_cfg: GraspGeneratorCfg | None = None + gripper_collision_cfg: GripperCollisionCfg | None = None + force_reannotate: bool = False + + def __post_init__(self) -> None: + for field_name in ("entity_id", "object_id", "native_name", "revision"): + _identifier(getattr(self, field_name), field_name=field_name) + object.__setattr__( + self, + "aliases", + _identifier_tuple(self.aliases, field_name="aliases"), + ) + object.__setattr__( + self, + "relative_pose", + _pose_tuple(self.relative_pose, field_name="relative_pose"), + ) + if ( + isinstance(self.mesh_env_id, bool) + or not isinstance(self.mesh_env_id, int) + or self.mesh_env_id < 0 + ): + raise ValueError("mesh_env_id must be a non-negative integer.") + if self.generator_cfg is not None and not isinstance( + self.generator_cfg, + GraspGeneratorCfg, + ): + raise TypeError("generator_cfg must be GraspGeneratorCfg or None.") + if self.gripper_collision_cfg is not None and not isinstance( + self.gripper_collision_cfg, + GripperCollisionCfg, + ): + raise TypeError( + "gripper_collision_cfg must be GripperCollisionCfg or None." + ) + if not isinstance(self.force_reannotate, bool): + raise TypeError("force_reannotate must be a bool.") + object.__setattr__(self, "generator_cfg", deepcopy(self.generator_cfg)) + object.__setattr__( + self, + "gripper_collision_cfg", + deepcopy(self.gripper_collision_cfg), + ) + + +def _validate_placement_binding(value: object) -> None: + """Validate fields shared by built-in placement-frame declarations.""" + for field_name in ("entity_id", "parent_id", "native_name"): + _identifier(getattr(value, field_name), field_name=field_name) + object.__setattr__( + value, + "aliases", + _identifier_tuple(getattr(value, "aliases"), field_name="aliases"), + ) + object.__setattr__( + value, + "object_target_pose", + _pose_tuple( + getattr(value, "object_target_pose"), + field_name="object_target_pose", + ), + ) + minimum_confidence = _finite( + getattr(value, "minimum_confidence"), + field_name="minimum_confidence", + ) + if not 0.0 <= minimum_confidence <= 1.0: + raise ValueError("minimum_confidence must be in [0, 1].") + object.__setattr__(value, "minimum_confidence", minimum_confidence) + if type(getattr(value, "is_default")) is not bool: + raise TypeError("is_default must be a bool.") + + +@dataclass(frozen=True, slots=True) +class SupportSurfaceAffordanceBinding: + """Declare one exact object target frame on a support parent. + + Args: + entity_id: Canonical ID of the placement affordance. + parent_id: Canonical object, articulation, or link parent ID. + native_name: Stable native name of this target frame. + aliases: Optional non-authoritative lookup aliases. + object_target_pose: Desired object pose relative to the parent. + minimum_confidence: Minimum parent/affordance observation confidence. + is_default: Whether this is the parent's default ``Place(on=...)`` frame. + """ + + entity_id: str + parent_id: str + native_name: str + aliases: tuple[str, ...] = () + object_target_pose: tuple[float, ...] = _IDENTITY_POSE + minimum_confidence: float = 0.0 + is_default: bool = False + + def __post_init__(self) -> None: + _validate_placement_binding(self) + + +@dataclass(frozen=True, slots=True) +class ContainerAffordanceBinding: + """Declare one exact object target frame inside a container parent. + + Args: + entity_id: Canonical ID of the placement affordance. + parent_id: Canonical object, articulation, or link parent ID. + native_name: Stable native name of this target frame. + aliases: Optional non-authoritative lookup aliases. + object_target_pose: Desired object pose relative to the parent. + minimum_confidence: Minimum parent/affordance observation confidence. + is_default: Whether this is the parent's default ``Place(inside=...)`` frame. + """ + + entity_id: str + parent_id: str + native_name: str + aliases: tuple[str, ...] = () + object_target_pose: tuple[float, ...] = _IDENTITY_POSE + minimum_confidence: float = 0.0 + is_default: bool = False + + def __post_init__(self) -> None: + _validate_placement_binding(self) + + +@dataclass(frozen=True, slots=True) +class ArticulationOperationTargetBinding: + """Declarative named target for one articulation operation.""" + + target_position: float + displacement: float + + def __post_init__(self) -> None: + object.__setattr__( + self, + "target_position", + _finite(self.target_position, field_name="target_position"), + ) + object.__setattr__( + self, + "displacement", + _finite(self.displacement, field_name="displacement"), + ) + + def build(self) -> ArticulationOperationTarget: + """Build the existing atomic-action target value.""" + return ArticulationOperationTarget( + target_position=self.target_position, + displacement=self.displacement, + ) + + +@dataclass(frozen=True, slots=True) +class ArticulationOperationAffordanceBinding: + """Bind one handle operation to an explicit native link and joint.""" + + entity_id: str + articulation_id: str + link_id: str + joint_id: str + revision: str + semantic_targets: Mapping[str, ArticulationOperationTargetBinding] + aliases: tuple[str, ...] = () + handle_pose_offset: tuple[float, ...] = _IDENTITY_POSE + approach_offset: tuple[float, ...] = _IDENTITY_POSE + contact_offset: tuple[float, ...] = _IDENTITY_POSE + operation_offset: tuple[float, ...] = _IDENTITY_POSE + retract_offset: tuple[float, ...] = _IDENTITY_POSE + operation_axis: tuple[float, float, float] = (1.0, 0.0, 0.0) + position_scale: float = 1.0 + + def __post_init__(self) -> None: + for field_name in ( + "entity_id", + "articulation_id", + "link_id", + "joint_id", + "revision", + ): + _identifier(getattr(self, field_name), field_name=field_name) + object.__setattr__( + self, + "aliases", + _identifier_tuple(self.aliases, field_name="aliases"), + ) + for field_name in ( + "handle_pose_offset", + "approach_offset", + "contact_offset", + "operation_offset", + "retract_offset", + ): + object.__setattr__( + self, + field_name, + _pose_tuple(getattr(self, field_name), field_name=field_name), + ) + axis = tuple( + _finite(value, field_name=f"operation_axis[{index}]") + for index, value in enumerate(self.operation_axis) + ) + if len(axis) != 3 or math.sqrt(sum(value * value for value in axis)) <= 0.0: + raise ValueError("operation_axis must contain three non-zero values.") + object.__setattr__(self, "operation_axis", axis) + position_scale = _finite(self.position_scale, field_name="position_scale") + if position_scale <= 0.0: + raise ValueError("position_scale must be positive.") + object.__setattr__(self, "position_scale", position_scale) + if not isinstance(self.semantic_targets, Mapping): + raise TypeError("semantic_targets must be a mapping.") + targets: dict[str, ArticulationOperationTargetBinding] = {} + for target_id, target in self.semantic_targets.items(): + _identifier(target_id, field_name="semantic target IDs") + if type(target) is not ArticulationOperationTargetBinding: + raise TypeError( + "semantic_targets values must be exact " + "ArticulationOperationTargetBinding values." + ) + targets[target_id] = target + object.__setattr__(self, "semantic_targets", MappingProxyType(targets)) + + +@dataclass(frozen=True, slots=True) +class _SimulationArticulationLinkStateProvider: + """Read one selected native link pose with an optional local offset.""" + + articulation: Any + native_link_name: str + local_offset: torch.Tensor = field(repr=False) + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp + getter = getattr(self.articulation, "get_link_pose", None) + if not callable(getter): + raise TypeError("Simulation articulation must provide get_link_pose().") + pose = getter( + self.native_link_name, + env_ids=env_ids.detach().to("cpu").tolist(), + to_matrix=True, + ) + if not isinstance(pose, torch.Tensor): + raise TypeError( + "Simulation articulation get_link_pose() must return a tensor." + ) + offset = self.local_offset.to(device=pose.device, dtype=pose.dtype) + return EntityState(torch.matmul(pose, offset)) + + +def _require_native_entity( + simulation: SimulationManager, + *, + getter_name: str, + registry_id: str, + simulation_uid: str, +) -> Any: + """Resolve one explicitly selected native entity or fail closed.""" + getter = getattr(simulation, getter_name, None) + if not callable(getter): + raise TypeError(f"simulation must provide {getter_name}().") + entity = getter(simulation_uid) + if entity is None: + raise KeyError( + f"Simulation UID {simulation_uid!r} selected for registry entity " + f"{registry_id!r} was not found." + ) + return entity + + +def _native_names(entity: Any, *, attribute: str, owner: str) -> tuple[str, ...]: + """Read and validate one existing native-name collection.""" + values = getattr(entity, attribute, None) + if values is None: + raise TypeError(f"{owner} must expose {attribute}.") + if isinstance(values, (str, bytes)): + raise TypeError(f"{owner}.{attribute} must be an iterable of names.") + try: + names = tuple(values) + except TypeError as exc: + raise TypeError(f"{owner}.{attribute} must be an iterable of names.") from exc + for name in names: + _identifier(name, field_name=f"{owner}.{attribute}") + if len(set(names)) != len(names): + raise ValueError(f"{owner}.{attribute} must contain unique names.") + return names + + +def _mesh_tensor( + entity: Any, + *, + getter_name: str, + mesh_env_id: int, + vertices: bool, +) -> torch.Tensor: + """Read one explicitly selected mesh row with strict shape validation.""" + getter = getattr(entity, getter_name, None) + if not callable(getter): + raise TypeError(f"Simulation rigid object must provide {getter_name}().") + if vertices: + value = getter(env_ids=[mesh_env_id], scale=True) + else: + value = getter(env_ids=[mesh_env_id]) + if not isinstance(value, torch.Tensor): + raise TypeError( + f"Simulation rigid object {getter_name}() must return a tensor." + ) + if value.dim() != 3 or value.shape[0] != 1 or value.shape[2] != 3: + raise ValueError( + f"Simulation rigid object {getter_name}() must return shape (1, N, 3)." + ) + selected = value[0].detach().clone() + if selected.shape[0] == 0: + raise ValueError(f"Simulation rigid object {getter_name}() returned no data.") + if vertices: + if not selected.is_floating_point() or not torch.isfinite(selected).all(): + raise ValueError("Antipodal mesh vertices must be finite floating values.") + elif selected.dtype == torch.bool or selected.is_floating_point(): + raise TypeError("Antipodal mesh triangles must use an integer dtype.") + return selected + + +def _antipodal_affordance( + binding: AntipodalGraspAffordanceBinding, + entity: Any, +) -> AntipodalAffordance: + """Build and validate one owned antipodal affordance payload.""" + vertices = _mesh_tensor( + entity, + getter_name="get_vertices", + mesh_env_id=binding.mesh_env_id, + vertices=True, + ) + triangles = _mesh_tensor( + entity, + getter_name="get_triangles", + mesh_env_id=binding.mesh_env_id, + vertices=False, + ) + if bool((triangles < 0).any()) or int(triangles.max().item()) >= vertices.shape[0]: + raise ValueError("Antipodal mesh triangles reference invalid vertex indices.") + return AntipodalAffordance( + mesh_vertices=vertices, + mesh_triangles=triangles, + generator_cfg=deepcopy(binding.generator_cfg), + gripper_collision_cfg=deepcopy(binding.gripper_collision_cfg), + force_reannotate=binding.force_reannotate, + ) + + +def _placement_parent_ref( + parent_id: str, + *, + objects: Mapping[str, SimulationRigidObjectBinding], + articulations: Mapping[str, SimulationArticulationBinding], + links: Mapping[str, SimulationArticulationLinkBinding], +) -> SceneEntityRef: + """Resolve an explicitly declared placement parent to its exact ref type.""" + if parent_id in objects: + return SceneObjectRef(parent_id) + if parent_id in articulations: + return SceneArticulationRef(parent_id) + if parent_id in links: + return SceneLinkRef(parent_id) + raise KeyError(f"Placement affordance references unbound parent {parent_id!r}.") + + +def _placement_defaults( + support_surfaces: tuple[SupportSurfaceAffordanceBinding, ...], + containers: tuple[ContainerAffordanceBinding, ...], +) -> Mapping[str, Mapping[str, SceneAffordanceRef]]: + """Collect explicitly selected capability-scoped placement defaults.""" + defaults: dict[str, dict[str, SceneAffordanceRef]] = {} + for capability, bindings in ( + (PLACE_ON_AFFORDANCE_CAPABILITY, support_surfaces), + (PLACE_IN_AFFORDANCE_CAPABILITY, containers), + ): + for binding in bindings: + if not binding.is_default: + continue + parent_defaults = defaults.setdefault(binding.parent_id, {}) + previous = parent_defaults.get(capability) + if previous is not None: + raise ValueError( + f"Placement parent {binding.parent_id!r} has multiple default " + f"affordances for capability {capability!r}: " + f"{previous.entity_id!r} and {binding.entity_id!r}." + ) + parent_defaults[capability] = SceneAffordanceRef(binding.entity_id) + return defaults + + +@dataclass(frozen=True, slots=True) +class SimulationSceneBinding: + """Build one authoritative registry from explicit simulation bindings.""" + + registry_id: str + rigid_objects: tuple[SimulationRigidObjectBinding, ...] = () + articulations: tuple[SimulationArticulationBinding, ...] = () + links: tuple[SimulationArticulationLinkBinding, ...] = () + antipodal_grasps: tuple[AntipodalGraspAffordanceBinding, ...] = () + articulation_operations: tuple[ArticulationOperationAffordanceBinding, ...] = () + support_surfaces: tuple[SupportSurfaceAffordanceBinding, ...] = () + containers: tuple[ContainerAffordanceBinding, ...] = () + collision_world_mode: SceneCollisionWorldMode | None = None + + def __post_init__(self) -> None: + _identifier(self.registry_id, field_name="registry_id") + expected_types = { + "rigid_objects": SimulationRigidObjectBinding, + "articulations": SimulationArticulationBinding, + "links": SimulationArticulationLinkBinding, + "antipodal_grasps": AntipodalGraspAffordanceBinding, + "articulation_operations": ArticulationOperationAffordanceBinding, + "support_surfaces": SupportSurfaceAffordanceBinding, + "containers": ContainerAffordanceBinding, + } + all_ids: list[str] = [] + for field_name, expected_type in expected_types.items(): + values = tuple(getattr(self, field_name)) + if not all(type(value) is expected_type for value in values): + raise TypeError( + f"{field_name} must contain exact {expected_type.__name__} values." + ) + object.__setattr__(self, field_name, values) + all_ids.extend(value.entity_id for value in values) + duplicates = sorted( + entity_id for entity_id in set(all_ids) if all_ids.count(entity_id) > 1 + ) + if duplicates: + raise ValueError(f"Scene binding entity IDs must be unique: {duplicates}.") + if self.collision_world_mode is not None and not isinstance( + self.collision_world_mode, + SceneCollisionWorldMode, + ): + raise TypeError( + "collision_world_mode must be SceneCollisionWorldMode or None." + ) + _placement_defaults(self.support_surfaces, self.containers) + + def declare(self) -> SceneManifest: + """Project the complete provider-free scene declaration. + + Canonical topology errors are rejected here, before a simulation is + constructed. Native simulation UIDs, mesh data, link names, and joint + names remain live validation owned by :meth:`build`. + """ + objects = {item.entity_id: item for item in self.rigid_objects} + articulations = {item.entity_id: item for item in self.articulations} + links = {item.entity_id: item for item in self.links} + placement_defaults = _placement_defaults( + self.support_surfaces, + self.containers, + ) + entries: list[SceneEntityManifest] = [] + + for binding in self.rigid_objects: + native_aliases = ( + () + if binding.simulation_uid == binding.entity_id + else (binding.simulation_uid,) + ) + defaults = dict(placement_defaults.get(binding.entity_id, {})) + if binding.default_grasp_affordance is not None: + defaults.update( + { + GRASP_AFFORDANCE_CAPABILITY: SceneAffordanceRef( + binding.default_grasp_affordance + ) + } + ) + entries.append( + SceneEntityManifest( + ref=SceneObjectRef(binding.entity_id), + aliases=(*native_aliases, *binding.aliases), + dynamics=binding.dynamics, + collision_role=binding.collision_role, + semantic_type=binding.semantic_type, + default_affordances=defaults, + ) + ) + + for binding in self.articulations: + native_aliases = ( + () + if binding.simulation_uid == binding.entity_id + else (binding.simulation_uid,) + ) + defaults = dict(placement_defaults.get(binding.entity_id, {})) + if binding.default_operation_affordance is not None: + defaults.update( + { + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY: ( + SceneAffordanceRef(binding.default_operation_affordance) + ) + } + ) + entries.append( + SceneEntityManifest( + ref=SceneArticulationRef(binding.entity_id), + aliases=(*native_aliases, *binding.aliases), + dynamics=binding.dynamics, + collision_role=binding.collision_role, + semantic_type=binding.semantic_type, + default_affordances=defaults, + ) + ) + + for binding in self.links: + if binding.articulation_id not in articulations: + raise KeyError( + f"Link {binding.entity_id!r} references unbound articulation " + f"{binding.articulation_id!r}." + ) + entries.append( + SceneEntityManifest( + ref=SceneLinkRef(binding.entity_id), + aliases=binding.aliases, + parent=SceneArticulationRef(binding.articulation_id), + native_name=binding.native_link_name, + dynamics=binding.dynamics, + semantic_type=binding.semantic_type, + default_affordances=placement_defaults.get( + binding.entity_id, + {}, + ), + ) + ) + + for binding in self.antipodal_grasps: + if binding.object_id not in objects: + raise KeyError( + f"Grasp affordance {binding.entity_id!r} references unbound " + f"object {binding.object_id!r}." + ) + entries.append( + SceneEntityManifest( + ref=SceneAffordanceRef(binding.entity_id), + aliases=binding.aliases, + parent=SceneObjectRef(binding.object_id), + native_name=binding.native_name, + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + affordance_payload_type=AntipodalAffordance, + affordance_revision=binding.revision, + relative_pose=binding.relative_pose, + ) + ) + + for binding in self.articulation_operations: + if binding.articulation_id not in articulations: + raise KeyError( + f"Operation affordance {binding.entity_id!r} references " + f"unbound articulation {binding.articulation_id!r}." + ) + link = links.get(binding.link_id) + if link is None: + raise KeyError( + f"Operation affordance {binding.entity_id!r} references " + f"unbound link {binding.link_id!r}." + ) + if link.articulation_id != binding.articulation_id: + raise ValueError( + f"Operation affordance {binding.entity_id!r} and link " + f"{binding.link_id!r} select different articulations." + ) + entries.append( + SceneEntityManifest( + ref=SceneAffordanceRef(binding.entity_id), + aliases=binding.aliases, + parent=SceneArticulationRef(binding.articulation_id), + native_name=link.native_link_name, + affordance_capabilities=frozenset( + {ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY} + ), + affordance_payload_type=ArticulationOperationAffordance, + affordance_revision=binding.revision, + ) + ) + + for capability, payload_type, bindings in ( + ( + PLACE_ON_AFFORDANCE_CAPABILITY, + SupportSurfaceAffordance, + self.support_surfaces, + ), + ( + PLACE_IN_AFFORDANCE_CAPABILITY, + ContainerAffordance, + self.containers, + ), + ): + for binding in bindings: + parent = _placement_parent_ref( + binding.parent_id, + objects=objects, + articulations=articulations, + links=links, + ) + entries.append( + SceneEntityManifest( + ref=SceneAffordanceRef(binding.entity_id), + aliases=binding.aliases, + parent=parent, + native_name=binding.native_name, + affordance_capabilities=frozenset({capability}), + affordance_payload_type=payload_type, + affordance_revision=PLACEMENT_TARGET_AFFORDANCE_REVISION, + relative_pose=binding.object_target_pose, + ) + ) + + return SceneManifest(entries) + + def build(self, simulation: SimulationManager) -> SceneRegistry: + """Build the existing authoritative scene registry. + + Args: + simulation: Live simulation used only for explicitly named lookups. + + Returns: + Immutable registry with typed roots, links, and affordances. + """ + objects = {item.entity_id: item for item in self.rigid_objects} + articulations = {item.entity_id: item for item in self.articulations} + placement_defaults = _placement_defaults( + self.support_surfaces, + self.containers, + ) + geometry = { + item.entity_id: item.geometry_provider + for item in (*self.rigid_objects, *self.articulations) + if item.geometry_provider is not None + } + roles = { + item.entity_id: item.collision_role + for item in (*self.rigid_objects, *self.articulations) + } + base = SceneRegistry.from_simulation( + simulation, + rigid_objects={ + item.entity_id: item.simulation_uid for item in self.rigid_objects + }, + articulations={ + item.entity_id: item.simulation_uid for item in self.articulations + }, + collision_roles=roles, + geometry_providers=geometry, + collision_world_mode=self.collision_world_mode, + ) + + registrations: list[SceneEntityRegistration] = [] + for registration in base.registrations: + entity_id = registration.ref.entity_id + if isinstance(registration.ref, SceneObjectRef): + binding = objects[entity_id] + defaults = dict(placement_defaults.get(entity_id, {})) + if binding.default_grasp_affordance is not None: + defaults.update( + { + GRASP_AFFORDANCE_CAPABILITY: SceneAffordanceRef( + binding.default_grasp_affordance + ) + } + ) + else: + binding = articulations[entity_id] + defaults = dict(placement_defaults.get(entity_id, {})) + if binding.default_operation_affordance is not None: + defaults.update( + { + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY: ( + SceneAffordanceRef(binding.default_operation_affordance) + ) + } + ) + registrations.append( + replace( + registration, + aliases=(*registration.aliases, *binding.aliases), + dynamics=binding.dynamics, + semantic_type=binding.semantic_type, + default_affordances=defaults, + ) + ) + + native_articulations: dict[str, Any] = {} + links: dict[str, SimulationArticulationLinkBinding] = {} + for binding in self.links: + articulation_binding = articulations.get(binding.articulation_id) + if articulation_binding is None: + raise KeyError( + f"Link {binding.entity_id!r} references unbound articulation " + f"{binding.articulation_id!r}." + ) + articulation = native_articulations.setdefault( + binding.articulation_id, + _require_native_entity( + simulation, + getter_name="get_articulation", + registry_id=binding.articulation_id, + simulation_uid=articulation_binding.simulation_uid, + ), + ) + native_links = _native_names( + articulation, + attribute="link_names", + owner=f"articulation {binding.articulation_id!r}", + ) + if binding.native_link_name not in native_links: + raise KeyError( + f"Native link {binding.native_link_name!r} selected for " + f"{binding.entity_id!r} was not found; available links are " + f"{sorted(native_links)}." + ) + links[binding.entity_id] = binding + registrations.append( + SceneEntityRegistration( + ref=SceneLinkRef(binding.entity_id), + state_provider=_SimulationArticulationLinkStateProvider( + articulation, + binding.native_link_name, + _pose_tensor(_IDENTITY_POSE), + ), + aliases=binding.aliases, + parent=SceneArticulationRef(binding.articulation_id), + native_name=binding.native_link_name, + dynamics=binding.dynamics, + semantic_type=binding.semantic_type, + default_affordances=placement_defaults.get( + binding.entity_id, + {}, + ), + ) + ) + + native_objects: dict[str, Any] = {} + for binding in self.antipodal_grasps: + object_binding = objects.get(binding.object_id) + if object_binding is None: + raise KeyError( + f"Grasp affordance {binding.entity_id!r} references unbound " + f"object {binding.object_id!r}." + ) + entity = native_objects.setdefault( + binding.object_id, + _require_native_entity( + simulation, + getter_name="get_rigid_object", + registry_id=binding.object_id, + simulation_uid=object_binding.simulation_uid, + ), + ) + registrations.append( + SceneEntityRegistration( + ref=SceneAffordanceRef(binding.entity_id), + aliases=binding.aliases, + parent=SceneObjectRef(binding.object_id), + native_name=binding.native_name, + affordance=_antipodal_affordance(binding, entity), + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + affordance_revision=binding.revision, + relative_pose=_pose_tensor(binding.relative_pose), + ) + ) + + for binding in self.articulation_operations: + articulation_binding = articulations.get(binding.articulation_id) + if articulation_binding is None: + raise KeyError( + f"Operation affordance {binding.entity_id!r} references " + f"unbound articulation {binding.articulation_id!r}." + ) + link_binding = links.get(binding.link_id) + if link_binding is None: + raise KeyError( + f"Operation affordance {binding.entity_id!r} references " + f"unbound link {binding.link_id!r}." + ) + if link_binding.articulation_id != binding.articulation_id: + raise ValueError( + f"Operation affordance {binding.entity_id!r} and link " + f"{binding.link_id!r} select different articulations." + ) + articulation = native_articulations[binding.articulation_id] + native_joints = _native_names( + articulation, + attribute="joint_names", + owner=f"articulation {binding.articulation_id!r}", + ) + if binding.joint_id not in native_joints: + raise KeyError( + f"Native joint {binding.joint_id!r} selected for " + f"{binding.entity_id!r} was not found; available joints are " + f"{sorted(native_joints)}." + ) + payload = ArticulationOperationAffordance( + joint_id=binding.joint_id, + approach_offset=_pose_tensor(binding.approach_offset), + contact_offset=_pose_tensor(binding.contact_offset), + operation_offset=_pose_tensor(binding.operation_offset), + retract_offset=_pose_tensor(binding.retract_offset), + operation_axis=torch.tensor( + binding.operation_axis, + dtype=torch.float32, + ), + position_scale=binding.position_scale, + semantic_targets={ + target_id: target.build() + for target_id, target in binding.semantic_targets.items() + }, + ) + registrations.append( + SceneEntityRegistration( + ref=SceneAffordanceRef(binding.entity_id), + state_provider=_SimulationArticulationLinkStateProvider( + articulation, + link_binding.native_link_name, + _pose_tensor(binding.handle_pose_offset), + ), + aliases=binding.aliases, + parent=SceneArticulationRef(binding.articulation_id), + native_name=link_binding.native_link_name, + affordance=payload, + affordance_capabilities=frozenset( + {ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY} + ), + affordance_revision=binding.revision, + ) + ) + + for capability, payload_type, bindings in ( + ( + PLACE_ON_AFFORDANCE_CAPABILITY, + SupportSurfaceAffordance, + self.support_surfaces, + ), + ( + PLACE_IN_AFFORDANCE_CAPABILITY, + ContainerAffordance, + self.containers, + ), + ): + for binding in bindings: + parent = _placement_parent_ref( + binding.parent_id, + objects=objects, + articulations=articulations, + links=links, + ) + payload = payload_type( + minimum_confidence=binding.minimum_confidence, + ) + registrations.append( + SceneEntityRegistration( + ref=SceneAffordanceRef(binding.entity_id), + aliases=binding.aliases, + parent=parent, + native_name=binding.native_name, + affordance=payload, + affordance_capabilities=frozenset({capability}), + affordance_revision=PLACEMENT_TARGET_AFFORDANCE_REVISION, + relative_pose=_pose_tensor(binding.object_target_pose), + ) + ) + + return SceneRegistry( + registrations, + collision_world_mode=self.collision_world_mode, + ) + + +@dataclass(frozen=True, slots=True) +class ControlPartCommandPreset: + """Named one-dimensional joint commands for one exact control part.""" + + preset_id: str + control_part: str + commands: Mapping[str, tuple[float, ...]] + + def __post_init__(self) -> None: + _identifier(self.preset_id, field_name="preset_id") + _identifier(self.control_part, field_name="control_part") + if not isinstance(self.commands, Mapping): + raise TypeError("commands must be a mapping.") + commands: dict[str, tuple[float, ...]] = {} + for command_id, positions in self.commands.items(): + _identifier(command_id, field_name="command IDs") + if isinstance(positions, (str, bytes)): + raise TypeError("command positions must be an iterable of numbers.") + normalized = tuple( + _finite(value, field_name=f"commands[{command_id!r}][{index}]") + for index, value in enumerate(positions) + ) + if not normalized: + raise ValueError("command positions must not be empty.") + commands[command_id] = normalized + object.__setattr__(self, "commands", MappingProxyType(commands)) + + def build(self, *, control_dof: int) -> ControlPartCommandProfile: + """Build a command profile after validating the native control width.""" + for command_id, positions in self.commands.items(): + if len(positions) != control_dof: + raise ValueError( + f"Command {command_id!r} in preset {self.preset_id!r} has " + f"{len(positions)} positions, but control part " + f"{self.control_part!r} has {control_dof} joints." + ) + return ControlPartCommandProfile.joint_positions( + **{ + command_id: torch.tensor(positions, dtype=torch.float32) + for command_id, positions in self.commands.items() + } + ) + + def declare(self) -> ControlPartCommandProfile: + """Build a provider-free command profile from declared tuple widths.""" + widths = {len(positions) for positions in self.commands.values()} + if len(widths) > 1: + raise ValueError( + f"Command preset {self.preset_id!r} declares inconsistent command " + f"widths {sorted(widths)}." + ) + return ControlPartCommandProfile.joint_positions( + **{ + command_id: torch.tensor(positions, dtype=torch.float32) + for command_id, positions in self.commands.items() + } + ) + + +def _require_control_part_dof(robot: Robot, control_part: str) -> int: + """Validate one native joint-backed control part and return its width.""" + control_parts = getattr(robot, "control_parts", None) + if not isinstance(control_parts, Mapping): + raise TypeError("robot must expose a control_parts mapping.") + get_joint_ids = getattr(robot, "get_joint_ids", None) + if not callable(get_joint_ids): + raise TypeError("robot must provide get_joint_ids().") + if control_part not in control_parts: + raise KeyError( + f"Robot control part {control_part!r} was not found; available " + f"control parts are {sorted(str(key) for key in control_parts)}." + ) + joint_ids = tuple(get_joint_ids(name=control_part)) + if not joint_ids: + raise ValueError(f"Robot control part {control_part!r} contains no joints.") + if not all( + isinstance(joint_id, int) and not isinstance(joint_id, bool) and joint_id >= 0 + for joint_id in joint_ids + ): + raise ValueError( + f"Robot control part {control_part!r} returned invalid joint IDs." + ) + if len(set(joint_ids)) != len(joint_ids): + raise ValueError( + f"Robot control part {control_part!r} contains duplicate joint IDs." + ) + return len(joint_ids) + + +@runtime_checkable +class SimulationResourceEndpointBinding(Protocol): + """Build one typed resource endpoint from an explicitly selected robot. + + Implementations are reusable robot-integration declarations. They may + validate embodiment-specific controller surfaces, but must only return an + owned :class:`ResourceEndpoint`; live controller handles remain in the + endpoint adapter and runtime transport. + """ + + @property + def endpoint_id(self) -> str: + """Return the stable endpoint ID within its containing resource.""" + + def build(self, robot: Robot) -> ResourceEndpoint: + """Build and validate one endpoint declaration for ``robot``.""" + + def declare(self) -> ResourceEndpoint: + """Return the provider-free endpoint declaration.""" + + +@runtime_checkable +class SimulationRobotResourceBinding(Protocol): + """Build one leaf or composite resource in the robot resource DAG.""" + + @property + def resource_id(self) -> str: + """Return the stable resource ID.""" + + @property + def members(self) -> tuple[str, ...]: + """Return explicitly declared child resource IDs.""" + + def build(self, robot: Robot) -> RobotResource: + """Build and validate one owned robot resource declaration.""" + + def declare(self) -> RobotResource: + """Return the provider-free resource declaration.""" + + +@dataclass(frozen=True, slots=True) +class RobotResourceBinding: + """Generic simulation binding for arbitrary typed resource endpoints. + + This is the direct configuration path for mobile bases, whole-body + controllers, tools, and other non-joint transports. Endpoint-specific + validation remains in the registered :class:`ResourceEndpointAdapter`; + this value owns the declaration and preserves the resource DAG exactly. + """ + + resource_id: str + endpoints: Mapping[str, ResourceEndpoint] = field(default_factory=dict) + members: tuple[str, ...] = () + + def __post_init__(self) -> None: + resource = RobotResource( + resource_id=self.resource_id, + endpoints=self.endpoints, + members=self.members, + ) + object.__setattr__(self, "endpoints", resource.endpoints) + object.__setattr__(self, "members", resource.members) + + def build(self, robot: Robot) -> RobotResource: + """Build an independently owned resource without assuming robot joints.""" + del robot + return RobotResource( + resource_id=self.resource_id, + endpoints=self.endpoints, + members=self.members, + ) + + def declare(self) -> RobotResource: + """Return an independently owned provider-free resource.""" + return RobotResource( + resource_id=self.resource_id, + endpoints=self.endpoints, + members=self.members, + ) + + +@dataclass(frozen=True, slots=True) +class ControlPartEndpointBinding: + """Profile endpoint backed by one explicit robot control part.""" + + endpoint_id: str + control_part: str + capabilities: frozenset[str] + command_preset: str | None = None + + def __post_init__(self) -> None: + _identifier(self.endpoint_id, field_name="endpoint_id") + _identifier(self.control_part, field_name="control_part") + if isinstance(self.capabilities, (str, bytes)): + raise TypeError("capabilities must be an iterable of identifiers.") + capabilities = frozenset(self.capabilities) + for capability in capabilities: + _identifier(capability, field_name="capabilities") + object.__setattr__(self, "capabilities", capabilities) + _optional_identifier(self.command_preset, field_name="command_preset") + + def build(self, robot: Robot) -> ResourceEndpoint: + """Build a joint-backed endpoint after native control-part validation.""" + _require_control_part_dof(robot, self.control_part) + return ControlPartEndpoint( + control_part=self.control_part, + command_profile=self.command_preset, + capabilities=self.capabilities, + ) + + def declare(self) -> ResourceEndpoint: + """Return the endpoint contract without reading a robot.""" + return ControlPartEndpoint( + control_part=self.control_part, + command_profile=self.command_preset, + capabilities=self.capabilities, + ) + + +@dataclass(frozen=True, slots=True) +class ControlPartResourceBinding: + """Joint-backed robot resource containing control-part endpoints.""" + + resource_id: str + endpoints: tuple[ControlPartEndpointBinding, ...] = () + members: tuple[str, ...] = () + + def __post_init__(self) -> None: + _identifier(self.resource_id, field_name="resource_id") + endpoints = tuple(self.endpoints) + if not all( + type(endpoint) is ControlPartEndpointBinding for endpoint in endpoints + ): + raise TypeError( + "endpoints must contain exact ControlPartEndpointBinding values." + ) + endpoint_ids = [endpoint.endpoint_id for endpoint in endpoints] + if len(set(endpoint_ids)) != len(endpoint_ids): + raise ValueError("endpoint_id values must be unique within a resource.") + object.__setattr__(self, "endpoints", endpoints) + object.__setattr__( + self, + "members", + _identifier_tuple(self.members, field_name="members"), + ) + + def build(self, robot: Robot) -> RobotResource: + """Build a resource containing strictly validated control-part endpoints.""" + endpoints: dict[str, ResourceEndpoint] = {} + for binding in self.endpoints: + endpoint = binding.build(robot) + if type(endpoint) is not ControlPartEndpoint: + raise TypeError( + "ControlPartEndpointBinding.build() must return exactly " + "ControlPartEndpoint." + ) + endpoints[binding.endpoint_id] = endpoint + return RobotResource( + resource_id=self.resource_id, + endpoints=endpoints, + members=self.members, + ) + + def declare(self) -> RobotResource: + """Return the resource graph without reading native control parts.""" + return RobotResource( + resource_id=self.resource_id, + endpoints={ + binding.endpoint_id: binding.declare() for binding in self.endpoints + }, + members=self.members, + ) + + +def _owned_nested_identifier_mapping( + values: Mapping[str, Mapping[str, str]], + *, + field_name: str, +) -> Mapping[str, Mapping[str, str]]: + """Own a strict two-level identifier mapping.""" + if not isinstance(values, Mapping): + raise TypeError(f"{field_name} must be a mapping.") + outer: dict[str, Mapping[str, str]] = {} + for key, nested in values.items(): + _identifier(key, field_name=f"{field_name} keys") + if not isinstance(nested, Mapping): + raise TypeError(f"{field_name}[{key!r}] must be a mapping.") + normalized: dict[str, str] = {} + for nested_key, nested_value in nested.items(): + _identifier(nested_key, field_name=f"{field_name} slot IDs") + _identifier(nested_value, field_name=f"{field_name} resource IDs") + normalized[nested_key] = nested_value + outer[key] = MappingProxyType(normalized) + return MappingProxyType(outer) + + +def _owned_identifier_mapping( + values: Mapping[str, str], + *, + field_name: str, +) -> Mapping[str, str]: + """Own one strict identifier mapping.""" + if not isinstance(values, Mapping): + raise TypeError(f"{field_name} must be a mapping.") + normalized: dict[str, str] = {} + for key, value in values.items(): + _identifier(key, field_name=f"{field_name} keys") + _identifier(value, field_name=f"{field_name} values") + normalized[key] = value + return MappingProxyType(normalized) + + +@dataclass(frozen=True, slots=True) +class SimulationRobotSkillProfileBinding: + """Build a profile from typed resources with strict native validation.""" + + profile_id: str + resources: tuple[SimulationRobotResourceBinding, ...] + command_presets: tuple[ControlPartCommandPreset, ...] = () + defaults: Mapping[str, Mapping[str, str]] = field(default_factory=dict) + presets: tuple[SkillPolicyPreset, ...] = () + default_preset: str | None = None + skill_presets: Mapping[str, str] = field(default_factory=dict) + grounding_providers: Mapping[str, str] = field(default_factory=dict) + + def __post_init__(self) -> None: + _identifier(self.profile_id, field_name="profile_id") + resources = tuple(self.resources) + if not all( + isinstance(resource, SimulationRobotResourceBinding) + for resource in resources + ): + raise TypeError("resources must implement SimulationRobotResourceBinding.") + for resource in resources: + _identifier(resource.resource_id, field_name="resource_id") + _identifier_tuple(resource.members, field_name="resource members") + resource_ids = [resource.resource_id for resource in resources] + if len(set(resource_ids)) != len(resource_ids): + raise ValueError("resource_id values must be unique.") + object.__setattr__(self, "resources", resources) + command_presets = tuple(self.command_presets) + if not all( + type(preset) is ControlPartCommandPreset for preset in command_presets + ): + raise TypeError( + "command_presets must contain exact ControlPartCommandPreset values." + ) + command_preset_ids = [preset.preset_id for preset in command_presets] + if len(set(command_preset_ids)) != len(command_preset_ids): + raise ValueError("command preset IDs must be unique.") + object.__setattr__(self, "command_presets", command_presets) + object.__setattr__( + self, + "defaults", + _owned_nested_identifier_mapping(self.defaults, field_name="defaults"), + ) + presets = tuple(self.presets) + if not all(type(preset) is SkillPolicyPreset for preset in presets): + raise TypeError("presets must contain exact SkillPolicyPreset values.") + preset_ids = [preset.preset_id for preset in presets] + if len(set(preset_ids)) != len(preset_ids): + raise ValueError("policy preset IDs must be unique.") + object.__setattr__(self, "presets", presets) + _optional_identifier(self.default_preset, field_name="default_preset") + object.__setattr__( + self, + "skill_presets", + _owned_identifier_mapping( + self.skill_presets, + field_name="skill_presets", + ), + ) + object.__setattr__( + self, + "grounding_providers", + _owned_identifier_mapping( + self.grounding_providers, + field_name="grounding_providers", + ), + ) + + def build(self, robot: Robot) -> RobotSkillProfile: + """Build the existing profile after validating every typed resource. + + Args: + robot: Live robot selected by the simulation factory. + + Returns: + Reusable, engine-independent robot skill profile. + """ + control_dofs: dict[str, int] = {} + + def require_control_part(control_part: str) -> int: + if control_part not in control_dofs: + control_dofs[control_part] = _require_control_part_dof( + robot, + control_part, + ) + return control_dofs[control_part] + + command_presets = {preset.preset_id: preset for preset in self.command_presets} + command_profiles: dict[str, ControlPartCommandProfile] = {} + for preset in self.command_presets: + command_profiles[preset.preset_id] = preset.build( + control_dof=require_control_part(preset.control_part) + ) + + resources: dict[str, RobotResource] = {} + for resource_binding in self.resources: + resource = resource_binding.build(robot) + if type(resource) is not RobotResource: + raise TypeError( + f"Resource binding {resource_binding.resource_id!r} must build " + "exactly RobotResource." + ) + if resource.resource_id != resource_binding.resource_id: + raise ValueError( + f"Resource binding {resource_binding.resource_id!r} built " + f"resource ID {resource.resource_id!r}." + ) + if resource.members != tuple(resource_binding.members): + raise ValueError( + f"Resource binding {resource_binding.resource_id!r} changed its " + "declared resource members while building." + ) + for endpoint_id, endpoint in resource.endpoints.items(): + if not isinstance(endpoint, ControlPartEndpoint): + continue + require_control_part(endpoint.control_part) + profile_id = ( + endpoint.control_part + if endpoint.command_profile is None + else endpoint.command_profile + ) + command_preset = command_presets.get(profile_id) + if endpoint.command_profile is not None and command_preset is None: + raise KeyError( + f"Endpoint {resource.resource_id!r}.{endpoint_id!r} " + "references unknown command " + f"preset {profile_id!r}." + ) + if ( + command_preset is not None + and command_preset.control_part != endpoint.control_part + ): + raise ValueError( + f"Endpoint {resource.resource_id!r}.{endpoint_id!r} uses " + "control part " + f"{endpoint.control_part!r}, but command preset " + f"{profile_id!r} targets " + f"{command_preset.control_part!r}." + ) + resources[resource.resource_id] = resource + + return RobotSkillProfile( + profile_id=self.profile_id, + resources=resources, + command_profiles=command_profiles, + defaults={ + skill_id: ResourceBinding(resources=bindings) + for skill_id, bindings in self.defaults.items() + }, + presets={preset.preset_id: preset for preset in self.presets}, + default_preset=self.default_preset, + skill_presets=self.skill_presets, + grounding_providers=self.grounding_providers, + ) + + def declare(self) -> RobotSkillProfile: + """Project the complete provider-free robot skill profile.""" + resources: dict[str, RobotResource] = {} + for binding in self.resources: + resource = binding.declare() + if type(resource) is not RobotResource: + raise TypeError( + f"Resource binding {binding.resource_id!r} must declare " + "exactly RobotResource." + ) + if resource.resource_id != binding.resource_id: + raise ValueError( + f"Resource binding {binding.resource_id!r} declared " + f"resource ID {resource.resource_id!r}." + ) + if resource.members != tuple(binding.members): + raise ValueError( + f"Resource binding {binding.resource_id!r} changed its " + "declared resource members." + ) + resources[resource.resource_id] = resource + + command_presets = {preset.preset_id: preset for preset in self.command_presets} + for resource in resources.values(): + for endpoint_id, endpoint in resource.endpoints.items(): + if not isinstance(endpoint, ControlPartEndpoint): + continue + preset_id = endpoint.command_profile + if preset_id is None: + continue + preset = command_presets.get(preset_id) + if preset is None: + raise KeyError( + f"Endpoint {resource.resource_id!r}.{endpoint_id!r} " + f"references unknown command preset {preset_id!r}." + ) + if preset.control_part != endpoint.control_part: + raise ValueError( + f"Endpoint {resource.resource_id!r}.{endpoint_id!r} uses " + f"control part {endpoint.control_part!r}, but command " + f"preset {preset_id!r} targets {preset.control_part!r}." + ) + + return RobotSkillProfile( + profile_id=self.profile_id, + resources=resources, + command_profiles={ + preset.preset_id: preset.declare() for preset in self.command_presets + }, + defaults={ + skill_id: ResourceBinding(resources=bindings) + for skill_id, bindings in self.defaults.items() + }, + presets={preset.preset_id: preset for preset in self.presets}, + default_preset=self.default_preset, + skill_presets=self.skill_presets, + grounding_providers=self.grounding_providers, + ) + + +__all__ = [ + "AntipodalGraspAffordanceBinding", + "ArticulationOperationAffordanceBinding", + "ArticulationOperationTargetBinding", + "ContainerAffordanceBinding", + "ControlPartCommandPreset", + "ControlPartEndpointBinding", + "ControlPartResourceBinding", + "RobotResourceBinding", + "SimulationArticulationBinding", + "SimulationArticulationLinkBinding", + "SimulationRigidObjectBinding", + "SimulationResourceEndpointBinding", + "SimulationRobotResourceBinding", + "SimulationRobotSkillProfileBinding", + "SimulationSceneBinding", + "SupportSurfaceAffordanceBinding", +] diff --git a/embodichain/lab/gym/envs/expert_program/simulation_environment.py b/embodichain/lab/gym/envs/expert_program/simulation_environment.py new file mode 100644 index 000000000..0c3c83c7e --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/simulation_environment.py @@ -0,0 +1,1146 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Production simulation assembly for Gym-backed Expert Programs. + +This module owns the reusable live wiring between declarative simulation +bindings and :class:`ExpertProgramEnvironmentAdapter`. A task declares a +scene binding and a robot profile binding; this factory constructs the motion +generator, atomic-action engine, planning observation port, effect-evidence +providers, and segment-policy port without task-local motion code. + +The resulting runtime is intentionally Gym-only. Its buffered command sink +must remain attached to :class:`AtomicDemoBridge`, which advances the shared +clock only after an ordinary ``env.step()`` consumes a yielded command. It is +therefore not a ``SkillRuntimeProvider`` for synchronous ``AtomicSkills`` use. +""" + +from __future__ import annotations + +from collections.abc import Callable, Iterable, Mapping +from copy import deepcopy +from dataclasses import replace +import math +from typing import Any, Protocol, TYPE_CHECKING + +import torch + +from embodichain.lab.sim.atomic_actions import ( + AtomicActionEngine, + EntityState, + ObservedArticulationJointState, + PlanningContext, + RobotObservation, + SceneProvider, + SceneSnapshot, + TaskState, +) +from embodichain.lab.sim.atomic_actions.bindings import ( + JointPositionTarget, + RuntimeEndpointTarget, +) +from embodichain.lab.sim.atomic_actions.control import ( + GRASP_COMMAND, + OPEN_COMMAND, + ControlPartCommandProfile, + JointPositionCommand, +) +from embodichain.lab.sim.atomic_actions.runtime_commands import ( + JointPositionPayload, + RuntimeCommandFrame, +) +from embodichain.lab.sim.planners import ( + BasePlannerCfg, + MotionGenCfg, + MotionGenerator, + ToppraPlannerCfg, +) +from embodichain.lab.sim.skills.effects import ( + ControlPartEvidenceAddress, +) +from embodichain.lab.sim.skills.evidence import ( + BinaryEffectEvidenceQuery, + BinaryEffectObservation, + ControlPartRobotEvidenceSource, + ControlPartSimulationEvidenceProvider, + EffectEvidenceCollectionContext, + EffectEvidenceProvider, + SceneArticulationEvidenceProvider, +) +from embodichain.lab.sim.skills.parallel_runtime import ( + ParallelCommandSafetyValidator, +) +from embodichain.lab.sim.skills.profiles import ( + RobotSkillProfile, + SkillPolicyPreset, +) +from embodichain.lab.sim.skills.scene import RegistrySceneProvider, SceneRegistry + +from .bridge import ( + AcceptedRuntimeCommandObserver, + EnvironmentStepClock, + GymPlanningObservationProvider, +) +from .catalog import SimulationExpertProgramRegistration +from .environment import ( + ExpertProgramEnvironmentAdapter, + ExpertProgramEnvironmentFactory, + PlanningObservationPort, +) +from .simulation_policies import SimulationSegmentPolicyPort + +if TYPE_CHECKING: + from embodichain.lab.sim.objects import Robot + from embodichain.lab.sim.sim_manager import SimulationManager + + +MotionGeneratorFactory = Callable[[], MotionGenerator] +"""Zero-argument factory that must return one fresh motion generator.""" + + +class ControlCommandStateEvidenceTracker(AcceptedRuntimeCommandObserver): + """Track row-local open/grasp state from accepted semantic commands. + + This is the explicit lightweight evidence option selected for simulations + that do not expose a typed contact sensor. It does not claim physical + contact by itself: the built-in effect contract still conjuncts this + binary command state with live object-to-endpoint pose evidence. State is + updated only after the complete command frame has been encoded and accepted + by :class:`BufferedGymCommandSink`. + + Args: + control_profiles: Exact semantic command profiles installed in the + atomic engine, keyed by concrete control-part name. + env_ids: Stable full simulation batch correlation IDs. + """ + + def __init__( + self, + control_profiles: Mapping[str, ControlPartCommandProfile], + env_ids: torch.Tensor, + ) -> None: + if not isinstance(control_profiles, Mapping): + raise TypeError("control_profiles must be a mapping.") + if ( + not isinstance(env_ids, torch.Tensor) + or env_ids.dtype != torch.long + or env_ids.dim() != 1 + or env_ids.numel() == 0 + ): + raise ValueError( + "env_ids must be a non-empty one-dimensional int64 tensor." + ) + if torch.unique(env_ids).numel() != env_ids.numel(): + raise ValueError("env_ids must be unique.") + + commands: dict[ + str, + tuple[JointPositionCommand, JointPositionCommand], + ] = {} + for control_part, profile in control_profiles.items(): + if type(control_part) is not str or not control_part: + raise ValueError("control_profiles keys must be non-empty strings.") + if not isinstance(profile, ControlPartCommandProfile): + raise TypeError( + "control_profiles values must be ControlPartCommandProfile values." + ) + open_command = profile.commands.get(OPEN_COMMAND) + grasp_command = profile.commands.get(GRASP_COMMAND) + if open_command is None and grasp_command is None: + continue + if not isinstance(open_command, JointPositionCommand) or not isinstance( + grasp_command, + JointPositionCommand, + ): + raise TypeError( + f"Control part {control_part!r} must define both open and grasp " + "as JointPositionCommand values for command-state evidence." + ) + if open_command.equivalent_to(grasp_command): + raise ValueError( + f"Control part {control_part!r} has indistinguishable open and " + "grasp commands." + ) + commands[control_part] = ( + open_command.snapshot(), + grasp_command.snapshot(), + ) + + self._commands = commands + self._env_ids = env_ids.clone() + self._row_by_env_id = { + int(env_id): row + for row, env_id in enumerate(env_ids.detach().cpu().tolist()) + } + batch_size = int(env_ids.numel()) + self._values = { + control_part: torch.zeros( + batch_size, + dtype=torch.bool, + device=env_ids.device, + ) + for control_part in commands + } + self._valid = { + control_part: torch.zeros_like(values) + for control_part, values in self._values.items() + } + + @property + def tracked_control_parts(self) -> tuple[str, ...]: + """Return control parts with exact open/grasp semantic commands.""" + return tuple(self._commands) + + def accepted(self, command: RuntimeCommandFrame) -> None: + """Commit exact open/grasp states for active rows in an accepted frame.""" + if not isinstance(command, RuntimeCommandFrame): + raise TypeError("command must be a RuntimeCommandFrame.") + rows = self._rows(command.env_ids) + for endpoint_command in command.commands: + target = endpoint_command.target + payload = endpoint_command.payload + if not isinstance(target, JointPositionTarget) or not isinstance( + payload, + JointPositionPayload, + ): + continue + semantic_commands = self._commands.get(target.control_part) + if semantic_commands is None: + continue + open_command, grasp_command = semantic_commands + open_positions = open_command.resolve( + num_envs=command.batch_size, + control_dof=payload.dof, + device=payload.device, + dtype=payload.positions.dtype, + ) + grasp_positions = grasp_command.resolve( + num_envs=command.batch_size, + control_dof=payload.dof, + device=payload.device, + dtype=payload.positions.dtype, + ) + is_open = torch.isclose( + payload.positions, + open_positions, + rtol=0.0, + atol=1.0e-7, + ).all(dim=1) + is_grasp = torch.isclose( + payload.positions, + grasp_positions, + rtol=0.0, + atol=1.0e-7, + ).all(dim=1) + if bool((is_open & is_grasp).any().item()): + raise ValueError( + "An accepted row matched both open and grasp commands." + ) + recognized = command.active_mask & (is_open | is_grasp) + if not bool(recognized.any().item()): + continue + destination_rows = torch.tensor( + rows, + dtype=torch.long, + device=self._env_ids.device, + ) + selected_rows = destination_rows[recognized.to(destination_rows.device)] + values = self._values[target.control_part] + valid = self._valid[target.control_part] + values[selected_rows] = is_grasp[recognized].to(values.device) + valid[selected_rows] = True + + def cancelled(self, targets: tuple[RuntimeEndpointTarget, ...]) -> None: + """Invalidate every row owned by cancelled control-part targets.""" + if not isinstance(targets, tuple) or not all( + isinstance(target, RuntimeEndpointTarget) for target in targets + ): + raise TypeError("targets must contain RuntimeEndpointTarget values.") + for target in targets: + if isinstance(target, JointPositionTarget): + self._clear_control_part(target.control_part) + + def discarded(self) -> None: + """Invalidate all command-derived state after a fail-closed discard.""" + for control_part in self._commands: + self._clear_control_part(control_part) + + def observe( + self, + query: BinaryEffectEvidenceQuery, + context: EffectEvidenceCollectionContext, + ) -> BinaryEffectObservation: + """Return selected command-state rows for one typed binary query.""" + if type(query) is not BinaryEffectEvidenceQuery: + raise TypeError("query must be exactly BinaryEffectEvidenceQuery.") + if type(context) is not EffectEvidenceCollectionContext: + raise TypeError("context must be exactly EffectEvidenceCollectionContext.") + address = query.source.address + if type(address) is not ControlPartEvidenceAddress: + raise TypeError( + "Command-state evidence requires ControlPartEvidenceAddress." + ) + rows = self._rows(context.env_ids) + values = self._values.get(address.control_part) + valid = self._valid.get(address.control_part) + if values is None or valid is None: + missing = torch.zeros( + context.env_ids.numel(), + dtype=torch.bool, + device=context.env_ids.device, + ) + return BinaryEffectObservation( + values=missing, + valid=missing, + acquisition_errors=( + f"Control part {address.control_part!r} has no exact open/grasp " + "command-state profile.", + ) + * int(context.env_ids.numel()), + ) + indices = torch.tensor(rows, dtype=torch.long, device=values.device) + selected_values = values.index_select(0, indices).to(context.env_ids.device) + selected_valid = valid.index_select(0, indices).to(context.env_ids.device) + errors = tuple( + ( + None + if bool(row_valid) + else "No accepted open/grasp command has established this row's state." + ) + for row_valid in selected_valid.detach().cpu().tolist() + ) + return BinaryEffectObservation( + values=selected_values, + valid=selected_valid, + acquisition_errors=errors, + ) + + def __call__( + self, + query: BinaryEffectEvidenceQuery, + context: EffectEvidenceCollectionContext, + ) -> BinaryEffectObservation: + """Delegate callback use to :meth:`observe`.""" + return self.observe(query, context) + + def _rows(self, env_ids: torch.Tensor) -> tuple[int, ...]: + """Resolve stable correlation IDs to full simulation row indices.""" + if ( + not isinstance(env_ids, torch.Tensor) + or env_ids.dtype != torch.long + or env_ids.dim() != 1 + or env_ids.numel() == 0 + ): + raise ValueError( + "env_ids must be a non-empty one-dimensional int64 tensor." + ) + if env_ids.device != self._env_ids.device: + raise ValueError("env_ids must share the tracker device.") + if torch.unique(env_ids).numel() != env_ids.numel(): + raise ValueError("env_ids must be unique.") + try: + return tuple( + self._row_by_env_id[int(env_id)] + for env_id in env_ids.detach().cpu().tolist() + ) + except KeyError as exc: + raise ValueError( + f"Environment ID {int(exc.args[0])} is absent from tracker env_ids." + ) from exc + + def _clear_control_part(self, control_part: str) -> None: + """Fail-closed reset one tracked control part when present.""" + values = self._values.get(control_part) + valid = self._valid.get(control_part) + if values is not None and valid is not None: + values.zero_() + valid.zero_() + + +class SimulationExpertProgramEnvironment(Protocol): + """Minimal Gym environment surface used by the simulation factory.""" + + sim: SimulationManager + robot: Robot + + @property + def step_dt(self) -> float: + """Return the authoritative Gym control cadence in seconds.""" + + +def _positive_finite(value: float, *, field_name: str) -> float: + """Validate one positive finite real number.""" + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise TypeError(f"{field_name} must be a real number.") + normalized = float(value) + if not math.isfinite(normalized) or normalized <= 0.0: + raise ValueError(f"{field_name} must be finite and positive.") + return normalized + + +def _non_negative_finite(value: float, *, field_name: str) -> float: + """Validate one non-negative finite real number.""" + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise TypeError(f"{field_name} must be a real number.") + normalized = float(value) + if not math.isfinite(normalized) or normalized < 0.0: + raise ValueError(f"{field_name} must be finite and non-negative.") + return normalized + + +def _robot_uid(robot: Robot) -> str: + """Return one strict live robot UID.""" + uid = getattr(robot, "uid", None) + if type(uid) is not str or not uid or uid != uid.strip(): + raise ValueError( + "robot.uid must be a non-empty string without outer whitespace." + ) + return uid + + +def _full_robot_tensor( + robot: Robot, + getter_name: str, + *, + required: bool, + reference: torch.Tensor | None = None, +) -> torch.Tensor | None: + """Read and validate one full-robot floating state tensor.""" + getter = getattr(robot, getter_name, None) + if not callable(getter): + if required: + raise TypeError(f"robot must provide {getter_name}().") + return None + value = getter() + if not isinstance(value, torch.Tensor): + raise TypeError(f"robot.{getter_name}() must return a torch.Tensor.") + if not value.is_floating_point() or value.dim() != 2: + raise ValueError( + f"robot.{getter_name}() must return floating shape (B, robot_dof)." + ) + if value.shape[0] == 0 or value.shape[1] == 0: + raise ValueError(f"robot.{getter_name}() dimensions must be non-zero.") + if reference is not None and ( + value.shape != reference.shape or value.device != reference.device + ): + raise ValueError( + f"robot.{getter_name}() must match robot.get_qpos() shape and device." + ) + if not bool(torch.isfinite(value).all().item()): + raise ValueError(f"robot.{getter_name}() must contain only finite values.") + return value.clone() + + +class SharedTickSceneProvider(SceneProvider): + """Share one immutable scene snapshot across consumers in the same tick. + + ``RegistrySceneProvider`` is stateful: every call observes native entities + and updates material-change baselines. Planning observations and multiple + evidence providers can legitimately request the same timestamp. This + wrapper always delegates one full-batch request per tick, then returns the + exact snapshot or an owned ordered-row projection to later consumers. + """ + + def __init__( + self, + delegate: RegistrySceneProvider, + full_env_ids: torch.Tensor, + ) -> None: + if type(delegate) is not RegistrySceneProvider: + raise TypeError("delegate must be exactly RegistrySceneProvider.") + if ( + not isinstance(full_env_ids, torch.Tensor) + or full_env_ids.dtype != torch.long + or full_env_ids.dim() != 1 + or full_env_ids.numel() == 0 + ): + raise ValueError("full_env_ids must be a non-empty 1D int64 tensor.") + if torch.unique(full_env_ids).numel() != full_env_ids.numel(): + raise ValueError("full_env_ids must be unique.") + self._delegate = delegate + self._full_env_ids = full_env_ids.clone() + self._row_by_env_id = { + int(env_id): row + for row, env_id in enumerate(full_env_ids.detach().cpu().tolist()) + } + self._timestamp: float | None = None + self._snapshot: SceneSnapshot | None = None + + @property + def delegate(self) -> RegistrySceneProvider: + """Return the authoritative stateful registry provider.""" + return self._delegate + + @property + def collision_entity_ids(self) -> tuple[str, ...]: + """Return canonical dynamic collision IDs from the delegate.""" + return self._delegate.collision_entity_ids + + @property + def full_env_ids(self) -> torch.Tensor: + """Return the authoritative full simulation batch order.""" + return self._full_env_ids.clone() + + def snapshot( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> SceneSnapshot: + """Return the single shared snapshot for ``timestamp`` and ``env_ids``.""" + if isinstance(timestamp, bool) or not isinstance(timestamp, (int, float)): + raise TypeError("timestamp must be a real number.") + normalized_timestamp = float(timestamp) + if not math.isfinite(normalized_timestamp) or normalized_timestamp < 0.0: + raise ValueError("timestamp must be finite and non-negative.") + if ( + not isinstance(env_ids, torch.Tensor) + or env_ids.dtype != torch.long + or env_ids.dim() != 1 + or env_ids.numel() == 0 + ): + raise ValueError("env_ids must be a non-empty 1D int64 tensor.") + if torch.unique(env_ids).numel() != env_ids.numel(): + raise ValueError("env_ids must be unique.") + if env_ids.device != self._full_env_ids.device: + raise ValueError("env_ids must share the full simulation batch device.") + try: + rows = tuple( + self._row_by_env_id[int(env_id)] + for env_id in env_ids.detach().cpu().tolist() + ) + except KeyError as exc: + raise ValueError( + f"Environment ID {int(exc.args[0])} is absent from full_env_ids." + ) from exc + + if self._timestamp is not None: + if normalized_timestamp < self._timestamp: + raise ValueError("Shared scene snapshot timestamps must be monotonic.") + if normalized_timestamp == self._timestamp: + assert self._snapshot is not None + return self._select_rows(self._snapshot, rows) + + snapshot = self._delegate.snapshot( + timestamp=normalized_timestamp, + env_ids=self._full_env_ids.clone(), + ) + if not isinstance(snapshot, SceneSnapshot): + raise TypeError( + "RegistrySceneProvider.snapshot() must return SceneSnapshot." + ) + if snapshot.timestamp != normalized_timestamp: + raise ValueError("Scene snapshot timestamp must match the requested tick.") + self._timestamp = normalized_timestamp + self._snapshot = snapshot + return self._select_rows(snapshot, rows) + + def _select_rows( + self, + snapshot: SceneSnapshot, + rows: tuple[int, ...], + ) -> SceneSnapshot: + """Project one cached full-batch snapshot to an ordered row subset.""" + full_size = int(self._full_env_ids.numel()) + if rows == tuple(range(full_size)): + return snapshot + entities: dict[str, EntityState] = {} + for entity_id, state in snapshot.entities.items(): + pose = state.pose + if pose.dim() == 3: + if pose.shape[0] != full_size: + raise ValueError( + f"Scene entity {entity_id!r} batch does not match " + "full_env_ids." + ) + index = torch.tensor(rows, dtype=torch.long, device=pose.device) + pose = pose.index_select(0, index) + entities[entity_id] = EntityState(pose, confidence=state.confidence) + + articulation_joints: dict[tuple[str, str], ObservedArticulationJointState] = {} + for address, state in snapshot.articulation_joints.items(): + position = state.position + valid = state.valid_mask + if position.dim() == 2: + if position.shape[0] != full_size: + raise ValueError( + f"Scene articulation joint {address!r} batch does not " + "match full_env_ids." + ) + index = torch.tensor(rows, dtype=torch.long, device=position.device) + position = position.index_select(0, index) + if valid is not None: + valid = valid.index_select(0, index.to(valid.device)) + articulation_joints[address] = ObservedArticulationJointState( + position, + valid, + ) + + revisions = snapshot.collision_world_revisions(full_size) + return SceneSnapshot( + timestamp=snapshot.timestamp, + version=snapshot.version, + entities=entities, + collision_world_revision=tuple(revisions[row] for row in rows), + collision_entity_ids=snapshot.collision_entity_ids, + articulation_joints=articulation_joints, + ) + + +class SimulationPlanningObservationProvider(GymPlanningObservationProvider): + """Gym planning observations backed by live robot and shared scene state.""" + + def __init__( + self, + robot: Robot, + scene_provider: SharedTickSceneProvider, + clock: EnvironmentStepClock, + env_ids: torch.Tensor, + command_state_tracker: ControlCommandStateEvidenceTracker, + *, + owner_token: object, + ) -> None: + if type(scene_provider) is not SharedTickSceneProvider: + raise TypeError("scene_provider must be exactly SharedTickSceneProvider.") + if type(clock) is not EnvironmentStepClock: + raise TypeError("clock must be exactly EnvironmentStepClock.") + qpos = _full_robot_tensor(robot, "get_qpos", required=True) + assert qpos is not None + if ( + not isinstance(env_ids, torch.Tensor) + or env_ids.dtype != torch.long + or env_ids.shape != (qpos.shape[0],) + ): + raise ValueError("env_ids must be int64 with one ID per robot row.") + if env_ids.device != qpos.device: + raise ValueError("env_ids and robot qpos must share a device.") + if torch.unique(env_ids).numel() != env_ids.numel(): + raise ValueError("env_ids must be unique.") + if type(command_state_tracker) is not ControlCommandStateEvidenceTracker: + raise TypeError( + "command_state_tracker must be exactly " + "ControlCommandStateEvidenceTracker." + ) + self._robot = robot + self._scene_provider = scene_provider + self._clock = clock + self._env_ids = env_ids.clone() + self._command_state_tracker = command_state_tracker + self._owner_token = owner_token + super().__init__(self._capture) + + @property + def scene_provider(self) -> SharedTickSceneProvider: + """Return the snapshot-sharing scene provider used by evidence ports.""" + return self._scene_provider + + @property + def env_ids(self) -> torch.Tensor: + """Return stable ordered simulation row IDs.""" + return self._env_ids.clone() + + @property + def command_state_tracker(self) -> ControlCommandStateEvidenceTracker: + """Return the runtime-local accepted-command evidence owner.""" + return self._command_state_tracker + + def is_owned_by(self, owner_token: object) -> bool: + """Return whether this provider belongs to one factory instance.""" + return self._owner_token is owner_token + + def _capture(self, task_state: TaskState) -> PlanningContext: + """Capture one synchronized robot and scene observation.""" + qpos = _full_robot_tensor(self._robot, "get_qpos", required=True) + assert qpos is not None + if ( + qpos.shape[0] != self._env_ids.numel() + or qpos.device != self._env_ids.device + ): + raise ValueError("Robot batch shape or device changed after assembly.") + qvel = _full_robot_tensor( + self._robot, + "get_qvel", + required=False, + reference=qpos, + ) + if qvel is None: + qvel = torch.zeros_like(qpos) + qeffort = _full_robot_tensor( + self._robot, + "get_qf", + required=False, + reference=qpos, + ) + timestamp = self._clock.now() + scene = self._scene_provider.snapshot( + timestamp=timestamp, + env_ids=self._env_ids.clone(), + ) + return PlanningContext( + robot=RobotObservation( + timestamp=timestamp, + qpos=qpos, + qvel=qvel, + qeffort=qeffort, + ), + task=task_state, + scene=scene, + env_ids=self._env_ids, + control_dt=self._clock.step_dt, + ) + + +class SimulationExpertProgramFactory(ExpertProgramEnvironmentFactory): + """Build every live Expert Program component from explicit declarations. + + Args: + simulation: Exact live simulation that owns ``robot`` and scene UIDs. + robot: Exact robot selected for planning and evidence acquisition. + registration: Exact task-owned static and live integration declaration. + step_dt: Authoritative Gym control cadence. + planner_cfg: Explicit planner configuration. ``None`` selects TOPPRA + for ``robot.uid``. + motion_generator_factory: Optional fresh-generator factory. It is + mutually exclusive with ``planner_cfg`` and intended for custom + planners and isolated tests. + translation_threshold: Material scene translation threshold. + rotation_threshold: Material scene rotation threshold. + + Every runner policy is rebuilt with ``minimum_cycle_time == step_dt``. The + Gym cadence is also carried by each ``PlanningContext``; motion policy stays + provider-free and does not own environment timing. + """ + + def __init__( + self, + simulation: SimulationManager, + robot: Robot, + registration: SimulationExpertProgramRegistration, + *, + step_dt: float, + planner_cfg: BasePlannerCfg | None = None, + motion_generator_factory: MotionGeneratorFactory | None = None, + translation_threshold: float = 1.0e-4, + rotation_threshold: float = 1.0e-3, + ) -> None: + if type(registration) is not SimulationExpertProgramRegistration: + raise TypeError( + "registration must be exactly SimulationExpertProgramRegistration." + ) + registration.assert_unchanged() + if planner_cfg is not None and motion_generator_factory is not None: + raise ValueError( + "planner_cfg and motion_generator_factory are mutually exclusive." + ) + if planner_cfg is not None and not isinstance(planner_cfg, BasePlannerCfg): + raise TypeError("planner_cfg must be a BasePlannerCfg or None.") + if motion_generator_factory is not None and not callable( + motion_generator_factory + ): + raise TypeError("motion_generator_factory must be callable or None.") + robot_uid = _robot_uid(robot) + get_robot = getattr(simulation, "get_robot", None) + if not callable(get_robot): + raise TypeError("simulation must provide get_robot().") + if get_robot(robot_uid) is not robot: + raise ValueError( + f"simulation.get_robot({robot_uid!r}) must return the exact " + "selected robot." + ) + selected_planner_cfg = deepcopy(planner_cfg) + if ( + selected_planner_cfg is not None + and selected_planner_cfg.robot_uid != robot_uid + ): + raise ValueError( + f"planner_cfg.robot_uid must equal selected robot UID {robot_uid!r}." + ) + + self._simulation = simulation + self._robot = robot + self._registration = registration + self._scene_binding = registration.scene_binding + self._robot_profile_binding = registration.robot_profile_binding + self._step_dt = _positive_finite(step_dt, field_name="step_dt") + self._planner_cfg = selected_planner_cfg + self._motion_generator_factory = motion_generator_factory + self._endpoint_adapters = dict(registration.endpoint_adapter_map) + self._translation_threshold = _non_negative_finite( + translation_threshold, + field_name="translation_threshold", + ) + self._rotation_threshold = _non_negative_finite( + rotation_threshold, + field_name="rotation_threshold", + ) + self._owner_token = object() + + qpos = _full_robot_tensor(robot, "get_qpos", required=True) + assert qpos is not None + self._env_ids = torch.arange( + qpos.shape[0], + dtype=torch.long, + device=qpos.device, + ) + self._segment_policy_port = SimulationSegmentPolicyPort( + simulation, + robot, + registration.scene_binding, + settle_presets=registration.settle_presets, + env_ids=self._env_ids, + ) + + @classmethod + def from_environment( + cls, + environment: SimulationExpertProgramEnvironment, + *, + registration: SimulationExpertProgramRegistration, + planner_cfg: BasePlannerCfg | None = None, + motion_generator_factory: MotionGeneratorFactory | None = None, + translation_threshold: float = 1.0e-4, + rotation_threshold: float = 1.0e-3, + ) -> SimulationExpertProgramFactory: + """Create a factory from the explicit standard Gym environment surface.""" + simulation = getattr(environment, "sim", None) + robot = getattr(environment, "robot", None) + try: + step_dt = environment.step_dt + except AttributeError as exc: + raise TypeError("environment must expose step_dt.") from exc + if simulation is None or robot is None: + raise TypeError("environment must expose non-None sim and robot values.") + return cls( + simulation, + robot, + registration, + step_dt=step_dt, + planner_cfg=planner_cfg, + motion_generator_factory=motion_generator_factory, + translation_threshold=translation_threshold, + rotation_threshold=rotation_threshold, + ) + + @property + def scene_registry_id(self) -> str: + """Return the exact configured scene-registry ID.""" + return self._scene_binding.registry_id + + @property + def robot_profile_id(self) -> str: + """Return the exact configured robot-profile ID.""" + return self._robot_profile_binding.profile_id + + @property + def step_dt(self) -> float: + """Return the authoritative Gym control cadence.""" + return self._step_dt + + @property + def expert_program_registration(self) -> SimulationExpertProgramRegistration: + """Return the exact standard registration owned by this factory.""" + return self._registration + + @property + def segment_policy_port(self) -> SimulationSegmentPolicyPort: + """Return the shared simulation post-policy and validator port.""" + return self._segment_policy_port + + def registration_owned_segment_policy_ports( + self, + ) -> tuple[SimulationSegmentPolicyPort, SimulationSegmentPolicyPort]: + """Return the exact factory-owned segment policy ports.""" + return self._segment_policy_port, self._segment_policy_port + + def create_scene_registry(self) -> SceneRegistry: + """Build one fresh authoritative registry from explicit bindings.""" + registry = self._scene_binding.build(self._simulation) + self._registration.validate_scene_registry(registry) + return registry + + def create_robot_skill_profile(self) -> RobotSkillProfile: + """Build a profile whose runner policy uses Gym cadence.""" + profile = self._robot_profile_binding.build(self._robot) + aligned_presets = { + preset_id: SkillPolicyPreset( + preset_id=preset.preset_id, + schema_version=preset.schema_version, + motion_policy=preset.motion_policy, + tracking_policy=preset.tracking_policy, + recovery_policy=preset.recovery_policy, + workflow_recovery_policy=preset.workflow_recovery_policy, + runner_cfg=replace( + preset.runner_cfg, + minimum_cycle_time=self._step_dt, + ), + effect_monitors=preset.effect_monitors, + action_option_templates=preset.action_option_templates, + required_planner=preset.required_planner, + ) + for preset_id, preset in profile.presets.items() + } + aligned = replace(profile, presets=aligned_presets) + if any( + preset.runner_cfg.minimum_cycle_time != self._step_dt + for preset in aligned.presets.values() + ): + raise AssertionError("Profile runner policies were not cadence-aligned.") + self._registration.validate_robot_profile( + aligned, + step_dt=self._step_dt, + ) + return aligned + + def create_atomic_action_engine( + self, + profile: RobotSkillProfile, + ) -> AtomicActionEngine: + """Create a fresh engine around the selected planner and exact profile.""" + if not isinstance(profile, RobotSkillProfile): + raise TypeError("profile must be a RobotSkillProfile.") + if profile.profile_id != self.robot_profile_id: + raise ValueError( + f"profile ID must be {self.robot_profile_id!r}, got " + f"{profile.profile_id!r}." + ) + motion_generator = self._create_motion_generator() + if motion_generator.robot is not self._robot: + raise ValueError( + "Motion generator must own the exact robot selected by the factory." + ) + engine = AtomicActionEngine( + motion_generator, + skill_profile=profile, + endpoint_adapters=self._endpoint_adapters, + ) + self._registration.validate_engine(engine) + return engine + + def create_planning_observation_provider( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + clock: EnvironmentStepClock, + ) -> PlanningObservationPort: + """Create one planning port and planner-validated shared scene provider.""" + if type(scene_registry) is not SceneRegistry: + raise TypeError("scene_registry must be exactly SceneRegistry.") + if not isinstance(engine, AtomicActionEngine): + raise TypeError("engine must be an AtomicActionEngine.") + if engine.robot is not self._robot: + raise ValueError("engine must own the exact factory robot.") + if type(clock) is not EnvironmentStepClock: + raise TypeError("clock must be exactly EnvironmentStepClock.") + if clock.step_dt != self._step_dt: + raise ValueError("clock.step_dt must equal the factory Gym cadence.") + provider = scene_registry.make_planning_scene_provider( + engine.motion_generator, + batch_size=int(self._env_ids.numel()), + translation_threshold=self._translation_threshold, + rotation_threshold=self._rotation_threshold, + ) + shared = SharedTickSceneProvider(provider, self._env_ids) + command_state_tracker = ControlCommandStateEvidenceTracker( + engine.control_profiles, + self._env_ids, + ) + return SimulationPlanningObservationProvider( + self._robot, + shared, + clock, + self._env_ids, + command_state_tracker, + owner_token=self._owner_token, + ) + + def create_effect_evidence_providers( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + observation_provider: PlanningObservationPort, + ) -> Iterable[EffectEvidenceProvider]: + """Create built-in control-part and articulation evidence providers.""" + if type(scene_registry) is not SceneRegistry: + raise TypeError("scene_registry must be exactly SceneRegistry.") + if not isinstance(engine, AtomicActionEngine): + raise TypeError("engine must be an AtomicActionEngine.") + if engine.robot is not self._robot: + raise ValueError("engine must own the exact factory robot.") + if type(observation_provider) is not SimulationPlanningObservationProvider: + raise TypeError( + "observation_provider must be exactly " + "SimulationPlanningObservationProvider." + ) + if not observation_provider.is_owned_by(self._owner_token): + raise ValueError("observation_provider belongs to another factory.") + scene_provider = observation_provider.scene_provider + command_state_tracker = observation_provider.command_state_tracker + providers: list[EffectEvidenceProvider] = [] + if isinstance(self._robot, ControlPartRobotEvidenceSource): + providers.append( + ControlPartSimulationEvidenceProvider( + self._robot, + scene_provider=scene_provider, + contact_observer=command_state_tracker, + constraint_observer=command_state_tracker, + ) + ) + providers.append( + SceneArticulationEvidenceProvider(scene_provider=scene_provider) + ) + return tuple(providers) + + def create_accepted_runtime_command_observer( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + observation_provider: PlanningObservationPort, + ) -> AcceptedRuntimeCommandObserver: + """Return the tracker already shared with this runtime's evidence ports.""" + if type(scene_registry) is not SceneRegistry: + raise TypeError("scene_registry must be exactly SceneRegistry.") + if not isinstance(engine, AtomicActionEngine): + raise TypeError("engine must be an AtomicActionEngine.") + if engine.robot is not self._robot: + raise ValueError("engine must own the exact factory robot.") + if type(observation_provider) is not SimulationPlanningObservationProvider: + raise TypeError( + "observation_provider must be exactly " + "SimulationPlanningObservationProvider." + ) + if not observation_provider.is_owned_by(self._owner_token): + raise ValueError("observation_provider belongs to another factory.") + return observation_provider.command_state_tracker + + def create_parallel_command_safety_validator( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + observation_provider: PlanningObservationPort, + ) -> ParallelCommandSafetyValidator: + """Create one fresh live gate from the registration-owned factory.""" + self._registration.assert_unchanged() + if type(scene_registry) is not SceneRegistry: + raise TypeError("scene_registry must be exactly SceneRegistry.") + if ( + not isinstance(engine, AtomicActionEngine) + or engine.robot is not self._robot + ): + raise ValueError("engine must own the exact factory robot.") + if type(observation_provider) is not SimulationPlanningObservationProvider: + raise TypeError( + "observation_provider must be exactly " + "SimulationPlanningObservationProvider." + ) + if not observation_provider.is_owned_by(self._owner_token): + raise ValueError("observation_provider belongs to another factory.") + if self._registration.parallel_safety_factory is None: + raise RuntimeError("No parallel_safety_factory is registered.") + validator = self._registration.create_parallel_safety_validator( + simulation=self._simulation, + robot=self._robot, + scene_registry=scene_registry, + engine=engine, + ) + if not isinstance(validator, ParallelCommandSafetyValidator): + raise TypeError( + "ParallelCommandSafetyValidatorFactory.create() must return a " + "ParallelCommandSafetyValidator." + ) + return validator + + def create_adapter(self) -> ExpertProgramEnvironmentAdapter: + """Create the exact Gym adapter with shared simulation policy ports.""" + self._registration.assert_unchanged() + return ExpertProgramEnvironmentAdapter( + self, + step_dt=self._step_dt, + registration=self._registration, + ) + + def _create_motion_generator(self) -> MotionGenerator: + """Create and validate one exact motion generator.""" + if self._motion_generator_factory is not None: + generator = self._motion_generator_factory() + else: + planner_cfg = ( + ToppraPlannerCfg(robot_uid=_robot_uid(self._robot)) + if self._planner_cfg is None + else deepcopy(self._planner_cfg) + ) + generator = MotionGenerator(MotionGenCfg(planner_cfg=planner_cfg)) + if not isinstance(generator, MotionGenerator): + raise TypeError( + "motion_generator_factory must return a MotionGenerator instance." + ) + return generator + + +def create_simulation_expert_program_adapter( + environment: SimulationExpertProgramEnvironment, + *, + registration: SimulationExpertProgramRegistration, + planner_cfg: BasePlannerCfg | None = None, + motion_generator_factory: MotionGeneratorFactory | None = None, + translation_threshold: float = 1.0e-4, + rotation_threshold: float = 1.0e-3, +) -> ExpertProgramEnvironmentAdapter: + """Create a complete production adapter from one standard Gym environment. + + This is the intended task-side one-line integration. Relation-target + grounders and embodiment-owned handover pose providers come exclusively + from ``registration``, so the statically fingerprinted objects are the exact + objects consumed by the runtime compiler. Calls that require an unregistered + provider remain fail-closed during program preflight. Endpoint adapters, + runtime transports, and parallel safety are also registration-owned; the + standard helper exposes no live extension override surface. + + Args: + environment: Standard Gym simulation environment exposing ``sim``, + ``robot``, and ``step_dt``. + registration: Exact task registration used during static config loading. + planner_cfg: Optional planner configuration owned by the factory. + motion_generator_factory: Optional factory for one fresh motion generator. + translation_threshold: Scene translation revision threshold. + rotation_threshold: Scene rotation revision threshold. + + Returns: + Complete production Expert Program environment adapter. + """ + factory = SimulationExpertProgramFactory.from_environment( + environment, + registration=registration, + planner_cfg=planner_cfg, + motion_generator_factory=motion_generator_factory, + translation_threshold=translation_threshold, + rotation_threshold=rotation_threshold, + ) + return factory.create_adapter() + + +__all__ = [ + "ControlCommandStateEvidenceTracker", + "MotionGeneratorFactory", + "SharedTickSceneProvider", + "SimulationExpertProgramEnvironment", + "SimulationExpertProgramFactory", + "SimulationPlanningObservationProvider", + "create_simulation_expert_program_adapter", +] 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..a346861d6 --- /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( + SemanticPose( + position=self.middle_position, + quaternion_wxyz=self.middle_quaternion_wxyz, + ) + ), + final=SemanticObjectTarget( + SemanticPose( + position=self.final_position, + quaternion_wxyz=self.final_quaternion_wxyz, + ) + ), + ) + + +__all__ = ["ConfiguredHandOverPoseProvider"] diff --git a/embodichain/lab/gym/envs/expert_program/simulation_parallel_safety.py b/embodichain/lab/gym/envs/expert_program/simulation_parallel_safety.py new file mode 100644 index 000000000..97317e3f6 --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/simulation_parallel_safety.py @@ -0,0 +1,356 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""cuRobo-backed physical safety gate for synchronized simulation commands.""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import dataclass +import math +from typing import ClassVar + +import torch + +from embodichain.lab.sim.atomic_actions import ( + AtomicActionEngine, + JointPositionPayload, + JointPositionTarget, + RuntimeCommandFrame, +) +from embodichain.lab.sim.planners import CuroboPlanner, MotionGenerator +from embodichain.lab.sim.skills import ( + ParallelSafetyError, + RegistrySceneProvider, + SceneRegistry, +) + + +def _identifier(value: object, *, field_name: str) -> str: + """Validate one exact identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +@dataclass(frozen=True, slots=True) +class CuroboParallelSafetyValidatorFactory: + """Create exact-sample collision gates for one aggregate control part. + + ``validation_control_part`` must contain every joint that any parallel + branch can command. A common dual-arm example is ``"dual_arm"``. The + cuRobo model for that part remains the authoritative bounds, self-collision, + and world-collision model. + + Args: + validation_control_part: Aggregate robot control part containing every + joint that a parallel lane may command. + max_joint_step: Maximum absolute joint displacement between collision + samples in radians or the joint's native linear unit. + max_interpolation_samples: Fail-closed upper bound on samples per frame. + """ + + validator_id: ClassVar[str] = "builtin.simulation.curobo_parallel_safety" + revision: ClassVar[str] = "1" + supported_transport_ids: ClassVar[frozenset[str]] = frozenset( + {JointPositionTarget.TRANSPORT_ID} + ) + + validation_control_part: str + max_joint_step: float = 0.025 + max_interpolation_samples: int = 256 + + def __post_init__(self) -> None: + _identifier( + self.validation_control_part, + field_name="validation_control_part", + ) + if isinstance(self.max_joint_step, bool) or not isinstance( + self.max_joint_step, + (int, float), + ): + raise TypeError("max_joint_step must be a real number.") + normalized_step = float(self.max_joint_step) + if not math.isfinite(normalized_step) or normalized_step <= 0.0: + raise ValueError("max_joint_step must be finite and positive.") + object.__setattr__(self, "max_joint_step", normalized_step) + if ( + type(self.max_interpolation_samples) is not int + or self.max_interpolation_samples < 2 + or self.max_interpolation_samples > 4096 + ): + raise ValueError( + "max_interpolation_samples must be an integer in [2, 4096]." + ) + + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + ) -> CuroboParallelCommandSafetyValidator: + """Create one fresh validator bound to the assembled live runtime.""" + del simulation + if type(scene_registry) is not SceneRegistry: + raise TypeError("scene_registry must be exactly SceneRegistry.") + if not isinstance(engine, AtomicActionEngine): + raise TypeError("engine must be an AtomicActionEngine.") + if engine.robot is not robot: + raise ValueError("engine and factory must reference the exact same robot.") + return CuroboParallelCommandSafetyValidator( + robot=robot, + motion_generator=engine.motion_generator, + scene_registry=scene_registry, + validation_control_part=self.validation_control_part, + max_joint_step=self.max_joint_step, + max_interpolation_samples=self.max_interpolation_samples, + ) + + +class CuroboParallelCommandSafetyValidator: + """Validate the exact synchronized joint segment before transport dispatch. + + Args: + robot: Live robot supplying measured joint state and control-part IDs. + motion_generator: Runtime motion generator backed by exact cuRobo. + scene_registry: Authoritative live collision-scene registry. + validation_control_part: Aggregate control part for merged commands. + max_joint_step: Maximum displacement between collision samples. + max_interpolation_samples: Fail-closed sample-count upper bound. + """ + + def __init__( + self, + *, + robot: object, + motion_generator: MotionGenerator, + scene_registry: SceneRegistry, + validation_control_part: str, + max_joint_step: float, + max_interpolation_samples: int, + ) -> None: + if type(scene_registry) is not SceneRegistry: + raise TypeError("scene_registry must be exactly SceneRegistry.") + if not isinstance(motion_generator, MotionGenerator): + raise TypeError("motion_generator must be a MotionGenerator.") + if type(motion_generator.planner) is not CuroboPlanner: + raise TypeError( + "CuroboParallelCommandSafetyValidator requires the active " + "CuroboPlanner backend." + ) + if not motion_generator.supports_joint_trajectory_validation: + raise ValueError( + "The active motion generator does not validate exact joint " + "trajectories." + ) + get_joint_ids = getattr(robot, "get_joint_ids", None) + if not callable(get_joint_ids): + raise TypeError("robot must provide get_joint_ids().") + joint_ids = tuple(get_joint_ids(name=validation_control_part)) + if not joint_ids or not all( + type(joint_id) is int and joint_id >= 0 for joint_id in joint_ids + ): + raise ValueError( + "The validation control part must resolve non-negative joint IDs." + ) + if len(set(joint_ids)) != len(joint_ids): + raise ValueError("The validation control part joint IDs must be unique.") + self._robot = robot + self._motion_generator = motion_generator + self._scene_registry = scene_registry + self._validation_control_part = validation_control_part + self._validation_joint_ids = joint_ids + self._local_joint_columns = { + joint_id: index for index, joint_id in enumerate(joint_ids) + } + self._max_joint_step = max_joint_step + self._max_interpolation_samples = max_interpolation_samples + self._scene_provider: RegistrySceneProvider | None = None + self._scene_timestamp = 0.0 + + def validate( + self, + *, + branch_frames: Mapping[str, RuntimeCommandFrame], + merged_frame: RuntimeCommandFrame, + ) -> None: + """Reject a merged command whose exact interpolated segment collides.""" + if not isinstance(branch_frames, Mapping) or len(branch_frames) < 2: + raise TypeError("branch_frames must contain at least two branch frames.") + if type(merged_frame) is not RuntimeCommandFrame: + raise TypeError("merged_frame must be exactly RuntimeCommandFrame.") + for branch_id, frame in branch_frames.items(): + _identifier(branch_id, field_name="parallel branch IDs") + if type(frame) is not RuntimeCommandFrame: + raise TypeError( + "branch_frames values must be exact RuntimeCommandFrame values." + ) + if not torch.equal(frame.env_ids, merged_frame.env_ids): + raise ValueError("Parallel branch and merged env_ids must match.") + + active = merged_frame.active_mask + if not bool(active.any().item()): + return + current = self._current_control_part_qpos(merged_frame.env_ids) + target = current.clone() + commanded_joint_ids: set[int] = set() + for command in merged_frame.commands: + if ( + type(command.target) is not JointPositionTarget + or type(command.payload) is not JointPositionPayload + ): + raise ParallelSafetyError( + "cuRobo parallel safety accepts only exact joint-position " + "targets and payloads." + ) + missing = sorted( + set(command.target.joint_ids).difference(self._local_joint_columns) + ) + if missing: + raise ParallelSafetyError( + f"Parallel target {command.target.target_id!r} commands joints " + f"{missing} outside validation control part " + f"{self._validation_control_part!r}." + ) + for payload_column, joint_id in enumerate(command.target.joint_ids): + if joint_id in commanded_joint_ids: + raise ParallelSafetyError( + f"Merged parallel commands overlap on joint {joint_id}." + ) + commanded_joint_ids.add(joint_id) + target[:, self._local_joint_columns[joint_id]] = ( + command.payload.positions[:, payload_column] + ) + target = torch.where(active[:, None], target, current) + trajectory = self._interpolate(current, target) + obstacle_poses = self._obstacle_poses( + env_ids=merged_frame.env_ids, + device=trajectory.device, + dtype=trajectory.dtype, + ) + validity = self._motion_generator.validate_joint_trajectory( + trajectory, + control_part=self._validation_control_part, + obstacle_poses=obstacle_poses, + ) + row_valid = validity.all(dim=1) + failed = active & ~row_valid + if not bool(failed.any().item()): + return + failed_rows = failed.nonzero(as_tuple=False).flatten() + failed_env_ids = merged_frame.env_ids.index_select(0, failed_rows) + first_invalid_samples = tuple( + int((~validity[row]).nonzero(as_tuple=False)[0, 0].item()) + for row in failed_rows.detach().cpu().tolist() + ) + raise ParallelSafetyError( + "Merged parallel joint segment is not collision-free for env IDs " + f"{tuple(failed_env_ids.detach().cpu().tolist())}; first invalid " + f"samples={first_invalid_samples}." + ) + + def _current_control_part_qpos(self, env_ids: torch.Tensor) -> torch.Tensor: + """Read current full robot state and select the validator joint order.""" + getter = getattr(self._robot, "get_qpos", None) + if not callable(getter): + raise TypeError("robot must provide get_qpos().") + full = getter(target=False) + if ( + not isinstance(full, torch.Tensor) + or not full.is_floating_point() + or full.dim() != 2 + or not bool(torch.isfinite(full).all().item()) + ): + raise ValueError("robot.get_qpos() must return finite floating (B, D).") + if env_ids.device != full.device: + raise ValueError("Parallel env_ids and robot qpos must share a device.") + if ( + bool((env_ids < 0).any().item()) + or int(env_ids.max().item()) >= full.shape[0] + ): + raise ValueError("Parallel env_ids do not address robot qpos rows.") + if max(self._validation_joint_ids) >= full.shape[1]: + raise ValueError( + "Validation control-part joint IDs exceed robot qpos width." + ) + rows = full.index_select(0, env_ids) + columns = torch.tensor( + self._validation_joint_ids, + dtype=torch.long, + device=full.device, + ) + return rows.index_select(1, columns).clone() + + def _interpolate( + self, + current: torch.Tensor, + target: torch.Tensor, + ) -> torch.Tensor: + """Densify the exact controller segment under a bounded joint step.""" + max_delta = float((target - current).abs().max().item()) + sample_count = max(2, math.ceil(max_delta / self._max_joint_step) + 1) + if sample_count > self._max_interpolation_samples: + raise ParallelSafetyError( + "Merged parallel joint segment needs " + f"{sample_count} collision samples at max_joint_step=" + f"{self._max_joint_step}, exceeding configured limit " + f"{self._max_interpolation_samples}." + ) + alpha = torch.linspace( + 0.0, + 1.0, + sample_count, + device=current.device, + dtype=current.dtype, + ) + return ( + current[:, None, :] + alpha[None, :, None] * (target - current)[:, None, :] + ) + + def _obstacle_poses( + self, + *, + env_ids: torch.Tensor, + device: torch.device, + dtype: torch.dtype, + ) -> Mapping[str, torch.Tensor] | None: + """Observe the exact dynamic collision world for this safety decision.""" + if not self._scene_registry.dynamic_collision_entity_ids: + return None + if self._scene_provider is None: + self._scene_provider = self._scene_registry.make_scene_provider( + batch_size=int(env_ids.numel()) + ) + snapshot = self._scene_provider.snapshot( + timestamp=self._scene_timestamp, + env_ids=env_ids, + ) + self._scene_timestamp += 1.0 + return snapshot.collision_obstacle_poses( + batch_size=int(env_ids.numel()), + device=device, + dtype=dtype, + ) + + +__all__ = [ + "CuroboParallelCommandSafetyValidator", + "CuroboParallelSafetyValidatorFactory", +] diff --git a/embodichain/lab/gym/envs/expert_program/simulation_policies.py b/embodichain/lab/gym/envs/expert_program/simulation_policies.py new file mode 100644 index 000000000..8795ce03a --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/simulation_policies.py @@ -0,0 +1,758 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Simulation-backed Expert Program post-policies and validators. + +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 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 + +from collections.abc import Iterator, Mapping +from copy import deepcopy +from dataclasses import dataclass +import math +from types import MappingProxyType +from typing import Any, TYPE_CHECKING + +import torch + +from embodichain.lab.gym.envs.settling import ( + DynamicSettleMonitor, + DynamicSettleMonitorCfg, + DynamicSettleSample, + DynamicSettleState, +) + +from .compiler import ( + CompiledPostPolicy, + CompiledProgramSegment, + CompiledProgramValidator, +) +from .simulation import SimulationSceneBinding + +if TYPE_CHECKING: + from embodichain.lab.sim.objects import Robot + from embodichain.lab.sim.sim_manager import SimulationManager + + +@dataclass(frozen=True, slots=True) +class _SimulationSettleTarget: + """One canonical entity resolved through an explicit native binding.""" + + canonical_id: str + kind: str + native_entity: Any + + +def default_simulation_settle_presets() -> Mapping[str, DynamicSettleMonitorCfg]: + """Return independently owned built-in post-policy presets.""" + return MappingProxyType( + { + "rigid_object": DynamicSettleMonitorCfg( + linear_velocity_threshold=0.03, + angular_velocity_threshold=0.20, + min_steps=10, + max_steps=240, + check_interval_steps=2, + required_stable_checks=3, + ) + } + ) + + +def _json_speed_values(value: torch.Tensor) -> list[float | None]: + """Convert speed evidence to finite JSON numbers or explicit unknowns.""" + return [ + float(item) if math.isfinite(float(item)) else None + for item in value.detach().cpu().tolist() + ] + + +class SimulationSegmentPolicyPort: + """Execute built-in segment policies against explicitly bound simulation data. + + Args: + simulation: Live simulation used only for UIDs declared in + ``scene_binding``. + 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. + env_ids: Optional stable logical row IDs. They describe correlation, + not simulator row indices; simulator rows remain ordered exactly as + returned by the robot and bound entities. + + The same instance implements both ``SegmentPostPolicyPort`` and + ``SegmentValidatorPort``. Unknown policy types, presets, canonical IDs, or + native entities fail before an action is emitted. + """ + + def __init__( + self, + simulation: SimulationManager, + robot: Robot, + scene_binding: SimulationSceneBinding, + *, + settle_presets: Mapping[str, DynamicSettleMonitorCfg] | None = None, + env_ids: torch.Tensor | None = None, + ) -> None: + if type(scene_binding) is not SimulationSceneBinding: + raise TypeError("scene_binding must be exactly SimulationSceneBinding.") + 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], + dtype=torch.long, + device=qpos.device, + ) + if not isinstance(env_ids, torch.Tensor): + raise TypeError("env_ids must be a torch.Tensor or None.") + if env_ids.dtype != torch.long or env_ids.shape != (qpos.shape[0],): + raise ValueError("env_ids must be int64 with one ID per simulator row.") + if env_ids.device != qpos.device: + raise ValueError("env_ids and robot qpos must share a device.") + if torch.unique(env_ids).numel() != env_ids.numel(): + raise ValueError("env_ids must contain unique values.") + + selected_presets = ( + default_simulation_settle_presets() + if settle_presets is None + else settle_presets + ) + if not isinstance(selected_presets, Mapping) or not selected_presets: + raise ValueError("settle_presets must be a non-empty mapping.") + normalized_presets: dict[str, DynamicSettleMonitorCfg] = {} + for preset_id, cfg in selected_presets.items(): + if ( + type(preset_id) is not str + or not preset_id + or preset_id != preset_id.strip() + ): + raise ValueError( + "Settle preset IDs must be non-empty strings without outer " + "whitespace." + ) + if not isinstance(cfg, DynamicSettleMonitorCfg): + raise TypeError( + "settle_presets values must be DynamicSettleMonitorCfg values." + ) + normalized_presets[preset_id] = cfg.snapshot() + + self._simulation = simulation + self._robot = robot + self._scene_binding = scene_binding + self._env_ids = env_ids.clone() + self._row_indices = torch.arange( + qpos.shape[0], + dtype=torch.long, + device=qpos.device, + ) + self._settle_presets = MappingProxyType(normalized_presets) + self._settle_targets, self._rigid_objects = self._resolve_native_entities() + self._post_policy_results: dict[int, dict[str, object]] = {} + self._post_policy_success: dict[int, torch.Tensor] = {} + self._validator_results: dict[int, dict[str, object]] = {} + + @property + def settle_preset_ids(self) -> tuple[str, ...]: + """Return installed post-policy preset IDs in declaration order.""" + return tuple(self._settle_presets) + + def validate_policy( + self, + policy: Any, + *, + segment: Any, + ) -> None: + """Validate one post-policy against static bindings without observation. + + This method reads only the compiled declaration, installed preset + table, and entities resolved when the port was constructed. It never + samples velocity or qpos and never emits a controller action. + """ + if type(policy) is not CompiledPostPolicy: + raise TypeError("policy must be exactly CompiledPostPolicy.") + self._validate_segment_membership(segment, policy, kind="post policy") + if policy.cfg.kind != "wait_stable": + raise ValueError( + f"Unsupported compiled post-policy kind {policy.cfg.kind!r}." + ) + if policy.cfg.preset not in self._settle_presets: + raise KeyError( + f"Unknown settle preset {policy.cfg.preset!r}; available presets " + f"are {sorted(self._settle_presets)}." + ) + entity_id = policy.entity.entity_id + target = self._settle_targets.get(entity_id) + if target is None: + raise KeyError( + f"Canonical settle entity {entity_id!r} has no explicit native " + "dynamic binding." + ) + if target.kind == "rigid_object" and bool( + getattr(target.native_entity, "is_non_dynamic", False) + ): + raise ValueError( + f"Canonical settle entity {entity_id!r} is static or kinematic." + ) + + def actions( + self, + policy: Any, + *, + segment: Any, + active_mask: torch.Tensor, + ) -> Iterator[torch.Tensor]: + """Yield full target-qpos hold actions until rows settle or time out. + + Args: + policy: Exact compiled ``wait_stable`` policy. + segment: Exact segment that owns ``policy``. + active_mask: Rows that remain eligible after runtime execution and + preceding post-policies. Inactive rows are held safely but do + not participate in settling, timeout, or success results. + + Yields: + 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 + :meth:`post_policy_metadata`; no batch-level exception is raised. + """ + self.validate_policy(policy, segment=segment) + active_mask = self._validate_active_mask(active_mask) + preset = self._settle_presets[policy.cfg.preset] + entity_id = policy.entity.entity_id + target = self._settle_targets[entity_id] + + result_key = id(policy) + self._post_policy_results.pop(result_key, None) + self._post_policy_success.pop(result_key, None) + if not bool(active_mask.any().item()): + self._post_policy_success[result_key] = active_mask.clone() + self._post_policy_results[result_key] = { + "kind": policy.cfg.kind, + "entity_id": entity_id, + "preset": policy.cfg.preset, + "source_path": list(policy.source_path), + "status": "skipped", + "active_mask": active_mask.detach().cpu().tolist(), + "thresholds": self._settle_threshold_metadata(preset), + "state": self._empty_settle_state_metadata(active_mask), + } + return + + active_rows = self._row_indices[active_mask] + monitor = DynamicSettleMonitor(preset, self._env_ids[active_mask]) + elapsed_steps = 0 + while True: + state = monitor.observe( + (self._measure_settle_target(target, row_indices=active_rows),), + elapsed_steps=elapsed_steps, + ) + settled_mask = torch.zeros_like(active_mask) + settled_mask[active_mask] = state.settled_mask + self._post_policy_results[result_key] = { + "kind": policy.cfg.kind, + "entity_id": entity_id, + "preset": policy.cfg.preset, + "source_path": list(policy.source_path), + "active_mask": active_mask.detach().cpu().tolist(), + "status": ( + "settled" + if bool(state.settled_mask.all().item()) + else ( + "timed_out" + if bool(state.timeout_mask.any().item()) + else "running" + ) + ), + "thresholds": self._settle_threshold_metadata(preset), + "state": self._expand_settle_state_metadata(state, active_mask), + } + self._post_policy_success[result_key] = settled_mask + if bool(state.settled_mask.all().item()): + return + if bool(state.timeout_mask.any().item()): + return + yield self._hold_robot_qpos(active_mask) + elapsed_steps += 1 + + def post_policy_result( + self, + policy: Any, + *, + segment: Any, + ) -> torch.Tensor: + """Return the latest independently owned per-row settling result.""" + if type(policy) is not CompiledPostPolicy: + raise TypeError("policy must be exactly CompiledPostPolicy.") + self._validate_segment_membership(segment, policy, kind="post policy") + result = self._post_policy_success.get(id(policy)) + if result is None: + raise RuntimeError("Post-policy result is unavailable before execution.") + return result.clone() + + def post_policy_metadata( + self, + policy: Any, + *, + segment: Any, + ) -> Mapping[str, object]: + """Return the latest JSON-safe settling trace for one policy. + + The trace is available after the policy generator has started. A + terminal trace has status ``"settled"``, ``"timed_out"``, or + ``"skipped"`` when no rows remain active; an early demo interruption + intentionally retains the latest ``"running"`` snapshot for diagnosis. + """ + if type(policy) is not CompiledPostPolicy: + raise TypeError("policy must be exactly CompiledPostPolicy.") + self._validate_segment_membership(segment, policy, kind="post policy") + metadata = self._post_policy_results.get(id(policy)) + if metadata is None: + raise RuntimeError("Post-policy metadata is unavailable before execution.") + return deepcopy(metadata) + + def validate_validator( + self, + validator: Any, + *, + segment: Any, + ) -> None: + """Validate one validator against static bindings without observation.""" + if type(validator) is not CompiledProgramValidator: + raise TypeError("validator must be exactly CompiledProgramValidator.") + self._validate_segment_membership(segment, validator, kind="validator") + if validator.cfg.kind != "object_near_target": + raise ValueError( + f"Unsupported compiled validator kind {validator.cfg.kind!r}." + ) + entity_id = validator.object.entity_id + if entity_id not in self._rigid_objects: + raise KeyError( + f"Canonical validator object {entity_id!r} has no explicit rigid-" + "object binding." + ) + + def validate(self, validator: Any, *, segment: Any) -> torch.Tensor: + """Observe an explicitly bound rigid object against a world target. + + Args: + validator: Exact compiled ``object_near_target`` validator. + segment: Exact segment that owns ``validator``. + + Returns: + Boolean tensor with one result per simulation row. + """ + self.validate_validator(validator, segment=segment) + entity_id = validator.object.entity_id + entity = self._rigid_objects[entity_id] + pose = self._read_pose(entity, entity_id=entity_id) + current_position = pose[:, :3, 3] + target_position = validator.target_pose.position.to( + device=current_position.device, + dtype=current_position.dtype, + ) + if target_position.dim() == 1: + target_position = target_position.unsqueeze(0).expand_as(current_position) + elif target_position.shape != current_position.shape: + raise ValueError( + "Validator target batch must be unbatched or match simulator rows." + ) + error = torch.linalg.vector_norm(current_position - target_position, dim=1) + accepted = torch.isfinite(error) & ( + error <= float(validator.cfg.position_tolerance) + ) + self._validator_results[id(validator)] = { + "kind": validator.cfg.kind, + "object_id": entity_id, + "target_id": validator.target_selection.target_id, + "target_value_index": validator.target_selection.value_index, + "source_path": list(validator.source_path), + "position_tolerance": float(validator.cfg.position_tolerance), + "env_ids": self._env_ids.detach().cpu().tolist(), + "object_position": current_position.detach().cpu().tolist(), + "target_position": target_position.detach().cpu().tolist(), + "position_error": error.detach().cpu().tolist(), + "accepted_mask": accepted.detach().cpu().tolist(), + } + return accepted + + def validator_metadata( + self, + validator: Any, + *, + segment: Any, + ) -> Mapping[str, object]: + """Return an owned JSON-safe trace for one completed validator.""" + if type(validator) is not CompiledProgramValidator: + raise TypeError("validator must be exactly CompiledProgramValidator.") + self._validate_segment_membership(segment, validator, kind="validator") + metadata = self._validator_results.get(id(validator)) + if metadata is None: + raise RuntimeError("Validator metadata is unavailable before validation.") + return deepcopy(metadata) + + @staticmethod + 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(f"robot must provide {call}.") + qpos = get_qpos(target=target) + if ( + not isinstance(qpos, torch.Tensor) + or not qpos.is_floating_point() + or qpos.dim() != 2 + or qpos.shape[0] == 0 + or qpos.shape[1] == 0 + ): + raise ValueError( + f"{call} must return {mode} floating full-qpos shape (B, J)." + ) + if not bool(torch.isfinite(qpos).all().item()): + 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): + raise TypeError("active_mask must be a torch.Tensor.") + if active_mask.dtype != torch.bool or active_mask.shape != self._env_ids.shape: + raise ValueError( + "active_mask must be bool with one value per simulator row." + ) + if active_mask.device != self._env_ids.device: + raise ValueError("active_mask and env_ids must share a device.") + return active_mask.clone() + + @staticmethod + def _settle_threshold_metadata( + preset: DynamicSettleMonitorCfg, + ) -> dict[str, float | int]: + """Serialize one settling preset without exposing mutable state.""" + return { + "linear_velocity": float(preset.linear_velocity_threshold), + "angular_velocity": float(preset.angular_velocity_threshold), + "min_steps": preset.min_steps, + "max_steps": preset.max_steps, + "check_interval_steps": preset.check_interval_steps, + "required_stable_checks": preset.required_stable_checks, + } + + def _empty_settle_state_metadata( + self, + active_mask: torch.Tensor, + ) -> dict[str, object]: + """Return a full-batch trace for a policy with no eligible rows.""" + batch_size = self._env_ids.numel() + return { + "elapsed_steps": 0, + "observation_count": 0, + "env_ids": self._env_ids.detach().cpu().tolist(), + "active_mask": active_mask.detach().cpu().tolist(), + "stable_counts": [0] * batch_size, + "settled_mask": [False] * batch_size, + "timeout_mask": [False] * batch_size, + "checked": False, + "max_linear_speed": [None] * batch_size, + "max_angular_speed": [None] * batch_size, + } + + def _expand_settle_state_metadata( + self, + state: DynamicSettleState, + active_mask: torch.Tensor, + ) -> dict[str, object]: + """Expand active-row monitor state to the stable full-batch ordering.""" + stable_counts = torch.zeros_like(self._env_ids) + settled_mask = torch.zeros_like(active_mask) + timeout_mask = torch.zeros_like(active_mask) + max_linear_speed = torch.full( + active_mask.shape, + float("inf"), + dtype=state.max_linear_speed.dtype, + device=active_mask.device, + ) + max_angular_speed = torch.full_like(max_linear_speed, float("inf")) + stable_counts[active_mask] = state.stable_counts + settled_mask[active_mask] = state.settled_mask + timeout_mask[active_mask] = state.timeout_mask + max_linear_speed[active_mask] = state.max_linear_speed + max_angular_speed[active_mask] = state.max_angular_speed + return { + "elapsed_steps": state.elapsed_steps, + "observation_count": state.observation_count, + "env_ids": self._env_ids.detach().cpu().tolist(), + "active_mask": active_mask.detach().cpu().tolist(), + "stable_counts": stable_counts.detach().cpu().tolist(), + "settled_mask": settled_mask.detach().cpu().tolist(), + "timeout_mask": timeout_mask.detach().cpu().tolist(), + "checked": state.checked, + "max_linear_speed": _json_speed_values(max_linear_speed), + "max_angular_speed": _json_speed_values(max_angular_speed), + } + + @staticmethod + def _validate_segment_membership( + segment: Any, + member: CompiledPostPolicy | CompiledProgramValidator, + *, + kind: str, + ) -> None: + """Require the supplied compiled value to belong to the exact segment.""" + if type(segment) is not CompiledProgramSegment: + raise TypeError("segment must be exactly CompiledProgramSegment.") + values = ( + segment.post_policies + if type(member) is CompiledPostPolicy + else segment.validators + ) + if not any(value is member for value in values): + raise ValueError( + f"Compiled {kind} does not belong to the supplied segment." + ) + + def _resolve_native_entities( + self, + ) -> tuple[ + Mapping[str, _SimulationSettleTarget], + Mapping[str, Any], + ]: + """Resolve only explicitly declared canonical/native pairs.""" + settle_targets: dict[str, _SimulationSettleTarget] = {} + rigid_objects: dict[str, Any] = {} + articulation_targets: dict[str, _SimulationSettleTarget] = {} + + for binding in self._scene_binding.rigid_objects: + entity = self._require_native( + "get_rigid_object", + canonical_id=binding.entity_id, + simulation_uid=binding.simulation_uid, + ) + target = _SimulationSettleTarget( + binding.entity_id, + "rigid_object", + entity, + ) + settle_targets[binding.entity_id] = target + rigid_objects[binding.entity_id] = entity + + for binding in self._scene_binding.articulations: + entity = self._require_native( + "get_articulation", + canonical_id=binding.entity_id, + simulation_uid=binding.simulation_uid, + ) + target = _SimulationSettleTarget( + binding.entity_id, + "articulation", + entity, + ) + settle_targets[binding.entity_id] = target + articulation_targets[binding.entity_id] = target + + for binding in self._scene_binding.links: + settle_targets[binding.entity_id] = self._require_parent_target( + articulation_targets, + child_id=binding.entity_id, + parent_id=binding.articulation_id, + ) + for binding in self._scene_binding.antipodal_grasps: + parent = settle_targets.get(binding.object_id) + if parent is None or parent.kind != "rigid_object": + raise KeyError( + f"Affordance {binding.entity_id!r} references unavailable rigid " + f"object {binding.object_id!r}." + ) + settle_targets[binding.entity_id] = parent + for binding in self._scene_binding.articulation_operations: + settle_targets[binding.entity_id] = self._require_parent_target( + articulation_targets, + child_id=binding.entity_id, + parent_id=binding.articulation_id, + ) + return MappingProxyType(settle_targets), MappingProxyType(rigid_objects) + + @staticmethod + def _require_parent_target( + targets: Mapping[str, _SimulationSettleTarget], + *, + child_id: str, + parent_id: str, + ) -> _SimulationSettleTarget: + """Resolve a child to one explicitly declared articulation root.""" + target = targets.get(parent_id) + if target is None: + raise KeyError( + f"Canonical entity {child_id!r} references unavailable parent " + f"{parent_id!r}." + ) + return target + + def _require_native( + self, + getter_name: str, + *, + canonical_id: str, + simulation_uid: str, + ) -> Any: + """Resolve one explicitly selected native simulation entity.""" + getter = getattr(self._simulation, getter_name, None) + if not callable(getter): + raise TypeError(f"simulation must provide {getter_name}().") + entity = getter(simulation_uid) + if entity is None: + raise KeyError( + f"Native entity {simulation_uid!r} selected for canonical entity " + f"{canonical_id!r} was not found." + ) + return entity + + def _measure_settle_target( + self, + target: _SimulationSettleTarget, + *, + row_indices: torch.Tensor, + ) -> DynamicSettleSample: + """Measure physical bodies for explicitly selected simulator rows.""" + if target.kind == "articulation": + body_data = getattr(target.native_entity, "body_data", None) + velocity = getattr(body_data, "body_link_vel", None) + if not isinstance(velocity, torch.Tensor): + raise RuntimeError( + f"Articulation settle target {target.canonical_id!r} has no " + "body_link_vel tensor." + ) + selected = velocity.index_select(0, row_indices.to(velocity.device)) + if selected.dim() != 3 or selected.shape[-1] != 6: + raise ValueError( + "Articulation body_link_vel must have shape (B, N, 6)." + ) + linear_velocity = selected[..., :3] + angular_velocity = selected[..., 3:] + else: + body_data = getattr(target.native_entity, "body_data", None) + linear_velocity = getattr(body_data, "lin_vel", None) + angular_velocity = getattr(body_data, "ang_vel", None) + if not isinstance(linear_velocity, torch.Tensor) or not isinstance( + angular_velocity, + torch.Tensor, + ): + raise RuntimeError( + f"Rigid settle target {target.canonical_id!r} has no linear/" + "angular velocity tensors." + ) + rows = row_indices.to(linear_velocity.device) + linear_velocity = linear_velocity.index_select(0, rows) + angular_velocity = angular_velocity.index_select( + 0, + row_indices.to(angular_velocity.device), + ) + if ( + linear_velocity.shape != angular_velocity.shape + or linear_velocity.dim() < 2 + or linear_velocity.shape[-1] != 3 + ): + raise ValueError( + "Rigid body velocities must have equal shape (B, ..., 3)." + ) + + linear_speed = torch.linalg.vector_norm(linear_velocity, dim=-1).reshape( + row_indices.numel(), + -1, + ) + angular_speed = torch.linalg.vector_norm(angular_velocity, dim=-1).reshape( + row_indices.numel(), + -1, + ) + device = self._env_ids.device + return DynamicSettleSample( + entity_id=target.canonical_id, + linear_speed=linear_speed.to(device=device), + angular_speed=angular_speed.to(device=device), + ) + + def _read_pose(self, entity: Any, *, entity_id: str) -> torch.Tensor: + """Read one rigid-object pose batch in simulator row order.""" + getter = getattr(entity, "get_local_pose", None) + if not callable(getter): + raise TypeError( + f"Native rigid object for {entity_id!r} must provide " + "get_local_pose()." + ) + pose = getter(to_matrix=True) + if not isinstance(pose, torch.Tensor) or not pose.is_floating_point(): + raise TypeError("get_local_pose(to_matrix=True) must return a tensor.") + batch_size = int(self._env_ids.numel()) + if pose.shape == (4, 4): + pose = pose.unsqueeze(0).expand(batch_size, -1, -1) + elif pose.shape != (batch_size, 4, 4): + raise ValueError( + f"Rigid object {entity_id!r} pose must have shape " + f"({batch_size}, 4, 4)." + ) + if not bool(torch.isfinite(pose).all().item()): + raise ValueError(f"Rigid object {entity_id!r} pose must be finite.") + return pose.clone() + + +__all__ = ["SimulationSegmentPolicyPort", "default_simulation_settle_presets"] diff --git a/embodichain/lab/gym/envs/managers/_event_functors/dynamic_settling.py b/embodichain/lab/gym/envs/managers/_event_functors/dynamic_settling.py index b857f1078..9418d6182 100644 --- a/embodichain/lab/gym/envs/managers/_event_functors/dynamic_settling.py +++ b/embodichain/lab/gym/envs/managers/_event_functors/dynamic_settling.py @@ -18,14 +18,17 @@ from __future__ import annotations -import math from collections.abc import Sequence -from numbers import Real from typing import TYPE_CHECKING, Literal import torch from embodichain.lab.gym.envs.managers.cfg import SceneEntityCfg +from embodichain.lab.gym.envs.settling import ( + DynamicSettleMonitor, + DynamicSettleMonitorCfg, + DynamicSettleSample, +) from embodichain.lab.sim.objects import Articulation, RigidObject, RigidObjectGroup from embodichain.utils import logger @@ -37,7 +40,6 @@ _DynamicEntity = RigidObject | RigidObjectGroup | Articulation _SettleEntity = tuple[str, SceneEntityCfg, _DynamicEntity] -_SpeedSample = tuple[str, torch.Tensor, torch.Tensor] def _validate_settle_parameters( @@ -49,48 +51,21 @@ def _validate_settle_parameters( required_stable_checks: int, timeout_behavior: str, allow_partial_envs: bool, -) -> None: - """Validate dynamic-object settle parameters.""" - for name, value in ( - ("min_steps", min_steps), - ("max_steps", max_steps), - ("check_interval_steps", check_interval_steps), - ("required_stable_checks", required_stable_checks), - ): - if isinstance(value, bool) or not isinstance(value, int): - raise TypeError(f"{name} must be an integer, got {type(value).__name__}.") - - if min_steps < 0: - raise ValueError("min_steps must be non-negative.") - if max_steps < min_steps: - raise ValueError("max_steps must be greater than or equal to min_steps.") - if check_interval_steps < 1: - raise ValueError("check_interval_steps must be at least 1.") - if required_stable_checks < 1: - raise ValueError("required_stable_checks must be at least 1.") - - for name, value in ( - ("linear_velocity_threshold", linear_velocity_threshold), - ("angular_velocity_threshold", angular_velocity_threshold), - ): - if isinstance(value, bool) or not isinstance(value, Real): - raise TypeError(f"{name} must be a real number.") - if not math.isfinite(float(value)) or value < 0: - raise ValueError(f"{name} must be finite and non-negative.") - - available_checks = ( - 1 + (max_steps - min_steps + check_interval_steps - 1) // check_interval_steps +) -> DynamicSettleMonitorCfg: + """Validate parameters and return the reusable monitor policy.""" + cfg = DynamicSettleMonitorCfg( + linear_velocity_threshold=linear_velocity_threshold, + angular_velocity_threshold=angular_velocity_threshold, + min_steps=min_steps, + max_steps=max_steps, + check_interval_steps=check_interval_steps, + required_stable_checks=required_stable_checks, ) - if required_stable_checks > available_checks: - raise ValueError( - "required_stable_checks cannot be reached within the configured " - f"step budget; at most {available_checks} checks are possible." - ) - if timeout_behavior not in ("warn", "raise"): raise ValueError("timeout_behavior must be either 'warn' or 'raise'.") if not isinstance(allow_partial_envs, bool): raise TypeError("allow_partial_envs must be a boolean.") + return cfg def _normalize_settle_env_ids( @@ -210,9 +185,9 @@ def _resolve_settle_entities( def _measure_settle_speeds( entities: Sequence[_SettleEntity], env_ids: torch.Tensor, -) -> list[_SpeedSample]: +) -> list[DynamicSettleSample]: """Measure per-body linear and angular speeds for selected environments.""" - samples: list[_SpeedSample] = [] + samples: list[DynamicSettleSample] = [] for kind, entity_cfg, entity in entities: if kind == "articulation": velocity = entity.body_data.body_link_vel[env_ids] @@ -238,29 +213,35 @@ def _measure_settle_speeds( angular_speed = torch.linalg.vector_norm(angular_velocity, dim=-1).reshape( env_ids.numel(), -1 ) - samples.append((entity_cfg.uid, linear_speed, angular_speed)) + samples.append( + DynamicSettleSample( + entity_id=entity_cfg.uid, + linear_speed=linear_speed, + angular_speed=angular_speed, + ) + ) return samples def _settle_samples_are_stable( - samples: Sequence[_SpeedSample], + samples: Sequence[DynamicSettleSample], linear_velocity_threshold: float, angular_velocity_threshold: float, ) -> bool: """Return whether every measured body is finite and below both thresholds.""" stable = [] - for _, linear_speed, angular_speed in samples: + for sample in samples: stable.append( - torch.isfinite(linear_speed) - & torch.isfinite(angular_speed) - & (linear_speed <= linear_velocity_threshold) - & (angular_speed <= angular_velocity_threshold) + torch.isfinite(sample.linear_speed) + & torch.isfinite(sample.angular_speed) + & (sample.linear_speed <= linear_velocity_threshold) + & (sample.angular_speed <= angular_velocity_threshold) ) return bool(torch.cat([value.reshape(-1) for value in stable]).all().item()) def _format_settle_timeout( - samples: Sequence[_SpeedSample], + samples: Sequence[DynamicSettleSample], env_ids: torch.Tensor, linear_velocity_threshold: float, angular_velocity_threshold: float, @@ -272,7 +253,9 @@ def _format_settle_timeout( unsettled: list[str] = [] all_linear_speeds: list[torch.Tensor] = [] all_angular_speeds: list[torch.Tensor] = [] - for uid, linear_speed, angular_speed in samples: + for sample in samples: + linear_speed = sample.linear_speed + angular_speed = sample.angular_speed stable = ( torch.isfinite(linear_speed) & torch.isfinite(angular_speed) @@ -282,7 +265,7 @@ def _format_settle_timeout( unsettled_mask = ~stable.all(dim=1) if bool(unsettled_mask.any().item()): unsettled_env_ids = env_ids[unsettled_mask].detach().cpu().tolist() - unsettled.append(f"{uid}(env_ids={unsettled_env_ids})") + unsettled.append(f"{sample.entity_id}(env_ids={unsettled_env_ids})") all_linear_speeds.append(linear_speed.reshape(-1)) all_angular_speeds.append(angular_speed.reshape(-1)) @@ -364,7 +347,7 @@ def wait_for_dynamic_objects_to_settle( TypeError: If a parameter or entity configuration has the wrong type. ValueError: If parameters, targets, or environment selection are invalid. """ - _validate_settle_parameters( + monitor_cfg = _validate_settle_parameters( linear_velocity_threshold=linear_velocity_threshold, angular_velocity_threshold=angular_velocity_threshold, min_steps=min_steps, @@ -395,20 +378,14 @@ def wait_for_dynamic_objects_to_settle( env.sim.update(step=min_steps) step_count = min_steps - stable_checks = 0 - samples: list[_SpeedSample] + monitor = DynamicSettleMonitor(monitor_cfg, target_env_ids) + samples: list[DynamicSettleSample] + settle_state = None while True: samples = _measure_settle_speeds(entities, target_env_ids) - if _settle_samples_are_stable( - samples, - linear_velocity_threshold=linear_velocity_threshold, - angular_velocity_threshold=angular_velocity_threshold, - ): - stable_checks += 1 - if stable_checks >= required_stable_checks: - return - else: - stable_checks = 0 + settle_state = monitor.observe(samples, elapsed_steps=step_count) + if bool(settle_state.settled_mask.all().item()): + return if step_count >= max_steps: break @@ -422,7 +399,7 @@ def wait_for_dynamic_objects_to_settle( linear_velocity_threshold=linear_velocity_threshold, angular_velocity_threshold=angular_velocity_threshold, max_steps=max_steps, - stable_checks=stable_checks, + stable_checks=int(settle_state.stable_counts.min().item()), required_stable_checks=required_stable_checks, ) if timeout_behavior == "raise": diff --git a/embodichain/lab/gym/envs/managers/actions.py b/embodichain/lab/gym/envs/managers/actions.py index bdecd3e57..4f9430f9b 100644 --- a/embodichain/lab/gym/envs/managers/actions.py +++ b/embodichain/lab/gym/envs/managers/actions.py @@ -284,7 +284,7 @@ def process_action(self, action: torch.Tensor) -> EnvAction: raise ValueError( f"EEF pose action must be 6D or 7D, got {scaled.shape[-1]}D" ) - # Batch IK: robot.compute_ik supports (n_envs, 4, 4) pose and (n_envs, dof) seed + # Batch IK: robot.compute_ik supports (num_envs, 4, 4) pose and (num_envs, dof) seed ret, qpos_ik = self._env.robot.compute_ik( pose=target_pose, joint_seed=current_qpos, diff --git a/embodichain/lab/gym/envs/settling.py b/embodichain/lab/gym/envs/settling.py new file mode 100644 index 000000000..39d5be171 --- /dev/null +++ b/embodichain/lab/gym/envs/settling.py @@ -0,0 +1,374 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Reusable per-environment dynamic-settling state machine.""" + +from __future__ import annotations + +import math +from collections.abc import Sequence +from dataclasses import dataclass +from numbers import Real + +import torch + +from embodichain.utils import configclass + + +@configclass +class DynamicSettleMonitorCfg: + """Threshold and cadence policy for :class:`DynamicSettleMonitor`. + + The monitor never advances an environment. Callers own the stepping path + and provide raw velocity samples after the configured minimum/cadence. + This lets reset events and demonstration post-policies share exactly the + same state transition rules while using different stepping ports. + """ + + linear_velocity_threshold: float = 0.03 + """Maximum stable linear speed in metres per second.""" + + angular_velocity_threshold: float = 0.20 + """Maximum stable angular speed in radians per second.""" + + min_steps: int = 10 + """Minimum number of environment steps before the first check.""" + + max_steps: int = 240 + """Maximum elapsed environment steps before unresolved rows time out.""" + + check_interval_steps: int = 2 + """Minimum number of steps between independent evidence checks.""" + + required_stable_checks: int = 3 + """Consecutive stable checks required independently for each row.""" + + def __post_init__(self) -> None: + for name in ( + "min_steps", + "max_steps", + "check_interval_steps", + "required_stable_checks", + ): + value = getattr(self, name) + if isinstance(value, bool) or not isinstance(value, int): + raise TypeError(f"{name} must be an integer.") + if self.min_steps < 0: + raise ValueError("min_steps must be non-negative.") + if self.max_steps < self.min_steps: + raise ValueError("max_steps must be greater than or equal to min_steps.") + if self.check_interval_steps < 1: + raise ValueError("check_interval_steps must be at least 1.") + if self.required_stable_checks < 1: + raise ValueError("required_stable_checks must be at least 1.") + for name in ( + "linear_velocity_threshold", + "angular_velocity_threshold", + ): + value = getattr(self, name) + if isinstance(value, bool) or not isinstance(value, Real): + raise TypeError(f"{name} must be a real number.") + if not math.isfinite(float(value)) or float(value) < 0.0: + raise ValueError(f"{name} must be finite and non-negative.") + available_checks = ( + 1 + + (self.max_steps - self.min_steps + self.check_interval_steps - 1) + // self.check_interval_steps + ) + if self.required_stable_checks > available_checks: + raise ValueError( + "required_stable_checks cannot be reached within the configured " + f"step budget; at most {available_checks} checks are possible." + ) + + def snapshot(self) -> DynamicSettleMonitorCfg: + """Return an independently owned configuration value.""" + return DynamicSettleMonitorCfg( + linear_velocity_threshold=self.linear_velocity_threshold, + angular_velocity_threshold=self.angular_velocity_threshold, + min_steps=self.min_steps, + max_steps=self.max_steps, + check_interval_steps=self.check_interval_steps, + required_stable_checks=self.required_stable_checks, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class DynamicSettleSample: + """Raw per-body speed evidence for one registered scene entity. + + Args: + entity_id: Stable entity identifier used in metadata and diagnostics. + linear_speed: Per-row body speeds with shape ``(B, N)``. + angular_speed: Per-row body speeds with shape ``(B, N)``. + """ + + entity_id: str + linear_speed: torch.Tensor + angular_speed: torch.Tensor + + def __post_init__(self) -> None: + if ( + type(self.entity_id) is not str + or not self.entity_id + or self.entity_id != self.entity_id.strip() + ): + raise ValueError( + "entity_id must be a non-empty string without outer whitespace." + ) + for name in ("linear_speed", "angular_speed"): + value = getattr(self, name) + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") + if not value.is_floating_point() or value.dim() != 2: + raise ValueError(f"{name} must be a floating tensor with shape (B, N).") + if value.shape[0] == 0 or value.shape[1] == 0: + raise ValueError(f"{name} must contain at least one row and body.") + if self.linear_speed.shape != self.angular_speed.shape: + raise ValueError("linear_speed and angular_speed must have equal shapes.") + if self.linear_speed.device != self.angular_speed.device: + raise ValueError("linear_speed and angular_speed must share a device.") + object.__setattr__(self, "linear_speed", self.linear_speed.clone()) + object.__setattr__(self, "angular_speed", self.angular_speed.clone()) + + def snapshot(self) -> DynamicSettleSample: + """Return an independently owned raw evidence sample.""" + return DynamicSettleSample( + entity_id=self.entity_id, + linear_speed=self.linear_speed, + angular_speed=self.angular_speed, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class DynamicSettleState: + """Owned state emitted after one monitor observation.""" + + env_ids: torch.Tensor + elapsed_steps: int + observation_count: int + checked: bool + stable_counts: torch.Tensor + settled_mask: torch.Tensor + timeout_mask: torch.Tensor + max_linear_speed: torch.Tensor + max_angular_speed: torch.Tensor + + def __post_init__(self) -> None: + if not isinstance(self.env_ids, torch.Tensor): + raise TypeError("env_ids must be a torch.Tensor.") + if self.env_ids.dtype != torch.long or self.env_ids.dim() != 1: + raise ValueError("env_ids must be a one-dimensional torch.long tensor.") + if self.env_ids.numel() == 0: + raise ValueError("env_ids must contain at least one row.") + if type(self.elapsed_steps) is not int or self.elapsed_steps < 0: + raise ValueError("elapsed_steps must be a non-negative integer.") + if type(self.observation_count) is not int or self.observation_count < 0: + raise ValueError("observation_count must be a non-negative integer.") + if type(self.checked) is not bool: + raise TypeError("checked must be a bool.") + row_count = self.env_ids.numel() + for name, dtype in ( + ("stable_counts", torch.long), + ("settled_mask", torch.bool), + ("timeout_mask", torch.bool), + ): + value = getattr(self, name) + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") + if value.dtype != dtype or value.shape != (row_count,): + raise ValueError(f"{name} must have shape (B,) and dtype {dtype}.") + if value.device != self.env_ids.device: + raise ValueError(f"{name} and env_ids must share a device.") + if (self.settled_mask & self.timeout_mask).any(): + raise ValueError("settled_mask and timeout_mask must not overlap.") + for name in ("max_linear_speed", "max_angular_speed"): + value = getattr(self, name) + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") + if not value.is_floating_point() or value.shape != (row_count,): + raise ValueError(f"{name} must be a floating tensor with shape (B,).") + if value.device != self.env_ids.device: + raise ValueError(f"{name} and env_ids must share a device.") + for name in ( + "env_ids", + "stable_counts", + "settled_mask", + "timeout_mask", + "max_linear_speed", + "max_angular_speed", + ): + object.__setattr__(self, name, getattr(self, name).clone()) + + @property + def complete(self) -> bool: + """Whether every row has either settled or timed out.""" + return bool((self.settled_mask | self.timeout_mask).all().item()) + + def to_metadata(self) -> dict[str, object]: + """Return deterministic, JSON-compatible post-policy metadata.""" + return { + "elapsed_steps": self.elapsed_steps, + "observation_count": self.observation_count, + "env_ids": self.env_ids.detach().to("cpu").tolist(), + "stable_counts": self.stable_counts.detach().to("cpu").tolist(), + "settled_mask": self.settled_mask.detach().to("cpu").tolist(), + "timeout_mask": self.timeout_mask.detach().to("cpu").tolist(), + "max_linear_speed": self.max_linear_speed.detach().to("cpu").tolist(), + "max_angular_speed": self.max_angular_speed.detach().to("cpu").tolist(), + } + + +class DynamicSettleMonitor: + """Track settling independently for stable environment IDs. + + Duplicate observations at the same ``elapsed_steps`` value are idempotent. + Regressing step counters are rejected, and a jump across multiple cadence + boundaries counts as one fresh observation rather than replaying one sample. + """ + + def __init__( + self, + cfg: DynamicSettleMonitorCfg, + env_ids: torch.Tensor, + ) -> None: + if not isinstance(cfg, DynamicSettleMonitorCfg): + raise TypeError("cfg must be a DynamicSettleMonitorCfg.") + if not isinstance(env_ids, torch.Tensor): + raise TypeError("env_ids must be a torch.Tensor.") + if env_ids.dtype != torch.long or env_ids.dim() != 1: + raise ValueError("env_ids must be a one-dimensional torch.long tensor.") + if env_ids.numel() == 0 or torch.unique(env_ids).numel() != env_ids.numel(): + raise ValueError("env_ids must contain unique environment IDs.") + self.cfg = cfg.snapshot() + self._env_ids = env_ids.clone() + self._stable_counts = torch.zeros_like(env_ids) + self._settled = torch.zeros_like(env_ids, dtype=torch.bool) + self._timeout = torch.zeros_like(env_ids, dtype=torch.bool) + self._max_linear = torch.full( + env_ids.shape, + float("inf"), + dtype=torch.float32, + device=env_ids.device, + ) + self._max_angular = self._max_linear.clone() + self._last_elapsed_steps = -1 + self._last_checked_steps = -1 + self._observation_count = 0 + + @property + def env_ids(self) -> torch.Tensor: + """Return the stable row IDs owned by this monitor.""" + return self._env_ids.clone() + + def observe( + self, + samples: Sequence[DynamicSettleSample], + *, + elapsed_steps: int, + ) -> DynamicSettleState: + """Consume one raw speed observation when the configured cadence is due. + + Args: + samples: One speed sample per monitored entity. + elapsed_steps: Steps advanced by the caller since post-policy start. + + Returns: + Per-row stable, settled, timeout, and velocity metadata. + """ + if type(elapsed_steps) is not int or elapsed_steps < 0: + raise ValueError("elapsed_steps must be a non-negative integer.") + if elapsed_steps < self._last_elapsed_steps: + raise ValueError("elapsed_steps must be monotonic.") + normalized = tuple(samples) + if not normalized or not all( + isinstance(sample, DynamicSettleSample) for sample in normalized + ): + raise ValueError("samples must contain DynamicSettleSample values.") + if len({sample.entity_id for sample in normalized}) != len(normalized): + raise ValueError("samples must use unique entity IDs.") + for sample in normalized: + if sample.linear_speed.shape[0] != self._env_ids.numel(): + raise ValueError("Every sample batch must match env_ids length.") + if sample.linear_speed.device != self._env_ids.device: + raise ValueError("Samples and env_ids must share a device.") + + duplicate = elapsed_steps == self._last_elapsed_steps + due = elapsed_steps >= self.cfg.min_steps and ( + self._last_checked_steps < 0 + or elapsed_steps - self._last_checked_steps >= self.cfg.check_interval_steps + or elapsed_steps >= self.cfg.max_steps + ) + checked = due and not duplicate and not self._timeout.all() + if checked: + linear = torch.cat([sample.linear_speed for sample in normalized], dim=1) + angular = torch.cat([sample.angular_speed for sample in normalized], dim=1) + finite = torch.isfinite(linear).all(dim=1) & torch.isfinite(angular).all( + dim=1 + ) + self._max_linear = torch.where( + torch.isfinite(linear), linear, torch.full_like(linear, float("inf")) + ).amax(dim=1) + self._max_angular = torch.where( + torch.isfinite(angular), + angular, + torch.full_like(angular, float("inf")), + ).amax(dim=1) + stable = ( + finite + & (self._max_linear <= self.cfg.linear_velocity_threshold) + & (self._max_angular <= self.cfg.angular_velocity_threshold) + ) + active = ~self._settled & ~self._timeout + self._stable_counts = torch.where( + active & stable, + self._stable_counts + 1, + torch.where( + active, torch.zeros_like(self._stable_counts), self._stable_counts + ), + ) + self._settled |= active & ( + self._stable_counts >= self.cfg.required_stable_checks + ) + self._observation_count += 1 + self._last_checked_steps = elapsed_steps + + if elapsed_steps >= self.cfg.max_steps: + self._timeout |= ~self._settled + self._last_elapsed_steps = elapsed_steps + return self._state(elapsed_steps=elapsed_steps, checked=checked) + + def _state(self, *, elapsed_steps: int, checked: bool) -> DynamicSettleState: + """Build an owned state snapshot.""" + return DynamicSettleState( + env_ids=self._env_ids, + elapsed_steps=elapsed_steps, + observation_count=self._observation_count, + checked=checked, + stable_counts=self._stable_counts, + settled_mask=self._settled, + timeout_mask=self._timeout, + max_linear_speed=self._max_linear, + max_angular_speed=self._max_angular, + ) + + +__all__ = [ + "DynamicSettleMonitor", + "DynamicSettleMonitorCfg", + "DynamicSettleSample", + "DynamicSettleState", +] diff --git a/embodichain/lab/gym/envs/tasks/__init__.py b/embodichain/lab/gym/envs/tasks/__init__.py deleted file mode 100644 index 48ad24d72..000000000 --- a/embodichain/lab/gym/envs/tasks/__init__.py +++ /dev/null @@ -1,89 +0,0 @@ -# ---------------------------------------------------------------------------- -# 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. -# ---------------------------------------------------------------------------- - -"""Deprecation shim — task environments have moved to ``embodichain_tasks``. - -This module re-exports all task classes from the new ``embodichain_tasks`` -package for backward compatibility. It will be removed in a future version. - -.. deprecated:: - Import from ``embodichain_tasks`` directly instead of - ``embodichain.lab.gym.envs.tasks``. -""" - -from __future__ import annotations - -import warnings - -warnings.warn( - "embodichain.lab.gym.envs.tasks is deprecated. " - "Import from embodichain_tasks instead.", - DeprecationWarning, - stacklevel=2, -) - -try: - from embodichain_tasks.tableware.base_agent_env import BaseAgentEnv # noqa: F401 - from embodichain_tasks.tableware.pour_water.pour_water import ( # noqa: F401 - PourWaterEnv, - PourWaterAgentEnv, - ) - from embodichain_tasks.tableware.rearrangement import ( # noqa: F401 - RearrangementEnv, - RearrangementAgentEnv, - ) - from embodichain_tasks.tableware.stack_blocks_two import ( - StackBlocksTwoEnv, - ) # noqa: F401 - from embodichain_tasks.tableware.stack_cups import StackCupsEnv # noqa: F401 - from embodichain_tasks.tableware.scoop_ice import ScoopIce # noqa: F401 - from embodichain_tasks.tableware.blocks_ranking_rgb import ( # noqa: F401 - BlocksRankingRGBEnv, - ) - from embodichain_tasks.tableware.blocks_ranking_size import ( # noqa: F401 - BlocksRankingSizeEnv, - ) - from embodichain_tasks.tableware.match_object_container import ( # noqa: F401 - MatchObjectContainerEnv, - ) - from embodichain_tasks.tableware.place_object_drawer import ( # noqa: F401 - PlaceObjectDrawerEnv, - ) - from embodichain_tasks.rl.push_cube import PushCubeEnv # noqa: F401 - from embodichain_tasks.rl.basic.cart_pole import CartPoleEnv # noqa: F401 - from embodichain_tasks.special.simple_task import SimpleTaskEnv # noqa: F401 - - __all__ = [ - "BaseAgentEnv", - "PourWaterEnv", - "PourWaterAgentEnv", - "RearrangementEnv", - "RearrangementAgentEnv", - "StackBlocksTwoEnv", - "StackCupsEnv", - "ScoopIce", - "BlocksRankingRGBEnv", - "BlocksRankingSizeEnv", - "MatchObjectContainerEnv", - "PlaceObjectDrawerEnv", - "PushCubeEnv", - "CartPoleEnv", - "SimpleTaskEnv", - ] -except ImportError: - # embodichain_tasks is not installed — tasks will be discovered via - # entry_points instead. - __all__: list[str] = [] diff --git a/embodichain/lab/gym/utils/gym_utils.py b/embodichain/lab/gym/utils/gym_utils.py index cf3c1086b..c3df4bba8 100644 --- a/embodichain/lab/gym/utils/gym_utils.py +++ b/embodichain/lab/gym/utils/gym_utils.py @@ -17,6 +17,7 @@ from __future__ import annotations import os +from pathlib import Path import numpy as np import torch import dexsim @@ -393,13 +394,26 @@ def cat_tensor_with_ids( return out -def config_to_cfg(config: dict, manager_modules: list = None) -> "EmbodiedEnvCfg": +def config_to_cfg( + config: dict, + manager_modules: list | None = None, + *, + source_path: str | os.PathLike[str] | None = None, + expert_program_path_override: str | os.PathLike[str] | None = None, +) -> "EmbodiedEnvCfg": """Parser configuration file into cfgs for env initialization. Args: config (dict): The configuration dictionary containing robot, sensor, light, background, and interactive objects. manager_modules (list): List of module paths for dataset, event, observation, and reward managers. If not provided, uses default module paths. + source_path: Optional path of the Gym configuration source file. A + relative top-level ``expert_program_path`` is resolved from this + file's directory. Without it, relative paths use the current + working directory. + expert_program_path_override: Optional explicit program path. This is + selected instead of the Gym-config path and resolves from the + process working directory. Returns: EmbodiedEnvCfg: A configuration object for initializing the environment. @@ -446,6 +460,49 @@ class ComponentCfg: if key not in config: log_error(f"Missing required config key: {key}") + configured_expert_program_path = config.get("expert_program_path") + if expert_program_path_override is not None or "expert_program_path" in config: + if expert_program_path_override is not None: + expert_program_path = expert_program_path_override + expert_program_base_dir = None + if not isinstance(expert_program_path, (str, os.PathLike)): + raise TypeError("expert_program_path must be a string or path.") + else: + expert_program_path = configured_expert_program_path + expert_program_base_dir = ( + None if source_path is None else Path(source_path).expanduser().parent + ) + if type(expert_program_path) is not str: + raise TypeError("expert_program_path must be an exact string.") + expert_program_path_text = os.fspath(expert_program_path) + if not expert_program_path_text or ( + expert_program_path_text != expert_program_path_text.strip() + ): + raise ValueError( + "expert_program_path must be a non-empty string without outer " + "whitespace." + ) + from embodichain.lab.gym.envs.expert_program.loader import ( + load_expert_program, + ) + from embodichain.lab.gym.utils.registration import get_env_spec + + env_spec = get_env_spec(config["id"]) + registration = env_spec.expert_program_registration + if registration is None: + raise ValueError( + f"Environment {config['id']!r} does not register an Expert " + "Program integration catalog." + ) + registration.assert_unchanged() + expert_program = load_expert_program( + expert_program_path_text, + base_dir=expert_program_base_dir, + validation_context=registration.catalog, + ) + registration.catalog.preflight(expert_program) + env_cfg.expert_program = expert_program + env_cfg.max_episode_steps = config.get("max_episode_steps", 300) env_cfg.num_envs = config.get("num_envs", 1) @@ -1021,16 +1078,21 @@ def build_env_cfg_from_args( tuple[EmbodiedEnvCfg, dict, dict]: A tuple containing the environment configuration object, the merged gym configuration dictionary, and the action configuration dictionary. """ + from embodichain.utils.config_paths import resolve_config_path from embodichain.utils.utility import load_config from embodichain.lab.gym.envs import EmbodiedEnvCfg - gym_config = load_config(args.gym_config) + gym_config_source_path = resolve_config_path(args.gym_config) + gym_config = load_config(gym_config_source_path) gym_config = merge_args_with_gym_config(args, gym_config) if gym_config_modifier is not None: gym_config_modifier(gym_config) cfg: EmbodiedEnvCfg = config_to_cfg( - gym_config, manager_modules=get_manager_modules() + gym_config, + manager_modules=get_manager_modules(), + source_path=gym_config_source_path, + expert_program_path_override=getattr(args, "expert_program", None), ) cfg.filter_visual_rand = args.filter_visual_rand cfg.filter_dataset_saving = args.filter_dataset_saving diff --git a/embodichain/lab/gym/utils/registration.py b/embodichain/lab/gym/utils/registration.py index 2fce236aa..d571317a0 100644 --- a/embodichain/lab/gym/utils/registration.py +++ b/embodichain/lab/gym/utils/registration.py @@ -37,6 +37,9 @@ if TYPE_CHECKING: from embodichain.lab.gym.envs import BaseEnv, EmbodiedEnvCfg + from embodichain.lab.gym.envs.expert_program import ( + SimulationExpertProgramRegistration, + ) _logger = logging.getLogger(__name__) @@ -48,12 +51,27 @@ def __init__( cls: Type[BaseEnv], max_episode_steps=None, default_kwargs: dict = None, + expert_program_registration: SimulationExpertProgramRegistration | None = None, ): """A specification for a Embodied environment.""" + if expert_program_registration is not None: + from embodichain.lab.gym.envs.expert_program import ( + SimulationExpertProgramRegistration, + ) + + if ( + type(expert_program_registration) + is not SimulationExpertProgramRegistration + ): + raise TypeError( + "expert_program_registration must be exactly " + "SimulationExpertProgramRegistration or None." + ) self.uid = uid self.cls = cls self.max_episode_steps = max_episode_steps self.default_kwargs = {} if default_kwargs is None else default_kwargs + self.expert_program_registration = expert_program_registration def make(self, **kwargs): _kwargs = self.default_kwargs.copy() @@ -76,7 +94,11 @@ def gym_spec(self): def register( - name: str, cls: Type[BaseEnv], max_episode_steps=None, default_kwargs: dict = None + name: str, + cls: Type[BaseEnv], + max_episode_steps=None, + default_kwargs: dict = None, + expert_program_registration: SimulationExpertProgramRegistration | None = None, ): """Register a Embodied environment.""" @@ -88,7 +110,11 @@ def register( if not (issubclass(cls, BaseEnv) or issubclass(cls, BaseEnv)): raise TypeError(f"Env {name} must inherit from BaseEnv or BaseEnv") REGISTERED_ENVS[name] = EnvSpec( - name, cls, max_episode_steps=max_episode_steps, default_kwargs=default_kwargs + name, + cls, + max_episode_steps=max_episode_steps, + default_kwargs=default_kwargs, + expert_program_registration=expert_program_registration, ) @@ -146,6 +172,16 @@ def make(env_id, **kwargs): return env +def get_env_spec(env_id: str) -> EnvSpec: + """Return one registered environment specification or fail closed.""" + if type(env_id) is not str or not env_id or env_id != env_id.strip(): + raise ValueError("env_id must be a non-empty string without outer whitespace.") + try: + return REGISTERED_ENVS[env_id] + except KeyError as exc: + raise KeyError(f"Env {env_id!r} not found in registry.") from exc + + def build_env(env_id: str, base_env_cfg: EmbodiedEnvCfg): """Create an environment from a registered env id. @@ -172,7 +208,14 @@ def make_vec(env_id, **kwargs): return env -def register_env(uid: str, max_episode_steps=None, override=False, **kwargs): +def register_env( + uid: str, + max_episode_steps=None, + override=False, + *, + expert_program_registration: SimulationExpertProgramRegistration | None = None, + **kwargs, +): """A decorator to register Embodied environments. Args: @@ -193,13 +236,28 @@ def register_env(uid: str, max_episode_steps=None, override=False, **kwargs): ) def _register_env(cls): - cls = register_env_function(cls, uid, override, max_episode_steps, **kwargs) + cls = register_env_function( + cls, + uid, + override, + max_episode_steps, + expert_program_registration=expert_program_registration, + **kwargs, + ) return cls return _register_env -def register_env_function(cls, uid, override=False, max_episode_steps=None, **kwargs): +def register_env_function( + cls, + uid, + override=False, + max_episode_steps=None, + *, + expert_program_registration: SimulationExpertProgramRegistration | None = None, + **kwargs, +): if uid in REGISTERED_ENVS: if override: from gymnasium.envs.registration import registry @@ -216,6 +274,7 @@ def register_env_function(cls, uid, override=False, max_episode_steps=None, **kw cls, max_episode_steps=max_episode_steps, default_kwargs=deepcopy(kwargs), + expert_program_registration=expert_program_registration, ) # Register for gym diff --git a/embodichain/lab/scripts/run_env.py b/embodichain/lab/scripts/run_env.py index 880d040fc..78cce4723 100644 --- a/embodichain/lab/scripts/run_env.py +++ b/embodichain/lab/scripts/run_env.py @@ -17,6 +17,7 @@ from __future__ import annotations import argparse +import json import os import select import sys @@ -289,6 +290,16 @@ def generate_function( f"Episode {time_id} attempt {attempt}/{max_attempts} failed: " f"{result.terminal_reason}. Discarding {result.length} frames." ) + if debug_mode: + log_warning( + "Failed demo trace: " + + json.dumps( + result.to_metadata(), + allow_nan=False, + sort_keys=True, + separators=(",", ":"), + ) + ) return False @@ -741,6 +752,18 @@ def _create_parser() -> argparse.ArgumentParser: add_env_launcher_args_to_parser(parser, require_gym_config=True) parser.set_defaults(viser_image_fps=None) + parser.add_argument( + "--expert-program", + type=str, + default=None, + help="Path to a declarative Expert Program (.json, .yaml, or .yml).", + ) + parser.add_argument( + "--debug-mode", + action="store_true", + help="Log the structured trace for each failed demo attempt.", + ) + parser.add_argument( "--replay", action="store_true", diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index 86e00f80e..82a247584 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -31,10 +31,20 @@ from .affordance import ( Affordance, AntipodalAffordance, + ArticulationOperationAffordance, + ArticulationOperationTarget, AssembleAffordance, InteractionPoints, + PressAffordance, + SlideAffordance, + TwistAffordance, +) +from .bindings import ( + ActionBinding, + EndpointBinding, + JointPositionTarget, + RuntimeEndpointTarget, ) -from .bindings import ActionBinding, ResolvedActionBinding, ResolvedControlPart from .control import ( ActionControlOverrides, ControlCommand, @@ -45,32 +55,106 @@ ) from .core import AtomicAction, ObjectSemantics, SkillDescriptor from .effects import StateDelta -from .engine import ( - AtomicActionEngine, - get_registered_actions, - register_action, - unregister_action, -) +from .engine import AtomicActionEngine from .execution import ( + EffectExpectationResult, EffectVerificationRequest, + EffectVerificationResult, ExecutionEvent, ExecutionEventKind, + ExecutionPlanAttempt, ExecutionSession, ExecutionStatus, ExecutionTick, - JointCommand, + HeldObjectGuardRequest, + HeldObjectGuardResult, + PhaseEffectGateRequest, + PhaseEffectGateResult, +) +from .goals import ( + ActionGoal, + ObjectActionGoal, + PoseGoalValue, + SceneArticulationOperationGeometry, + SceneEntityPose, +) +from .invocation import ( + ActionInvocation, + ActionOptions, + PhaseEffectGateRequirement, + ResolvedActionRequest, ) -from .goals import ActionGoal, ObjectActionGoal, PoseGoalValue, SceneEntityPose -from .invocation import ActionInvocation, ActionOptions, ResolvedActionRequest from .plans import ( ActionPlan, CompiledTrajectory, + EffectVerificationRequirement, PlannerDiagnostics, TimedTrajectory, TrajectorySegment, ) from .policies import DynamicCollisionMode, MotionPolicy, RecoveryPolicy +from .requirements import ( + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + DisjointResourceSlots, + DisjointSlotEndpoints, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + INVERSE_KINEMATICS_CAPABILITY, + JOINT_POSITION_CAPABILITY, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) from .runtime import ActionPlanningServices +from .runtime_commands import ( + EndpointCommand, + JointPositionPayload, + RuntimeCommandFrame, + RuntimeCommandPayload, + TimedCommandSequence, +) +from .transports import EndpointCommandRouter, EndpointCommandTransport +from .tracking import ( + BASE_POSE_CHANNEL, + JOINT_POSITION_CHANNEL, + WHOLE_BODY_POSE_CHANNEL, + EndpointTrackingChannelBinding, + EndpointTrackingFeedbackAddress, + FeedbackTerminalAcceptance, + InFlightTrackingPolicy, + JointPositionTrackingEvaluator, + JointPositionTrackingMetric, + JointPositionTrackingProjector, + JointPositionTrackingState, + PlanningContextTrackingFeedbackProvider, + PoseTrackingEvaluator, + PoseTrackingMetric, + PoseTrackingState, + TerminalAcceptance, + TimedTerminalAcceptance, + TimedTrackingSequence, + TrackingCommandProjector, + TrackingEvaluation, + TrackingEvaluatorRegistry, + TrackingFeedbackAddress, + TrackingFeedbackBatch, + TrackingFeedbackProvider, + TrackingFeedbackProviderRegistry, + TrackingFeedbackSourceRef, + TrackingFrame, + TrackingMetricCfg, + TrackingMetricEvaluator, + TrackingPolicy, + TrackingProjectorRef, + TrackingProjectorRegistry, + TrackingRuntime, + TrackingSetpoint, + TrackingState, + WholeBodyPoseTrackingEvaluator, + WholeBodyPoseTrackingMetric, + WholeBodyPoseTrackingState, +) from .primitives import ( AssembleGoal, BUILTIN_ACTION_TYPES, @@ -92,6 +176,9 @@ MoveHeldObjectOptions, MoveJoints, MoveJointsOptions, + OperateArticulation, + OperateArticulationGoal, + OperateArticulationOptions, PickUp, PickUpOptions, Place, @@ -100,6 +187,12 @@ Press, PressGoal, PressOptions, + Slide, + SlideGoal, + SlideOptions, + Twist, + TwistGoal, + TwistOptions, ) from .runner import ( CommandAcknowledgement, @@ -111,8 +204,10 @@ ExecutionClock, ExecutionRunner, ExecutionRunnerCfg, + HeldObjectGuardVerifier, MonotonicExecutionClock, ObservationProvider, + PhaseEffectGateVerifier, RunnerStatus, RunnerStep, RunnerStepCallback, @@ -125,9 +220,11 @@ SimulationExecutionAdapter, ) from .state import ( + ArticulationJointState, CoordinatedHeldObjectState, EntityState, HeldObjectState, + ObservedArticulationJointState, PlanningContext, RobotObservation, SceneSnapshot, @@ -137,18 +234,22 @@ __all__ = [ "ActionBinding", "ActionControlOverrides", - "ActionGoal", "ActionInvocation", "ActionOptions", "ActionPlan", "ActionPlanningServices", "Affordance", "AntipodalAffordance", + "ArticulationOperationAffordance", + "ArticulationOperationTarget", + "ArticulationJointState", "AssembleAffordance", "AssembleGoal", "AtomicAction", "AtomicActionEngine", "BUILTIN_ACTION_TYPES", + "BATCH_INVERSE_KINEMATICS_CAPABILITY", + "CARTESIAN_POSE_CAPABILITY", "CompiledTrajectory", "CommandAcknowledgement", "CommandAckStatus", @@ -157,7 +258,6 @@ "CommandSink", "ControlCommand", "ControlPartCommandProfile", - "CoordinatedHeldObjectState", "CoordinatedPickGoal", "CoordinatedPickment", "CoordinatedPickmentOptions", @@ -165,28 +265,57 @@ "CoordinatedPlacementGoal", "CoordinatedPlacementOptions", "DynamicCollisionMode", + "DisjointResourceSlots", + "DisjointSlotEndpoints", "EndEffectorPoseGoal", + "EndpointBinding", + "EndpointCommand", + "EndpointCommandRouter", + "EndpointCommandTransport", "EntityState", + "EffectExpectationResult", "EffectVerificationRequest", + "EffectVerificationRequirement", + "EffectVerificationResult", "EffectVerifier", "ExecutionClock", "ExecutionEvent", "ExecutionEventKind", + "ExecutionPlanAttempt", "ExecutionRunner", "ExecutionRunnerCfg", "ExecutionSession", "ExecutionStatus", "ExecutionTick", + "HeldObjectGuardRequest", + "HeldObjectGuardResult", + "PhaseEffectGateRequest", + "PhaseEffectGateRequirement", + "PhaseEffectGateResult", + "PhaseEffectGateVerifier", + "HeldObjectGuardVerifier", + "EndpointTrackingChannelBinding", + "EndpointTrackingFeedbackAddress", + "FeedbackTerminalAcceptance", "GRASP_COMMAND", + "GRASP_CAPABILITY", "GraspGoal", "HandOver", "HandOverOptions", "HeldObjectPoseGoal", "HeldObjectState", + "FORWARD_KINEMATICS_CAPABILITY", + "INVERSE_KINEMATICS_CAPABILITY", + "InFlightTrackingPolicy", "InteractionPoints", "JointPositionGoal", - "JointCommand", "JointPositionCommand", + "JointPositionPayload", + "JointPositionTarget", + "JOINT_POSITION_CAPABILITY", + "JOINT_POSITION_CHANNEL", + "JointPositionTrackingMetric", + "JointPositionTrackingState", "MotionPolicy", "MonotonicExecutionClock", "MoveEndEffector", @@ -199,6 +328,10 @@ "ObjectSemantics", "OPEN_COMMAND", "ObservationProvider", + "ObservedArticulationJointState", + "OperateArticulation", + "OperateArticulationGoal", + "OperateArticulationOptions", "PickUp", "PickUpOptions", "Place", @@ -207,29 +340,74 @@ "PlannerDiagnostics", "PlanningContext", "PoseGoalValue", + "PoseTrackingMetric", + "PoseTrackingState", "Press", + "PressAffordance", "PressGoal", "PressOptions", + "SlideAffordance", + "Slide", + "SlideGoal", + "SlideOptions", + "Twist", + "TwistGoal", + "TwistOptions", "RecoveryPolicy", "RigidObjectSceneProvider", "RigidObjectSceneProviderCfg", "ResolvedActionRequest", - "ResolvedActionBinding", - "ResolvedControlPart", "RobotObservation", + "RuntimeCommandFrame", + "RuntimeCommandPayload", + "RuntimeEndpointTarget", "RunnerStatus", "RunnerStep", "RunnerStepCallback", "SceneProvider", + "SceneArticulationOperationGeometry", "SceneSnapshot", "SceneSnapshotSupplier", "SceneEntityPose", "SkillDescriptor", + "SkillBindingContract", + "SkillEndpointRequirement", + "SkillResourceSlot", "StateDelta", "SimulationExecutionAdapter", "TaskState", + "TimedCommandSequence", + "TimedTerminalAcceptance", + "TimedTrackingSequence", "TimedTrajectory", + "TerminalAcceptance", "TrajectorySegment", + "TrackingCommandProjector", + "TrackingEvaluation", + "TrackingEvaluatorRegistry", + "TrackingFeedbackAddress", + "TrackingFeedbackBatch", + "TrackingFeedbackProvider", + "TrackingFeedbackProviderRegistry", + "TrackingFeedbackSourceRef", + "TrackingFrame", + "TrackingMetricCfg", + "TrackingMetricEvaluator", + "TrackingPolicy", + "TrackingProjectorRef", + "TrackingProjectorRegistry", + "TrackingRuntime", + "TrackingSetpoint", + "TrackingState", + "BASE_POSE_CHANNEL", + "JointPositionTrackingEvaluator", + "JointPositionTrackingProjector", + "PlanningContextTrackingFeedbackProvider", + "PoseTrackingEvaluator", + "WHOLE_BODY_POSE_CHANNEL", + "WholeBodyPoseTrackingEvaluator", + "WholeBodyPoseTrackingMetric", + "WholeBodyPoseTrackingState", "get_registered_actions", "register_action", "unregister_action", diff --git a/embodichain/lab/sim/atomic_actions/affordance.py b/embodichain/lab/sim/atomic_actions/affordance.py index fdab5e91f..7a08dccdc 100644 --- a/embodichain/lab/sim/atomic_actions/affordance.py +++ b/embodichain/lab/sim/atomic_actions/affordance.py @@ -16,8 +16,14 @@ from __future__ import annotations +from collections.abc import Callable + import torch +from collections.abc import Mapping +from copy import deepcopy from dataclasses import dataclass, field +import math +from types import MappingProxyType from typing import Any, TYPE_CHECKING from embodichain.toolkits.graspkit.pg_grasp import ( @@ -63,7 +69,11 @@ def get_batch_size(self) -> int: @dataclass class AntipodalAffordance(Affordance): - """Antipodal grasp affordance for parallel-jaw grippers.""" + """Antipodal grasp affordance for parallel-jaw grippers. + + The affordance owns only target-local triangle-mesh data. Simulator entity + handles and live poses belong to scene grounding, not semantic geometry. + """ mesh_vertices: torch.Tensor | None = None """Object mesh vertices, shape [N, 3].""" @@ -114,16 +124,27 @@ def get_valid_grasp_poses( [0, 0, -1], dtype=torch.float32 ), object_part: str = "center", + grasp_cost_fn: ( + Callable[[torch.Tensor, torch.Tensor, torch.Tensor], torch.Tensor] | None + ) = None, ) -> list[tuple[torch.Tensor, torch.Tensor]]: if self._generator is None: self._init_generator() approach_direction = self._resolve_approach_direction(approach_direction) results = [] for i, obj_pose in enumerate(obj_poses): + pose_cost_fn = None + if grasp_cost_fn is not None: + pose_cost_fn = lambda grasp_poses, costs: grasp_cost_fn( + obj_pose, + grasp_poses, + costs, + ) is_success, grasp_poses, _, costs = self._generator.get_valid_grasp_poses( object_pose=obj_pose, approach_direction=approach_direction, object_part=object_part, + pose_cost_fn=pose_cost_fn, ) if grasp_poses.shape == (4, 4): grasp_poses = grasp_poses.unsqueeze(0) @@ -172,15 +193,41 @@ def get_best_grasp_poses( [0, 0, -1], dtype=torch.float32 ), ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: + """Return the best antipodal grasp for each object pose. + + Args: + obj_poses: Batched object poses with shape ``(B, 4, 4)``. + approach_direction: One shared ``(3,)`` world-frame direction or + per-object directions with shape ``(B, 3)``. + + Returns: + A success mask, best grasp poses, and gripper opening lengths with + batch dimension ``B``. + + Raises: + ValueError: If ``approach_direction`` has an incompatible shape. + """ if self._generator is None: self._init_generator() approach_direction = self._resolve_approach_direction(approach_direction) + if approach_direction.shape == (3,): + approach_directions = approach_direction.unsqueeze(0).expand( + obj_poses.shape[0], -1 + ) + elif approach_direction.shape == (obj_poses.shape[0], 3): + approach_directions = approach_direction + else: + raise ValueError( + "approach_direction must have shape (3,) or " + f"({obj_poses.shape[0]}, 3), got " + f"{tuple(approach_direction.shape)}." + ) grasp_xpos_list: list[torch.Tensor] = [] is_success_list: list[bool] = [] open_length_list: list[float] = [] for i, obj_pose in enumerate(obj_poses): is_success, grasp_xpos, open_length = self._generator.get_grasp_poses( - obj_pose, approach_direction + obj_pose, approach_directions[i] ) if is_success: grasp_xpos_list.append(grasp_xpos.unsqueeze(0)) @@ -203,6 +250,277 @@ def get_best_grasp_poses( return is_success_t, grasp_xpos, open_length_t +@dataclass +class TwistAffordance(Affordance): + """Target-local grasp point and rotation-axis geometry for twisting.""" + + grasp_position: tuple[float, float, float] = field(kw_only=True) + """Explicit target-local center of the gripper contact region.""" + + axis_origin: tuple[float, float, float] = field(kw_only=True) + """Explicit point on the rotation axis in the target-local frame.""" + + twist_axis: torch.Tensor = field( + default_factory=lambda: torch.tensor([0.0, 1.0, 0.0]) + ) + """Twist axis expressed in the target object's local frame.""" + + joint_name: str | None = None + """Optional stable articulation-joint name associated with the axis.""" + + joint_limits: tuple[float, float] | None = None + """Optional lower and upper angular limits in radians.""" + + def __post_init__(self) -> None: + if ( + not isinstance(self.twist_axis, torch.Tensor) + or self.twist_axis.shape != (3,) + or not torch.isfinite(self.twist_axis).all() + ): + raise ValueError("TwistAffordance.twist_axis must be a finite (3,) tensor.") + if torch.linalg.vector_norm(self.twist_axis) <= 1.0e-6: + raise ValueError("TwistAffordance.twist_axis must be non-zero.") + self.twist_axis = self.twist_axis.clone() + self.grasp_position = _validate_local_point( + self.grasp_position, "TwistAffordance.grasp_position" + ) + self.axis_origin = _validate_local_point( + self.axis_origin, "TwistAffordance.axis_origin" + ) + _validate_joint_metadata(self.joint_name, self.joint_limits) + + def get_grasp_pose(self, target_pose: torch.Tensor) -> torch.Tensor: + """Construct a deterministic world grasp pose from local geometry. + + The pose z-axis follows :attr:`twist_axis`. The remaining axes are + formed with an adaptive reference so the result is always in SO(3). + + Returns: + Batched world-frame grasp poses with shape ``(B, 4, 4)``. + + Raises: + ValueError: If ``target_pose`` is not a batched pose tensor. + """ + if target_pose.dim() != 3 or target_pose.shape[1:] != (4, 4): + raise ValueError("Target pose must have shape (B, 4, 4).") + target_pose = target_pose.to(dtype=torch.float32) + device = target_pose.device + twist_axis = self.twist_axis.to(device=device, dtype=torch.float32) + twist_axis = twist_axis / torch.linalg.vector_norm(twist_axis) + + z_axis = torch.matmul(target_pose[:, :3, :3], twist_axis) + z_axis = torch.nn.functional.normalize(z_axis, dim=1) + x_axis, y_axis = _orthogonal_xy_from_z(z_axis) + + grasp_pose = torch.eye(4, dtype=torch.float32, device=device).repeat( + target_pose.shape[0], 1, 1 + ) + grasp_pose[:, :3, 0] = x_axis + grasp_pose[:, :3, 1] = y_axis + grasp_pose[:, :3, 2] = z_axis + local_grasp = torch.tensor( + self.grasp_position, dtype=torch.float32, device=device + ) + grasp_pose[:, :3, 3] = ( + torch.matmul(target_pose[:, :3, :3], local_grasp) + target_pose[:, :3, 3] + ) + return grasp_pose + + +@dataclass +class SlideAffordance(AntipodalAffordance): + """Target-local antipodal grasp and translation-axis geometry. + + The positive translation-axis direction denotes approaching and pushing + the articulated part closed. Pulling moves in the opposite direction. + The mesh describes the actual graspable contact surface. The target pose is + supplied separately by :class:`~.goals.SceneEntityPose` or a pose snapshot. + """ + + mesh_vertices: torch.Tensor = field(kw_only=True) + """Target-local vertices for the graspable contact surface.""" + + mesh_triangles: torch.Tensor = field(kw_only=True) + """Triangle indices for the graspable contact surface.""" + + translation_axis: torch.Tensor = field( + default_factory=lambda: torch.tensor([0.0, 1.0, 0.0]) + ) + """Approach and push/close direction in the articulation-link frame.""" + + joint_name: str | None = None + """Optional stable prismatic-joint name associated with the link.""" + + joint_limits: tuple[float, float] | None = None + """Optional lower and upper translation limits in metres.""" + + def __post_init__(self) -> None: + if self.mesh_vertices.dim() != 2 or self.mesh_vertices.shape[1] != 3: + raise ValueError("SlideAffordance.mesh_vertices must have shape (N, 3).") + if ( + self.mesh_vertices.shape[0] == 0 + or not torch.isfinite(self.mesh_vertices).all() + ): + raise ValueError( + "SlideAffordance.mesh_vertices must be finite and non-empty." + ) + if self.mesh_triangles.dim() != 2 or self.mesh_triangles.shape[1] != 3: + raise ValueError("SlideAffordance.mesh_triangles must have shape (M, 3).") + if ( + not isinstance(self.translation_axis, torch.Tensor) + or self.translation_axis.shape != (3,) + or not torch.isfinite(self.translation_axis).all() + ): + raise ValueError( + "SlideAffordance.translation_axis must be a finite (3,) tensor." + ) + if torch.linalg.vector_norm(self.translation_axis) <= 1.0e-6: + raise ValueError("SlideAffordance.translation_axis must be non-zero.") + self.translation_axis = self.translation_axis.clone() + _validate_joint_metadata(self.joint_name, self.joint_limits) + + +@dataclass +class PressAffordance(Affordance): + """Explicit target-local contact point and pressing direction.""" + + press_axis: torch.Tensor = field( + default_factory=lambda: torch.tensor([0.0, 0.0, 1.0]) + ) + """Press direction expressed in the target object's local frame.""" + + press_position: tuple[float, float, float] = field(kw_only=True) + """Explicit local-frame point on the pressable contact surface.""" + + def __post_init__(self) -> None: + if ( + not isinstance(self.press_axis, torch.Tensor) + or self.press_axis.shape != (3,) + or not torch.isfinite(self.press_axis).all() + ): + raise ValueError("PressAffordance.press_axis must be a finite (3,) tensor.") + if torch.linalg.vector_norm(self.press_axis) <= 1.0e-6: + raise ValueError("PressAffordance.press_axis must be non-zero.") + self.press_axis = self.press_axis.clone() + self.press_position = _validate_local_point( + self.press_position, "PressAffordance.press_position" + ) + + def get_press_pose( + self, + target_pose: torch.Tensor, + press_position: tuple[float, float, float] | None = None, + ) -> torch.Tensor: + """Construct a press pose at the configured surface point. + + The end-effector z-axis follows :attr:`press_axis` in world space. An + adaptive reference produces an orthonormal, right-handed frame. + + Args: + target_pose: Current target world pose with shape ``(B, 4, 4)``. + press_position: Optional per-call exact local-frame press position. + It overrides :attr:`press_position`. + + Returns: + Batched world-frame press poses with shape ``(B, 4, 4)``. + + Raises: + ValueError: If an input has an invalid shape or value. + """ + if target_pose.dim() != 3 or target_pose.shape[1:] != (4, 4): + raise ValueError("Target pose must have shape (B, 4, 4).") + target_pose = target_pose.to(dtype=torch.float32) + device = target_pose.device + press_axis = self.press_axis.to(device=device, dtype=torch.float32) + press_axis = press_axis / torch.linalg.vector_norm(press_axis) + configured_position = self._validate_press_position( + press_position, + field_name="press_position", + ) + configured_position = ( + self.press_position if configured_position is None else configured_position + ) + local_press_position = torch.tensor( + configured_position, + dtype=torch.float32, + device=device, + ) + + z_axis = torch.matmul(target_pose[:, :3, :3], press_axis) + z_axis = torch.nn.functional.normalize(z_axis, dim=1) + x_axis, y_axis = _orthogonal_xy_from_z(z_axis) + + press_pose = torch.eye(4, dtype=torch.float32, device=device).repeat( + target_pose.shape[0], 1, 1 + ) + press_pose[:, :3, 0] = x_axis + press_pose[:, :3, 1] = y_axis + press_pose[:, :3, 2] = z_axis + press_pose[:, :3, 3] = ( + torch.matmul(target_pose[:, :3, :3], local_press_position) + + target_pose[:, :3, 3] + ) + return press_pose + + @staticmethod + def _validate_press_position( + value: tuple[float, float, float] | None, + *, + field_name: str, + ) -> tuple[float, float, float] | None: + """Validate and normalize an optional local-frame press position.""" + if value is None: + return None + position = torch.as_tensor(value, dtype=torch.float32) + if position.shape != (3,) or not torch.isfinite(position).all(): + raise ValueError(f"{field_name} must be a finite (x, y, z) tuple.") + return tuple(float(component) for component in position) + + +def _orthogonal_xy_from_z(z_axis: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor]: + """Complete normalized z axes into right-handed orthonormal frames.""" + basis = torch.eye(3, dtype=z_axis.dtype, device=z_axis.device) + reference_indices = torch.argmin(torch.abs(z_axis), dim=1) + reference = basis[reference_indices] + y_axis = torch.nn.functional.normalize( + torch.linalg.cross(reference, z_axis, dim=1), dim=1 + ) + x_axis = torch.nn.functional.normalize( + torch.linalg.cross(y_axis, z_axis, dim=1), dim=1 + ) + return x_axis, y_axis + + +def _validate_local_point( + value: tuple[float, float, float], field_name: str +) -> tuple[float, float, float]: + """Validate and normalize one target-local 3D point.""" + point = torch.as_tensor(value, dtype=torch.float32) + if point.shape != (3,) or not torch.isfinite(point).all(): + raise ValueError(f"{field_name} must be a finite (x, y, z) tuple.") + return tuple(float(component) for component in point) + + +def _validate_joint_metadata( + joint_name: str | None, + joint_limits: tuple[float, float] | None, +) -> None: + """Validate optional articulation joint metadata.""" + if joint_name is not None and ( + not isinstance(joint_name, str) or not joint_name.strip() + ): + raise ValueError("joint_name must be a non-empty string when provided.") + if joint_limits is None: + return + limits = torch.as_tensor(joint_limits, dtype=torch.float32) + if ( + limits.shape != (2,) + or not torch.isfinite(limits).all() + or limits[0] > limits[1] + ): + raise ValueError("joint_limits must be finite and ordered (lower, upper).") + + @dataclass class InteractionPoints(Affordance): """Batch of 3D interaction points on an object surface.""" @@ -236,21 +554,319 @@ def get_approach_direction(self, point_idx: int) -> torch.Tensor: ) +def _owned_se3_offset(value: torch.Tensor, *, field_name: str) -> torch.Tensor: + """Validate and own one affordance-local homogeneous transform.""" + if not isinstance(value, torch.Tensor): + raise TypeError(f"{field_name} must be a torch.Tensor.") + if value.shape != (4, 4): + raise ValueError(f"{field_name} must have shape (4, 4).") + if not value.is_floating_point(): + raise TypeError(f"{field_name} must use a floating-point dtype.") + if not torch.isfinite(value).all(): + raise ValueError(f"{field_name} must contain only finite values.") + checked = value.to(dtype=torch.float64) + bottom = checked.new_tensor((0.0, 0.0, 0.0, 1.0)) + if not torch.allclose(checked[3], bottom, atol=1.0e-6, rtol=0.0): + raise ValueError(f"{field_name} must have bottom row [0, 0, 0, 1].") + rotation = checked[:3, :3] + if not torch.allclose( + rotation.T @ rotation, + torch.eye(3, dtype=checked.dtype, device=checked.device), + atol=1.0e-6, + rtol=0.0, + ) or not torch.isclose( + torch.linalg.det(rotation), + checked.new_tensor(1.0), + atol=1.0e-6, + rtol=0.0, + ): + raise ValueError(f"{field_name} must contain a proper SE(3) rotation.") + return value.clone() + + +def _finite_scalar(value: float, *, field_name: str) -> float: + """Return one finite non-boolean scalar as a float.""" + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise TypeError(f"{field_name} must be a finite scalar.") + normalized = float(value) + if not math.isfinite(normalized): + raise ValueError(f"{field_name} must be finite.") + return normalized + + +@dataclass(frozen=True, slots=True) +class ArticulationOperationTarget: + """Named joint target and handle-relative operation displacement. + + ``displacement`` is deliberately explicit: it is the full signed handle + stroke from the live source joint position captured during semantic + grounding to ``target_position``. Recovery replans scale this stroke by + the remaining live joint progress. + """ + + target_position: float + """Absolute desired articulation joint position.""" + + displacement: float + """Signed operation displacement from the currently observed handle pose.""" + + def __post_init__(self) -> None: + object.__setattr__( + self, + "target_position", + _finite_scalar( + self.target_position, + field_name="ArticulationOperationTarget.target_position", + ), + ) + object.__setattr__( + self, + "displacement", + _finite_scalar( + self.displacement, + field_name="ArticulationOperationTarget.displacement", + ), + ) + + def snapshot(self) -> ArticulationOperationTarget: + """Return an independently constructed immutable target.""" + return ArticulationOperationTarget(self.target_position, self.displacement) + + +@dataclass(eq=False) +class ArticulationOperationAffordance(Affordance): + """Declarative handle geometry for one articulated joint operation. + + The four offsets are expressed in the live handle frame. During semantic + grounding the approach and contact poses are ``handle @ offset``. The + operation and retract poses additionally insert a local translation of + ``operation_axis * displacement * position_scale`` before their offsets. + This keeps task code free of pose-matrix construction; the semantic + compiler copies the geometry into a late-bound atomic goal. + """ + + joint_id: str = "" + """Canonical joint identifier written to the atomic goal and effect.""" + + approach_offset: torch.Tensor = field( + default_factory=lambda: torch.eye(4, dtype=torch.float32) + ) + contact_offset: torch.Tensor = field( + default_factory=lambda: torch.eye(4, dtype=torch.float32) + ) + operation_offset: torch.Tensor = field( + default_factory=lambda: torch.eye(4, dtype=torch.float32) + ) + retract_offset: torch.Tensor = field( + default_factory=lambda: torch.eye(4, dtype=torch.float32) + ) + operation_axis: torch.Tensor = field( + default_factory=lambda: torch.tensor((1.0, 0.0, 0.0), dtype=torch.float32) + ) + """Unit operation direction expressed in the observed handle frame.""" + + position_scale: float = 1.0 + """Positive conversion from declared displacement units to pose metres.""" + + semantic_targets: Mapping[str, ArticulationOperationTarget] = field( + default_factory=dict + ) + """Optional stable target names mapped to position/displacement pairs.""" + + def __post_init__(self) -> None: + if ( + type(self.joint_id) is not str + or not self.joint_id + or self.joint_id != self.joint_id.strip() + ): + raise ValueError( + "ArticulationOperationAffordance.joint_id must be a non-empty " + "canonical identifier." + ) + for field_name in ( + "approach_offset", + "contact_offset", + "operation_offset", + "retract_offset", + ): + setattr( + self, + field_name, + _owned_se3_offset( + getattr(self, field_name), + field_name=f"ArticulationOperationAffordance.{field_name}", + ), + ) + axis = self.operation_axis + if not isinstance(axis, torch.Tensor): + raise TypeError( + "ArticulationOperationAffordance.operation_axis must be a tensor." + ) + if axis.shape != (3,) or not axis.is_floating_point(): + raise ValueError( + "ArticulationOperationAffordance.operation_axis must be a " + "floating tensor with shape (3,)." + ) + if not torch.isfinite(axis).all(): + raise ValueError( + "ArticulationOperationAffordance.operation_axis must be finite." + ) + norm = torch.linalg.vector_norm(axis) + if float(norm) <= torch.finfo(axis.dtype).eps: + raise ValueError( + "ArticulationOperationAffordance.operation_axis must be non-zero." + ) + self.operation_axis = (axis / norm).clone() + self.position_scale = _finite_scalar( + self.position_scale, + field_name="ArticulationOperationAffordance.position_scale", + ) + if self.position_scale <= 0.0: + raise ValueError( + "ArticulationOperationAffordance.position_scale must be positive." + ) + if not isinstance(self.semantic_targets, Mapping): + raise TypeError( + "ArticulationOperationAffordance.semantic_targets must be a mapping." + ) + targets: dict[str, ArticulationOperationTarget] = {} + for target_id, target in self.semantic_targets.items(): + if ( + type(target_id) is not str + or not target_id + or target_id != target_id.strip() + ): + raise ValueError( + "Articulation operation target IDs must be non-empty canonical " + "identifiers." + ) + if type(target) is not ArticulationOperationTarget: + raise TypeError( + "semantic_targets values must be exact " + "ArticulationOperationTarget values." + ) + targets[target_id] = target.snapshot() + self.semantic_targets = MappingProxyType(targets) + + def resolve_target(self, target_id: str) -> ArticulationOperationTarget: + """Return an owned named target or raise with deterministic candidates.""" + if type(target_id) is not str or not target_id: + raise ValueError("target_id must be a non-empty string.") + try: + target = self.semantic_targets[target_id] + except KeyError as exc: + raise KeyError( + f"Unknown articulation target {target_id!r}; available targets are " + f"{sorted(self.semantic_targets)}." + ) from exc + return target.snapshot() + + def __deepcopy__(self, memo: dict[int, object]) -> ArticulationOperationAffordance: + """Copy immutable configuration despite ``MappingProxyType`` storage.""" + existing = memo.get(id(self)) + if existing is not None: + assert isinstance(existing, ArticulationOperationAffordance) + return existing + copied = ArticulationOperationAffordance( + object_label=self.object_label, + custom_config=deepcopy(self.custom_config, memo), + joint_id=self.joint_id, + approach_offset=self.approach_offset, + contact_offset=self.contact_offset, + operation_offset=self.operation_offset, + retract_offset=self.retract_offset, + operation_axis=self.operation_axis, + position_scale=self.position_scale, + semantic_targets={ + target_id: target.snapshot() + for target_id, target in self.semantic_targets.items() + }, + ) + memo[id(self)] = copied + return copied + + def ground_poses( + self, + handle_pose: torch.Tensor, + *, + displacement: float, + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor]: + """Ground four end-effector poses from a fresh handle observation. + + Args: + handle_pose: Live handle pose with shape ``(4, 4)`` or ``(B, 4, 4)``. + displacement: Signed displacement from this observed handle pose. + + Returns: + Approach, contact, operation, and retract pose batches. + """ + if not isinstance(handle_pose, torch.Tensor): + raise TypeError("handle_pose must be a torch.Tensor.") + if handle_pose.shape == (4, 4): + handles = handle_pose.unsqueeze(0) + elif ( + handle_pose.dim() == 3 + and handle_pose.shape[0] > 0 + and handle_pose.shape[-2:] == (4, 4) + ): + handles = handle_pose + else: + raise ValueError("handle_pose must have shape (4, 4) or (B, 4, 4).") + if not handle_pose.is_floating_point() or not torch.isfinite(handle_pose).all(): + raise ValueError("handle_pose must be a finite floating tensor.") + displacement = _finite_scalar(displacement, field_name="displacement") + offsets = tuple( + getattr(self, field_name) + .to( + device=handles.device, + dtype=handles.dtype, + ) + .unsqueeze(0) + .expand(handles.shape[0], -1, -1) + for field_name in ( + "approach_offset", + "contact_offset", + "operation_offset", + "retract_offset", + ) + ) + translation = ( + torch.eye( + 4, + dtype=handles.dtype, + device=handles.device, + ) + .unsqueeze(0) + .repeat(handles.shape[0], 1, 1) + ) + translation[:, :3, 3] = self.operation_axis.to( + device=handles.device, + dtype=handles.dtype, + ) * (displacement * self.position_scale) + approach = torch.bmm(handles, offsets[0]) + contact = torch.bmm(handles, offsets[1]) + moved_handle = torch.bmm(handles, translation) + operation = torch.bmm(moved_handle, offsets[2]) + retract = torch.bmm(moved_handle, offsets[3]) + return tuple(pose.clone() for pose in (approach, contact, operation, retract)) + + @dataclass class AssembleAffordance(Affordance): """Affordance describing how an assemble object fits onto a base object. - The base object anchors the assembly: its world pose is read at planning - time from :attr:`base_object_entity` so the target tracks a moved base. The - assemble object is the part that is picked up and placed; its target pose is - ``base_pose @ assemble_to_base_pose``. + The affordance stores the relative assembly relation. Canonical planning + supplies the base object's snapshot pose through ``AssembleGoal.base_pose``; + :attr:`base_object_entity` is retained only as a deprecated direct-core + fallback when that goal field is omitted. The assemble object's target pose + is ``base_pose @ assemble_to_base_pose``. """ base_object_label: str = "" """Label of the base object the assemble object is placed onto.""" base_object_entity: BatchEntity | None = None - """Simulation entity for the base object; its pose anchors the assembly.""" + """Legacy live base entity used only when ``AssembleGoal.base_pose`` is absent.""" assemble_object_label: str = "" """Label of the assemble object that is picked up and placed.""" @@ -262,7 +878,7 @@ class AssembleAffordance(Affordance): default_factory=lambda: torch.eye(4, dtype=torch.float32) ) """Pose of the assemble object relative to the base object frame, shape - ``(4, 4)`` or ``(n_envs, 4, 4)``.""" + ``(4, 4)`` or ``(num_envs, 4, 4)``.""" def get_assemble_object_pose(self, base_pose: torch.Tensor) -> torch.Tensor: """Return the assemble-object target pose for a given base-object pose. @@ -270,28 +886,53 @@ def get_assemble_object_pose(self, base_pose: torch.Tensor) -> torch.Tensor: The assemble object is placed at ``base_pose @ assemble_to_base_pose``. Args: - base_pose: Base-object pose with shape ``(4, 4)`` or ``(n_envs, 4, 4)``. + base_pose: Base-object pose with shape ``(4, 4)`` or ``(num_envs, 4, 4)``. Returns: - Assemble-object target pose with shape ``(n_envs, 4, 4)``. + Assemble-object target pose with shape ``(num_envs, 4, 4)``. + + Raises: + TypeError: If either pose value is not a tensor. + ValueError: If either pose has an unsupported shape or batch size. """ + if not isinstance(base_pose, torch.Tensor): + raise TypeError("base_pose must be a torch.Tensor.") base_pose = base_pose.to(dtype=torch.float32) - if base_pose.dim() == 2: + if base_pose.shape == (4, 4): base_pose = base_pose.unsqueeze(0) - n_envs = base_pose.shape[0] + elif ( + base_pose.dim() != 3 + or base_pose.shape[0] == 0 + or base_pose.shape[-2:] != (4, 4) + ): + raise ValueError("base_pose must have shape (4, 4) or (num_envs, 4, 4).") + num_envs = base_pose.shape[0] + if not isinstance(self.assemble_to_base_pose, torch.Tensor): + raise TypeError("assemble_to_base_pose must be a torch.Tensor.") rel = self.assemble_to_base_pose.to( device=base_pose.device, dtype=torch.float32 ) - if rel.dim() == 2: - rel = rel.unsqueeze(0).repeat(n_envs, 1, 1) + if rel.shape == (4, 4): + rel = rel.unsqueeze(0).repeat(num_envs, 1, 1) + elif rel.dim() != 3 or rel.shape[-2:] != (4, 4) or rel.shape[0] == 0: + raise ValueError( + "assemble_to_base_pose must have shape (4, 4), (1, 4, 4), " + "or (num_envs, 4, 4)." + ) elif rel.shape[0] == 1: - rel = rel.repeat(n_envs, 1, 1) + rel = rel.repeat(num_envs, 1, 1) + elif rel.shape[0] != num_envs: + raise ValueError( + "assemble_to_base_pose batch size must match base_pose batch size." + ) return torch.bmm(base_pose, rel) __all__ = [ "Affordance", "AntipodalAffordance", + "ArticulationOperationAffordance", + "ArticulationOperationTarget", "InteractionPoints", "AssembleAffordance", ] diff --git a/embodichain/lab/sim/atomic_actions/bindings.py b/embodichain/lab/sim/atomic_actions/bindings.py index 5257c5035..9717304d4 100644 --- a/embodichain/lab/sim/atomic_actions/bindings.py +++ b/embodichain/lab/sim/atomic_actions/bindings.py @@ -14,279 +14,524 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Semantic-role to robot control-part bindings for atomic actions.""" +"""Generic runtime endpoint bindings consumed by atomic actions.""" from __future__ import annotations +from abc import ABC, abstractmethod +from collections.abc import Hashable +from copy import deepcopy from dataclasses import dataclass, field from types import MappingProxyType -from typing import Mapping +from typing import Mapping, TypeVar import torch -from .control import ControlCommand, JointPositionCommand +from .control import ControlCommand +from .tracking import ( + EndpointTrackingChannelBinding, + EndpointTrackingFeedbackAddress, +) -def _normalize_resource_map( - values: Mapping[str, str], +def _validate_identifier(value: str, *, field_name: str) -> str: + """Validate and return one strict identifier.""" + if not isinstance(value, str) or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +def _normalize_identifiers( + values: frozenset[str], *, field_name: str, -) -> Mapping[str, str]: - """Validate and freeze a semantic-role resource mapping.""" +) -> frozenset[str]: + """Validate and freeze an identifier set.""" + if isinstance(values, (str, bytes)): + raise TypeError(f"{field_name} must be an iterable of strings.") + try: + normalized = frozenset(values) + except TypeError as exc: + raise TypeError(f"{field_name} must be an iterable of strings.") from exc + for value in normalized: + _validate_identifier(value, field_name=field_name) + return normalized + + +def _snapshot_commands( + values: Mapping[str, ControlCommand], +) -> Mapping[str, ControlCommand]: + """Validate semantic endpoint commands and own their snapshots.""" if not isinstance(values, Mapping): - raise TypeError(f"{field_name} must be a mapping.") - normalized: dict[str, str] = {} - for role, resource in values.items(): - if not isinstance(role, str) or not role.strip(): - raise ValueError(f"{field_name} roles must be non-empty strings.") - if not isinstance(resource, str) or not resource.strip(): - raise ValueError(f"{field_name} resources must be non-empty strings.") - normalized[role] = resource - return MappingProxyType(normalized) + raise TypeError("EndpointBinding.commands must be a mapping.") + commands: dict[str, ControlCommand] = {} + for name, command in values.items(): + _validate_identifier(name, field_name="EndpointBinding command names") + if not isinstance(command, ControlCommand): + raise TypeError( + "EndpointBinding.commands values must be ControlCommand instances." + ) + snapshot = command.snapshot() + if type(snapshot) is not type(command) or snapshot is command: + raise TypeError( + "ControlCommand.snapshot() must return an independently owned " + "value of the same command type." + ) + commands[name] = snapshot + return MappingProxyType(commands) -@dataclass(frozen=True, slots=True) -class ActionBinding: - """Bind semantic action roles to names from ``Robot.control_parts``. - - A role such as ``primary``, ``source`` or ``destination`` is an - action-defined semantic participant slot. It describes the responsibility - a resource has within that action and is not itself a robot resource. - Actions publish their required slots through ``manipulator_roles`` and - ``end_effector_roles``. Role names are scoped independently to those two - maps, so matching names associate an arm and hand/tool with the same - functional participant without making the maps interchangeable. - - ``primary`` has no inherent left/right, ordering, or default-control-part - meaning. Only the compiler or application binding layer needs to map it to - concrete robot control-part names such as ``left_arm`` and ``left_hand``. - - Every mapping value is a key from the current robot's ``control_parts`` - configuration. This value object validates the mapping shape; the - :class:`~embodichain.lab.sim.atomic_actions.AtomicActionEngine` validates - the names against its owned robot before planning. ``end_effectors`` refers - to actuated tool/hand control parts, not TCP or kinematic frame names. - """ +def _snapshot_tracking_channels( + values: Mapping[str, EndpointTrackingChannelBinding], + *, + target: RuntimeEndpointTarget, +) -> Mapping[str, EndpointTrackingChannelBinding]: + """Validate and own endpoint-local tracking-channel bindings.""" + if not isinstance(values, Mapping): + raise TypeError("EndpointBinding.tracking_channels must be a mapping.") + channels: dict[str, EndpointTrackingChannelBinding] = {} + for channel_id, binding in values.items(): + _validate_identifier( + channel_id, + field_name="EndpointBinding tracking channel IDs", + ) + if not isinstance(binding, EndpointTrackingChannelBinding): + raise TypeError( + "EndpointBinding.tracking_channels values must be " + "EndpointTrackingChannelBinding instances." + ) + if binding.channel_id != channel_id: + raise ValueError( + f"Tracking channel key {channel_id!r} disagrees with its binding " + f"channel {binding.channel_id!r}." + ) + snapshot = binding.snapshot() + if snapshot is binding: + raise TypeError( + "EndpointTrackingChannelBinding.snapshot() must return an " + "independently owned value." + ) + address = snapshot.source.address + if ( + isinstance(address, EndpointTrackingFeedbackAddress) + and address.target.address_fingerprint != target.address_fingerprint + ): + raise ValueError( + f"Tracking channel {channel_id!r} addresses a different runtime " + "endpoint target." + ) + channels[channel_id] = snapshot + return MappingProxyType(channels) - manipulators: Mapping[str, str] = field(default_factory=dict) - """Manipulator control-part names keyed by semantic role.""" - end_effectors: Mapping[str, str] = field(default_factory=dict) - """Tool or hand control-part names keyed by semantic role.""" +def _validate_target_fingerprint( + target: RuntimeEndpointTarget, + *, + field_name: str, +) -> Hashable: + """Return one hashable, snapshot-stable target address fingerprint.""" + fingerprint = target.address_fingerprint + try: + hash(fingerprint) + except TypeError as exc: + raise TypeError(f"{field_name} must be hashable.") from exc + return fingerprint + + +class RuntimeEndpointTarget(ABC): + """Stable controller destination produced by an endpoint adapter. + + Targets contain immutable addressing data only. Live controllers, sockets, + simulator entities, and other process-owned handles belong to an + endpoint-command transport rather than this value. + """ - def __post_init__(self) -> None: - object.__setattr__( - self, - "manipulators", - _normalize_resource_map(self.manipulators, field_name="manipulators"), - ) - object.__setattr__( - self, - "end_effectors", - _normalize_resource_map(self.end_effectors, field_name="end_effectors"), - ) + @property + @abstractmethod + def transport_id(self) -> str: + """Return the registered transport kind used by this target.""" - def manipulator(self, role: str = "primary") -> str: - """Return the manipulator control-part name bound to ``role``. + @property + @abstractmethod + def target_id(self) -> str: + """Return the destination identifier within its transport.""" - Args: - role: Semantic manipulator role. + @property + def address_fingerprint(self) -> Hashable: + """Return the stable controller-address and safe-hold fingerprint. + + The default covers the exact target type and transport-scoped + destination. Target types whose hold footprint depends on additional + immutable addressing fields must override this property and include + those fields. Replans and explicit revisions may replace payloads, but + they may not change this fingerprint in place. + """ + return type(self), self.transport_id, self.target_id - Returns: - Key from the current robot's ``control_parts`` mapping. + def snapshot(self) -> RuntimeEndpointTarget: + """Return an independently owned target snapshot.""" + return deepcopy(self) - Raises: - KeyError: If the requested role is not bound. - """ - try: - return self.manipulators[role] - except KeyError as exc: - raise KeyError(f"No manipulator is bound to role {role!r}.") from exc - def end_effector(self, role: str = "primary") -> str: - """Return the tool/hand control-part name bound to ``role``. +@dataclass(frozen=True, slots=True) +class JointPositionTarget(RuntimeEndpointTarget): + """Joint-position destination backed by one robot control part.""" - Args: - role: Semantic end-effector role. + TRANSPORT_ID = "robot.joint_position" - Returns: - Key from the current robot's ``control_parts`` mapping. + control_part: str + joint_ids: tuple[int, ...] - Raises: - KeyError: If the requested role is not bound. - """ - try: - return self.end_effectors[role] - except KeyError as exc: - raise KeyError(f"No end effector is bound to role {role!r}.") from exc + def __post_init__(self) -> None: + _validate_identifier( + self.control_part, + field_name="JointPositionTarget.control_part", + ) + joint_ids = tuple(self.joint_ids) + if not joint_ids or not all( + isinstance(joint_id, int) + and not isinstance(joint_id, bool) + and joint_id >= 0 + for joint_id in joint_ids + ): + raise ValueError( + "JointPositionTarget.joint_ids must contain non-negative integers." + ) + if len(set(joint_ids)) != len(joint_ids): + raise ValueError("JointPositionTarget.joint_ids must be unique.") + object.__setattr__(self, "joint_ids", joint_ids) + @property + def transport_id(self) -> str: + """Return the built-in joint-position transport identifier.""" + return self.TRANSPORT_ID -@dataclass(frozen=True, slots=True) -class ResolvedControlPart: - """One engine-validated robot control part. + @property + def target_id(self) -> str: + """Return the robot control-part destination.""" + return self.control_part - Instances are produced by engine-owned planning services. They keep - robot-specific indices out of :class:`ActionBinding` and agent-facing - invocation schemas. - """ + @property + def address_fingerprint(self) -> Hashable: + """Return the destination plus the joints that must remain holdable.""" + return ( + type(self), + self.transport_id, + self.target_id, + self.joint_ids, + ) - name: str - """Key from ``Robot.control_parts``.""" - joint_ids: tuple[int, ...] - """Full-robot joint indices belonging to this control part.""" +TargetT = TypeVar("TargetT", bound=RuntimeEndpointTarget) + +@dataclass(frozen=True, slots=True) +class EndpointBinding: + """One action-local endpoint resolved to a runtime controller target.""" + + slot_id: str + endpoint_id: str + resource_id: str + adapter_id: str + target: RuntimeEndpointTarget + task_state_key: str | None = None + """Symbolic task-state key; direct-core defaults to ``target.target_id``.""" + + tracking_channels: Mapping[str, EndpointTrackingChannelBinding] = field( + default_factory=dict + ) + capabilities: frozenset[str] = frozenset() commands: Mapping[str, ControlCommand] = field(default_factory=dict) - """Engine-profile commands, including invocation-level overrides.""" + claim_tokens: frozenset[str] = frozenset() + joint_ids: tuple[int, ...] = () def __post_init__(self) -> None: - if not isinstance(self.name, str) or not self.name.strip(): - raise ValueError("ResolvedControlPart.name must be a non-empty string.") + _validate_identifier(self.slot_id, field_name="EndpointBinding.slot_id") + _validate_identifier( + self.endpoint_id, + field_name="EndpointBinding.endpoint_id", + ) + _validate_identifier( + self.resource_id, + field_name="EndpointBinding.resource_id", + ) + _validate_identifier(self.adapter_id, field_name="EndpointBinding.adapter_id") + if not isinstance(self.target, RuntimeEndpointTarget): + raise TypeError("EndpointBinding.target must be a RuntimeEndpointTarget.") + target = self.target.snapshot() + if type(target) is not type(self.target) or target is self.target: + raise TypeError( + "RuntimeEndpointTarget.snapshot() must return an independently " + "owned value of the same target type." + ) + _validate_identifier( + target.transport_id, + field_name="RuntimeEndpointTarget.transport_id", + ) + _validate_identifier( + target.target_id, + field_name="RuntimeEndpointTarget.target_id", + ) + source_fingerprint = _validate_target_fingerprint( + self.target, + field_name="RuntimeEndpointTarget.address_fingerprint", + ) + target_fingerprint = _validate_target_fingerprint( + target, + field_name="RuntimeEndpointTarget.snapshot().address_fingerprint", + ) + if target_fingerprint != source_fingerprint: + raise ValueError( + "RuntimeEndpointTarget.snapshot() must preserve its address " + "fingerprint." + ) + object.__setattr__(self, "target", target) + task_state_key = ( + target.target_id if self.task_state_key is None else self.task_state_key + ) + _validate_identifier( + task_state_key, + field_name="EndpointBinding.task_state_key", + ) + object.__setattr__(self, "task_state_key", task_state_key) + object.__setattr__( + self, + "tracking_channels", + _snapshot_tracking_channels(self.tracking_channels, target=target), + ) + object.__setattr__( + self, + "capabilities", + _normalize_identifiers( + self.capabilities, + field_name="EndpointBinding.capabilities", + ), + ) + object.__setattr__(self, "commands", _snapshot_commands(self.commands)) + object.__setattr__( + self, + "claim_tokens", + _normalize_identifiers( + self.claim_tokens, + field_name="EndpointBinding.claim_tokens", + ), + ) joint_ids = tuple(self.joint_ids) - if not joint_ids or not all( - isinstance(joint_id, int) and joint_id >= 0 for joint_id in joint_ids + if not all( + isinstance(joint_id, int) + and not isinstance(joint_id, bool) + and joint_id >= 0 + for joint_id in joint_ids ): raise ValueError( - "ResolvedControlPart.joint_ids must contain non-negative integers." + "EndpointBinding.joint_ids must contain non-negative integers." ) if len(set(joint_ids)) != len(joint_ids): - raise ValueError("ResolvedControlPart.joint_ids must be unique.") - object.__setattr__(self, "joint_ids", joint_ids) - if not isinstance(self.commands, Mapping): - raise TypeError("ResolvedControlPart.commands must be a mapping.") - commands: dict[str, ControlCommand] = {} - for name, command in self.commands.items(): - if not isinstance(name, str) or not name.strip(): - raise ValueError("Control command names must be non-empty strings.") - if not isinstance(command, ControlCommand): - raise TypeError( - "ResolvedControlPart.commands values must be ControlCommand " - "instances." + raise ValueError("EndpointBinding.joint_ids must be unique.") + if isinstance(target, JointPositionTarget): + if joint_ids and joint_ids != target.joint_ids: + raise ValueError( + "EndpointBinding.joint_ids must match its JointPositionTarget." ) - commands[name] = command.snapshot() - object.__setattr__(self, "commands", MappingProxyType(commands)) + joint_ids = target.joint_ids + object.__setattr__(self, "joint_ids", joint_ids) @property - def dof(self) -> int: - """Return the number of joints in this control part.""" - return len(self.joint_ids) + def key(self) -> tuple[str, str]: + """Return the action-local ``(slot, endpoint)`` key.""" + return self.slot_id, self.endpoint_id - def with_command_overrides( - self, - overrides: Mapping[str, ControlCommand], - ) -> ResolvedControlPart: - """Return a snapshot with role-local semantic command overrides.""" - merged = dict(self.commands) - merged.update(overrides) - return ResolvedControlPart( - name=self.name, - joint_ids=self.joint_ids, - commands=merged, - ) + @property + def destination_key(self) -> tuple[str, str]: + """Return the transport-scoped physical destination key.""" + return self.target.transport_id, self.target.target_id + + def require_target(self, target_type: type[TargetT]) -> TargetT: + """Return the runtime target after an explicit type check.""" + if not isinstance(target_type, type) or not issubclass( + target_type, RuntimeEndpointTarget + ): + raise TypeError("target_type must be a RuntimeEndpointTarget subclass.") + if not isinstance(self.target, target_type): + raise TypeError( + f"Endpoint {self.slot_id}.{self.endpoint_id} uses " + f"{type(self.target).__name__}, expected {target_type.__name__}." + ) + return self.target.snapshot() def command(self, name: str) -> ControlCommand: - """Return an owned semantic command snapshot. - - Args: - name: Semantic command name, for example ``open`` or ``grasp``. - - Raises: - KeyError: If this control part does not define ``name``. - """ + """Return one owned semantic-command snapshot.""" try: command = self.commands[name] except KeyError as exc: raise KeyError( - f"Control part {self.name!r} has no command {name!r}. " - f"Available commands: {sorted(self.commands)}." + f"Endpoint {self.slot_id}.{self.endpoint_id} has no command " + f"{name!r}; available commands are {sorted(self.commands)}." ) from exc return command.snapshot() + def tracking_channel(self, channel_id: str) -> EndpointTrackingChannelBinding: + """Return one independently owned typed tracking-channel binding.""" + try: + binding = self.tracking_channels[channel_id] + except KeyError as exc: + raise KeyError( + f"Endpoint {self.slot_id}.{self.endpoint_id} has no tracking " + f"channel {channel_id!r}; available channels are " + f"{sorted(self.tracking_channels)}." + ) from exc + return binding.snapshot() + def joint_positions( self, name: str, *, - n_envs: int, + num_envs: int, device: torch.device | str, dtype: torch.dtype | None = None, ) -> torch.Tensor: """Resolve a named joint-position command for a planning batch.""" - try: - command = self.commands[name] - except KeyError as exc: - raise KeyError( - f"Control part {self.name!r} has no command {name!r}. " - f"Available commands: {sorted(self.commands)}." - ) from exc + from .control import JointPositionCommand + + target = self.require_target(JointPositionTarget) + command = self.command(name) if not isinstance(command, JointPositionCommand): raise TypeError( - f"Control command {name!r} on {self.name!r} is " - f"{type(command).__name__}, not JointPositionCommand." + f"Endpoint command {name!r} is {type(command).__name__}, not " + "JointPositionCommand." ) return command.resolve( - n_envs=n_envs, - control_dof=self.dof, + num_envs=num_envs, + control_dof=len(target.joint_ids), device=device, dtype=dtype, ) + def with_commands( + self, + overrides: Mapping[str, ControlCommand], + ) -> EndpointBinding: + """Return an endpoint snapshot with semantic-command overrides.""" + merged = dict(self.commands) + merged.update(overrides) + return EndpointBinding( + slot_id=self.slot_id, + endpoint_id=self.endpoint_id, + resource_id=self.resource_id, + adapter_id=self.adapter_id, + target=self.target, + task_state_key=self.task_state_key, + tracking_channels=self.tracking_channels, + capabilities=self.capabilities, + commands=merged, + claim_tokens=self.claim_tokens, + joint_ids=self.joint_ids, + ) -def _normalize_resolved_map( - values: Mapping[str, ResolvedControlPart], - *, - field_name: str, -) -> Mapping[str, ResolvedControlPart]: - """Validate and freeze a resolved semantic-role mapping.""" - if not isinstance(values, Mapping): - raise TypeError(f"{field_name} must be a mapping.") - normalized: dict[str, ResolvedControlPart] = {} - for role, resource in values.items(): - if not isinstance(role, str) or not role.strip(): - raise ValueError(f"{field_name} roles must be non-empty strings.") - if not isinstance(resource, ResolvedControlPart): - raise TypeError( - f"{field_name} values must be ResolvedControlPart instances." - ) - normalized[role] = resource - return MappingProxyType(normalized) + def snapshot(self) -> EndpointBinding: + """Return an independently owned endpoint-binding snapshot.""" + return EndpointBinding( + slot_id=self.slot_id, + endpoint_id=self.endpoint_id, + resource_id=self.resource_id, + adapter_id=self.adapter_id, + target=self.target, + task_state_key=self.task_state_key, + tracking_channels=self.tracking_channels, + capabilities=self.capabilities, + commands=self.commands, + claim_tokens=self.claim_tokens, + joint_ids=self.joint_ids, + ) @dataclass(frozen=True, slots=True) -class ResolvedActionBinding: - """Runtime control parts resolved from an :class:`ActionBinding`.""" +class ActionBinding: + """Engine-owned generic endpoint bindings for one atomic action call.""" - manipulators: Mapping[str, ResolvedControlPart] = field(default_factory=dict) - end_effectors: Mapping[str, ResolvedControlPart] = field(default_factory=dict) + owner_id: str + endpoints: tuple[EndpointBinding, ...] = () def __post_init__(self) -> None: - object.__setattr__( - self, - "manipulators", - _normalize_resolved_map( - self.manipulators, field_name="resolved manipulators" - ), + _validate_identifier(self.owner_id, field_name="ActionBinding.owner_id") + if isinstance(self.endpoints, (str, bytes)): + raise TypeError("ActionBinding.endpoints must be an iterable.") + try: + endpoints = tuple(self.endpoints) + except TypeError as exc: + raise TypeError("ActionBinding.endpoints must be an iterable.") from exc + if not all(isinstance(endpoint, EndpointBinding) for endpoint in endpoints): + raise TypeError( + "ActionBinding.endpoints values must be EndpointBinding instances." + ) + keys = [endpoint.key for endpoint in endpoints] + if len(set(keys)) != len(keys): + raise ValueError("ActionBinding endpoint keys must be unique.") + snapshots = tuple(endpoint.snapshot() for endpoint in endpoints) + object.__setattr__(self, "endpoints", snapshots) + + @property + def endpoint_keys(self) -> tuple[tuple[str, str], ...]: + """Return action-local endpoint keys in binding order.""" + return tuple(endpoint.key for endpoint in self.endpoints) + + @property + def targets(self) -> tuple[RuntimeEndpointTarget, ...]: + """Return unique owned runtime targets in binding order.""" + targets: list[RuntimeEndpointTarget] = [] + seen: set[tuple[str, str]] = set() + for endpoint in self.endpoints: + if endpoint.destination_key in seen: + continue + seen.add(endpoint.destination_key) + targets.append(endpoint.target.snapshot()) + return tuple(targets) + + def endpoint( + self, + slot_id: str, + endpoint_id: str, + ) -> EndpointBinding: + """Return one action-local resolved endpoint.""" + key = (slot_id, endpoint_id) + for endpoint in self.endpoints: + if endpoint.key == key: + return endpoint.snapshot() + raise KeyError( + f"No endpoint is bound to {slot_id}.{endpoint_id}; available endpoints " + f"are {list(self.endpoint_keys)}." ) - object.__setattr__( - self, - "end_effectors", - _normalize_resolved_map( - self.end_effectors, field_name="resolved end_effectors" + + def with_command_overrides( + self, + overrides: Mapping[tuple[str, str], Mapping[str, ControlCommand]], + ) -> ActionBinding: + """Return a binding snapshot with endpoint-scoped command overrides.""" + if not isinstance(overrides, Mapping): + raise TypeError("overrides must be a mapping.") + unknown = set(overrides).difference(self.endpoint_keys) + if unknown: + raise KeyError( + f"Command overrides reference unbound endpoints {sorted(unknown)}." + ) + return ActionBinding( + owner_id=self.owner_id, + endpoints=tuple( + ( + endpoint.with_commands(overrides[endpoint.key]) + if endpoint.key in overrides + else endpoint + ) + for endpoint in self.endpoints ), ) - def manipulator(self, role: str = "primary") -> ResolvedControlPart: - """Return the resolved manipulator for ``role``.""" - try: - return self.manipulators[role] - except KeyError as exc: - raise KeyError(f"No manipulator is bound to role {role!r}.") from exc - - def end_effector(self, role: str = "primary") -> ResolvedControlPart: - """Return the resolved tool/hand control part for ``role``.""" - try: - return self.end_effectors[role] - except KeyError as exc: - raise KeyError(f"No end effector is bound to role {role!r}.") from exc - -__all__ = ["ActionBinding", "ResolvedActionBinding", "ResolvedControlPart"] +__all__ = [ + "ActionBinding", + "EndpointBinding", + "JointPositionTarget", + "RuntimeEndpointTarget", +] diff --git a/embodichain/lab/sim/atomic_actions/control.py b/embodichain/lab/sim/atomic_actions/control.py index 80be94029..3af145d9c 100644 --- a/embodichain/lab/sim/atomic_actions/control.py +++ b/embodichain/lab/sim/atomic_actions/control.py @@ -44,13 +44,17 @@ class ControlCommand(ABC): def snapshot(self) -> ControlCommand: """Return an independently owned copy of this command.""" + @abstractmethod + def equivalent_to(self, other: ControlCommand) -> bool: + """Return whether ``other`` has exactly the same command semantics.""" + @dataclass(frozen=True, slots=True, eq=False, init=False) class JointPositionCommand(ControlCommand): """A semantic command represented by one or batched joint positions. ``positions`` has shape ``(control_dof,)`` or - ``(n_envs, control_dof)``. A one-dimensional command is broadcast to the + ``(num_envs, control_dof)``. A one-dimensional command is broadcast to the planning batch when resolved. """ @@ -62,7 +66,7 @@ def __init__(self, positions: torch.Tensor) -> None: if positions.dim() not in (1, 2) or positions.shape[-1] == 0: raise ValueError( "positions must have shape (control_dof,) or " - "(n_envs, control_dof), got " + "(num_envs, control_dof), got " f"{tuple(positions.shape)}." ) if not torch.isfinite(positions).all().item(): @@ -78,10 +82,16 @@ def snapshot(self) -> JointPositionCommand: """Return an independently owned command snapshot.""" return JointPositionCommand(self._positions) + def equivalent_to(self, other: ControlCommand) -> bool: + """Return whether ``other`` owns identical joint positions.""" + return isinstance(other, JointPositionCommand) and self._positions.equal( + other._positions + ) + def resolve( self, *, - n_envs: int, + num_envs: int, control_dof: int, device: torch.device | str, dtype: torch.dtype | None = None, @@ -89,20 +99,20 @@ def resolve( """Validate, move, and broadcast this command for a planning batch. Args: - n_envs: Number of selected environments. + num_envs: Number of selected environments. control_dof: Joint count of the resolved control part. device: Target planning device. dtype: Optional target dtype. Returns: - Independently owned tensor with shape ``(n_envs, control_dof)``. + Independently owned tensor with shape ``(num_envs, control_dof)``. Raises: ValueError: If the command shape does not match the control part or selected environment batch. """ - if not isinstance(n_envs, int) or n_envs < 1: - raise ValueError("n_envs must be a positive integer.") + if not isinstance(num_envs, int) or num_envs < 1: + raise ValueError("num_envs must be a positive integer.") if not isinstance(control_dof, int) or control_dof < 1: raise ValueError("control_dof must be a positive integer.") if self._positions.shape[-1] != control_dof: @@ -112,11 +122,11 @@ def resolve( ) resolved = self._positions.to(device=device, dtype=dtype) if resolved.dim() == 1: - return resolved.unsqueeze(0).expand(n_envs, -1).clone() - if resolved.shape[0] != n_envs: + return resolved.unsqueeze(0).expand(num_envs, -1).clone() + if resolved.shape[0] != num_envs: raise ValueError( f"Batched joint-position command has {resolved.shape[0]} " - f"environments, expected {n_envs}." + f"environments, expected {num_envs}." ) return resolved.clone() @@ -131,11 +141,20 @@ def _snapshot_commands( raise TypeError(f"{field_name} must be a mapping.") snapshots: dict[str, ControlCommand] = {} for name, command in commands.items(): - if not isinstance(name, str) or not name.strip(): - raise ValueError(f"{field_name} keys must be non-empty strings.") + if not isinstance(name, str) or not name or name != name.strip(): + raise ValueError( + f"{field_name} keys must be non-empty strings without outer " + "whitespace." + ) if not isinstance(command, ControlCommand): raise TypeError(f"{field_name} values must be ControlCommand instances.") - snapshots[name] = command.snapshot() + snapshot = command.snapshot() + if type(snapshot) is not type(command) or snapshot is command: + raise TypeError( + f"{field_name}[{name!r}].snapshot() must return an independently " + "owned value of the same ControlCommand type." + ) + snapshots[name] = snapshot return MappingProxyType(snapshots) @@ -176,65 +195,84 @@ def snapshot(self) -> ControlPartCommandProfile: return ControlPartCommandProfile(commands=self.commands) -def _snapshot_role_commands( - values: Mapping[str, Mapping[str, ControlCommand]], +def _snapshot_endpoint_commands( + values: Mapping[str, Mapping[str, Mapping[str, ControlCommand]]], *, field_name: str, -) -> Mapping[str, Mapping[str, ControlCommand]]: - """Validate and freeze role-scoped invocation command overrides.""" +) -> Mapping[str, Mapping[str, Mapping[str, ControlCommand]]]: + """Validate and freeze slot/endpoint-scoped command overrides.""" if not isinstance(values, Mapping): raise TypeError(f"{field_name} must be a mapping.") - snapshots: dict[str, Mapping[str, ControlCommand]] = {} - for role, commands in values.items(): - if not isinstance(role, str) or not role.strip(): - raise ValueError(f"{field_name} roles must be non-empty strings.") - snapshots[role] = _snapshot_commands( - commands, - field_name=f"{field_name}[{role!r}]", - ) - return MappingProxyType(snapshots) + slots: dict[str, Mapping[str, Mapping[str, ControlCommand]]] = {} + for slot_id, endpoints in values.items(): + if not isinstance(slot_id, str) or not slot_id or slot_id != slot_id.strip(): + raise ValueError( + f"{field_name} slot IDs must be non-empty strings without outer " + "whitespace." + ) + if not isinstance(endpoints, Mapping): + raise TypeError(f"{field_name}[{slot_id!r}] must be a mapping.") + endpoint_snapshots: dict[str, Mapping[str, ControlCommand]] = {} + for endpoint_id, commands in endpoints.items(): + if ( + not isinstance(endpoint_id, str) + or not endpoint_id + or endpoint_id != endpoint_id.strip() + ): + raise ValueError( + f"{field_name} endpoint IDs must be non-empty strings without " + "outer whitespace." + ) + endpoint_snapshots[endpoint_id] = _snapshot_commands( + commands, + field_name=f"{field_name}[{slot_id!r}][{endpoint_id!r}]", + ) + slots[slot_id] = MappingProxyType(endpoint_snapshots) + return MappingProxyType(slots) @dataclass(frozen=True, slots=True) class ActionControlOverrides: - """Per-invocation semantic command overrides keyed by binding role. + """Per-invocation semantic commands keyed by slot and endpoint. - The outer keys are action roles such as ``primary``, ``source`` or - ``destination``. The inner keys are semantic command names. The engine - applies these values after resolving the role to a concrete control part, - and the resulting commands are captured in the invocation revision's - immutable planning snapshot. + The first two keys match a skill's ``(slot_id, endpoint_id)`` contract. + The innermost mapping contains semantic command names. Overrides are + captured in the invocation revision's immutable planning snapshot. """ - manipulators: Mapping[str, Mapping[str, ControlCommand]] = field( - default_factory=dict - ) - end_effectors: Mapping[str, Mapping[str, ControlCommand]] = field( - default_factory=dict + endpoints: Mapping[ + str, + Mapping[str, Mapping[str, ControlCommand]], + ] = field( + default_factory=dict, ) def __post_init__(self) -> None: object.__setattr__( self, - "manipulators", - _snapshot_role_commands( - self.manipulators, - field_name="manipulators", - ), - ) - object.__setattr__( - self, - "end_effectors", - _snapshot_role_commands( - self.end_effectors, - field_name="end_effectors", + "endpoints", + _snapshot_endpoint_commands( + self.endpoints, + field_name="endpoints", ), ) @property def is_empty(self) -> bool: """Whether this invocation defines no command overrides.""" - return not self.manipulators and not self.end_effectors + return not self.endpoints + + def as_flat_mapping( + self, + ) -> Mapping[tuple[str, str], Mapping[str, ControlCommand]]: + """Return immutable overrides keyed by ``(slot_id, endpoint_id)``.""" + return MappingProxyType( + { + (slot_id, endpoint_id): commands + for slot_id, endpoints in self.endpoints.items() + for endpoint_id, commands in endpoints.items() + } + ) __all__ = [ diff --git a/embodichain/lab/sim/atomic_actions/core.py b/embodichain/lab/sim/atomic_actions/core.py index ee3b1f391..226182c27 100644 --- a/embodichain/lab/sim/atomic_actions/core.py +++ b/embodichain/lab/sim/atomic_actions/core.py @@ -22,6 +22,7 @@ from collections.abc import Mapping from copy import deepcopy from dataclasses import dataclass, field, replace +from functools import cached_property from typing import Any, ClassVar, Generic, TYPE_CHECKING import torch @@ -29,6 +30,7 @@ from embodichain.lab.sim.common import BatchEntity from .affordance import Affordance +from .bindings import EndpointBinding, JointPositionTarget from .effects import StateDelta from .goals import collect_scene_dependencies from .invocation import ( @@ -40,12 +42,26 @@ ) from .plans import ( ActionPlan, + EffectVerificationRequirement, PlannerDiagnostics, TimedTrajectory, TrajectorySegment, normalize_success_mask, ) from .policies import DynamicCollisionMode +from .requirements import SkillBindingContract +from .runtime_commands import ( + EndpointCommand, + JointPositionPayload, + RuntimeCommandFrame, + TimedCommandSequence, +) +from .tracking import ( + FeedbackTerminalAcceptance, + TimedTrackingSequence, + TrackingFrame, + TrackingSetpoint, +) if TYPE_CHECKING: from embodichain.lab.sim.objects import Robot @@ -70,9 +86,15 @@ def resolve_runtime_device(device: torch.device | str) -> torch.device: return resolved -@dataclass +@dataclass(frozen=True, slots=True, eq=False) class ObjectSemantics: - """Semantic and geometric information about an interaction object.""" + """Shallow-frozen semantic information about an interaction object. + + .. attention:: + Top-level fields cannot be rebound after construction. Nested + affordance and metadata objects may remain mutable but never establish + object identity. + """ affordance: Affordance """Affordance data describing supported interactions.""" @@ -89,6 +111,9 @@ class ObjectSemantics: entity: BatchEntity | None = None """Optional simulation entity used by deterministic grounding.""" + entity_id: str | None = None + """Stable scene identifier used by snapshot grounding and explicit identity.""" + def __post_init__(self) -> None: if not isinstance(self.affordance, Affordance): raise TypeError("affordance must be an Affordance instance.") @@ -98,9 +123,39 @@ def __post_init__(self) -> None: raise TypeError("properties must be a dict.") if not isinstance(self.label, str) or not self.label: raise ValueError("label must be a non-empty string.") + if self.entity_id is not None and ( + not isinstance(self.entity_id, str) or not self.entity_id.strip() + ): + raise ValueError("entity_id must be a non-empty string when set.") self.affordance.object_label = self.label +def _legacy_object_uid(semantics: ObjectSemantics) -> str | None: + """Return a valid legacy simulation UID without alias normalization.""" + uid = getattr(semantics.entity, "uid", None) + return uid if isinstance(uid, str) and uid.strip() else None + + +def _same_object_identity( + left: ObjectSemantics, + right: ObjectSemantics, +) -> bool: + """Return whether two semantic snapshots identify the same object.""" + if left is right: + return True + if left.entity_id is not None or right.entity_id is not None: + return ( + left.entity_id is not None + and right.entity_id is not None + and left.entity_id == right.entity_id + ) + left_uid = _legacy_object_uid(left) + right_uid = _legacy_object_uid(right) + if left_uid is not None or right_uid is not None: + return left_uid is not None and right_uid is not None and left_uid == right_uid + return left.entity is not None and left.entity is right.entity + + @dataclass(frozen=True, slots=True) class SkillDescriptor: """Machine-readable metadata for one registered atomic skill.""" @@ -108,9 +163,11 @@ class SkillDescriptor: skill_id: str goal_type: type[Any] | tuple[type[Any], ...] options_type: type[ActionOptions] - manipulator_roles: tuple[str, ...] = () - end_effector_roles: tuple[str, ...] = () agent_visible: bool = True + open_loop: bool = False + """Whether completion reports motion execution without physical-effect proof.""" + binding_contract: SkillBindingContract | None = None + """Explicit generic resource contract used by the semantic skill layer.""" def __post_init__(self) -> None: if not isinstance(self.skill_id, str) or not self.skill_id: @@ -126,13 +183,14 @@ def __post_init__(self) -> None: raise TypeError( "SkillDescriptor.options_type must be an ActionOptions subclass." ) - for field_name in ("manipulator_roles", "end_effector_roles"): - roles = tuple(getattr(self, field_name)) - if len(set(roles)) != len(roles) or not all( - isinstance(role, str) and role for role in roles - ): - raise ValueError(f"{field_name} must contain unique non-empty roles.") - object.__setattr__(self, field_name, roles) + if not isinstance(self.open_loop, bool): + raise TypeError("SkillDescriptor.open_loop must be a bool.") + if self.binding_contract is not None: + if not isinstance(self.binding_contract, SkillBindingContract): + raise TypeError( + "SkillDescriptor.binding_contract must be a " + "SkillBindingContract or None." + ) class AtomicAction(Generic[GoalT, OptionsT], ABC): @@ -152,15 +210,20 @@ class AtomicAction(Generic[GoalT, OptionsT], ABC): OptionsType: ClassVar[type[ActionOptions]] = ActionOptions """Concrete per-invocation runtime options accepted by this skill.""" - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) - """Required semantic manipulator roles.""" - - end_effector_roles: ClassVar[tuple[str, ...]] = () - """Required semantic end-effector roles.""" - agent_visible: ClassVar[bool] = True """Whether an Action Agent should expose this skill by default.""" + open_loop: ClassVar[bool] = False + """Whether the skill intentionally declares no verified physical effect.""" + + binding_contract: ClassVar[SkillBindingContract | None] = None + """Explicit robot-independent requirements for semantic discovery. + + Concrete action classes must declare this attribute in their own class + body to opt into the semantic catalog. Inheriting another action's contract + does not silently expose a new skill identifier. + """ + def __init_subclass__(cls, **kwargs: Any) -> None: """Reject skill classes that bypass framework-owned scene binding.""" super().__init_subclass__(**kwargs) @@ -208,7 +271,7 @@ def planning_services(self) -> ActionPlanningServices: if self._planning_services is None: raise RuntimeError( f"Atomic action {self.skill_id!r} is not bound to an " - "AtomicActionEngine. Register it or call engine.plan_action()." + "AtomicActionEngine. Register it with engine.register()." ) return self._planning_services @@ -227,6 +290,16 @@ def device(self) -> torch.device: """Return the concrete runtime device associated with the engine.""" return self.planning_services.device + @cached_property + def num_envs(self) -> int: + """Number of environments owned by the bound robot.""" + return int(self.robot.get_qpos().shape[0]) + + @cached_property + def robot_dof(self) -> int: + """Number of full-robot degrees of freedom.""" + return int(self.robot.dof) + def _bind(self, services: ActionPlanningServices) -> None: """Bind engine-owned planning services exactly once.""" if self._planning_services is services: @@ -237,14 +310,6 @@ def _bind(self, services: ActionPlanningServices) -> None: "AtomicActionEngine." ) self._planning_services = services - try: - self._on_bind() - except Exception: - self._planning_services = None - raise - - def _on_bind(self) -> None: - """Initialize implementation state that depends on engine resources.""" @classmethod def descriptor(cls) -> SkillDescriptor: @@ -253,9 +318,9 @@ def descriptor(cls) -> SkillDescriptor: skill_id=cls.skill_id, goal_type=cls.GoalType, options_type=cls.OptionsType, - manipulator_roles=cls.manipulator_roles, - end_effector_roles=cls.end_effector_roles, agent_visible=cls.agent_visible, + open_loop=cls.open_loop, + binding_contract=cls.__dict__.get("binding_contract"), ) def resolve_request( @@ -290,10 +355,12 @@ def resolve_request( f"Skill {self.skill_id!r} expects goal {expected}, got " f"{type(invocation.goal).__name__}." ) - for role in self.manipulator_roles: - invocation.binding.manipulator(role) - for role in self.end_effector_roles: - invocation.binding.end_effector(role) + contract = type(self).__dict__.get("binding_contract") + if contract is None: + raise ValueError( + f"Skill {self.skill_id!r} has no explicit SkillBindingContract." + ) + self.planning_services.validate_binding(invocation.binding, contract) options = ( self._default_options if invocation.skill_options is None @@ -304,22 +371,17 @@ def resolve_request( f"Skill {self.skill_id!r} expects options " f"{self.OptionsType.__name__}, got {type(options).__name__}." ) - required_planner = invocation.motion_policy.planner - configured_planner_name = self.planning_services.planner_name - if required_planner is not None and required_planner != configured_planner_name: - raise ValueError( - f"Motion policy requires planner {required_planner!r}, but this " - f"action uses {configured_planner_name!r}." - ) return ResolvedActionRequest( skill_id=invocation.skill_id, goal=invocation.goal, - binding=self.planning_services.resolve_binding( + binding=self.planning_services.apply_command_overrides( invocation.binding, invocation.control_overrides, ), motion_policy=invocation.motion_policy, + tracking_policy=invocation.tracking_policy, recovery_policy=invocation.recovery_policy, + phase_effect_gates=invocation.phase_effect_gates, skill_options=options, invocation_id=invocation.invocation_id, revision=invocation.revision, @@ -345,10 +407,12 @@ def require_goal( f"Skill {self.skill_id!r} received incompatible options " f"{type(request.skill_options).__name__}." ) - for role in self.manipulator_roles: - request.binding.manipulator(role) - for role in self.end_effector_roles: - request.binding.end_effector(role) + contract = type(self).__dict__.get("binding_contract") + if contract is None: + raise ValueError( + f"Skill {self.skill_id!r} has no explicit SkillBindingContract." + ) + self.planning_services.validate_binding(request.binding, contract) return request.goal def plan( @@ -367,7 +431,13 @@ def plan( """ self.require_goal(request) prepared = self._prepare_request(request, context) - return self._plan(prepared, context) + plan = self._plan(prepared, context) + if not isinstance(plan, ActionPlan): + raise TypeError("AtomicAction._plan() must return an ActionPlan.") + return replace( + plan, + commands=self._authorize_command_targets(prepared, plan.commands), + ) def _prepare_request( self, @@ -429,17 +499,26 @@ def _uses_collision_world( ) return available + def _scene_dependencies( + self, + request: ResolvedActionRequest[GoalT, OptionsT], + ) -> tuple[str, ...]: + """Return scene entities whose poses materially affect this plan.""" + return collect_scene_dependencies(request.goal) + def build_plan( self, request: ResolvedActionRequest[GoalT, OptionsT], context: PlanningContext, *, success: bool | torch.Tensor, - trajectory: TimedTrajectory | torch.Tensor, + trajectory: TimedTrajectory, expected_effects: StateDelta | None = None, + effect_verification: EffectVerificationRequirement | None = None, replannable: bool = True, diagnostics: PlannerDiagnostics | None = None, segment_lengths: Mapping[str, int] | None = None, + scene_dependency_monitor_until: Mapping[str, int] | None = None, ) -> ActionPlan: """Build a validated action plan for a primitive implementation. @@ -447,66 +526,131 @@ def build_plan( request: Resolved invocation snapshot being planned. context: Planning input used for the plan. success: Per-environment planning success or scalar planner result. - trajectory: Full-robot timed trajectory or position tensor. + trajectory: Full-robot trajectory with explicit timing. expected_effects: Symbolic effects to verify after execution. + effect_verification: Optional explicit physical-effect boundary. + Use this when verification is required without a symbolic task- + state delta. replannable: Whether the execution runtime may replan this action. diagnostics: Optional retained planner diagnostics. segment_lengths: Optional ordered mapping from semantic segment names to waypoint counts. Zero-length entries are omitted. + scene_dependency_monitor_until: Optional per-entity exclusive + waypoint-index upper bound for scene-motion invalidation. An + entity is monitored while the current waypoint index is smaller + than its bound. ``0`` disables monitoring immediately; omitted + dependencies remain monitored for the full action. Once the bound + is reached, all pose changes for that entity are ignored. Returns: Side-effect-free action plan. """ - self.require_goal(request) success_mask = normalize_success_mask( success, - n_envs=context.batch_size, + num_envs=context.batch_size, device=self.device, name="Planning success", ) - if isinstance(trajectory, torch.Tensor): - timed = TimedTrajectory.from_positions( - trajectory, - env_ids=context.env_ids, - control_dt=request.motion_policy.control_dt, + if not isinstance(trajectory, TimedTrajectory): + raise TypeError( + "trajectory must be a TimedTrajectory with explicit dt; atomic " + "actions may not return untimed position tensors." ) - elif isinstance(trajectory, TimedTrajectory): - timed = trajectory - else: - raise TypeError("trajectory must be TimedTrajectory or torch.Tensor.") + timed = trajectory if timed.batch_size != context.batch_size: raise ValueError("Trajectory and planning context batch sizes must match.") if timed.robot_dof != context.robot.robot_dof: raise ValueError("Trajectory robot_dof must match the planning context.") timed = timed.hold_rows(success_mask, context.robot.qpos) - segments: list[TrajectorySegment] = [] - if segment_lengths is not None: - offset = 0 - for name, length in segment_lengths.items(): - if not isinstance(name, str) or not name: - raise ValueError("Trajectory segment names must be non-empty.") - if isinstance(length, bool) or not isinstance(length, int): - raise TypeError("Trajectory segment lengths must be integers.") - if length < 0: - raise ValueError("Trajectory segment lengths must be non-negative.") - if length == 0: - continue - segments.append( - TrajectorySegment( - name=name, - start=offset, - stop=offset + length, - ) - ) - offset += length - if offset != timed.waypoint_count: - raise ValueError( - "Trajectory segment lengths must sum to the trajectory " - f"waypoint count ({timed.waypoint_count}), got {offset}." - ) + commands = self._joint_command_sequence( + request, + timed, + active_mask=success_mask, + ) + return self.build_command_plan( + request, + context, + success=success_mask, + commands=commands, + expected_effects=expected_effects, + effect_verification=effect_verification, + replannable=replannable, + diagnostics=diagnostics, + segment_lengths=segment_lengths, + scene_dependency_monitor_until=scene_dependency_monitor_until, + joint_trajectory=timed, + ) + + def build_command_plan( + self, + request: ResolvedActionRequest[GoalT, OptionsT], + context: PlanningContext, + *, + success: bool | torch.Tensor, + commands: TimedCommandSequence, + expected_effects: StateDelta | None = None, + effect_verification: EffectVerificationRequirement | None = None, + replannable: bool = True, + diagnostics: PlannerDiagnostics | None = None, + segment_lengths: Mapping[str, int] | None = None, + scene_dependency_monitor_until: Mapping[str, int] | None = None, + joint_trajectory: TimedTrajectory | None = None, + ) -> ActionPlan: + """Build a plan from transport-neutral runtime command frames. + + Tracking targets are projected from the command payloads through the + typed channels declared by each bound endpoint. Semantic effects remain + externally verified through the execution session. + + Args: + request: Resolved invocation snapshot being planned. + context: Planning input used for the plan. + success: Per-environment planning success or scalar planner result. + commands: Transport-neutral command sequence for the action. + expected_effects: Symbolic effects to verify after execution. + effect_verification: Optional explicit physical-effect boundary. + replannable: Whether the execution runtime may replan this action. + diagnostics: Optional retained planner diagnostics. + segment_lengths: Optional ordered mapping from semantic segment names + to command-frame counts. Zero-length entries are omitted. + scene_dependency_monitor_until: Optional per-entity exclusive + command-frame-index upper bound for scene-motion invalidation. An + entity is monitored while the current frame index is smaller than + its bound. ``0`` disables monitoring immediately; omitted + dependencies remain monitored for the full action. Once the bound + is reached, all pose changes for that entity are ignored. + joint_trajectory: Optional joint trajectory retained for offline + compilation and inspection. + Returns: + Side-effect-free action plan. + """ + if not isinstance(commands, TimedCommandSequence): + raise TypeError("commands must be a TimedCommandSequence.") + if commands.batch_size != context.batch_size: + raise ValueError( + "Command sequence and planning context batch sizes must match." + ) + if not torch.equal(commands.env_ids, context.env_ids): + raise ValueError("Command sequence env_ids must match the context.") + success_mask = normalize_success_mask( + success, + num_envs=context.batch_size, + device=self.device, + name="Planning success", + ) + commands = self._authorize_command_targets( + request, + commands, + active_mask=success_mask, + ) + tracking = self._tracking_sequence(request, commands) + segments = self._build_segments( + segment_lengths, + frame_count=commands.frame_count, + ) if diagnostics is None: diagnostics = PlannerDiagnostics( backend=self.planning_services.planner_name @@ -514,25 +658,286 @@ def build_plan( return ActionPlan( skill_id=self.skill_id, plan_success=success_mask, - trajectory=timed, + commands=commands, recovery_policy=request.recovery_policy, + tracking_policy=request.tracking_policy, planned_scene_version=context.scene.version, planned_collision_world_revision=( context.scene.collision_world_revisions(context.batch_size) ), diagnostics=diagnostics, - segments=tuple(segments), - scene_dependencies=collect_scene_dependencies(request.goal), - collision_world_sensitive=self._uses_collision_world( - request, - context, + tracking=tracking, + joint_trajectory=joint_trajectory, + segments=segments, + scene_dependencies=self._scene_dependencies(request), + scene_dependency_monitor_until=( + {} + if scene_dependency_monitor_until is None + else scene_dependency_monitor_until ), + collision_world_sensitive=self._uses_collision_world(request, context), replannable=replannable, expected_effects=expected_effects or StateDelta(), + effect_verification=effect_verification, invocation_id=request.invocation_id, invocation_revision=request.revision, ) + def _tracking_sequence( + self, + request: ResolvedActionRequest[GoalT, OptionsT], + commands: TimedCommandSequence, + ) -> TimedTrackingSequence | None: + """Project command payloads through binding-owned tracking channels.""" + policy = request.tracking_policy + metrics = list(() if policy.in_flight is None else policy.in_flight.metrics) + if isinstance(policy.terminal, FeedbackTerminalAcceptance): + metrics.extend(policy.terminal.metrics) + if not metrics: + return None + + runtime = self.planning_services.tracking_runtime + for metric in metrics: + runtime.evaluators.resolve(metric) + metrics_by_channel = {metric.channel_id: metric for metric in metrics} + + endpoints_by_destination: dict[ + tuple[str, str], + tuple[EndpointBinding, ...], + ] = {} + for endpoint in request.binding.endpoints: + endpoints_by_destination.setdefault(endpoint.destination_key, ()) + endpoints_by_destination[endpoint.destination_key] += (endpoint,) + + tracking_frames: list[TrackingFrame] = [] + for frame_index, frame in enumerate(commands.frames): + setpoints: list[TrackingSetpoint] = [] + for command in frame.commands: + endpoints = endpoints_by_destination[command.destination_key] + for endpoint in endpoints: + for channel_id in metrics_by_channel: + channel = endpoint.tracking_channels.get(channel_id) + if channel is None: + continue + runtime.providers.resolve(channel.source) + runtime.projectors.resolve(channel.projector) + setpoints.append( + TrackingSetpoint( + endpoint_key=endpoint.key, + binding=channel, + desired=runtime.project(command, channel), + ) + ) + covered_channels = {setpoint.binding.channel_id for setpoint in setpoints} + missing_channels = sorted( + set(metrics_by_channel).difference(covered_channels) + ) + if missing_channels: + raise ValueError( + f"Command frame {frame_index} cannot project configured " + f"tracking channels {missing_channels}; bound endpoints must " + "declare a typed feedback source and projector." + ) + tracking_frames.append(TrackingFrame(tuple(setpoints))) + return TimedTrackingSequence( + env_ids=commands.env_ids, + frames=tuple(tracking_frames), + ) + + @staticmethod + def _authorize_command_targets( + request: ResolvedActionRequest[GoalT, OptionsT], + commands: TimedCommandSequence, + *, + active_mask: torch.Tensor | None = None, + ) -> TimedCommandSequence: + """Bind every emitted command to an endpoint authorized by the request. + + Actions may choose a subset of their bound endpoints for any frame, but + they cannot synthesize a destination outside the resolved resource + binding. The returned sequence replaces caller-provided target metadata + with the engine-owned binding snapshot, so transports never receive + altered joint claims or other target fields. When ``active_mask`` is + provided, authorization and failed-row masking share the same rebuild. + """ + authorized: dict[tuple[str, str], list[EndpointBinding]] = {} + for endpoint in request.binding.endpoints: + authorized.setdefault(endpoint.destination_key, []).append(endpoint) + unknown = sorted( + { + command.destination_key + for frame in commands.frames + for command in frame.commands + if command.destination_key not in authorized + } + ) + if unknown: + raise ValueError( + "Runtime commands reference destinations not authorized by the " + f"action binding: {unknown}." + ) + + frames: list[RuntimeCommandFrame] = [] + for frame in commands.frames: + endpoint_commands: list[EndpointCommand] = [] + joint_owners: dict[int, tuple[str, str]] = {} + token_owners: dict[str, tuple[str, str]] = {} + for command in frame.commands: + bound_endpoints = authorized[command.destination_key] + target = bound_endpoints[0].target + if any( + type(endpoint.target) is not type(target) + for endpoint in bound_endpoints[1:] + ): + raise ValueError( + f"Action binding destination {command.destination_key} has " + "incompatible target declarations." + ) + if type(command.target) is not type(target): + raise TypeError( + f"Runtime command destination {command.destination_key} uses " + f"target type {type(command.target).__name__}, but its bound " + f"endpoint uses {type(target).__name__}." + ) + if isinstance(target, JointPositionTarget) and command.target != target: + raise ValueError( + f"Runtime command destination {command.destination_key} " + "does not preserve its bound joint-position target." + ) + joint_ids = { + joint_id + for endpoint in bound_endpoints + for joint_id in endpoint.joint_ids + } + claim_tokens = { + token + for endpoint in bound_endpoints + for token in endpoint.claim_tokens + } + overlapping_joints = sorted(joint_ids & joint_owners.keys()) + overlapping_tokens = sorted(claim_tokens & token_owners.keys()) + if overlapping_joints or overlapping_tokens: + conflicting_destinations = sorted( + {joint_owners[joint_id] for joint_id in overlapping_joints} + | {token_owners[token] for token in overlapping_tokens} + ) + raise ValueError( + f"Runtime command destination {command.destination_key} " + f"conflicts with {conflicting_destinations} on bound joint " + f"IDs {overlapping_joints} or claim tokens " + f"{overlapping_tokens}." + ) + for joint_id in joint_ids: + joint_owners[joint_id] = command.destination_key + for token in claim_tokens: + token_owners[token] = command.destination_key + endpoint_commands.append( + EndpointCommand(target=target, payload=command.payload) + ) + frames.append( + RuntimeCommandFrame( + commands=tuple(endpoint_commands), + active_mask=( + frame.active_mask + if active_mask is None + else frame.active_mask & active_mask + ), + env_ids=frame.env_ids, + hold_duration=frame.hold_duration, + ) + ) + return TimedCommandSequence(frames=tuple(frames), env_ids=commands.env_ids) + + def _joint_command_sequence( + self, + request: ResolvedActionRequest[GoalT, OptionsT], + trajectory: TimedTrajectory, + *, + active_mask: torch.Tensor, + ) -> TimedCommandSequence: + """Lower one full-robot planner trajectory to endpoint commands.""" + targets = tuple( + ( + endpoint, + endpoint.require_target(JointPositionTarget), + ) + for endpoint in request.binding.endpoints + ) + if not targets: + raise ValueError( + "Joint trajectory plans require at least one bound " + "JointPositionTarget endpoint." + ) + frames: list[RuntimeCommandFrame] = [] + for waypoint_index in range(trajectory.waypoint_count): + endpoint_commands: list[EndpointCommand] = [] + for _, target in targets: + joint_ids = list(target.joint_ids) + velocities = ( + None + if trajectory.velocities is None + else trajectory.velocities[:, waypoint_index, joint_ids] + ) + endpoint_commands.append( + EndpointCommand( + target=target, + payload=JointPositionPayload( + positions=trajectory.positions[ + :, waypoint_index, joint_ids + ], + velocities=velocities, + ), + ) + ) + next_waypoint_index = min( + waypoint_index + 1, + trajectory.waypoint_count - 1, + ) + # ``dt[:, i]`` is the arrival interval for waypoint ``i``. After + # dispatching it, wait for the next arrival interval; the terminal + # frame deliberately reuses its own interval as a settling window, + # preserving the closed-loop runner's pre-PR2C timing contract. + frames.append( + RuntimeCommandFrame( + commands=tuple(endpoint_commands), + active_mask=active_mask, + env_ids=trajectory.env_ids, + hold_duration=trajectory.dt[:, next_waypoint_index], + ) + ) + return TimedCommandSequence(frames=tuple(frames), env_ids=trajectory.env_ids) + + @staticmethod + def _build_segments( + segment_lengths: Mapping[str, int] | None, + *, + frame_count: int, + ) -> tuple[TrajectorySegment, ...]: + """Validate optional named ranges for one command sequence.""" + if segment_lengths is None: + return () + segments: list[TrajectorySegment] = [] + offset = 0 + for name, length in segment_lengths.items(): + if not isinstance(name, str) or not name: + raise ValueError("Trajectory segment names must be non-empty.") + if isinstance(length, bool) or not isinstance(length, int): + raise TypeError("Trajectory segment lengths must be integers.") + if length < 0: + raise ValueError("Trajectory segment lengths must be non-negative.") + if length == 0: + continue + segments.append( + TrajectorySegment(name=name, start=offset, stop=offset + length) + ) + offset += length + if offset != frame_count: + raise ValueError( + "Trajectory segment lengths must sum to the command frame count " + f"({frame_count}), got {offset}." + ) + return tuple(segments) + def failed_plan( self, request: ResolvedActionRequest[GoalT, OptionsT], @@ -550,23 +955,35 @@ def failed_plan( Returns: Failed action plan with an empty trajectory. """ - return self.build_plan( + success = torch.zeros(context.batch_size, dtype=torch.bool, device=self.device) + diagnostics = PlannerDiagnostics( + backend=self.planning_services.planner_name, + messages=(() if message is None else (message,)), + ) + if request.binding.endpoints and all( + isinstance(endpoint.target, JointPositionTarget) + for endpoint in request.binding.endpoints + ): + return self.build_plan( + request, + context, + success=success, + trajectory=TimedTrajectory.empty( + batch_size=context.batch_size, + robot_dof=context.robot.robot_dof, + device=self.device, + env_ids=context.env_ids, + ), + replannable=True, + diagnostics=diagnostics, + ) + return self.build_command_plan( request, context, - success=torch.zeros( - context.batch_size, dtype=torch.bool, device=self.device - ), - trajectory=TimedTrajectory.empty( - batch_size=context.batch_size, - robot_dof=context.robot.robot_dof, - device=self.device, - env_ids=context.env_ids, - ), + success=success, + commands=TimedCommandSequence(frames=(), env_ids=context.env_ids), replannable=True, - diagnostics=PlannerDiagnostics( - backend=self.planning_services.planner_name, - messages=(() if message is None else (message,)), - ), + diagnostics=diagnostics, ) @abstractmethod diff --git a/embodichain/lab/sim/atomic_actions/effects.py b/embodichain/lab/sim/atomic_actions/effects.py index f9c6507aa..740cf4887 100644 --- a/embodichain/lab/sim/atomic_actions/effects.py +++ b/embodichain/lab/sim/atomic_actions/effects.py @@ -18,21 +18,117 @@ from __future__ import annotations -from dataclasses import dataclass, field +from collections.abc import Mapping +from copy import deepcopy +from dataclasses import dataclass, field, fields, is_dataclass from types import MappingProxyType -from typing import Mapping +from typing import TYPE_CHECKING import torch +from embodichain.lab.sim.common import BatchEntity + from .state import ( + ArticulationJointState, CoordinatedHeldObjectState, HeldObjectState, TaskState, + _normalize_articulation_joint, _normalize_coordinated_held, _normalize_held, _normalize_mask, ) +if TYPE_CHECKING: + from .core import ObjectSemantics + + +def _effect_snapshot_memo(value: object) -> dict[int, object]: + """Preserve live entities and private runtime caches during effect copies.""" + memo: dict[int, object] = {} + visited: set[int] = set() + + def visit(nested: object) -> None: + nested_id = id(nested) + if nested_id in visited: + return + visited.add(nested_id) + if isinstance(nested, BatchEntity): + memo[nested_id] = nested + return + if is_dataclass(nested) and not isinstance(nested, type): + for data_field in fields(nested): + child = getattr(nested, data_field.name) + if data_field.name == "_generator" and child is not None: + memo[id(child)] = None + elif not data_field.init and child is not None: + memo[id(child)] = child + else: + visit(child) + return + if isinstance(nested, Mapping): + for key, child in nested.items(): + visit(key) + visit(child) + return + if isinstance(nested, (list, tuple, set, frozenset)): + for child in nested: + visit(child) + + visit(value) + return memo + + +def _snapshot_semantics(value: ObjectSemantics) -> ObjectSemantics: + """Copy semantic data while retaining live simulation-entity identity.""" + try: + copied = deepcopy(value, _effect_snapshot_memo(value)) + except Exception as exc: + raise TypeError( + "ObjectSemantics effect metadata must be copyable without cloning " + "live simulation entities." + ) from exc + if type(copied) is not type(value) or copied is value: + raise TypeError( + "ObjectSemantics effect snapshots must produce a distinct value " + "of the same exact type." + ) + return copied + + +def _snapshot_held(value: HeldObjectState) -> HeldObjectState: + """Return an independently owned held-object effect value.""" + return HeldObjectState( + semantics=_snapshot_semantics(value.semantics), + object_to_eef=value.object_to_eef.clone(), + grasp_xpos=value.grasp_xpos.clone(), + env_mask=None if value.env_mask is None else value.env_mask.clone(), + ) + + +def _snapshot_coordinated( + value: CoordinatedHeldObjectState, +) -> CoordinatedHeldObjectState: + """Return an independently owned coordinated held-object effect value.""" + return CoordinatedHeldObjectState( + semantics=_snapshot_semantics(value.semantics), + left_object_to_eef=value.left_object_to_eef.clone(), + right_object_to_eef=value.right_object_to_eef.clone(), + left_grasp_xpos=value.left_grasp_xpos.clone(), + right_grasp_xpos=value.right_grasp_xpos.clone(), + env_mask=None if value.env_mask is None else value.env_mask.clone(), + ) + + +def _snapshot_articulation_joint( + value: ArticulationJointState, +) -> ArticulationJointState: + """Return an independently owned articulation-joint effect value.""" + return ArticulationJointState( + position=value.position.clone(), + env_mask=None if value.env_mask is None else value.env_mask.clone(), + ) + def _with_held_mask( value: HeldObjectState, @@ -62,12 +158,22 @@ def _with_coordinated_mask( ) +def _with_articulation_joint_mask( + value: ArticulationJointState, + env_mask: torch.Tensor, +) -> ArticulationJointState: + """Copy an articulation-joint state with a replacement mask.""" + return ArticulationJointState(position=value.position, env_mask=env_mask) + + def _merge_held( previous: HeldObjectState | None, candidate: HeldObjectState | None, update_mask: torch.Tensor, ) -> HeldObjectState | None: """Apply one optional held-object update per environment.""" + from .core import _same_object_identity + if previous is None and candidate is None: return None if previous is None: @@ -85,7 +191,7 @@ def _merge_held( if ( previous_retained and candidate_applied - and previous.semantics is not candidate.semantics + and not _same_object_identity(previous.semantics, candidate.semantics) ): raise ValueError( "Cannot merge different held-object semantics for one resource " @@ -96,7 +202,7 @@ def _merge_held( return None selector = update_mask[:, None, None] return HeldObjectState( - semantics=candidate.semantics if candidate_applied else previous.semantics, + semantics=(previous.semantics if previous_retained else candidate.semantics), object_to_eef=torch.where( selector, candidate.object_to_eef, previous.object_to_eef ), @@ -111,6 +217,8 @@ def _merge_coordinated( update_mask: torch.Tensor, ) -> CoordinatedHeldObjectState | None: """Apply one optional coordinated relation update per environment.""" + from .core import _same_object_identity + if previous is None and candidate is None: return None if previous is None: @@ -128,7 +236,7 @@ def _merge_coordinated( if ( previous_retained and candidate_applied - and previous.semantics is not candidate.semantics + and not _same_object_identity(previous.semantics, candidate.semantics) ): raise ValueError( "Cannot merge different coordinated held-object semantics for one " @@ -139,7 +247,7 @@ def _merge_coordinated( return None selector = update_mask[:, None, None] return CoordinatedHeldObjectState( - semantics=candidate.semantics if candidate_applied else previous.semantics, + semantics=(previous.semantics if previous_retained else candidate.semantics), left_object_to_eef=torch.where( selector, candidate.left_object_to_eef, previous.left_object_to_eef ), @@ -156,6 +264,48 @@ def _merge_coordinated( ) +def _merge_articulation_joint( + previous: ArticulationJointState | None, + candidate: ArticulationJointState | None, + update_mask: torch.Tensor, +) -> ArticulationJointState | None: + """Apply one optional articulation-joint update per environment.""" + if previous is None and candidate is None: + return None + if previous is None: + assert candidate is not None and candidate.env_mask is not None + env_mask = candidate.env_mask & update_mask + return ( + _with_articulation_joint_mask(candidate, env_mask) + if env_mask.any() + else None + ) + assert previous.env_mask is not None + if candidate is None: + env_mask = previous.env_mask & ~update_mask + return ( + _with_articulation_joint_mask(previous, env_mask) + if env_mask.any() + else None + ) + assert candidate.env_mask is not None + if candidate.position.shape != previous.position.shape: + raise ValueError( + "Cannot merge articulation-joint states with different joint widths." + ) + env_mask = torch.where(update_mask, candidate.env_mask, previous.env_mask) + if not env_mask.any(): + return None + return ArticulationJointState( + position=torch.where( + update_mask[:, None], + candidate.position, + previous.position, + ), + env_mask=env_mask, + ) + + @dataclass(frozen=True, slots=True, eq=False) class StateDelta: """Expected task-state changes that require post-execution verification. @@ -175,9 +325,15 @@ class StateDelta: ] = field(default_factory=dict) """Per-resource-pair coordinated attachment replacements or removals.""" + articulation_joint_updates: Mapping[ + tuple[str, str], ArticulationJointState | None + ] = field(default_factory=dict) + """Per-articulation/joint verified state replacements or removals.""" + def __post_init__(self) -> None: held = dict(self.held_object_updates) coordinated = dict(self.coordinated_held_object_updates) + articulation = dict(self.articulation_joint_updates) for resource, value in held.items(): if not isinstance(resource, str) or not resource: raise ValueError( @@ -201,17 +357,67 @@ def __post_init__(self) -> None: "coordinated_held_object_updates values must be " "CoordinatedHeldObjectState or None." ) + for key, value in articulation.items(): + if ( + not isinstance(key, tuple) + or len(key) != 2 + or not all( + type(item) is str and item and item == item.strip() for item in key + ) + ): + raise ValueError( + "articulation_joint_updates keys must be canonical " + "articulation/joint pairs." + ) + if value is not None and not isinstance(value, ArticulationJointState): + raise TypeError( + "articulation_joint_updates values must be " + "ArticulationJointState or None." + ) object.__setattr__(self, "held_object_updates", MappingProxyType(held)) object.__setattr__( self, "coordinated_held_object_updates", MappingProxyType(coordinated), ) + object.__setattr__( + self, + "articulation_joint_updates", + MappingProxyType(articulation), + ) @property def is_empty(self) -> bool: """Whether this delta declares no symbolic state changes.""" - return not self.held_object_updates and not self.coordinated_held_object_updates + return ( + not self.held_object_updates + and not self.coordinated_held_object_updates + and not self.articulation_joint_updates + ) + + def snapshot(self) -> StateDelta: + """Return an independently owned symbolic-effect snapshot. + + Live simulation entities retain identity, while semantic metadata, + affordance data, and every attachment tensor are copied. + + Returns: + Independently owned state delta. + """ + return StateDelta( + held_object_updates={ + resource: None if value is None else _snapshot_held(value) + for resource, value in self.held_object_updates.items() + }, + coordinated_held_object_updates={ + resources: (None if value is None else _snapshot_coordinated(value)) + for resources, value in self.coordinated_held_object_updates.items() + }, + articulation_joint_updates={ + key: (None if value is None else _snapshot_articulation_joint(value)) + for key, value in self.articulation_joint_updates.items() + }, + ) def apply( self, @@ -226,7 +432,7 @@ def apply( Args: state: Input task state. - update_mask: Successful and verified rows, shape ``(n_envs,)``. + update_mask: Successful and verified rows, shape ``(num_envs,)``. Returns: New task state with masked updates. @@ -273,11 +479,33 @@ def apply( else: coordinated[resources] = merged + articulation = dict(state.articulation_joints) + for key, candidate in self.articulation_joint_updates.items(): + normalized = ( + None + if candidate is None + else _normalize_articulation_joint( + candidate, + batch_size=state.batch_size, + device=state.device, + ) + ) + merged = _merge_articulation_joint( + articulation.get(key), + normalized, + mask, + ) + if merged is None: + articulation.pop(key, None) + else: + articulation[key] = merged + return TaskState( batch_size=state.batch_size, device=state.device, held_objects=held, coordinated_held_objects=coordinated, + articulation_joints=articulation, ) diff --git a/embodichain/lab/sim/atomic_actions/engine.py b/embodichain/lab/sim/atomic_actions/engine.py index 080b3ef37..3e3ccc728 100644 --- a/embodichain/lab/sim/atomic_actions/engine.py +++ b/embodichain/lab/sim/atomic_actions/engine.py @@ -18,67 +18,34 @@ from __future__ import annotations +from types import MappingProxyType from typing import Iterable, Mapping, TYPE_CHECKING import torch -from .core import AtomicAction -from .control import ControlPartCommandProfile -from .invocation import ActionInvocation, ResolvedActionRequest +from .bindings import ActionBinding +from .core import AtomicAction, SkillDescriptor +from .control import ActionControlOverrides, ControlPartCommandProfile +from .invocation import ActionInvocation, GoalT, OptionsT, ResolvedActionRequest from .plans import ActionPlan, CompiledTrajectory, TimedTrajectory +from .policies import MotionPolicy, RecoveryPolicy from .runtime import ActionPlanningServices from .state import PlanningContext, RobotObservation, SceneSnapshot, TaskState +from .tracking import TrackingRuntime if TYPE_CHECKING: from embodichain.lab.sim.objects import Robot from embodichain.lab.sim.planners import MotionGenerator + from embodichain.lab.sim.skills import ( + BoundRobotSkillProfile, + ResourceEndpoint, + ResourceEndpointAdapter, + RobotSkillProfile, + ) from .execution import ExecutionSession -_global_extension_registry: dict[str, type[AtomicAction]] = {} - - -def register_action(action_class: type[AtomicAction]) -> None: - """Register an extension action type for process-wide discovery. - - This catalog does not bind the type to an engine or automatically load it. - Built-in types live in ``BUILTIN_ACTION_TYPES`` and are loaded separately - by each :class:`AtomicActionEngine`. - - Args: - action_class: Concrete :class:`AtomicAction` subclass. - - Raises: - TypeError: If ``action_class`` is not an AtomicAction subclass. - ValueError: If another class already owns the same skill identifier. - """ - if not isinstance(action_class, type) or not issubclass(action_class, AtomicAction): - raise TypeError("action_class must be an AtomicAction subclass.") - descriptor = action_class.descriptor() - existing = _global_extension_registry.get(descriptor.skill_id) - if existing is not None and existing is not action_class: - raise ValueError( - f"Skill id {descriptor.skill_id!r} is already registered by " - f"{existing.__name__}." - ) - _global_extension_registry[descriptor.skill_id] = action_class - - -def unregister_action(skill_id: str) -> None: - """Remove a globally discoverable extension action type if present. - - Args: - skill_id: Stable registered skill identifier. - """ - _global_extension_registry.pop(skill_id, None) - - -def get_registered_actions() -> dict[str, type[AtomicAction]]: - """Return a copy of the process-wide extension action-type registry.""" - return dict(_global_extension_registry) - - class AtomicActionEngine: """Own planning resources and coordinate side-effect-free atomic actions.""" @@ -88,6 +55,11 @@ def __init__( control_profiles: Mapping[str, ControlPartCommandProfile] | None = None, *, load_builtins: bool = True, + skill_profile: RobotSkillProfile | None = None, + endpoint_adapters: ( + Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None + ) = None, + tracking_runtime: TrackingRuntime | None = None, ) -> None: """Initialize one engine and bind its built-in action implementations. @@ -96,14 +68,44 @@ def __init__( control_profiles: Semantic commands keyed by robot control-part name. load_builtins: Whether to instantiate and register every built-in action. Disable this for isolated tests or fully custom engines. + skill_profile: Optional authoritative robot skill profile. Its + command profiles are installed automatically and validated + after built-in actions are loaded. ``control_profiles`` and + ``skill_profile`` are mutually exclusive. + endpoint_adapters: Optional exact-type endpoint adapters used when + binding ``skill_profile``. Invalid without a profile. + tracking_runtime: Optional exact-version feedback, projector, and + metric registries. Built-in joint tracking is installed when + omitted. """ + if endpoint_adapters is not None and skill_profile is None: + raise ValueError("endpoint_adapters requires skill_profile.") + if skill_profile is not None: + from embodichain.lab.sim.skills import RobotSkillProfile + + if not isinstance(skill_profile, RobotSkillProfile): + raise TypeError("skill_profile must be a RobotSkillProfile or None.") + if control_profiles is not None: + raise ValueError( + "control_profiles and skill_profile are mutually exclusive; " + "the profile is the authoritative semantic-command source." + ) + control_profiles = skill_profile.action_control_profiles() self._planning_services = ActionPlanningServices( motion_generator, control_profiles=control_profiles, + tracking_runtime=tracking_runtime, ) self._actions: dict[str, AtomicAction] = {} + self._skill_catalog_revision = 0 + self._skill_profile: BoundRobotSkillProfile | None = None if load_builtins: self._load_builtin_actions() + if skill_profile is not None: + self._skill_profile = skill_profile.bind( + self, + endpoint_adapters=endpoint_adapters, + ) @property def motion_generator(self) -> MotionGenerator: @@ -125,6 +127,16 @@ def planning_services(self) -> ActionPlanningServices: """Engine-owned resources shared by every bound atomic action.""" return self._planning_services + @property + def tracking_runtime(self) -> TrackingRuntime: + """Typed endpoint-feedback runtime used by plans and sessions.""" + return self._planning_services.tracking_runtime + + @property + def binding_owner_id(self) -> str: + """Return the opaque owner identity required by action bindings.""" + return self._planning_services.binding_owner_id + @property def control_profiles(self) -> Mapping[str, ControlPartCommandProfile]: """Semantic command profiles registered for robot control parts.""" @@ -135,6 +147,197 @@ def actions(self) -> dict[str, AtomicAction]: """Registered action instances keyed by stable skill identifier.""" return dict(self._actions) + @property + def skills(self) -> Mapping[str, SkillDescriptor]: + """Return explicitly declared, agent-visible installed skill metadata. + + Process-wide type discovery, engine installation, and semantic exposure + are separate boundaries. Only an action installed in this engine whose + concrete class explicitly declares a generic binding contract appears + here. Direct-core callers may continue to use every entry in + :attr:`actions`. + """ + return MappingProxyType( + { + skill_id: descriptor + for skill_id, action in self._actions.items() + if (descriptor := action.descriptor()).agent_visible + and descriptor.binding_contract is not None + } + ) + + @property + def skill_catalog_revision(self) -> int: + """Return the monotonic installed semantic-skill catalog revision. + + Replacing an agent-visible implementation advances the revision even + when its public descriptor is equal. Bound profiles and semantic + compilers can therefore reject stale implementation ownership. + """ + return self._skill_catalog_revision + + @property + def skill_profile(self) -> BoundRobotSkillProfile | None: + """Return the currently bound semantic robot profile, when configured.""" + return self._skill_profile + + def bind_skill_profile( + self, + profile: RobotSkillProfile, + *, + endpoint_adapters: ( + Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None + ) = None, + ) -> BoundRobotSkillProfile: + """Validate and bind a profile after custom action installation. + + The engine's immutable control-part profiles must already contain the + profile commands lowered into the current action core. Generic + non-core endpoint commands remain on resolved endpoints. Prefer the + constructor's ``skill_profile`` argument when no custom actions need + to be installed first. + + Args: + profile: Authoritative robot resource and policy profile. + endpoint_adapters: Optional exact-type endpoint adapters used for + custom controller declarations. + + Returns: + Validated profile bound to this engine and its installed actions. + """ + from embodichain.lab.sim.skills import RobotSkillProfile + + if not isinstance(profile, RobotSkillProfile): + raise TypeError("profile must be a RobotSkillProfile.") + bound = profile.bind(self, endpoint_adapters=endpoint_adapters) + self._skill_profile = bound + return bound + + def bind_control_parts( + self, + skill: str | AtomicAction, + endpoints: Mapping[str, Mapping[str, str]], + *, + task_state_keys: Mapping[str, str] | None = None, + ) -> ActionBinding: + """Build an advanced direct-core binding from control-part names. + + Args: + skill: Installed skill ID or an explicit action passed later to + :meth:`plan_action`. + endpoints: Nested ``slot_id -> endpoint_id -> control_part`` mapping. + task_state_keys: Optional explicit stable task-state key for each + resource slot. See + :meth:`ActionPlanningServices.bind_control_parts` for inference + rules when omitted. + + Returns: + Engine-owned generic endpoint binding. + """ + if isinstance(skill, str): + action = self._actions.get(skill) + if action is None: + raise KeyError(f"No atomic action registered for skill {skill!r}.") + elif isinstance(skill, AtomicAction): + action = skill + if ( + action.is_bound + and action.planning_services is not self._planning_services + ): + raise ValueError( + f"Atomic action {action.skill_id!r} belongs to another engine." + ) + else: + raise TypeError("skill must be an installed skill ID or AtomicAction.") + contract = type(action).__dict__.get("binding_contract") + if contract is None: + raise ValueError( + f"Skill {action.skill_id!r} has no explicit SkillBindingContract." + ) + return self._planning_services.bind_control_parts( + contract, + endpoints, + task_state_keys=task_state_keys, + ) + + def make_invocation( + self, + skill_id: str, + goal: GoalT, + *, + control_parts: Mapping[str, Mapping[str, str]] | None = None, + resources: Mapping[str, str] | None = None, + motion_policy: MotionPolicy | None = None, + recovery_policy: RecoveryPolicy | None = None, + skill_options: OptionsT | None = None, + control_overrides: ActionControlOverrides | None = None, + invocation_id: str | None = None, + revision: int = 0, + ) -> ActionInvocation[GoalT, OptionsT]: + """Construct a grounded invocation while naming the skill only once. + + ``control_parts`` uses the advanced direct-core binding path. When it is + omitted, the engine must own a bound robot skill profile; ``resources`` + then optionally selects logical resource IDs by skill-local slot. An + omitted resource selection uses the profile's unique or default binding. + This method resolves bindings only; profile policy presets and runner + configuration remain responsibilities of the semantic runtime layer. + + Args: + skill_id: Stable identifier of an installed atomic skill. + goal: Action-specific typed goal. + control_parts: Optional direct ``slot -> endpoint -> control_part`` + mapping. + resources: Optional profile ``slot -> resource_id`` selections. + motion_policy: Optional invocation motion policy. + recovery_policy: Optional invocation recovery policy. + skill_options: Optional action-specific invocation options. + control_overrides: Optional endpoint-scoped command overrides. + invocation_id: Optional correlation identifier. + revision: Monotonic invocation revision. + + Returns: + A standard :class:`ActionInvocation` accepted by ``plan``, + ``compile``, and ``start``. + + Raises: + ValueError: If binding sources conflict or no binding source is + available. + KeyError: If the skill or an explicitly selected resource is unknown. + TypeError: If an invocation field or binding input has an invalid type. + """ + if control_parts is not None and resources is not None: + raise ValueError("control_parts and resources are mutually exclusive.") + if control_parts is not None: + binding = self.bind_control_parts(skill_id, control_parts) + else: + profile = self.skill_profile + if profile is None: + if resources is not None: + raise ValueError("resources requires a bound RobotSkillProfile.") + raise ValueError( + "control_parts is required when no RobotSkillProfile is bound." + ) + binding = profile.resolve(skill_id, resources).action_binding + + return ActionInvocation( + skill_id=skill_id, + goal=goal, + binding=binding, + motion_policy=MotionPolicy() if motion_policy is None else motion_policy, + recovery_policy=( + RecoveryPolicy() if recovery_policy is None else recovery_policy + ), + skill_options=skill_options, + control_overrides=( + ActionControlOverrides() + if control_overrides is None + else control_overrides + ), + invocation_id=invocation_id, + revision=revision, + ) + def register(self, action: AtomicAction, *, replace: bool = False) -> None: """Register one action instance using its descriptor. @@ -159,6 +362,14 @@ def register(self, action: AtomicAction, *, replace: bool = False) -> None: ) action._bind(self._planning_services) self._actions[descriptor.skill_id] = action + existing_descriptor = None if existing is None else existing.descriptor() + if (descriptor.agent_visible and descriptor.binding_contract is not None) or ( + existing_descriptor is not None + and existing_descriptor.agent_visible + and existing_descriptor.binding_contract is not None + ): + self._skill_catalog_revision += 1 + self._skill_profile = None def _load_builtin_actions(self) -> None: """Create and bind fresh built-in action instances for this engine.""" @@ -175,24 +386,19 @@ def plan_action( invocation: ActionInvocation, context: PlanningContext, ) -> ActionPlan: - """Plan with a configured action using this engine's resources. + """Plan with an unregistered action using this engine's resources. - Unlike :meth:`plan`, the supplied action does not need to be in the - skill registry. This is an advanced extension and testing escape hatch; - built-in parameter variants should use ``ActionInvocation.skill_options`` - with the engine's registered implementation. + This is an advanced extension and testing escape hatch. Built-in + parameter variants should use invocation ``skill_options`` with the + engine's registered implementation. Args: action: Configured action implementation to invoke. - invocation: Grounded request matching the action's skill identifier. + invocation: Grounded request matching the action skill identifier. context: Latest measured planning state. Returns: Validated side-effect-free action plan. - - Raises: - TypeError: If ``action`` is not an :class:`AtomicAction`. - ValueError: If the action, invocation, context, or plan is invalid. """ if not isinstance(action, AtomicAction): raise TypeError("action must be an AtomicAction instance.") @@ -206,6 +412,21 @@ def plan_action( def resolve( self, invocation: ActionInvocation, + ) -> ResolvedActionRequest: + """Resolve a registered invocation into an engine-owned snapshot.""" + return self._resolve(invocation) + + def plan_request( + self, + request: ResolvedActionRequest, + context: PlanningContext | None = None, + ) -> ActionPlan: + """Plan an already-resolved request without rebuilding its snapshot.""" + return self._plan_request(request, context) + + def _resolve( + self, + invocation: ActionInvocation, ) -> ResolvedActionRequest: """Resolve one registered invocation into an engine-owned snapshot. @@ -229,7 +450,7 @@ def resolve( ) return action.resolve_request(invocation) - def plan_request( + def _plan_request( self, request: ResolvedActionRequest, context: PlanningContext | None = None, @@ -241,7 +462,7 @@ def plan_request( calls this method for every replan. Args: - request: Immutable request previously returned by :meth:`resolve`. + request: Immutable request previously returned by :meth:`_resolve`. context: Optional latest planning state; captured when omitted. Returns: @@ -278,8 +499,8 @@ def plan( KeyError: If the invocation references an unregistered skill. """ current = self.initial_context() if context is None else context - request = self.resolve(invocation) - return self.plan_request(request, current) + request = self._resolve(invocation) + return self._plan_request(request, current) def initial_context( self, @@ -287,6 +508,7 @@ def initial_context( task: TaskState | None = None, scene: SceneSnapshot | None = None, timestamp: float = 0.0, + control_dt: float | None = None, ) -> PlanningContext: """Capture the robot state needed to start offline compilation. @@ -294,6 +516,7 @@ def initial_context( task: Optional symbolic task state; an empty state is used otherwise. scene: Optional scene snapshot; an empty snapshot is used otherwise. timestamp: Timestamp assigned to the captured robot observation. + control_dt: Explicit command period for action-owned interpolation. Returns: Planning context containing owned robot tensors. @@ -316,6 +539,7 @@ def initial_context( task=task, scene=scene, env_ids=torch.arange(batch_size, dtype=torch.long, device=self.device), + control_dt=control_dt, ) def compile( @@ -356,7 +580,15 @@ def compile( previous_qpos = projected.robot.qpos plan = self.plan(invocation, projected) step_success = alive & plan.plan_success.to(self.device) - trajectory = plan.trajectory.hold_rows(step_success, previous_qpos) + if plan.joint_trajectory is None: + raise ValueError( + f"Skill {plan.skill_id!r} emits non-joint runtime commands and " + "cannot be used with offline joint-trajectory compilation." + ) + trajectory = plan.joint_trajectory.hold_rows( + step_success, + previous_qpos, + ) plans.append(plan) trajectories.append(trajectory) @@ -384,6 +616,8 @@ def start( self, invocations: Iterable[ActionInvocation], context: PlanningContext | None = None, + *, + eligible_mask: torch.Tensor | None = None, ) -> ExecutionSession: """Start closed-loop execution for a grounded invocation sequence. @@ -391,6 +625,8 @@ def start( invocations: Grounded action requests in execution order. context: Initial measured state and scene snapshot. The engine captures one when omitted. + eligible_mask: Optional rows allowed to enter this session. Inactive + rows remain inactive across every invocation in the sequence. Returns: Stateful execution session advanced by ``session.tick(...)``. @@ -398,7 +634,12 @@ def start( from .execution import ExecutionSession initial = self.initial_context() if context is None else context - return ExecutionSession(self, tuple(invocations), initial) + return ExecutionSession( + self, + tuple(invocations), + initial, + eligible_mask=eligible_mask, + ) def _validate_context(self, context: PlanningContext) -> None: """Validate an externally supplied planning context.""" @@ -436,15 +677,28 @@ def _validate_plan( raise ValueError( "ActionPlan.invocation_revision must preserve the request revision." ) - trajectory = plan.trajectory - if trajectory.batch_size != context.batch_size: + if plan.tracking_policy != request.tracking_policy: + raise ValueError( + "ActionPlan.tracking_policy must preserve the resolved request " + "tracking policy." + ) + commands = plan.commands + if commands.batch_size != context.batch_size: raise ValueError("Action plan batch size does not match the context.") - if trajectory.robot_dof != self.robot.dof: - raise ValueError("Action plan robot_dof does not match the engine robot.") - if trajectory.positions.device != self.device: + if commands.device != self.device: raise ValueError("Action plan and engine must share a device.") - if not torch.equal(trajectory.env_ids, context.env_ids): + if not torch.equal(commands.env_ids, context.env_ids): raise ValueError("Action plan and context must share ordered env_ids.") + if plan.joint_trajectory is not None: + if plan.joint_trajectory.robot_dof != self.robot.dof: + raise ValueError( + "Action plan joint_trajectory robot_dof does not match the " + "engine robot." + ) + if plan.joint_trajectory.positions.device != self.device: + raise ValueError( + "Action plan joint_trajectory and engine must share a device." + ) if plan.planned_scene_version != context.scene.version: raise ValueError("Action plan must record the planning scene version.") collision_revision = context.scene.collision_world_revisions(context.batch_size) @@ -454,9 +708,4 @@ def _validate_plan( ) -__all__ = [ - "AtomicActionEngine", - "get_registered_actions", - "register_action", - "unregister_action", -] +__all__ = ["AtomicActionEngine"] diff --git a/embodichain/lab/sim/atomic_actions/execution.py b/embodichain/lab/sim/atomic_actions/execution.py index 1b95e64f9..0835ddb92 100644 --- a/embodichain/lab/sim/atomic_actions/execution.py +++ b/embodichain/lab/sim/atomic_actions/execution.py @@ -18,16 +18,35 @@ from __future__ import annotations -from dataclasses import dataclass +from dataclasses import dataclass, field from enum import Enum +import math from typing import TYPE_CHECKING import torch from .effects import StateDelta -from .invocation import ActionInvocation, ResolvedActionRequest -from .plans import ActionPlan, TimedTrajectory, TrajectorySegment +from .invocation import ( + ActionInvocation, + PhaseEffectGateRequirement, + ResolvedActionRequest, +) +from .bindings import RuntimeEndpointTarget +from .plans import ( + ActionPlan, + EffectVerificationRequirement, + TrajectorySegment, +) +from .policies import RecoveryPolicy +from .runtime_commands import RuntimeCommandFrame, TimedCommandSequence from .state import EntityState, PlanningContext, SceneSnapshot, TaskState +from .tracking import ( + FeedbackTerminalAcceptance, + TimedTerminalAcceptance, + TrackingEvaluation, + TrackingFrame, + TrackingMetricCfg, +) if TYPE_CHECKING: from .engine import AtomicActionEngine @@ -47,16 +66,29 @@ class ExecutionEventKind(str, Enum): ACTION_PLANNED = "action_planned" INVOCATION_REVISED = "invocation_revised" REPLANNED = "replanned" - TRACKING_ERROR = "tracking_error" + TRACKING_DIVERGED = "tracking_diverged" + TRACKING_FEEDBACK_FAILED = "tracking_feedback_failed" + TERMINAL_ACCEPTANCE_PENDING = "terminal_acceptance_pending" + TERMINAL_ACCEPTANCE_FAILED = "terminal_acceptance_failed" DYNAMIC_GOAL_CHANGED = "dynamic_goal_changed" COLLISION_WORLD_CHANGED = "collision_world_changed" + ACTION_PLANNING_FAILED = "action_planning_failed" ACTION_TIMEOUT = "action_timeout" TRAJECTORY_COMPLETED = "trajectory_completed" EFFECT_VERIFICATION_REQUIRED = "effect_verification_required" + EFFECT_VERIFICATION_FAILED = "effect_verification_failed" + EFFECT_VERIFICATION_TIMEOUT = "effect_verification_timeout" + PHASE_EFFECT_GATE_REQUIRED = "phase_effect_gate_required" + PHASE_EFFECT_GATE_SATISFIED = "phase_effect_gate_satisfied" + PHASE_EFFECT_GATE_FAILED = "phase_effect_gate_failed" + 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" + SESSION_FAILED = "session_failed" @dataclass(frozen=True, slots=True, eq=False) @@ -79,24 +111,152 @@ def __post_init__(self) -> None: raise ValueError("invocation_index must be non-negative.") if self.invocation_revision < 0: raise ValueError("invocation_revision must be non-negative.") + 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("ExecutionEvent.env_mask must be a 1D bool tensor.") object.__setattr__(self, "env_mask", self.env_mask.clone()) +@dataclass(frozen=True, slots=True, eq=False) +class ExecutionPlanAttempt: + """Owned inspection snapshot for one installed action plan. + + Recovery can install several plans for one logical invocation. This value + preserves the exact scene/collision revisions and trajectory structure of + every installation, correlated with the session-local attempt generation + and row-local recovery counters. + """ + + attempt_generation: int + event_kind: ExecutionEventKind + planned_at: float + invocation_index: int + planned_mask: torch.Tensor + action_retry_counts: tuple[int, ...] + replan_counts: tuple[int, ...] + request: ResolvedActionRequest + plan: ActionPlan + + def __post_init__(self) -> None: + if type(self.attempt_generation) is not int or self.attempt_generation < 0: + raise ValueError("attempt_generation must be a non-negative integer.") + if self.event_kind not in { + ExecutionEventKind.ACTION_PLANNED, + ExecutionEventKind.INVOCATION_REVISED, + ExecutionEventKind.REPLANNED, + }: + raise ValueError("event_kind must describe an installed action plan.") + if not math.isfinite(self.planned_at) or self.planned_at < 0.0: + raise ValueError("planned_at must be finite and non-negative.") + if type(self.invocation_index) is not int or self.invocation_index < 0: + raise ValueError("invocation_index must be a non-negative integer.") + if ( + not isinstance(self.planned_mask, torch.Tensor) + or self.planned_mask.dtype != torch.bool + or self.planned_mask.dim() != 1 + ): + raise ValueError("planned_mask must be a one-dimensional bool tensor.") + retries = tuple(self.action_retry_counts) + replans = tuple(self.replan_counts) + batch_size = int(self.planned_mask.numel()) + if len(retries) != batch_size or len(replans) != batch_size: + raise ValueError("Recovery counters must contain one value per row.") + if any(type(value) is not int or value < 0 for value in (*retries, *replans)): + raise ValueError("Recovery counters must be non-negative integers.") + if not isinstance(self.request, ResolvedActionRequest): + raise TypeError("request must be a ResolvedActionRequest.") + if not isinstance(self.plan, ActionPlan): + raise TypeError("plan must be an ActionPlan.") + if ( + self.request.skill_id != self.plan.skill_id + or self.request.invocation_id != self.plan.invocation_id + or self.request.revision != self.plan.invocation_revision + ): + raise ValueError("request identity must match the installed plan.") + if self.plan.plan_success.shape != self.planned_mask.shape: + raise ValueError("plan and planned_mask batch shapes must match.") + if self.plan.plan_success.device != self.planned_mask.device: + raise ValueError("plan and planned_mask must share a device.") + object.__setattr__(self, "planned_mask", self.planned_mask.clone()) + object.__setattr__(self, "action_retry_counts", retries) + object.__setattr__(self, "replan_counts", replans) + object.__setattr__(self, "request", self.request.snapshot()) + object.__setattr__(self, "plan", self.plan.snapshot()) + + def snapshot(self) -> ExecutionPlanAttempt: + """Return an independently owned plan-attempt trace.""" + return ExecutionPlanAttempt( + attempt_generation=self.attempt_generation, + event_kind=self.event_kind, + planned_at=self.planned_at, + invocation_index=self.invocation_index, + planned_mask=self.planned_mask, + action_retry_counts=self.action_retry_counts, + replan_counts=self.replan_counts, + request=self.request, + plan=self.plan, + ) + + +@dataclass(frozen=True, slots=True) +class _ExecutionPlanAttemptRecord: + """Session-private plan reference converted to an owned public snapshot.""" + + attempt_generation: int + event_kind: ExecutionEventKind + planned_at: float + invocation_index: int + planned_mask: torch.Tensor + action_retry_counts: tuple[int, ...] + replan_counts: tuple[int, ...] + request: ResolvedActionRequest + plan: ActionPlan + + def snapshot(self) -> ExecutionPlanAttempt: + return ExecutionPlanAttempt( + attempt_generation=self.attempt_generation, + event_kind=self.event_kind, + planned_at=self.planned_at, + invocation_index=self.invocation_index, + planned_mask=self.planned_mask, + action_retry_counts=self.action_retry_counts, + replan_counts=self.replan_counts, + request=self.request, + plan=self.plan, + ) + + @dataclass(frozen=True, slots=True, eq=False) class EffectVerificationRequest: - """Typed boundary describing a semantic effect awaiting verification.""" + """Typed boundary describing a physical effect awaiting verification. + + ``requested_at`` and ``deadline`` use the same timestamp domain as + :class:`RobotObservation`. Request-mask shrinkage retains both values; + only a newly installed plan starts a new attempt deadline. + ``attempt_generation`` is session-local and remains stable when partial + resolution or row deactivation replaces only the request ID. + ``failure_invalidation`` is a core-owned removal-only delta; verification + results may select failed rows on which to apply it but cannot replace it. + """ + verification_id: int skill_id: str invocation_id: str | None invocation_revision: int invocation_index: int + attempt_generation: int terminal_segment: str | None + requested_at: float + deadline: float env_mask: torch.Tensor expected_effects: StateDelta + effect_verification: EffectVerificationRequirement | None = None + failure_invalidation: StateDelta = field(default_factory=StateDelta) 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 not isinstance(self.skill_id, str) or not self.skill_id: raise ValueError("skill_id must be a non-empty string.") if self.invocation_id is not None and ( @@ -107,69 +267,601 @@ def __post_init__(self) -> None: raise ValueError("invocation_revision must be non-negative.") if self.invocation_index < 0: raise ValueError("invocation_index must be non-negative.") + if type(self.attempt_generation) is not int or self.attempt_generation < 0: + raise ValueError("attempt_generation must be a non-negative integer.") if self.terminal_segment is not None and ( not isinstance(self.terminal_segment, str) or not self.terminal_segment ): raise ValueError("terminal_segment must be a non-empty string or None.") + if not math.isfinite(self.requested_at) or self.requested_at < 0.0: + raise ValueError("requested_at must be finite and non-negative.") + if not math.isfinite(self.deadline) or self.deadline < self.requested_at: + raise ValueError( + "deadline must be finite and no earlier than requested_at." + ) + 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 1D bool tensor.") + if not self.env_mask.any(): + raise ValueError("env_mask must contain at least one requested row.") if not isinstance(self.expected_effects, StateDelta): raise TypeError("expected_effects must be a StateDelta.") - if self.expected_effects.is_empty: - raise ValueError("Effect verification requires a non-empty StateDelta.") + if ( + self.effect_verification is not None + and type(self.effect_verification) is not EffectVerificationRequirement + ): + raise TypeError( + "effect_verification must be exactly " + "EffectVerificationRequirement or None." + ) + if self.expected_effects.is_empty and self.effect_verification is None: + raise ValueError( + "Effect verification requires expected symbolic effects or an " + "explicit physical-effect requirement." + ) + if not isinstance(self.failure_invalidation, StateDelta): + raise TypeError("failure_invalidation must be a StateDelta.") + if ( + any( + value is not None + for value in self.failure_invalidation.held_object_updates.values() + ) + or any( + value is not None + for value in self.failure_invalidation.coordinated_held_object_updates.values() + ) + or any( + value is not None + for value in self.failure_invalidation.articulation_joint_updates.values() + ) + ): + raise ValueError( + "failure_invalidation may only remove previously verified state." + ) object.__setattr__(self, "env_mask", self.env_mask.clone()) + object.__setattr__(self, "expected_effects", self.expected_effects.snapshot()) + object.__setattr__( + self, + "failure_invalidation", + self.failure_invalidation.snapshot(), + ) + object.__setattr__( + self, + "effect_verification", + ( + None + if self.effect_verification is None + else self.effect_verification.snapshot() + ), + ) + + def snapshot(self) -> EffectVerificationRequest: + """Return a request snapshot with an independently owned row mask.""" + return EffectVerificationRequest( + 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, + terminal_segment=self.terminal_segment, + requested_at=self.requested_at, + deadline=self.deadline, + env_mask=self.env_mask, + expected_effects=self.expected_effects, + effect_verification=self.effect_verification, + failure_invalidation=self.failure_invalidation, + ) @dataclass(frozen=True, slots=True, eq=False) -class JointCommand: - """Full-robot command produced by one session tick.""" +class EffectExpectationResult: + """Current-observation outcome for one physical state expectation. + + ``inverse_satisfied_mask`` is stronger than contradiction: every clause + must have reached its explicit inverse band for the monitor's complete + hysteresis window. It may therefore be used to retain a pre-existing + relation during failure reconciliation, while a single contradictory + clause may not. + """ - positions: torch.Tensor - velocities: torch.Tensor | None - active_mask: torch.Tensor - env_ids: torch.Tensor - hold_duration: torch.Tensor - """Per-environment delay before the next observation/command cycle.""" + expectation_id: str + satisfied_mask: torch.Tensor + contradicted_mask: torch.Tensor + inverse_satisfied_mask: torch.Tensor def __post_init__(self) -> None: - if self.positions.dim() != 2: - raise ValueError("JointCommand.positions must have shape (B, robot_dof).") if ( - self.velocities is not None - and self.velocities.shape != self.positions.shape - ): - raise ValueError("JointCommand.velocities must match positions shape.") - if self.active_mask.dtype != torch.bool or self.active_mask.shape != ( - self.positions.shape[0], - ): - raise ValueError("JointCommand.active_mask must be bool with shape (B,).") - if self.env_ids.dtype != torch.long or self.env_ids.shape != ( - self.positions.shape[0], - ): - raise ValueError("JointCommand.env_ids must be int64 with shape (B,).") - if not isinstance(self.hold_duration, torch.Tensor): - raise TypeError("JointCommand.hold_duration must be a torch.Tensor.") - if self.hold_duration.shape != (self.positions.shape[0],): - raise ValueError("JointCommand.hold_duration must have shape (B,).") + type(self.expectation_id) is not str + or not self.expectation_id + or self.expectation_id != self.expectation_id.strip() + ): + raise ValueError( + "expectation_id must be a non-empty string without outer whitespace." + ) + for name in ( + "satisfied_mask", + "contradicted_mask", + "inverse_satisfied_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.") + masks = ( + self.satisfied_mask, + self.contradicted_mask, + self.inverse_satisfied_mask, + ) + if any(mask.shape != masks[0].shape for mask in masks[1:]): + raise ValueError("Expectation-result masks must have equal shapes.") + if any(mask.device != masks[0].device for mask in masks[1:]): + raise ValueError("Expectation-result masks must use the same device.") + if (self.satisfied_mask & self.contradicted_mask).any(): + raise ValueError("satisfied_mask and contradicted_mask must not overlap.") + if (self.inverse_satisfied_mask & ~self.contradicted_mask).any(): + raise ValueError( + "inverse_satisfied_mask must be a subset of contradicted_mask." + ) + for name in ( + "satisfied_mask", + "contradicted_mask", + "inverse_satisfied_mask", + ): + object.__setattr__(self, name, getattr(self, name).clone()) + + def snapshot(self) -> EffectExpectationResult: + """Return an independently owned expectation outcome.""" + return EffectExpectationResult( + expectation_id=self.expectation_id, + satisfied_mask=self.satisfied_mask, + contradicted_mask=self.contradicted_mask, + inverse_satisfied_mask=self.inverse_satisfied_mask, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class EffectVerificationResult: + """Correlated per-environment update for one effect boundary. + + Rows absent from both ``success_mask`` and ``failure_mask`` remain + unresolved. ``invalidation_mask`` and ``retry_mask`` classify only failed + rows: the former selects the request's core-owned removal delta, while the + latter authorizes replay of the same invocation. Failed rows outside the + retry mask require external recovery. This lets one shared batch barrier + commit verified rows while other rows continue observing the same physical + effect. + """ + + verification_id: int + success_mask: torch.Tensor + failure_mask: torch.Tensor + invalidation_mask: torch.Tensor + retry_mask: torch.Tensor + expectation_results: tuple[EffectExpectationResult, ...] = () + + 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.") + for name in ( + "success_mask", + "failure_mask", + "invalidation_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.") + masks = ( + self.success_mask, + self.failure_mask, + self.invalidation_mask, + self.retry_mask, + ) + if any(mask.shape != masks[0].shape for mask in masks[1:]): + raise ValueError("Effect-result masks must have equal shapes.") + if any(mask.device != masks[0].device for mask in masks[1:]): + raise ValueError("Effect-result masks must use the same device.") + if (self.success_mask & self.failure_mask).any(): + raise ValueError("success_mask and failure_mask must not overlap.") + if (self.invalidation_mask & ~self.failure_mask).any(): + raise ValueError("invalidation_mask must be a subset of failure_mask.") + if (self.retry_mask & ~self.failure_mask).any(): + raise ValueError("retry_mask must be a subset of failure_mask.") + expectation_results = tuple(self.expectation_results) + if not all( + type(value) is EffectExpectationResult for value in expectation_results + ): + raise TypeError( + "expectation_results must contain exact EffectExpectationResult values." + ) + expectation_ids = [value.expectation_id for value in expectation_results] + if len(set(expectation_ids)) != len(expectation_ids): + raise ValueError("Effect expectation-result IDs must be unique.") + if expectation_results: + expected_success = torch.ones_like(self.success_mask) + expected_failure = torch.zeros_like(self.failure_mask) + for value in expectation_results: + if value.satisfied_mask.shape != self.success_mask.shape: + raise ValueError( + "Expectation and aggregate result masks must have equal shapes." + ) + if value.satisfied_mask.device != self.success_mask.device: + raise ValueError( + "Expectation and aggregate result masks must use the same device." + ) + expected_success &= value.satisfied_mask + expected_failure |= value.contradicted_mask + if not torch.equal(self.success_mask, expected_success): + raise ValueError( + "success_mask must equal the conjunction of expectation results." + ) + if not torch.equal(self.failure_mask, expected_failure): + raise ValueError( + "failure_mask must equal the union of expectation results." + ) + for name in ( + "success_mask", + "failure_mask", + "invalidation_mask", + "retry_mask", + ): + object.__setattr__(self, name, getattr(self, name).clone()) + object.__setattr__( + self, + "expectation_results", + tuple(value.snapshot() for value in expectation_results), + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class PhaseEffectGateRequest: + """Correlate a blocking physical-effect check with a segment entry. + + The action's preceding command remains active while the gate is unresolved. + A gate is scoped to the enclosing action attempt and does not create a + separate planning, recovery, or timeout budget. + + Args: + verification_id: Session-local single-use request identity. + gate_id: Invocation-local stable gate identity. + skill_id: Registered action skill identity. + invocation_id: Optional logical invocation correlation identity. + invocation_revision: Active invocation revision. + invocation_index: Active invocation position in the session. + attempt_generation: Installed action-plan attempt generation. + next_waypoint_index: First command frame blocked by the gate. + segment_name: Named trajectory segment blocked by the gate. + requested_at: Request creation time in the observation timestamp domain. + deadline: Enclosing action deadline in that same timestamp domain. + env_mask: Active rows that must satisfy the gate together. + """ + + verification_id: int + gate_id: str + skill_id: str + invocation_id: str | None + invocation_revision: int + invocation_index: int + attempt_generation: int + next_waypoint_index: int + segment_name: str + requested_at: float + deadline: float + env_mask: torch.Tensor + + def __post_init__(self) -> None: + for name in ( + "verification_id", + "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.") + for name in ("gate_id", "skill_id", "segment_name"): + value = getattr(self, name) + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{name} must be a non-empty string without outer whitespace." + ) + 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.") + if not math.isfinite(self.requested_at) or self.requested_at < 0.0: + raise ValueError("requested_at must be finite and non-negative.") + if not math.isfinite(self.deadline) or self.deadline < self.requested_at: + raise ValueError( + "deadline must be finite and no earlier than requested_at." + ) + 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 gated row.") + object.__setattr__(self, "env_mask", self.env_mask.clone()) + + def snapshot(self) -> PhaseEffectGateRequest: + """Return an independently owned gate request.""" + return PhaseEffectGateRequest( + verification_id=self.verification_id, + gate_id=self.gate_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, + requested_at=self.requested_at, + deadline=self.deadline, + env_mask=self.env_mask, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class PhaseEffectGateResult: + """Current-observation decision for one blocking segment-entry gate. + + Rows absent from both decision masks remain unresolved. ``retry_mask`` is + a subset of failed rows for which replaying the enclosing action remains + valid; no gate outcome mutates verified task state. + + Args: + verification_id: Identity copied from the consumed gate request. + gate_id: Stable gate identity copied from the request. + attempt_generation: Action attempt copied from the request. + invocation_index: Session invocation index copied from the request. + next_waypoint_index: Blocked waypoint copied from the request. + success_mask: Rows whose current evidence satisfies the gate. + failure_mask: Rows whose current evidence contradicts the gate. + retry_mask: Failed rows allowed to retry the enclosing action. + message: Optional physical-failure diagnostic. + """ + + verification_id: int + gate_id: str + attempt_generation: int + invocation_index: int + next_waypoint_index: int + success_mask: torch.Tensor + failure_mask: torch.Tensor + retry_mask: torch.Tensor + message: str = "" + + def __post_init__(self) -> None: + for name in ( + "verification_id", + "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.") if ( - not torch.isfinite(self.hold_duration).all() - or (self.hold_duration < 0.0).any() + type(self.gate_id) is not str + or not self.gate_id + or self.gate_id != self.gate_id.strip() + ): + raise ValueError( + "gate_id must be a non-empty string without outer whitespace." + ) + masks = (self.success_mask, self.failure_mask, self.retry_mask) + for name, value in zip( + ("success_mask", "failure_mask", "retry_mask"), + masks, + strict=True, + ): + 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 any(value.shape != masks[0].shape for value in masks[1:]): + raise ValueError("Phase-effect gate masks must have equal shapes.") + if any(value.device != masks[0].device for value in masks[1:]): + raise ValueError("Phase-effect gate masks must use the same device.") + if (self.success_mask & self.failure_mask).any(): + raise ValueError("Gate success and failure masks must not overlap.") + if (self.retry_mask & ~self.failure_mask).any(): + raise ValueError("retry_mask must be a subset of failure_mask.") + if type(self.message) is not str: + raise TypeError("message must be a string.") + for name in ("success_mask", "failure_mask", "retry_mask"): + object.__setattr__(self, name, getattr(self, name).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( - "JointCommand.hold_duration must contain finite non-negative values." - ) - if self.active_mask.device != self.positions.device: - raise ValueError("JointCommand tensors must share a device.") - if self.env_ids.device != self.positions.device: - raise ValueError("JointCommand tensors must share a device.") - if self.hold_duration.device != self.positions.device: - raise ValueError("JointCommand tensors must share a device.") - object.__setattr__(self, "positions", self.positions.clone()) - if self.velocities is not None: - object.__setattr__(self, "velocities", self.velocities.clone()) - object.__setattr__(self, "active_mask", self.active_mask.clone()) - object.__setattr__(self, "env_ids", self.env_ids.clone()) - object.__setattr__(self, "hold_duration", self.hold_duration.clone()) + "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) @@ -178,10 +870,12 @@ class ExecutionTick: status: ExecutionStatus eligible_mask: torch.Tensor - command: JointCommand | None + command: RuntimeCommandFrame | None + hold_targets: tuple[RuntimeEndpointTarget, ...] events: tuple[ExecutionEvent, ...] task_state: TaskState pending_effect: EffectVerificationRequest | None = None + pending_phase_effect_gate: PhaseEffectGateRequest | None = None def __post_init__(self) -> None: if self.eligible_mask.dtype != torch.bool or self.eligible_mask.dim() != 1: @@ -192,21 +886,76 @@ def __post_init__(self) -> None: raise TypeError( "pending_effect must be an EffectVerificationRequest or None." ) + if self.pending_phase_effect_gate is not None and not isinstance( + self.pending_phase_effect_gate, + PhaseEffectGateRequest, + ): + raise TypeError( + "pending_phase_effect_gate must be a PhaseEffectGateRequest or None." + ) + if ( + self.pending_effect is not None + and self.pending_phase_effect_gate is not None + ): + raise ValueError( + "Terminal effect verification and a phase-effect gate cannot be " + "pending together." + ) + if self.command is not None and not isinstance( + self.command, + RuntimeCommandFrame, + ): + raise TypeError("command must be a RuntimeCommandFrame or None.") + if isinstance(self.hold_targets, (str, bytes)) or not all( + isinstance(target, RuntimeEndpointTarget) for target in self.hold_targets + ): + raise TypeError("hold_targets must contain RuntimeEndpointTarget values.") + if self.command is not None and self.hold_targets: + raise ValueError("A tick cannot send commands and request a hold together.") + if self.pending_effect is not None: + if not isinstance(self.pending_effect, EffectVerificationRequest): + raise TypeError( + "pending_effect must be an EffectVerificationRequest or None." + ) + object.__setattr__( + self, + "pending_effect", + self.pending_effect.snapshot(), + ) + if self.pending_phase_effect_gate is not None: + object.__setattr__( + self, + "pending_phase_effect_gate", + self.pending_phase_effect_gate.snapshot(), + ) + hold_targets: list[RuntimeEndpointTarget] = [] + for target in self.hold_targets: + snapshot = target.snapshot() + if type(snapshot) is not type(target) or snapshot is target: + raise TypeError( + "RuntimeEndpointTarget.snapshot() must return an independently " + "owned value of the same target type." + ) + hold_targets.append(snapshot) object.__setattr__(self, "eligible_mask", self.eligible_mask.clone()) object.__setattr__(self, "events", tuple(self.events)) + object.__setattr__(self, "hold_targets", tuple(hold_targets)) class ExecutionSession: """Execute grounded invocations incrementally with bounded local recovery. The session never steps a simulator itself. Each :meth:`tick` consumes the - latest observation and scene snapshot and emits at most one full-robot - command. Expected symbolic effects are committed only after the caller - supplies ``effect_success`` for a non-empty :class:`StateDelta`. + latest observation and scene snapshot and emits at most one synchronized + endpoint-command frame. A declared physical-effect boundary resolves only + after the caller supplies a correlated :class:`EffectVerificationResult`. + Non-empty expected symbolic effects are committed for verified rows only. Environment eligibility and recovery budgets are tracked per row. The waypoint cursor is batch-synchronized: a recoverable row replans the active cohort from the latest observation and restarts the action trajectory. + Calls that mutate the session must be serialized by its owner; the session + does not provide thread synchronization. """ def __init__( @@ -214,35 +963,80 @@ def __init__( engine: AtomicActionEngine, invocations: tuple[ActionInvocation, ...], context: PlanningContext, + *, + eligible_mask: torch.Tensor | None = None, ) -> None: if not invocations: raise ValueError("ExecutionSession requires at least one invocation.") engine._validate_context(context) self._engine = engine self._requests: tuple[ResolvedActionRequest, ...] = tuple( - engine.resolve(invocation) for invocation in invocations + engine._resolve(invocation) for invocation in invocations ) self._task_state = context.task self._context = context self._invocation_index = 0 self._waypoint_index = 0 self._plan: ActionPlan | None = None + self._active_targets: dict[ + tuple[str, str], + RuntimeEndpointTarget, + ] = {} + self._active_tracking_routes: dict[ + tuple[str, str, str], + tuple[object, str, str], + ] = {} self._planned_scene = context.scene self._action_started_at = context.robot.timestamp - self._last_command: torch.Tensor | None = None + self._attempt_generation = -1 + self._last_tracking_frame: TrackingFrame | None = None self._last_command_mask = torch.zeros( context.batch_size, dtype=torch.bool, device=context.robot.qpos.device ) - self._eligible = torch.ones_like(self._last_command_mask) + self._tracking_violation_counts = torch.zeros( + context.batch_size, + dtype=torch.long, + device=context.robot.qpos.device, + ) + self._terminal_acceptance_counts = torch.zeros_like( + self._tracking_violation_counts + ) + self._terminal_started_at: float | None = None + self._terminal_pending_reported = False + self._eligible = ( + torch.ones_like(self._last_command_mask) + if eligible_mask is None + else self._normalize_mask(eligible_mask, "eligible_mask") + ) self._pending = self._eligible.clone() self._action_retries = torch.zeros( context.batch_size, dtype=torch.long, device=context.robot.qpos.device ) self._replans = torch.zeros_like(self._action_retries) self._pending_effect: EffectVerificationRequest | None = None - self._status = ExecutionStatus.RUNNING + 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._pending_phase_effect_gate: PhaseEffectGateRequest | None = None + self._satisfied_phase_effect_gates: set[str] = set() + self._reported_phase_effect_gates: set[str] = set() + self._next_phase_effect_gate_verification_id = 0 + self._plan_attempt_records: list[_ExecutionPlanAttemptRecord] = [] + self._status = ( + ExecutionStatus.RUNNING if self._eligible.any() else ExecutionStatus.FAILED + ) self._queued_events: list[ExecutionEvent] = [] - self._plan_current(context, ExecutionEventKind.ACTION_PLANNED) + if self._status is ExecutionStatus.RUNNING: + self._plan_current(context, ExecutionEventKind.ACTION_PLANNED) + else: + self._queued_events.append( + self._event( + ExecutionEventKind.SESSION_FAILED, + self._eligible, + "No environment was initially eligible for execution.", + ) + ) @property def status(self) -> ExecutionStatus: @@ -264,75 +1058,277 @@ def task_state(self) -> TaskState: """Verified symbolic task state accumulated by this session.""" return self._task_state - def revise_current(self, invocation: ActionInvocation) -> None: + @property + def effect_verification_pending(self) -> bool: + """Whether the current physical effect still requires verification.""" + return self._pending_effect is not None + + @property + 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 phase_effect_gate_request(self) -> PhaseEffectGateRequest | None: + """Return the blocking gate at the next trajectory-segment entry. + + Returns: + Owned request snapshot, or ``None`` when the next command is not + blocked by a physical-effect gate. + """ + request = self._phase_effect_gate_request() + return None if request is None else request.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, + *, + reason: str, + ) -> torch.Tensor: + """Permanently remove selected rows from this invocation sequence. + + Deactivation is sticky across action barriers and recovery replans. + The next emitted command frame marks those rows inactive so the command + sink can apply target-specific safe hold behavior. + + Args: + env_mask: Rows requested for deactivation. + reason: Human-readable event message. + + Returns: + Owned mask of rows that changed from eligible to inactive. + + Raises: + RuntimeError: If the session is already terminal. + ValueError: If ``reason`` is empty or the mask shape is invalid. + """ + if self._status is not ExecutionStatus.RUNNING: + raise RuntimeError("Only a running execution session can deactivate rows.") + if type(reason) is not str or not reason: + raise ValueError("reason must be a non-empty string.") + requested = self._normalize_mask(env_mask, "env_mask") + changed = requested & self._eligible + if not changed.any(): + return changed + self._eligible &= ~changed + self._pending &= ~changed + self._effect_failures &= ~changed + self._last_command_mask &= ~changed + self._queued_events.append( + self._event(ExecutionEventKind.ROWS_DEACTIVATED, changed, reason) + ) + if self._pending_effect is not None: + assert self._plan is not None + previous_effect = self._pending_effect + remaining_effect = ( + previous_effect.env_mask & self._pending & self._plan.plan_success + ) + if torch.equal(remaining_effect, previous_effect.env_mask): + self._pending_effect = previous_effect + elif remaining_effect.any(): + self._pending_effect = self._effect_verification_request( + remaining_effect + ) + else: + self._pending_effect = None + if self._pending_phase_effect_gate is not None: + self._pending_phase_effect_gate = None + self._next_phase_effect_gate_verification_id += 1 + terminal_event = self._update_terminal_status() + if terminal_event is not None: + self._queued_events.append(terminal_event) + return changed.clone() + + def revise_current( + self, + invocation: ActionInvocation, + *, + context: PlanningContext | None = None, + ) -> None: """Replace and replan the current invocation with a newer revision. The replacement is resolved into a new immutable request snapshot from - the latest observation. Retry and replan budgets restart for the new - revision, while verified task state, the current batch barrier, and - per-environment eligibility are preserved. Ordinary recovery replans - continue to reuse this snapshot until another explicit revision. + ``context`` or the session's latest observation. Retry and replan + budgets restart for the new revision, while verified task state, the + current batch barrier, and per-environment eligibility are preserved. + Ordinary recovery replans continue to reuse this snapshot until another + explicit revision. Once the action owns runtime destinations, the + replacement must preserve their exact address fingerprints; changing + controllers or safe-hold footprints requires a new invocation. Args: invocation: Grounded replacement for the currently active skill. Its ``revision`` must be strictly greater than the active one, and its ``skill_id`` and ``invocation_id`` must identify the same logical call. + context: Optional fresh observation used to ground the replacement. + A manually ticked caller may omit it to reuse + :attr:`latest_context`. Runner-driven code stages revisions on + :class:`ExecutionRunner`, which supplies a due-time observation. Raises: TypeError: If ``invocation`` is not an ActionInvocation. - RuntimeError: If the session is no longer running. + RuntimeError: If the session is no longer running or a physical + effect is awaiting verification. ValueError: If the replacement identifies another invocation or - does not advance the revision. + does not advance the revision, or if its plan changes the + active runtime target addresses. """ + replacement = self._prepare_revision(invocation) + replacement_context = self._context if context is None else context + self._install_prepared_revision(replacement, replacement_context) + + def _prepare_revision( + self, + invocation: ActionInvocation, + ) -> ResolvedActionRequest: + """Validate and snapshot one revision without planning or installing it.""" if not isinstance(invocation, ActionInvocation): raise TypeError("invocation must be an ActionInvocation.") if self._status is not ExecutionStatus.RUNNING: raise RuntimeError("Only a running execution session can be revised.") - current = self._requests[self._invocation_index] - if invocation.skill_id != current.skill_id: - raise ValueError( - f"Revision skill_id {invocation.skill_id!r} does not match " - f"the active skill {current.skill_id!r}." - ) - if invocation.invocation_id != current.invocation_id: - raise ValueError( - "Revision invocation_id must match the active invocation_id." + if ( + self._pending_effect is not None + or self._pending_phase_effect_gate is not None + or self._effect_failures.any() + ): + raise RuntimeError( + "Cannot revise while physical-effect resolution is pending; " + "resolve it or cancel and start a new invocation." ) - if invocation.revision <= current.revision: - raise ValueError( - f"Revision must advance beyond {current.revision}, got " - f"{invocation.revision}." + self._validate_revision_identity( + skill_id=invocation.skill_id, + invocation_id=invocation.invocation_id, + revision=invocation.revision, + ) + return self._engine.resolve(invocation) + + def _install_prepared_revision( + self, + replacement: ResolvedActionRequest, + context: PlanningContext, + ) -> None: + """Plan and transactionally install a previously snapshotted revision.""" + if not isinstance(replacement, ResolvedActionRequest): + raise TypeError("replacement must be a ResolvedActionRequest.") + if self._status is not ExecutionStatus.RUNNING: + raise RuntimeError("Only a running execution session can be revised.") + if ( + self._pending_effect is not None + or self._pending_phase_effect_gate is not None + or self._effect_failures.any() + ): + raise RuntimeError( + "Cannot revise while physical-effect resolution is pending; " + "resolve it or cancel and start a new invocation." ) + self._validate_revision_identity( + skill_id=replacement.skill_id, + invocation_id=replacement.invocation_id, + revision=replacement.revision, + ) + replacement_context = self._validated_context(context) + replacement_plan = self._engine.plan_request( + replacement, + replacement_context, + ) + self._validate_destination_continuity( + replacement_plan, + ExecutionEventKind.INVOCATION_REVISED, + ) + self._validate_tracking_continuity( + replacement_plan, + ExecutionEventKind.INVOCATION_REVISED, + ) - replacement = self._engine.resolve(invocation) - replacement_plan = self._engine.plan_request(replacement, self._context) requests = list(self._requests) requests[self._invocation_index] = replacement self._requests = tuple(requests) + self._context = replacement_context self._waypoint_index = 0 self._action_retries.zero_() self._replans.zero_() self._install_plan( replacement_plan, - self._context, + replacement_context, ExecutionEventKind.INVOCATION_REVISED, + destination_continuity_validated=True, ) + def _validate_revision_identity( + self, + *, + skill_id: str, + invocation_id: str | None, + revision: int, + ) -> None: + """Validate identity and ordering shared by staged and direct revisions.""" + current = self._requests[self._invocation_index] + if skill_id != current.skill_id: + raise ValueError( + f"Revision skill_id {skill_id!r} does not match " + f"the active skill {current.skill_id!r}." + ) + if invocation_id != current.invocation_id: + raise ValueError( + "Revision invocation_id must match the active invocation_id." + ) + if revision <= current.revision: + raise ValueError( + f"Revision must advance beyond {current.revision}, got " f"{revision}." + ) + @property def latest_context(self) -> PlanningContext: """Latest validated context with the session's verified task state.""" return self._context @property - def active_trajectory(self) -> TimedTrajectory: - """Return an owned snapshot of the active action trajectory. + def active_commands(self) -> TimedCommandSequence: + """Return an owned snapshot of the active action command sequence. This inspection surface is intended for diagnostics and visualization. Mutating the returned tensors cannot affect execution state. """ assert self._plan is not None - return self._plan.trajectory.snapshot() + return self._plan.commands.snapshot() + + @property + def active_plan(self) -> ActionPlan: + """Return an independently owned snapshot of the active action plan. + + This is a read-only diagnostics boundary for runtime metadata, + visualization, and tests. Planning and recovery remain session-owned; + mutating any tensor in the returned value cannot affect execution. + """ + assert self._plan is not None + return self._plan.snapshot() + + @property + def plan_attempts(self) -> tuple[ExecutionPlanAttempt, ...]: + """Return every installed plan in deterministic recovery order. + + The initial plan has generation zero. Each invocation revision, + recovery replan, or whole-action retry appends a new generation instead + of replacing earlier scene/collision evidence. + """ + return tuple(record.snapshot() for record in self._plan_attempt_records) def trajectory_segment(self, name: str) -> TrajectorySegment: """Return named segment metadata for the active action plan. @@ -347,61 +1343,263 @@ def tick( self, context: PlanningContext, *, - effect_success: torch.Tensor | None = None, + effect_result: EffectVerificationResult | None = None, + phase_effect_gate_result: PhaseEffectGateResult | None = None, + held_object_guard_result: HeldObjectGuardResult | None = None, ) -> ExecutionTick: """Advance execution by one observation/command cycle. Args: context: Latest measured robot and versioned scene state. Its task state is replaced by the session's verified task state. - effect_success: Optional per-environment semantic-effect verification - for an action waiting at its terminal waypoint. + effect_result: Optional correlated semantic-effect result for an + action waiting at its terminal waypoint. + phase_effect_gate_result: Optional correlated physical-effect + decision for a blocked trajectory-segment entry. + 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. """ - self._engine._validate_context(context) - if context.robot.timestamp < self._context.robot.timestamp: - raise ValueError("Execution tick timestamps must be monotonic.") - if context.scene.timestamp < self._context.scene.timestamp: - raise ValueError("Scene snapshot timestamps must be monotonic.") - if context.scene.version < self._context.scene.version: - raise ValueError("Scene snapshot versions must be monotonic.") - previous_collision_revision = torch.tensor( - self._context.scene.collision_world_revisions(context.batch_size), - dtype=torch.long, - device=context.robot.qpos.device, - ) - current_collision_revision = torch.tensor( - context.scene.collision_world_revisions(context.batch_size), - dtype=torch.long, - device=context.robot.qpos.device, - ) - if (current_collision_revision < previous_collision_revision).any(): - raise ValueError("Collision-world revisions must be monotonic.") - if not torch.equal(context.env_ids, self._context.env_ids): - raise ValueError("Execution tick env_ids must remain stable and ordered.") - self._context = PlanningContext( - robot=context.robot, - task=self._task_state, - scene=context.scene, - env_ids=context.env_ids, - ) + self._context = self._validated_context(context) events = self._drain_events() + if effect_result is not None: + if type(effect_result) is not EffectVerificationResult: + raise TypeError( + "effect_result must be exactly EffectVerificationResult or None." + ) + if self._pending_effect is None: + raise ValueError("No physical effect is awaiting verification.") + if effect_result.verification_id != self._pending_effect.verification_id: + raise ValueError( + "effect_result verification_id does not match the pending " + "effect boundary." + ) + phase_gate_request = self._phase_effect_gate_request() + if phase_effect_gate_result is not None: + if type(phase_effect_gate_result) is not PhaseEffectGateResult: + raise TypeError( + "phase_effect_gate_result must be exactly " + "PhaseEffectGateResult or None." + ) + if phase_gate_request is None: + raise ValueError("No phase-effect gate is awaiting verification.") + if phase_effect_gate_result.verification_id != ( + phase_gate_request.verification_id + ): + raise ValueError( + "phase_effect_gate_result verification_id does not match " + "the pending gate." + ) + for name in ( + "gate_id", + "attempt_generation", + "invocation_index", + "next_waypoint_index", + ): + if getattr(phase_effect_gate_result, name) != getattr( + phase_gate_request, + name, + ): + raise ValueError( + f"phase_effect_gate_result {name} does not match the " + "pending gate." + ) + self._next_phase_effect_gate_verification_id += 1 + self._pending_phase_effect_gate = None + 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 phase_effect_gate_result is not None: + assert phase_gate_request is not None + events.extend( + self._apply_phase_effect_gate_result( + phase_effect_gate_result, + phase_gate_request, + ) + ) + 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, + None, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) + if self._pending_effect is not None: execution_mask = ( self._pending_effect.env_mask & self._pending & self._plan.plan_success ) - command, completion_events = self._finish_action( - execution_mask, - effect_success, + if self._action_timed_out(self._plan, execution_mask): + pending_request = self._pending_effect + timed_out = execution_mask.clone() + known_failures = self._effect_failures.clone() + planning_failed = self._pending & ~self._plan.plan_success + invalidation_presence = self._failure_invalidation_presence_mask( + pending_request.failure_invalidation + ) + external_recovery = timed_out & invalidation_presence + retry_mask = ( + (timed_out & ~external_recovery) | known_failures | planning_failed + ) + self._apply_effect_failure_invalidation( + pending_request.failure_invalidation, + timed_out, + ) + self._pending_effect = None + self._effect_failures.zero_() + if external_recovery.any(): + self._eligible &= ~external_recovery + self._pending &= ~external_recovery + self._last_command_mask &= ~external_recovery + events.append( + self._event( + ExecutionEventKind.RECOVERY_REQUIRED, + external_recovery, + "Effect evidence remained unresolved at the action " + "deadline, so previously verified state was " + "invalidated before external recovery.", + ) + ) + if known_failures.any(): + events.append( + self._event( + ExecutionEventKind.EFFECT_VERIFICATION_FAILED, + known_failures, + "Required physical effects were not observed.", + ) + ) + if planning_failed.any(): + events.append( + self._event( + ExecutionEventKind.ACTION_PLANNING_FAILED, + planning_failed, + "Planning failed for pending environments.", + ) + ) + events.extend( + self._attempt_action_retry( + retry_mask, + ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT, + "Effect verification exceeded the action attempt timeout.", + reason_mask=timed_out, + ) + ) + if self._status is not ExecutionStatus.RUNNING: + return self._tick_result(command=None, events=events) + if not self._pending.any(): + command, hold_targets, completion_events = self._finish_action( + self._pending, + None, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) + assert self._plan is not None + effect_result = None + else: + command, hold_targets, completion_events = self._finish_action( + execution_mask, + effect_result, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) + + if self._effect_failures.any(): + failed_effect = self._effect_failures.clone() + planning_failed = self._pending & ~self._plan.plan_success + retry_mask = failed_effect | planning_failed + self._effect_failures.zero_() + if planning_failed.any(): + events.append( + self._event( + ExecutionEventKind.ACTION_PLANNING_FAILED, + planning_failed, + "Planning failed for pending environments.", + ) + ) + events.extend( + self._attempt_action_retry( + retry_mask, + ExecutionEventKind.EFFECT_VERIFICATION_FAILED, + "Required physical effects were not observed.", + reason_mask=failed_effect, + ) ) - events.extend(completion_events) - return self._tick_result(command=command, events=events) + if self._status is not ExecutionStatus.RUNNING: + return self._tick_result(command=None, events=events) + if not self._pending.any(): + command, hold_targets, completion_events = self._finish_action( + self._pending, + None, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) + assert self._plan is not None plan = self._plan execution_mask = self._pending & plan.plan_success @@ -409,44 +1607,138 @@ def tick( events.extend(recovery_events) if self._status is not ExecutionStatus.RUNNING: return self._tick_result(command=None, events=events) - if recovery_events and any( - event.kind - in { - ExecutionEventKind.REPLANNED, - ExecutionEventKind.RECOVERY_EXHAUSTED, - } - for event in recovery_events - ): + if recovery_events: assert self._plan is not None plan = self._plan execution_mask = self._pending & self._plan.plan_success + if not self._pending.any(): + command, hold_targets, completion_events = self._finish_action( + self._pending, + None, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) - trajectory = plan.trajectory - if self._waypoint_index < trajectory.waypoint_count: + if not execution_mask.any(): + command, hold_targets, completion_events = self._finish_action( + execution_mask, + None, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) + + phase_gate_request = self._phase_effect_gate_request() + if phase_gate_request is not None: + events.extend(self._phase_effect_gate_required_events(phase_gate_request)) + preceding_waypoint = phase_gate_request.next_waypoint_index - 1 + command = self._command_at(plan, preceding_waypoint, execution_mask) + return self._tick_result(command=command, events=events) + + commands = plan.commands + if self._waypoint_index < commands.frame_count: command = self._command_at(plan, self._waypoint_index, execution_mask) self._waypoint_index += 1 return self._tick_result(command=command, events=events) - terminal_error = self._terminal_error(plan) - not_reached = execution_mask & ( - terminal_error > plan.recovery_policy.tracking_error_threshold - ) - if not_reached.any(): - events.extend( - self._attempt_replan( - not_reached, - ExecutionEventKind.TRACKING_ERROR, - "Terminal command has not been reached.", + terminal = plan.tracking_policy.terminal + if self._terminal_started_at is None: + self._terminal_started_at = self._context.robot.timestamp + elapsed_terminal = self._context.robot.timestamp - self._terminal_started_at + terminal_pending = torch.zeros_like(execution_mask) + if isinstance(terminal, TimedTerminalAcceptance): + if elapsed_terminal < terminal.settle_duration: + terminal_pending = execution_mask.clone() + elif isinstance(terminal, FeedbackTerminalAcceptance): + if plan.tracking is None or not plan.tracking.frames: + raise RuntimeError( + "Feedback terminal acceptance requires a terminal tracking " + "frame." + ) + try: + accepted, valid, normalized_error = self._evaluate_tracking_frame( + plan.tracking.frames[-1], + terminal.metrics, + ) + except Exception as exc: # noqa: BLE001 - fail required feedback closed + events.extend( + self._fail_tracking_feedback( + execution_mask, + "Terminal tracking feedback evaluation failed: " + f"{type(exc).__name__}: {exc}", + ) ) - ) - if self._status is not ExecutionStatus.RUNNING: return self._tick_result(command=None, events=events) - assert self._plan is not None - plan = self._plan - execution_mask = self._pending & self._plan.plan_success - command = self._command_at(plan, 0, execution_mask) - self._waypoint_index = 1 - return self._tick_result(command=command, events=events) + invalid = execution_mask & ~valid + if invalid.any(): + events.extend( + self._fail_tracking_feedback( + invalid, + "Required terminal tracking feedback was invalid.", + ) + ) + if self._status is not ExecutionStatus.RUNNING: + return self._tick_result(command=None, events=events) + execution_mask = self._pending & plan.plan_success + accepted_now = execution_mask & valid & accepted + self._terminal_acceptance_counts[accepted_now] += 1 + self._terminal_acceptance_counts[execution_mask & ~accepted_now] = 0 + terminal_pending = execution_mask & ( + self._terminal_acceptance_counts < terminal.consecutive_acceptances + ) + if terminal_pending.any() and elapsed_terminal >= terminal.settle_timeout: + max_error = float(normalized_error[terminal_pending].amax().item()) + events.extend( + self._attempt_action_retry( + terminal_pending, + ExecutionEventKind.TERMINAL_ACCEPTANCE_FAILED, + "Terminal feedback did not satisfy the acceptance " + "contract before its settle timeout " + f"(max_normalized_error={max_error:.6f}).", + ) + ) + if self._status is not ExecutionStatus.RUNNING: + return self._tick_result(command=None, events=events) + assert self._plan is not None + plan = self._plan + execution_mask = self._pending & plan.plan_success + if plan.commands.frame_count > 0 and execution_mask.any(): + command = self._command_at(plan, 0, execution_mask) + self._waypoint_index = 1 + return self._tick_result(command=command, events=events) + terminal_pending.zero_() + else: # pragma: no cover - TrackingPolicy validates exact alternatives + raise AssertionError( + f"Unsupported terminal acceptance {type(terminal).__name__}." + ) + + if terminal_pending.any(): + if not self._terminal_pending_reported: + events.append( + self._event( + ExecutionEventKind.TERMINAL_ACCEPTANCE_PENDING, + terminal_pending, + "Maintaining the terminal command while acceptance is " + "pending.", + ) + ) + self._terminal_pending_reported = True + if plan.commands.frame_count == 0: + raise RuntimeError( + "Terminal settling requires an executable terminal command " + "frame." + ) + terminal_command = plan.commands.frames[-1].with_active_mask( + plan.commands.frames[-1].active_mask & terminal_pending + ) + return self._tick_result(command=terminal_command, events=events) events.append( self._event( @@ -456,12 +1748,47 @@ def tick( ) ) - command, completion_events = self._finish_action( + command, hold_targets, completion_events = self._finish_action( execution_mask, - effect_success, + effect_result, ) events.extend(completion_events) - return self._tick_result(command=command, events=events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) + + def _validated_context(self, context: PlanningContext) -> PlanningContext: + """Validate one monotonic observation and attach verified task state.""" + self._engine._validate_context(context) + if context.robot.timestamp < self._context.robot.timestamp: + raise ValueError("Execution tick timestamps must be monotonic.") + if context.scene.timestamp < self._context.scene.timestamp: + raise ValueError("Scene snapshot timestamps must be monotonic.") + if context.scene.version < self._context.scene.version: + raise ValueError("Scene snapshot versions must be monotonic.") + previous_collision_revision = torch.tensor( + self._context.scene.collision_world_revisions(context.batch_size), + dtype=torch.long, + device=context.robot.qpos.device, + ) + current_collision_revision = torch.tensor( + context.scene.collision_world_revisions(context.batch_size), + dtype=torch.long, + device=context.robot.qpos.device, + ) + if (current_collision_revision < previous_collision_revision).any(): + raise ValueError("Collision-world revisions must be monotonic.") + if not torch.equal(context.env_ids, self._context.env_ids): + raise ValueError("Execution tick env_ids must remain stable and ordered.") + return PlanningContext( + robot=context.robot, + task=self._task_state, + scene=context.scene, + env_ids=context.env_ids, + control_dt=context.control_dt, + ) def _plan_current( self, @@ -470,7 +1797,7 @@ def _plan_current( ) -> None: """Plan the current invocation from the latest observation.""" request = self._requests[self._invocation_index] - plan = self._engine.plan_request(request, context) + plan = self._engine._plan_request(request, context) self._install_plan(plan, context, event_kind) def _install_plan( @@ -478,20 +1805,210 @@ def _install_plan( plan: ActionPlan, context: PlanningContext, event_kind: ExecutionEventKind, + *, + destination_continuity_validated: bool = False, ) -> None: - """Install an already validated plan as the current execution plan.""" + """Install a plan, checking target continuity unless already checked.""" + replacement_targets = { + (target.transport_id, target.target_id): target + for target in plan.commands.targets + } + replacement_destinations = frozenset(replacement_targets) + replacement_tracking_routes = self._tracking_routes(plan) + self._validate_destination_continuity(plan, event_kind) + self._validate_tracking_continuity(plan, event_kind) + self._validate_phase_effect_gates(plan) + if ( + event_kind + not in ( + ExecutionEventKind.REPLANNED, + ExecutionEventKind.INVOCATION_REVISED, + ) + or replacement_destinations + ): + self._active_targets = replacement_targets + if ( + event_kind + not in ( + ExecutionEventKind.REPLANNED, + ExecutionEventKind.INVOCATION_REVISED, + ) + or replacement_tracking_routes + ): + self._active_tracking_routes = replacement_tracking_routes self._plan = plan + self._attempt_generation += 1 self._waypoint_index = 0 self._planned_scene = context.scene self._action_started_at = context.robot.timestamp - self._last_command = None + self._last_tracking_frame = None self._last_command_mask.zero_() + self._tracking_violation_counts.zero_() + self._terminal_acceptance_counts.zero_() + self._terminal_started_at = None + self._terminal_pending_reported = False self._pending_effect = None + self._effect_failures.zero_() + self._effect_requested_at = None + self._pending_phase_effect_gate = None + self._satisfied_phase_effect_gates.clear() + self._reported_phase_effect_gates.clear() planned_mask = self._pending & plan.plan_success + self._plan_attempt_records.append( + _ExecutionPlanAttemptRecord( + attempt_generation=self._attempt_generation, + event_kind=event_kind, + planned_at=context.robot.timestamp, + invocation_index=self._invocation_index, + planned_mask=planned_mask.clone(), + action_retry_counts=tuple( + int(value) for value in self._action_retries.detach().cpu().tolist() + ), + replan_counts=tuple( + int(value) for value in self._replans.detach().cpu().tolist() + ), + request=self._requests[self._invocation_index].snapshot(), + plan=plan.snapshot(), + ) + ) self._queued_events.append( self._event(event_kind, planned_mask, "Planned from the latest context.") ) + def _validate_phase_effect_gates(self, plan: ActionPlan) -> None: + """Bind invocation-owned gates to non-initial named plan segments.""" + request = self._requests[self._invocation_index] + for requirement in request.phase_effect_gates: + if type(requirement) is not PhaseEffectGateRequirement: + raise TypeError( + "Resolved phase-effect gates must be exact " + "PhaseEffectGateRequirement values." + ) + try: + segment = plan.segment(requirement.segment_name) + except KeyError as exc: + raise ValueError( + f"Phase-effect gate {requirement.gate_id!r} references " + f"missing segment {requirement.segment_name!r}." + ) from exc + if segment.start == 0: + raise ValueError( + f"Phase-effect gate {requirement.gate_id!r} cannot block the " + "first trajectory segment because no preceding command exists " + "to preserve while evidence is acquired." + ) + + def _validate_destination_continuity( + self, + plan: ActionPlan, + event_kind: ExecutionEventKind, + ) -> None: + """Reject in-place plans that change controller or safe-hold ownership.""" + if event_kind not in ( + ExecutionEventKind.REPLANNED, + ExecutionEventKind.INVOCATION_REVISED, + ): + return + replacement_targets = { + (target.transport_id, target.target_id): target + for target in plan.commands.targets + } + active_destinations = frozenset(self._active_targets) + replacement_destinations = frozenset(replacement_targets) + if not active_destinations: + return + if not replacement_destinations: + if event_kind is ExecutionEventKind.REPLANNED: + return + raise ValueError( + "Invocation revisions must declare the active runtime destination " + "set; an empty replacement plan cannot prove target continuity." + ) + if replacement_destinations == active_destinations: + mismatched_fingerprints = sorted( + destination + for destination in active_destinations + if replacement_targets[destination].address_fingerprint + != self._active_targets[destination].address_fingerprint + ) + if not mismatched_fingerprints: + return + prefix = ( + "Recovery replans" + if event_kind is ExecutionEventKind.REPLANNED + else "Invocation revisions" + ) + guidance = ( + "" + if event_kind is ExecutionEventKind.REPLANNED + else " Start a new invocation to change runtime target addresses." + ) + raise ValueError( + f"{prefix} must preserve each runtime target address fingerprint; " + f"changed={mismatched_fingerprints}.{guidance}" + ) + if event_kind is ExecutionEventKind.REPLANNED: + prefix = "Recovery replans" + guidance = "" + else: + prefix = "Invocation revisions" + guidance = " Start a new invocation to change runtime destinations." + raise ValueError( + f"{prefix} must preserve the active runtime destination set; " + f"previous={sorted(active_destinations)}, " + f"replacement={sorted(replacement_destinations)}.{guidance}" + ) + + def _validate_tracking_continuity( + self, + plan: ActionPlan, + event_kind: ExecutionEventKind, + ) -> None: + """Reject in-place replacement of feedback ownership or projection.""" + if event_kind not in ( + ExecutionEventKind.REPLANNED, + ExecutionEventKind.INVOCATION_REVISED, + ): + return + if self._plan is None: + return + previous_routes = self._active_tracking_routes + replacement_routes = self._tracking_routes(plan) + if ( + event_kind is ExecutionEventKind.REPLANNED + and not plan.commands.targets + and not replacement_routes + ): + return + if previous_routes == replacement_routes: + return + prefix = ( + "Recovery replans" + if event_kind is ExecutionEventKind.REPLANNED + else "Invocation revisions" + ) + raise ValueError( + f"{prefix} must preserve endpoint tracking source fingerprints and " + "projector routes; start a new invocation to change feedback " + "ownership." + ) + + @staticmethod + def _tracking_routes( + plan: ActionPlan, + ) -> dict[tuple[str, str, str], tuple[object, str, str]]: + """Return the complete feedback/projector route owned by one plan.""" + if plan.tracking is None or not plan.tracking.frames: + return {} + return { + setpoint.key: ( + setpoint.binding.source.source_fingerprint, + setpoint.binding.projector.projector_id, + setpoint.binding.projector.revision, + ) + for setpoint in plan.tracking.frames[0].setpoints + } + def _recover_if_needed( self, plan: ActionPlan, @@ -501,10 +2018,7 @@ def _recover_if_needed( events: list[ExecutionEvent] = [] if not execution_mask.any(): return events - if ( - self._context.robot.timestamp - self._action_started_at - > plan.recovery_policy.action_timeout - ): + if self._action_timed_out(plan, execution_mask): return self._attempt_action_retry( execution_mask, ExecutionEventKind.ACTION_TIMEOUT, @@ -517,30 +2031,74 @@ def _recover_if_needed( ExecutionEventKind.COLLISION_WORLD_CHANGED, "The collision world changed after this trajectory was planned.", ) - if self._last_command is not None: - tracking_error = torch.amax( - torch.abs(self._context.robot.qpos - self._last_command), dim=1 - ) - tracking_mask = ( - execution_mask - & self._last_command_mask - & (tracking_error > plan.recovery_policy.tracking_error_threshold) - ) - if tracking_mask.any(): - return self._attempt_replan( - tracking_mask, - ExecutionEventKind.TRACKING_ERROR, - "Observed joint tracking error exceeded the policy threshold.", + in_flight = plan.tracking_policy.in_flight + if ( + in_flight is not None + and self._last_tracking_frame is not None + and self._waypoint_index < plan.commands.frame_count + ): + tracking_mask = execution_mask & self._last_command_mask + if ( + tracking_mask.any() + and self._context.robot.timestamp - self._action_started_at + >= in_flight.grace_period + ): + try: + accepted, valid, normalized_error = self._evaluate_tracking_frame( + self._last_tracking_frame, + in_flight.metrics, + ) + except Exception as exc: # noqa: BLE001 - fail required feedback closed + return self._fail_tracking_feedback( + tracking_mask, + "In-flight tracking feedback evaluation failed: " + f"{type(exc).__name__}: {exc}", + ) + invalid = tracking_mask & ~valid + if invalid.any(): + return self._fail_tracking_feedback( + invalid, + "Required in-flight tracking feedback was invalid.", + ) + violated = tracking_mask & valid & ~accepted + self._tracking_violation_counts[violated] += 1 + self._tracking_violation_counts[tracking_mask & ~violated] = 0 + diverged = tracking_mask & ( + self._tracking_violation_counts >= in_flight.consecutive_violations ) - scene_mask = self._dynamic_scene_change_mask(plan) - if (execution_mask & scene_mask).any(): + if diverged.any(): + max_error = float(normalized_error[diverged].amax().item()) + return self._attempt_replan( + diverged, + ExecutionEventKind.TRACKING_DIVERGED, + "Observed in-flight tracking diverged from the commanded " + f"setpoint (max_normalized_error={max_error:.6f}).", + ) + scene_mask, scene_message = self._dynamic_scene_change( + plan, + execution_mask, + ) + if scene_mask.any(): + assert scene_message is not None return self._attempt_replan( - execution_mask & scene_mask, + scene_mask, ExecutionEventKind.DYNAMIC_GOAL_CHANGED, - "A referenced scene entity moved beyond the policy threshold.", + scene_message, ) return events + def _action_timed_out( + self, + plan: ActionPlan, + execution_mask: torch.Tensor, + ) -> bool: + """Return whether an active action attempt exceeded its deadline.""" + return bool( + execution_mask.any() + and self._context.robot.timestamp - self._action_started_at + > plan.recovery_policy.action_timeout + ) + def _attempt_replan( self, trigger_mask: torch.Tensor, @@ -571,7 +2129,9 @@ def _attempt_replan( self._replans[allowed] += 1 self._plan_current(self._context, ExecutionEventKind.REPLANNED) events.extend(self._drain_events()) - self._update_terminal_status() + terminal_event = self._update_terminal_status() + if terminal_event is not None: + events.append(terminal_event) return events def _attempt_action_retry( @@ -579,11 +2139,16 @@ def _attempt_action_retry( trigger_mask: torch.Tensor, reason: ExecutionEventKind, message: str, + *, + reason_mask: torch.Tensor | None = None, ) -> list[ExecutionEvent]: """Retry the current action or permanently fail exhausted rows.""" assert self._plan is not None policy = self._plan.recovery_policy - events = [self._event(reason, trigger_mask, message)] + cause_mask = trigger_mask if reason_mask is None else reason_mask + events = [self._event(reason, cause_mask, message)] + self._pending_effect = None + self._effect_failures &= ~trigger_mask allowed = trigger_mask & (self._action_retries < policy.max_action_retries) exhausted = trigger_mask & ~allowed if exhausted.any(): @@ -598,7 +2163,7 @@ def _attempt_action_retry( ) if allowed.any(): self._action_retries[allowed] += 1 - self._replans.zero_() + self._replans[allowed] = 0 events.append( self._event( ExecutionEventKind.ACTION_RETRY, @@ -608,75 +2173,225 @@ def _attempt_action_retry( ) self._plan_current(self._context, ExecutionEventKind.REPLANNED) events.extend(self._drain_events()) - self._update_terminal_status() + terminal_event = self._update_terminal_status() + if terminal_event is not None: + events.append(terminal_event) return events def _finish_action( self, execution_mask: torch.Tensor, - effect_success: torch.Tensor | None, - ) -> tuple[JointCommand | None, list[ExecutionEvent]]: + effect_result: EffectVerificationResult | None, + ) -> tuple[ + RuntimeCommandFrame | None, + tuple[RuntimeEndpointTarget, ...], + list[ExecutionEvent], + ]: """Verify effects, update symbolic state, and advance the action barrier.""" assert self._plan is not None + plan_targets = self._plan.commands.targets + active_targets = ( + plan_targets + if plan_targets + else tuple(target.snapshot() for target in self._active_targets.values()) + ) + orphaned_targets = bool(active_targets) and not plan_targets events: list[ExecutionEvent] = [] + if not self._pending.any(): + hold_targets, barrier_events = self._advance_action_barrier( + active_targets, + orphaned_targets=orphaned_targets, + ) + return None, hold_targets, barrier_events + planning_failed = self._pending & ~self._plan.plan_success if not execution_mask.any() and planning_failed.any(): events.extend( self._attempt_action_retry( planning_failed, - ExecutionEventKind.ACTION_RETRY, + ExecutionEventKind.ACTION_PLANNING_FAILED, "Planning failed for every pending environment.", ) ) if self._status is not ExecutionStatus.RUNNING: - return None, events - return self._hold_command(), events - - if self._plan.expected_effects.is_empty: + return None, active_targets, events + if not self._pending.any(): + hold_targets, barrier_events = self._advance_action_barrier( + active_targets, + orphaned_targets=orphaned_targets, + ) + events.extend(barrier_events) + return None, hold_targets, events + return None, active_targets, events + + failed_effect = torch.zeros_like(execution_mask) + unresolved = torch.zeros_like(execution_mask) + made_progress = False + if not self._plan.requires_effect_verification: verified = execution_mask - elif effect_success is None: + elif effect_result is None: if self._pending_effect is None: self._pending_effect = self._effect_verification_request(execution_mask) events.append( self._event( ExecutionEventKind.EFFECT_VERIFICATION_REQUIRED, execution_mask, - "Expected symbolic effects require external verification.", + "The action requires external physical-effect verification.", ) ) - return self._hold_command(), events + return None, active_targets, events else: - verified_input = self._normalize_mask(effect_success, "effect_success") - verified = execution_mask & verified_input - self._pending_effect = None + assert self._pending_effect is not None + pending_request = self._pending_effect + success_input = self._normalize_mask( + effect_result.success_mask, + "effect_result.success_mask", + ) + failure_input = self._normalize_mask( + effect_result.failure_mask, + "effect_result.failure_mask", + ) + invalidation_input = self._normalize_mask( + effect_result.invalidation_mask, + "effect_result.invalidation_mask", + ) + retry_input = self._normalize_mask( + effect_result.retry_mask, + "effect_result.retry_mask", + ) + reported = success_input | failure_input + if (reported & ~execution_mask).any(): + raise ValueError( + "Effect verification masks must be subsets of the pending " + "effect request env_mask." + ) + for outcome in effect_result.expectation_results: + for name in ( + "satisfied_mask", + "contradicted_mask", + "inverse_satisfied_mask", + ): + outcome_mask = self._normalize_mask( + getattr(outcome, name), + f"effect_result.expectation_results.{name}", + ) + if (outcome_mask & ~execution_mask).any(): + raise ValueError( + "Effect expectation-result masks must be subsets " + "of the pending effect request env_mask." + ) + verified = execution_mask & success_input + failed_effect = execution_mask & failure_input + unresolved = execution_mask & ~reported + made_progress = bool(reported.any().item()) + invalidated = failed_effect & invalidation_input + retryable_failure = failed_effect & retry_input + external_recovery = failed_effect & ~retry_input + self._apply_effect_failure_invalidation( + pending_request.failure_invalidation, + invalidated, + ) + self._effect_failures |= retryable_failure + if external_recovery.any(): + self._eligible &= ~external_recovery + self._pending &= ~external_recovery + self._effect_failures &= ~external_recovery + self._last_command_mask &= ~external_recovery + events.extend( + ( + self._event( + ExecutionEventKind.EFFECT_VERIFICATION_FAILED, + external_recovery, + "Required physical effects were contradicted.", + ), + self._event( + ExecutionEventKind.RECOVERY_REQUIRED, + external_recovery, + "The reconciled effect failure cannot safely replay " + "the current invocation.", + ), + ) + ) + if not unresolved.any(): + self._pending_effect = None if verified.any(): - self._task_state = self._plan.expected_effects.apply( - self._task_state, verified + if not self._plan.expected_effects.is_empty: + self._task_state = self._plan.expected_effects.apply( + self._task_state, verified + ) + self._context = PlanningContext( + robot=self._context.robot, + task=self._task_state, + scene=self._context.scene, + env_ids=self._context.env_ids, + ) + self._pending &= ~verified + if unresolved.any(): + if made_progress: + self._pending_effect = self._effect_verification_request(unresolved) + return None, active_targets, events + terminal_event = self._update_terminal_status() + if terminal_event is not None: + events.append(terminal_event) + return None, active_targets, events + retry_candidates = self._effect_failures | planning_failed + if retry_candidates.any(): + effect_failure_mask = self._effect_failures.clone() + self._effect_failures.zero_() + reason = ( + ExecutionEventKind.EFFECT_VERIFICATION_FAILED + if effect_failure_mask.any() + else ExecutionEventKind.ACTION_PLANNING_FAILED ) - self._context = PlanningContext( - robot=self._context.robot, - task=self._task_state, - scene=self._context.scene, - env_ids=self._context.env_ids, + reason_mask = ( + effect_failure_mask if effect_failure_mask.any() else retry_candidates ) - self._pending &= ~verified - failed_effect = execution_mask & ~verified - retry_mask = failed_effect | planning_failed - if retry_mask.any(): + if effect_failure_mask.any() and planning_failed.any(): + events.append( + self._event( + ExecutionEventKind.ACTION_PLANNING_FAILED, + planning_failed, + "Planning failed for pending environments.", + ) + ) events.extend( self._attempt_action_retry( - retry_mask, - ExecutionEventKind.ACTION_RETRY, + retry_candidates, + reason, "Planning or expected-effect verification failed.", + reason_mask=reason_mask, ) ) if self._status is not ExecutionStatus.RUNNING: - return None, events - return self._hold_command(), events + return None, active_targets, events + if self._pending.any(): + return None, active_targets, events + + if self._pending.any(): + return None, active_targets, events + hold_targets, barrier_events = self._advance_action_barrier( + active_targets, + orphaned_targets=orphaned_targets, + ) + events.extend(barrier_events) + return None, hold_targets, events + def _advance_action_barrier( + self, + active_targets: tuple[RuntimeEndpointTarget, ...], + *, + orphaned_targets: bool, + ) -> tuple[tuple[RuntimeEndpointTarget, ...], list[ExecutionEvent]]: + """Complete an empty action cohort and install the next invocation.""" + if self._status is not ExecutionStatus.RUNNING or self._plan is None: + raise RuntimeError("Only a running planned action can cross its barrier.") if self._pending.any(): - return self._hold_command(), events + raise RuntimeError("The action barrier cannot advance with pending rows.") + self._pending_effect = None + self._effect_failures.zero_() + self._effect_requested_at = None + events: list[ExecutionEvent] = [] events.append( self._event( ExecutionEventKind.ACTION_COMPLETED, @@ -691,95 +2406,143 @@ def _finish_action( if self._eligible.any() else ExecutionStatus.FAILED ) + terminal_kind = ( + ExecutionEventKind.SESSION_COMPLETED + if self._status is ExecutionStatus.COMPLETED + else ExecutionEventKind.SESSION_FAILED + ) events.append( self._event( - ExecutionEventKind.SESSION_COMPLETED, + terminal_kind, self._eligible, "Invocation sequence completed.", ) ) - return None, events + return (active_targets if orphaned_targets else ()), events self._pending = self._eligible.clone() self._pending_effect = None + self._effect_failures.zero_() self._action_retries.zero_() self._replans.zero_() self._plan_current(self._context, ExecutionEventKind.ACTION_PLANNED) events.extend(self._drain_events()) - return self._hold_command(), events + return active_targets, events def _command_at( self, plan: ActionPlan, waypoint_index: int, active_mask: torch.Tensor, - ) -> JointCommand: - """Build one command and retain it for tracking-error monitoring.""" - positions = plan.trajectory.positions[:, waypoint_index] - hold = self._context.robot.qpos - positions = torch.where(active_mask[:, None], positions, hold) - velocities = None - if plan.trajectory.velocities is not None: - values = plan.trajectory.velocities[:, waypoint_index] - velocities = torch.where( - active_mask[:, None], values, torch.zeros_like(values) - ) - self._last_command = positions.clone() - self._last_command_mask = active_mask.clone() - # ``dt[:, i]`` leads to waypoint ``i``. After dispatching waypoint - # ``i``, wait for ``dt[:, i + 1]`` before the next dispatch. Reuse the - # final arrival interval as its terminal settling window. - next_waypoint_index = min( - waypoint_index + 1, - plan.trajectory.waypoint_count - 1, - ) - hold_duration = plan.trajectory.dt[:, next_waypoint_index] - return JointCommand( - positions=positions, - velocities=velocities, - active_mask=active_mask, - env_ids=plan.trajectory.env_ids, - hold_duration=hold_duration, - ) - - def _hold_command(self) -> JointCommand: - """Build a passive hold command from the latest observation.""" - return JointCommand( - positions=self._context.robot.qpos, - velocities=torch.zeros_like(self._context.robot.qpos), - active_mask=torch.zeros_like(self._eligible), - env_ids=self._context.env_ids, - hold_duration=torch.zeros( - self._context.batch_size, - dtype=torch.float32, - device=self._context.robot.qpos.device, - ), + ) -> RuntimeCommandFrame: + """Return one frame and retain its generic typed tracking targets.""" + frame = plan.commands.frames[waypoint_index] + frame = frame.with_active_mask(frame.active_mask & active_mask) + self._last_tracking_frame = ( + None + if plan.tracking is None + else plan.tracking.frames[waypoint_index].snapshot() ) - - def _terminal_error(self, plan: ActionPlan) -> torch.Tensor: - """Return per-row max joint error to the action terminal command.""" - if plan.trajectory.waypoint_count == 0: - return torch.full_like(self._eligible, float("inf"), dtype=torch.float32) - return torch.amax( - torch.abs(self._context.robot.qpos - plan.trajectory.positions[:, -1]), - dim=1, + self._last_command_mask = frame.active_mask.clone() + if waypoint_index == plan.commands.frame_count - 1: + self._terminal_started_at = self._context.robot.timestamp + self._terminal_acceptance_counts.zero_() + self._terminal_pending_reported = False + return frame + + def _evaluate_tracking_frame( + self, + frame: TrackingFrame, + metrics: tuple[TrackingMetricCfg, ...], + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: + """Aggregate typed endpoint predicates without mixing physical units.""" + evaluations = self._engine.tracking_runtime.evaluate_frame( + frame, + metrics, + self._context, ) + accepted = torch.ones_like(self._eligible) + valid = torch.ones_like(self._eligible) + normalized_error = torch.zeros( + self._context.batch_size, + dtype=self._context.robot.qpos.dtype, + device=self._context.robot.qpos.device, + ) + for evaluation in evaluations.values(): + if not isinstance(evaluation, TrackingEvaluation): + raise TypeError( + "TrackingRuntime.evaluate_frame() must return " + "TrackingEvaluation values." + ) + accepted &= evaluation.accepted_mask + valid &= evaluation.valid_mask + normalized_error = torch.maximum( + normalized_error, + evaluation.normalized_error.to(normalized_error.dtype), + ) + return accepted, valid, normalized_error + + def _fail_tracking_feedback( + self, + failed_mask: torch.Tensor, + message: str, + ) -> list[ExecutionEvent]: + """Fail affected rows closed when required feedback is unavailable.""" + self._eligible &= ~failed_mask + self._pending &= ~failed_mask + events = [ + self._event( + ExecutionEventKind.TRACKING_FEEDBACK_FAILED, + failed_mask, + message, + ) + ] + terminal_event = self._update_terminal_status() + if terminal_event is not None: + events.append(terminal_event) + return events - def _dynamic_scene_change_mask(self, plan: ActionPlan) -> torch.Tensor: - """Detect material motion of entities referenced by the action goal.""" + def _dynamic_scene_change( + self, + plan: ActionPlan, + execution_mask: torch.Tensor, + ) -> tuple[torch.Tensor, str | None]: + """Detect and describe material scene-dependency invalidation.""" dependencies = plan.scene_dependencies changed = torch.zeros_like(self._eligible) if ( not dependencies or self._context.scene.version == self._planned_scene.version ): - return changed + return changed, None policy = plan.recovery_policy - for entity_id in dependencies: + details: list[str] = [] + for entity_id in sorted(dependencies): + monitor_until = plan.scene_dependency_monitor_until.get(entity_id) + if monitor_until is not None and self._waypoint_index >= monitor_until: + continue previous = self._planned_scene.entities.get(entity_id) current = self._context.scene.entities.get(entity_id) if previous is None or current is None: - changed |= self._eligible + entity_changed = execution_mask.clone() + if not entity_changed.any(): + continue + changed |= entity_changed + missing = [] + if previous is None: + missing.append("planned_scene") + if current is None: + missing.append("current_scene") + details.append( + self._scene_dependency_change_detail( + entity_id=entity_id, + monitor_until=monitor_until, + policy=policy, + max_translation=None, + max_rotation=None, + missing=",".join(missing), + ) + ) continue previous_pose = self._batched_entity_pose(previous) current_pose = self._batched_entity_pose(current) @@ -794,10 +2557,55 @@ def _dynamic_scene_change_mask(self, plan: ActionPlan) -> torch.Tensor: (relative_rotation.diagonal(dim1=1, dim2=2).sum(dim=1) - 1.0) / 2.0 ).clamp(-1.0, 1.0) rotation = torch.acos(cosine) - changed |= (translation > policy.goal_translation_threshold) | ( - rotation > policy.goal_rotation_threshold + entity_changed = execution_mask & ( + (translation > policy.goal_translation_threshold) + | (rotation > policy.goal_rotation_threshold) + ) + if not entity_changed.any(): + continue + changed |= entity_changed + details.append( + self._scene_dependency_change_detail( + entity_id=entity_id, + monitor_until=monitor_until, + policy=policy, + max_translation=float(translation[entity_changed].amax().item()), + max_rotation=float(rotation[entity_changed].amax().item()), + missing=None, + ) ) - return changed + if not details: + return changed, None + return ( + changed, + "Scene dependency invalidated the active plan at " + f"waypoint_index={self._waypoint_index}: " + " | ".join(details) + ".", + ) + + @staticmethod + def _scene_dependency_change_detail( + *, + entity_id: str, + monitor_until: int | None, + policy: RecoveryPolicy, + max_translation: float | None, + max_rotation: float | None, + missing: str | None, + ) -> str: + """Return one stable scene-dependency diagnostic fragment.""" + cutoff = "none" if monitor_until is None else str(monitor_until) + translation = ( + "unavailable" if max_translation is None else f"{max_translation:.6f}" + ) + rotation = "unavailable" if max_rotation is None else f"{max_rotation:.6f}" + missing_detail = "" if missing is None else f", missing={missing}" + return ( + f"entity_id={entity_id!r}, monitor_cutoff={cutoff}{missing_detail}, " + f"max_translation={translation}, " + f"translation_threshold={policy.goal_translation_threshold:.6f}, " + f"max_rotation={rotation}, " + f"rotation_threshold={policy.goal_rotation_threshold:.6f}" + ) def _collision_world_change_mask(self, plan: ActionPlan) -> torch.Tensor: """Detect collision revisions newer than the active action plan.""" @@ -827,8 +2635,369 @@ def _batched_entity_pose(self, state: EntityState) -> torch.Tensor: raise ValueError("Scene entity pose batch does not match the session.") return pose + def _phase_effect_gate_requirement( + self, + ) -> PhaseEffectGateRequirement | None: + """Resolve a gate exactly at the next named segment's first frame.""" + 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 + or self._waypoint_index >= self._plan.commands.frame_count + ): + return None + segment = self._plan.segment_at(self._waypoint_index) + if self._waypoint_index != segment.start: + return None + request = self._requests[self._invocation_index] + return next( + ( + value + for value in request.phase_effect_gates + if value.segment_name == segment.name + and value.gate_id not in self._satisfied_phase_effect_gates + ), + None, + ) + + def _phase_effect_gate_request(self) -> PhaseEffectGateRequest | None: + """Build or retain the current blocking segment-entry gate request.""" + requirement = self._phase_effect_gate_requirement() + if requirement is None: + self._pending_phase_effect_gate = None + return None + assert self._plan is not None + env_mask = self._pending & self._plan.plan_success + if not env_mask.any(): + self._pending_phase_effect_gate = None + return None + current = self._pending_phase_effect_gate + if ( + current is not None + and current.gate_id == requirement.gate_id + and current.attempt_generation == self._attempt_generation + and current.next_waypoint_index == self._waypoint_index + and torch.equal(current.env_mask, env_mask) + ): + return current + invocation = self._requests[self._invocation_index] + deadline = self._action_started_at + self._plan.recovery_policy.action_timeout + current = PhaseEffectGateRequest( + verification_id=self._next_phase_effect_gate_verification_id, + gate_id=requirement.gate_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=self._waypoint_index, + segment_name=requirement.segment_name, + requested_at=min(self._context.robot.timestamp, deadline), + deadline=deadline, + env_mask=env_mask, + ) + self._pending_phase_effect_gate = current + return current + + def _phase_effect_gate_required_events( + self, + request: PhaseEffectGateRequest, + ) -> list[ExecutionEvent]: + """Emit the gate boundary once per installed action attempt.""" + if request.gate_id in self._reported_phase_effect_gates: + return [] + self._reported_phase_effect_gates.add(request.gate_id) + return [ + self._event( + ExecutionEventKind.PHASE_EFFECT_GATE_REQUIRED, + request.env_mask, + f"Physical-effect gate {request.gate_id!r} blocks segment " + f"{request.segment_name!r} until current evidence succeeds.", + ) + ] + + def _apply_phase_effect_gate_result( + self, + result: PhaseEffectGateResult, + request: PhaseEffectGateRequest, + ) -> list[ExecutionEvent]: + """Resolve one gate observation without mutating verified task state.""" + success = self._normalize_mask( + result.success_mask, + "phase_effect_gate_result.success_mask", + ) + failure = self._normalize_mask( + result.failure_mask, + "phase_effect_gate_result.failure_mask", + ) + retry = self._normalize_mask( + result.retry_mask, + "phase_effect_gate_result.retry_mask", + ) + request_mask = request.env_mask.to(self._eligible.device) + if ((success | failure | retry) & ~request_mask).any(): + raise ValueError( + "Phase-effect gate result masks must be subsets of the pending " + "request env_mask." + ) + events = self._phase_effect_gate_required_events(request) + message = result.message or ( + f"Physical evidence contradicted gate {request.gate_id!r} before " + f"segment {request.segment_name!r}." + ) + non_retry = failure & ~retry + if non_retry.any(): + self._eligible &= ~non_retry + self._pending &= ~non_retry + self._last_command_mask &= ~non_retry + events.extend( + ( + self._event( + ExecutionEventKind.PHASE_EFFECT_GATE_FAILED, + non_retry, + message, + ), + self._event( + ExecutionEventKind.RECOVERY_REQUIRED, + non_retry, + "The failed segment-entry gate requires recovery outside " + "the current action retry policy.", + ), + ) + ) + previous_generation = self._attempt_generation + if retry.any(): + events.extend( + self._attempt_action_retry( + retry, + ExecutionEventKind.PHASE_EFFECT_GATE_FAILED, + message, + ) + ) + else: + terminal_event = self._update_terminal_status() + if terminal_event is not None: + events.append(terminal_event) + if ( + self._status is not ExecutionStatus.RUNNING + or self._attempt_generation != previous_generation + ): + return events + assert self._plan is not None + remaining = request_mask & self._pending & self._plan.plan_success + if remaining.any() and torch.equal(success & remaining, remaining): + self._satisfied_phase_effect_gates.add(request.gate_id) + events.append( + self._event( + ExecutionEventKind.PHASE_EFFECT_GATE_SATISFIED, + remaining, + f"Physical-effect gate {request.gate_id!r} released segment " + f"{request.segment_name!r}.", + ) + ) + return events + + 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._phase_effect_gate_request() 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): + raise TypeError(f"{name} must be a torch.Tensor.") if value.dtype != torch.bool or value.shape != (self._context.batch_size,): raise ValueError( f"{name} must be bool with shape ({self._context.batch_size},)." @@ -842,18 +3011,90 @@ def _effect_verification_request( """Describe the current action's pending semantic-effect boundary.""" assert self._plan is not None request = self._requests[self._invocation_index] + verification_id = self._next_effect_verification_id + self._next_effect_verification_id += 1 + if self._effect_requested_at is None: + self._effect_requested_at = self._context.robot.timestamp return EffectVerificationRequest( + verification_id=verification_id, skill_id=request.skill_id, invocation_id=request.invocation_id, invocation_revision=request.revision, invocation_index=self._invocation_index, + attempt_generation=self._attempt_generation, terminal_segment=( self._plan.segments[-1].name if self._plan.segments else None ), + requested_at=self._effect_requested_at, + deadline=( + self._action_started_at + self._plan.recovery_policy.action_timeout + ), env_mask=env_mask, expected_effects=self._plan.expected_effects, + effect_verification=self._plan.effect_verification, + failure_invalidation=self._effect_failure_invalidation(), + ) + + def _effect_failure_invalidation(self) -> StateDelta: + """Build the core-owned fail-closed state removal for this effect.""" + assert self._plan is not None + expected = self._plan.expected_effects + held_keys = set(expected.held_object_updates) + coordinated_keys = set(expected.coordinated_held_object_updates) + coordinated_keys.update( + resources + for resources in self._task_state.coordinated_held_objects + if not set(resources).isdisjoint(held_keys) + ) + return StateDelta( + held_object_updates={key: None for key in held_keys}, + coordinated_held_object_updates={ + resources: None for resources in coordinated_keys + }, + articulation_joint_updates={ + key: None for key in expected.articulation_joint_updates + }, + ) + + def _apply_effect_failure_invalidation( + self, + state_invalidation: StateDelta, + env_mask: torch.Tensor, + ) -> None: + """Apply a request-owned failure delta and refresh planning context.""" + if not env_mask.any() or state_invalidation.is_empty: + return + self._task_state = state_invalidation.apply(self._task_state, env_mask) + self._context = PlanningContext( + robot=self._context.robot, + task=self._task_state, + scene=self._context.scene, + env_ids=self._context.env_ids, ) + def _failure_invalidation_presence_mask( + self, + state_invalidation: StateDelta, + ) -> torch.Tensor: + """Return rows whose verified state would actually be removed.""" + present = torch.zeros_like(self._eligible) + for key in state_invalidation.held_object_updates: + value = self._task_state.held_objects.get(key) + if value is not None: + assert value.env_mask is not None + present |= value.env_mask.to(present.device) + for key in state_invalidation.coordinated_held_object_updates: + value = self._task_state.coordinated_held_objects.get(key) + if value is not None: + assert value.env_mask is not None + present |= value.env_mask.to(present.device) + for key in state_invalidation.articulation_joint_updates: + value = self._task_state.articulation_joints.get(key) + if value is not None: + assert value.env_mask is not None + present |= value.env_mask.to(present.device) + return present + def _event( self, kind: ExecutionEventKind, @@ -893,34 +3134,56 @@ def _drain_events(self) -> list[ExecutionEvent]: self._queued_events = [] return events - def _update_terminal_status(self) -> None: - """Mark the session failed when no environment can continue.""" - if not self._eligible.any(): + def _update_terminal_status(self) -> ExecutionEvent | None: + """Mark and report failure when no environment can continue.""" + if not self._eligible.any() and self._status is ExecutionStatus.RUNNING: self._status = ExecutionStatus.FAILED + self._pending_effect = None + self._pending_phase_effect_gate = None + self._effect_failures.zero_() + self._effect_requested_at = None + return self._event( + ExecutionEventKind.SESSION_FAILED, + self._eligible, + "No environment remains eligible for execution.", + ) + return None def _tick_result( self, *, - command: JointCommand | None, + command: RuntimeCommandFrame | None, events: list[ExecutionEvent], + hold_targets: tuple[RuntimeEndpointTarget, ...] = (), ) -> ExecutionTick: """Build an immutable tick result.""" + phase_gate = self._phase_effect_gate_request() + if phase_gate is not None: + events.extend(self._phase_effect_gate_required_events(phase_gate)) return ExecutionTick( status=self._status, eligible_mask=self._eligible, command=command, + hold_targets=hold_targets, events=tuple(events), task_state=self._task_state, pending_effect=self._pending_effect, + pending_phase_effect_gate=phase_gate, ) __all__ = [ + "EffectExpectationResult", "EffectVerificationRequest", + "EffectVerificationResult", "ExecutionEvent", "ExecutionEventKind", + "ExecutionPlanAttempt", "ExecutionSession", "ExecutionStatus", "ExecutionTick", - "JointCommand", + "HeldObjectGuardRequest", + "HeldObjectGuardResult", + "PhaseEffectGateRequest", + "PhaseEffectGateResult", ] diff --git a/embodichain/lab/sim/atomic_actions/goals.py b/embodichain/lab/sim/atomic_actions/goals.py index bdf38811a..3ee3a651d 100644 --- a/embodichain/lab/sim/atomic_actions/goals.py +++ b/embodichain/lab/sim/atomic_actions/goals.py @@ -18,8 +18,10 @@ from __future__ import annotations +import warnings from collections.abc import Mapping, Sequence from dataclasses import dataclass, fields, is_dataclass +import math from typing import Any, ClassVar, Protocol, TYPE_CHECKING import torch @@ -30,13 +32,7 @@ class ActionGoal(Protocol): - """Structural protocol implemented by atomic-action goal value objects. - - Goals are action-owned dataclasses. They do not have to inherit from a - marker base class; the owning action declares its concrete ``GoalType``. - ``goal_kind`` supplies the stable semantic discriminator needed by skill - catalogs and agent-facing schemas. - """ + """Structural protocol implemented by atomic-action goal value objects.""" goal_kind: ClassVar[str] @@ -68,9 +64,142 @@ def __post_init__(self) -> None: "relative_pose", allow_waypoints=False, ) + object.__setattr__(self, "relative_pose", self.relative_pose.clone()) if not 0.0 <= self.minimum_confidence <= 1.0: raise ValueError("minimum_confidence must be in [0, 1].") + def snapshot(self) -> SceneEntityPose: + """Return an independently owned late-bound pose value. + + Returns: + Exact scene reference with an owned relative-pose tensor. + """ + return SceneEntityPose( + self.entity_id, + relative_pose=self.relative_pose, + minimum_confidence=self.minimum_confidence, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class SceneArticulationOperationGeometry: + """Late-bound handle geometry for an articulation operation. + + The offsets and operation axis are immutable grounded affordance data. The + handle itself remains a :class:`SceneEntityPose`, so every atomic plan or + recovery replan resolves it from the latest :class:`SceneSnapshot`. + """ + + handle_pose: SceneEntityPose + approach_offset: torch.Tensor + contact_offset: torch.Tensor + operation_offset: torch.Tensor + retract_offset: torch.Tensor + operation_axis: torch.Tensor + position_scale: float = 1.0 + + def __post_init__(self) -> None: + if not isinstance(self.handle_pose, SceneEntityPose): + raise TypeError("handle_pose must be a SceneEntityPose.") + object.__setattr__(self, "handle_pose", self.handle_pose.snapshot()) + for field_name in ( + "approach_offset", + "contact_offset", + "operation_offset", + "retract_offset", + ): + offset = getattr(self, field_name) + validate_pose_tensor(offset, field_name, allow_waypoints=False) + if offset.shape != (4, 4): + raise ValueError(f"{field_name} must have shape (4, 4).") + if not offset.is_floating_point() or not torch.isfinite(offset).all(): + raise ValueError(f"{field_name} must be a finite floating tensor.") + object.__setattr__(self, field_name, offset.clone()) + axis = self.operation_axis + if ( + not isinstance(axis, torch.Tensor) + or axis.shape != (3,) + or not axis.is_floating_point() + ): + raise ValueError("operation_axis must be a floating tensor of shape (3,).") + if not torch.isfinite(axis).all(): + raise ValueError("operation_axis must contain only finite values.") + norm = torch.linalg.vector_norm(axis) + if float(norm) <= torch.finfo(axis.dtype).eps: + raise ValueError("operation_axis must be non-zero.") + object.__setattr__(self, "operation_axis", (axis / norm).clone()) + scale = self.position_scale + if isinstance(scale, bool) or not isinstance(scale, (int, float)): + raise TypeError("position_scale must be a finite positive scalar.") + scale = float(scale) + if not math.isfinite(scale) or scale <= 0.0: + raise ValueError("position_scale must be a finite positive scalar.") + object.__setattr__(self, "position_scale", scale) + + def resolve( + self, + context: PlanningContext, + *, + displacement: torch.Tensor, + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor]: + """Resolve four poses using a fresh handle and row-local displacement. + + Args: + context: Latest immutable planning observation. + displacement: Remaining signed handle displacement, shape ``(B,)``. + + Returns: + Approach, contact, operation, and retract pose batches. + """ + if not isinstance(displacement, torch.Tensor): + raise TypeError("displacement must be a torch.Tensor.") + if displacement.shape != (context.batch_size,): + raise ValueError("displacement must have one scalar for each planning row.") + if ( + not displacement.is_floating_point() + or not torch.isfinite(displacement).all() + ): + raise ValueError("displacement must be a finite floating tensor.") + handle = resolve_pose_goal( + self.handle_pose, + context, + name="handle_pose", + ) + offsets = tuple( + getattr(self, field_name) + .to(device=handle.device, dtype=handle.dtype) + .unsqueeze(0) + .expand(context.batch_size, -1, -1) + for field_name in ( + "approach_offset", + "contact_offset", + "operation_offset", + "retract_offset", + ) + ) + translation = ( + torch.eye( + 4, + dtype=handle.dtype, + device=handle.device, + ) + .unsqueeze(0) + .repeat(context.batch_size, 1, 1) + ) + axis = self.operation_axis.to(device=handle.device, dtype=handle.dtype) + translation[:, :3, 3] = ( + axis.unsqueeze(0) + * displacement.to(device=handle.device, dtype=handle.dtype).unsqueeze(1) + * self.position_scale + ) + moved_handle = torch.bmm(handle, translation) + return ( + torch.bmm(handle, offsets[0]), + torch.bmm(handle, offsets[1]), + torch.bmm(moved_handle, offsets[2]), + torch.bmm(moved_handle, offsets[3]), + ) + PoseGoalValue = torch.Tensor | SceneEntityPose """Explicit pose tensor or a pose resolved from the latest scene snapshot.""" @@ -97,9 +226,9 @@ def validate_pose_tensor( raise TypeError(f"{name} must be a torch.Tensor, got {type(value).__name__}.") valid_dims = {2, 3, 4} if allow_waypoints else {2, 3} if value.dim() not in valid_dims or value.shape[-2:] != (4, 4): - supported = "(4, 4), (n_envs, 4, 4)" + supported = "(4, 4), (num_envs, 4, 4)" if allow_waypoints: - supported += ", or (n_envs, n_waypoint, 4, 4)" + supported += ", or (num_envs, n_waypoint, 4, 4)" raise ValueError( f"{name} must have shape {supported}, got {tuple(value.shape)}." ) @@ -163,13 +292,55 @@ def resolve_pose_goal( return torch.bmm(pose, relative) +def _resolve_object_pose( + semantics: ObjectSemantics, + context: PlanningContext, + *, + name: str = "object", +) -> torch.Tensor: + """Resolve an object's pose from a snapshot or the deprecated live handle.""" + from .core import ObjectSemantics + + if not isinstance(semantics, ObjectSemantics): + raise TypeError("semantics must be an ObjectSemantics instance.") + if semantics.entity_id is not None: + return resolve_pose_goal( + SceneEntityPose(semantics.entity_id), + context, + name=name, + ) + if semantics.entity is None: + raise ValueError( + f"{name} requires ObjectSemantics.entity_id or a legacy entity handle." + ) + warnings.warn( + "Live pose grounding through ObjectSemantics.entity is deprecated; " + "set entity_id and provide the entity through PlanningContext.scene.", + DeprecationWarning, + stacklevel=2, + ) + pose = semantics.entity.get_local_pose(to_matrix=True) + if not isinstance(pose, torch.Tensor): + raise TypeError(f"{name} legacy entity pose must be a torch.Tensor.") + pose = pose.to(device=context.robot.qpos.device, dtype=torch.float32) + if pose.shape == (4, 4): + pose = pose.unsqueeze(0).expand(context.batch_size, -1, -1) + elif pose.shape != (context.batch_size, 4, 4): + raise ValueError(f"{name} legacy entity pose must match planning batch size.") + return pose.clone() + + def collect_scene_dependencies(value: Any) -> tuple[str, ...]: """Collect stable scene entity identifiers referenced by a goal value.""" + from .core import ObjectSemantics + found: set[str] = set() def visit(item: Any) -> None: if isinstance(item, SceneEntityPose): found.add(item.entity_id) + elif isinstance(item, ObjectSemantics): + return elif is_dataclass(item) and not isinstance(item, type): for data_field in fields(item): visit(getattr(item, data_field.name)) @@ -191,8 +362,6 @@ def visit(item: Any) -> None: class ObjectActionGoal: """Shared semantic-object goal contract for object-centric skills.""" - goal_kind: ClassVar[str] = "semantic_object" - semantics: ObjectSemantics """Semantic and geometric description of the object.""" @@ -207,6 +376,7 @@ def __post_init__(self) -> None: "ActionGoal", "ObjectActionGoal", "PoseGoalValue", + "SceneArticulationOperationGeometry", "SceneEntityPose", "collect_scene_dependencies", "resolve_pose_goal", diff --git a/embodichain/lab/sim/atomic_actions/invocation.py b/embodichain/lab/sim/atomic_actions/invocation.py index b47795612..31a038d7c 100644 --- a/embodichain/lab/sim/atomic_actions/invocation.py +++ b/embodichain/lab/sim/atomic_actions/invocation.py @@ -25,12 +25,12 @@ from embodichain.lab.sim.common import BatchEntity -from .bindings import ActionBinding, ResolvedActionBinding +from .bindings import ActionBinding from .control import ActionControlOverrides -from .goals import ActionGoal from .policies import MotionPolicy, RecoveryPolicy +from .tracking import TrackingPolicy -GoalT = TypeVar("GoalT", bound=ActionGoal) +GoalT = TypeVar("GoalT") @dataclass(frozen=True, slots=True, eq=False) @@ -46,6 +46,38 @@ class ActionOptions: OptionsT = TypeVar("OptionsT", bound=ActionOptions) +@dataclass(frozen=True, slots=True) +class PhaseEffectGateRequirement: + """Require physical-effect evidence before one trajectory segment starts. + + The requirement carries only stable core correlation data. Semantic + integrations own the corresponding observation specification and monitor; + the execution session owns blocking, timeout, and action-retry behavior. + + Args: + gate_id: Invocation-local stable gate identifier. + segment_name: Exact named trajectory segment blocked by this gate. + """ + + gate_id: str + segment_name: str + + def __post_init__(self) -> None: + for name in ("gate_id", "segment_name"): + value = getattr(self, name) + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{name} must be a non-empty string without outer whitespace." + ) + + def snapshot(self) -> PhaseEffectGateRequirement: + """Return an independently constructed immutable requirement.""" + return PhaseEffectGateRequirement( + gate_id=self.gate_id, + segment_name=self.segment_name, + ) + + def _goal_snapshot_memo(goal: ActionGoal) -> dict[int, object]: """Return deepcopy memo entries for live goal references and runtime caches.""" memo: dict[int, object] = {} @@ -82,7 +114,7 @@ def visit(value: object) -> None: @dataclass(frozen=True, slots=True) class ActionInvocation(Generic[GoalT, OptionsT]): - """One fully typed and embodiment-bound atomic skill request. + """One fully typed and endpoint-bound atomic skill request. This is a runtime-domain object, not the JSON protocol emitted by an MLLM. An action compiler is responsible for converting a semantic ``SkillCallSpec`` @@ -96,14 +128,22 @@ class ActionInvocation(Generic[GoalT, OptionsT]): """Action-specific goal value object.""" binding: ActionBinding - """Semantic-role bindings to keys in the selected robot's control parts.""" + """Generic skill endpoint bindings owned by the selected engine.""" motion_policy: MotionPolicy = field(default_factory=MotionPolicy) """Reusable motion-generation settings.""" + tracking_policy: TrackingPolicy = field( + default_factory=TrackingPolicy.joint_position + ) + """Typed in-flight tracking and terminal-acceptance settings.""" + recovery_policy: RecoveryPolicy = field(default_factory=RecoveryPolicy) """Bounded local execution recovery settings.""" + phase_effect_gates: tuple[PhaseEffectGateRequirement, ...] = () + """Physical-effect gates enforced at named trajectory-segment entries.""" + skill_options: OptionsT | None = None """Optional per-invocation behavior override for the selected skill.""" @@ -121,18 +161,30 @@ class ActionInvocation(Generic[GoalT, OptionsT]): def __post_init__(self) -> None: if not isinstance(self.skill_id, str) or not self.skill_id.strip(): raise ValueError("skill_id must be a non-empty string.") - goal_kind = getattr(type(self.goal), "goal_kind", None) - if not isinstance(goal_kind, str) or not goal_kind: - raise TypeError( - "goal must implement the ActionGoal protocol with a non-empty " - "goal_kind class variable." - ) if not isinstance(self.binding, ActionBinding): raise TypeError("binding must be an ActionBinding.") if not isinstance(self.motion_policy, MotionPolicy): raise TypeError("motion_policy must be a MotionPolicy.") + if not isinstance(self.tracking_policy, TrackingPolicy): + raise TypeError("tracking_policy must be a TrackingPolicy.") if not isinstance(self.recovery_policy, RecoveryPolicy): raise TypeError("recovery_policy must be a RecoveryPolicy.") + phase_effect_gates = tuple(self.phase_effect_gates) + if not all( + type(value) is PhaseEffectGateRequirement for value in phase_effect_gates + ): + raise TypeError( + "phase_effect_gates must contain exact " + "PhaseEffectGateRequirement values." + ) + gate_ids = [value.gate_id for value in phase_effect_gates] + segment_names = [value.segment_name for value in phase_effect_gates] + if len(set(gate_ids)) != len(gate_ids): + raise ValueError("Phase-effect gate IDs must be unique per invocation.") + if len(set(segment_names)) != len(segment_names): + raise ValueError( + "At most one phase-effect gate may block each trajectory segment." + ) if self.skill_options is not None and not isinstance( self.skill_options, ActionOptions ): @@ -145,6 +197,11 @@ def __post_init__(self) -> None: raise ValueError("invocation_id must be a non-empty string when set.") if not isinstance(self.revision, int) or self.revision < 0: raise ValueError("revision must be a non-negative integer.") + object.__setattr__( + self, + "phase_effect_gates", + tuple(value.snapshot() for value in phase_effect_gates), + ) @dataclass(frozen=True, slots=True) @@ -159,22 +216,42 @@ class ResolvedActionRequest(Generic[GoalT, OptionsT]): skill_id: str goal: GoalT - binding: ResolvedActionBinding + binding: ActionBinding motion_policy: MotionPolicy + tracking_policy: TrackingPolicy recovery_policy: RecoveryPolicy skill_options: OptionsT + phase_effect_gates: tuple[PhaseEffectGateRequirement, ...] = () invocation_id: str | None = None revision: int = 0 def __post_init__(self) -> None: if not isinstance(self.skill_id, str) or not self.skill_id.strip(): raise ValueError("skill_id must be a non-empty string.") - if not isinstance(self.binding, ResolvedActionBinding): - raise TypeError("binding must be a ResolvedActionBinding.") + if not isinstance(self.binding, ActionBinding): + raise TypeError("binding must be an ActionBinding.") if not isinstance(self.motion_policy, MotionPolicy): raise TypeError("motion_policy must be a MotionPolicy.") + if not isinstance(self.tracking_policy, TrackingPolicy): + raise TypeError("tracking_policy must be a TrackingPolicy.") if not isinstance(self.recovery_policy, RecoveryPolicy): raise TypeError("recovery_policy must be a RecoveryPolicy.") + phase_effect_gates = tuple(self.phase_effect_gates) + if not all( + type(value) is PhaseEffectGateRequirement for value in phase_effect_gates + ): + raise TypeError( + "phase_effect_gates must contain exact " + "PhaseEffectGateRequirement values." + ) + gate_ids = [value.gate_id for value in phase_effect_gates] + segment_names = [value.segment_name for value in phase_effect_gates] + if len(set(gate_ids)) != len(gate_ids): + raise ValueError("Phase-effect gate IDs must be unique per request.") + if len(set(segment_names)) != len(segment_names): + raise ValueError( + "At most one phase-effect gate may block each trajectory segment." + ) if not isinstance(self.skill_options, ActionOptions): raise TypeError("skill_options must be an ActionOptions instance.") if self.invocation_id is not None and ( @@ -188,15 +265,45 @@ def __post_init__(self) -> None: "goal", deepcopy(self.goal, _goal_snapshot_memo(self.goal)), ) + object.__setattr__( + self, + "binding", + ActionBinding( + owner_id=self.binding.owner_id, + endpoints=self.binding.endpoints, + ), + ) object.__setattr__(self, "motion_policy", deepcopy(self.motion_policy)) + object.__setattr__(self, "tracking_policy", deepcopy(self.tracking_policy)) object.__setattr__(self, "recovery_policy", deepcopy(self.recovery_policy)) + object.__setattr__( + self, + "phase_effect_gates", + tuple(value.snapshot() for value in phase_effect_gates), + ) object.__setattr__(self, "skill_options", deepcopy(self.skill_options)) + def snapshot(self) -> ResolvedActionRequest[GoalT, OptionsT]: + """Return an independently owned resolved-request snapshot.""" + return ResolvedActionRequest( + skill_id=self.skill_id, + goal=self.goal, + binding=self.binding, + motion_policy=self.motion_policy, + tracking_policy=self.tracking_policy, + recovery_policy=self.recovery_policy, + phase_effect_gates=self.phase_effect_gates, + skill_options=self.skill_options, + invocation_id=self.invocation_id, + revision=self.revision, + ) + __all__ = [ "ActionInvocation", "ActionOptions", "GoalT", "OptionsT", + "PhaseEffectGateRequirement", "ResolvedActionRequest", ] diff --git a/embodichain/lab/sim/atomic_actions/plans.py b/embodichain/lab/sim/atomic_actions/plans.py index 356a0d6f6..4bcdd6815 100644 --- a/embodichain/lab/sim/atomic_actions/plans.py +++ b/embodichain/lab/sim/atomic_actions/plans.py @@ -18,7 +18,9 @@ from __future__ import annotations +from copy import deepcopy from dataclasses import dataclass, field +import math from types import MappingProxyType from typing import Any, Mapping, Sequence @@ -28,7 +30,13 @@ from .effects import StateDelta from .policies import RecoveryPolicy +from .runtime_commands import TimedCommandSequence from .state import PlanningContext +from .tracking import ( + FeedbackTerminalAcceptance, + TimedTrackingSequence, + TrackingPolicy, +) def _validate_optional_trajectory_field( @@ -58,7 +66,6 @@ class TimedTrajectory: accelerations: torch.Tensor | None dt: torch.Tensor """Per-waypoint arrival intervals; the first sample normally has zero dt.""" - duration: torch.Tensor env_ids: torch.Tensor def __post_init__(self) -> None: @@ -88,28 +95,31 @@ def __post_init__(self) -> None: raise ValueError("dt must share the positions device.") if not torch.isfinite(self.dt).all() or (self.dt < 0).any(): raise ValueError("dt must contain finite non-negative values.") - if not isinstance(self.duration, torch.Tensor) or self.duration.shape != ( - batch_size, - ): - raise ValueError(f"duration must have shape ({batch_size},).") - if self.duration.device != self.positions.device: - raise ValueError("duration must share the positions device.") - if not torch.isfinite(self.duration).all() or (self.duration < 0).any(): - raise ValueError("duration must contain finite non-negative values.") - if not torch.allclose( - self.duration, - self.dt.sum(dim=1), - rtol=1e-4, - atol=1e-6, - ): - raise ValueError("duration must equal the sum of dt for each environment.") if not isinstance(self.env_ids, torch.Tensor): raise TypeError("env_ids must be a torch.Tensor.") if self.env_ids.dtype != torch.long or self.env_ids.shape != (batch_size,): raise ValueError(f"env_ids must be int64 with shape ({batch_size},).") if self.env_ids.device != self.positions.device: raise ValueError("env_ids must share the positions device.") - object.__setattr__(self, "env_ids", self.env_ids.clone()) + if torch.unique(self.env_ids).numel() != batch_size: + raise ValueError("env_ids must contain unique values.") + object.__setattr__(self, "positions", self.positions.detach().clone()) + object.__setattr__( + self, + "velocities", + None if self.velocities is None else self.velocities.detach().clone(), + ) + object.__setattr__( + self, + "accelerations", + ( + None + if self.accelerations is None + else self.accelerations.detach().clone() + ), + ) + object.__setattr__(self, "dt", self.dt.detach().clone()) + object.__setattr__(self, "env_ids", self.env_ids.detach().clone()) @property def batch_size(self) -> int: @@ -126,6 +136,11 @@ def robot_dof(self) -> int: """Number of full-robot command columns.""" return int(self.positions.shape[2]) + @property + def duration(self) -> torch.Tensor: + """Per-environment trajectory duration derived from waypoint intervals.""" + return self.dt.sum(dim=1) + def snapshot(self) -> TimedTrajectory: """Return an independently owned copy of this trajectory. @@ -140,7 +155,6 @@ def snapshot(self) -> TimedTrajectory: None if self.accelerations is None else self.accelerations.clone() ), dt=self.dt.clone(), - duration=self.duration.clone(), env_ids=self.env_ids.clone(), ) @@ -150,73 +164,82 @@ def from_positions( positions: torch.Tensor, *, env_ids: torch.Tensor, - control_dt: float, + dt: torch.Tensor, velocities: torch.Tensor | None = None, accelerations: torch.Tensor | None = None, - dt: torch.Tensor | None = None, - duration: torch.Tensor | float | None = None, ) -> TimedTrajectory: - """Build a timed trajectory and synthesize missing timing metadata. + """Build a trajectory from positions and explicit per-sample timing. Args: positions: Full-robot positions, shape ``(B, N, D)``. env_ids: Environment identifiers, shape ``(B,)``. - control_dt: Fallback interval used when ``dt`` is absent. + dt: Per-sample arrival intervals, shape ``(B, N)``. velocities: Optional joint velocities. accelerations: Optional joint accelerations. - dt: Optional per-sample time deltas. - duration: Optional duration used to synthesize or validate ``dt``. Returns: Validated timed trajectory. """ - if control_dt <= 0.0: - raise ValueError("control_dt must be greater than zero.") if not isinstance(positions, torch.Tensor) or positions.dim() != 3: raise ValueError("positions must have shape (B, N, D).") - batch_size, waypoint_count, _ = positions.shape - if dt is None: - dt = torch.zeros( - (batch_size, waypoint_count), - dtype=torch.float32, - device=positions.device, - ) - if waypoint_count > 1: - if duration is None: - dt[:, 1:] = control_dt - else: - duration_tensor = torch.as_tensor( - duration, dtype=torch.float32, device=positions.device - ) - if duration_tensor.dim() == 0: - duration_tensor = duration_tensor.expand(batch_size) - if duration_tensor.shape != (batch_size,): - raise ValueError(f"duration must have shape ({batch_size},).") - dt[:, 1:] = duration_tensor[:, None] / (waypoint_count - 1) - else: - dt = dt.to(device=positions.device, dtype=torch.float32) - computed_duration = dt.sum(dim=1) - if duration is not None: - duration_tensor = torch.as_tensor( - duration, dtype=torch.float32, device=positions.device - ) - if duration_tensor.dim() == 0: - duration_tensor = duration_tensor.expand(batch_size) - if duration_tensor.shape != (batch_size,): - raise ValueError(f"duration must have shape ({batch_size},).") - if not torch.allclose( - computed_duration, duration_tensor, rtol=1e-4, atol=1e-6 - ): - raise ValueError("duration does not match the supplied dt.") + if not isinstance(dt, torch.Tensor): + raise TypeError("dt must be a torch.Tensor.") return cls( positions=positions, velocities=velocities, accelerations=accelerations, - dt=dt, - duration=computed_duration, + dt=dt.to(device=positions.device, dtype=torch.float32), env_ids=env_ids, ) + @classmethod + def from_uniform_step( + cls, + positions: torch.Tensor, + *, + env_ids: torch.Tensor, + step_dt: float, + velocities: torch.Tensor | None = None, + accelerations: torch.Tensor | None = None, + ) -> TimedTrajectory: + """Build an explicitly uniform-time trajectory. + + The first waypoint has zero arrival time; every following waypoint uses + ``step_dt``. This factory is intended for interpolation algorithms whose + cadence is selected by the caller, not for repairing untimed plans. + + Args: + positions: Full-robot positions, shape ``(B, N, D)``. + env_ids: Environment identifiers, shape ``(B,)``. + step_dt: Explicit interval between consecutive waypoints. + velocities: Optional joint velocities. + accelerations: Optional joint accelerations. + + Returns: + Validated uniformly timed trajectory. + """ + if isinstance(step_dt, bool) or not isinstance(step_dt, (int, float)): + raise TypeError("step_dt must be a real number.") + if not math.isfinite(step_dt) or step_dt <= 0.0: + raise ValueError("step_dt must be finite and greater than zero.") + if not isinstance(positions, torch.Tensor) or positions.dim() != 3: + raise ValueError("positions must have shape (B, N, D).") + batch_size, waypoint_count, _ = positions.shape + dt = torch.zeros( + (batch_size, waypoint_count), + dtype=torch.float32, + device=positions.device, + ) + if waypoint_count > 1: + dt[:, 1:] = float(step_dt) + return cls.from_positions( + positions, + env_ids=env_ids, + dt=dt, + velocities=velocities, + accelerations=accelerations, + ) + @classmethod def empty( cls, @@ -235,7 +258,6 @@ def empty( velocities=None, accelerations=None, dt=torch.empty((batch_size, 0), dtype=torch.float32, device=resolved), - duration=torch.zeros(batch_size, dtype=torch.float32, device=resolved), env_ids=env_ids, ) @@ -280,7 +302,6 @@ def mask_derivative(value: torch.Tensor | None) -> torch.Tensor | None: velocities=mask_derivative(self.velocities), accelerations=mask_derivative(self.accelerations), dt=self.dt, - duration=self.duration, env_ids=self.env_ids, ) @@ -332,7 +353,6 @@ def concatenate_optional(name: str) -> torch.Tensor | None: velocities=concatenate_optional("velocities"), accelerations=concatenate_optional("accelerations"), dt=dt, - duration=dt.sum(dim=1), env_ids=first.env_ids, ) @@ -348,8 +368,47 @@ class PlannerDiagnostics: def __post_init__(self) -> None: if not isinstance(self.backend, str) or not self.backend: raise ValueError("PlannerDiagnostics.backend must be non-empty.") - object.__setattr__(self, "messages", tuple(self.messages)) - object.__setattr__(self, "metadata", MappingProxyType(dict(self.metadata))) + if not isinstance(self.metadata, Mapping): + raise TypeError("PlannerDiagnostics.metadata must be a mapping.") + messages = tuple(self.messages) + if not all(type(message) is str for message in messages): + raise TypeError("PlannerDiagnostics.messages must contain strings.") + object.__setattr__(self, "messages", messages) + object.__setattr__( + self, + "metadata", + MappingProxyType(deepcopy(dict(self.metadata))), + ) + + +@dataclass(frozen=True, slots=True) +class EffectVerificationRequirement: + """Explicit physical-effect verification independent of symbolic state. + + Presence of this value on an :class:`ActionPlan` forces a terminal effect + boundary even when the plan declares no :class:`StateDelta`. The open + ``kind`` identifier lets an external runtime select an appropriate + verifier without placing backend-specific callbacks in the core plan. + + Args: + kind: Stable, non-empty discriminator for the physical effect. + """ + + kind: str + + def __post_init__(self) -> None: + if ( + type(self.kind) is not str + or not self.kind + or self.kind != self.kind.strip() + ): + raise ValueError( + "kind must be a non-empty string without outer whitespace." + ) + + def snapshot(self) -> EffectVerificationRequirement: + """Return an independently owned requirement value.""" + return EffectVerificationRequirement(kind=self.kind) @dataclass(frozen=True, slots=True) @@ -391,23 +450,37 @@ def contains(self, waypoint_index: int) -> bool: class ActionPlan: """Scene-bound planning result for one grounded atomic action invocation. - An action owns one trajectory and one recovery boundary. Named + An action owns one timed command sequence and one recovery boundary. Named :class:`TrajectorySegment` values describe semantic structure within that - trajectory without implying independent planning or recovery boundaries. + sequence without implying independent planning or recovery boundaries. + + Attributes: + scene_dependency_monitor_until: Optional exclusive waypoint-index upper + bounds for individual ``scene_dependencies``. An entity is monitored + while the current waypoint index is smaller than its bound; ``0`` + disables monitoring immediately, while an omitted entity remains + monitored for the action's full execution. Once the bound is reached, + all pose changes for that entity are ignored, regardless of whether + they were caused by the action or by an external disturbance. """ skill_id: str plan_success: torch.Tensor - trajectory: TimedTrajectory + commands: TimedCommandSequence recovery_policy: RecoveryPolicy + tracking_policy: TrackingPolicy planned_scene_version: int planned_collision_world_revision: tuple[int, ...] diagnostics: PlannerDiagnostics + tracking: TimedTrackingSequence | None = None + joint_trajectory: TimedTrajectory | None = None segments: tuple[TrajectorySegment, ...] = () scene_dependencies: tuple[str, ...] = () + scene_dependency_monitor_until: Mapping[str, int] = field(default_factory=dict) collision_world_sensitive: bool = False replannable: bool = True expected_effects: StateDelta = field(default_factory=StateDelta) + effect_verification: EffectVerificationRequirement | None = None invocation_id: str | None = None invocation_revision: int = 0 @@ -423,21 +496,166 @@ def __post_init__(self) -> None: raise TypeError("plan_success must be a torch.Tensor.") if self.plan_success.dtype != torch.bool or self.plan_success.dim() != 1: raise ValueError("plan_success must be a 1D bool tensor.") - if not isinstance(self.trajectory, TimedTrajectory): - raise TypeError("trajectory must be a TimedTrajectory.") - if self.trajectory.batch_size != self.plan_success.shape[0]: - raise ValueError("plan_success batch must match the trajectory.") - if self.trajectory.positions.device != self.plan_success.device: - raise ValueError("plan_success and trajectory must share a device.") + if not isinstance(self.commands, TimedCommandSequence): + raise TypeError("commands must be a TimedCommandSequence.") + if self.commands.batch_size != self.plan_success.shape[0]: + raise ValueError("plan_success batch must match the command sequence.") + if self.commands.device != self.plan_success.device: + raise ValueError("plan_success and commands must share a device.") + if not isinstance(self.tracking_policy, TrackingPolicy): + raise TypeError("tracking_policy must be a TrackingPolicy.") + expected_target_types: dict[tuple[str, str], type[object]] | None = None + expected_target_fingerprints: dict[tuple[str, str], object] | None = None + for frame_index, frame in enumerate(self.commands.frames): + target_types = { + command.destination_key: type(command.target) + for command in frame.commands + } + target_fingerprints = { + command.destination_key: command.target.address_fingerprint + for command in frame.commands + } + if expected_target_types is None: + expected_target_types = target_types + expected_target_fingerprints = target_fingerprints + continue + if target_types.keys() != expected_target_types.keys(): + raise ValueError( + "ActionPlan command frames must preserve the same destination " + f"set; frame {frame_index} differs from frame 0." + ) + mismatched_types = sorted( + destination + for destination, target_type in target_types.items() + if target_type is not expected_target_types[destination] + ) + if mismatched_types: + raise ValueError( + "ActionPlan command frames must preserve the exact target type " + f"for each destination; frame {frame_index} differs at " + f"{mismatched_types}." + ) + assert expected_target_fingerprints is not None + mismatched_fingerprints = sorted( + destination + for destination, fingerprint in target_fingerprints.items() + if fingerprint != expected_target_fingerprints[destination] + ) + if mismatched_fingerprints: + raise ValueError( + "ActionPlan command frames must preserve the target address " + f"fingerprint for each destination; frame {frame_index} " + f"differs at {mismatched_fingerprints}." + ) + if self.joint_trajectory is not None: + if not isinstance(self.joint_trajectory, TimedTrajectory): + raise TypeError("joint_trajectory must be a TimedTrajectory or None.") + if self.joint_trajectory.batch_size != self.commands.batch_size: + raise ValueError( + "joint_trajectory batch must match the command sequence." + ) + if self.joint_trajectory.waypoint_count != self.commands.frame_count: + raise ValueError( + "joint_trajectory waypoints must match command sequence frames." + ) + if not torch.equal(self.joint_trajectory.env_ids, self.commands.env_ids): + raise ValueError( + "joint_trajectory env_ids must match the command sequence." + ) + if self.joint_trajectory.positions.device != self.commands.device: + raise ValueError("joint_trajectory and commands must share a device.") + required_channels = { + metric.channel_id + for metric in ( + () + if self.tracking_policy.in_flight is None + else self.tracking_policy.in_flight.metrics + ) + } + if isinstance( + self.tracking_policy.terminal, + FeedbackTerminalAcceptance, + ): + required_channels.update( + metric.channel_id for metric in self.tracking_policy.terminal.metrics + ) + if self.tracking is None: + if required_channels: + raise ValueError( + "Feedback tracking policies require an owned tracking sequence." + ) + else: + if not isinstance(self.tracking, TimedTrackingSequence): + raise TypeError("tracking must be a TimedTrackingSequence or None.") + if self.tracking.batch_size != self.commands.batch_size: + raise ValueError("tracking batch must match the command sequence.") + if self.tracking.frame_count != self.commands.frame_count: + raise ValueError("tracking frames must match command sequence frames.") + if not torch.equal(self.tracking.env_ids, self.commands.env_ids): + raise ValueError("tracking env_ids must match the command sequence.") + if self.tracking.device != self.commands.device: + raise ValueError("tracking and commands must share a device.") + if not required_channels: + raise ValueError( + "A tracking sequence requires an in-flight or terminal " + "feedback metric." + ) + if bool(self.plan_success.any().item()) and not self.tracking.frames: + raise ValueError( + "Feedback tracking requires command frames when any " + "environment planned successfully." + ) + expected_setpoint_keys: set[tuple[str, str, str]] | None = None + expected_setpoint_routes: ( + dict[ + tuple[str, str, str], + tuple[object, str, str], + ] + | None + ) = None + for frame_index, frame in enumerate(self.tracking.frames): + frame_keys = {setpoint.key for setpoint in frame.setpoints} + frame_routes = { + setpoint.key: ( + setpoint.binding.source.source_fingerprint, + setpoint.binding.projector.projector_id, + setpoint.binding.projector.revision, + ) + for setpoint in frame.setpoints + } + frame_channels = { + setpoint.binding.channel_id for setpoint in frame.setpoints + } + if frame_channels != required_channels: + raise ValueError( + "Every tracking frame must cover exactly the configured " + f"feedback channels; frame {frame_index} has " + f"{sorted(frame_channels)}, expected " + f"{sorted(required_channels)}." + ) + if expected_setpoint_keys is None: + expected_setpoint_keys = frame_keys + expected_setpoint_routes = frame_routes + elif frame_keys != expected_setpoint_keys: + raise ValueError( + "Tracking frames must preserve the same endpoint/channel " + f"set; frame {frame_index} differs from frame 0." + ) + elif frame_routes != expected_setpoint_routes: + raise ValueError( + "Tracking frames must preserve each endpoint/channel " + "source fingerprint and projector route; " + f"frame {frame_index} differs from frame 0." + ) if not isinstance(self.recovery_policy, RecoveryPolicy): raise TypeError("recovery_policy must be a RecoveryPolicy.") if self.planned_scene_version < 0: raise ValueError("planned_scene_version must be non-negative.") revisions = tuple(self.planned_collision_world_revision) - if len(revisions) != self.trajectory.batch_size: + if len(revisions) != self.commands.batch_size: raise ValueError( "planned_collision_world_revision must contain one value per " - "trajectory environment." + "command-sequence environment." ) if any( isinstance(value, bool) or not isinstance(value, int) or value < 0 @@ -456,13 +674,37 @@ def __post_init__(self) -> None: raise ValueError( "scene_dependencies must contain unique non-empty entity ids." ) + waypoint_count = self.commands.frame_count + monitor_until = dict(self.scene_dependency_monitor_until) + if not set(monitor_until).issubset(dependencies): + raise ValueError( + "scene_dependency_monitor_until keys must be scene dependencies." + ) + for entity_id, waypoint_index in monitor_until.items(): + if ( + type(entity_id) is not str + or not entity_id + or type(waypoint_index) is not int + or not 0 <= waypoint_index <= waypoint_count + ): + raise ValueError( + "scene_dependency_monitor_until must map non-empty entity IDs " + "to waypoint indices within the command sequence." + ) if not isinstance(self.collision_world_sensitive, bool): raise TypeError("collision_world_sensitive must be a bool.") if not isinstance(self.replannable, bool): raise TypeError("replannable must be a bool.") if not isinstance(self.expected_effects, StateDelta): raise TypeError("expected_effects must be a StateDelta.") - waypoint_count = self.trajectory.waypoint_count + if ( + self.effect_verification is not None + and type(self.effect_verification) is not EffectVerificationRequirement + ): + raise TypeError( + "effect_verification must be exactly " + "EffectVerificationRequirement or None." + ) segments = tuple(self.segments) if not all(isinstance(segment, TrajectorySegment) for segment in segments): raise TypeError("segments must contain only TrajectorySegment values.") @@ -473,7 +715,7 @@ def __post_init__(self) -> None: raise ValueError("ActionPlan segment names must be unique.") if waypoint_count == 0: if segments: - raise ValueError("An empty trajectory cannot contain segments.") + raise ValueError("An empty command sequence cannot contain segments.") elif ( not segments or segments[0].start != 0 @@ -484,19 +726,103 @@ def __post_init__(self) -> None: ) ): raise ValueError( - "ActionPlan segments must cover the trajectory exactly without " + "ActionPlan segments must cover the command sequence exactly without " "gaps or overlaps." ) object.__setattr__(self, "plan_success", self.plan_success.clone()) + object.__setattr__(self, "commands", self.commands.snapshot()) + object.__setattr__( + self, + "tracking_policy", + self.tracking_policy.snapshot(), + ) + object.__setattr__( + self, + "tracking", + None if self.tracking is None else self.tracking.snapshot(), + ) + object.__setattr__( + self, + "joint_trajectory", + ( + None + if self.joint_trajectory is None + else self.joint_trajectory.snapshot() + ), + ) object.__setattr__(self, "planned_collision_world_revision", revisions) + object.__setattr__( + self, + "diagnostics", + PlannerDiagnostics( + backend=self.diagnostics.backend, + messages=self.diagnostics.messages, + metadata=self.diagnostics.metadata, + ), + ) object.__setattr__(self, "scene_dependencies", dependencies) + object.__setattr__( + self, + "scene_dependency_monitor_until", + MappingProxyType(monitor_until), + ) object.__setattr__(self, "segments", segments) + object.__setattr__( + self, + "effect_verification", + ( + None + if self.effect_verification is None + else self.effect_verification.snapshot() + ), + ) @property def success_all(self) -> bool: """Whether every environment row planned successfully.""" return bool(self.plan_success.all().item()) + def snapshot(self) -> ActionPlan: + """Return an independently owned inspection snapshot of this plan. + + Runtime tracing and visualization need access to the exact plan that + reached an execution boundary without being able to mutate the live + session. Reconstructing the value through the public constructor also + re-applies every plan invariant and snapshots all tensor-owning nested + contracts. + + Returns: + A validated plan with independently owned tensor storage. + """ + return ActionPlan( + skill_id=self.skill_id, + plan_success=self.plan_success, + commands=self.commands, + recovery_policy=self.recovery_policy, + tracking_policy=self.tracking_policy, + planned_scene_version=self.planned_scene_version, + planned_collision_world_revision=self.planned_collision_world_revision, + diagnostics=self.diagnostics, + tracking=self.tracking, + joint_trajectory=self.joint_trajectory, + segments=self.segments, + scene_dependencies=self.scene_dependencies, + scene_dependency_monitor_until=self.scene_dependency_monitor_until, + collision_world_sensitive=self.collision_world_sensitive, + replannable=self.replannable, + expected_effects=self.expected_effects, + effect_verification=self.effect_verification, + invocation_id=self.invocation_id, + invocation_revision=self.invocation_revision, + ) + + @property + def requires_effect_verification(self) -> bool: + """Whether execution must verify a terminal physical effect.""" + return ( + self.effect_verification is not None or not self.expected_effects.is_empty + ) + def segment(self, name: str) -> TrajectorySegment: """Return a named trajectory segment. @@ -516,9 +842,10 @@ def segment(self, name: str) -> TrajectorySegment: def segment_at(self, waypoint_index: int) -> TrajectorySegment: """Return the segment containing a global action waypoint index.""" - if waypoint_index < 0 or waypoint_index >= self.trajectory.waypoint_count: + if waypoint_index < 0 or waypoint_index >= self.commands.frame_count: raise IndexError( - f"waypoint_index {waypoint_index} is outside the action trajectory." + f"waypoint_index {waypoint_index} is outside the action command " + "sequence." ) for segment in self.segments: if segment.contains(waypoint_index): @@ -554,7 +881,12 @@ def action_waypoint_offset(self, action_index: int) -> int: f"action_index {action_index} is outside the compiled sequence." ) return sum( - plan.trajectory.waypoint_count for plan in self.action_plans[:action_index] + ( + 0 + if plan.joint_trajectory is None + else plan.joint_trajectory.waypoint_count + ) + for plan in self.action_plans[:action_index] ) def segment(self, action_index: int, name: str) -> TrajectorySegment: @@ -571,6 +903,7 @@ def segment(self, action_index: int, name: str) -> TrajectorySegment: __all__ = [ "ActionPlan", "CompiledTrajectory", + "EffectVerificationRequirement", "PlannerDiagnostics", "TimedTrajectory", "TrajectorySegment", diff --git a/embodichain/lab/sim/atomic_actions/policies.py b/embodichain/lab/sim/atomic_actions/policies.py index 8f82b49a6..668f1669e 100644 --- a/embodichain/lab/sim/atomic_actions/policies.py +++ b/embodichain/lab/sim/atomic_actions/policies.py @@ -56,24 +56,12 @@ class MotionPolicy: cannot change an invocation after it has been created. """ - planner: str | None = None - """Optional required planner backend name; ``None`` accepts the configured one.""" - strategy: Literal["motion_gen", "ik_interp"] = "ik_interp" """Motion strategy: ``motion_gen`` or ``ik_interp``.""" sample_count: int = 50 """Requested trajectory sample count when the backend does not preserve samples.""" - control_dt: float = 1.0 / 60.0 - """Fallback command period in seconds when a planner supplies no timing.""" - - velocity_limit: float | None = None - """Optional planner velocity limit.""" - - acceleration_limit: float | None = None - """Optional planner acceleration limit.""" - dynamic_collision_mode: DynamicCollisionMode = DynamicCollisionMode.AUTO """How this invocation consumes live scene-snapshot collision entities.""" @@ -89,12 +77,6 @@ def __post_init__(self) -> None: ) if self.sample_count < 2: raise ValueError("sample_count must be at least 2.") - if self.control_dt <= 0.0: - raise ValueError("control_dt must be greater than zero.") - if self.velocity_limit is not None and self.velocity_limit <= 0.0: - raise ValueError("velocity_limit must be greater than zero when set.") - if self.acceleration_limit is not None and self.acceleration_limit <= 0.0: - raise ValueError("acceleration_limit must be greater than zero when set.") mode = self.dynamic_collision_mode if isinstance(mode, str): try: @@ -117,6 +99,8 @@ def to_motion_gen_options( start_qpos: "torch.Tensor", control_part: str, sample_count: int | None = None, + interpolation_dt: float | None = None, + cartesian_linear: bool = False, ) -> "MotionGenOptions": """Translate this atomic policy into motion-generator options. @@ -124,6 +108,10 @@ def to_motion_gen_options( start_qpos: Observed controlled-joint start positions. control_part: Bound robot control-part name. sample_count: Optional segment-local sample-count override. + interpolation_dt: Explicit waypoint interval used only by + deterministic interpolation. + cartesian_linear: Whether every supplied Cartesian keyframe is a + required linear-path sample rather than a sparse endpoint. Returns: Independently owned options for :class:`MotionGenerator`. @@ -133,12 +121,13 @@ def to_motion_gen_options( return MotionGenOptions( strategy=self.strategy, sample_count=self.sample_count if sample_count is None else sample_count, - velocity_limit=self.velocity_limit, - acceleration_limit=self.acceleration_limit, start_qpos=start_qpos, control_part=control_part, plan_opts=self.plan_opts, is_interpolate=True, + interpolation_dt=interpolation_dt, + is_linear=cartesian_linear, + preserve_cartesian_samples=cartesian_linear, ) @@ -152,9 +141,6 @@ class RecoveryPolicy: max_action_retries: int = 2 """Maximum whole-action retries after planning, execution, or effect failure.""" - tracking_error_threshold: float = 0.05 - """Joint tracking-error threshold in radians.""" - goal_translation_threshold: float = 0.02 """Dynamic-goal translation threshold in metres.""" @@ -162,7 +148,7 @@ class RecoveryPolicy: """Dynamic-goal rotation threshold in radians (five degrees by default).""" action_timeout: float = 30.0 - """Maximum execution time for one action attempt in seconds.""" + """Maximum time for one action attempt, including terminal effect verification.""" def __post_init__(self) -> None: if self.max_replans < 0: @@ -170,7 +156,6 @@ def __post_init__(self) -> None: if self.max_action_retries < 0: raise ValueError("max_action_retries must be non-negative.") threshold_fields = ( - "tracking_error_threshold", "goal_translation_threshold", "goal_rotation_threshold", "action_timeout", diff --git a/embodichain/lab/sim/atomic_actions/primitives/__init__.py b/embodichain/lab/sim/atomic_actions/primitives/__init__.py index 85de2c985..ac82134d3 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/__init__.py +++ b/embodichain/lab/sim/atomic_actions/primitives/__init__.py @@ -41,9 +41,20 @@ MoveHeldObjectOptions, ) from .move_joints import JointPositionGoal, MoveJoints, MoveJointsOptions +from .operate_articulation import ( + OperateArticulation, + OperateArticulationGoal, + OperateArticulationOptions, +) from .pick_up import GraspGoal, PickUp, PickUpOptions from .place import AssembleGoal, Place, PlaceGoal, PlaceOptions from .press import Press, PressGoal, PressOptions +from .slide import ( + Slide, + SlideGoal, + SlideOptions, +) +from .twist import Twist, TwistGoal, TwistOptions BUILTIN_ACTION_TYPES: tuple[type[AtomicAction], ...] = ( MoveEndEffector, @@ -52,9 +63,12 @@ MoveHeldObject, Place, Press, + Slide, + Twist, CoordinatedPickment, CoordinatedPlacement, HandOver, + OperateArticulation, ) """Built-in action implementations instantiated once per action engine.""" @@ -79,6 +93,9 @@ "MoveHeldObjectOptions", "MoveJoints", "MoveJointsOptions", + "OperateArticulation", + "OperateArticulationGoal", + "OperateArticulationOptions", "PickUp", "PickUpOptions", "Place", @@ -87,4 +104,10 @@ "Press", "PressGoal", "PressOptions", + "Slide", + "SlideGoal", + "SlideOptions", + "Twist", + "TwistGoal", + "TwistOptions", ] diff --git a/embodichain/lab/sim/atomic_actions/primitives/_binding_contracts.py b/embodichain/lab/sim/atomic_actions/primitives/_binding_contracts.py new file mode 100644 index 000000000..4824d4e80 --- /dev/null +++ b/embodichain/lab/sim/atomic_actions/primitives/_binding_contracts.py @@ -0,0 +1,73 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Shared binding-contract declarations for built-in manipulation skills.""" + +from __future__ import annotations + +from collections.abc import Mapping + +from ..control import ControlCommand +from ..requirements import ( + DisjointSlotEndpoints, + GRASP_CAPABILITY, + SkillEndpointRequirement, + SkillResourceSlot, +) + + +def make_motion_slot( + role: str, + *, + capabilities: frozenset[str], +) -> SkillResourceSlot: + """Build one motion-endpoint resource slot.""" + return SkillResourceSlot( + slot_id=role, + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=capabilities, + ), + ), + ) + + +def make_manipulation_slot( + role: str, + *, + motion_capabilities: frozenset[str], + grasp_commands: Mapping[str, type[ControlCommand]], +) -> SkillResourceSlot: + """Build one disjoint motion-and-grasp participant slot.""" + return SkillResourceSlot( + slot_id=role, + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=motion_capabilities, + ), + SkillEndpointRequirement( + endpoint_id="grasp", + capabilities=frozenset({GRASP_CAPABILITY}), + required_commands=grasp_commands, + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + ) + + +__all__ = ["make_manipulation_slot", "make_motion_slot"] diff --git a/embodichain/lab/sim/atomic_actions/primitives/_helpers.py b/embodichain/lab/sim/atomic_actions/primitives/_helpers.py index fe3f4c60f..84839a878 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/_helpers.py +++ b/embodichain/lab/sim/atomic_actions/primitives/_helpers.py @@ -18,30 +18,138 @@ from __future__ import annotations +from collections.abc import Sequence +from typing import TYPE_CHECKING + import torch from embodichain.utils import logger +from ..bindings import EndpointBinding from ..state import PlanningContext +from ..trajectory_ops import build_pose_plan_states + +if TYPE_CHECKING: + from embodichain.lab.sim.planners import MotionGenerator + + from ..policies import MotionPolicy + + +def resolve_batched_pose( + pose: torch.Tensor, + *, + num_envs: int, + device: torch.device, + name: str, +) -> torch.Tensor: + """Copy and broadcast one homogeneous pose to ``(num_envs, 4, 4)``.""" + pose = pose.to(device=device, dtype=torch.float32).clone() + if pose.shape == (4, 4): + pose = pose.unsqueeze(0).repeat(num_envs, 1, 1) + if pose.shape != (num_envs, 4, 4): + raise ValueError( + f"{name} must have shape (4, 4) or ({num_envs}, 4, 4), " + f"but got {pose.shape}" + ) + return pose + + +def require_shared_task_state_key( + motion: EndpointBinding, + grasp: EndpointBinding, + *, + participant: str, +) -> str: + """Return the stable task-state key shared by one participant's endpoints. + + Args: + motion: Participant endpoint used for motion control. + grasp: Participant endpoint used for grasp control. + participant: Human-readable participant name used in validation errors. + + Returns: + Stable logical key used to address held-object task state. + + Raises: + ValueError: If the participant endpoints use different task-state keys. + """ + motion_key = motion.task_state_key + grasp_key = grasp.task_state_key + if motion_key != grasp_key: + raise ValueError( + f"{participant} motion and grasp endpoints must share one " + f"task_state_key, but got {motion_key!r} and {grasp_key!r}." + ) + if not isinstance(motion_key, str) or not motion_key: + raise ValueError(f"{participant} task_state_key must be a non-empty string.") + return motion_key def resolve_object_target( target: torch.Tensor, *, - n_envs: int, + num_envs: int, device: torch.device, name: str = "object_target_pose", ) -> torch.Tensor: - """Broadcast an object target pose to ``(n_envs, 4, 4)`` or validate it.""" - target = target.to(device=device, dtype=torch.float32).clone() - if target.shape == (4, 4): - target = target.unsqueeze(0).repeat(n_envs, 1, 1) - if target.shape != (n_envs, 4, 4): - logger.log_error( - f"{name} must be (4, 4) or ({n_envs}, 4, 4), but got {target.shape}", - ValueError, - ) - return target + """Broadcast an object target pose to ``(num_envs, 4, 4)`` or validate it.""" + return resolve_batched_pose( + target, + num_envs=num_envs, + device=device, + name=name, + ) + + +def repeat_qpos(qpos: torch.Tensor, n_waypoints: int) -> torch.Tensor: + """Repeat batched joint positions along a waypoint dimension.""" + return qpos.unsqueeze(1).repeat(1, n_waypoints, 1) + + +def assemble_full_robot_trajectory( + base_qpos: torch.Tensor, + part_trajectories: Sequence[tuple[Sequence[int], torch.Tensor]], +) -> torch.Tensor: + """Overlay control-part trajectories on repeated full-robot positions.""" + if not part_trajectories: + raise ValueError("part_trajectories must not be empty.") + n_waypoints = part_trajectories[0][1].shape[1] + full = repeat_qpos( + base_qpos.to( + device=part_trajectories[0][1].device, + dtype=torch.float32, + ), + n_waypoints, + ).clone() + for joint_ids, trajectory in part_trajectories: + full[:, :, list(joint_ids)] = trajectory + return full + + +def plan_named_arm_trajectory( + motion_generator: MotionGenerator, + control_part: str, + start_qpos: torch.Tensor, + target_poses: torch.Tensor, + n_waypoints: int, + motion_policy: MotionPolicy, + interpolation_dt: float | None, +) -> tuple[torch.Tensor, torch.Tensor]: + """Plan a fixed-size pose trajectory for one named manipulator.""" + result = motion_generator.generate( + build_pose_plan_states(target_poses), + options=motion_policy.to_motion_gen_options( + start_qpos=start_qpos, + control_part=control_part, + sample_count=n_waypoints, + interpolation_dt=interpolation_dt, + ), + ) + if not isinstance(result.success, torch.Tensor): + raise TypeError("Motion planning success must be a torch.Tensor.") + if result.positions is None: + raise ValueError("Motion planning result must contain joint positions.") + return result.success, result.positions def arm_qpos_from_state( @@ -52,4 +160,8 @@ def arm_qpos_from_state( return context.robot.qpos[:, arm_joint_ids] -__all__ = ["arm_qpos_from_state", "resolve_object_target"] +__all__ = [ + "arm_qpos_from_state", + "require_shared_task_state_key", + "resolve_object_target", +] diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py index f59d5286c..75dac8f21 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py @@ -26,21 +26,45 @@ from embodichain.utils import logger from embodichain.utils.math import matrix_from_quat, pose_inv, quat_from_matrix -from ..affordance import AntipodalAffordance -from ..bindings import ResolvedControlPart -from ..control import GRASP_COMMAND, OPEN_COMMAND -from ..core import AtomicAction, ObjectSemantics -from ..effects import StateDelta -from ..goals import ( +from embodichain.lab.sim.atomic_actions.affordance import AntipodalAffordance +from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget +from embodichain.lab.sim.atomic_actions.control import ( + GRASP_COMMAND, + OPEN_COMMAND, + JointPositionCommand, +) +from embodichain.lab.sim.atomic_actions.core import AtomicAction, ObjectSemantics +from embodichain.lab.sim.atomic_actions.effects import StateDelta +from embodichain.lab.sim.atomic_actions.goals import ( ObjectActionGoal, PoseGoalValue, + _resolve_object_pose, resolve_pose_goal, validate_pose_goal, ) -from ..invocation import ActionOptions, ResolvedActionRequest -from ..plans import ActionPlan, normalize_success_mask -from ..state import CoordinatedHeldObjectState, PlanningContext +from embodichain.lab.sim.atomic_actions.invocation import ( + ActionOptions, + ResolvedActionRequest, +) +from embodichain.lab.sim.atomic_actions.plans import ( + ActionPlan, + TimedTrajectory, + normalize_success_mask, +) +from embodichain.lab.sim.atomic_actions.requirements import ( + DisjointResourceSlots, + INVERSE_KINEMATICS_CAPABILITY, + SkillBindingContract, +) +from ..state import CoordinatedHeldObjectState, HeldObjectState, PlanningContext from ..trajectory_ops import interpolate_joint_trajectory, translate_pose_world +from ._binding_contracts import make_manipulation_slot +from ._helpers import ( + assemble_full_robot_trajectory, + repeat_qpos, + require_shared_task_state_key, + resolve_batched_pose, +) @dataclass(frozen=True, slots=True, eq=False) @@ -53,13 +77,15 @@ class CoordinatedPickGoal(ObjectActionGoal): :class:`CoordinatedPickmentOptions`. """ - goal_kind: ClassVar[str] = "coordinated_pick" - object_target_pose: PoseGoalValue - """Target pose for the shared object, shape ``(4, 4)`` or ``(n_envs, 4, 4)``.""" + """Target pose for the shared object, shape ``(4, 4)`` or ``(num_envs, 4, 4)``.""" object_initial_pose: PoseGoalValue | None = None - """Optional initial object pose. Defaults to ``semantics.entity`` pose.""" + """Optional initial object pose. + + When omitted, the pose is grounded through the semantic object's stable + scene identity, with its live entity retained only as a legacy fallback. + """ def __post_init__(self) -> None: ObjectActionGoal.__post_init__(self) @@ -144,10 +170,12 @@ def __post_init__(self) -> None: class _CoordinatedPickResources: """Invocation-bound control parts and compatible hand commands.""" - left_arm: ResolvedControlPart - right_arm: ResolvedControlPart - left_hand: ResolvedControlPart - right_hand: ResolvedControlPart + left_task_state_key: str + right_task_state_key: str + left_arm: JointPositionTarget + right_arm: JointPositionTarget + left_hand: JointPositionTarget + right_hand: JointPositionTarget left_hand_open_qpos: torch.Tensor left_hand_close_qpos: torch.Tensor right_hand_open_qpos: torch.Tensor @@ -160,27 +188,21 @@ class _DualArmHelpers: def _expand_qpos(self, qpos: torch.Tensor, dof: int, name: str) -> torch.Tensor: qpos = qpos.to(device=self.device, dtype=torch.float32) if qpos.shape == (dof,): - return qpos.unsqueeze(0).repeat(self.n_envs, 1) - if qpos.shape == (self.n_envs, dof): + return qpos.unsqueeze(0).repeat(self.num_envs, 1) + if qpos.shape == (self.num_envs, dof): return qpos - logger.log_error( + raise ValueError( f"{name} must have shape ({dof},) or " - f"({self.n_envs}, {dof}), but got {qpos.shape}", - ValueError, + f"({self.num_envs}, {dof}), but got {qpos.shape}" ) - raise AssertionError("unreachable") def _resolve_pose(self, pose: torch.Tensor, name: str) -> torch.Tensor: - pose = pose.to(device=self.device, dtype=torch.float32) - if pose.shape == (4, 4): - pose = pose.unsqueeze(0).repeat(self.n_envs, 1, 1) - if pose.shape != (self.n_envs, 4, 4): - logger.log_error( - f"{name} must have shape (4, 4) or " - f"({self.n_envs}, 4, 4), but got {pose.shape}", - ValueError, - ) - return pose + return resolve_batched_pose( + pose, + num_envs=self.num_envs, + device=self.device, + name=name, + ) def _resolve_dual_arm_start( self, @@ -203,22 +225,15 @@ def _assemble_segment( *, resources: _CoordinatedPickResources, ) -> torch.Tensor: - n_waypoints = first_arm_traj.shape[1] - full = torch.empty( - (self.n_envs, n_waypoints, self.robot_dof), - dtype=torch.float32, - device=self.device, + return assemble_full_robot_trajectory( + state.last_qpos, + ( + (resources.left_arm.joint_ids, first_arm_traj), + (resources.right_arm.joint_ids, second_arm_traj), + (resources.left_hand.joint_ids, first_hand_traj), + (resources.right_hand.joint_ids, second_hand_traj), + ), ) - full[:, :, :] = state.last_qpos.to(self.device).unsqueeze(1) - full[:, :, list(resources.left_arm.joint_ids)] = first_arm_traj - full[:, :, list(resources.right_arm.joint_ids)] = second_arm_traj - full[:, :, list(resources.left_hand.joint_ids)] = first_hand_traj - full[:, :, list(resources.right_hand.joint_ids)] = second_hand_traj - return full - - @staticmethod - def _repeat_qpos(qpos: torch.Tensor, n_waypoints: int) -> torch.Tensor: - return qpos.unsqueeze(1).repeat(1, n_waypoints, 1) def _interpolate_qpos( self, @@ -264,7 +279,7 @@ def _interpolate_qpos_keyframes( n_waypoints: int, ) -> torch.Tensor: trajectory = torch.zeros( - (self.n_envs, n_waypoints, keyframe_qpos.shape[-1]), + (self.num_envs, n_waypoints, keyframe_qpos.shape[-1]), dtype=torch.float32, device=self.device, ) @@ -322,7 +337,7 @@ def _interpolate_object_pose( ) quat = quat / torch.linalg.norm(quat, dim=-1, keepdim=True).clamp_min(1e-8) poses[:, :, :3, :3] = matrix_from_quat(quat.reshape(-1, 4)).reshape( - self.n_envs, n_waypoints, 3, 3 + self.num_envs, n_waypoints, 3, 3 ) return poses @@ -335,8 +350,20 @@ class CoordinatedPickment( skill_id: ClassVar[str] = "coordinated_pickment" GoalType: ClassVar[type] = CoordinatedPickGoal OptionsType: ClassVar[type] = CoordinatedPickmentOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("left", "right") - end_effector_roles: ClassVar[tuple[str, ...]] = ("left", "right") + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=tuple( + make_manipulation_slot( + role, + motion_capabilities=frozenset({INVERSE_KINEMATICS_CAPABILITY}), + grasp_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, + ) + for role in ("left", "right") + ), + constraints=(DisjointResourceSlots(("left", "right")),), + ) _assemble_segment = _DualArmHelpers._assemble_segment _expand_qpos = _DualArmHelpers._expand_qpos @@ -344,20 +371,25 @@ class CoordinatedPickment( _interpolate_object_pose = _DualArmHelpers._interpolate_object_pose _interpolate_qpos = _DualArmHelpers._interpolate_qpos _interpolate_qpos_keyframes = _DualArmHelpers._interpolate_qpos_keyframes - _repeat_qpos = staticmethod(_DualArmHelpers._repeat_qpos) + _repeat_qpos = staticmethod(repeat_qpos) _resolve_dual_arm_start = _DualArmHelpers._resolve_dual_arm_start _resolve_pose = _DualArmHelpers._resolve_pose - def __init__( + def _scene_dependencies( self, - default_options: CoordinatedPickmentOptions | None = None, - ) -> None: - super().__init__(default_options) - - def _on_bind(self) -> None: - """Resolve engine-wide resources from the owning engine.""" - self.n_envs = self.robot.get_qpos().shape[0] - self.robot_dof = self.robot.dof + request: ResolvedActionRequest[ + CoordinatedPickGoal, + CoordinatedPickmentOptions, + ], + ) -> tuple[str, ...]: + """Track the semantic object only when it supplies the initial pose.""" + dependencies = set(super()._scene_dependencies(request)) + target = request.goal + if target.object_initial_pose is None: + entity_id = target.semantics.entity_id + if entity_id is not None: + dependencies.add(entity_id) + return tuple(sorted(dependencies)) def _resolve_resources( self, @@ -365,46 +397,67 @@ def _resolve_resources( ) -> _CoordinatedPickResources: """Resolve left/right roles from robot control parts.""" binding = request.binding - left_arm = binding.manipulator("left") - right_arm = binding.manipulator("right") - left_hand = binding.end_effector("left") - right_hand = binding.end_effector("right") - if left_arm.name == right_arm.name: + left_motion = binding.endpoint("left", "motion") + right_motion = binding.endpoint("right", "motion") + left_grasp = binding.endpoint("left", "grasp") + right_grasp = binding.endpoint("right", "grasp") + left_arm = left_motion.require_target(JointPositionTarget) + right_arm = right_motion.require_target(JointPositionTarget) + left_hand = left_grasp.require_target(JointPositionTarget) + right_hand = right_grasp.require_target(JointPositionTarget) + left_task_state_key = require_shared_task_state_key( + left_motion, + left_grasp, + participant="CoordinatedPickment left participant", + ) + right_task_state_key = require_shared_task_state_key( + right_motion, + right_grasp, + participant="CoordinatedPickment right participant", + ) + if left_task_state_key == right_task_state_key: + raise ValueError( + "CoordinatedPickment left and right participants must use " + "different task_state_key values." + ) + if left_arm.control_part == right_arm.control_part: raise ValueError( "CoordinatedPickment left and right roles must use different " "manipulator control parts." ) - if left_hand.name == right_hand.name: + if left_hand.control_part == right_hand.control_part: raise ValueError( "CoordinatedPickment left and right roles must use different " "end-effector control parts." ) return _CoordinatedPickResources( + left_task_state_key=left_task_state_key, + right_task_state_key=right_task_state_key, left_arm=left_arm, right_arm=right_arm, left_hand=left_hand, right_hand=right_hand, - left_hand_open_qpos=left_hand.joint_positions( + left_hand_open_qpos=left_grasp.joint_positions( OPEN_COMMAND, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, dtype=torch.float32, ), - left_hand_close_qpos=left_hand.joint_positions( + left_hand_close_qpos=left_grasp.joint_positions( GRASP_COMMAND, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, dtype=torch.float32, ), - right_hand_open_qpos=right_hand.joint_positions( + right_hand_open_qpos=right_grasp.joint_positions( OPEN_COMMAND, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, dtype=torch.float32, ), - right_hand_close_qpos=right_hand.joint_positions( + right_hand_close_qpos=right_grasp.joint_positions( GRASP_COMMAND, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, dtype=torch.float32, ), @@ -424,14 +477,12 @@ def _resolve_object_initial_pose( ), "object_initial_pose", ) - if target.semantics.entity is None: - logger.log_error( - "CoordinatedPickGoal requires object_initial_pose when " - "semantics.entity is not provided.", - ValueError, - ) return self._resolve_pose( - target.semantics.entity.get_local_pose(to_matrix=True), + _resolve_object_pose( + target.semantics, + context, + name="object_initial_pose", + ), "object_initial_pose", ) @@ -447,7 +498,7 @@ def _resolve_target( torch.Tensor, torch.Tensor, torch.Tensor, - CoordinatedHeldObjectState, + tuple[HeldObjectState, HeldObjectState], torch.Tensor, ]: object_initial_pose = self._resolve_object_initial_pose(target, context) @@ -468,12 +519,17 @@ def _resolve_target( right_object_to_eef = torch.bmm(pose_inv(object_initial_pose), right_grasp_xpos) left_target_xpos = torch.bmm(object_target_pose, left_object_to_eef) right_target_xpos = torch.bmm(object_target_pose, right_object_to_eef) - held_state = CoordinatedHeldObjectState( - semantics=target.semantics, - left_object_to_eef=left_object_to_eef, - right_object_to_eef=right_object_to_eef, - left_grasp_xpos=left_grasp_xpos, - right_grasp_xpos=right_grasp_xpos, + held_states = ( + HeldObjectState( + semantics=target.semantics, + object_to_eef=left_object_to_eef, + grasp_xpos=left_grasp_xpos, + ), + HeldObjectState( + semantics=target.semantics, + object_to_eef=right_object_to_eef, + grasp_xpos=right_grasp_xpos, + ), ) return ( object_initial_pose, @@ -482,7 +538,7 @@ def _resolve_target( right_grasp_xpos, left_target_xpos, right_target_xpos, - held_state, + held_states, grasp_success, ) @@ -496,27 +552,26 @@ def _resolve_dual_arm_grasp_poses( Args: semantics: Object semantics carrying an :class:`AntipodalAffordance`. - object_poses: Object poses with shape ``(n_envs, 4, 4)``. + object_poses: Object poses with shape ``(num_envs, 4, 4)``. options: Coordinated pickment options carrying the dual-arm and approach directions used by the affordance sampler. Returns: ``(left_grasp_xpos, right_grasp_xpos, success_mask)``. The grasp poses - have shape ``(n_envs, 4, 4)`` and the success mask has shape - ``(n_envs,)``. Environments without a valid left or right grasp hold + have shape ``(num_envs, 4, 4)`` and the success mask has shape + ``(num_envs,)``. Environments without a valid left or right grasp hold the identity pose and are marked ``False``. """ if not isinstance(semantics.affordance, AntipodalAffordance): - logger.log_error( + raise ValueError( "CoordinatedPickment requires an AntipodalAffordance to sample " - "dual-arm grasps.", - ValueError, + "dual-arm grasps." ) - n_envs = object_poses.shape[0] + num_envs = object_poses.shape[0] identity = torch.eye(4, dtype=torch.float32, device=self.device) - left_grasp_xpos = identity.unsqueeze(0).repeat(n_envs, 1, 1) - right_grasp_xpos = identity.unsqueeze(0).repeat(n_envs, 1, 1) - success_mask = torch.zeros(n_envs, dtype=torch.bool, device=self.device) + left_grasp_xpos = identity.unsqueeze(0).repeat(num_envs, 1, 1) + right_grasp_xpos = identity.unsqueeze(0).repeat(num_envs, 1, 1) + success_mask = torch.zeros(num_envs, dtype=torch.bool, device=self.device) approach_direction = options.approach_direction.to( device=self.device, dtype=torch.float32 ) @@ -593,10 +648,9 @@ def _compute_segment_lengths( n_lift = n_motion // 3 n_move = n_motion - n_approach - n_lift if min(n_approach, n_lift, n_move) < 2: - logger.log_error( + raise ValueError( "Not enough waypoints for coordinated pickment. Please increase " - "sample_count or decrease hand_interp_steps/hold_steps.", - ValueError, + "sample_count or decrease hand_interp_steps/hold_steps." ) return { "approach": n_approach, @@ -650,7 +704,7 @@ def _plan_masked_arm_trajectory( ) -> tuple[torch.Tensor, torch.Tensor]: n_state = target_poses.shape[1] keyframe_qpos = torch.zeros( - (self.n_envs, n_state, start_qpos.shape[-1]), + (self.num_envs, n_state, start_qpos.shape[-1]), dtype=torch.float32, device=self.device, ) @@ -664,7 +718,7 @@ def _plan_masked_arm_trajectory( ) ik_success = normalize_success_mask( ik_success, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, name=f"IK success for {control_part} target state {target_idx}", ) @@ -701,12 +755,12 @@ def _plan_synchronized_object_motion( n_waypoints = object_pose_traj.shape[1] keyframe_indices = self._select_motion_keyframe_indices(n_waypoints, options) left_traj = torch.zeros( - (self.n_envs, len(keyframe_indices), left_start_qpos.shape[-1]), + (self.num_envs, len(keyframe_indices), left_start_qpos.shape[-1]), dtype=torch.float32, device=self.device, ) right_traj = torch.zeros( - (self.n_envs, len(keyframe_indices), right_start_qpos.shape[-1]), + (self.num_envs, len(keyframe_indices), right_start_qpos.shape[-1]), dtype=torch.float32, device=self.device, ) @@ -720,39 +774,39 @@ def _plan_synchronized_object_motion( ) left_success, left_qpos = self.robot.compute_ik( pose=left_xpos, - name=resources.left_arm.name, + name=resources.left_arm.control_part, joint_seed=left_qpos_seed, ) right_success, right_qpos = self.robot.compute_ik( pose=right_xpos, - name=resources.right_arm.name, + name=resources.right_arm.control_part, joint_seed=right_qpos_seed, ) left_success = normalize_success_mask( left_success, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, name=( - f"IK success for {resources.left_arm.name} object waypoint " + f"IK success for {resources.left_arm.control_part} object waypoint " f"{waypoint_idx}" ), ) right_success = normalize_success_mask( right_success, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, name=( - f"IK success for {resources.right_arm.name} object waypoint " + f"IK success for {resources.right_arm.control_part} object waypoint " f"{waypoint_idx}" ), ) self._log_ik_failures( - resources.left_arm.name, + resources.left_arm.control_part, f"object waypoint {waypoint_idx}", success_mask & ~left_success, ) self._log_ik_failures( - resources.right_arm.name, + resources.right_arm.control_part, f"object waypoint {waypoint_idx}", success_mask & ~right_success, ) @@ -784,7 +838,7 @@ def _plan( context: PlanningContext, ) -> ActionPlan: """Plan a coordinated pick without committing the dual attachment.""" - target = self.require_goal(request) + target = request.goal options = request.skill_options resources = self._resolve_resources(request) if ( @@ -802,9 +856,10 @@ def _plan( right_grasp_xpos, left_target_xpos, right_target_xpos, - held_state, + held_states, grasp_success, ) = self._resolve_target(target, context, options) + left_held_state, right_held_state = held_states if not grasp_success.any(): logger.log_warning("CoordinatedPickment failed to resolve dual-arm grasps.") return self.failed_plan( @@ -828,14 +883,14 @@ def _plan( ) success_mask = grasp_success.clone() success_mask, left_approach_traj = self._plan_masked_arm_trajectory( - resources.left_arm.name, + resources.left_arm.control_part, left_start_qpos, left_approach_targets, segments["approach"], success_mask, ) success_mask, right_approach_traj = self._plan_masked_arm_trajectory( - resources.right_arm.name, + resources.right_arm.control_part, right_start_qpos, right_approach_targets, segments["approach"], @@ -885,8 +940,8 @@ def _plan( left_grasp_qpos, right_grasp_qpos, lift_object_traj, - held_state.left_object_to_eef, - held_state.right_object_to_eef, + left_held_state.object_to_eef, + right_held_state.object_to_eef, success_mask, resources, options, @@ -915,8 +970,8 @@ def _plan( left_lift_qpos, right_lift_qpos, move_object_traj, - held_state.left_object_to_eef, - held_state.right_object_to_eef, + left_held_state.object_to_eef, + right_held_state.object_to_eef, success_mask, resources, options, @@ -935,7 +990,9 @@ def _plan( ) hold_trajectory = torch.empty( - (self.n_envs, 0, self.robot_dof), dtype=torch.float32, device=self.device + (self.num_envs, 0, self.robot_dof), + dtype=torch.float32, + device=self.device, ) if segments["hold"] > 0: hold_trajectory = self._assemble_segment( @@ -958,9 +1015,9 @@ def _plan( dim=1, ) coordinated_held_object = CoordinatedHeldObjectState( - semantics=held_state.semantics, - left_object_to_eef=held_state.left_object_to_eef, - right_object_to_eef=held_state.right_object_to_eef, + semantics=left_held_state.semantics, + left_object_to_eef=left_held_state.object_to_eef, + right_object_to_eef=right_held_state.object_to_eef, left_grasp_xpos=left_target_xpos, right_grasp_xpos=right_target_xpos, ) @@ -968,16 +1025,20 @@ def _plan( request, context, success=success_mask, - trajectory=full, + trajectory=TimedTrajectory.from_uniform_step( + full, + env_ids=context.env_ids, + step_dt=context.require_control_dt(), + ), expected_effects=StateDelta( held_object_updates={ - resources.left_arm.name: None, - resources.right_arm.name: None, + resources.left_task_state_key: None, + resources.right_task_state_key: None, }, coordinated_held_object_updates={ ( - resources.left_arm.name, - resources.right_arm.name, + resources.left_task_state_key, + resources.right_task_state_key, ): coordinated_held_object, }, ), diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py index bba2c2739..20812641f 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py @@ -25,29 +25,39 @@ from embodichain.utils import logger -from ._helpers import resolve_object_target -from ..bindings import ResolvedControlPart -from ..control import GRASP_COMMAND, OPEN_COMMAND +from ..bindings import JointPositionTarget +from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand from ..core import AtomicAction from ..effects import StateDelta from ..goals import PoseGoalValue, resolve_pose_goal, validate_pose_goal from ..invocation import ActionOptions, ResolvedActionRequest -from ..plans import ActionPlan, normalize_success_mask +from ..plans import ActionPlan, TimedTrajectory, normalize_success_mask from ..policies import MotionPolicy -from ..state import HeldObjectState, PlanningContext -from ..trajectory_ops import ( - build_pose_plan_states, +from ..requirements import ( + CARTESIAN_POSE_CAPABILITY, + DisjointResourceSlots, + SkillBindingContract, +) +from embodichain.lab.sim.atomic_actions.state import HeldObjectState, PlanningContext +from embodichain.lab.sim.atomic_actions.trajectory_ops import ( interpolate_hand_qpos, translate_pose_world, ) +from ._binding_contracts import make_manipulation_slot +from ._helpers import ( + assemble_full_robot_trajectory, + plan_named_arm_trajectory, + repeat_qpos, + require_shared_task_state_key, + resolve_batched_pose, + resolve_object_target, +) @dataclass(frozen=True, slots=True, eq=False) class CoordinatedPlacementGoal: """Object-centric target for dual-arm coordinated placement.""" - goal_kind: ClassVar[str] = "coordinated_placement" - placing_object_target_pose: PoseGoalValue """Target pose for the object released by the placing arm.""" @@ -113,10 +123,12 @@ def __post_init__(self) -> None: class _CoordinatedPlacementResources: """Invocation-bound control parts and compatible hand commands.""" - placing_arm: ResolvedControlPart - support_arm: ResolvedControlPart - placing_hand: ResolvedControlPart - support_hand: ResolvedControlPart + placing_task_state_key: str + support_task_state_key: str + placing_arm: JointPositionTarget + support_arm: JointPositionTarget + placing_hand: JointPositionTarget + support_hand: JointPositionTarget placing_hand_open_qpos: torch.Tensor placing_hand_close_qpos: torch.Tensor support_hand_close_qpos: torch.Tensor @@ -130,19 +142,25 @@ class CoordinatedPlacement( skill_id: ClassVar[str] = "coordinated_placement" GoalType: ClassVar[type] = CoordinatedPlacementGoal OptionsType: ClassVar[type] = CoordinatedPlacementOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("placing", "support") - end_effector_roles: ClassVar[tuple[str, ...]] = ("placing", "support") - - def __init__( - self, - default_options: CoordinatedPlacementOptions | None = None, - ) -> None: - super().__init__(default_options) - - def _on_bind(self) -> None: - """Resolve engine-wide resources from the owning engine.""" - self.n_envs = self.robot.get_qpos().shape[0] - self.robot_dof = self.robot.dof + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + make_manipulation_slot( + "placing", + motion_capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + grasp_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, + ), + make_manipulation_slot( + "support", + motion_capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + grasp_commands={GRASP_COMMAND: JointPositionCommand}, + ), + ), + constraints=(DisjointResourceSlots(("placing", "support")),), + ) + _repeat_qpos = staticmethod(repeat_qpos) def _resolve_resources( self, @@ -152,40 +170,61 @@ def _resolve_resources( ) -> _CoordinatedPlacementResources: """Resolve placing/support roles from robot control parts.""" binding = request.binding - placing_arm = binding.manipulator("placing") - support_arm = binding.manipulator("support") - placing_hand = binding.end_effector("placing") - support_hand = binding.end_effector("support") - if placing_arm.name == support_arm.name: + placing_motion = binding.endpoint("placing", "motion") + support_motion = binding.endpoint("support", "motion") + placing_grasp = binding.endpoint("placing", "grasp") + support_grasp = binding.endpoint("support", "grasp") + placing_arm = placing_motion.require_target(JointPositionTarget) + support_arm = support_motion.require_target(JointPositionTarget) + placing_hand = placing_grasp.require_target(JointPositionTarget) + support_hand = support_grasp.require_target(JointPositionTarget) + placing_task_state_key = require_shared_task_state_key( + placing_motion, + placing_grasp, + participant="CoordinatedPlacement placing participant", + ) + support_task_state_key = require_shared_task_state_key( + support_motion, + support_grasp, + participant="CoordinatedPlacement support participant", + ) + if placing_task_state_key == support_task_state_key: + raise ValueError( + "CoordinatedPlacement placing and support participants must " + "use different task_state_key values." + ) + if placing_arm.control_part == support_arm.control_part: raise ValueError( "CoordinatedPlacement placing and support roles must use " "different manipulator control parts." ) - if placing_hand.name == support_hand.name: + if placing_hand.control_part == support_hand.control_part: raise ValueError( "CoordinatedPlacement placing and support roles must use " "different end-effector control parts." ) return _CoordinatedPlacementResources( + placing_task_state_key=placing_task_state_key, + support_task_state_key=support_task_state_key, placing_arm=placing_arm, support_arm=support_arm, placing_hand=placing_hand, support_hand=support_hand, - placing_hand_open_qpos=placing_hand.joint_positions( + placing_hand_open_qpos=placing_grasp.joint_positions( OPEN_COMMAND, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, dtype=torch.float32, ), - placing_hand_close_qpos=placing_hand.joint_positions( + placing_hand_close_qpos=placing_grasp.joint_positions( GRASP_COMMAND, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, dtype=torch.float32, ), - support_hand_close_qpos=support_hand.joint_positions( + support_hand_close_qpos=support_grasp.joint_positions( GRASP_COMMAND, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, dtype=torch.float32, ), @@ -199,7 +238,7 @@ def _plan( context: PlanningContext, ) -> ActionPlan: """Plan coordinated placement without committing attachment changes.""" - target = self.require_goal(request) + target = request.goal options = request.skill_options resources = self._resolve_resources(request) if ( @@ -217,6 +256,18 @@ def _plan( placing_held_object, support_held_object, ) = self._resolve_target(target, state, resources, options) + eligible = context.task.exclusive_held_object_mask( + resources.placing_task_state_key + ) & context.task.exclusive_held_object_mask(resources.support_task_state_key) + if not eligible.any(): + logger.log_warning( + "CoordinatedPlacement requires two exclusively held objects." + ) + return self.failed_plan( + request, + context, + message="Placing and support objects must be held exclusively.", + ) placing_start_qpos, support_start_qpos = self._resolve_start_qpos( state, resources ) @@ -233,21 +284,19 @@ def _plan( ), ) - success_mask = torch.ones( - self.n_envs, - dtype=torch.bool, - device=self.device, - ) - segment_success, placing_approach_traj = self._plan_named_arm_trajectory( - resources.placing_arm.name, + success_mask = eligible.clone() + segment_success, placing_approach_traj = plan_named_arm_trajectory( + self.motion_generator, + resources.placing_arm.control_part, placing_start_qpos, torch.stack([placing_lift_xpos, placing_xpos], dim=1), segments["approach"], request.motion_policy, + context.control_dt, ) success_mask &= normalize_success_mask( segment_success, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, name="Placing-approach success", ) @@ -257,16 +306,18 @@ def _plan( request, context, message="Placing approach failed." ) - segment_success, support_approach_traj = self._plan_named_arm_trajectory( - resources.support_arm.name, + segment_success, support_approach_traj = plan_named_arm_trajectory( + self.motion_generator, + resources.support_arm.control_part, support_start_qpos, support_xpos.unsqueeze(1), segments["approach"], request.motion_policy, + context.control_dt, ) success_mask &= normalize_success_mask( segment_success, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, name="Support-approach success", ) @@ -315,16 +366,18 @@ def _plan( resources=resources, ) - segment_success, placing_retreat_traj = self._plan_named_arm_trajectory( - resources.placing_arm.name, + segment_success, placing_retreat_traj = plan_named_arm_trajectory( + self.motion_generator, + resources.placing_arm.control_part, placing_place_qpos, placing_lift_xpos.unsqueeze(1), segments["retreat"], request.motion_policy, + context.control_dt, ) success_mask &= normalize_success_mask( segment_success, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, name="Placing-retreat success", ) @@ -355,28 +408,31 @@ def _plan( ], dim=1, ) - involved_control_parts = { - resources.placing_arm.name, - resources.support_arm.name, + involved_task_state_keys = { + resources.placing_task_state_key, + resources.support_task_state_key, } coordinated_removals = { key: None - for key in state.coordinated_held_objects - if not involved_control_parts.isdisjoint(key) + for key in state.task.coordinated_held_objects + if not involved_task_state_keys.isdisjoint(key) } return self.build_plan( request, context, success=success_mask, - trajectory=full, + trajectory=TimedTrajectory.from_uniform_step( + full, + env_ids=context.env_ids, + step_dt=context.require_control_dt(), + ), expected_effects=StateDelta( held_object_updates={ - resources.placing_arm.name: ( + resources.placing_task_state_key: ( None if release else placing_held_object ), - resources.support_arm.name: support_held_object, + resources.support_task_state_key: support_held_object, }, - coordinated_held_object_updates=coordinated_removals, ), segment_lengths={ "approach": approach_trajectory.shape[1], @@ -395,7 +451,7 @@ def _resolve_object_pose( ) -> torch.Tensor: object_pose = resolve_object_target( resolve_pose_goal(pose, context, name=name), - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, name=name, ) @@ -419,16 +475,12 @@ def _resolve_object_to_eef( ) def _resolve_held_matrix(self, matrix: torch.Tensor, name: str) -> torch.Tensor: - matrix = matrix.to(device=self.device, dtype=torch.float32) - if matrix.shape == (4, 4): - matrix = matrix.unsqueeze(0).repeat(self.n_envs, 1, 1) - if matrix.shape != (self.n_envs, 4, 4): - logger.log_error( - f"{name} must have shape (4, 4) or ({self.n_envs}, 4, 4), " - f"but got {matrix.shape}", - ValueError, - ) - return matrix + return resolve_batched_pose( + matrix, + num_envs=self.num_envs, + device=self.device, + name=name, + ) def _resolve_held_state( self, @@ -458,20 +510,20 @@ def _resolve_target( HeldObjectState, HeldObjectState, ]: - placing_control_part = resources.placing_arm.name - support_control_part = resources.support_arm.name - placing_held_object = state.get_held_object(placing_control_part) + placing_task_state_key = resources.placing_task_state_key + support_task_state_key = resources.support_task_state_key + placing_held_object = state.get_held_object(placing_task_state_key) if placing_held_object is None: logger.log_error( - "CoordinatedPlacement requires an object held by placing control " - f"part {placing_control_part!r}.", + "CoordinatedPlacement requires an object held by placing " + f"task-state resource {placing_task_state_key!r}.", ValueError, ) - support_held_object = state.get_held_object(support_control_part) + support_held_object = state.get_held_object(support_task_state_key) if support_held_object is None: logger.log_error( - "CoordinatedPlacement requires an object held by support control " - f"part {support_control_part!r}.", + "CoordinatedPlacement requires an object held by support " + f"task-state resource {support_task_state_key!r}.", ValueError, ) placing_height_offset = ( @@ -528,12 +580,11 @@ def _resolve_start_qpos( state: PlanningContext, resources: _CoordinatedPlacementResources, ) -> tuple[torch.Tensor, torch.Tensor]: - if state.last_qpos.shape != (self.n_envs, self.robot_dof): - logger.log_error( + if state.last_qpos.shape != (self.num_envs, self.robot_dof): + raise ValueError( "PlanningContext.last_qpos must have shape " - f"({self.n_envs}, {self.robot_dof}), " - f"but got {state.last_qpos.shape}", - ValueError, + f"({self.num_envs}, {self.robot_dof}), " + f"but got {state.last_qpos.shape}" ) start_qpos = state.last_qpos.to(device=self.device, dtype=torch.float32) return ( @@ -553,10 +604,9 @@ def _compute_segment_lengths( n_retreat = max(2, options.retreat_steps) n_approach = sample_count - n_hold - n_release - n_retreat if n_approach < 2: - logger.log_error( + raise ValueError( "Not enough waypoints for coordinated placement. Increase " - "sample_count or decrease hold/release/retreat steps.", - ValueError, + "sample_count or decrease hold/release/retreat steps." ) return { "approach": n_approach, @@ -565,33 +615,9 @@ def _compute_segment_lengths( "retreat": n_retreat, } - def _plan_named_arm_trajectory( - self, - control_part: str, - start_qpos: torch.Tensor, - target_poses: torch.Tensor, - n_waypoints: int, - motion_policy: MotionPolicy, - ) -> tuple[torch.Tensor, torch.Tensor]: - result = self.motion_generator.generate( - build_pose_plan_states(target_poses), - options=motion_policy.to_motion_gen_options( - start_qpos=start_qpos, - control_part=control_part, - sample_count=n_waypoints, - ), - ) - assert isinstance(result.success, torch.Tensor) - assert result.positions is not None - return result.success, result.positions - - @staticmethod - def _repeat_qpos(qpos: torch.Tensor, n_waypoints: int) -> torch.Tensor: - return qpos.unsqueeze(1).repeat(1, n_waypoints, 1) - def _empty_segment(self) -> torch.Tensor: return torch.empty( - (self.n_envs, 0, self.robot_dof), + (self.num_envs, 0, self.robot_dof), dtype=torch.float32, device=self.device, ) @@ -606,14 +632,15 @@ def _assemble_segment( *, resources: _CoordinatedPlacementResources, ) -> torch.Tensor: - n_waypoints = placing_arm_traj.shape[1] - full = base_full_qpos.to(device=self.device, dtype=torch.float32) - full = full.unsqueeze(1).repeat(1, n_waypoints, 1).clone() - full[:, :, list(resources.placing_arm.joint_ids)] = placing_arm_traj - full[:, :, list(resources.support_arm.joint_ids)] = support_arm_traj - full[:, :, list(resources.placing_hand.joint_ids)] = placing_hand_traj - full[:, :, list(resources.support_hand.joint_ids)] = support_hand_traj - return full + return assemble_full_robot_trajectory( + base_full_qpos, + ( + (resources.placing_arm.joint_ids, placing_arm_traj), + (resources.support_arm.joint_ids, support_arm_traj), + (resources.placing_hand.joint_ids, placing_hand_traj), + (resources.support_hand.joint_ids, support_hand_traj), + ), + ) __all__ = [ diff --git a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py index 4c0878773..3a06dce1b 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py +++ b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py @@ -26,19 +26,52 @@ from embodichain.utils import logger from embodichain.utils.math import pose_inv -from ..bindings import ResolvedControlPart -from ..control import GRASP_COMMAND, OPEN_COMMAND -from ..core import AtomicAction, ObjectSemantics -from ..effects import StateDelta -from ..invocation import ActionOptions, ResolvedActionRequest -from ..plans import ActionPlan, normalize_success_mask -from ..policies import MotionPolicy -from ..state import HeldObjectState, PlanningContext -from ..trajectory_ops import ( - build_pose_plan_states, +from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget +from embodichain.lab.sim.atomic_actions.control import ( + GRASP_COMMAND, + OPEN_COMMAND, + JointPositionCommand, +) +from embodichain.lab.sim.atomic_actions.core import ( + AtomicAction, + ObjectSemantics, + _same_object_identity, +) +from embodichain.lab.sim.atomic_actions.effects import StateDelta +from embodichain.lab.sim.atomic_actions.goals import ( + PoseGoalValue, + collect_scene_dependencies, + resolve_pose_goal, + validate_pose_goal, +) +from embodichain.lab.sim.atomic_actions.invocation import ( + ActionOptions, + ResolvedActionRequest, +) +from embodichain.lab.sim.atomic_actions.plans import ( + ActionPlan, + TimedTrajectory, + normalize_success_mask, +) +from embodichain.lab.sim.atomic_actions.requirements import ( + CARTESIAN_POSE_CAPABILITY, + DisjointResourceSlots, + FORWARD_KINEMATICS_CAPABILITY, + SkillBindingContract, +) +from embodichain.lab.sim.atomic_actions.state import HeldObjectState, PlanningContext +from embodichain.lab.sim.atomic_actions.trajectory_ops import ( interpolate_hand_qpos, translate_pose_world, ) +from ._binding_contracts import make_manipulation_slot +from ._helpers import ( + assemble_full_robot_trajectory, + plan_named_arm_trajectory, + repeat_qpos, + require_shared_task_state_key, + resolve_batched_pose, +) from .pick_up import GraspGoal @@ -50,13 +83,15 @@ class HandOverOptions(ActionOptions): """Object part the receiving arm grasps during the handover (see :meth:`AntipodalAffordance.get_valid_grasp_poses`).""" - middle_object_pose: torch.Tensor | None = None + middle_object_pose: PoseGoalValue | None = None """Object pose at the handover point where the receiving arm grasps it, - shape ``(4, 4)`` or ``(n_envs, 4, 4)``. Must be set by the caller.""" + either a scene-relative pose or a tensor with shape ``(4, 4)`` or + ``(n_envs, 4, 4)``. Must be set by the caller.""" - final_object_pose: torch.Tensor | None = None - """Object pose the receiving arm delivers the object to, shape ``(4, 4)`` - or ``(n_envs, 4, 4)``. Must be set by the caller.""" + final_object_pose: PoseGoalValue | None = None + """Object pose the receiving arm delivers the object to, either a + scene-relative pose or a tensor with shape ``(4, 4)`` or + ``(n_envs, 4, 4)``. Must be set by the caller.""" receive_approach_direction: torch.Tensor = torch.tensor( [0.0, 0.0, -1.0], dtype=torch.float32 @@ -106,17 +141,28 @@ def __post_init__(self) -> None: for name in ("middle_object_pose", "final_object_pose"): value = getattr(self, name) if value is not None: - object.__setattr__(self, name, value.clone()) + validate_pose_goal(value, name, allow_waypoints=False) + object.__setattr__( + self, + name, + ( + value.clone() + if isinstance(value, torch.Tensor) + else value.snapshot() + ), + ) @dataclass(frozen=True, slots=True, eq=False) class _HandOverResources: """Invocation-bound control parts and compatible hand commands.""" - transfer_arm: ResolvedControlPart - receive_arm: ResolvedControlPart - transfer_hand: ResolvedControlPart - receive_hand: ResolvedControlPart + transfer_task_state_key: str + receive_task_state_key: str + transfer_arm: JointPositionTarget + receive_arm: JointPositionTarget + transfer_hand: JointPositionTarget + receive_hand: JointPositionTarget transfer_hand_open_qpos: torch.Tensor transfer_hand_close_qpos: torch.Tensor receive_hand_open_qpos: torch.Tensor @@ -135,19 +181,49 @@ class HandOver(AtomicAction[GraspGoal, HandOverOptions]): skill_id: ClassVar[str] = "hand_over" GoalType: ClassVar[type] = GraspGoal OptionsType: ClassVar[type] = HandOverOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("source", "destination") - end_effector_roles: ClassVar[tuple[str, ...]] = ("source", "destination") + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + make_manipulation_slot( + "source", + motion_capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } + ), + grasp_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, + ), + make_manipulation_slot( + "destination", + motion_capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + grasp_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, + ), + ), + constraints=(DisjointResourceSlots(("source", "destination")),), + ) + _repeat_qpos = staticmethod(repeat_qpos) - def __init__( + def _scene_dependencies( self, - default_options: HandOverOptions | None = None, - ) -> None: - super().__init__(default_options) - - def _on_bind(self) -> None: - """Resolve engine-wide resources from the owning engine.""" - self.n_envs = self.robot.get_qpos().shape[0] - self.robot_dof = self.robot.dof + request: ResolvedActionRequest[GraspGoal, HandOverOptions], + ) -> tuple[str, ...]: + """Return scene entities referenced by late-bound handover targets.""" + return collect_scene_dependencies( + tuple( + target + for target in ( + request.skill_options.middle_object_pose, + request.skill_options.final_object_pose, + ) + if target is not None + ) + ) def _resolve_resources( self, @@ -155,46 +231,67 @@ def _resolve_resources( ) -> _HandOverResources: """Resolve source/destination roles from robot control parts.""" binding = request.binding - transfer_arm = binding.manipulator("source") - receive_arm = binding.manipulator("destination") - transfer_hand = binding.end_effector("source") - receive_hand = binding.end_effector("destination") - if transfer_arm.name == receive_arm.name: + transfer_motion = binding.endpoint("source", "motion") + receive_motion = binding.endpoint("destination", "motion") + transfer_grasp = binding.endpoint("source", "grasp") + receive_grasp = binding.endpoint("destination", "grasp") + transfer_arm = transfer_motion.require_target(JointPositionTarget) + receive_arm = receive_motion.require_target(JointPositionTarget) + transfer_hand = transfer_grasp.require_target(JointPositionTarget) + receive_hand = receive_grasp.require_target(JointPositionTarget) + transfer_task_state_key = require_shared_task_state_key( + transfer_motion, + transfer_grasp, + participant="HandOver source participant", + ) + receive_task_state_key = require_shared_task_state_key( + receive_motion, + receive_grasp, + participant="HandOver destination participant", + ) + if transfer_task_state_key == receive_task_state_key: + raise ValueError( + "HandOver source and destination must use different " + "task_state_key values." + ) + if transfer_arm.control_part == receive_arm.control_part: raise ValueError( "HandOver source and destination must use different manipulator " "control parts." ) - if transfer_hand.name == receive_hand.name: + if transfer_hand.control_part == receive_hand.control_part: raise ValueError( "HandOver source and destination must use different end-effector " "control parts." ) return _HandOverResources( + transfer_task_state_key=transfer_task_state_key, + receive_task_state_key=receive_task_state_key, transfer_arm=transfer_arm, receive_arm=receive_arm, transfer_hand=transfer_hand, receive_hand=receive_hand, - transfer_hand_open_qpos=transfer_hand.joint_positions( + transfer_hand_open_qpos=transfer_grasp.joint_positions( OPEN_COMMAND, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, dtype=torch.float32, ), - transfer_hand_close_qpos=transfer_hand.joint_positions( + transfer_hand_close_qpos=transfer_grasp.joint_positions( GRASP_COMMAND, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, dtype=torch.float32, ), - receive_hand_open_qpos=receive_hand.joint_positions( + receive_hand_open_qpos=receive_grasp.joint_positions( OPEN_COMMAND, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, dtype=torch.float32, ), - receive_hand_close_qpos=receive_hand.joint_positions( + receive_hand_close_qpos=receive_grasp.joint_positions( GRASP_COMMAND, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, dtype=torch.float32, ), @@ -210,7 +307,7 @@ def _plan( context: PlanningContext, ) -> ActionPlan: """Plan a handover without committing the attachment transfer.""" - target = self.require_goal(request) + target = request.goal options = request.skill_options self._validate_pose_options(options) resources = self._resolve_resources(request) @@ -224,15 +321,40 @@ def _plan( state = context semantics = target.semantics transfer_object_to_eef = self._resolve_transfer_object_to_eef( - state, resources.transfer_arm.name + state, + resources.transfer_task_state_key, + semantics, + ) + eligible = context.task.exclusive_held_object_mask( + resources.transfer_task_state_key + ) + if not eligible.any(): + return self.failed_plan( + request, + context, + message="Source object must be held exclusively.", + ) + transfer_start_qpos, receive_start_qpos = self._resolve_start_qpos( + state, + resources, ) assert options.middle_object_pose is not None assert options.final_object_pose is not None middle_object_pose = self._resolve_matrix( - options.middle_object_pose, "middle_object_pose" + resolve_pose_goal( + options.middle_object_pose, + context, + name="middle_object_pose", + ), + "middle_object_pose", ) final_object_pose = self._resolve_matrix( - options.final_object_pose, "final_object_pose" + resolve_pose_goal( + options.final_object_pose, + context, + name="final_object_pose", + ), + "final_object_pose", ) receive_approach_direction = options.receive_approach_direction.to( device=self.device, dtype=torch.float32 @@ -241,8 +363,17 @@ def _plan( receive_approach_direction / torch.linalg.vector_norm(receive_approach_direction) ) - # force object pose to have the same rotation as the current object pose, so that the handover is feasible. - current_object_pose = target.semantics.entity.get_local_pose(to_matrix=True) + # Keep the requested object orientation consistent with the verified + # attachment and the transferring arm's current measured pose. + transfer_current_eef = self.robot.compute_fk( + qpos=transfer_start_qpos, + name=resources.transfer_arm.control_part, + to_matrix=True, + ) + current_object_pose = torch.bmm( + transfer_current_eef, + pose_inv(transfer_object_to_eef), + ) middle_object_pose[:, :3, :3] = current_object_pose[:, :3, :3] final_object_pose[:, :3, :3] = current_object_pose[:, :3, :3] @@ -258,10 +389,11 @@ def _plan( ) success_mask = normalize_success_mask( grasp_success, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, name="Receiving-grasp success", ) + success_mask &= eligible if not success_mask.any(): logger.log_warning("HandOver failed to resolve a receiving grasp pose.") return self.failed_plan(request, context, message="No receiving grasp.") @@ -285,23 +417,22 @@ def _plan( ), ) - transfer_start_qpos, receive_start_qpos = self._resolve_start_qpos( - state, resources - ) segments = self._compute_segment_lengths( request.motion_policy.sample_count, options ) - segment_success, transfer_move_traj = self._plan_named_arm_trajectory( - resources.transfer_arm.name, + segment_success, transfer_move_traj = plan_named_arm_trajectory( + self.motion_generator, + resources.transfer_arm.control_part, transfer_start_qpos, transfer_middle_eef.unsqueeze(1), segments["transfer"], request.motion_policy, + context.control_dt, ) success_mask &= normalize_success_mask( segment_success, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, name="Transfer-move success", ) @@ -309,16 +440,18 @@ def _plan( logger.log_warning("HandOver failed to plan the transfer move.") return self.failed_plan(request, context, message="Transfer move failed.") - segment_success, receive_approach_traj = self._plan_named_arm_trajectory( - resources.receive_arm.name, + segment_success, receive_approach_traj = plan_named_arm_trajectory( + self.motion_generator, + resources.receive_arm.control_part, receive_start_qpos, torch.stack([receive_pre_grasp_eef, receive_grasp_xpos], dim=1), segments["approach"], request.motion_policy, + context.control_dt, ) success_mask &= normalize_success_mask( segment_success, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, name="Receiving-approach success", ) @@ -331,16 +464,18 @@ def _plan( transfer_hold_qpos = transfer_move_traj[:, -1] receive_grasp_qpos = receive_approach_traj[:, -1] - segment_success, transfer_retreat_traj = self._plan_named_arm_trajectory( - resources.transfer_arm.name, + segment_success, transfer_retreat_traj = plan_named_arm_trajectory( + self.motion_generator, + resources.transfer_arm.control_part, transfer_hold_qpos, transfer_retreat_eef.unsqueeze(1), segments["deliver"], request.motion_policy, + context.control_dt, ) success_mask &= normalize_success_mask( segment_success, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, name="Transfer-retreat success", ) @@ -350,16 +485,18 @@ def _plan( request, context, message="Transfer retreat failed." ) - segment_success, receive_deliver_traj = self._plan_named_arm_trajectory( - resources.receive_arm.name, + segment_success, receive_deliver_traj = plan_named_arm_trajectory( + self.motion_generator, + resources.receive_arm.control_part, receive_grasp_qpos, receive_final_eef.unsqueeze(1), segments["deliver"], request.motion_policy, + context.control_dt, ) success_mask &= normalize_success_mask( segment_success, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, name="Receiving-delivery success", ) @@ -484,11 +621,15 @@ def _plan( request, context, success=success_mask, - trajectory=full, + trajectory=TimedTrajectory.from_uniform_step( + full, + env_ids=context.env_ids, + step_dt=context.require_control_dt(), + ), expected_effects=StateDelta( held_object_updates={ - resources.transfer_arm.name: None, - resources.receive_arm.name: held_object, + resources.transfer_task_state_key: None, + resources.receive_task_state_key: held_object, } ), segment_lengths=segment_lengths, @@ -502,33 +643,32 @@ def _plan( def _validate_pose_options(options: HandOverOptions) -> None: for name in ("middle_object_pose", "final_object_pose"): if getattr(options, name) is None: - logger.log_error( - f"{name} must be specified in HandOverOptions", ValueError - ) + raise ValueError(f"{name} must be specified in HandOverOptions") def _resolve_matrix(self, matrix: torch.Tensor, name: str) -> torch.Tensor: - matrix = matrix.to(device=self.device, dtype=torch.float32) - if matrix.shape == (4, 4): - matrix = matrix.unsqueeze(0).repeat(self.n_envs, 1, 1) - if matrix.shape != (self.n_envs, 4, 4): - logger.log_error( - f"{name} must have shape (4, 4) or ({self.n_envs}, 4, 4), " - f"but got {matrix.shape}", - ValueError, - ) - return matrix + return resolve_batched_pose( + matrix, + num_envs=self.num_envs, + device=self.device, + name=name, + ) def _resolve_transfer_object_to_eef( self, state: PlanningContext, - transfer_control_part: str, + transfer_task_state_key: str, + target_semantics: ObjectSemantics, ) -> torch.Tensor: - held = state.get_held_object(transfer_control_part) + held = state.get_held_object(transfer_task_state_key) if held is None: - logger.log_error( - "HandOver requires an object held by transfer control part " - f"{transfer_control_part!r} (run PickUp first).", - ValueError, + raise ValueError( + "HandOver requires an object held by source task-state resource " + f"{transfer_task_state_key!r} (run PickUp first)." + ) + if not _same_object_identity(target_semantics, held.semantics): + raise ValueError( + "HandOver target semantics must identify the object held by " + f"source task-state resource {transfer_task_state_key!r}." ) return self._resolve_matrix(held.object_to_eef, "held_object.object_to_eef") @@ -545,14 +685,14 @@ def _resolve_receive_grasp( approach_direction=approach_direction, object_part=object_part, ) - n_envs = object_pose.shape[0] + num_envs = object_pose.shape[0] grasp_xpos = ( torch.eye(4, device=self.device, dtype=torch.float32) .unsqueeze(0) - .repeat(n_envs, 1, 1) + .repeat(num_envs, 1, 1) ) - is_success = torch.ones(n_envs, dtype=torch.bool, device=self.device) - for i in range(n_envs): + is_success = torch.ones(num_envs, dtype=torch.bool, device=self.device) + for i in range(num_envs): poses, costs = grasp_poses_result[i] poses = poses.to(device=self.device, dtype=torch.float32) costs = costs.to(device=self.device, dtype=torch.float32) @@ -570,11 +710,11 @@ def _resolve_start_qpos( state: PlanningContext, resources: _HandOverResources, ) -> tuple[torch.Tensor, torch.Tensor]: - if state.last_qpos.shape != (self.n_envs, self.robot_dof): - logger.log_error( + if state.last_qpos.shape != (self.num_envs, self.robot_dof): + raise ValueError( f"PlanningContext.last_qpos must have shape " - f"({self.n_envs}, {self.robot_dof}), but got {state.last_qpos.shape}", - ValueError, + f"({self.num_envs}, {self.robot_dof}), but got " + f"{state.last_qpos.shape}" ) start_qpos = state.last_qpos.to(device=self.device, dtype=torch.float32) return ( @@ -594,10 +734,9 @@ def _compute_segment_lengths( n_transfer = max(2, (sample_count - reserved) // 2) n_approach = sample_count - reserved - n_transfer if n_approach < 2: - logger.log_error( + raise ValueError( "Not enough waypoints for handover. Increase sample_count or " - "decrease hand_interp_steps/hold_steps/retreat_steps.", - ValueError, + "decrease hand_interp_steps/hold_steps/retreat_steps." ) return { "transfer": n_transfer, @@ -612,30 +751,6 @@ def _compute_segment_lengths( # Planning / assembly helpers # ------------------------------------------------------------------ - def _plan_named_arm_trajectory( - self, - control_part: str, - start_qpos: torch.Tensor, - target_poses: torch.Tensor, - n_waypoints: int, - motion_policy: MotionPolicy, - ) -> tuple[torch.Tensor, torch.Tensor]: - result = self.motion_generator.generate( - build_pose_plan_states(target_poses), - options=motion_policy.to_motion_gen_options( - start_qpos=start_qpos, - control_part=control_part, - sample_count=n_waypoints, - ), - ) - assert isinstance(result.success, torch.Tensor) - assert result.positions is not None - return result.success, result.positions - - @staticmethod - def _repeat_qpos(qpos: torch.Tensor, n_waypoints: int) -> torch.Tensor: - return qpos.unsqueeze(1).repeat(1, n_waypoints, 1) - def _assemble_segment( self, state: PlanningContext, @@ -646,18 +761,15 @@ def _assemble_segment( *, resources: _HandOverResources, ) -> torch.Tensor: - n_waypoints = transfer_arm_traj.shape[1] - base = ( - state.last_qpos.to(device=self.device, dtype=torch.float32) - .unsqueeze(1) - .repeat(1, n_waypoints, 1) - .clone() - ) - base[:, :, list(resources.transfer_arm.joint_ids)] = transfer_arm_traj - base[:, :, list(resources.receive_arm.joint_ids)] = receive_arm_traj - base[:, :, list(resources.transfer_hand.joint_ids)] = transfer_hand_traj - base[:, :, list(resources.receive_hand.joint_ids)] = receive_hand_traj - return base + return assemble_full_robot_trajectory( + state.last_qpos, + ( + (resources.transfer_arm.joint_ids, transfer_arm_traj), + (resources.receive_arm.joint_ids, receive_arm_traj), + (resources.transfer_hand.joint_ids, transfer_hand_traj), + (resources.receive_hand.joint_ids, receive_hand_traj), + ), + ) __all__ = ["HandOver", "HandOverOptions"] diff --git a/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py b/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py index d5ae8f76a..c0a787241 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py @@ -23,24 +23,37 @@ import torch -from ..core import AtomicAction -from ..goals import PoseGoalValue, resolve_pose_goal, validate_pose_goal -from ..invocation import ActionOptions, ResolvedActionRequest -from ..plans import ActionPlan -from ..state import PlanningContext -from ..trajectory_ops import ( +from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget +from embodichain.lab.sim.atomic_actions.core import AtomicAction +from embodichain.lab.sim.atomic_actions.goals import ( + PoseGoalValue, + resolve_pose_goal, + validate_pose_goal, +) +from embodichain.lab.sim.atomic_actions.invocation import ( + ActionOptions, + ResolvedActionRequest, +) +from embodichain.lab.sim.atomic_actions.plans import ActionPlan +from embodichain.lab.sim.atomic_actions.requirements import ( + CARTESIAN_POSE_CAPABILITY, + SkillBindingContract, +) +from embodichain.lab.sim.atomic_actions.state import PlanningContext +from embodichain.lab.sim.atomic_actions.trajectory_ops import ( build_pose_plan_states, resolve_pose_target, to_full_robot_trajectory, ) +from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( + make_motion_slot, +) @dataclass(frozen=True, slots=True, eq=False) class EndEffectorPoseGoal: """End-effector pose goal with optional batched intermediate waypoints.""" - goal_kind: ClassVar[str] = "end_effector_pose" - xpos: PoseGoalValue """Homogeneous pose with shape ``(4,4)``, ``(B,4,4)`` or ``(B,N,4,4)``.""" @@ -58,14 +71,15 @@ class MoveEndEffector(AtomicAction[EndEffectorPoseGoal, MoveEndEffectorOptions]) skill_id: ClassVar[str] = "move_end_effector" GoalType: ClassVar[type] = EndEffectorPoseGoal + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + make_motion_slot( + "primary", + capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + ), + ), + ) OptionsType: ClassVar[type] = MoveEndEffectorOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) - - def __init__( - self, - default_options: MoveEndEffectorOptions | None = None, - ) -> None: - super().__init__(default_options) def _plan( self, @@ -73,13 +87,15 @@ def _plan( context: PlanningContext, ) -> ActionPlan: """Plan an end-effector pose goal from the observed joint state.""" - goal = self.require_goal(request) - manipulator = request.binding.manipulator("primary") - control_part = manipulator.name - joint_ids = list(manipulator.joint_ids) + goal = request.goal + motion_target = request.binding.endpoint("primary", "motion").require_target( + JointPositionTarget + ) + control_part = motion_target.control_part + joint_ids = list(motion_target.joint_ids) move_xpos = resolve_pose_target( resolve_pose_goal(goal.xpos, context, name="xpos"), - n_envs=context.batch_size, + num_envs=context.batch_size, device=self.device, ) start_qpos = context.robot.qpos[:, joint_ids] @@ -88,6 +104,7 @@ def _plan( options=request.motion_policy.to_motion_gen_options( start_qpos=start_qpos, control_part=control_part, + interpolation_dt=context.control_dt, ), ) success, trajectory = to_full_robot_trajectory( @@ -95,7 +112,6 @@ def _plan( base_qpos=context.robot.qpos, joint_ids=joint_ids, env_ids=context.env_ids, - control_dt=request.motion_policy.control_dt, ) return self.build_plan( request, diff --git a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py index 6bf8743b5..44e388357 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py @@ -23,15 +23,33 @@ import torch -from embodichain.utils import logger -from embodichain.utils.math import axis_angle_to_rotation_matrix, get_relative_rotation +from embodichain.utils.math import ( + axis_angle_to_rotation_matrix, + get_relative_rotation, + pose_inv, +) -from ._helpers import arm_qpos_from_state, resolve_object_target -from ..control import GRASP_COMMAND +from ._helpers import ( + arm_qpos_from_state, + require_shared_task_state_key, + resolve_object_target, +) +from ._binding_contracts import make_manipulation_slot +from ..bindings import JointPositionTarget +from ..control import GRASP_COMMAND, JointPositionCommand from ..core import AtomicAction from ..goals import PoseGoalValue, resolve_pose_goal, validate_pose_goal from ..invocation import ActionOptions, ResolvedActionRequest -from ..plans import ActionPlan +from ..plans import ActionPlan, TimedTrajectory +from ..requirements import ( + CARTESIAN_POSE_CAPABILITY, + DisjointSlotEndpoints, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) from ..state import PlanningContext from ..trajectory_ops import build_pose_plan_states @@ -40,10 +58,8 @@ class HeldObjectPoseGoal: """Desired pose for the object held by this action's control part.""" - goal_kind: ClassVar[str] = "held_object_pose" - object_target_pose: PoseGoalValue - """Target object pose, shape ``(4, 4)`` or ``(n_envs, 4, 4)``.""" + """Target object pose, shape ``(4, 4)`` or ``(num_envs, 4, 4)``.""" def __post_init__(self) -> None: validate_pose_goal( @@ -83,19 +99,20 @@ class MoveHeldObject(AtomicAction[HeldObjectPoseGoal, MoveHeldObjectOptions]): skill_id: ClassVar[str] = "move_held_object" GoalType: ClassVar[type] = HeldObjectPoseGoal OptionsType: ClassVar[type] = MoveHeldObjectOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) - end_effector_roles: ClassVar[tuple[str, ...]] = ("primary",) - - def __init__( - self, - default_options: MoveHeldObjectOptions | None = None, - ) -> None: - super().__init__(default_options) - - def _on_bind(self) -> None: - """Resolve engine-wide resources from the owning engine.""" - self.n_envs = self.robot.get_qpos().shape[0] - self.robot_dof = self.robot.dof + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + make_manipulation_slot( + "primary", + motion_capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } + ), + grasp_commands={GRASP_COMMAND: JointPositionCommand}, + ), + ), + ) def _plan( self, @@ -103,27 +120,40 @@ def _plan( context: PlanningContext, ) -> ActionPlan: """Plan held-object transport without changing the attachment relation.""" - target = self.require_goal(request) + target = request.goal options = request.skill_options binding = request.binding - manipulator = binding.manipulator() - end_effector = binding.end_effector() - control_part = manipulator.name - arm_joint_ids = list(manipulator.joint_ids) - hand_joint_ids = list(end_effector.joint_ids) - hand_grasp_qpos = end_effector.joint_positions( + motion = binding.endpoint("primary", "motion") + grasp = binding.endpoint("primary", "grasp") + motion_target = motion.require_target(JointPositionTarget) + grasp_target = grasp.require_target(JointPositionTarget) + task_state_key = require_shared_task_state_key( + motion, + grasp, + participant="MoveHeldObject primary participant", + ) + control_part = motion_target.control_part + arm_joint_ids = list(motion_target.joint_ids) + hand_joint_ids = list(grasp_target.joint_ids) + hand_grasp_qpos = grasp.joint_positions( GRASP_COMMAND, - n_envs=context.batch_size, + num_envs=context.batch_size, device=self.device, dtype=context.robot.qpos.dtype, ) state = context - held_object = state.get_held_object(control_part) + held_object = state.get_held_object(task_state_key) if held_object is None: - logger.log_error( - "MoveHeldObject requires an object held by control part " - f"{control_part!r} - run PickUp first.", - ValueError, + raise ValueError( + "MoveHeldObject requires an object held by task-state resource " + f"{task_state_key!r} - run PickUp first." + ) + eligible = context.task.exclusive_held_object_mask(task_state_key) + if not eligible.any(): + return self.failed_plan( + request, + context, + message="Held object is not exclusive to the task-state resource.", ) object_target_pose = resolve_object_target( resolve_pose_goal( @@ -131,25 +161,26 @@ def _plan( context, name="object_target_pose", ), - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, ) start_arm_qpos = arm_qpos_from_state(state, arm_joint_ids) end_arm_xpos = self.robot.compute_fk( start_arm_qpos, name=control_part, to_matrix=True ) + object_to_eef = held_object.object_to_eef.to( + device=self.device, dtype=torch.float32 + ) + if object_to_eef.shape == (4, 4): + object_to_eef = object_to_eef.unsqueeze(0).repeat(self.num_envs, 1, 1) + current_object_pose = torch.bmm(end_arm_xpos, pose_inv(object_to_eef)) if options.pick_rotate_upright is not None: self._apply_configured_upright_rotation( object_target_pose, end_arm_xpos, - held_object.semantics.entity.get_local_pose(to_matrix=True), + current_object_pose, options, ) - object_to_eef = held_object.object_to_eef.to( - device=self.device, dtype=torch.float32 - ) - if object_to_eef.shape == (4, 4): - object_to_eef = object_to_eef.unsqueeze(0).repeat(self.n_envs, 1, 1) move_eef_xpos = torch.bmm(object_target_pose, object_to_eef) if options.pick_rotate_upright is None: @@ -160,27 +191,33 @@ def _plan( options=request.motion_policy.to_motion_gen_options( start_qpos=start_arm_qpos, control_part=control_part, + interpolation_dt=context.control_dt, ), ) assert isinstance(result.success, torch.Tensor) assert result.positions is not None - success = result.success + success = result.success & eligible arm_traj = result.positions full = torch.empty( - (self.n_envs, arm_traj.shape[1], self.robot_dof), + (self.num_envs, arm_traj.shape[1], self.robot_dof), dtype=torch.float32, device=self.device, ) full[:, :, :] = state.last_qpos.unsqueeze(1) full[:, :, arm_joint_ids] = arm_traj full[:, :, hand_joint_ids] = hand_grasp_qpos.unsqueeze(1) + assert result.dt is not None return self.build_plan( request, context, success=success, - trajectory=full, + trajectory=TimedTrajectory.from_positions( + full, + env_ids=context.env_ids, + dt=result.dt, + ), segment_lengths={"transport": full.shape[1]}, ) @@ -228,7 +265,7 @@ def _apply_automatic_transport_rotation( revert_flag = torch.where(end_arm_xpos[:, 2, 1] > 0, 1.0, -1.0) rotation_axis = torch.tensor( [1.0, 0.0, 0.0], device=self.device, dtype=torch.float32 - ).repeat(self.n_envs, 1) + ).repeat(self.num_envs, 1) axis_angle = ( (torch.pi * 0.5 - arm_dot_angle).unsqueeze(-1) * rotation_axis @@ -239,12 +276,12 @@ def _apply_automatic_transport_rotation( [[1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, -1.0]], device=self.device, dtype=torch.float32, - ).repeat(self.n_envs, 1, 1) + ).repeat(self.num_envs, 1, 1) template_rotation_b = torch.tensor( [[-1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, -1.0]], device=self.device, dtype=torch.float32, - ).repeat(self.n_envs, 1, 1) + ).repeat(self.num_envs, 1, 1) target_rotation_a = torch.bmm(template_rotation_a, rotation_offset) target_rotation_b = torch.bmm(template_rotation_b, rotation_offset) relative_rotation_a = get_relative_rotation( diff --git a/embodichain/lab/sim/atomic_actions/primitives/move_joints.py b/embodichain/lab/sim/atomic_actions/primitives/move_joints.py index c2693d091..fe2d0e4ea 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_joints.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_joints.py @@ -23,23 +23,32 @@ import torch -from ..core import AtomicAction -from ..invocation import ActionOptions, ResolvedActionRequest -from ..plans import ActionPlan -from ..state import PlanningContext -from ..trajectory_ops import ( +from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget +from embodichain.lab.sim.atomic_actions.core import AtomicAction +from embodichain.lab.sim.atomic_actions.invocation import ( + ActionOptions, + ResolvedActionRequest, +) +from embodichain.lab.sim.atomic_actions.plans import ActionPlan +from embodichain.lab.sim.atomic_actions.requirements import ( + JOINT_POSITION_CAPABILITY, + SkillBindingContract, +) +from embodichain.lab.sim.atomic_actions.state import PlanningContext +from embodichain.lab.sim.atomic_actions.trajectory_ops import ( build_joint_plan_states, resolve_joint_target, to_full_robot_trajectory, ) +from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( + make_motion_slot, +) @dataclass(frozen=True, slots=True, eq=False) class JointPositionGoal: """Explicit or named joint-space goal for a bound robot resource.""" - goal_kind: ClassVar[str] = "joint_position" - target: torch.Tensor | str """Joint qpos/waypoints or a named control-part profile command.""" @@ -56,8 +65,8 @@ def __post_init__(self) -> None: if self.target.dim() not in (1, 2, 3) or self.target.shape[-1] == 0: raise ValueError( "Tensor target must have shape (control_dof,), " - "(n_envs, control_dof), " - "or (n_envs, n_waypoint, control_dof), " + "(num_envs, control_dof), " + "or (num_envs, n_waypoint, control_dof), " f"got {tuple(self.target.shape)}." ) @@ -73,14 +82,15 @@ class MoveJoints(AtomicAction[JointPositionGoal, MoveJointsOptions]): skill_id: ClassVar[str] = "move_joints" GoalType: ClassVar[type] = JointPositionGoal OptionsType: ClassVar[type] = MoveJointsOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) agent_visible: ClassVar[bool] = False - - def __init__( - self, - default_options: MoveJointsOptions | None = None, - ) -> None: - super().__init__(default_options) + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + make_motion_slot( + "primary", + capabilities=frozenset({JOINT_POSITION_CAPABILITY}), + ), + ), + ) def _plan( self, @@ -88,18 +98,19 @@ def _plan( context: PlanningContext, ) -> ActionPlan: """Plan a joint-space goal without mutating the robot or task state.""" - goal = self.require_goal(request) - manipulator = request.binding.manipulator("primary") - control_part = manipulator.name - joint_ids = list(manipulator.joint_ids) - joint_dof = manipulator.dof + goal = request.goal + motion = request.binding.endpoint("primary", "motion") + motion_target = motion.require_target(JointPositionTarget) + control_part = motion_target.control_part + joint_ids = list(motion_target.joint_ids) + joint_dof = len(motion_target.joint_ids) target_qpos = resolve_joint_target( self._resolve_target_qpos( goal, request=request, context=context, ), - n_envs=context.batch_size, + num_envs=context.batch_size, joint_dof=joint_dof, control_part=control_part, device=self.device, @@ -110,6 +121,7 @@ def _plan( options=request.motion_policy.to_motion_gen_options( start_qpos=start_qpos, control_part=control_part, + interpolation_dt=context.control_dt, ), ) success, trajectory = to_full_robot_trajectory( @@ -117,7 +129,6 @@ def _plan( base_qpos=context.robot.qpos, joint_ids=joint_ids, env_ids=context.env_ids, - control_dt=request.motion_policy.control_dt, ) return self.build_plan( request, @@ -136,9 +147,9 @@ def _resolve_target_qpos( """Resolve an explicit or named joint goal to a tensor.""" if isinstance(goal.target, torch.Tensor): return goal.target - return request.binding.manipulator("primary").joint_positions( + return request.binding.endpoint("primary", "motion").joint_positions( goal.target, - n_envs=context.batch_size, + num_envs=context.batch_size, device=self.device, dtype=context.robot.qpos.dtype, ) diff --git a/embodichain/lab/sim/atomic_actions/primitives/operate_articulation.py b/embodichain/lab/sim/atomic_actions/primitives/operate_articulation.py new file mode 100644 index 000000000..ec00958f7 --- /dev/null +++ b/embodichain/lab/sim/atomic_actions/primitives/operate_articulation.py @@ -0,0 +1,478 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Reusable contact-and-drag operation for articulated mechanisms.""" + +from __future__ import annotations + +from dataclasses import dataclass +import math +from typing import ClassVar + +import torch + +from ..bindings import JointPositionTarget +from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand +from ..core import AtomicAction +from ..effects import StateDelta +from ..goals import SceneArticulationOperationGeometry +from ..invocation import ActionOptions, ResolvedActionRequest +from ..plans import ( + ActionPlan, + EffectVerificationRequirement, + PlannerDiagnostics, + TimedTrajectory, +) +from ..requirements import ( + CARTESIAN_POSE_CAPABILITY, + DisjointSlotEndpoints, + GRASP_CAPABILITY, + JOINT_POSITION_CAPABILITY, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) +from ..state import ArticulationJointState, PlanningContext +from ..trajectory_ops import ( + build_pose_plan_states, + interpolate_hand_qpos, + resolve_pose_target, +) + + +def _validate_identifier(value: str, *, field_name: str) -> None: + """Validate one canonical articulation identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError(f"{field_name} must be a non-empty canonical identifier.") + + +@dataclass(frozen=True, slots=True, eq=False) +class OperateArticulationGoal: + """Grounded interaction path and desired state for one articulation joint. + + The semantic compiler copies immutable affordance geometry and records the + live source joint position. The atomic planner combines those values with + the latest handle and joint observation, so the same resolved request can + safely replan drawers, doors, sliders, and similar interactions. + """ + + goal_kind: ClassVar[str] = "operate_articulation" + + articulation_id: str + """Canonical scene-registry articulation identifier.""" + + joint_id: str + """Canonical joint identifier within the articulation.""" + + geometry: SceneArticulationOperationGeometry + """Handle-relative geometry resolved again for every plan and replan.""" + + source_position: torch.Tensor + """Live joint position at semantic grounding, shape ``(1,)`` or ``(B, 1)``.""" + + target_position: torch.Tensor + """Absolute desired joint position, shape ``(1,)`` or ``(B, 1)``.""" + + target_displacement: float + """Signed handle displacement from source position to target position.""" + + def __post_init__(self) -> None: + _validate_identifier(self.articulation_id, field_name="articulation_id") + _validate_identifier(self.joint_id, field_name="joint_id") + if not isinstance(self.geometry, SceneArticulationOperationGeometry): + raise TypeError("geometry must be a SceneArticulationOperationGeometry.") + for field_name in ("source_position", "target_position"): + position = getattr(self, field_name) + if not isinstance(position, torch.Tensor): + raise TypeError(f"{field_name} must be a torch.Tensor.") + if position.dim() not in (1, 2) or position.shape[-1:] != (1,): + raise ValueError(f"{field_name} must have shape (1,) or (B, 1).") + if not position.is_floating_point(): + raise TypeError(f"{field_name} must use a floating-point dtype.") + if not torch.isfinite(position).all(): + raise ValueError(f"{field_name} must contain only finite values.") + object.__setattr__(self, field_name, position.clone()) + displacement = self.target_displacement + if isinstance(displacement, bool) or not isinstance(displacement, (int, float)): + raise TypeError("target_displacement must be a finite scalar.") + displacement = float(displacement) + if not math.isfinite(displacement): + raise ValueError("target_displacement must be finite.") + object.__setattr__(self, "target_displacement", displacement) + + +@dataclass(frozen=True, slots=True, eq=False) +class OperateArticulationOptions(ActionOptions): + """Per-invocation contact sequencing for articulation operations.""" + + engage_steps: int = 5 + """Number of gripper-closing waypoints at the contact pose.""" + + release_steps: int = 5 + """Number of gripper-opening waypoints before retracting.""" + + def __post_init__(self) -> None: + for field_name in ("engage_steps", "release_steps"): + value = getattr(self, field_name) + if type(value) is not int or value <= 0: + raise ValueError(f"{field_name} must be a positive integer.") + + +class OperateArticulation( + AtomicAction[OperateArticulationGoal, OperateArticulationOptions] +): + """Approach, engage, move, release, and retract an articulated affordance.""" + + skill_id: ClassVar[str] = "operate_articulation" + GoalType: ClassVar[type] = OperateArticulationGoal + OptionsType: ClassVar[type] = OperateArticulationOptions + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + JOINT_POSITION_CAPABILITY, + } + ), + ), + SkillEndpointRequirement( + endpoint_id="interaction", + capabilities=frozenset({GRASP_CAPABILITY}), + required_commands={ + GRASP_COMMAND: JointPositionCommand, + OPEN_COMMAND: JointPositionCommand, + }, + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "interaction")),), + ), + ), + ) + + def __init__( + self, + default_options: OperateArticulationOptions | None = None, + ) -> None: + super().__init__(default_options) + + def _on_bind(self) -> None: + """Capture immutable robot dimensions from engine-owned services.""" + self.n_envs = int(self.robot.get_qpos().shape[0]) + self.robot_dof = int(self.robot.dof) + + def _plan( + self, + request: ResolvedActionRequest[ + OperateArticulationGoal, + OperateArticulationOptions, + ], + context: PlanningContext, + ) -> ActionPlan: + """Plan the complete contact interaction from one observed context.""" + goal = self.require_goal(request) + options = request.skill_options + motion = request.binding.endpoint("primary", "motion").require_target( + JointPositionTarget + ) + interaction = request.binding.endpoint("primary", "interaction") + interaction_target = interaction.require_target(JointPositionTarget) + arm_joint_ids = list(motion.joint_ids) + interaction_joint_ids = list(interaction_target.joint_ids) + + remaining_displacement = self._remaining_displacement(goal, context) + poses = tuple( + resolve_pose_target( + pose, + num_envs=context.batch_size, + device=self.device, + ) + for pose in goal.geometry.resolve( + context, + displacement=remaining_displacement, + ) + ) + motion_counts = self._motion_sample_counts( + request.motion_policy.sample_count, + options, + ) + arm_segments: list[torch.Tensor] = [] + phase_diagnostics: dict[str, dict[str, object]] = {} + arm_start = context.robot.qpos[:, arm_joint_ids] + success = torch.ones( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ) + phase_names = ("approach", "contact", "operate", "retract") + for phase_name, pose, sample_count in zip( + phase_names, + poses, + motion_counts, + strict=True, + ): + result = self.motion_generator.generate( + build_pose_plan_states(pose), + options=request.motion_policy.to_motion_gen_options( + start_qpos=arm_start, + control_part=motion.control_part, + sample_count=sample_count, + interpolation_dt=context.require_control_dt(), + ), + ) + if result.positions is None or not isinstance(result.success, torch.Tensor): + return self.failed_plan( + request, + context, + message=( + "The articulation motion planner returned no trajectory for " + f"phase {phase_name!r}." + ), + ) + phase_success = result.success.to( + device=success.device, + dtype=torch.bool, + ) + failed_rows = ( + (~phase_success).nonzero(as_tuple=False).flatten().detach().cpu() + ) + phase_diagnostics[phase_name] = { + "success": phase_success.detach().cpu().tolist(), + "failed_rows": failed_rows.tolist(), + "waypoint_count": int(result.positions.shape[1]), + } + arm_segments.append(result.positions) + arm_start = result.positions[:, -1] + success &= phase_success + + grasp_qpos = interaction.joint_positions( + GRASP_COMMAND, + num_envs=self.num_envs, + device=self.device, + dtype=context.robot.qpos.dtype, + ) + open_qpos = interaction.joint_positions( + OPEN_COMMAND, + num_envs=self.num_envs, + device=self.device, + dtype=context.robot.qpos.dtype, + ) + initial_interaction = context.robot.qpos[:, interaction_joint_ids] + engage_path = interpolate_hand_qpos( + initial_interaction, + grasp_qpos, + n_waypoints=options.engage_steps, + ) + release_path = interpolate_hand_qpos( + grasp_qpos, + open_qpos, + n_waypoints=options.release_steps, + ) + + approach_arm, contact_arm, operation_arm, retract_arm = arm_segments + lengths = { + "approach": int(approach_arm.shape[1]), + "engage": int(contact_arm.shape[1] + engage_path.shape[1]), + "operate": int(operation_arm.shape[1]), + "release": int(release_path.shape[1]), + "retract": int(retract_arm.shape[1]), + } + full = torch.empty( + ( + context.batch_size, + sum(lengths.values()), + self.robot_dof, + ), + dtype=context.robot.qpos.dtype, + device=context.robot.qpos.device, + ) + full[:] = context.robot.qpos.unsqueeze(1) + cursor = 0 + + def append_motion( + segment: torch.Tensor, + interaction_qpos: torch.Tensor, + ) -> None: + nonlocal cursor + count = int(segment.shape[1]) + full[:, cursor : cursor + count, arm_joint_ids] = segment + full[:, cursor : cursor + count, interaction_joint_ids] = ( + interaction_qpos.unsqueeze(1) + ) + cursor += count + + append_motion(approach_arm, initial_interaction) + append_motion(contact_arm, initial_interaction) + full[:, cursor : cursor + options.engage_steps, arm_joint_ids] = contact_arm[ + :, -1 + ].unsqueeze(1) + full[:, cursor : cursor + options.engage_steps, interaction_joint_ids] = ( + engage_path + ) + cursor += options.engage_steps + append_motion(operation_arm, grasp_qpos) + full[:, cursor : cursor + options.release_steps, arm_joint_ids] = operation_arm[ + :, -1 + ].unsqueeze(1) + full[:, cursor : cursor + options.release_steps, interaction_joint_ids] = ( + release_path + ) + cursor += options.release_steps + append_motion(retract_arm, open_qpos) + assert cursor == full.shape[1] + + target_position = goal.target_position.to( + device=context.robot.qpos.device, + dtype=context.robot.qpos.dtype, + ) + expected = StateDelta( + articulation_joint_updates={ + (goal.articulation_id, goal.joint_id): ArticulationJointState( + target_position + ) + } + ) + return self.build_plan( + request, + context, + success=success, + trajectory=TimedTrajectory.from_uniform_step( + full, + env_ids=context.env_ids, + step_dt=context.require_control_dt(), + ), + expected_effects=expected, + effect_verification=EffectVerificationRequirement( + kind="articulation.joint_progress" + ), + segment_lengths=lengths, + scene_dependency_monitor_until={ + goal.geometry.handle_pose.entity_id: lengths["approach"] + + lengths["engage"] + }, + diagnostics=PlannerDiagnostics( + backend=self.planning_services.planner_name, + messages=tuple( + f"Articulation motion phase {phase_name!r} failed for rows " + f"{details['failed_rows']}." + for phase_name, details in phase_diagnostics.items() + if details["failed_rows"] + ), + metadata={"motion_phases": phase_diagnostics}, + ), + ) + + @staticmethod + def _position_batch( + value: torch.Tensor, + context: PlanningContext, + *, + field_name: str, + ) -> torch.Tensor: + """Broadcast one scalar joint position to the planning batch.""" + position = value.to( + device=context.robot.qpos.device, + dtype=context.robot.qpos.dtype, + ) + if position.shape == (1,): + return position.unsqueeze(0).expand(context.batch_size, -1).clone() + if position.shape != (context.batch_size, 1): + raise ValueError( + f"{field_name} must have shape (1,) or " f"({context.batch_size}, 1)." + ) + return position.clone() + + @classmethod + def _remaining_displacement( + cls, + goal: OperateArticulationGoal, + context: PlanningContext, + ) -> torch.Tensor: + """Map remaining joint stroke to a bounded signed handle displacement. + + For each row, ``remaining = target_displacement * clamp( + (target - current) / (target - source), 0, 1)``. A zero-length source + stroke, a reached target, and an overshot target all resolve to zero. + """ + observed = context.scene.get_articulation_joint_state( + goal.articulation_id, + goal.joint_id, + ) + address = (goal.articulation_id, goal.joint_id) + if observed is None: + raise ValueError( + "OperateArticulation recovery-safe planning requires a live " + f"ObservedArticulationJointState for {address!r}." + ) + current = cls._position_batch( + observed.position, + context, + field_name=f"observed articulation joint {address!r}", + ) + if observed.valid_mask is not None: + valid = observed.valid_mask.to(device=context.robot.qpos.device) + if not bool(valid.all()): + invalid_rows = (~valid).nonzero(as_tuple=False).flatten().tolist() + raise ValueError( + f"Live articulation joint {address!r} is invalid for planning " + f"rows {invalid_rows}." + ) + source = cls._position_batch( + goal.source_position, + context, + field_name="source_position", + ) + target = cls._position_batch( + goal.target_position, + context, + field_name="target_position", + ) + total = target - source + tolerance = torch.finfo(total.dtype).eps * 16.0 + nonzero_stroke = total.abs() > tolerance + fraction = torch.zeros_like(total) + fraction[nonzero_stroke] = ( + (target - current)[nonzero_stroke] / total[nonzero_stroke] + ).clamp(0.0, 1.0) + return fraction[:, 0] * goal.target_displacement + + @staticmethod + def _motion_sample_counts( + sample_count: int, + options: OperateArticulationOptions, + ) -> tuple[int, int, int, int]: + """Allocate the preset sample budget across four motion phases.""" + remaining = sample_count - options.engage_steps - options.release_steps + if remaining < 8: + raise ValueError( + "MotionPolicy.sample_count must leave at least two waypoints for " + "each articulation motion phase." + ) + base, remainder = divmod(remaining, 4) + counts = tuple(base + (1 if index < remainder else 0) for index in range(4)) + assert len(counts) == 4 and all(value >= 2 for value in counts) + return counts + + +__all__ = [ + "OperateArticulation", + "OperateArticulationGoal", + "OperateArticulationOptions", +] diff --git a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py index ed81535c6..8916536c5 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py @@ -19,7 +19,7 @@ from __future__ import annotations import math -from dataclasses import dataclass +from dataclasses import dataclass, replace from typing import ClassVar import torch @@ -32,37 +32,80 @@ quat_from_matrix, ) -from ._helpers import arm_qpos_from_state +from ._helpers import arm_qpos_from_state, require_shared_task_state_key from ..affordance import AntipodalAffordance -from ..bindings import ResolvedControlPart -from ..control import GRASP_COMMAND, OPEN_COMMAND +from ..bindings import JointPositionTarget +from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand from ..core import AtomicAction, ObjectSemantics from ..effects import StateDelta from ..goals import ( ObjectActionGoal, PoseGoalValue, + _resolve_object_pose, + collect_scene_dependencies, resolve_pose_goal, validate_pose_goal, ) -from ..invocation import ActionOptions, ResolvedActionRequest -from ..plans import ActionPlan, normalize_success_mask -from ..policies import MotionPolicy -from ..state import HeldObjectState, PlanningContext -from ..trajectory_ops import ( +from embodichain.lab.sim.atomic_actions.invocation import ( + ActionOptions, + ResolvedActionRequest, +) +from embodichain.lab.sim.atomic_actions.plans import ( + ActionPlan, + TimedTrajectory, + normalize_success_mask, +) +from embodichain.lab.sim.atomic_actions.policies import MotionPolicy +from embodichain.lab.sim.atomic_actions.requirements import ( + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + SkillBindingContract, +) +from embodichain.lab.sim.atomic_actions.state import HeldObjectState, PlanningContext +from embodichain.lab.sim.atomic_actions.trajectory_ops import ( build_pose_plan_states, interpolate_hand_qpos, resolve_pose_target, split_three_segments, translate_pose_world, ) +from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( + make_manipulation_slot, +) + +_UPRIGHT_SIDE_GRASP_MAX_AXIS_ALIGNMENT = 0.65 +_UPRIGHT_SIDE_GRASP_MIN_AXIS_FRACTION = 0.35 +_UPRIGHT_SIDE_GRASP_MAX_AXIS_FRACTION = 0.75 +_UPRIGHT_SIDE_GRASP_HEIGHT_COST_WEIGHT = 2.0 + + +def _upright_yaw_pose_variants( + target_pose: torch.Tensor, + sample_count: int, +) -> torch.Tensor: + """Return object poses with evenly sampled world-Z yaw rotations.""" + signed_steps = [0] + for step in range(1, (sample_count + 1) // 2): + signed_steps.extend((step, -step)) + if sample_count % 2 == 0: + signed_steps.append(sample_count // 2) + angles = target_pose.new_tensor(signed_steps) * (2.0 * math.pi / sample_count) + yaw = target_pose.new_zeros((sample_count, 3, 3)) + yaw[:, 0, 0] = torch.cos(angles) + yaw[:, 0, 1] = -torch.sin(angles) + yaw[:, 1, 0] = torch.sin(angles) + yaw[:, 1, 1] = torch.cos(angles) + yaw[:, 2, 2] = 1.0 + variants = target_pose[:, None].repeat(1, sample_count, 1, 1) + variants[:, :, :3, :3] = torch.matmul(yaw[None], target_pose[:, None, :3, :3]) + return variants @dataclass(frozen=True, slots=True, eq=False) class GraspGoal(ObjectActionGoal): """Pickup target with an affordance-selected or supplied grasp pose.""" - goal_kind: ClassVar[str] = "grasp" - grasp_xpos: PoseGoalValue | None = None """Optional end-effector grasp pose. @@ -101,9 +144,12 @@ class PickUpOptions(ActionOptions): approach_alignment_max_angle: float | None = None """Optional maximum TCP z-axis deviation from the approach direction.""" - downstream_object_target_poses: tuple[torch.Tensor, ...] = () + downstream_object_target_poses: tuple[PoseGoalValue, ...] = () """Future object poses that must be reachable with the selected grasp.""" + upright_yaw_samples: int = 1 + """Equivalent world-yaw samples for semantically upright downstream targets.""" + obj_upright_direction: torch.Tensor | None = None """Optional object local direction used to choose the upright grasp rotation.""" @@ -119,6 +165,8 @@ def __post_init__(self) -> None: raise ValueError("lift_height must be non-negative.") if self.pre_grasp_distance < 0.0: raise ValueError("pre_grasp_distance must be non-negative.") + if self.upright_yaw_samples < 1: + raise ValueError("upright_yaw_samples must be positive.") if self.approach_direction.shape != (3,): raise ValueError("approach_direction must have shape (3,).") if not torch.isfinite(self.approach_direction).all(): @@ -135,10 +183,20 @@ def __post_init__(self) -> None: ): raise ValueError("obj_upright_direction must be a finite (3,) tensor.") object.__setattr__(self, "approach_direction", self.approach_direction.clone()) + downstream_targets: list[PoseGoalValue] = [] + for index, value in enumerate(self.downstream_object_target_poses): + validate_pose_goal( + value, + f"downstream_object_target_poses[{index}]", + allow_waypoints=False, + ) + downstream_targets.append( + value.clone() if isinstance(value, torch.Tensor) else value.snapshot() + ) object.__setattr__( self, "downstream_object_target_poses", - tuple(value.clone() for value in self.downstream_object_target_poses), + tuple(downstream_targets), ) if self.obj_upright_direction is not None: object.__setattr__( @@ -152,19 +210,40 @@ class PickUp(AtomicAction[GraspGoal, PickUpOptions]): skill_id: ClassVar[str] = "pick_up" GoalType: ClassVar[type] = GraspGoal OptionsType: ClassVar[type] = PickUpOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) - end_effector_roles: ClassVar[tuple[str, ...]] = ("primary",) + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + make_manipulation_slot( + "primary", + motion_capabilities=frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } + ), + grasp_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, + ), + ), + ) - def __init__( + def _scene_dependencies( self, - default_options: PickUpOptions | None = None, - ) -> None: - super().__init__(default_options) - - def _on_bind(self) -> None: - """Resolve engine-wide resources from the owning engine.""" - self.n_envs = self.robot.get_qpos().shape[0] - self.robot_dof = self.robot.dof + request: ResolvedActionRequest[GraspGoal, PickUpOptions], + ) -> tuple[str, ...]: + """Include the semantic object when it has a stable scene identity.""" + dependencies = set(super()._scene_dependencies(request)) + entity_id = request.goal.semantics.entity_id + if entity_id is not None: + dependencies.add(entity_id) + dependencies.update( + collect_scene_dependencies( + request.skill_options.downstream_object_target_poses + ) + ) + return tuple(sorted(dependencies)) def _get_full_pickup_trajectory( self, @@ -174,10 +253,11 @@ def _get_full_pickup_trajectory( motion_policy: MotionPolicy, options: PickUpOptions, approach_direction: torch.Tensor, - manipulator: ResolvedControlPart, - end_effector: ResolvedControlPart, + manipulator: JointPositionTarget, + end_effector: JointPositionTarget, hand_open_qpos: torch.Tensor, hand_grasp_qpos: torch.Tensor, + interpolation_dt: float, ) -> tuple[torch.Tensor, torch.Tensor, dict[str, int]]: pre_grasp_xpos = translate_pose_world( grasp_xpos, -approach_direction * options.pre_grasp_distance @@ -194,8 +274,9 @@ def _get_full_pickup_trajectory( build_pose_plan_states(torch.stack([pre_grasp_xpos, grasp_xpos], dim=1)), options=motion_policy.to_motion_gen_options( start_qpos=start_arm_qpos, - control_part=manipulator.name, + control_part=manipulator.control_part, sample_count=n_approach, + interpolation_dt=interpolation_dt, ), ) assert isinstance(approach_result.success, torch.Tensor) @@ -212,8 +293,9 @@ def _get_full_pickup_trajectory( build_pose_plan_states(lift_xpos), options=motion_policy.to_motion_gen_options( start_qpos=grasp_arm_qpos, - control_part=manipulator.name, + control_part=manipulator.control_part, sample_count=n_lift, + interpolation_dt=interpolation_dt, ), ) assert isinstance(lift_result.success, torch.Tensor) @@ -228,7 +310,11 @@ def _get_full_pickup_trajectory( n_approach_actual = approach_arm.shape[1] n_lift_actual = lift_arm.shape[1] full = torch.empty( - (self.n_envs, n_approach_actual + n_close + n_lift_actual, self.robot_dof), + ( + self.num_envs, + n_approach_actual + n_close + n_lift_actual, + self.robot_dof, + ), dtype=torch.float32, device=self.device, ) @@ -264,7 +350,19 @@ def _plan( ) -> ActionPlan: """Plan approach, close, and lift segments without committing attachment.""" target = self.require_goal(request) - options = request.skill_options + options = replace( + request.skill_options, + downstream_object_target_poses=tuple( + resolve_pose_goal( + downstream_target, + context, + name=f"downstream_object_target_poses[{index}]", + ) + for index, downstream_target in enumerate( + request.skill_options.downstream_object_target_poses + ) + ), + ) approach_direction = options.approach_direction.to( device=self.device, dtype=torch.float32 ) @@ -272,33 +370,40 @@ def _plan( approach_direction ) binding = request.binding - manipulator = binding.manipulator() - end_effector = binding.end_effector() - hand_open_qpos = end_effector.joint_positions( + motion = binding.endpoint("primary", "motion") + grasp = binding.endpoint("primary", "grasp") + manipulator = motion.require_target(JointPositionTarget) + end_effector = grasp.require_target(JointPositionTarget) + task_state_key = require_shared_task_state_key( + motion, + grasp, + participant="PickUp primary participant", + ) + hand_open_qpos = grasp.joint_positions( OPEN_COMMAND, - n_envs=context.batch_size, + num_envs=context.batch_size, device=self.device, dtype=context.robot.qpos.dtype, ) - hand_grasp_qpos = end_effector.joint_positions( + hand_grasp_qpos = grasp.joint_positions( GRASP_COMMAND, - n_envs=context.batch_size, + num_envs=context.batch_size, device=self.device, dtype=context.robot.qpos.dtype, ) - control_part = manipulator.name + control_part = manipulator.control_part state = context sem = target.semantics + object_pose = _resolve_object_pose( + sem, + context, + name="pickup_object_pose", + ) if target.grasp_xpos is None and not isinstance( sem.affordance, AntipodalAffordance ): - logger.log_error( - "PickUp requires an AntipodalAffordance when grasp_xpos is not set.", - ValueError, - ) - if sem.entity is None: - logger.log_error( - "PickUp requires an entity on the target semantics.", ValueError + raise ValueError( + "PickUp requires an AntipodalAffordance when grasp_xpos is not set." ) start_arm_qpos = arm_qpos_from_state( state, @@ -306,22 +411,29 @@ def _plan( ) if target.grasp_xpos is None: is_success, grasp_xpos = self._resolve_grasp_pose( - sem, start_arm_qpos, manipulator, options, approach_direction + sem, + object_pose, + start_arm_qpos, + manipulator, + options, + approach_direction, ) else: grasp_xpos = resolve_pose_target( resolve_pose_goal(target.grasp_xpos, context, name="grasp_xpos"), - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, ) if options.rotate_upright is not None: grasp_xpos = self._upright_adjusted_grasp_poses( - sem, grasp_xpos, options + grasp_xpos, + object_pose, + options, ) - is_success = torch.ones(self.n_envs, dtype=torch.bool, device=self.device) + is_success = torch.ones(self.num_envs, dtype=torch.bool, device=self.device) grasp_success = normalize_success_mask( is_success, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, name="Grasp-pose success", ) @@ -342,60 +454,83 @@ def _plan( end_effector, hand_open_qpos, hand_grasp_qpos, + context.require_control_dt(), ) success_mask = grasp_success & normalize_success_mask( trajectory_success, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, name="Pick-up trajectory success", ) - obj_poses = sem.entity.get_local_pose(to_matrix=True) - object_to_eef = torch.bmm(pose_inv(obj_poses), grasp_xpos) + object_to_eef = torch.bmm(pose_inv(object_pose), grasp_xpos) held = HeldObjectState( semantics=sem, object_to_eef=object_to_eef, grasp_xpos=grasp_xpos ) coordinated_updates = { - key: None for key in state.coordinated_held_objects if control_part in key + key: None + for key in state.task.coordinated_held_objects + if task_state_key in key } return self.build_plan( request, context, success=success_mask, - trajectory=full, + trajectory=TimedTrajectory.from_uniform_step( + full, + env_ids=context.env_ids, + step_dt=context.require_control_dt(), + ), expected_effects=StateDelta( - held_object_updates={control_part: held}, + held_object_updates={task_state_key: held}, coordinated_held_object_updates=coordinated_updates, ), segment_lengths=segment_lengths, + scene_dependency_monitor_until=( + {} + if sem.entity_id is None + else {sem.entity_id: segment_lengths["approach"]} + ), ) def _resolve_grasp_pose( self, semantics: ObjectSemantics, + object_pose: torch.Tensor, start_qpos: torch.Tensor, - manipulator: ResolvedControlPart, + manipulator: JointPositionTarget, options: PickUpOptions, approach_direction: torch.Tensor, ) -> tuple[torch.Tensor, torch.Tensor]: - obj_poses = semantics.entity.get_local_pose(to_matrix=True) + grasp_cost_fn = None + if options.rotate_upright is not None: + grasp_cost_fn = lambda object_pose, grasp_poses, costs: ( + self._upright_grasp_costs( + semantics, + object_pose, + grasp_poses, + costs, + options, + ) + ) grasp_poses_result = semantics.affordance.get_valid_grasp_poses( - obj_poses=obj_poses, + obj_poses=object_pose, approach_direction=approach_direction, object_part=options.pick_object_part, + grasp_cost_fn=grasp_cost_fn, ) - n_envs = obj_poses.shape[0] + num_envs = object_pose.shape[0] n_max_pose = max(r[0].shape[0] for r in grasp_poses_result) grasp_xpos_padding = torch.zeros( - (n_envs, n_max_pose, 4, 4), dtype=torch.float32, device=self.device + (num_envs, n_max_pose, 4, 4), dtype=torch.float32, device=self.device ) grasp_cost_padding = torch.full( - (n_envs, n_max_pose), + (num_envs, n_max_pose), float("inf"), dtype=torch.float32, device=self.device, ) - for i in range(n_envs): + for i in range(num_envs): n_pose = grasp_poses_result[i][0].shape[0] grasp_poses = grasp_poses_result[i][0].to( device=self.device, dtype=torch.float32 @@ -408,10 +543,9 @@ def _resolve_grasp_pose( grasp_xpos_padding[i, n_pose:] = grasp_poses[0] grasp_cost_padding[i, n_pose:] = grasp_costs[0] grasp_xpos_padding, ik_success = self._select_feasible_grasp_variants( - semantics, grasp_xpos_padding, start_qpos, - obj_poses, + object_pose, manipulator, options, approach_direction, @@ -420,28 +554,29 @@ def _resolve_grasp_pose( best_cost, best_idx = grasp_cost_masked.min(dim=1) is_success = best_cost < 9999.0 best_grasp_xpos = grasp_xpos_padding[ - torch.arange(n_envs, device=self.device), best_idx + torch.arange(num_envs, device=self.device), best_idx ] return is_success, best_grasp_xpos def _select_feasible_grasp_variants( self, - semantics: ObjectSemantics, grasp_xpos: torch.Tensor, start_qpos: torch.Tensor, object_poses: torch.Tensor, - manipulator: ResolvedControlPart, + manipulator: JointPositionTarget, options: PickUpOptions, approach_direction: torch.Tensor, ) -> tuple[torch.Tensor, torch.Tensor]: """Choose a TCP-roll variant with a feasible pickup and transport path.""" - n_envs, n_pose = grasp_xpos.shape[:2] + num_envs, n_pose = grasp_xpos.shape[:2] mirrored_grasp_xpos = grasp_xpos.clone() mirrored_grasp_xpos[..., :3, 0] = -mirrored_grasp_xpos[..., :3, 0] mirrored_grasp_xpos[..., :3, 1] = -mirrored_grasp_xpos[..., :3, 1] selection_variants = torch.stack([grasp_xpos, mirrored_grasp_xpos], dim=2) grasp_variants = self._upright_adjusted_grasp_poses( - semantics, selection_variants, options + selection_variants, + object_poses, + options, ) pre_grasp_variants = grasp_variants.clone() @@ -466,8 +601,17 @@ def _select_feasible_grasp_variants( alignment_success = self._approach_alignment_mask( grasp_variants, options, approach_direction ) + upright_compatible = self._upright_grasp_compatibility_mask( + grasp_variants, + object_poses, + options, + ) pickup_success = ( - alignment_success & pre_grasp_success & grasp_success & lift_success + upright_compatible + & alignment_success + & pre_grasp_success + & grasp_success + & lift_success ) downstream_success_counts: list[list[int]] = [] object_to_eef_variants = torch.matmul( @@ -482,26 +626,45 @@ def _select_feasible_grasp_variants( ) if object_target_pose.shape == (4, 4): object_target_pose = object_target_pose.unsqueeze(0).repeat( - n_envs, 1, 1 + num_envs, 1, 1 ) - if object_target_pose.shape != (n_envs, 4, 4): - logger.log_error( + if object_target_pose.shape != (num_envs, 4, 4): + raise ValueError( "downstream_object_target_poses entries must have shape " - f"(4, 4) or ({n_envs}, 4, 4), but got " - f"{object_target_pose.shape}.", - ValueError, + f"(4, 4) or ({num_envs}, 4, 4), but got " + f"{object_target_pose.shape}." ) - downstream_eef_variants = torch.matmul( - object_target_pose[:, None, None], object_to_eef_variants - ) - downstream_success, downstream_seed = self._compute_batch_candidate_ik( - downstream_eef_variants, downstream_seed, manipulator + object_target_variants = _upright_yaw_pose_variants( + object_target_pose, + options.upright_yaw_samples, ) + downstream_success = torch.zeros_like(pickup_success) + selected_qpos = downstream_seed + for yaw_target in object_target_variants.unbind(dim=1): + downstream_eef_variants = torch.matmul( + yaw_target[:, None, None], object_to_eef_variants + ) + yaw_success, yaw_qpos = self._compute_batch_candidate_ik( + downstream_eef_variants, + downstream_seed, + manipulator, + ) + newly_solved = ~downstream_success & yaw_success + selected_qpos = torch.where( + newly_solved[..., None], + yaw_qpos, + selected_qpos, + ) + downstream_success |= yaw_success + if bool((pickup_success & downstream_success).any(dim=(1, 2)).all()): + break + downstream_seed = selected_qpos pickup_success &= downstream_success downstream_success_counts.append(pickup_success.sum(dim=(1, 2)).tolist()) if not pickup_success.any(dim=(1, 2)).all(): logger.log_warning( "PickUp found no candidate with a feasible vertical pickup path: " + f"upright_compatible={upright_compatible.sum(dim=(1, 2)).tolist()}, " f"aligned={alignment_success.sum(dim=(1, 2)).tolist()}, " f"pre_grasp={pre_grasp_success.sum(dim=(1, 2)).tolist()}, " f"grasp={(pre_grasp_success & grasp_success).sum(dim=(1, 2)).tolist()}, " @@ -511,7 +674,7 @@ def _select_feasible_grasp_variants( start_xpos = self.robot.compute_fk( qpos=start_qpos, - name=manipulator.name, + name=manipulator.control_part, to_matrix=True, ) start_quat = quat_from_matrix(start_xpos[:, :3, :3]) @@ -522,7 +685,7 @@ def _select_feasible_grasp_variants( rotation_error = quat_error_magnitude( variant_quat.reshape(-1, 4), start_quat.reshape(-1, 4), - ).reshape(n_envs, n_pose, 2) + ).reshape(num_envs, n_pose, 2) feasible_rotation_error = torch.where( pickup_success, rotation_error, @@ -530,7 +693,7 @@ def _select_feasible_grasp_variants( ) best_variant_idx = feasible_rotation_error.argmin(dim=2) - env_idx = torch.arange(n_envs, device=self.device)[:, None] + env_idx = torch.arange(num_envs, device=self.device)[:, None] pose_idx = torch.arange(n_pose, device=self.device)[None, :] selected_grasp_xpos = grasp_variants[env_idx, pose_idx, best_variant_idx] ik_success = pickup_success[env_idx, pose_idx, best_variant_idx] @@ -556,44 +719,154 @@ def _compute_batch_candidate_ik( self, poses: torch.Tensor, joint_seed: torch.Tensor, - manipulator: ResolvedControlPart, + manipulator: JointPositionTarget, ) -> tuple[torch.Tensor, torch.Tensor]: """Solve candidate IK poses while preserving the candidate dimensions.""" - n_envs, n_pose, n_variant = poses.shape[:3] - flat_poses = poses.reshape(n_envs, n_pose * n_variant, 4, 4) + num_envs, n_pose, n_variant = poses.shape[:3] + flat_poses = poses.reshape(num_envs, n_pose * n_variant, 4, 4) if joint_seed.dim() == 2: joint_seed = joint_seed[:, None, None, :].expand(-1, n_pose, n_variant, -1) - flat_seed = joint_seed.reshape(n_envs, n_pose * n_variant, manipulator.dof) + manipulator_dof = len(manipulator.joint_ids) + flat_seed = joint_seed.reshape(num_envs, n_pose * n_variant, manipulator_dof) is_success, qpos = self.robot.compute_batch_ik( pose=flat_poses, - name=manipulator.name, + name=manipulator.control_part, joint_seed=flat_seed, ) return ( - is_success.reshape(n_envs, n_pose, n_variant), - qpos.reshape(n_envs, n_pose, n_variant, manipulator.dof), + is_success.reshape(num_envs, n_pose, n_variant), + qpos.reshape(num_envs, n_pose, n_variant, manipulator_dof), ) - def _upright_adjusted_grasp_poses( + def _upright_grasp_compatibility_mask( + self, + grasp_xpos: torch.Tensor, + object_poses: torch.Tensor, + options: PickUpOptions, + ) -> torch.Tensor: + """Reject upright grasps that clamp the object's support and top faces.""" + shape = grasp_xpos.shape[:3] + if options.rotate_upright is None: + return torch.ones(shape, dtype=torch.bool, device=grasp_xpos.device) + local_upright = self._normalized_obj_upright_direction(options).to( + device=grasp_xpos.device, + dtype=grasp_xpos.dtype, + ) + object_poses = object_poses.to( + device=grasp_xpos.device, + dtype=grasp_xpos.dtype, + ) + world_upright = torch.matmul(object_poses[:, :3, :3], local_upright) + closing_axes = torch.nn.functional.normalize( + grasp_xpos[..., :3, 0], + dim=-1, + ) + axis_alignment = torch.abs( + torch.sum(closing_axes * world_upright[:, None, None, :], dim=-1) + ) + return axis_alignment <= _UPRIGHT_SIDE_GRASP_MAX_AXIS_ALIGNMENT + + def _upright_grasp_costs( self, semantics: ObjectSemantics, + object_pose: torch.Tensor, + grasp_poses: torch.Tensor, + costs: torch.Tensor, + options: PickUpOptions, + ) -> torch.Tensor: + """Rank side grasps before generator top-k truncation.""" + local_upright = self._normalized_obj_upright_direction(options).to( + device=grasp_poses.device, + dtype=grasp_poses.dtype, + ) + object_pose = object_pose.to( + device=grasp_poses.device, + dtype=grasp_poses.dtype, + ) + world_upright = torch.matmul(object_pose[:3, :3], local_upright) + closing_axes = torch.nn.functional.normalize( + grasp_poses[:, :3, 0], + dim=-1, + ) + axis_alignment = torch.abs( + torch.sum(closing_axes * world_upright[None, :], dim=-1) + ) + adjusted = torch.where( + axis_alignment <= _UPRIGHT_SIDE_GRASP_MAX_AXIS_ALIGNMENT, + costs, + torch.full_like(costs, torch.inf), + ) + + vertices = semantics.geometry.get("mesh_vertices") + if vertices is None: + return adjusted + vertices = torch.as_tensor( + vertices, + dtype=grasp_poses.dtype, + device=grasp_poses.device, + ) + if vertices.ndim != 2 or vertices.shape[-1] != 3 or vertices.numel() == 0: + return adjusted + vertex_axis_positions = torch.matmul(vertices, local_upright) + axis_min = vertex_axis_positions.min() + axis_extent = vertex_axis_positions.max() - axis_min + if float(axis_extent) <= 1.0e-6: + return adjusted + + relative_centers = grasp_poses[:, :3, 3] - object_pose[None, :3, 3] + center_axis_positions = torch.sum( + relative_centers * world_upright[None, :], + dim=-1, + ) + center_fractions = (center_axis_positions - axis_min) / axis_extent + interval = ( + _UPRIGHT_SIDE_GRASP_MAX_AXIS_FRACTION + - _UPRIGHT_SIDE_GRASP_MIN_AXIS_FRACTION + ) + height_penalty = ( + torch.clamp( + _UPRIGHT_SIDE_GRASP_MIN_AXIS_FRACTION - center_fractions, + min=0.0, + ) + + torch.clamp( + center_fractions - _UPRIGHT_SIDE_GRASP_MAX_AXIS_FRACTION, + min=0.0, + ) + ) / interval + return adjusted + _UPRIGHT_SIDE_GRASP_HEIGHT_COST_WEIGHT * height_penalty + + def _normalized_obj_upright_direction( + self, + options: PickUpOptions, + ) -> torch.Tensor: + direction = options.obj_upright_direction + if direction is None: + direction = torch.tensor([0, 0, 1], dtype=torch.float32) + direction = direction.to(device=self.device, dtype=torch.float32) + norm = torch.linalg.vector_norm(direction) + if norm <= 1.0e-6: + logger.log_error("obj_upright_direction must be non-zero.", ValueError) + return direction / norm + + def _upright_adjusted_grasp_poses( + self, grasp_xpos: torch.Tensor, + object_pose: torch.Tensor, options: PickUpOptions, ) -> torch.Tensor: """Return grasp poses after the optional upright-in-place roll adjustment.""" if options.rotate_upright is None: return grasp_xpos - if options.obj_upright_direction is None: - upright_direction = torch.tensor( - [0, 0, 1], dtype=torch.float32, device=self.device - ) - else: - upright_direction = options.obj_upright_direction.to( - device=self.device, dtype=torch.float32 - ) - obj_pose = semantics.entity.get_local_pose(to_matrix=True) - obj_upright = torch.matmul(obj_pose[:, :3, :3], upright_direction) + upright_direction = self._normalized_obj_upright_direction(options).to( + device=grasp_xpos.device, + dtype=grasp_xpos.dtype, + ) + object_pose = object_pose.to( + device=grasp_xpos.device, + dtype=grasp_xpos.dtype, + ) + obj_upright = torch.matmul(object_pose[:, :3, :3], upright_direction) adjusted_grasp_xpos = grasp_xpos.clone() grasp_ry = adjusted_grasp_xpos[..., :3, 1] object_axes = obj_upright.reshape( diff --git a/embodichain/lab/sim/atomic_actions/primitives/place.py b/embodichain/lab/sim/atomic_actions/primitives/place.py index e809c0a2a..7ba5c0d63 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/place.py +++ b/embodichain/lab/sim/atomic_actions/primitives/place.py @@ -18,29 +18,50 @@ from __future__ import annotations +import warnings from dataclasses import dataclass from typing import ClassVar, Literal import torch -from embodichain.utils import logger from embodichain.utils.math import quat_error_magnitude, quat_from_matrix -from ._helpers import arm_qpos_from_state, resolve_object_target +from ._helpers import ( + arm_qpos_from_state, + require_shared_task_state_key, + resolve_object_target, +) from ..affordance import AssembleAffordance -from ..control import GRASP_COMMAND, OPEN_COMMAND +from ..bindings import JointPositionTarget +from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand from ..core import AtomicAction from ..effects import StateDelta -from ..goals import PoseGoalValue, resolve_pose_goal, validate_pose_goal -from ..invocation import ActionOptions, ResolvedActionRequest -from ..plans import ActionPlan -from ..state import PlanningContext -from ..trajectory_ops import ( +from ..goals import ( + PoseGoalValue, + SceneEntityPose, + resolve_pose_goal, + validate_pose_goal, +) +from embodichain.lab.sim.atomic_actions.invocation import ( + ActionOptions, + ResolvedActionRequest, +) +from embodichain.lab.sim.atomic_actions.plans import ActionPlan, TimedTrajectory +from embodichain.lab.sim.atomic_actions.requirements import ( + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + SkillBindingContract, +) +from embodichain.lab.sim.atomic_actions.state import PlanningContext +from embodichain.lab.sim.atomic_actions.trajectory_ops import ( build_pose_plan_states, interpolate_hand_qpos, resolve_pose_target, split_three_segments, ) +from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( + make_manipulation_slot, +) TcpSymmetry = Literal["none", "z_roll_180"] @@ -49,13 +70,11 @@ class PlaceGoal: """End-effector release-pose target used by :class:`Place`.""" - goal_kind: ClassVar[str] = "place_pose" - xpos: PoseGoalValue """Target end-effector release pose. - Accepts ``(4, 4)``, ``(n_envs, 4, 4)``, or - ``(n_envs, n_waypoint, 4, 4)``. + Accepts ``(4, 4)``, ``(num_envs, 4, 4)``, or + ``(num_envs, n_waypoint, 4, 4)``. """ tcp_symmetry: TcpSymmetry = "none" @@ -79,18 +98,29 @@ def __post_init__(self) -> None: class AssembleGoal: """Place a held assemble object onto a base object at a relative pose. - The base object pose is read at planning time from - :attr:`AssembleAffordance.base_object_entity`, and the assemble object's - target pose is ``base_pose @ assemble_to_base_pose``. The held-object - transform (``object_to_eef``) is read from :class:`PlanningContext` - for the place control part, which a prior :class:`PickUp` populates. + The preferred base object pose is a late-bound :class:`SceneEntityPose`. + Omitting it temporarily falls back to + :attr:`AssembleAffordance.base_object_entity`. The assemble object's target + pose is ``base_pose @ assemble_to_base_pose``. The held-object transform + (``object_to_eef``) is read from :class:`PlanningContext` for the place + control part, which a prior :class:`PickUp` populates. """ - goal_kind: ClassVar[str] = "assemble" - affordance: AssembleAffordance """Assembly affordance anchoring the assemble object to the base object.""" + base_pose: SceneEntityPose | None = None + """Late-bound base-object pose used for snapshot-consistent planning.""" + + def __post_init__(self) -> None: + if not isinstance(self.affordance, AssembleAffordance): + raise TypeError("affordance must be an AssembleAffordance instance.") + if self.base_pose is not None and not isinstance( + self.base_pose, + SceneEntityPose, + ): + raise TypeError("base_pose must be a SceneEntityPose or None.") + @dataclass(frozen=True, slots=True, eq=False) class PlaceOptions(ActionOptions): @@ -121,17 +151,18 @@ class Place(AtomicAction[PlaceGoal | AssembleGoal, PlaceOptions]): """Lower the held object to a place pose, open the gripper, retract. The :class:`PlaceGoal` may carry either a single waypoint - ``(n_envs, 4, 4)`` (or a broadcastable ``(4, 4)``) or a multi-waypoint - trajectory ``(n_envs, n_waypoint, 4, 4)``. In the multi-waypoint case the + ``(num_envs, 4, 4)`` (or a broadcastable ``(4, 4)``) or a multi-waypoint + trajectory ``(num_envs, n_waypoint, 4, 4)``. In the multi-waypoint case the approach segment visits every waypoint in order; approaching from above the first waypoint, descending through each waypoint, then opening the gripper at the final waypoint and retracting to above the last waypoint. Starting joint positions are inherited from :class:`PlanningContext`. An :class:`AssembleGoal` replaces the explicit EEF pose with an assembly - affordance: the place pose is derived from the base object's current pose - and ``assemble_to_base_pose``, converted to an EEF pose through the held - object's ``object_to_eef`` (read from :class:`PlanningContext`). + affordance: the place pose is derived from the base object's snapshot pose + (or deprecated live fallback) and ``assemble_to_base_pose``, converted to an + EEF pose through the held object's ``object_to_eef`` (read from + :class:`PlanningContext`). """ skill_id: ClassVar[str] = "place" @@ -140,19 +171,34 @@ class Place(AtomicAction[PlaceGoal | AssembleGoal, PlaceOptions]): AssembleGoal, ) OptionsType: ClassVar[type] = PlaceOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) - end_effector_roles: ClassVar[tuple[str, ...]] = ("primary",) + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + make_manipulation_slot( + "primary", + motion_capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } + ), + grasp_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, + ), + ), + ) - def __init__( + def _scene_dependencies( self, - default_options: PlaceOptions | None = None, - ) -> None: - super().__init__(default_options) - - def _on_bind(self) -> None: - """Resolve engine-wide resources from the owning engine.""" - self.n_envs = self.robot.get_qpos().shape[0] - self.robot_dof = self.robot.dof + request: ResolvedActionRequest[PlaceGoal | AssembleGoal, PlaceOptions], + ) -> tuple[str, ...]: + """Include an explicitly snapshot-grounded assembly base.""" + dependencies = set(super()._scene_dependencies(request)) + target = request.goal + if isinstance(target, AssembleGoal) and target.base_pose is not None: + dependencies.add(target.base_pose.entity_id) + return tuple(sorted(dependencies)) def _plan( self, @@ -160,28 +206,48 @@ def _plan( context: PlanningContext, ) -> ActionPlan: """Plan approach, release, and retract without committing detachment.""" - target = self.require_goal(request) + target = request.goal options = request.skill_options binding = request.binding - manipulator = binding.manipulator() - end_effector = binding.end_effector() - control_part = manipulator.name - arm_joint_ids = list(manipulator.joint_ids) - hand_joint_ids = list(end_effector.joint_ids) - hand_open_qpos = end_effector.joint_positions( + motion = binding.endpoint("primary", "motion") + grasp = binding.endpoint("primary", "grasp") + motion_target = motion.require_target(JointPositionTarget) + grasp_target = grasp.require_target(JointPositionTarget) + task_state_key = require_shared_task_state_key( + motion, + grasp, + participant="Place primary participant", + ) + control_part = motion_target.control_part + arm_joint_ids = list(motion_target.joint_ids) + hand_joint_ids = list(grasp_target.joint_ids) + hand_open_qpos = grasp.joint_positions( OPEN_COMMAND, - n_envs=context.batch_size, + num_envs=context.batch_size, device=self.device, dtype=context.robot.qpos.dtype, ) - hand_grasp_qpos = end_effector.joint_positions( + hand_grasp_qpos = grasp.joint_positions( GRASP_COMMAND, - n_envs=context.batch_size, + num_envs=context.batch_size, device=self.device, dtype=context.robot.qpos.dtype, ) state = context - place_xpos = self._resolve_place_xpos(target, state, control_part) + held_mask = context.task.held_object_mask(task_state_key) + exclusive_mask = context.task.exclusive_held_object_mask(task_state_key) + eligible = ( + exclusive_mask + if isinstance(target, AssembleGoal) + else ~held_mask | exclusive_mask + ) + place_xpos = self._resolve_place_xpos(target, state, task_state_key) + if not eligible.any(): + return self.failed_plan( + request, + context, + message="Place requires an exclusive held-object relation.", + ) if place_xpos.dim() == 3: place_xpos = place_xpos.unsqueeze(1) @@ -214,6 +280,7 @@ def _plan( start_qpos=start_arm_qpos, control_part=control_part, sample_count=n_down, + interpolation_dt=context.control_dt, ), ) assert isinstance(down_result.success, torch.Tensor) @@ -231,13 +298,14 @@ def _plan( start_qpos=reach_arm_qpos, control_part=control_part, sample_count=n_back, + interpolation_dt=context.control_dt, ), ) assert isinstance(back_result.success, torch.Tensor) assert back_result.positions is not None back_success = back_result.success back_arm = back_result.positions - success = down_success & back_success + success = down_success & back_success & eligible hand_open_path = interpolate_hand_qpos( hand_grasp_qpos, hand_open_qpos, n_waypoints=n_open @@ -248,7 +316,7 @@ def _plan( n_down_actual = down_arm.shape[1] n_back_actual = back_arm.shape[1] full = torch.empty( - (self.n_envs, n_down_actual + n_open + n_back_actual, self.robot_dof), + (self.num_envs, n_down_actual + n_open + n_back_actual, self.robot_dof), dtype=torch.float32, device=self.device, ) @@ -263,15 +331,21 @@ def _plan( full[:, n_down_actual + n_open :, hand_joint_ids] = hand_open_qpos.unsqueeze(1) coordinated_updates = { - key: None for key in state.coordinated_held_objects if control_part in key + key: None + for key in state.task.coordinated_held_objects + if task_state_key in key } return self.build_plan( request, context, success=success, - trajectory=full, + trajectory=TimedTrajectory.from_uniform_step( + full, + env_ids=context.env_ids, + step_dt=context.require_control_dt(), + ), expected_effects=StateDelta( - held_object_updates={control_part: None}, + held_object_updates={task_state_key: None}, coordinated_held_object_updates=coordinated_updates, ), segment_lengths={ @@ -285,31 +359,32 @@ def _resolve_place_xpos( self, target: PlaceGoal | AssembleGoal, state: PlanningContext, - control_part: str, + task_state_key: str, ) -> torch.Tensor: """Resolve the place EEF poses from a typed target. Args: target: Either an explicit EEF pose target or an assembly target. state: World state carrying the held-object transform. + task_state_key: Stable logical resource used for held-object state. Returns: - Place EEF poses with shape ``(n_envs, 4, 4)`` or - ``(n_envs, n_waypoint, 4, 4)``. + Place EEF poses with shape ``(num_envs, 4, 4)`` or + ``(num_envs, n_waypoint, 4, 4)``. """ if isinstance(target, PlaceGoal): return resolve_pose_target( resolve_pose_goal(target.xpos, state, name="xpos"), - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, ) - return self._resolve_assemble_place_xpos(target, state, control_part) + return self._resolve_assemble_place_xpos(target, state, task_state_key) def _resolve_assemble_place_xpos( self, target: AssembleGoal, state: PlanningContext, - control_part: str, + task_state_key: str, ) -> torch.Tensor: """Derive the place EEF pose from an assembly affordance. @@ -320,34 +395,55 @@ def _resolve_assemble_place_xpos( Args: target: Assembly target carrying the base/assemble affordance. state: World state carrying the held-object transform. + task_state_key: Stable logical resource used for held-object state. Returns: - Place EEF poses with shape ``(n_envs, 4, 4)``. + Place EEF poses with shape ``(num_envs, 4, 4)``. Raises: - ValueError: If no held object or no base object entity is available. + ValueError: If no held object or base-pose source is available. """ - held = state.get_held_object(control_part) + held = state.get_held_object(task_state_key) if held is None: - logger.log_error( - "Place with AssembleGoal requires an object held by control " - f"part {control_part!r} (run PickUp first).", - ValueError, + raise ValueError( + "Place with AssembleGoal requires an object held by task-state " + f"resource {task_state_key!r} (run PickUp first)." ) affordance = target.affordance - if affordance.base_object_entity is None: - logger.log_error( - "AssembleAffordance.base_object_entity must be set to assemble " - "onto a base object.", - ValueError, + if target.base_pose is not None: + base_pose = resolve_object_target( + resolve_pose_goal( + target.base_pose, + state, + name="base_pose", + ), + num_envs=self.num_envs, + device=self.device, + name="base_pose", + ) + else: + if affordance.base_object_entity is None: + raise ValueError( + "AssembleGoal requires base_pose or " + "AssembleAffordance.base_object_entity." + ) + warnings.warn( + "AssembleGoal without base_pose reads " + "AssembleAffordance.base_object_entity live; provide " + "base_pose=SceneEntityPose(...) instead.", + DeprecationWarning, + stacklevel=3, + ) + base_pose = resolve_object_target( + affordance.base_object_entity.get_local_pose(to_matrix=True), + num_envs=self.num_envs, + device=self.device, + name="legacy_base_pose", ) - base_pose = affordance.base_object_entity.get_local_pose(to_matrix=True).to( - device=self.device, dtype=torch.float32 - ) assemble_object_pose = affordance.get_assemble_object_pose(base_pose) object_to_eef = resolve_object_target( held.object_to_eef, - n_envs=self.n_envs, + num_envs=self.num_envs, device=self.device, name="object_to_eef", ) @@ -424,10 +520,10 @@ def _select_tcp_symmetric_place_variant( rotation_error = quat_error_magnitude( first_waypoint_quat.reshape(-1, 4), start_quat.reshape(-1, 4), - ).reshape(self.n_envs, 2) + ).reshape(self.num_envs, 2) best_variant_idx = rotation_error.argmin(dim=1) - env_idx = torch.arange(self.n_envs, device=self.device)[:, None] + env_idx = torch.arange(self.num_envs, device=self.device)[:, None] waypoint_idx = torch.arange(place_xpos.shape[1], device=self.device)[None, :] return place_variants[ env_idx, diff --git a/embodichain/lab/sim/atomic_actions/primitives/press.py b/embodichain/lab/sim/atomic_actions/primitives/press.py index a57e44bb8..e03e4e537 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/press.py +++ b/embodichain/lab/sim/atomic_actions/primitives/press.py @@ -18,189 +18,353 @@ from __future__ import annotations +import math from dataclasses import dataclass from typing import ClassVar import torch -from embodichain.utils import logger - -from ._helpers import arm_qpos_from_state -from ..control import GRASP_COMMAND -from ..core import AtomicAction -from ..goals import PoseGoalValue, resolve_pose_goal, validate_pose_goal -from ..invocation import ActionOptions, ResolvedActionRequest -from ..plans import ActionPlan -from ..state import PlanningContext -from ..trajectory_ops import ( - build_joint_plan_states, +from embodichain.lab.sim.atomic_actions.primitives._helpers import arm_qpos_from_state +from embodichain.lab.sim.atomic_actions.affordance import PressAffordance +from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget +from embodichain.lab.sim.atomic_actions.control import ( + GRASP_COMMAND, + JointPositionCommand, +) +from embodichain.lab.sim.atomic_actions.core import AtomicAction, ObjectSemantics +from embodichain.lab.sim.atomic_actions.effects import StateDelta +from embodichain.lab.sim.atomic_actions.goals import ( + ObjectActionGoal, + PoseGoalValue, + resolve_pose_goal, + validate_pose_goal, +) +from embodichain.lab.sim.atomic_actions.invocation import ( + ActionOptions, + ResolvedActionRequest, +) +from embodichain.lab.sim.atomic_actions.plans import ActionPlan, TimedTrajectory +from embodichain.lab.sim.atomic_actions.requirements import ( + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + SkillBindingContract, +) +from embodichain.utils.math import get_relative_rotation +from embodichain.lab.sim.atomic_actions.state import PlanningContext +from embodichain.lab.sim.atomic_actions.trajectory_ops import ( + axis_translation_keyframes, build_pose_plan_states, interpolate_hand_qpos, resolve_pose_target, + translate_pose_world, +) +from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( + make_manipulation_slot, ) @dataclass(frozen=True, slots=True, eq=False) -class PressGoal: - """Single end-effector contact pose used by :class:`Press`.""" +class PressGoal(ObjectActionGoal): + """Target object described by a press affordance.""" - goal_kind: ClassVar[str] = "press_pose" + goal_kind: ClassVar[str] = "press" - xpos: PoseGoalValue - """Contact pose, shape ``(4, 4)`` or ``(n_envs, 4, 4)``.""" + target_pose: PoseGoalValue + """Target pose snapshot or late-bound stable scene-entity reference.""" def __post_init__(self) -> None: - validate_pose_goal(self.xpos, "xpos", allow_waypoints=False) + ObjectActionGoal.__post_init__(self) + validate_pose_goal(self.target_pose, "target_pose", allow_waypoints=False) @dataclass(frozen=True, slots=True, eq=False) class PressOptions(ActionOptions): - """Per-invocation press behavior.""" + """Per-invocation pressing behavior.""" hand_interp_steps: int = 5 - """Number of waypoints for closing the gripper before pressing.""" + """Number of waypoints used to close the hand.""" + + approach_distance: float = 0.1 + """Distance from the press position opposite the press direction.""" + + press_distance: float = 0.05 + """Distance traveled into the target along its press axis.""" + + press_position: tuple[float, float, float] | None = None + """Optional local-frame position overriding the affordance press position.""" def __post_init__(self) -> None: if self.hand_interp_steps < 1: raise ValueError("hand_interp_steps must be at least 1.") + if not math.isfinite(self.approach_distance): + raise ValueError("approach_distance must be finite.") + if self.approach_distance < 0.0: + raise ValueError("approach_distance must be non-negative.") + if not math.isfinite(self.press_distance): + raise ValueError("press_distance must be finite.") + if self.press_distance <= 0.0: + raise ValueError("press_distance must be positive.") + if self.press_position is not None: + position = torch.as_tensor(self.press_position, dtype=torch.float32) + if position.shape != (3,) or not torch.isfinite(position).all(): + raise ValueError("press_position must be a finite (x, y, z) tuple.") + object.__setattr__( + self, + "press_position", + tuple(float(component) for component in position), + ) class Press(AtomicAction[PressGoal, PressOptions]): - """Close the gripper, press down to a target pose, then return.""" + """Open-loop motion primitive that approaches, presses, and retracts.""" skill_id: ClassVar[str] = "press" GoalType: ClassVar[type] = PressGoal OptionsType: ClassVar[type] = PressOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) - end_effector_roles: ClassVar[tuple[str, ...]] = ("primary",) + open_loop: ClassVar[bool] = True + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + make_manipulation_slot( + "primary", + motion_capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } + ), + grasp_commands={GRASP_COMMAND: JointPositionCommand}, + ), + ), + ) - def __init__( - self, - default_options: PressOptions | None = None, - ) -> None: + def __init__(self, default_options: PressOptions | None = None) -> None: super().__init__(default_options) def _on_bind(self) -> None: - """Resolve engine-wide resources from the owning engine.""" - self.n_envs = self.robot.get_qpos().shape[0] + """Resolve dimensions owned by the engine's robot.""" + self.num_envs = self.robot.get_qpos().shape[0] self.robot_dof = self.robot.dof + def _find_symmetric_nearest_xpos( + self, target_xpos: torch.Tensor, reference_xpos: torch.Tensor + ) -> torch.Tensor: + """Find the nearest symmetric pose to the reference pose.""" + symmetric_xpos = target_xpos.clone() + symmetric_xpos[:, :3, 0] = -symmetric_xpos[:, :3, 0] + symmetric_xpos[:, :3, 1] = -symmetric_xpos[:, :3, 1] + angle_a = get_relative_rotation( + reference_xpos[:, :3, :3], target_xpos[:, :3, :3] + ) + angle_b = get_relative_rotation( + reference_xpos[:, :3, :3], symmetric_xpos[:, :3, :3] + ) + choose_target = (angle_a < angle_b)[..., None, None] + target_xpos = torch.where(choose_target, target_xpos, symmetric_xpos) + return target_xpos + def _plan( self, request: ResolvedActionRequest[PressGoal, PressOptions], context: PlanningContext, ) -> ActionPlan: - """Plan a close, press, and retract sequence.""" + """Plan close, approach, press, and retract without stepping simulation.""" target = self.require_goal(request) + affordance = self._require_press_affordance(target.semantics) options = request.skill_options + interpolation_dt = context.require_control_dt() binding = request.binding - manipulator = binding.manipulator() - end_effector = binding.end_effector() - control_part = manipulator.name - arm_joint_ids = list(manipulator.joint_ids) - hand_joint_ids = list(end_effector.joint_ids) - hand_close_qpos = end_effector.joint_positions( + motion_target = binding.endpoint("primary", "motion").require_target( + JointPositionTarget + ) + grasp = binding.endpoint("primary", "grasp") + grasp_target = grasp.require_target(JointPositionTarget) + control_part = motion_target.control_part + arm_joint_ids = list(motion_target.joint_ids) + hand_joint_ids = list(grasp_target.joint_ids) + start_arm_qpos = arm_qpos_from_state(context, arm_joint_ids) + start_hand_qpos = context.last_qpos[:, hand_joint_ids] + hand_grasp_qpos = grasp.joint_positions( GRASP_COMMAND, - n_envs=self.n_envs, + num_envs=context.batch_size, device=self.device, dtype=context.robot.qpos.dtype, ) - state = context - press_xpos = resolve_pose_target( - resolve_pose_goal(target.xpos, context, name="xpos"), - n_envs=self.n_envs, + + target_pose = resolve_pose_target( + resolve_pose_goal(target.target_pose, context, name="target_pose"), + num_envs=self.num_envs, device=self.device, ) - start_arm_qpos = arm_qpos_from_state(state, arm_joint_ids) - start_hand_qpos = state.last_qpos[:, hand_joint_ids] - - n_close, n_down, n_back = self._compute_segment_waypoints( - request.motion_policy.sample_count, options + contact_xpos = affordance.get_press_pose( + target_pose, + press_position=options.press_position, + ).to(device=self.device, dtype=torch.float32) + contact_xpos = self._find_symmetric_nearest_xpos( + contact_xpos, + reference_xpos=self.robot.compute_fk( + qpos=start_arm_qpos, name=control_part, to_matrix=True + ), ) - - hand_close_path = interpolate_hand_qpos( + approach_xpos = translate_pose_world( + contact_xpos, + -contact_xpos[:, :3, 2] * options.approach_distance, + ) + pressed_xpos = translate_pose_world( + contact_xpos, + contact_xpos[:, :3, 2] * options.press_distance, + ) + n_approach, n_contact, n_press, n_retract = self._motion_segment_lengths( + request.motion_policy.sample_count, + options.hand_interp_steps, + ) + hand_close = interpolate_hand_qpos( start_hand_qpos, - hand_close_qpos, - n_waypoints=n_close, + hand_grasp_qpos, + n_waypoints=options.hand_interp_steps, ) - - down_result = self.motion_generator.generate( - build_pose_plan_states(press_xpos), - options=request.motion_policy.to_motion_gen_options( - start_qpos=start_arm_qpos, - control_part=control_part, - sample_count=n_down, - ), + approach_success, approach_arm = self._plan_pose_segment( + approach_xpos, + start_arm_qpos, + control_part, + request, + n_approach, + interpolation_dt=interpolation_dt, ) - assert isinstance(down_result.success, torch.Tensor) - assert down_result.positions is not None - down_success = down_result.success - down_arm = down_result.positions - - press_arm_qpos = down_arm[:, -1, :] - back_result = self.motion_generator.generate( - build_joint_plan_states(start_arm_qpos), - options=request.motion_policy.to_motion_gen_options( - start_qpos=press_arm_qpos, - control_part=control_part, - sample_count=n_back, - ), + contact_keyframes = axis_translation_keyframes( + approach_xpos, + contact_xpos, + contact_xpos[:, :3, 2], + n_waypoints=n_contact - 1, + ) + contact_success, contact_arm = self._plan_pose_segment( + contact_keyframes, + approach_arm[:, -1], + control_part, + request, + n_contact, + interpolation_dt=interpolation_dt, + cartesian_linear=True, + ) + press_keyframes = axis_translation_keyframes( + contact_xpos, + pressed_xpos, + contact_xpos[:, :3, 2], + n_waypoints=n_press - 1, ) - assert isinstance(back_result.success, torch.Tensor) - assert back_result.positions is not None - back_success = back_result.success - back_arm = back_result.positions - success = down_success & back_success + press_success, press_arm = self._plan_pose_segment( + press_keyframes, + contact_arm[:, -1], + control_part, + request, + n_press, + interpolation_dt=interpolation_dt, + cartesian_linear=True, + ) + retract_keyframes = axis_translation_keyframes( + pressed_xpos, + approach_xpos, + contact_xpos[:, :3, 2], + n_waypoints=n_retract - 1, + ) + retract_success, retract_arm = self._plan_pose_segment( + retract_keyframes, + press_arm[:, -1], + control_part, + request, + n_retract, + interpolation_dt=interpolation_dt, + cartesian_linear=True, + ) + success = approach_success & contact_success & press_success & retract_success - # Allocate from the actually returned segment lengths so collision-aware - # planners (which preserve their own sample count) are accommodated. - n_down_actual = down_arm.shape[1] - n_back_actual = back_arm.shape[1] + parts = (hand_close, approach_arm, contact_arm, press_arm, retract_arm) + lengths = tuple(part.shape[1] for part in parts) full = torch.empty( - (self.n_envs, n_close + n_down_actual + n_back_actual, self.robot_dof), - dtype=torch.float32, + (self.num_envs, sum(lengths), self.robot_dof), + dtype=context.robot.qpos.dtype, device=self.device, ) - full[:, :, :] = state.last_qpos.unsqueeze(1) - full[:, :n_close, arm_joint_ids] = start_arm_qpos.unsqueeze(1) - full[:, :n_close, hand_joint_ids] = hand_close_path - full[:, n_close : n_close + n_down_actual, arm_joint_ids] = down_arm - full[:, n_close : n_close + n_down_actual, hand_joint_ids] = ( - hand_close_qpos.unsqueeze(1) - ) - full[:, n_close + n_down_actual :, arm_joint_ids] = back_arm - full[:, n_close + n_down_actual :, hand_joint_ids] = hand_close_qpos.unsqueeze( - 1 - ) + full[:] = context.last_qpos.unsqueeze(1) + offset = 0 + + stop = offset + hand_close.shape[1] + full[:, offset:stop, arm_joint_ids] = start_arm_qpos.unsqueeze(1) + full[:, offset:stop, hand_joint_ids] = hand_close + offset = stop + + for arm in (approach_arm, contact_arm, press_arm, retract_arm): + stop = offset + arm.shape[1] + full[:, offset:stop, arm_joint_ids] = arm + full[:, offset:stop, hand_joint_ids] = hand_grasp_qpos.unsqueeze(1) + offset = stop return self.build_plan( request, context, success=success, - trajectory=full, + trajectory=TimedTrajectory.from_uniform_step( + full, + env_ids=context.env_ids, + step_dt=interpolation_dt, + ), + expected_effects=StateDelta(), segment_lengths={ - "close": n_close, - "press": n_down_actual, - "retract": n_back_actual, + "close": lengths[0], + "approach": lengths[1], + "contact": lengths[2], + "press": lengths[3], + "retract": lengths[4], }, ) - def _compute_segment_waypoints( - self, sample_count: int, options: PressOptions - ) -> tuple[int, int, int]: - """Split the invocation sample budget across press segments.""" - n_close = options.hand_interp_steps - - motion_waypoints = sample_count - n_close - n_down = motion_waypoints // 2 - n_back = motion_waypoints - n_down - if n_down < 2 or n_back < 2: - logger.log_error( - "Not enough waypoints for press trajectory. Increase " - "MotionPolicy.sample_count or decrease hand_interp_steps.", - ValueError, + @staticmethod + def _require_press_affordance( + semantics: ObjectSemantics, + ) -> PressAffordance: + affordance = semantics.affordance + if not isinstance(affordance, PressAffordance): + raise ValueError("Press requires a PressAffordance.") + return affordance + + @staticmethod + def _motion_segment_lengths( + sample_count: int, + hand_interp_steps: int, + ) -> tuple[int, int, int, int]: + motion_count = sample_count - hand_interp_steps + if motion_count < 8: + raise ValueError( + "Not enough waypoints for Press. Increase sample_count or " + "decrease hand_interp_steps." ) - return n_close, n_down, n_back + base, remainder = divmod(motion_count, 4) + values = [base + (index < remainder) for index in range(4)] + return values[0], values[1], values[2], values[3] + + def _plan_pose_segment( + self, + target_pose: torch.Tensor, + start_qpos: torch.Tensor, + control_part: str, + request: ResolvedActionRequest[PressGoal, PressOptions], + sample_count: int, + *, + interpolation_dt: float, + cartesian_linear: bool = False, + ) -> tuple[torch.Tensor, torch.Tensor]: + result = self.motion_generator.generate( + build_pose_plan_states(target_pose), + options=request.motion_policy.to_motion_gen_options( + start_qpos=start_qpos, + control_part=control_part, + sample_count=sample_count, + interpolation_dt=interpolation_dt, + cartesian_linear=cartesian_linear, + ), + ) + assert isinstance(result.success, torch.Tensor) + assert result.positions is not None + return result.success, result.positions __all__ = ["Press", "PressGoal", "PressOptions"] diff --git a/embodichain/lab/sim/atomic_actions/primitives/slide.py b/embodichain/lab/sim/atomic_actions/primitives/slide.py new file mode 100644 index 000000000..574372f9f --- /dev/null +++ b/embodichain/lab/sim/atomic_actions/primitives/slide.py @@ -0,0 +1,410 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Slide atomic action implementation.""" + +from __future__ import annotations + +import math +from dataclasses import dataclass +from typing import ClassVar, Literal + +import torch + +from embodichain.lab.sim.atomic_actions.affordance import SlideAffordance +from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget +from embodichain.lab.sim.atomic_actions.control import ( + GRASP_COMMAND, + OPEN_COMMAND, + JointPositionCommand, +) +from embodichain.lab.sim.atomic_actions.core import AtomicAction, ObjectSemantics +from embodichain.lab.sim.atomic_actions.effects import StateDelta +from embodichain.lab.sim.atomic_actions.goals import ( + ObjectActionGoal, + PoseGoalValue, + resolve_pose_goal, + validate_pose_goal, +) +from embodichain.lab.sim.atomic_actions.invocation import ( + ActionOptions, + ResolvedActionRequest, +) +from embodichain.lab.sim.atomic_actions.plans import ( + ActionPlan, + TimedTrajectory, + normalize_success_mask, +) +from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( + make_manipulation_slot, +) +from embodichain.lab.sim.atomic_actions.primitives._helpers import arm_qpos_from_state +from embodichain.lab.sim.atomic_actions.requirements import ( + CARTESIAN_POSE_CAPABILITY, + SkillBindingContract, +) +from embodichain.lab.sim.atomic_actions.state import PlanningContext +from embodichain.lab.sim.atomic_actions.trajectory_ops import ( + axis_translation_keyframes, + build_pose_plan_states, + interpolate_hand_qpos, + resolve_pose_target, + translate_pose_world, +) + + +@dataclass(frozen=True, slots=True, eq=False) +class SlideGoal(ObjectActionGoal): + """Translating articulation link described by a slide affordance.""" + + goal_kind: ClassVar[str] = "slide" + + target_pose: PoseGoalValue + """Link pose snapshot or late-bound stable scene-entity reference.""" + + def __post_init__(self) -> None: + ObjectActionGoal.__post_init__(self) + validate_pose_goal(self.target_pose, "target_pose", allow_waypoints=False) + + +@dataclass(frozen=True, slots=True, eq=False) +class SlideOptions(ActionOptions): + """Per-invocation sliding behavior for a translating articulation link.""" + + direction: Literal["pull", "push"] = "pull" + """Whether to pull the part open or push it closed.""" + + hand_interp_steps: int = 5 + """Number of waypoints used for each close/open hand segment.""" + + approach_distance: float = 0.1 + """Pre-grasp distance opposite the approach/push axis.""" + + translation_distance: float = 0.15 + """Distance traveled along the pull or push direction.""" + + def __post_init__(self) -> None: + if self.direction not in ("pull", "push"): + raise ValueError("direction must be either 'pull' or 'push'.") + if self.hand_interp_steps < 1: + raise ValueError("hand_interp_steps must be at least 1.") + if not math.isfinite(self.approach_distance): + raise ValueError("approach_distance must be finite.") + if self.approach_distance < 0.0: + raise ValueError("approach_distance must be non-negative.") + if not math.isfinite(self.translation_distance): + raise ValueError("translation_distance must be finite.") + if self.translation_distance <= 0.0: + raise ValueError("translation_distance must be positive.") + + +class Slide(AtomicAction[SlideGoal, SlideOptions]): + """Open-loop approach, grasp, and axis-constrained sliding motion.""" + + skill_id: ClassVar[str] = "slide" + GoalType: ClassVar[type] = SlideGoal + OptionsType: ClassVar[type] = SlideOptions + open_loop: ClassVar[bool] = True + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + make_manipulation_slot( + "primary", + motion_capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + grasp_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, + ), + ), + ) + + def __init__( + self, + default_options: SlideOptions | None = None, + ) -> None: + super().__init__(default_options) + + def _on_bind(self) -> None: + """Resolve dimensions owned by the engine's robot.""" + self.num_envs = self.robot.get_qpos().shape[0] + self.robot_dof = self.robot.dof + + def _plan( + self, + request: ResolvedActionRequest[ + SlideGoal, + SlideOptions, + ], + context: PlanningContext, + ) -> ActionPlan: + """Plan the complete pull/push sequence without stepping simulation.""" + target = self.require_goal(request) + affordance = self._require_slide_affordance(target.semantics) + options = request.skill_options + interpolation_dt = context.require_control_dt() + binding = request.binding + motion_target = binding.endpoint("primary", "motion").require_target( + JointPositionTarget + ) + grasp = binding.endpoint("primary", "grasp") + grasp_target = grasp.require_target(JointPositionTarget) + control_part = motion_target.control_part + arm_joint_ids = list(motion_target.joint_ids) + hand_joint_ids = list(grasp_target.joint_ids) + start_arm_qpos = arm_qpos_from_state(context, arm_joint_ids) + hand_open_qpos = grasp.joint_positions( + OPEN_COMMAND, + num_envs=context.batch_size, + device=self.device, + dtype=context.robot.qpos.dtype, + ) + hand_grasp_qpos = grasp.joint_positions( + GRASP_COMMAND, + num_envs=context.batch_size, + device=self.device, + dtype=context.robot.qpos.dtype, + ) + + link_pose = resolve_pose_target( + resolve_pose_goal(target.target_pose, context, name="target_pose"), + num_envs=self.num_envs, + device=self.device, + ) + translation_axis = affordance.translation_axis.to( + device=self.device, dtype=torch.float32 + ) + translation_axis = translation_axis / torch.linalg.vector_norm(translation_axis) + translation_axis_world = torch.matmul(link_pose[:, :3, :3], translation_axis) + grasp_success, grasp_xpos, _ = affordance.get_best_grasp_poses( + obj_poses=link_pose, + approach_direction=translation_axis_world, + ) + grasp_xpos = grasp_xpos.to(device=self.device, dtype=torch.float32) + grasp_success = normalize_success_mask( + grasp_success, + num_envs=self.num_envs, + device=self.device, + name="Slide grasp-pose success", + ) + if not grasp_success.any(): + return self.failed_plan( + request, + context, + message="Failed to resolve an articulated-part grasp pose.", + ) + approach_xpos = translate_pose_world( + grasp_xpos, + -translation_axis_world * options.approach_distance, + ) + translation_sign = -1.0 if options.direction == "pull" else 1.0 + translated_xpos = translate_pose_world( + grasp_xpos, + translation_axis_world * (translation_sign * options.translation_distance), + ) + + motion_lengths = self._motion_segment_lengths( + request.motion_policy.sample_count, + options.hand_interp_steps, + direction=options.direction, + ) + approach_success, approach_arm = self._plan_pose_segment( + approach_xpos, + start_arm_qpos, + control_part, + request, + motion_lengths[0], + interpolation_dt=interpolation_dt, + ) + reach_keyframes = axis_translation_keyframes( + approach_xpos, + grasp_xpos, + translation_axis_world, + n_waypoints=motion_lengths[1] - 1, + ) + reach_success, reach_arm = self._plan_pose_segment( + reach_keyframes, + approach_arm[:, -1], + control_part, + request, + motion_lengths[1], + interpolation_dt=interpolation_dt, + cartesian_linear=True, + ) + translate_keyframes = axis_translation_keyframes( + grasp_xpos, + translated_xpos, + translation_axis_world, + n_waypoints=motion_lengths[2] - 1, + ) + translate_success, translate_arm = self._plan_pose_segment( + translate_keyframes, + reach_arm[:, -1], + control_part, + request, + motion_lengths[2], + interpolation_dt=interpolation_dt, + cartesian_linear=True, + ) + success = grasp_success & approach_success & reach_success & translate_success + + return_arm: torch.Tensor | None = None + if options.direction == "push": + return_keyframes = axis_translation_keyframes( + translated_xpos, + approach_xpos, + translation_axis_world, + n_waypoints=motion_lengths[3] - 1, + ) + return_success, return_arm = self._plan_pose_segment( + return_keyframes, + translate_arm[:, -1], + control_part, + request, + motion_lengths[3], + interpolation_dt=interpolation_dt, + cartesian_linear=True, + ) + success = success & return_success + + hand_close = interpolate_hand_qpos( + hand_open_qpos, + hand_grasp_qpos, + n_waypoints=options.hand_interp_steps, + ) + hand_open = interpolate_hand_qpos( + hand_grasp_qpos, + hand_open_qpos, + n_waypoints=options.hand_interp_steps, + ) + named_parts: list[tuple[str, torch.Tensor]] = [ + ("approach", approach_arm), + ("reach", reach_arm), + ("close", hand_close), + (options.direction, translate_arm), + ("open", hand_open), + ] + if return_arm is not None: + named_parts.append(("return", return_arm)) + + segment_lengths = {name: part.shape[1] for name, part in named_parts} + full = torch.empty( + (self.num_envs, sum(segment_lengths.values()), self.robot_dof), + dtype=context.robot.qpos.dtype, + device=self.device, + ) + full[:] = context.last_qpos.unsqueeze(1) + offset = 0 + + for arm in (approach_arm, reach_arm): + stop = offset + arm.shape[1] + full[:, offset:stop, arm_joint_ids] = arm + full[:, offset:stop, hand_joint_ids] = hand_open_qpos.unsqueeze(1) + offset = stop + + stop = offset + hand_close.shape[1] + full[:, offset:stop, arm_joint_ids] = reach_arm[:, -1].unsqueeze(1) + full[:, offset:stop, hand_joint_ids] = hand_close + offset = stop + + stop = offset + translate_arm.shape[1] + full[:, offset:stop, arm_joint_ids] = translate_arm + full[:, offset:stop, hand_joint_ids] = hand_grasp_qpos.unsqueeze(1) + offset = stop + + stop = offset + hand_open.shape[1] + full[:, offset:stop, arm_joint_ids] = translate_arm[:, -1].unsqueeze(1) + full[:, offset:stop, hand_joint_ids] = hand_open + offset = stop + + if return_arm is not None: + full[:, offset:, arm_joint_ids] = return_arm + full[:, offset:, hand_joint_ids] = hand_open_qpos.unsqueeze(1) + + return self.build_plan( + request, + context, + success=success, + trajectory=TimedTrajectory.from_uniform_step( + full, + env_ids=context.env_ids, + step_dt=interpolation_dt, + ), + expected_effects=StateDelta(), + segment_lengths=segment_lengths, + ) + + @staticmethod + def _require_slide_affordance( + semantics: ObjectSemantics, + ) -> SlideAffordance: + affordance = semantics.affordance + if not isinstance(affordance, SlideAffordance): + raise ValueError("Slide requires a SlideAffordance.") + return affordance + + @staticmethod + def _motion_segment_lengths( + sample_count: int, + hand_interp_steps: int, + *, + direction: Literal["pull", "push"], + ) -> tuple[int, ...]: + motion_segment_count = 3 if direction == "pull" else 4 + motion_count = sample_count - 2 * hand_interp_steps + if motion_count < 2 * motion_segment_count: + raise ValueError( + "Not enough waypoints for Slide. Increase " + "sample_count or decrease hand_interp_steps." + ) + base, remainder = divmod(motion_count, motion_segment_count) + return tuple( + base + (index < remainder) for index in range(motion_segment_count) + ) + + def _plan_pose_segment( + self, + target_pose: torch.Tensor, + start_qpos: torch.Tensor, + control_part: str, + request: ResolvedActionRequest[ + SlideGoal, + SlideOptions, + ], + sample_count: int, + *, + interpolation_dt: float, + cartesian_linear: bool = False, + ) -> tuple[torch.Tensor, torch.Tensor]: + result = self.motion_generator.generate( + build_pose_plan_states(target_pose), + options=request.motion_policy.to_motion_gen_options( + start_qpos=start_qpos, + control_part=control_part, + sample_count=sample_count, + interpolation_dt=interpolation_dt, + cartesian_linear=cartesian_linear, + ), + ) + assert isinstance(result.success, torch.Tensor) + assert result.positions is not None + return result.success, result.positions + + +__all__ = [ + "Slide", + "SlideGoal", + "SlideOptions", +] diff --git a/embodichain/lab/sim/atomic_actions/primitives/twist.py b/embodichain/lab/sim/atomic_actions/primitives/twist.py new file mode 100644 index 000000000..fed1b2f98 --- /dev/null +++ b/embodichain/lab/sim/atomic_actions/primitives/twist.py @@ -0,0 +1,423 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Twist atomic action implementation.""" + +from __future__ import annotations + +import math +from dataclasses import dataclass +from typing import ClassVar + +import torch + +from embodichain.utils.math import ( + axis_angle_to_rotation_matrix, + pose_inv, + get_relative_rotation, +) + +from embodichain.lab.sim.atomic_actions.affordance import TwistAffordance +from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget +from embodichain.lab.sim.atomic_actions.control import ( + GRASP_COMMAND, + OPEN_COMMAND, + JointPositionCommand, +) +from embodichain.lab.sim.atomic_actions.core import AtomicAction, ObjectSemantics +from embodichain.lab.sim.atomic_actions.effects import StateDelta +from embodichain.lab.sim.atomic_actions.goals import ( + ObjectActionGoal, + PoseGoalValue, + resolve_pose_goal, + validate_pose_goal, +) +from embodichain.lab.sim.atomic_actions.invocation import ( + ActionOptions, + ResolvedActionRequest, +) +from embodichain.lab.sim.atomic_actions.plans import ActionPlan, TimedTrajectory +from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( + make_manipulation_slot, +) +from embodichain.lab.sim.atomic_actions.primitives._helpers import arm_qpos_from_state +from embodichain.lab.sim.atomic_actions.requirements import ( + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + SkillBindingContract, +) +from embodichain.lab.sim.atomic_actions.state import PlanningContext +from embodichain.lab.sim.atomic_actions.trajectory_ops import ( + build_pose_plan_states, + interpolate_hand_qpos, + resolve_pose_target, + translate_pose_world, +) + + +@dataclass(frozen=True, slots=True, eq=False) +class TwistGoal(ObjectActionGoal): + """Target object described by a twist affordance.""" + + goal_kind: ClassVar[str] = "twist" + + target_pose: PoseGoalValue + """Target pose snapshot or late-bound stable scene-entity reference.""" + + def __post_init__(self) -> None: + ObjectActionGoal.__post_init__(self) + validate_pose_goal(self.target_pose, "target_pose", allow_waypoints=False) + + +@dataclass(frozen=True, slots=True, eq=False) +class TwistOptions(ActionOptions): + """Per-invocation twisting behavior.""" + + hand_interp_steps: int = 5 + """Number of waypoints used for each close/open hand segment.""" + + twist_waypoint_count: int = 8 + """Number of Cartesian keyframes along the target's circular twist arc.""" + + pre_grasp_distance: float = 0.1 + """Distance from the grasp pose along its negative z-axis.""" + + twist_angle: float = math.pi / 4 + """Requested twist rotation in radians.""" + + def __post_init__(self) -> None: + if self.hand_interp_steps < 1: + raise ValueError("hand_interp_steps must be at least 1.") + if self.twist_waypoint_count < 1: + raise ValueError("twist_waypoint_count must be at least 1.") + if not math.isfinite(self.pre_grasp_distance): + raise ValueError("pre_grasp_distance must be finite.") + if self.pre_grasp_distance < 0.0: + raise ValueError("pre_grasp_distance must be non-negative.") + if not math.isfinite(self.twist_angle): + raise ValueError("twist_angle must be finite.") + + +class Twist(AtomicAction[TwistGoal, TwistOptions]): + """Open-loop approach, grasp, twist, release, and retract motion.""" + + skill_id: ClassVar[str] = "twist" + GoalType: ClassVar[type] = TwistGoal + OptionsType: ClassVar[type] = TwistOptions + open_loop: ClassVar[bool] = True + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + make_manipulation_slot( + "primary", + motion_capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } + ), + grasp_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, + ), + ), + ) + + def __init__(self, default_options: TwistOptions | None = None) -> None: + super().__init__(default_options) + + def _on_bind(self) -> None: + """Resolve dimensions owned by the engine's robot.""" + self.num_envs = self.robot.get_qpos().shape[0] + self.robot_dof = self.robot.dof + + def _find_symmetric_nearest_xpos( + self, target_xpos: torch.Tensor, reference_xpos: torch.Tensor + ) -> torch.Tensor: + """Find the nearest symmetric pose to the reference pose.""" + symmetric_xpos = target_xpos.clone() + symmetric_xpos[:, :3, 0] = -symmetric_xpos[:, :3, 0] + symmetric_xpos[:, :3, 1] = -symmetric_xpos[:, :3, 1] + angle_a = get_relative_rotation( + reference_xpos[:, :3, :3], target_xpos[:, :3, :3] + ) + angle_b = get_relative_rotation( + reference_xpos[:, :3, :3], symmetric_xpos[:, :3, :3] + ) + choose_target = (angle_a < angle_b)[..., None, None] + target_xpos = torch.where(choose_target, target_xpos, symmetric_xpos) + return target_xpos + + def _plan( + self, + request: ResolvedActionRequest[TwistGoal, TwistOptions], + context: PlanningContext, + ) -> ActionPlan: + """Plan all six twisting segments without stepping simulation.""" + target = self.require_goal(request) + affordance = self._require_twist_affordance(target.semantics) + options = request.skill_options + interpolation_dt = context.require_control_dt() + binding = request.binding + motion_target = binding.endpoint("primary", "motion").require_target( + JointPositionTarget + ) + grasp = binding.endpoint("primary", "grasp") + grasp_target = grasp.require_target(JointPositionTarget) + control_part = motion_target.control_part + arm_joint_ids = list(motion_target.joint_ids) + hand_joint_ids = list(grasp_target.joint_ids) + start_arm_qpos = arm_qpos_from_state(context, arm_joint_ids) + hand_open_qpos = grasp.joint_positions( + OPEN_COMMAND, + num_envs=context.batch_size, + device=self.device, + dtype=context.robot.qpos.dtype, + ) + hand_grasp_qpos = grasp.joint_positions( + GRASP_COMMAND, + num_envs=context.batch_size, + device=self.device, + dtype=context.robot.qpos.dtype, + ) + + link_pose = resolve_pose_target( + resolve_pose_goal(target.target_pose, context, name="target_pose"), + num_envs=self.num_envs, + device=self.device, + ) + grasp_xpos = affordance.get_grasp_pose(link_pose).to( + device=self.device, dtype=torch.float32 + ) + grasp_xpos = self._find_symmetric_nearest_xpos( + grasp_xpos, + reference_xpos=self.robot.compute_fk( + qpos=start_arm_qpos, name=control_part, to_matrix=True + ), + ) + pre_grasp_xpos = translate_pose_world( + grasp_xpos, + -grasp_xpos[:, :3, 2] * options.pre_grasp_distance, + ) + twist_xpos = self._twisted_grasp_poses( + link_pose, + grasp_xpos, + affordance.twist_axis, + affordance.axis_origin, + options.twist_angle, + options.twist_waypoint_count, + ) + + n_approach, n_reach, n_twist, n_retract = self._motion_segment_lengths( + request.motion_policy.sample_count, + options.hand_interp_steps, + ) + + approach_success, approach_arm = self._plan_pose_segment( + pre_grasp_xpos, + start_arm_qpos, + control_part, + request, + n_approach, + interpolation_dt=interpolation_dt, + ) + reach_success, reach_arm = self._plan_pose_segment( + grasp_xpos, + approach_arm[:, -1], + control_part, + request, + n_reach, + interpolation_dt=interpolation_dt, + ) + twist_success, twist_arm = self._plan_pose_segment( + twist_xpos, + reach_arm[:, -1], + control_part, + request, + n_twist, + interpolation_dt=interpolation_dt, + ) + retract_success, retract_arm = self._plan_pose_segment( + pre_grasp_xpos, + twist_arm[:, -1], + control_part, + request, + n_retract, + interpolation_dt=interpolation_dt, + ) + success = approach_success & reach_success & twist_success & retract_success + + hand_close = interpolate_hand_qpos( + hand_open_qpos, + hand_grasp_qpos, + n_waypoints=options.hand_interp_steps, + ) + hand_open = interpolate_hand_qpos( + hand_grasp_qpos, + hand_open_qpos, + n_waypoints=options.hand_interp_steps, + ) + parts = ( + approach_arm, + reach_arm, + hand_close, + twist_arm, + hand_open, + retract_arm, + ) + lengths = tuple(part.shape[1] for part in parts) + full = torch.empty( + (self.num_envs, sum(lengths), self.robot_dof), + dtype=context.robot.qpos.dtype, + device=self.device, + ) + full[:] = context.last_qpos.unsqueeze(1) + offset = 0 + arm_parts = (approach_arm, reach_arm, twist_arm, retract_arm) + arm_hands = ( + hand_open_qpos, + hand_open_qpos, + hand_grasp_qpos, + hand_open_qpos, + ) + for arm, hand in zip(arm_parts[:2], arm_hands[:2]): + stop = offset + arm.shape[1] + full[:, offset:stop, arm_joint_ids] = arm + full[:, offset:stop, hand_joint_ids] = hand.unsqueeze(1) + offset = stop + stop = offset + hand_close.shape[1] + full[:, offset:stop, arm_joint_ids] = reach_arm[:, -1].unsqueeze(1) + full[:, offset:stop, hand_joint_ids] = hand_close + offset = stop + stop = offset + twist_arm.shape[1] + full[:, offset:stop, arm_joint_ids] = twist_arm + full[:, offset:stop, hand_joint_ids] = hand_grasp_qpos.unsqueeze(1) + offset = stop + stop = offset + hand_open.shape[1] + full[:, offset:stop, arm_joint_ids] = twist_arm[:, -1].unsqueeze(1) + full[:, offset:stop, hand_joint_ids] = hand_open + offset = stop + full[:, offset:, arm_joint_ids] = retract_arm + full[:, offset:, hand_joint_ids] = hand_open_qpos.unsqueeze(1) + + return self.build_plan( + request, + context, + success=success, + trajectory=TimedTrajectory.from_uniform_step( + full, + env_ids=context.env_ids, + step_dt=interpolation_dt, + ), + expected_effects=StateDelta(), + segment_lengths={ + "approach": lengths[0], + "reach": lengths[1], + "close": lengths[2], + "twist": lengths[3], + "open": lengths[4], + "retract": lengths[5], + }, + ) + + @staticmethod + def _require_twist_affordance( + semantics: ObjectSemantics, + ) -> TwistAffordance: + affordance = semantics.affordance + if not isinstance(affordance, TwistAffordance): + raise ValueError("Twist requires a TwistAffordance.") + return affordance + + @staticmethod + def _motion_segment_lengths( + sample_count: int, + hand_interp_steps: int, + ) -> tuple[int, int, int, int]: + motion_count = sample_count - 2 * hand_interp_steps + if motion_count < 8: + raise ValueError( + "Not enough waypoints for Twist. Increase sample_count or " + "decrease hand_interp_steps." + ) + base, remainder = divmod(motion_count, 4) + values = [base + (index < remainder) for index in range(4)] + return values[0], values[1], values[2], values[3] + + def _plan_pose_segment( + self, + target_pose: torch.Tensor, + start_qpos: torch.Tensor, + control_part: str, + request: ResolvedActionRequest[TwistGoal, TwistOptions], + sample_count: int, + *, + interpolation_dt: float, + ) -> tuple[torch.Tensor, torch.Tensor]: + result = self.motion_generator.generate( + build_pose_plan_states(target_pose), + options=request.motion_policy.to_motion_gen_options( + start_qpos=start_qpos, + control_part=control_part, + sample_count=sample_count, + interpolation_dt=interpolation_dt, + ), + ) + assert isinstance(result.success, torch.Tensor) + assert result.positions is not None + return result.success, result.positions + + def _twisted_grasp_poses( + self, + link_pose: torch.Tensor, + grasp_xpos: torch.Tensor, + twist_axis: torch.Tensor, + axis_origin: tuple[float, float, float], + twist_angle: float, + waypoint_count: int, + ) -> torch.Tensor: + """Build Cartesian EEF keyframes that follow the target's twist arc.""" + axis = twist_axis.to(device=self.device, dtype=torch.float32) + axis = axis / torch.linalg.vector_norm(axis) + angles = torch.linspace( + twist_angle / waypoint_count, + twist_angle, + waypoint_count, + dtype=torch.float32, + device=self.device, + ) + rotations = ( + torch.eye(4, dtype=torch.float32, device=self.device) + .reshape(1, 4, 4) + .repeat(waypoint_count, 1, 1) + ) + rotations[:, :3, :3] = axis_angle_to_rotation_matrix(angles[:, None] * axis) + link_to_eef = torch.bmm(pose_inv(link_pose), grasp_xpos) + origin = torch.tensor(axis_origin, dtype=torch.float32, device=self.device) + to_origin = torch.eye(4, dtype=torch.float32, device=self.device) + from_origin = torch.eye(4, dtype=torch.float32, device=self.device) + to_origin[:3, 3] = origin + from_origin[:3, 3] = -origin + local_rotations = torch.matmul( + torch.matmul(to_origin[None], rotations), from_origin[None] + ) + return torch.matmul( + torch.matmul(link_pose[:, None], local_rotations[None]), + link_to_eef[:, None], + ) + + +__all__ = ["Twist", "TwistGoal", "TwistOptions"] diff --git a/embodichain/lab/sim/atomic_actions/requirements.py b/embodichain/lab/sim/atomic_actions/requirements.py new file mode 100644 index 000000000..7b62233d2 --- /dev/null +++ b/embodichain/lab/sim/atomic_actions/requirements.py @@ -0,0 +1,315 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Robot-independent resource requirements published by atomic skills.""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from types import MappingProxyType +from typing import Mapping + +from .control import ControlCommand + +JOINT_POSITION_CAPABILITY = "motion.joint_position" +"""Capability for planning and executing joint-position motion.""" + +CARTESIAN_POSE_CAPABILITY = "motion.cartesian_pose" +"""Capability for planning and executing Cartesian-pose motion.""" + +FORWARD_KINEMATICS_CAPABILITY = "kinematics.forward" +"""Capability for resolving forward kinematics for an endpoint.""" + +INVERSE_KINEMATICS_CAPABILITY = "kinematics.inverse" +"""Capability for resolving inverse kinematics for an endpoint.""" + +BATCH_INVERSE_KINEMATICS_CAPABILITY = "kinematics.batch_inverse" +"""Capability for resolving batched inverse kinematics for an endpoint.""" + +GRASP_CAPABILITY = "interaction.grasp" +"""Capability for commanding a grasping end effector.""" + + +def _validate_identifier(value: str, *, field_name: str) -> str: + """Return one strict, whitespace-free identifier.""" + if not isinstance(value, str) or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +def _normalize_identifiers( + values: frozenset[str], + *, + field_name: str, +) -> frozenset[str]: + """Validate one immutable identifier set.""" + if isinstance(values, (str, bytes)): + raise TypeError(f"{field_name} must be an iterable of strings, not a string.") + try: + normalized = frozenset(values) + except TypeError as exc: + raise TypeError(f"{field_name} must be an iterable of strings.") from exc + for value in normalized: + _validate_identifier(value, field_name=field_name) + return normalized + + +def _normalize_required_commands( + values: Mapping[str, type[ControlCommand]], +) -> Mapping[str, type[ControlCommand]]: + """Validate and freeze endpoint command requirements.""" + if not isinstance(values, Mapping): + raise TypeError("required_commands must be a mapping.") + normalized: dict[str, type[ControlCommand]] = {} + for name, command_type in values.items(): + _validate_identifier(name, field_name="required command names") + if not isinstance(command_type, type) or not issubclass( + command_type, ControlCommand + ): + raise TypeError( + "required_commands values must be ControlCommand subclasses." + ) + normalized[name] = command_type + return MappingProxyType(normalized) + + +@dataclass(frozen=True, slots=True) +class SkillEndpointRequirement: + """Capabilities and commands required from one slot-local endpoint.""" + + endpoint_id: str + """Endpoint selector local to the containing participant slot.""" + + capabilities: frozenset[str] = frozenset() + """Open, namespaced all-of capability identifiers.""" + + required_commands: Mapping[str, type[ControlCommand]] = field(default_factory=dict) + """Semantic command names and their required typed command contracts.""" + + def __post_init__(self) -> None: + _validate_identifier( + self.endpoint_id, + field_name="SkillEndpointRequirement.endpoint_id", + ) + object.__setattr__( + self, + "capabilities", + _normalize_identifiers( + self.capabilities, + field_name="SkillEndpointRequirement.capabilities", + ), + ) + object.__setattr__( + self, + "required_commands", + _normalize_required_commands(self.required_commands), + ) + + +@dataclass(frozen=True, slots=True) +class DisjointSlotEndpoints: + """Require selected endpoints within one participant to be disjoint.""" + + endpoint_ids: tuple[str, ...] + + def __post_init__(self) -> None: + if isinstance(self.endpoint_ids, (str, bytes)): + raise TypeError("endpoint_ids must be an iterable of endpoint IDs.") + try: + endpoint_ids = tuple(self.endpoint_ids) + except TypeError as exc: + raise TypeError( + "endpoint_ids must be an iterable of endpoint IDs." + ) from exc + if len(endpoint_ids) < 2: + raise ValueError("DisjointSlotEndpoints requires at least two endpoints.") + for endpoint_id in endpoint_ids: + _validate_identifier( + endpoint_id, + field_name="DisjointSlotEndpoints.endpoint_ids", + ) + if len(set(endpoint_ids)) != len(endpoint_ids): + raise ValueError("DisjointSlotEndpoints.endpoint_ids must be unique.") + object.__setattr__(self, "endpoint_ids", endpoint_ids) + + +@dataclass(frozen=True, slots=True) +class SkillResourceSlot: + """One skill-local participant selected as an indivisible resource unit.""" + + slot_id: str + """Skill-local participant name, such as ``primary`` or ``source``.""" + + endpoints: tuple[SkillEndpointRequirement, ...] + """Endpoint requirements that the selected robot resource must satisfy.""" + + constraints: tuple[DisjointSlotEndpoints, ...] = () + """Physical constraints among endpoint views in this participant.""" + + def __post_init__(self) -> None: + _validate_identifier(self.slot_id, field_name="SkillResourceSlot.slot_id") + if isinstance(self.endpoints, (str, bytes)): + raise TypeError( + "SkillResourceSlot.endpoints must be an iterable of endpoint " + "requirements." + ) + try: + endpoints = tuple(self.endpoints) + except TypeError as exc: + raise TypeError( + "SkillResourceSlot.endpoints must be an iterable of endpoint " + "requirements." + ) from exc + if not endpoints or not all( + isinstance(endpoint, SkillEndpointRequirement) for endpoint in endpoints + ): + raise ValueError( + "SkillResourceSlot.endpoints must contain at least one " + "SkillEndpointRequirement." + ) + endpoint_ids = [endpoint.endpoint_id for endpoint in endpoints] + if len(set(endpoint_ids)) != len(endpoint_ids): + raise ValueError( + f"Skill resource slot {self.slot_id!r} contains duplicate endpoint " + "identifiers." + ) + object.__setattr__(self, "endpoints", endpoints) + if isinstance(self.constraints, (str, bytes)): + raise TypeError( + "SkillResourceSlot.constraints must be an iterable of endpoint " + "constraints." + ) + try: + constraints = tuple(self.constraints) + except TypeError as exc: + raise TypeError( + "SkillResourceSlot.constraints must be an iterable of endpoint " + "constraints." + ) from exc + if not all( + isinstance(constraint, DisjointSlotEndpoints) for constraint in constraints + ): + raise TypeError( + "SkillResourceSlot.constraints values must be " + "DisjointSlotEndpoints instances." + ) + known_endpoints = set(endpoint_ids) + for constraint in constraints: + unknown = sorted(set(constraint.endpoint_ids) - known_endpoints) + if unknown: + raise ValueError( + f"Slot {self.slot_id!r} constraint references unknown endpoints " + f"{unknown}; known endpoints are {sorted(known_endpoints)}." + ) + object.__setattr__(self, "constraints", constraints) + + +@dataclass(frozen=True, slots=True) +class DisjointResourceSlots: + """Require selected slots to have pairwise-disjoint physical claims.""" + + slots: tuple[str, ...] + + def __post_init__(self) -> None: + if isinstance(self.slots, (str, bytes)): + raise TypeError("DisjointResourceSlots.slots must be an iterable.") + try: + slots = tuple(self.slots) + except TypeError as exc: + raise TypeError("DisjointResourceSlots.slots must be an iterable.") from exc + if len(slots) < 2: + raise ValueError("DisjointResourceSlots requires at least two slots.") + for slot in slots: + _validate_identifier(slot, field_name="DisjointResourceSlots.slots") + if len(set(slots)) != len(slots): + raise ValueError("DisjointResourceSlots.slots must be unique.") + object.__setattr__(self, "slots", slots) + + +@dataclass(frozen=True, slots=True) +class SkillBindingContract: + """Complete robot-independent binding contract for one atomic skill. + + ``slots=()`` explicitly declares that a skill consumes no robot resource. + ``None`` on :class:`~embodichain.lab.sim.atomic_actions.SkillDescriptor` + instead means that no semantic binding contract was declared. + """ + + slots: tuple[SkillResourceSlot, ...] = () + constraints: tuple[DisjointResourceSlots, ...] = () + + def __post_init__(self) -> None: + if isinstance(self.slots, (str, bytes)): + raise TypeError("slots must be an iterable of SkillResourceSlot values.") + try: + slots = tuple(self.slots) + except TypeError as exc: + raise TypeError( + "slots must be an iterable of SkillResourceSlot values." + ) from exc + if not all(isinstance(slot, SkillResourceSlot) for slot in slots): + raise TypeError("slots values must be SkillResourceSlot instances.") + slot_ids = [slot.slot_id for slot in slots] + if len(set(slot_ids)) != len(slot_ids): + raise ValueError("SkillBindingContract slot identifiers must be unique.") + if isinstance(self.constraints, (str, bytes)): + raise TypeError( + "constraints must be an iterable of DisjointResourceSlots values." + ) + try: + constraints = tuple(self.constraints) + except TypeError as exc: + raise TypeError( + "constraints must be an iterable of DisjointResourceSlots values." + ) from exc + if not all( + isinstance(constraint, DisjointResourceSlots) for constraint in constraints + ): + raise TypeError( + "constraints values must be DisjointResourceSlots instances." + ) + known_slots = set(slot_ids) + for constraint in constraints: + unknown = sorted(set(constraint.slots) - known_slots) + if unknown: + raise ValueError( + f"Resource constraint references unknown slots {unknown}; " + f"known slots are {sorted(known_slots)}." + ) + object.__setattr__(self, "slots", slots) + object.__setattr__(self, "constraints", constraints) + + @property + def slot_ids(self) -> tuple[str, ...]: + """Return required slot identifiers in declaration order.""" + return tuple(slot.slot_id for slot in self.slots) + + +__all__ = [ + "BATCH_INVERSE_KINEMATICS_CAPABILITY", + "CARTESIAN_POSE_CAPABILITY", + "DisjointResourceSlots", + "DisjointSlotEndpoints", + "FORWARD_KINEMATICS_CAPABILITY", + "GRASP_CAPABILITY", + "INVERSE_KINEMATICS_CAPABILITY", + "JOINT_POSITION_CAPABILITY", + "SkillBindingContract", + "SkillEndpointRequirement", + "SkillResourceSlot", +] diff --git a/embodichain/lab/sim/atomic_actions/runner.py b/embodichain/lab/sim/atomic_actions/runner.py index 38c504136..bd231598b 100644 --- a/embodichain/lab/sim/atomic_actions/runner.py +++ b/embodichain/lab/sim/atomic_actions/runner.py @@ -19,7 +19,7 @@ from __future__ import annotations from collections.abc import Callable -from dataclasses import dataclass +from dataclasses import dataclass, replace from enum import Enum import math import time @@ -29,12 +29,20 @@ from embodichain.utils import configclass +from .bindings import RuntimeEndpointTarget from .execution import ( + EffectVerificationRequest, + EffectVerificationResult, ExecutionSession, ExecutionStatus, ExecutionTick, - JointCommand, + HeldObjectGuardRequest, + HeldObjectGuardResult, + PhaseEffectGateRequest, + PhaseEffectGateResult, ) +from .invocation import ActionInvocation, ResolvedActionRequest +from .runtime_commands import RuntimeCommandFrame from .state import PlanningContext, TaskState @@ -123,15 +131,17 @@ class CommandSink(Protocol): def send( self, - command: JointCommand, + command: RuntimeCommandFrame, *, timeout: float, ) -> CommandAcknowledgement: - """Submit an active joint command and acknowledge its acceptance. + """Submit one synchronized endpoint-command frame. Args: - command: Full-robot command with an explicit active mask. Inactive - rows contain hold targets and must not retain stale commands. + command: Transport-neutral command frame with an active-row mask. + The sink must actively neutralize inactive rows for every + addressed target; omission is not a safe state for persistent + controllers. timeout: Maximum acknowledgement latency in seconds. Returns: @@ -140,24 +150,32 @@ def send( def hold( self, - command: JointCommand, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, *, timeout: float, ) -> CommandAcknowledgement: - """Hold the supplied observed position as a safety command. + """Apply transport-specific safe state to the supplied targets. Args: - command: Full-robot observed-position hold command. + targets: Runtime targets that may retain controller state. + context: Latest observation used by position-hold transports. timeout: Maximum acknowledgement latency in seconds. Returns: Transport or controller acknowledgement. """ - def cancel(self, *, timeout: float) -> CommandAcknowledgement: + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: """Cancel any controller-side command that has not completed. Args: + targets: Runtime targets whose queued work must be cancelled. timeout: Maximum acknowledgement latency in seconds. Returns: @@ -222,6 +240,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) @@ -231,6 +258,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): @@ -279,8 +308,23 @@ def is_waiting(self) -> bool: ) -EffectVerifier = Callable[[PlanningContext, ExecutionTick], torch.Tensor | None] -"""Callback that verifies a pending semantic effect for each environment.""" +EffectVerifier = Callable[ + [PlanningContext, EffectVerificationRequest], + EffectVerificationResult, +] +"""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.""" + +PhaseEffectGateVerifier = Callable[ + [PlanningContext, PhaseEffectGateRequest], + PhaseEffectGateResult, +] +"""Synchronous verifier for one blocking trajectory-segment entry gate.""" RunnerStepCallback = Callable[[RunnerStep], None] """Optional observer called after every blocking runner-loop iteration.""" @@ -290,10 +334,13 @@ class ExecutionRunner: """Connect an execution session to observation, controller, and time ports. :meth:`step` is non-blocking. It observes and advances the session only when - the next command is due according to :attr:`JointCommand.hold_duration`. + the next command is due according to + :attr:`RuntimeCommandFrame.hold_duration`. :meth:`run_until_blocked` supplies the blocking loop for tutorials and simple applications. Controller rejection, timeout, observation failure, and session exceptions all trigger a best-effort cancel-then-hold sequence. + Runner methods are designed for serialized event-loop use and are not + thread-safe. Args: session: Stateful atomic-action execution session. @@ -334,10 +381,17 @@ def __init__( self._message: str | None = None self._effect_context: PlanningContext | None = None self._effect_tick: ExecutionTick | None = None + self._armed_targets: dict[tuple[str, str], RuntimeEndpointTarget] = {} + self._pending_revision: ResolvedActionRequest | None = None @property def session(self) -> ExecutionSession: - """Execution session advanced by this runner.""" + """Execution session advanced by this runner. + + Call :meth:`revise_current` or :meth:`deactivate_rows` on the runner, + rather than mutating the session directly, while this runner owns + scheduling. + """ return self._session @property @@ -358,22 +412,139 @@ def effect_verification_pending(self) -> bool: and self._effect_tick.pending_effect is not None ) + def revise_current(self, invocation: ActionInvocation) -> None: + """Stage a newer revision for the next scheduled observation boundary. + + Staging preserves the active frame deadline. When that deadline is due, + :meth:`step` observes fresh state, atomically plans and installs the + replacement, and dispatches its first command. The submitted invocation + is resolved into an owned snapshot immediately, so later caller + mutation cannot alter the staged revision. + + Args: + invocation: Strictly newer revision of the active logical call. + + Raises: + TypeError: If ``invocation`` is not an ActionInvocation. + RuntimeError: If this runner or its session is no longer running, + or if a physical effect is awaiting verification. + ValueError: If session-level revision invariants are violated. + """ + if not isinstance(invocation, ActionInvocation): + raise TypeError("invocation must be an ActionInvocation.") + if self._status is not RunnerStatus.RUNNING: + raise RuntimeError("Only a running execution runner can be revised.") + prepared = self._session._prepare_revision(invocation) + if ( + self._pending_revision is not None + and prepared.revision <= self._pending_revision.revision + ): + raise ValueError( + "A staged revision must advance beyond the pending revision " + f"{self._pending_revision.revision}, got {prepared.revision}." + ) + self._pending_revision = prepared + + def deactivate_rows( + self, + env_mask: torch.Tensor, + *, + reason: str, + ) -> torch.Tensor: + """Permanently deactivate environment rows owned by this runner. + + The runner refreshes its cached effect boundary so a verifier cannot + submit a result correlated with a request that deactivation replaced. + In-flight controller work is neutralized for those rows by the next + due command frame according to the :class:`CommandSink` contract. + + Args: + env_mask: Rows requested for deactivation. + reason: Human-readable event message. + + Returns: + Owned mask of rows that changed from eligible to inactive. + + Raises: + RuntimeError: If the runner is already terminal. + TypeError: If ``env_mask`` is not a tensor. + ValueError: If the mask or reason is invalid. + """ + if self._status is not RunnerStatus.RUNNING: + raise RuntimeError("Only a running execution runner can deactivate rows.") + changed = self._session.deactivate_rows(env_mask, reason=reason) + if self._session.status is not ExecutionStatus.RUNNING: + self._pending_revision = None + pending_effect = self._session.pending_effect + if pending_effect is None: + self._clear_effect_boundary() + elif self._effect_tick is not None: + self._effect_tick = replace( + self._effect_tick, + status=self._session.status, + eligible_mask=self._session.eligible_mask, + task_state=self._session.task_state, + pending_effect=pending_effect, + ) + return changed + def step( self, *, - effect_success: torch.Tensor | None = None, + effect_result: EffectVerificationResult | None = None, + effect_verifier: EffectVerifier | None = None, + phase_effect_gate_result: PhaseEffectGateResult | None = None, + phase_effect_gate_verifier: PhaseEffectGateVerifier | None = None, + held_object_guard_verifier: HeldObjectGuardVerifier | None = None, ) -> RunnerStep: """Perform one due observation/session/controller update without sleeping. Args: - effect_success: Optional per-environment verification mask. If this - call occurs before the next cycle is due, it is not consumed and + effect_result: Optional correlated effect result. If this call + occurs before the next cycle is due, it is not consumed and must be supplied again on a later call. + effect_verifier: Optional synchronous verifier for the current + pending request. It runs after a fresh due-cycle observation + and before the session consumes the result. It is not called + after the request deadline. Mutually exclusive with + ``effect_result``. + phase_effect_gate_result: Optional externally produced result for + the current blocking trajectory-segment entry gate. + phase_effect_gate_verifier: Optional synchronous verifier for the + current gate. It runs on a fresh due-cycle observation and is + mutually exclusive with ``phase_effect_gate_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, and time remaining before another update is due. """ + if effect_result is not None and effect_verifier is not None: + raise ValueError( + "effect_result and effect_verifier are mutually exclusive." + ) + if ( + phase_effect_gate_result is not None + and phase_effect_gate_verifier is not None + ): + raise ValueError( + "phase_effect_gate_result and phase_effect_gate_verifier are " + "mutually exclusive." + ) + 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.") + if phase_effect_gate_verifier is not None and not callable( + phase_effect_gate_verifier + ): + raise TypeError("phase_effect_gate_verifier must be callable or None.") now = self._clock_now() if self._status is not RunnerStatus.RUNNING: return self._result(timestamp=now) @@ -398,7 +569,96 @@ def step( self._last_context = context try: - tick = self._session.tick(context, effect_success=effect_success) + 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 + and pending_effect is not None + and context.robot.timestamp <= pending_effect.deadline + ): + try: + effect_result = effect_verifier(context, pending_effect) + if type(effect_result) is not EffectVerificationResult: + raise TypeError( + "EffectVerifier must return exactly " + "EffectVerificationResult." + ) + except Exception as exc: + return self._fail( + f"Effect verifier failed: {type(exc).__name__}: {exc}", + context=context, + ) + + phase_effect_gate_request = self._session.phase_effect_gate_request + if ( + phase_effect_gate_verifier is not None + and phase_effect_gate_request is not None + and context.robot.timestamp <= phase_effect_gate_request.deadline + ): + try: + phase_effect_gate_result = phase_effect_gate_verifier( + context, + phase_effect_gate_request, + ) + if type(phase_effect_gate_result) is not PhaseEffectGateResult: + raise TypeError( + "PhaseEffectGateVerifier must return exactly " + "PhaseEffectGateResult." + ) + except Exception as exc: + return self._fail( + "Phase-effect gate verifier failed: " + f"{type(exc).__name__}: {exc}", + context=context, + ) + + 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, + ) + 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, + phase_effect_gate_result=phase_effect_gate_result, + held_object_guard_result=held_object_guard_result, + ) + context = self._session.latest_context + self._last_context = context except Exception as exc: return self._fail( f"Execution session failed: {type(exc).__name__}: {exc}", @@ -408,12 +668,18 @@ def step( dispatches: list[CommandDispatch] = [] if tick.command is not None: + self._remember_targets(tick.command.targets) operation = ( CommandOperation.SEND if bool(tick.command.active_mask.any().item()) else CommandOperation.HOLD ) - dispatch = self._dispatch(operation, tick.command) + dispatch = self._dispatch( + operation, + command=(tick.command if operation is CommandOperation.SEND else None), + targets=tick.command.targets, + context=context, + ) dispatches.append(dispatch) if not dispatch.acknowledgement.accepted: failure = dispatch.acknowledgement @@ -433,6 +699,34 @@ def step( self._command_count += 1 interval = self._command_interval(tick.command) self._next_step_at = self._clock_now() + interval + 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, + targets=tick.hold_targets, + context=context, + ) + dispatches.append(hold_dispatch) + if not hold_dispatch.acknowledgement.accepted: + failure = hold_dispatch.acknowledgement + message = ( + "Controller did not accept the requested hold: " + f"{failure.status.value}." + ) + if failure.message: + message += f" {failure.message}" + return self._fail( + message, + context=context, + tick=tick, + dispatches=dispatches, + ) + 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() @@ -440,7 +734,8 @@ def step( if self.cfg.hold_on_completion: hold_dispatch = self._dispatch( CommandOperation.HOLD, - self._hold_command(context), + targets=self._armed_target_snapshots(), + context=context, ) dispatches.append(hold_dispatch) if not hold_dispatch.acknowledgement.accepted: @@ -461,7 +756,7 @@ def step( self._next_step_at = self._clock_now() elif tick.status is ExecutionStatus.FAILED: return self._fail( - "Execution session exhausted its recovery budget.", + "Execution session failed; inspect its terminal events for the cause.", context=context, tick=tick, dispatches=dispatches, @@ -499,6 +794,7 @@ def cancel(self, reason: str = "Execution cancelled by caller.") -> RunnerStep: self._status = RunnerStatus.FAILED self._message = f"{reason} Safe stop acknowledgement failed." self._clear_effect_boundary() + self._pending_revision = None self._next_step_at = self._clock_now() return self._result( timestamp=self._clock_now(), @@ -510,15 +806,22 @@ def run_until_blocked( self, *, effect_verifier: EffectVerifier | None = None, + phase_effect_gate_verifier: PhaseEffectGateVerifier | None = None, + held_object_guard_verifier: HeldObjectGuardVerifier | None = None, on_step: RunnerStepCallback | None = None, max_steps: int = 100_000, ) -> RunnerStep: """Run with clock-driven waiting until terminal or effect verification blocks. Args: - effect_verifier: Optional callback used after an - ``effect_verification_required`` event. Without one, the method - returns the running step so the caller can verify externally. + effect_verifier: Optional synchronous callback used on fresh + due-cycle observations while effect verification is pending. + Without one, the method returns the running boundary so the + caller can verify externally. + phase_effect_gate_verifier: Optional synchronous callback used on + fresh observations while a trajectory-segment entry is gated. + 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. @@ -527,7 +830,6 @@ def run_until_blocked( """ if max_steps <= 0: raise ValueError("max_steps must be greater than zero.") - effect_success: torch.Tensor | None = None now = self._clock_now() last_result = self._result( timestamp=now, @@ -535,30 +837,14 @@ def run_until_blocked( context=self._effect_context, tick=self._effect_tick, ) - if self.effect_verification_pending: - if ( - effect_verifier is None - or self._effect_context is None - or self._effect_tick is None - ): - return last_result - try: - effect_success = effect_verifier( - self._effect_context, - self._effect_tick, - ) - except Exception as exc: - return self._fail( - f"Effect verifier failed: {type(exc).__name__}: {exc}", - context=self._effect_context, - tick=self._effect_tick, - ) - if effect_success is None: - return last_result + if self.effect_verification_pending and effect_verifier is None: + return last_result for _ in range(max_steps): - result = self.step(effect_success=effect_success) - if result.tick is not None: - effect_success = None + result = self.step( + effect_verifier=effect_verifier, + phase_effect_gate_verifier=phase_effect_gate_verifier, + held_object_guard_verifier=held_object_guard_verifier, + ) if on_step is not None: try: on_step(result) @@ -575,20 +861,14 @@ def run_until_blocked( verification_required = ( result.tick is not None and result.tick.pending_effect is not None ) - if verification_required: - if effect_verifier is None or result.context is None: - return result - try: - effect_success = effect_verifier(result.context, result.tick) - except Exception as exc: - return self._fail( - f"Effect verifier failed: {type(exc).__name__}: {exc}", - context=result.context, - tick=result.tick, - dispatches=list(result.dispatches), - ) - if effect_success is None: - return result + if verification_required and effect_verifier is None: + return result + gate_required = ( + result.tick is not None + and result.tick.pending_phase_effect_gate is not None + ) + if gate_required and phase_effect_gate_verifier is None: + return result if result.wait_duration > 0.0: try: self._clock.sleep(result.wait_duration) @@ -630,7 +910,7 @@ def _clock_now(self) -> float: raise ValueError("ExecutionClock.now() must be finite and non-negative.") return value - def _command_interval(self, command: JointCommand) -> float: + def _command_interval(self, command: RuntimeCommandFrame) -> float: """Resolve a synchronized batch interval from per-environment durations.""" durations = ( command.hold_duration[command.active_mask] @@ -649,27 +929,31 @@ def _remaining_wait(self, now: float) -> float: def _dispatch( self, operation: CommandOperation, - command: JointCommand | None, + command: RuntimeCommandFrame | None = None, + *, + targets: tuple[RuntimeEndpointTarget, ...] = (), + context: PlanningContext | None = None, ) -> CommandDispatch: """Call one sink operation and convert exceptions to rejection acks.""" try: if operation is CommandOperation.SEND: if command is None: - raise ValueError("SEND requires a JointCommand.") + raise ValueError("SEND requires a RuntimeCommandFrame.") acknowledgement = self._command_sink.send( command, timeout=self.cfg.command_timeout, ) elif operation is CommandOperation.HOLD: - if command is None: - raise ValueError("HOLD requires a JointCommand.") + if context is None: + raise ValueError("HOLD requires a PlanningContext.") acknowledgement = self._command_sink.hold( - command, + targets, + context, timeout=self.cfg.safe_stop_timeout, ) else: acknowledgement = self._command_sink.cancel( - timeout=self.cfg.safe_stop_timeout + targets, timeout=self.cfg.safe_stop_timeout ) if not isinstance(acknowledgement, CommandAcknowledgement): raise TypeError( @@ -682,6 +966,19 @@ def _dispatch( ) return CommandDispatch(operation, acknowledgement) + def _remember_targets( + self, + targets: tuple[RuntimeEndpointTarget, ...], + ) -> None: + """Remember every controller destination armed during this run.""" + for target in targets: + key = (target.transport_id, target.target_id) + self._armed_targets[key] = target.snapshot() + + def _armed_target_snapshots(self) -> tuple[RuntimeEndpointTarget, ...]: + """Return owned armed targets in first-use order.""" + return tuple(target.snapshot() for target in self._armed_targets.values()) + def _observe_for_stop(self) -> PlanningContext | None: """Best-effort observation used to build a cancellation hold command.""" try: @@ -698,32 +995,18 @@ def _safe_stop( context: PlanningContext | None, ) -> list[CommandDispatch]: """Attempt controller cancellation followed by an observed-position hold.""" - dispatches = [self._dispatch(CommandOperation.CANCEL, None)] + targets = self._armed_target_snapshots() + dispatches = [self._dispatch(CommandOperation.CANCEL, targets=targets)] if context is not None: dispatches.append( - self._dispatch(CommandOperation.HOLD, self._hold_command(context)) + self._dispatch( + CommandOperation.HOLD, + targets=targets, + context=context, + ) ) return dispatches - @staticmethod - def _hold_command(context: PlanningContext) -> JointCommand: - """Build an all-environment passive hold command from an observation.""" - return JointCommand( - positions=context.robot.qpos, - velocities=torch.zeros_like(context.robot.qpos), - active_mask=torch.zeros( - context.batch_size, - dtype=torch.bool, - device=context.robot.qpos.device, - ), - env_ids=context.env_ids, - hold_duration=torch.zeros( - context.batch_size, - dtype=torch.float32, - device=context.robot.qpos.device, - ), - ) - def _fail( self, message: str, @@ -738,6 +1021,7 @@ def _fail( self._status = RunnerStatus.FAILED self._message = message self._clear_effect_boundary() + self._pending_revision = None self._next_step_at = self._clock_now() return self._result( timestamp=self._clock_now(), @@ -778,8 +1062,10 @@ def _result( "ExecutionClock", "ExecutionRunner", "ExecutionRunnerCfg", + "HeldObjectGuardVerifier", "MonotonicExecutionClock", "ObservationProvider", + "PhaseEffectGateVerifier", "RunnerStatus", "RunnerStep", "RunnerStepCallback", diff --git a/embodichain/lab/sim/atomic_actions/runtime.py b/embodichain/lab/sim/atomic_actions/runtime.py index b8a62d9d0..868d31bc6 100644 --- a/embodichain/lab/sim/atomic_actions/runtime.py +++ b/embodichain/lab/sim/atomic_actions/runtime.py @@ -21,16 +21,26 @@ from collections.abc import Mapping from types import MappingProxyType from typing import TYPE_CHECKING +from uuid import uuid4 import torch -from .bindings import ActionBinding, ResolvedActionBinding, ResolvedControlPart -from .control import ( - ActionControlOverrides, - ControlCommand, - ControlPartCommandProfile, -) +from .bindings import ActionBinding, EndpointBinding, JointPositionTarget +from .control import ActionControlOverrides, ControlPartCommandProfile from .core import resolve_runtime_device +from .requirements import ( + DisjointResourceSlots, + DisjointSlotEndpoints, + SkillBindingContract, +) +from .tracking import ( + JOINT_POSITION_CHANNEL, + EndpointTrackingChannelBinding, + EndpointTrackingFeedbackAddress, + TrackingFeedbackSourceRef, + TrackingProjectorRef, + TrackingRuntime, +) if TYPE_CHECKING: from embodichain.lab.sim.objects import Robot @@ -38,34 +48,27 @@ class ActionPlanningServices: - """Planning resources exclusively owned by one atomic-action engine. - - An action may borrow these resources after the engine binds it, but callers - never pass a motion generator to individual actions. Keeping the generator - here gives one engine a single planner backend, robot, device, cache, and - collision-world owner. - - Args: - motion_generator: Motion generator owned by the engine. - control_profiles: Semantic command profiles keyed by names from the - owned robot's ``control_parts`` mapping. - """ + """Planning resources exclusively owned by one atomic-action engine.""" def __init__( self, motion_generator: MotionGenerator, control_profiles: Mapping[str, ControlPartCommandProfile] | None = None, + tracking_runtime: TrackingRuntime | None = None, ) -> None: self._motion_generator = motion_generator self._robot: Robot = motion_generator.robot self._device = resolve_runtime_device(motion_generator.device) + self._binding_owner_id = uuid4().hex + if tracking_runtime is not None and not isinstance( + tracking_runtime, + TrackingRuntime, + ): + raise TypeError("tracking_runtime must be a TrackingRuntime or None.") + self._tracking_runtime = tracking_runtime or TrackingRuntime.with_builtins() self._control_profiles = self._snapshot_control_profiles( {} if control_profiles is None else control_profiles ) - self._binding_cache: dict[ - tuple[tuple[tuple[str, str], ...], tuple[tuple[str, str], ...]], - ResolvedActionBinding, - ] = {} @property def motion_generator(self) -> MotionGenerator: @@ -82,9 +85,19 @@ def device(self) -> torch.device: """Return the concrete device used for planning.""" return self._device + @property + def binding_owner_id(self) -> str: + """Return the opaque identity required by this engine's bindings.""" + return self._binding_owner_id + + @property + def tracking_runtime(self) -> TrackingRuntime: + """Return the engine-owned typed tracking runtime.""" + return self._tracking_runtime + @property def control_profiles(self) -> Mapping[str, ControlPartCommandProfile]: - """Return owned semantic command profiles keyed by control-part name.""" + """Return owned direct-core command profiles by control-part name.""" return MappingProxyType( { name: profile.snapshot() @@ -101,110 +114,293 @@ def planner_name(self) -> str: planner_name = getattr(planner_cfg, "planner_type", None) return "unknown" if planner_name is None else str(planner_name) - def resolve_binding( + def bind_control_parts( self, - binding: ActionBinding, - control_overrides: ActionControlOverrides | None = None, - ) -> ResolvedActionBinding: - """Resolve binding names against the owned robot's control parts. + contract: SkillBindingContract, + endpoints: Mapping[str, Mapping[str, str]], + *, + task_state_keys: Mapping[str, str] | None = None, + ) -> ActionBinding: + """Build a generic binding from explicit robot control-part names. - ``ActionBinding`` deliberately carries stable string references only. - This method establishes that every reference is a key in - ``Robot.control_parts`` and resolves its full-robot joint indices. + This is the advanced direct-core construction path. Profile-backed + callers obtain the same :class:`ActionBinding` from + ``BoundRobotSkillProfile.resolve()``. Args: - binding: Semantic-role mapping to validate and resolve. - control_overrides: Optional per-role command replacements for this - invocation revision. + contract: Typed endpoint contract for the bound skill. + endpoints: Nested ``slot_id -> endpoint_id -> control_part`` mapping. + task_state_keys: Optional explicit stable task-state key for each + resource slot. When omitted, a slot inherits its ``motion`` + endpoint's control part. A slot without ``motion`` can be + inferred only when all of its endpoints use one control part. Returns: - Immutable runtime resources for action planning. - - Raises: - TypeError: If ``binding`` or ``Robot.control_parts`` is invalid. - ValueError: If a referenced control part is unknown or empty. + Engine-owned generic endpoint binding. """ - if not isinstance(binding, ActionBinding): - raise TypeError("binding must be an ActionBinding.") - cache_key = ( - tuple(sorted(binding.manipulators.items())), - tuple(sorted(binding.end_effectors.items())), - ) - resolved = self._binding_cache.get(cache_key) - if resolved is None: - control_parts = getattr(self.robot, "control_parts", None) - if not isinstance(control_parts, Mapping): - if binding.manipulators or binding.end_effectors: - raise TypeError( - "ActionBinding resources must come from " - "Robot.control_parts, but the engine robot does not " - "define a control-parts mapping." + if not isinstance(contract, SkillBindingContract): + raise TypeError("contract must be a SkillBindingContract.") + if not isinstance(endpoints, Mapping): + raise TypeError("endpoints must be a slot-to-endpoint mapping.") + expected = { + (slot.slot_id, requirement.endpoint_id): requirement + for slot in contract.slots + for requirement in slot.endpoints + } + supplied: dict[tuple[str, str], str] = {} + for slot_id, slot_endpoints in endpoints.items(): + if not isinstance(slot_id, str) or not slot_id.strip(): + raise ValueError("Binding slot IDs must be non-empty strings.") + if not isinstance(slot_endpoints, Mapping): + raise TypeError(f"Binding slot {slot_id!r} must contain a mapping.") + for endpoint_id, control_part in slot_endpoints.items(): + key = (slot_id, endpoint_id) + if key in supplied: + raise ValueError( + f"Binding endpoint {slot_id}.{endpoint_id} repeats." ) - control_parts = {} - - resolved = ResolvedActionBinding( - manipulators=self._resolve_resource_map( - binding.manipulators, - control_parts=control_parts, - resource_kind="manipulator", - ), - end_effectors=self._resolve_resource_map( - binding.end_effectors, - control_parts=control_parts, - resource_kind="end effector", - ), + if not isinstance(endpoint_id, str) or not endpoint_id.strip(): + raise ValueError("Binding endpoint IDs must be non-empty strings.") + if not isinstance(control_part, str) or not control_part.strip(): + raise ValueError("Control-part names must be non-empty strings.") + supplied[key] = control_part + if set(supplied) != set(expected): + missing = sorted(set(expected) - set(supplied)) + extra = sorted(set(supplied) - set(expected)) + raise ValueError( + "Direct binding must cover the skill contract exactly: " + f"missing={missing}, extra={extra}." ) - self._binding_cache[cache_key] = resolved - - if control_overrides is None: - return resolved - if not isinstance(control_overrides, ActionControlOverrides): - raise TypeError("control_overrides must be an ActionControlOverrides.") - if control_overrides.is_empty: - return resolved - return ResolvedActionBinding( - manipulators=self._apply_command_overrides( - resolved.manipulators, - control_overrides.manipulators, - resource_kind="manipulator", - ), - end_effectors=self._apply_command_overrides( - resolved.end_effectors, - control_overrides.end_effectors, - resource_kind="end effector", - ), - ) + slot_ids = {slot.slot_id for slot in contract.slots} + if task_state_keys is not None: + if not isinstance(task_state_keys, Mapping): + raise TypeError("task_state_keys must be a slot-to-key mapping.") + for slot_id, task_state_key in task_state_keys.items(): + if ( + not isinstance(slot_id, str) + or not slot_id + or slot_id != slot_id.strip() + ): + raise ValueError( + "task_state_keys slot IDs must be non-empty strings " + "without outer whitespace." + ) + if not isinstance(task_state_key, str) or not task_state_key.strip(): + raise ValueError( + "task_state_keys values must be non-empty strings." + ) + if task_state_key != task_state_key.strip(): + raise ValueError( + "task_state_keys values must not contain outer whitespace." + ) + supplied_task_slots = set(task_state_keys) + if supplied_task_slots != slot_ids: + missing = sorted(slot_ids - supplied_task_slots) + extra = sorted(supplied_task_slots - slot_ids) + raise ValueError( + "task_state_keys must cover the binding slots exactly: " + f"missing={missing}, extra={extra}." + ) + if not expected: + binding = ActionBinding(owner_id=self.binding_owner_id) + self.validate_binding(binding, contract) + return binding - def _resolve_resource_map( - self, - resources: Mapping[str, str], - *, - control_parts: Mapping[str, object], - resource_kind: str, - ) -> dict[str, ResolvedControlPart]: - """Resolve one role map through ``Robot.control_parts``.""" + control_parts = getattr(self.robot, "control_parts", None) + if not isinstance(control_parts, Mapping): + raise TypeError("Direct control-part binding requires Robot.control_parts.") available = sorted(str(name) for name in control_parts) - resolved: dict[str, ResolvedControlPart] = {} - for role, name in resources.items(): - if name not in control_parts: + resolved_task_state_keys: dict[str, str] + if task_state_keys is not None: + resolved_task_state_keys = dict(task_state_keys) + else: + resolved_task_state_keys = {} + for slot in contract.slots: + motion_key = (slot.slot_id, "motion") + if motion_key in supplied: + resolved_task_state_keys[slot.slot_id] = supplied[motion_key] + continue + slot_control_parts = { + supplied[(slot.slot_id, endpoint.endpoint_id)] + for endpoint in slot.endpoints + } + if len(slot_control_parts) != 1: + raise ValueError( + f"Direct binding slot {slot.slot_id!r} has no 'motion' " + "endpoint and spans multiple control parts; provide an " + "explicit task_state_keys entry for this slot." + ) + resolved_task_state_keys[slot.slot_id] = next(iter(slot_control_parts)) + resolved: list[EndpointBinding] = [] + for key, requirement in expected.items(): + slot_id, endpoint_id = key + control_part = supplied[key] + if control_part not in control_parts: raise ValueError( - f"ActionBinding {resource_kind} role {role!r} references " - f"control part {name!r}, but Robot.control_parts contains " - f"{available}." + f"Endpoint {slot_id}.{endpoint_id} references control part " + f"{control_part!r}, but Robot.control_parts contains {available}." ) - joint_ids = tuple(self.robot.get_joint_ids(name=name)) + joint_ids = tuple(self.robot.get_joint_ids(name=control_part)) if not joint_ids: + raise ValueError(f"Control part {control_part!r} contains no joints.") + profile = self._control_profiles.get(control_part) + commands = {} if profile is None else profile.commands + for name, command_type in requirement.required_commands.items(): + command = commands.get(name) + if not isinstance(command, command_type): + raise ValueError( + f"Endpoint {slot_id}.{endpoint_id} requires command {name!r} " + f"of type {command_type.__name__}." + ) + target = JointPositionTarget(control_part, joint_ids) + resolved.append( + EndpointBinding( + slot_id=slot_id, + endpoint_id=endpoint_id, + resource_id=f"direct.{slot_id}", + adapter_id="control_part", + target=target, + task_state_key=resolved_task_state_keys[slot_id], + tracking_channels={ + JOINT_POSITION_CHANNEL: EndpointTrackingChannelBinding( + channel_id=JOINT_POSITION_CHANNEL, + source=TrackingFeedbackSourceRef( + provider_id="planning_context.robot", + revision="1", + address=EndpointTrackingFeedbackAddress( + target=target, + channel_id=JOINT_POSITION_CHANNEL, + ), + ), + projector=TrackingProjectorRef( + projector_id="joint_position_payload", + revision="1", + ), + ) + }, + capabilities=requirement.capabilities, + commands=commands, + claim_tokens=frozenset({f"robot.control_part:{control_part}"}), + joint_ids=joint_ids, + ) + ) + binding = ActionBinding( + owner_id=self.binding_owner_id, + endpoints=tuple(resolved), + ) + self.validate_binding(binding, contract) + return binding + + def validate_binding( + self, + binding: ActionBinding, + contract: SkillBindingContract, + ) -> None: + """Validate endpoint coverage, ownership, capabilities, and claims.""" + if not isinstance(binding, ActionBinding): + raise TypeError("binding must be an ActionBinding.") + if binding.owner_id != self.binding_owner_id: + raise ValueError("ActionBinding belongs to another engine instance.") + expected = { + (slot.slot_id, requirement.endpoint_id): requirement + for slot in contract.slots + for requirement in slot.endpoints + } + if set(binding.endpoint_keys) != set(expected): + missing = sorted(set(expected) - set(binding.endpoint_keys)) + extra = sorted(set(binding.endpoint_keys) - set(expected)) + raise ValueError( + "ActionBinding must cover the skill contract exactly: " + f"missing={missing}, extra={extra}." + ) + for key, requirement in expected.items(): + endpoint = binding.endpoint(*key) + missing_capabilities = requirement.capabilities - endpoint.capabilities + if missing_capabilities: raise ValueError( - f"Robot control part {name!r} bound to {resource_kind} role " - f"{role!r} contains no joints." + f"Endpoint {key[0]}.{key[1]} is missing capabilities " + f"{sorted(missing_capabilities)}." ) - profile = self._control_profiles.get(name) - resolved[role] = ResolvedControlPart( - name=name, - joint_ids=joint_ids, - commands={} if profile is None else profile.commands, + for name, command_type in requirement.required_commands.items(): + command = endpoint.commands.get(name) + if not isinstance(command, command_type): + raise ValueError( + f"Endpoint {key[0]}.{key[1]} requires command {name!r} " + f"of type {command_type.__name__}." + ) + for slot in contract.slots: + for constraint in slot.constraints: + if not isinstance(constraint, DisjointSlotEndpoints): + continue + selected = [ + binding.endpoint(slot.slot_id, endpoint_id) + for endpoint_id in constraint.endpoint_ids + ] + self._validate_disjoint(selected, label=f"slot {slot.slot_id!r}") + for constraint in contract.constraints: + if not isinstance(constraint, DisjointResourceSlots): + continue + for index, left_slot in enumerate(constraint.slots): + left = [ + endpoint + for endpoint in binding.endpoints + if endpoint.slot_id == left_slot + ] + for right_slot in constraint.slots[index + 1 :]: + right = [ + endpoint + for endpoint in binding.endpoints + if endpoint.slot_id == right_slot + ] + self._validate_disjoint( + left + right, + label=f"slots {left_slot!r} and {right_slot!r}", + only_across=len(left), + ) + + def apply_command_overrides( + self, + binding: ActionBinding, + overrides: ActionControlOverrides, + ) -> ActionBinding: + """Apply endpoint-scoped commands to an owned validated binding.""" + if not isinstance(overrides, ActionControlOverrides): + raise TypeError("overrides must be an ActionControlOverrides.") + if overrides.is_empty: + return ActionBinding(binding.owner_id, binding.endpoints) + return binding.with_command_overrides(overrides.as_flat_mapping()) + + @staticmethod + def _validate_disjoint( + endpoints: list[EndpointBinding], + *, + label: str, + only_across: int | None = None, + ) -> None: + """Reject overlapping destination, claim-token, or joint ownership.""" + pairs = ( + ( + (left, right) + for left in endpoints[:only_across] + for right in endpoints[only_across:] ) - return resolved + if only_across is not None + else ( + (left, right) + for index, left in enumerate(endpoints) + for right in endpoints[index + 1 :] + ) + ) + for left, right in pairs: + same_destination = left.destination_key == right.destination_key + overlapping_tokens = left.claim_tokens & right.claim_tokens + left_joints = set(left.joint_ids) + right_joints = set(right.joint_ids) + if same_destination or overlapping_tokens or left_joints & right_joints: + raise ValueError( + f"ActionBinding violates disjoint constraint for {label}: " + f"{left.key} conflicts with {right.key}." + ) def _snapshot_control_profiles( self, @@ -240,24 +436,5 @@ def _snapshot_control_profiles( snapshots[name] = profile.snapshot() return MappingProxyType(snapshots) - @staticmethod - def _apply_command_overrides( - resources: Mapping[str, ResolvedControlPart], - overrides: Mapping[str, Mapping[str, ControlCommand]], - *, - resource_kind: str, - ) -> dict[str, ResolvedControlPart]: - """Apply role-scoped commands to already resolved control parts.""" - unknown_roles = sorted(set(overrides) - set(resources)) - if unknown_roles: - raise KeyError( - f"Command overrides reference unbound {resource_kind} roles " - f"{unknown_roles}; bound roles are {sorted(resources)}." - ) - return { - role: resource.with_command_overrides(overrides.get(role, {})) - for role, resource in resources.items() - } - __all__ = ["ActionPlanningServices"] diff --git a/embodichain/lab/sim/atomic_actions/runtime_commands.py b/embodichain/lab/sim/atomic_actions/runtime_commands.py new file mode 100644 index 000000000..aeaa3ffd1 --- /dev/null +++ b/embodichain/lab/sim/atomic_actions/runtime_commands.py @@ -0,0 +1,481 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Transport-neutral runtime command values for atomic actions.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from dataclasses import dataclass +from typing import ClassVar + +import torch + +from .bindings import JointPositionTarget, RuntimeEndpointTarget + + +def _validate_identifier(value: str, *, field_name: str) -> str: + """Validate and return one strict identifier.""" + if not isinstance(value, str) or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +def _snapshot_target(target: RuntimeEndpointTarget) -> RuntimeEndpointTarget: + """Validate and own one runtime target snapshot.""" + if not isinstance(target, RuntimeEndpointTarget): + raise TypeError("target must be a RuntimeEndpointTarget.") + snapshot = target.snapshot() + if type(snapshot) is not type(target) or snapshot is target: + raise TypeError( + "RuntimeEndpointTarget.snapshot() must return an independently owned " + "value of the same target type." + ) + _validate_identifier( + snapshot.transport_id, + field_name="RuntimeEndpointTarget.transport_id", + ) + _validate_identifier( + snapshot.target_id, + field_name="RuntimeEndpointTarget.target_id", + ) + source_fingerprint = target.address_fingerprint + snapshot_fingerprint = snapshot.address_fingerprint + try: + hash(source_fingerprint) + hash(snapshot_fingerprint) + except TypeError as exc: + raise TypeError( + "RuntimeEndpointTarget.address_fingerprint must be hashable." + ) from exc + if snapshot_fingerprint != source_fingerprint: + raise ValueError( + "RuntimeEndpointTarget.snapshot() must preserve its address fingerprint." + ) + return snapshot + + +class RuntimeCommandPayload(ABC): + """Immutable-by-ownership payload submitted to one runtime transport.""" + + @property + @abstractmethod + def batch_size(self) -> int: + """Return the number of environment rows in this payload.""" + + @property + @abstractmethod + def device(self) -> torch.device: + """Return the device shared by this payload's batched values.""" + + @property + @abstractmethod + def transport_id(self) -> str: + """Return the transport kind that accepts this payload.""" + + @abstractmethod + def snapshot(self) -> RuntimeCommandPayload: + """Return an independently owned payload snapshot.""" + + +def _validate_payload_metadata(payload: RuntimeCommandPayload) -> None: + """Validate transport-neutral payload metadata.""" + if ( + not isinstance(payload.batch_size, int) + or isinstance(payload.batch_size, bool) + or payload.batch_size < 1 + ): + raise ValueError("RuntimeCommandPayload.batch_size must be a positive integer.") + if not isinstance(payload.device, torch.device): + raise TypeError("RuntimeCommandPayload.device must be a torch.device.") + _validate_identifier( + payload.transport_id, + field_name="RuntimeCommandPayload.transport_id", + ) + + +def _snapshot_payload(payload: RuntimeCommandPayload) -> RuntimeCommandPayload: + """Validate and own one runtime payload snapshot.""" + if not isinstance(payload, RuntimeCommandPayload): + raise TypeError("payload must be a RuntimeCommandPayload.") + snapshot = payload.snapshot() + if type(snapshot) is not type(payload) or snapshot is payload: + raise TypeError( + "RuntimeCommandPayload.snapshot() must return an independently owned " + "value of the same payload type." + ) + _validate_payload_metadata(snapshot) + return snapshot + + +@dataclass(frozen=True, slots=True, eq=False) +class JointPositionPayload(RuntimeCommandPayload): + """Batched joint-position targets for the built-in robot transport. + + Args: + positions: Joint positions with shape ``(batch_size, control_dof)``. + velocities: Optional joint velocities with the same shape and device. + """ + + TRANSPORT_ID: ClassVar[str] = JointPositionTarget.TRANSPORT_ID + + positions: torch.Tensor + velocities: torch.Tensor | None = None + + def __post_init__(self) -> None: + if not isinstance(self.positions, torch.Tensor): + raise TypeError("positions must be a torch.Tensor.") + if ( + self.positions.dim() != 2 + or self.positions.shape[0] < 1 + or self.positions.shape[1] < 1 + ): + raise ValueError( + "positions must have shape (batch_size, control_dof) with non-zero " + "dimensions." + ) + if not torch.isfinite(self.positions).all().item(): + raise ValueError("positions must contain only finite values.") + if self.velocities is not None: + if not isinstance(self.velocities, torch.Tensor): + raise TypeError("velocities must be a torch.Tensor or None.") + if self.velocities.shape != self.positions.shape: + raise ValueError("velocities must match positions shape.") + if self.velocities.device != self.positions.device: + raise ValueError("velocities must share the positions device.") + if not torch.isfinite(self.velocities).all().item(): + raise ValueError("velocities must contain only finite values.") + object.__setattr__(self, "positions", self.positions.clone()) + if self.velocities is not None: + object.__setattr__(self, "velocities", self.velocities.clone()) + + @property + def batch_size(self) -> int: + """Return the number of environment rows.""" + return int(self.positions.shape[0]) + + @property + def dof(self) -> int: + """Return the number of controlled joints.""" + return int(self.positions.shape[1]) + + @property + def device(self) -> torch.device: + """Return the tensor device.""" + return self.positions.device + + @property + def transport_id(self) -> str: + """Return the built-in joint-position transport identifier.""" + return self.TRANSPORT_ID + + def snapshot(self) -> JointPositionPayload: + """Return an independently owned joint payload.""" + return JointPositionPayload( + positions=self.positions, + velocities=self.velocities, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class EndpointCommand: + """One transport-compatible payload addressed to one runtime target. + + Args: + target: Immutable destination resolved from an action endpoint. + payload: Batched command value accepted by the target transport. + """ + + target: RuntimeEndpointTarget + payload: RuntimeCommandPayload + + def __post_init__(self) -> None: + target = _snapshot_target(self.target) + payload = _snapshot_payload(self.payload) + if target.transport_id != payload.transport_id: + raise ValueError( + f"Target transport {target.transport_id!r} does not accept payload " + f"transport {payload.transport_id!r}." + ) + object.__setattr__(self, "target", target) + object.__setattr__(self, "payload", payload) + + @property + def transport_id(self) -> str: + """Return the common target and payload transport identifier.""" + return self.target.transport_id + + @property + def destination_key(self) -> tuple[str, str]: + """Return the transport-scoped destination identifier.""" + return self.transport_id, self.target.target_id + + @property + def batch_size(self) -> int: + """Return the payload batch size.""" + return self.payload.batch_size + + @property + def device(self) -> torch.device: + """Return the payload device.""" + return self.payload.device + + def snapshot(self) -> EndpointCommand: + """Return an independently owned endpoint command.""" + return EndpointCommand(target=self.target, payload=self.payload) + + +@dataclass(frozen=True, slots=True, eq=False) +class RuntimeCommandFrame: + """Synchronized endpoint commands for one batched runtime instant. + + Args: + commands: Commands dispatched together for this frame. + active_mask: Boolean environment rows allowed to execute commands. + Transports must actively neutralize addressed targets for false + rows rather than leaving a previously persistent command running. + env_ids: Stable environment identifiers for the batch rows. + hold_duration: Per-row delay before advancing to the next frame. + """ + + commands: tuple[EndpointCommand, ...] + active_mask: torch.Tensor + env_ids: torch.Tensor + hold_duration: torch.Tensor + + def __post_init__(self) -> None: + if isinstance(self.commands, (str, bytes)): + raise TypeError("commands must be an iterable of EndpointCommand values.") + try: + commands = tuple(self.commands) + except TypeError as exc: + raise TypeError( + "commands must be an iterable of EndpointCommand values." + ) from exc + if not all(isinstance(command, EndpointCommand) for command in commands): + raise TypeError("commands values must be EndpointCommand instances.") + + if not isinstance(self.env_ids, torch.Tensor): + raise TypeError("env_ids must be a torch.Tensor.") + if ( + self.env_ids.dtype != torch.long + or self.env_ids.dim() != 1 + or self.env_ids.shape[0] < 1 + ): + raise ValueError("env_ids must be int64 with shape (batch_size,).") + batch_size = int(self.env_ids.shape[0]) + if torch.unique(self.env_ids).numel() != batch_size: + raise ValueError("env_ids must be unique.") + if not isinstance(self.active_mask, torch.Tensor): + raise TypeError("active_mask must be a torch.Tensor.") + if self.active_mask.dtype != torch.bool or self.active_mask.shape != ( + batch_size, + ): + raise ValueError(f"active_mask must be bool with shape ({batch_size},).") + if not isinstance(self.hold_duration, torch.Tensor): + raise TypeError("hold_duration must be a torch.Tensor.") + if self.hold_duration.shape != (batch_size,): + raise ValueError(f"hold_duration must have shape ({batch_size},).") + if ( + not torch.isfinite(self.hold_duration).all().item() + or (self.hold_duration < 0.0).any().item() + ): + raise ValueError("hold_duration must contain finite non-negative values.") + if self.active_mask.device != self.env_ids.device: + raise ValueError("active_mask and env_ids must share a device.") + if self.hold_duration.device != self.env_ids.device: + raise ValueError("hold_duration and env_ids must share a device.") + + snapshots = tuple(command.snapshot() for command in commands) + destinations: set[tuple[str, str]] = set() + joint_owners: dict[int, tuple[str, str]] = {} + for command in snapshots: + if command.batch_size != batch_size: + raise ValueError( + f"Payload for destination {command.destination_key} has batch " + f"size {command.batch_size}, expected {batch_size}." + ) + if command.device != self.env_ids.device: + raise ValueError( + f"Payload for destination {command.destination_key} must share " + "the frame device." + ) + if command.destination_key in destinations: + raise ValueError( + f"RuntimeCommandFrame contains duplicate destination " + f"{command.destination_key}." + ) + destinations.add(command.destination_key) + + if isinstance(command.target, JointPositionTarget): + if not isinstance(command.payload, JointPositionPayload): + raise TypeError( + "JointPositionTarget requires a JointPositionPayload." + ) + expected_dof = len(command.target.joint_ids) + if command.payload.dof != expected_dof: + raise ValueError( + f"Joint payload for destination {command.destination_key} has " + f"DOF {command.payload.dof}, expected {expected_dof}." + ) + overlaps = sorted( + joint_id + for joint_id in command.target.joint_ids + if joint_id in joint_owners + ) + if overlaps: + owners = sorted({joint_owners[joint_id] for joint_id in overlaps}) + raise ValueError( + f"Joint destination {command.destination_key} overlaps joint " + f"IDs {overlaps} already owned by {owners}." + ) + for joint_id in command.target.joint_ids: + joint_owners[joint_id] = command.destination_key + + object.__setattr__(self, "commands", snapshots) + object.__setattr__(self, "active_mask", self.active_mask.clone()) + object.__setattr__(self, "env_ids", self.env_ids.clone()) + object.__setattr__(self, "hold_duration", self.hold_duration.clone()) + + @property + def batch_size(self) -> int: + """Return the number of environment rows.""" + return int(self.env_ids.shape[0]) + + @property + def device(self) -> torch.device: + """Return the shared frame device.""" + return self.env_ids.device + + @property + def targets(self) -> tuple[RuntimeEndpointTarget, ...]: + """Return owned targets in command order.""" + return tuple(_snapshot_target(command.target) for command in self.commands) + + def with_active_mask(self, active_mask: torch.Tensor) -> RuntimeCommandFrame: + """Return a frame snapshot with a replacement active-row mask. + + Args: + active_mask: Boolean mask with one value per environment row. + + Returns: + Independently owned frame with unchanged commands and timing. + """ + return RuntimeCommandFrame( + commands=self.commands, + active_mask=active_mask, + env_ids=self.env_ids, + hold_duration=self.hold_duration, + ) + + def snapshot(self) -> RuntimeCommandFrame: + """Return an independently owned command frame.""" + return RuntimeCommandFrame( + commands=self.commands, + active_mask=self.active_mask, + env_ids=self.env_ids, + hold_duration=self.hold_duration, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class TimedCommandSequence: + """Ordered runtime command frames for one stable environment batch. + + ``env_ids`` is authoritative even when ``frames`` is empty, preserving the + batch size and device needed by compilation and execution boundaries. + + Args: + frames: Ordered command frames in execution order. + env_ids: Stable environment identifiers retained for empty sequences. + """ + + frames: tuple[RuntimeCommandFrame, ...] + env_ids: torch.Tensor + + def __post_init__(self) -> None: + if not isinstance(self.env_ids, torch.Tensor): + raise TypeError("env_ids must be a torch.Tensor.") + if ( + self.env_ids.dtype != torch.long + or self.env_ids.dim() != 1 + or self.env_ids.shape[0] < 1 + ): + raise ValueError("env_ids must be int64 with shape (batch_size,).") + if torch.unique(self.env_ids).numel() != self.env_ids.numel(): + raise ValueError("env_ids must be unique.") + if isinstance(self.frames, (str, bytes)): + raise TypeError("frames must be an iterable of RuntimeCommandFrame values.") + try: + frames = tuple(self.frames) + except TypeError as exc: + raise TypeError( + "frames must be an iterable of RuntimeCommandFrame values." + ) from exc + if not all(isinstance(frame, RuntimeCommandFrame) for frame in frames): + raise TypeError("frames values must be RuntimeCommandFrame instances.") + snapshots: list[RuntimeCommandFrame] = [] + for index, frame in enumerate(frames): + if frame.device != self.env_ids.device: + raise ValueError(f"Frame {index} must share the sequence device.") + if not torch.equal(frame.env_ids, self.env_ids): + raise ValueError(f"Frame {index} env_ids do not match the sequence.") + snapshots.append(frame.snapshot()) + object.__setattr__(self, "frames", tuple(snapshots)) + object.__setattr__(self, "env_ids", self.env_ids.clone()) + + @property + def batch_size(self) -> int: + """Return the preserved environment batch size.""" + return int(self.env_ids.shape[0]) + + @property + def device(self) -> torch.device: + """Return the preserved batch device.""" + return self.env_ids.device + + @property + def frame_count(self) -> int: + """Return the number of command frames.""" + return len(self.frames) + + @property + def targets(self) -> tuple[RuntimeEndpointTarget, ...]: + """Return unique owned destinations in first-use order.""" + targets: list[RuntimeEndpointTarget] = [] + seen: set[tuple[str, str]] = set() + for frame in self.frames: + for command in frame.commands: + if command.destination_key in seen: + continue + seen.add(command.destination_key) + targets.append(_snapshot_target(command.target)) + return tuple(targets) + + def snapshot(self) -> TimedCommandSequence: + """Return an independently owned timed sequence.""" + return TimedCommandSequence(frames=self.frames, env_ids=self.env_ids) + + +__all__ = [ + "EndpointCommand", + "JointPositionPayload", + "RuntimeCommandFrame", + "RuntimeCommandPayload", + "TimedCommandSequence", +] diff --git a/embodichain/lab/sim/atomic_actions/sim_adapter.py b/embodichain/lab/sim/atomic_actions/sim_adapter.py index 91f5e61fe..7500ed2ef 100644 --- a/embodichain/lab/sim/atomic_actions/sim_adapter.py +++ b/embodichain/lab/sim/atomic_actions/sim_adapter.py @@ -26,11 +26,12 @@ from embodichain.utils import configclass -from .execution import JointCommand +from .bindings import JointPositionTarget, RuntimeEndpointTarget from .runner import ( CommandAcknowledgement, CommandAckStatus, ) +from .runtime_commands import JointPositionPayload, RuntimeCommandFrame from .scene import SceneProvider from .state import ( EntityState, @@ -254,6 +255,9 @@ class SimulationExecutionAdapter: simulation: Simulation manager advanced by the execution clock. robot: Robot observed and commanded by the adapter. physics_dt: Optional physics period. Defaults to the simulation config. + control_dt: Optional command period exposed to action interpolation. + Defaults to ``physics_dt`` because that is the adapter's minimum + executable command cadence. env_ids: Optional stable correlation IDs matching every robot row. They are not used as simulator indices; row order maps to robot instances. scene_provider: Optional provider for versioned scene observations. @@ -262,12 +266,16 @@ class SimulationExecutionAdapter: initial_time: Initial elapsed simulation time in seconds. """ + transport_id = JointPositionTarget.TRANSPORT_ID + payload_type = JointPositionPayload + def __init__( self, simulation: SimulationManager, robot: Robot, *, physics_dt: float | None = None, + control_dt: float | None = None, env_ids: torch.Tensor | None = None, scene_provider: SceneProvider | None = None, scene_supplier: SceneSnapshotSupplier | None = None, @@ -282,6 +290,11 @@ def __init__( ) if not math.isfinite(resolved_physics_dt) or resolved_physics_dt <= 0.0: raise ValueError("physics_dt must be finite and greater than zero.") + resolved_control_dt = ( + resolved_physics_dt if control_dt is None else float(control_dt) + ) + if not math.isfinite(resolved_control_dt) or resolved_control_dt <= 0.0: + raise ValueError("control_dt must be finite and greater than zero.") qpos = robot.get_qpos() if not isinstance(qpos, torch.Tensor) or qpos.dim() != 2: raise ValueError("robot.get_qpos() must return shape (B, robot_dof).") @@ -301,6 +314,7 @@ def __init__( self.simulation = simulation self.robot = robot self.physics_dt = resolved_physics_dt + self.control_dt = resolved_control_dt self.env_ids = env_ids.clone() self._robot_env_indices = list(range(qpos.shape[0])) if scene_provider is not None and scene_supplier is not None: @@ -391,20 +405,20 @@ def observe(self, task_state: TaskState) -> PlanningContext: task=task_state, scene=scene, env_ids=self.env_ids, + control_dt=self.control_dt, ) def send( self, - command: JointCommand, + command: RuntimeCommandFrame, *, timeout: float, ) -> CommandAcknowledgement: - """Write active targets and observed-position holds as one batch. + """Write joint endpoint targets and neutralize inactive rows. Args: - command: Full-robot batched command. Inactive rows already contain - observed-position holds and are written with active rows so no - environment continues tracking a stale target. + command: Joint-position endpoint frame. Inactive rows are replaced + with observed positions by this transport. timeout: Positive acknowledgement deadline. Simulation writes are synchronous, so this is validated but otherwise unused. @@ -413,16 +427,43 @@ def send( """ self._validate_timeout(timeout) try: - self._validate_command(command) - self.robot.set_qpos( - command.positions, - env_ids=self._robot_env_indices, - ) - if command.velocities is not None: - self.robot.set_qvel( - command.velocities, + self._validate_command_frame(command) + observed_positions = self.robot.get_qpos() + for endpoint_command in command.commands: + target = endpoint_command.target + payload = endpoint_command.payload + assert isinstance(target, JointPositionTarget) + assert isinstance(payload, JointPositionPayload) + joint_ids = list(target.joint_ids) + positions = torch.where( + command.active_mask[:, None], + payload.positions, + observed_positions[:, joint_ids], + ) + self.robot.set_qpos( + positions, + joint_ids=joint_ids, env_ids=self._robot_env_indices, ) + velocities = payload.velocities + if velocities is None and not command.active_mask.all().item(): + observed_velocities = self._read_optional_tensor("get_qvel") + velocities = ( + torch.zeros_like(observed_positions[:, joint_ids]) + if observed_velocities is None + else observed_velocities[:, joint_ids] + ) + if velocities is not None: + velocities = torch.where( + command.active_mask[:, None], + velocities, + torch.zeros_like(velocities), + ) + self.robot.set_qvel( + velocities, + joint_ids=joint_ids, + env_ids=self._robot_env_indices, + ) return CommandAcknowledgement.accepted_ack() except Exception as exc: return CommandAcknowledgement( @@ -432,15 +473,16 @@ def send( def hold( self, - command: JointCommand, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, *, timeout: float, ) -> CommandAcknowledgement: - """Set every represented environment to an observed-position hold. + """Set every represented joint endpoint to an observed-position hold. Args: - command: Full-robot hold positions. ``active_mask`` is intentionally - ignored because safety hold applies to every environment row. + targets: Joint-position destinations to place in a safe hold. + context: Latest observed positions and stable environment IDs. timeout: Positive acknowledgement deadline. Returns: @@ -448,14 +490,25 @@ def hold( """ self._validate_timeout(timeout) try: - self._validate_command(command) - self.robot.set_qpos( - command.positions, - env_ids=self._robot_env_indices, - ) - if command.velocities is not None: + self._validate_targets(targets) + if not isinstance(context, PlanningContext): + raise TypeError("context must be a PlanningContext.") + if not torch.equal(context.env_ids, self.env_ids): + raise ValueError("Hold context env_ids must match the adapter.") + if context.robot.qpos.shape != self.robot.get_qpos().shape: + raise ValueError("Hold context qpos shape must match the robot.") + for target in targets: + assert isinstance(target, JointPositionTarget) + joint_ids = list(target.joint_ids) + observed_positions = context.robot.qpos[:, joint_ids] + self.robot.set_qpos( + observed_positions, + joint_ids=joint_ids, + env_ids=self._robot_env_indices, + ) self.robot.set_qvel( - command.velocities, + torch.zeros_like(observed_positions), + joint_ids=joint_ids, env_ids=self._robot_env_indices, ) return CommandAcknowledgement.accepted_ack() @@ -465,10 +518,16 @@ def hold( f"{type(exc).__name__}: {exc}", ) - def cancel(self, *, timeout: float) -> CommandAcknowledgement: + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: """Acknowledge cancellation of synchronous simulation target writes. Args: + targets: Joint-position destinations whose queued work is cancelled. timeout: Positive acknowledgement deadline. Returns: @@ -476,6 +535,13 @@ def cancel(self, *, timeout: float) -> CommandAcknowledgement: actual safe target. """ self._validate_timeout(timeout) + try: + self._validate_targets(targets) + except Exception as exc: + return CommandAcknowledgement( + CommandAckStatus.REJECTED, + f"{type(exc).__name__}: {exc}", + ) return CommandAcknowledgement.accepted_ack( "Simulation commands are synchronous; no queued command remained." ) @@ -505,18 +571,43 @@ def _read_optional_proprioception_tensor( return None return value if isinstance(value, torch.Tensor) else None - def _validate_command(self, command: JointCommand) -> None: - """Validate command identity and shape against the attached robot.""" - if not isinstance(command, JointCommand): - raise TypeError("command must be a JointCommand.") - qpos = self.robot.get_qpos() - if command.positions.shape != qpos.shape: - raise ValueError( - "Command shape must match full robot qpos, " - f"got {tuple(command.positions.shape)} and {tuple(qpos.shape)}." - ) + def _validate_command_frame(self, command: RuntimeCommandFrame) -> None: + """Validate one joint-position frame against the attached robot.""" + if not isinstance(command, RuntimeCommandFrame): + raise TypeError("command must be a RuntimeCommandFrame.") if not torch.equal(command.env_ids, self.env_ids): raise ValueError("Command env_ids must match the simulation adapter.") + self._validate_targets(command.targets) + for endpoint_command in command.commands: + if not isinstance(endpoint_command.payload, JointPositionPayload): + raise TypeError( + "SimulationExecutionAdapter accepts JointPositionPayload only." + ) + + def _validate_targets( + self, + targets: tuple[RuntimeEndpointTarget, ...], + ) -> None: + """Validate joint target ownership and robot dimensions.""" + if isinstance(targets, (str, bytes)): + raise TypeError("targets must be an iterable of runtime targets.") + qpos = self.robot.get_qpos() + seen_joints: set[int] = set() + for target in targets: + if not isinstance(target, JointPositionTarget): + raise TypeError( + "SimulationExecutionAdapter accepts JointPositionTarget only." + ) + if target.transport_id != self.transport_id: + raise ValueError("Target transport does not match this adapter.") + if max(target.joint_ids) >= qpos.shape[1]: + raise ValueError( + f"Target {target.target_id!r} references a joint outside robot DOF." + ) + overlaps = seen_joints.intersection(target.joint_ids) + if overlaps: + raise ValueError(f"Joint targets overlap on IDs {sorted(overlaps)}.") + seen_joints.update(target.joint_ids) @staticmethod def _validate_timeout(timeout: float) -> None: diff --git a/embodichain/lab/sim/atomic_actions/state.py b/embodichain/lab/sim/atomic_actions/state.py index 985599ac3..47ad90cec 100644 --- a/embodichain/lab/sim/atomic_actions/state.py +++ b/embodichain/lab/sim/atomic_actions/state.py @@ -18,9 +18,11 @@ from __future__ import annotations +import math +from collections.abc import Iterator, Mapping from dataclasses import dataclass, field from types import MappingProxyType -from typing import Mapping, TYPE_CHECKING +from typing import TYPE_CHECKING import torch @@ -28,6 +30,16 @@ from .core import ObjectSemantics +def _same_physical_object( + first: ObjectSemantics, + second: ObjectSemantics, +) -> bool: + """Return whether two semantic records identify one physical object.""" + from .core import _same_object_identity + + return _same_object_identity(first, second) + + def _resolve_runtime_device(device: torch.device | str) -> torch.device: """Resolve an indexless CUDA device to the active concrete GPU index.""" resolved = torch.device(device) @@ -44,7 +56,7 @@ def _validate_pose(value: torch.Tensor, name: str) -> int | None: return None if value.dim() != 3 or value.shape[-2:] != (4, 4) or value.shape[0] == 0: raise ValueError( - f"{name} must have shape (4, 4) or (n_envs, 4, 4), " + f"{name} must have shape (4, 4) or (num_envs, 4, 4), " f"got {tuple(value.shape)}." ) return int(value.shape[0]) @@ -91,6 +103,80 @@ def _broadcast_pose( return value.clone() +def _broadcast_joint_position( + value: torch.Tensor, + *, + batch_size: int, + device: torch.device, + name: str, +) -> torch.Tensor: + """Resolve an optionally batched joint-position value to a task batch.""" + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") + if value.dim() == 1: + if value.numel() == 0: + raise ValueError(f"{name} must contain at least one joint value.") + value = value.unsqueeze(0).expand(batch_size, -1) + elif value.dim() != 2 or value.shape[0] != batch_size or value.shape[1] == 0: + raise ValueError( + f"{name} must have shape (n_joints,) or " f"({batch_size}, n_joints)." + ) + if not value.is_floating_point(): + raise TypeError(f"{name} must use a floating-point dtype.") + if value.device != device: + raise ValueError(f"{name} must use task-state device {device}.") + if not torch.isfinite(value).all(): + raise ValueError(f"{name} must contain only finite values.") + return value.clone() + + +@dataclass(frozen=True, slots=True, eq=False) +class ArticulationJointState: + """Verified symbolic state for one named articulation joint. + + ``position`` may describe one scalar joint or a multi-DoF joint. The + surrounding :class:`TaskState` supplies the stable articulation/joint key; + this value only owns row-local verified measurements and activity. + """ + + position: torch.Tensor + """Joint positions with shape ``(J,)`` or ``(B, J)``.""" + + env_mask: torch.Tensor | None = None + """Rows for which the verified state is present.""" + + def __post_init__(self) -> None: + if not isinstance(self.position, torch.Tensor): + raise TypeError("ArticulationJointState.position must be a tensor.") + if self.position.dim() not in (1, 2) or self.position.numel() == 0: + raise ValueError( + "ArticulationJointState.position must have shape (J,) or (B, J)." + ) + if not self.position.is_floating_point(): + raise TypeError("ArticulationJointState.position must be floating point.") + if not torch.isfinite(self.position).all(): + raise ValueError("ArticulationJointState.position must be finite.") + object.__setattr__(self, "position", self.position.clone()) + if self.env_mask is not None: + batch_size = int(self.position.shape[0]) if self.position.dim() == 2 else -1 + if batch_size <= 0: + if self.env_mask.dim() != 1 or self.env_mask.numel() == 0: + raise ValueError( + "ArticulationJointState.env_mask must be a non-empty vector." + ) + batch_size = int(self.env_mask.shape[0]) + object.__setattr__( + self, + "env_mask", + _normalize_mask( + self.env_mask, + batch_size=batch_size, + device=self.position.device, + name="ArticulationJointState.env_mask", + ), + ) + + @dataclass(frozen=True, slots=True, eq=False) class HeldObjectState: """Observed or projected relation between an object and one manipulator.""" @@ -251,6 +337,29 @@ def _normalize_coordinated_held( ) +def _normalize_articulation_joint( + value: ArticulationJointState, + *, + batch_size: int, + device: torch.device, +) -> ArticulationJointState: + """Normalize one articulation-joint state to a task-state batch.""" + return ArticulationJointState( + position=_broadcast_joint_position( + value.position, + batch_size=batch_size, + device=device, + name="ArticulationJointState.position", + ), + env_mask=_normalize_mask( + value.env_mask, + batch_size=batch_size, + device=device, + name="ArticulationJointState.env_mask", + ), + ) + + @dataclass(frozen=True, slots=True, eq=False) class TaskState: """Symbolic task state, separate from measured robot state.""" @@ -262,12 +371,17 @@ class TaskState: """Device used by per-environment masks and relation tensors.""" held_objects: Mapping[str, HeldObjectState] = field(default_factory=dict) - """Single-manipulator held-object relations keyed by control resource.""" + """Held-object relations keyed by stable logical task-state resource.""" coordinated_held_objects: Mapping[tuple[str, str], CoordinatedHeldObjectState] = ( field(default_factory=dict) ) - """Two-manipulator held-object relations keyed by ordered resource pairs.""" + """Coordinated relations keyed by ordered logical task-state resource pairs.""" + + articulation_joints: Mapping[tuple[str, str], ArticulationJointState] = field( + default_factory=dict + ) + """Verified articulation states keyed by canonical articulation and joint IDs.""" def __post_init__(self) -> None: if self.batch_size <= 0: @@ -302,6 +416,30 @@ def __post_init__(self) -> None: value, batch_size=self.batch_size, device=device ) + normalized_articulation: dict[tuple[str, str], ArticulationJointState] = {} + for key, value in self.articulation_joints.items(): + if ( + not isinstance(key, tuple) + or len(key) != 2 + or not all( + type(item) is str and item and item == item.strip() for item in key + ) + ): + raise TypeError( + "articulation_joints keys must be pairs of non-empty " + "canonical identifiers." + ) + if not isinstance(value, ArticulationJointState): + raise TypeError( + "articulation_joints values must be ArticulationJointState " + "objects." + ) + normalized_articulation[key] = _normalize_articulation_joint( + value, + batch_size=self.batch_size, + device=device, + ) + object.__setattr__(self, "device", device) object.__setattr__(self, "held_objects", MappingProxyType(normalized_held)) object.__setattr__( @@ -309,6 +447,11 @@ def __post_init__(self) -> None: "coordinated_held_objects", MappingProxyType(normalized_coordinated), ) + object.__setattr__( + self, + "articulation_joints", + MappingProxyType(normalized_articulation), + ) @classmethod def empty( @@ -331,6 +474,53 @@ def get_held_object(self, resource: str) -> HeldObjectState | None: """Return the object held by ``resource``, if any.""" return self.held_objects.get(resource) + def held_object_mask(self, resource: str) -> torch.Tensor: + """Return environments where ``resource`` holds an object. + + Args: + resource: Manipulator control-resource name. + + Returns: + Owned boolean mask with shape ``(batch_size,)``. Missing resources + produce an all-false mask. + """ + held = self.get_held_object(resource) + if held is None: + return torch.zeros( + self.batch_size, + dtype=torch.bool, + device=self.device, + ) + assert held.env_mask is not None + return held.env_mask.clone() + + def exclusive_held_object_mask(self, resource: str) -> torch.Tensor: + """Return environments where only ``resource`` holds its object. + + Object identity is established by the exact semantic record or by a + shared non-null simulation entity. Labels and structural equality are + deliberately ignored because distinct physical objects may look alike. + + Args: + resource: Manipulator control-resource name. + + Returns: + Owned boolean mask with shape ``(batch_size,)``. + """ + held = self.get_held_object(resource) + if held is None: + return self.held_object_mask(resource) + + exclusive = self.held_object_mask(resource) + for other_resource, other in self.held_objects.items(): + if other_resource == resource: + continue + if not _same_physical_object(held.semantics, other.semantics): + continue + assert other.env_mask is not None + exclusive &= ~other.env_mask + return exclusive + def get_coordinated_held_object( self, first_resource: str, @@ -339,6 +529,14 @@ def get_coordinated_held_object( """Return the relation for an ordered resource pair, if any.""" return self.coordinated_held_objects.get((first_resource, second_resource)) + def get_articulation_joint_state( + self, + articulation_id: str, + joint_id: str, + ) -> ArticulationJointState | None: + """Return verified state for one canonical articulation joint.""" + return self.articulation_joints.get((articulation_id, joint_id)) + @dataclass(frozen=True, slots=True, eq=False) class RobotObservation: @@ -356,7 +554,7 @@ def __post_init__(self) -> None: raise ValueError("RobotObservation.timestamp must be non-negative.") if not isinstance(self.qpos, torch.Tensor) or self.qpos.dim() != 2: raise ValueError( - "RobotObservation.qpos must have shape (n_envs, robot_dof)." + "RobotObservation.qpos must have shape (num_envs, robot_dof)." ) if self.qpos.shape[0] == 0 or self.qpos.shape[1] == 0: raise ValueError("RobotObservation.qpos dimensions must be non-zero.") @@ -371,6 +569,38 @@ def __post_init__(self) -> None: raise ValueError("RobotObservation.qeffort must match qpos shape.") if self.qeffort.device != self.qpos.device: raise ValueError("RobotObservation.qeffort must share the qpos device.") + if self.root_pose is not None: + if not isinstance(self.root_pose, torch.Tensor): + raise TypeError("RobotObservation.root_pose must be a tensor or None.") + if self.root_pose.shape != (self.qpos.shape[0], 4, 4): + raise ValueError( + "RobotObservation.root_pose must have shape " + f"({self.qpos.shape[0]}, 4, 4)." + ) + if not self.root_pose.is_floating_point(): + raise TypeError("RobotObservation.root_pose must be floating point.") + if self.root_pose.device != self.qpos.device: + raise ValueError( + "RobotObservation.root_pose must share the qpos device." + ) + if not torch.isfinite(self.root_pose).all(): + raise ValueError("RobotObservation.root_pose must be finite.") + if self.root_twist is not None: + if not isinstance(self.root_twist, torch.Tensor): + raise TypeError("RobotObservation.root_twist must be a tensor or None.") + if self.root_twist.shape != (self.qpos.shape[0], 6): + raise ValueError( + "RobotObservation.root_twist must have shape " + f"({self.qpos.shape[0]}, 6)." + ) + if not self.root_twist.is_floating_point(): + raise TypeError("RobotObservation.root_twist must be floating point.") + if self.root_twist.device != self.qpos.device: + raise ValueError( + "RobotObservation.root_twist must share the qpos device." + ) + if not torch.isfinite(self.root_twist).all(): + raise ValueError("RobotObservation.root_twist must be finite.") object.__setattr__(self, "qpos", self.qpos.clone()) object.__setattr__(self, "qvel", self.qvel.clone()) if self.qeffort is not None: @@ -423,6 +653,122 @@ def __post_init__(self) -> None: object.__setattr__(self, "pose", self.pose.clone()) +@dataclass(frozen=True, slots=True, eq=False) +class ObservedArticulationJointState: + """Live measured state for one scene articulation joint. + + This value belongs to :class:`SceneSnapshot`, not :class:`TaskState`. + ``ArticulationJointState`` records a verified symbolic effect after an + operation, while this class records the physical position used by online + grounding and recovery replans. + """ + + position: torch.Tensor + """Measured joint position with shape ``(J,)`` or ``(B, J)``.""" + + valid_mask: torch.Tensor | None = None + """Optional row-validity mask for a batched observation.""" + + def __post_init__(self) -> None: + position = self.position + if not isinstance(position, torch.Tensor): + raise TypeError("ObservedArticulationJointState.position must be a tensor.") + if position.dim() not in (1, 2) or position.numel() == 0: + raise ValueError( + "ObservedArticulationJointState.position must have shape (J,) " + "or (B, J)." + ) + if not position.is_floating_point(): + raise TypeError( + "ObservedArticulationJointState.position must be floating point." + ) + if not torch.isfinite(position).all(): + raise ValueError( + "ObservedArticulationJointState.position must contain only " + "finite values." + ) + object.__setattr__(self, "position", position.clone()) + if self.valid_mask is None: + return + valid_mask = self.valid_mask + if not isinstance(valid_mask, torch.Tensor): + raise TypeError( + "ObservedArticulationJointState.valid_mask must be a tensor or None." + ) + if position.dim() != 2: + raise ValueError( + "ObservedArticulationJointState.valid_mask requires a batched " + "position." + ) + if valid_mask.dtype != torch.bool or valid_mask.shape != (position.shape[0],): + raise ValueError( + "ObservedArticulationJointState.valid_mask must have shape (B,) " + "and dtype torch.bool." + ) + if valid_mask.device != position.device: + raise ValueError( + "ObservedArticulationJointState position and valid_mask must " + "share a device." + ) + object.__setattr__(self, "valid_mask", valid_mask.clone()) + + def snapshot(self) -> ObservedArticulationJointState: + """Return an independently owned observation value.""" + return ObservedArticulationJointState(self.position, self.valid_mask) + + +class _ImmutableEntityMapping(Mapping[str, EntityState]): + """Own entity states and return defensive copies on every public read.""" + + __slots__ = ("_states",) + + def __init__(self, states: Mapping[str, EntityState]) -> None: + self._states = MappingProxyType( + { + entity_id: EntityState(state.pose, confidence=state.confidence) + for entity_id, state in states.items() + } + ) + + def __getitem__(self, entity_id: str) -> EntityState: + state = self._states[entity_id] + return EntityState(state.pose, confidence=state.confidence) + + def __iter__(self) -> Iterator[str]: + return iter(self._states) + + def __len__(self) -> int: + return len(self._states) + + +class _ImmutableObservedArticulationJointMapping( + Mapping[tuple[str, str], ObservedArticulationJointState] +): + """Own live joint observations and copy values on every public read.""" + + __slots__ = ("_states",) + + def __init__( + self, + states: Mapping[tuple[str, str], ObservedArticulationJointState], + ) -> None: + self._states = MappingProxyType( + {key: state.snapshot() for key, state in states.items()} + ) + + def __getitem__( + self, + key: tuple[str, str], + ) -> ObservedArticulationJointState: + return self._states[key].snapshot() + + def __iter__(self) -> Iterator[tuple[str, str]]: + return iter(self._states) + + def __len__(self) -> int: + return len(self._states) + + @dataclass(frozen=True, slots=True, eq=False) class SceneSnapshot: """Versioned scene state used to ground dynamic goals and obstacles.""" @@ -436,6 +782,11 @@ class SceneSnapshot: collision_entity_ids: tuple[str, ...] = () """Entity IDs whose poses update a planner's dynamic collision world.""" + articulation_joints: Mapping[tuple[str, str], ObservedArticulationJointState] = ( + field(default_factory=dict) + ) + """Live physical joint observations keyed by articulation and joint ID.""" + def __post_init__(self) -> None: if self.timestamp < 0.0: raise ValueError("SceneSnapshot.timestamp must be non-negative.") @@ -473,6 +824,28 @@ def __post_init__(self) -> None: "SceneSnapshot entities must contain EntityState values." ) normalized[entity_id] = state + normalized_joints: dict[tuple[str, str], ObservedArticulationJointState] = {} + for key, state in self.articulation_joints.items(): + if ( + not isinstance(key, tuple) + or len(key) != 2 + or not all( + type(identifier) is str + and identifier + and identifier == identifier.strip() + for identifier in key + ) + ): + raise TypeError( + "SceneSnapshot articulation_joints keys must be canonical " + "(articulation_id, joint_id) pairs." + ) + if not isinstance(state, ObservedArticulationJointState): + raise TypeError( + "SceneSnapshot articulation_joints values must be " + "ObservedArticulationJointState objects." + ) + normalized_joints[key] = state collision_entity_ids = tuple(self.collision_entity_ids) if len(set(collision_entity_ids)) != len(collision_entity_ids) or not all( isinstance(entity_id, str) and entity_id @@ -487,9 +860,30 @@ def __post_init__(self) -> None: "collision_entity_ids reference missing scene entities: " f"{sorted(missing)}." ) - object.__setattr__(self, "entities", MappingProxyType(normalized)) + object.__setattr__(self, "entities", _ImmutableEntityMapping(normalized)) + object.__setattr__( + self, + "articulation_joints", + _ImmutableObservedArticulationJointMapping(normalized_joints), + ) object.__setattr__(self, "collision_entity_ids", collision_entity_ids) + def get_articulation_joint_state( + self, + articulation_id: str, + joint_id: str, + ) -> ObservedArticulationJointState | None: + """Return an owned live joint observation for a canonical address.""" + for value, field_name in ( + (articulation_id, "articulation_id"), + (joint_id, "joint_id"), + ): + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty canonical identifier." + ) + return self.articulation_joints.get((articulation_id, joint_id)) + def collision_world_revisions(self, batch_size: int) -> tuple[int, ...]: """Expand the collision revision to one value per environment. @@ -560,6 +954,8 @@ class PlanningContext: task: TaskState scene: SceneSnapshot env_ids: torch.Tensor + control_dt: float | None = None + """Explicit command period used by action-owned interpolation.""" def __post_init__(self) -> None: if not isinstance(self.robot, RobotObservation): @@ -579,6 +975,18 @@ def __post_init__(self) -> None: f"Scene entity {entity_id!r} pose batch must match the " "planning context." ) + for ( + articulation_id, + joint_id, + ), state in self.scene.articulation_joints.items(): + if ( + state.position.dim() == 2 + and state.position.shape[0] != self.robot.batch_size + ): + raise ValueError( + f"Scene articulation joint ({articulation_id!r}, {joint_id!r}) " + "position batch must match the planning context." + ) if not isinstance(self.env_ids, torch.Tensor): raise TypeError("env_ids must be a torch.Tensor.") if self.env_ids.dtype != torch.long: @@ -592,6 +1000,14 @@ def __post_init__(self) -> None: raise ValueError("env_ids and robot tensors must share a device.") if torch.unique(self.env_ids).numel() != self.env_ids.numel(): raise ValueError("env_ids must be unique.") + if self.control_dt is not None: + if isinstance(self.control_dt, bool) or not isinstance( + self.control_dt, (int, float) + ): + raise TypeError("control_dt must be a real number or None.") + if not math.isfinite(self.control_dt) or self.control_dt <= 0.0: + raise ValueError("control_dt must be finite and greater than zero.") + object.__setattr__(self, "control_dt", float(self.control_dt)) object.__setattr__(self, "env_ids", self.env_ids.clone()) @property @@ -609,6 +1025,10 @@ def held_objects(self) -> Mapping[str, HeldObjectState]: """Single-resource held-object relations.""" return self.task.held_objects + def get_held_object(self, resource: str) -> HeldObjectState | None: + """Return the object held by ``resource``, if any.""" + return self.task.get_held_object(resource) + @property def coordinated_held_objects( self, @@ -616,10 +1036,6 @@ def coordinated_held_objects( """Coordinated held-object relations.""" return self.task.coordinated_held_objects - def get_held_object(self, resource: str) -> HeldObjectState | None: - """Return the object held by ``resource``, if any.""" - return self.task.get_held_object(resource) - def get_coordinated_held_object( self, first_resource: str, @@ -628,6 +1044,34 @@ def get_coordinated_held_object( """Return a coordinated held-object relation, if any.""" return self.task.get_coordinated_held_object(first_resource, second_resource) + def require_control_dt(self) -> float: + """Return the explicit command period required for interpolation. + + Raises: + ValueError: If the caller did not provide ``control_dt``. + """ + if self.control_dt is None: + raise ValueError( + "This action performs interpolation and requires an explicit " + "PlanningContext.control_dt." + ) + return self.control_dt + + @property + def articulation_joints( + self, + ) -> Mapping[tuple[str, str], ArticulationJointState]: + """Verified articulation-joint states.""" + return self.task.articulation_joints + + def get_articulation_joint_state( + self, + articulation_id: str, + joint_id: str, + ) -> ArticulationJointState | None: + """Return verified state for one canonical articulation joint.""" + return self.task.get_articulation_joint_state(articulation_id, joint_id) + def project( self, *, @@ -648,13 +1092,16 @@ def project( task=task, scene=self.scene, env_ids=self.env_ids, + control_dt=self.control_dt, ) __all__ = [ + "ArticulationJointState", "CoordinatedHeldObjectState", "EntityState", "HeldObjectState", + "ObservedArticulationJointState", "PlanningContext", "RobotObservation", "SceneSnapshot", diff --git a/embodichain/lab/sim/atomic_actions/tracking.py b/embodichain/lab/sim/atomic_actions/tracking.py new file mode 100644 index 000000000..2a9e12acc --- /dev/null +++ b/embodichain/lab/sim/atomic_actions/tracking.py @@ -0,0 +1,1210 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Typed, transport-neutral tracking contracts for atomic-action execution.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from copy import deepcopy +from dataclasses import dataclass, field +from types import MappingProxyType +from typing import TYPE_CHECKING, ClassVar, Hashable, Iterable, Mapping, Protocol + +import torch + +if TYPE_CHECKING: + from .bindings import RuntimeEndpointTarget + from .runtime_commands import EndpointCommand + from .state import PlanningContext + + +TrackingChannelId = str +"""Open string identifier for one typed endpoint-feedback channel.""" + +JOINT_POSITION_CHANNEL: TrackingChannelId = "joint.position" +BASE_POSE_CHANNEL: TrackingChannelId = "base.pose" +WHOLE_BODY_POSE_CHANNEL: TrackingChannelId = "whole_body.pose" + + +def _identifier(value: str, *, field_name: str) -> str: + if not isinstance(value, str) or not value or value != value.strip(): + raise ValueError(f"{field_name} must be a non-empty trimmed string.") + return value + + +def _positive_float(value: float, *, field_name: str) -> float: + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise TypeError(f"{field_name} must be a number.") + normalized = float(value) + if not torch.isfinite(torch.tensor(normalized)).item() or normalized <= 0.0: + raise ValueError(f"{field_name} must be finite and positive.") + return normalized + + +def _non_negative_float(value: float, *, field_name: str) -> float: + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise TypeError(f"{field_name} must be a number.") + normalized = float(value) + if not torch.isfinite(torch.tensor(normalized)).item() or normalized < 0.0: + raise ValueError(f"{field_name} must be finite and non-negative.") + return normalized + + +def _tensor(value: torch.Tensor, *, field_name: str, dimensions: int) -> torch.Tensor: + if not isinstance(value, torch.Tensor): + raise TypeError(f"{field_name} must be a torch.Tensor.") + if value.dim() != dimensions or any(size < 1 for size in value.shape): + raise ValueError(f"{field_name} must be a non-empty {dimensions}-D tensor.") + if not torch.is_floating_point(value) or not torch.isfinite(value).all().item(): + raise ValueError(f"{field_name} must contain finite floating-point values.") + return value.clone() + + +class TrackingFeedbackAddress(ABC): + """Immutable address understood by one tracking-feedback provider.""" + + @property + @abstractmethod + def address_fingerprint(self) -> Hashable: + """Return a stable, hashable address identity.""" + + def snapshot(self) -> TrackingFeedbackAddress: + """Return an independently owned address snapshot.""" + return deepcopy(self) + + +@dataclass(frozen=True, slots=True) +class EndpointTrackingFeedbackAddress(TrackingFeedbackAddress): + """Feedback address for one runtime endpoint and open tracking channel.""" + + target: RuntimeEndpointTarget + channel_id: TrackingChannelId + + def __post_init__(self) -> None: + from .bindings import RuntimeEndpointTarget + + if not isinstance(self.target, RuntimeEndpointTarget): + raise TypeError("target must be a RuntimeEndpointTarget.") + snapshot = self.target.snapshot() + if type(snapshot) is not type(self.target) or snapshot is self.target: + raise TypeError("RuntimeEndpointTarget.snapshot() must own a new value.") + if snapshot.address_fingerprint != self.target.address_fingerprint: + raise ValueError("Target snapshot must preserve its address fingerprint.") + _identifier(self.channel_id, field_name="channel_id") + object.__setattr__(self, "target", snapshot) + + @property + def address_fingerprint(self) -> Hashable: + """Return the endpoint- and channel-scoped address identity.""" + return self.target.address_fingerprint, self.channel_id + + +@dataclass(frozen=True, slots=True) +class TrackingFeedbackSourceRef: + """Versioned provider route plus one immutable feedback address.""" + + provider_id: str + revision: str + address: TrackingFeedbackAddress + + def __post_init__(self) -> None: + _identifier(self.provider_id, field_name="provider_id") + _identifier(self.revision, field_name="revision") + if not isinstance(self.address, TrackingFeedbackAddress): + raise TypeError("address must be a TrackingFeedbackAddress.") + snapshot = self.address.snapshot() + if type(snapshot) is not type(self.address) or snapshot is self.address: + raise TypeError("TrackingFeedbackAddress.snapshot() must own a new value.") + if snapshot.address_fingerprint != self.address.address_fingerprint: + raise ValueError("Address snapshot must preserve its fingerprint.") + hash(snapshot.address_fingerprint) + object.__setattr__(self, "address", snapshot) + + @property + def source_fingerprint(self) -> Hashable: + """Return the exact versioned source identity.""" + return self.provider_id, self.revision, self.address.address_fingerprint + + def snapshot(self) -> TrackingFeedbackSourceRef: + """Return an independently owned source reference.""" + return TrackingFeedbackSourceRef(self.provider_id, self.revision, self.address) + + +@dataclass(frozen=True, slots=True) +class TrackingProjectorRef: + """Exact version of a command-to-tracking-state projector.""" + + projector_id: str + revision: str + + def __post_init__(self) -> None: + _identifier(self.projector_id, field_name="projector_id") + _identifier(self.revision, field_name="revision") + + def snapshot(self) -> TrackingProjectorRef: + """Return an independently owned projector route.""" + return TrackingProjectorRef(self.projector_id, self.revision) + + +@dataclass(frozen=True, slots=True) +class EndpointTrackingChannelBinding: + """Resolved source and projector for one endpoint tracking channel.""" + + channel_id: TrackingChannelId + source: TrackingFeedbackSourceRef + projector: TrackingProjectorRef + + def __post_init__(self) -> None: + _identifier(self.channel_id, field_name="channel_id") + if not isinstance(self.source, TrackingFeedbackSourceRef): + raise TypeError("source must be a TrackingFeedbackSourceRef.") + if not isinstance(self.projector, TrackingProjectorRef): + raise TypeError("projector must be a TrackingProjectorRef.") + address = self.source.address + if isinstance(address, EndpointTrackingFeedbackAddress): + if address.channel_id != self.channel_id: + raise ValueError("Binding and feedback-address channels must match.") + object.__setattr__(self, "source", self.source.snapshot()) + object.__setattr__(self, "projector", self.projector.snapshot()) + + def snapshot(self) -> EndpointTrackingChannelBinding: + """Return an independently owned channel binding.""" + return EndpointTrackingChannelBinding( + self.channel_id, self.source, self.projector + ) + + @property + def route_fingerprint(self) -> tuple[str, Hashable, str, str]: + """Return the exact channel, source, and projector route identity.""" + return ( + self.channel_id, + self.source.source_fingerprint, + self.projector.projector_id, + self.projector.revision, + ) + + +class TrackingState(ABC): + """Immutable-by-ownership typed desired or observed tracking state.""" + + channel_id: ClassVar[TrackingChannelId] + + @property + @abstractmethod + def batch_size(self) -> int: + """Return the represented environment count.""" + + @property + @abstractmethod + def device(self) -> torch.device: + """Return the tensor device.""" + + @abstractmethod + def snapshot(self) -> TrackingState: + """Return an independently owned state snapshot.""" + + +@dataclass(frozen=True, slots=True, eq=False) +class JointPositionTrackingState(TrackingState): + """Batched joint positions with shape ``(B, D)``.""" + + channel_id: ClassVar[str] = JOINT_POSITION_CHANNEL + positions: torch.Tensor + + def __post_init__(self) -> None: + object.__setattr__( + self, + "positions", + _tensor(self.positions, field_name="positions", dimensions=2), + ) + + @property + def batch_size(self) -> int: + return int(self.positions.shape[0]) + + @property + def device(self) -> torch.device: + return self.positions.device + + def snapshot(self) -> JointPositionTrackingState: + return JointPositionTrackingState(self.positions) + + +@dataclass(frozen=True, slots=True, eq=False) +class PoseTrackingState(TrackingState): + """Batched homogeneous poses with shape ``(B, 4, 4)``.""" + + channel_id: ClassVar[str] = BASE_POSE_CHANNEL + poses: torch.Tensor + + def __post_init__(self) -> None: + poses = _tensor(self.poses, field_name="poses", dimensions=3) + if poses.shape[1:] != (4, 4): + raise ValueError("poses must have shape (batch_size, 4, 4).") + object.__setattr__(self, "poses", poses) + + @property + def batch_size(self) -> int: + return int(self.poses.shape[0]) + + @property + def device(self) -> torch.device: + return self.poses.device + + def snapshot(self) -> PoseTrackingState: + return PoseTrackingState(self.poses) + + +@dataclass(frozen=True, slots=True, eq=False) +class WholeBodyPoseTrackingState(TrackingState): + """Batched base poses and joint positions for whole-body tracking.""" + + channel_id: ClassVar[str] = WHOLE_BODY_POSE_CHANNEL + root_poses: torch.Tensor + joint_positions: torch.Tensor + + def __post_init__(self) -> None: + root_poses = _tensor(self.root_poses, field_name="root_poses", dimensions=3) + joints = _tensor( + self.joint_positions, + field_name="joint_positions", + dimensions=2, + ) + if root_poses.shape[1:] != (4, 4): + raise ValueError("root_poses must have shape (batch_size, 4, 4).") + if root_poses.shape[0] != joints.shape[0]: + raise ValueError("root_poses and joint_positions batches must match.") + if root_poses.device != joints.device: + raise ValueError("root_poses and joint_positions must share a device.") + object.__setattr__(self, "root_poses", root_poses) + object.__setattr__(self, "joint_positions", joints) + + @property + def batch_size(self) -> int: + return int(self.root_poses.shape[0]) + + @property + def device(self) -> torch.device: + return self.root_poses.device + + def snapshot(self) -> WholeBodyPoseTrackingState: + return WholeBodyPoseTrackingState(self.root_poses, self.joint_positions) + + +class TrackingMetricCfg(ABC): + """Immutable tolerance configuration dispatched by exact metric ID/revision.""" + + metric_id: ClassVar[str] + revision: ClassVar[str] = "1" + channel_id: ClassVar[TrackingChannelId] + + def snapshot(self) -> TrackingMetricCfg: + """Return an independently owned metric configuration.""" + return deepcopy(self) + + +@dataclass(frozen=True, slots=True) +class JointPositionTrackingMetric(TrackingMetricCfg): + """Maximum absolute joint-error tolerance.""" + + metric_id: ClassVar[str] = "joint.max_abs" + channel_id: ClassVar[str] = JOINT_POSITION_CHANNEL + tolerance: float = 0.05 + + def __post_init__(self) -> None: + object.__setattr__( + self, "tolerance", _positive_float(self.tolerance, field_name="tolerance") + ) + + +@dataclass(frozen=True, slots=True) +class PoseTrackingMetric(TrackingMetricCfg): + """Independent translation and rotation tolerances for base pose.""" + + metric_id: ClassVar[str] = "pose.se3" + channel_id: ClassVar[str] = BASE_POSE_CHANNEL + translation_tolerance: float = 0.02 + rotation_tolerance: float = 0.05 + + def __post_init__(self) -> None: + object.__setattr__( + self, + "translation_tolerance", + _positive_float( + self.translation_tolerance, field_name="translation_tolerance" + ), + ) + object.__setattr__( + self, + "rotation_tolerance", + _positive_float(self.rotation_tolerance, field_name="rotation_tolerance"), + ) + + +@dataclass(frozen=True, slots=True) +class WholeBodyPoseTrackingMetric(TrackingMetricCfg): + """Independent base-pose and joint-position tolerances.""" + + metric_id: ClassVar[str] = "whole_body.pose" + channel_id: ClassVar[str] = WHOLE_BODY_POSE_CHANNEL + translation_tolerance: float = 0.02 + rotation_tolerance: float = 0.05 + joint_position_tolerance: float = 0.05 + + def __post_init__(self) -> None: + for field_name in ( + "translation_tolerance", + "rotation_tolerance", + "joint_position_tolerance", + ): + object.__setattr__( + self, + field_name, + _positive_float(getattr(self, field_name), field_name=field_name), + ) + + +def _own_metrics( + metrics: Iterable[TrackingMetricCfg], *, field_name: str +) -> tuple[TrackingMetricCfg, ...]: + snapshots: list[TrackingMetricCfg] = [] + channels: set[str] = set() + for metric in metrics: + if not isinstance(metric, TrackingMetricCfg): + raise TypeError(f"{field_name} must contain TrackingMetricCfg values.") + _identifier(metric.metric_id, field_name=f"{field_name}.metric_id") + _identifier(metric.revision, field_name=f"{field_name}.revision") + _identifier(metric.channel_id, field_name=f"{field_name}.channel_id") + if metric.channel_id in channels: + raise ValueError( + f"{field_name} contains duplicate channel {metric.channel_id!r}." + ) + snapshot = metric.snapshot() + if type(snapshot) is not type(metric) or snapshot is metric: + raise TypeError("TrackingMetricCfg.snapshot() must own a same-type value.") + channels.add(metric.channel_id) + snapshots.append(snapshot) + if not snapshots: + raise ValueError(f"{field_name} must contain at least one metric.") + return tuple(snapshots) + + +@dataclass(frozen=True, slots=True) +class InFlightTrackingPolicy: + """Feedback checks used while a command sequence is still in flight.""" + + metrics: tuple[TrackingMetricCfg, ...] + consecutive_violations: int = 1 + grace_period: float = 0.0 + + def __post_init__(self) -> None: + object.__setattr__( + self, "metrics", _own_metrics(self.metrics, field_name="metrics") + ) + if ( + not isinstance(self.consecutive_violations, int) + or isinstance(self.consecutive_violations, bool) + or self.consecutive_violations < 1 + ): + raise ValueError("consecutive_violations must be a positive integer.") + object.__setattr__( + self, + "grace_period", + _non_negative_float(self.grace_period, field_name="grace_period"), + ) + + def snapshot(self) -> InFlightTrackingPolicy: + return InFlightTrackingPolicy( + self.metrics, self.consecutive_violations, self.grace_period + ) + + +@dataclass(frozen=True, slots=True) +class FeedbackTerminalAcceptance: + """Terminal acceptance proven by typed endpoint feedback.""" + + metrics: tuple[TrackingMetricCfg, ...] + settle_timeout: float = 0.0 + consecutive_acceptances: int = 1 + + def __post_init__(self) -> None: + object.__setattr__( + self, "metrics", _own_metrics(self.metrics, field_name="metrics") + ) + object.__setattr__( + self, + "settle_timeout", + _non_negative_float(self.settle_timeout, field_name="settle_timeout"), + ) + if ( + not isinstance(self.consecutive_acceptances, int) + or isinstance(self.consecutive_acceptances, bool) + or self.consecutive_acceptances < 1 + ): + raise ValueError("consecutive_acceptances must be a positive integer.") + + def snapshot(self) -> FeedbackTerminalAcceptance: + return FeedbackTerminalAcceptance( + self.metrics, self.settle_timeout, self.consecutive_acceptances + ) + + +@dataclass(frozen=True, slots=True) +class TimedTerminalAcceptance: + """Explicit terminal acceptance without endpoint feedback.""" + + settle_duration: float = 0.0 + + def __post_init__(self) -> None: + object.__setattr__( + self, + "settle_duration", + _non_negative_float(self.settle_duration, field_name="settle_duration"), + ) + + def snapshot(self) -> TimedTerminalAcceptance: + return TimedTerminalAcceptance(self.settle_duration) + + +TerminalAcceptance = FeedbackTerminalAcceptance | TimedTerminalAcceptance + + +@dataclass(frozen=True, slots=True) +class TrackingPolicy: + """Independent in-flight recovery signal and terminal acceptance contract.""" + + in_flight: InFlightTrackingPolicy | None + terminal: TerminalAcceptance + + def __post_init__(self) -> None: + if self.in_flight is not None and not isinstance( + self.in_flight, InFlightTrackingPolicy + ): + raise TypeError("in_flight must be InFlightTrackingPolicy or None.") + if not isinstance( + self.terminal, (FeedbackTerminalAcceptance, TimedTerminalAcceptance) + ): + raise TypeError("terminal must be a terminal-acceptance contract.") + if self.in_flight is not None: + object.__setattr__(self, "in_flight", self.in_flight.snapshot()) + object.__setattr__(self, "terminal", self.terminal.snapshot()) + in_flight = self.in_flight + terminal = self.terminal + if in_flight is not None and isinstance(terminal, FeedbackTerminalAcceptance): + in_flight_by_channel = { + metric.channel_id: metric for metric in in_flight.metrics + } + for terminal_metric in terminal.metrics: + in_flight_metric = in_flight_by_channel.get(terminal_metric.channel_id) + if in_flight_metric is None: + continue + if ( + in_flight_metric.metric_id != terminal_metric.metric_id + or in_flight_metric.revision != terminal_metric.revision + or type(in_flight_metric) is not type(terminal_metric) + ): + raise ValueError( + "In-flight and terminal metrics sharing a channel must " + "use the same exact metric ID, revision, and type." + ) + + def snapshot(self) -> TrackingPolicy: + return TrackingPolicy(self.in_flight, self.terminal) + + @classmethod + def timed(cls, *, settle_duration: float = 0.0) -> TrackingPolicy: + """Create an explicit time-only terminal contract with no tracking.""" + return cls( + in_flight=None, + terminal=TimedTerminalAcceptance(settle_duration=settle_duration), + ) + + @classmethod + def joint_position( + cls, + *, + in_flight_max_abs_error: float = 0.05, + terminal_max_abs_error: float = 0.05, + terminal_settle_timeout: float = 0.5, + consecutive_violations: int = 1, + consecutive_acceptances: int = 1, + grace_period: float = 0.0, + ) -> TrackingPolicy: + """Create the built-in joint-position tracking and acceptance contract.""" + return cls( + in_flight=InFlightTrackingPolicy( + metrics=(JointPositionTrackingMetric(in_flight_max_abs_error),), + consecutive_violations=consecutive_violations, + grace_period=grace_period, + ), + terminal=FeedbackTerminalAcceptance( + metrics=(JointPositionTrackingMetric(terminal_max_abs_error),), + settle_timeout=terminal_settle_timeout, + consecutive_acceptances=consecutive_acceptances, + ), + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class TrackingSetpoint: + """One endpoint-local desired state and its typed feedback route.""" + + endpoint_key: tuple[str, str] + binding: EndpointTrackingChannelBinding + desired: TrackingState + + def __post_init__(self) -> None: + if not isinstance(self.endpoint_key, tuple) or len(self.endpoint_key) != 2: + raise TypeError("endpoint_key must be a (slot_id, endpoint_id) tuple.") + _identifier(self.endpoint_key[0], field_name="endpoint_key.slot_id") + _identifier(self.endpoint_key[1], field_name="endpoint_key.endpoint_id") + if not isinstance(self.binding, EndpointTrackingChannelBinding): + raise TypeError("binding must be an EndpointTrackingChannelBinding.") + if not isinstance(self.desired, TrackingState): + raise TypeError("desired must be a TrackingState.") + if self.binding.channel_id != self.desired.channel_id: + raise ValueError("Binding and desired-state channels must match.") + desired = self.desired.snapshot() + if type(desired) is not type(self.desired) or desired is self.desired: + raise TypeError("TrackingState.snapshot() must own a same-type value.") + object.__setattr__(self, "binding", self.binding.snapshot()) + object.__setattr__(self, "desired", desired) + + @property + def key(self) -> tuple[str, str, str]: + return self.endpoint_key[0], self.endpoint_key[1], self.binding.channel_id + + def snapshot(self) -> TrackingSetpoint: + return TrackingSetpoint(self.endpoint_key, self.binding, self.desired) + + +@dataclass(frozen=True, slots=True) +class TrackingFrame: + """Desired endpoint states associated with one command frame.""" + + setpoints: tuple[TrackingSetpoint, ...] = () + + def __post_init__(self) -> None: + snapshots: list[TrackingSetpoint] = [] + keys: set[tuple[str, str, str]] = set() + for setpoint in self.setpoints: + if not isinstance(setpoint, TrackingSetpoint): + raise TypeError("setpoints must contain TrackingSetpoint values.") + if setpoint.key in keys: + raise ValueError(f"Duplicate tracking setpoint {setpoint.key!r}.") + keys.add(setpoint.key) + snapshots.append(setpoint.snapshot()) + object.__setattr__(self, "setpoints", tuple(snapshots)) + + def snapshot(self) -> TrackingFrame: + return TrackingFrame(self.setpoints) + + +@dataclass(frozen=True, slots=True) +class TimedTrackingSequence: + """Tracking frames aligned by index with an authoritative command sequence.""" + + env_ids: torch.Tensor + frames: tuple[TrackingFrame, ...] + + def __post_init__(self) -> None: + if not isinstance(self.env_ids, torch.Tensor): + raise TypeError("env_ids must be a torch.Tensor.") + if ( + self.env_ids.dtype != torch.long + or self.env_ids.dim() != 1 + or self.env_ids.numel() < 1 + ): + raise ValueError("env_ids must be a non-empty one-dimensional long tensor.") + if torch.unique(self.env_ids).numel() != self.env_ids.numel(): + raise ValueError("env_ids must be unique.") + frames: list[TrackingFrame] = [] + for frame in self.frames: + if not isinstance(frame, TrackingFrame): + raise TypeError("frames must contain TrackingFrame values.") + snapshot = frame.snapshot() + for setpoint in snapshot.setpoints: + if setpoint.desired.batch_size != self.env_ids.numel(): + raise ValueError("Every setpoint batch must match env_ids.") + if setpoint.desired.device != self.env_ids.device: + raise ValueError("Every setpoint and env_ids must share a device.") + frames.append(snapshot) + object.__setattr__(self, "env_ids", self.env_ids.clone()) + object.__setattr__(self, "frames", tuple(frames)) + + @property + def batch_size(self) -> int: + """Return the represented environment count.""" + return int(self.env_ids.numel()) + + @property + def device(self) -> torch.device: + """Return the sequence tensor device.""" + return self.env_ids.device + + @property + def frame_count(self) -> int: + """Return the number of command-aligned tracking frames.""" + return len(self.frames) + + def snapshot(self) -> TimedTrackingSequence: + return TimedTrackingSequence(self.env_ids, self.frames) + + +@dataclass(frozen=True, slots=True, eq=False) +class TrackingFeedbackBatch: + """One synchronized typed observation from an exact feedback source.""" + + source: TrackingFeedbackSourceRef + state: TrackingState + valid_mask: torch.Tensor + timestamp: float + + def __post_init__(self) -> None: + if not isinstance(self.source, TrackingFeedbackSourceRef): + raise TypeError("source must be a TrackingFeedbackSourceRef.") + if not isinstance(self.state, TrackingState): + raise TypeError("state must be a TrackingState.") + if self.valid_mask.dtype != torch.bool or self.valid_mask.shape != ( + self.state.batch_size, + ): + raise ValueError("valid_mask must have shape (batch_size,) and bool dtype.") + if self.valid_mask.device != self.state.device: + raise ValueError("valid_mask and state must share a device.") + object.__setattr__(self, "source", self.source.snapshot()) + object.__setattr__(self, "state", self.state.snapshot()) + object.__setattr__(self, "valid_mask", self.valid_mask.clone()) + object.__setattr__( + self, + "timestamp", + _non_negative_float(self.timestamp, field_name="timestamp"), + ) + + def snapshot(self) -> TrackingFeedbackBatch: + return TrackingFeedbackBatch( + self.source, self.state, self.valid_mask, self.timestamp + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class TrackingEvaluation: + """Per-row metric result with unit-preserving component errors.""" + + channel_id: TrackingChannelId + accepted_mask: torch.Tensor + valid_mask: torch.Tensor + normalized_error: torch.Tensor + component_errors: Mapping[str, torch.Tensor] = field(default_factory=dict) + + def __post_init__(self) -> None: + _identifier(self.channel_id, field_name="channel_id") + expected = self.accepted_mask.shape + if self.accepted_mask.dtype != torch.bool or self.accepted_mask.dim() != 1: + raise ValueError("accepted_mask must be a one-dimensional bool tensor.") + if self.valid_mask.dtype != torch.bool or self.valid_mask.shape != expected: + raise ValueError("valid_mask must match accepted_mask with bool dtype.") + if self.normalized_error.shape != expected or not torch.is_floating_point( + self.normalized_error + ): + raise ValueError("normalized_error must be a floating tensor per row.") + if not ( + self.accepted_mask.device + == self.valid_mask.device + == self.normalized_error.device + ): + raise ValueError("Evaluation tensors must share a device.") + components: dict[str, torch.Tensor] = {} + for name, value in self.component_errors.items(): + _identifier(name, field_name="component_errors key") + if value.shape != expected or value.device != self.normalized_error.device: + raise ValueError("Every component error must be a per-row tensor.") + components[name] = value.clone() + object.__setattr__(self, "accepted_mask", self.accepted_mask.clone()) + object.__setattr__(self, "valid_mask", self.valid_mask.clone()) + object.__setattr__(self, "normalized_error", self.normalized_error.clone()) + object.__setattr__(self, "component_errors", MappingProxyType(components)) + + def snapshot(self) -> TrackingEvaluation: + return TrackingEvaluation( + self.channel_id, + self.accepted_mask, + self.valid_mask, + self.normalized_error, + self.component_errors, + ) + + +class TrackingFeedbackProvider(Protocol): + """Versioned live port that reads one exact tracking source.""" + + provider_id: str + revision: str + + def observe( + self, source: TrackingFeedbackSourceRef, context: PlanningContext + ) -> TrackingFeedbackBatch: + """Read one synchronized typed feedback batch.""" + + +class TrackingCommandProjector(Protocol): + """Versioned pure projector from an endpoint command to desired state.""" + + projector_id: str + revision: str + + def project( + self, command: EndpointCommand, binding: EndpointTrackingChannelBinding + ) -> TrackingState: + """Project one command into the binding's desired tracking channel.""" + + +class TrackingMetricEvaluator(Protocol): + """Versioned evaluator for one exact metric configuration type.""" + + metric_id: str + revision: str + metric_type: type[TrackingMetricCfg] + + def evaluate( + self, + desired: TrackingState, + observed: TrackingState, + valid_mask: torch.Tensor, + metric: TrackingMetricCfg, + ) -> TrackingEvaluation: + """Evaluate a desired and observed batch row by row.""" + + +class _ExactRegistry: + __slots__ = ("_values", "_kind") + + def __init__(self, values: Iterable[object], *, kind: str) -> None: + normalized: dict[tuple[str, str], object] = {} + for value in values: + identifier = _identifier( + getattr(value, f"{kind}_id"), field_name=f"{kind}_id" + ) + revision = _identifier(getattr(value, "revision"), field_name="revision") + key = identifier, revision + if key in normalized: + raise ValueError(f"Duplicate {kind} registration {key!r}.") + normalized[key] = value + self._values = MappingProxyType(normalized) + self._kind = kind + + @property + def values(self) -> Mapping[tuple[str, str], object]: + return self._values + + def _resolve(self, identifier: str, revision: str) -> object: + key = identifier, revision + try: + return self._values[key] + except KeyError as exc: + raise KeyError(f"Unknown {self._kind} registration {key!r}.") from exc + + +class TrackingFeedbackProviderRegistry(_ExactRegistry): + """Immutable exact-version feedback-provider registry.""" + + def __init__(self, providers: Iterable[TrackingFeedbackProvider] = ()) -> None: + super().__init__(providers, kind="provider") + + def resolve(self, source: TrackingFeedbackSourceRef) -> TrackingFeedbackProvider: + return self._resolve(source.provider_id, source.revision) # type: ignore[return-value] + + +class TrackingProjectorRegistry(_ExactRegistry): + """Immutable exact-version command-projector registry.""" + + def __init__(self, projectors: Iterable[TrackingCommandProjector] = ()) -> None: + super().__init__(projectors, kind="projector") + + def resolve(self, route: TrackingProjectorRef) -> TrackingCommandProjector: + return self._resolve(route.projector_id, route.revision) # type: ignore[return-value] + + +class TrackingEvaluatorRegistry(_ExactRegistry): + """Immutable exact-version metric-evaluator registry.""" + + def __init__(self, evaluators: Iterable[TrackingMetricEvaluator] = ()) -> None: + super().__init__(evaluators, kind="metric") + + def resolve(self, metric: TrackingMetricCfg) -> TrackingMetricEvaluator: + evaluator = self._resolve(metric.metric_id, metric.revision) + if type(metric) is not evaluator.metric_type: # type: ignore[attr-defined] + raise TypeError( + f"Metric {metric.metric_id!r} requires " + f"{evaluator.metric_type.__name__}." # type: ignore[attr-defined] + ) + return evaluator # type: ignore[return-value] + + +class PlanningContextTrackingFeedbackProvider: + """Built-in provider backed by :class:`PlanningContext.robot`.""" + + provider_id = "planning_context.robot" + revision = "1" + + def observe( + self, source: TrackingFeedbackSourceRef, context: PlanningContext + ) -> TrackingFeedbackBatch: + from .bindings import JointPositionTarget + from .state import PlanningContext + + if not isinstance(context, PlanningContext): + raise TypeError("context must be a PlanningContext.") + address = source.address + if not isinstance(address, EndpointTrackingFeedbackAddress): + raise TypeError( + "Built-in provider requires EndpointTrackingFeedbackAddress." + ) + target = address.target + if address.channel_id == JOINT_POSITION_CHANNEL: + if not isinstance(target, JointPositionTarget): + raise TypeError("joint.position requires a JointPositionTarget.") + state: TrackingState = JointPositionTrackingState( + context.robot.qpos[:, target.joint_ids] + ) + elif address.channel_id == BASE_POSE_CHANNEL: + if context.robot.root_pose is None: + raise RuntimeError("RobotObservation.root_pose is unavailable.") + state = PoseTrackingState(context.robot.root_pose) + elif address.channel_id == WHOLE_BODY_POSE_CHANNEL: + if context.robot.root_pose is None: + raise RuntimeError("RobotObservation.root_pose is unavailable.") + joints = ( + context.robot.qpos[:, target.joint_ids] + if isinstance(target, JointPositionTarget) + else context.robot.qpos + ) + state = WholeBodyPoseTrackingState(context.robot.root_pose, joints) + else: + raise KeyError( + f"Unsupported built-in tracking channel {address.channel_id!r}." + ) + return TrackingFeedbackBatch( + source=source, + state=state, + valid_mask=torch.ones( + context.batch_size, dtype=torch.bool, device=state.device + ), + timestamp=context.robot.timestamp, + ) + + +class JointPositionTrackingProjector: + """Built-in projector for joint-position endpoint commands.""" + + projector_id = "joint_position_payload" + revision = "1" + + def project( + self, command: EndpointCommand, binding: EndpointTrackingChannelBinding + ) -> JointPositionTrackingState: + from .runtime_commands import EndpointCommand, JointPositionPayload + + if not isinstance(command, EndpointCommand): + raise TypeError("command must be an EndpointCommand.") + if binding.channel_id != JOINT_POSITION_CHANNEL: + raise ValueError("Joint projector requires the joint.position channel.") + if not isinstance(command.payload, JointPositionPayload): + raise TypeError("Joint projector requires JointPositionPayload.") + address = binding.source.address + if isinstance(address, EndpointTrackingFeedbackAddress): + if address.target.address_fingerprint != command.target.address_fingerprint: + raise ValueError( + "Command and feedback binding target different endpoints." + ) + return JointPositionTrackingState(command.payload.positions) + + +def _compatible( + desired: TrackingState, + observed: TrackingState, + valid_mask: torch.Tensor, + expected_type: type[TrackingState], +) -> None: + if type(desired) is not expected_type or type(observed) is not expected_type: + raise TypeError(f"Metric requires {expected_type.__name__} values.") + if desired.batch_size != observed.batch_size or desired.device != observed.device: + raise ValueError("Desired and observed batches must match.") + if valid_mask.dtype != torch.bool or valid_mask.shape != (desired.batch_size,): + raise ValueError("valid_mask must be a bool tensor with one value per row.") + if valid_mask.device != desired.device: + raise ValueError("valid_mask and states must share a device.") + + +def _pose_errors( + desired: torch.Tensor, observed: torch.Tensor +) -> tuple[torch.Tensor, torch.Tensor]: + translation = torch.linalg.vector_norm( + desired[:, :3, 3] - observed[:, :3, 3], dim=1 + ) + relative = desired[:, :3, :3].transpose(1, 2) @ observed[:, :3, :3] + cosine = ((relative.diagonal(dim1=1, dim2=2).sum(dim=1) - 1.0) * 0.5).clamp( + -1.0, 1.0 + ) + return translation, torch.acos(cosine) + + +class JointPositionTrackingEvaluator: + """Evaluator for :class:`JointPositionTrackingMetric`.""" + + metric_id = JointPositionTrackingMetric.metric_id + revision = JointPositionTrackingMetric.revision + metric_type = JointPositionTrackingMetric + + def evaluate(self, desired, observed, valid_mask, metric) -> TrackingEvaluation: + _compatible(desired, observed, valid_mask, JointPositionTrackingState) + if type(metric) is not JointPositionTrackingMetric: + raise TypeError("metric must be JointPositionTrackingMetric.") + if desired.positions.shape != observed.positions.shape: + raise ValueError("Joint-position state shapes must match.") + error = (desired.positions - observed.positions).abs().amax(dim=1) + normalized = error / metric.tolerance + return TrackingEvaluation( + JOINT_POSITION_CHANNEL, + valid_mask & (error <= metric.tolerance), + valid_mask, + normalized, + {"joint_max_abs": error}, + ) + + +class PoseTrackingEvaluator: + """Evaluator for :class:`PoseTrackingMetric`.""" + + metric_id = PoseTrackingMetric.metric_id + revision = PoseTrackingMetric.revision + metric_type = PoseTrackingMetric + + def evaluate(self, desired, observed, valid_mask, metric) -> TrackingEvaluation: + _compatible(desired, observed, valid_mask, PoseTrackingState) + if type(metric) is not PoseTrackingMetric: + raise TypeError("metric must be PoseTrackingMetric.") + translation, rotation = _pose_errors(desired.poses, observed.poses) + normalized = torch.maximum( + translation / metric.translation_tolerance, + rotation / metric.rotation_tolerance, + ) + return TrackingEvaluation( + BASE_POSE_CHANNEL, + valid_mask & (normalized <= 1.0), + valid_mask, + normalized, + {"translation": translation, "rotation": rotation}, + ) + + +class WholeBodyPoseTrackingEvaluator: + """Evaluator for :class:`WholeBodyPoseTrackingMetric`.""" + + metric_id = WholeBodyPoseTrackingMetric.metric_id + revision = WholeBodyPoseTrackingMetric.revision + metric_type = WholeBodyPoseTrackingMetric + + def evaluate(self, desired, observed, valid_mask, metric) -> TrackingEvaluation: + _compatible(desired, observed, valid_mask, WholeBodyPoseTrackingState) + if type(metric) is not WholeBodyPoseTrackingMetric: + raise TypeError("metric must be WholeBodyPoseTrackingMetric.") + if desired.joint_positions.shape != observed.joint_positions.shape: + raise ValueError("Whole-body joint-position shapes must match.") + translation, rotation = _pose_errors(desired.root_poses, observed.root_poses) + joint = (desired.joint_positions - observed.joint_positions).abs().amax(dim=1) + normalized = torch.maximum( + torch.maximum( + translation / metric.translation_tolerance, + rotation / metric.rotation_tolerance, + ), + joint / metric.joint_position_tolerance, + ) + return TrackingEvaluation( + WHOLE_BODY_POSE_CHANNEL, + valid_mask & (normalized <= 1.0), + valid_mask, + normalized, + {"translation": translation, "rotation": rotation, "joint_max_abs": joint}, + ) + + +class TrackingRuntime: + """Runtime facade for projecting commands and evaluating typed feedback.""" + + __slots__ = ("_providers", "_projectors", "_evaluators") + + def __init__( + self, + providers: TrackingFeedbackProviderRegistry, + projectors: TrackingProjectorRegistry, + evaluators: TrackingEvaluatorRegistry, + ) -> None: + if type(providers) is not TrackingFeedbackProviderRegistry: + raise TypeError( + "providers must be exactly TrackingFeedbackProviderRegistry." + ) + if type(projectors) is not TrackingProjectorRegistry: + raise TypeError("projectors must be exactly TrackingProjectorRegistry.") + if type(evaluators) is not TrackingEvaluatorRegistry: + raise TypeError("evaluators must be exactly TrackingEvaluatorRegistry.") + self._providers = providers + self._projectors = projectors + self._evaluators = evaluators + + @property + def providers(self) -> TrackingFeedbackProviderRegistry: + """Return the immutable exact-version provider registry.""" + return self._providers + + @property + def projectors(self) -> TrackingProjectorRegistry: + """Return the immutable exact-version projector registry.""" + return self._projectors + + @property + def evaluators(self) -> TrackingEvaluatorRegistry: + """Return the immutable exact-version evaluator registry.""" + return self._evaluators + + @classmethod + def with_builtins(cls) -> TrackingRuntime: + """Create a runtime with context feedback and built-in typed metrics.""" + return cls( + TrackingFeedbackProviderRegistry( + [PlanningContextTrackingFeedbackProvider()] + ), + TrackingProjectorRegistry([JointPositionTrackingProjector()]), + TrackingEvaluatorRegistry( + [ + JointPositionTrackingEvaluator(), + PoseTrackingEvaluator(), + WholeBodyPoseTrackingEvaluator(), + ] + ), + ) + + def project( + self, command: EndpointCommand, binding: EndpointTrackingChannelBinding + ) -> TrackingState: + """Project one command through the exact binding-owned projector.""" + return self.projectors.resolve(binding.projector).project(command, binding) + + def observe( + self, setpoint: TrackingSetpoint, context: PlanningContext + ) -> TrackingFeedbackBatch: + """Read the exact feedback source for one setpoint.""" + feedback = self.providers.resolve(setpoint.binding.source).observe( + setpoint.binding.source, context + ) + if ( + feedback.source.source_fingerprint + != setpoint.binding.source.source_fingerprint + ): + raise ValueError("Feedback provider returned a different source.") + if feedback.state.channel_id != setpoint.binding.channel_id: + raise TypeError("Feedback state does not match the bound channel.") + if feedback.timestamp != context.robot.timestamp: + raise ValueError( + "Tracking feedback must use the current planning-context timestamp." + ) + if feedback.state.batch_size != context.batch_size: + raise ValueError("Tracking feedback batch must match the context batch.") + if feedback.state.device != context.robot.qpos.device: + raise ValueError("Tracking feedback and context must share a device.") + return feedback + + def evaluate( + self, + setpoint: TrackingSetpoint, + feedback: TrackingFeedbackBatch, + metric: TrackingMetricCfg, + ) -> TrackingEvaluation: + """Evaluate one observed setpoint with an exact metric implementation.""" + if metric.channel_id != setpoint.binding.channel_id: + raise ValueError("Metric and setpoint channels must match.") + if ( + feedback.source.source_fingerprint + != setpoint.binding.source.source_fingerprint + ): + raise ValueError("Feedback source does not match the setpoint binding.") + return self.evaluators.resolve(metric).evaluate( + setpoint.desired, feedback.state, feedback.valid_mask, metric + ) + + def evaluate_frame( + self, + frame: TrackingFrame, + metrics: Iterable[TrackingMetricCfg], + context: PlanningContext, + ) -> Mapping[tuple[str, str, str], TrackingEvaluation]: + """Observe and evaluate every setpoint required by one frame.""" + by_channel = {metric.channel_id: metric for metric in metrics} + results: dict[tuple[str, str, str], TrackingEvaluation] = {} + for setpoint in frame.setpoints: + try: + metric = by_channel[setpoint.binding.channel_id] + except KeyError as exc: + raise KeyError( + f"No metric configured for channel {setpoint.binding.channel_id!r}." + ) from exc + results[setpoint.key] = self.evaluate( + setpoint, self.observe(setpoint, context), metric + ) + return MappingProxyType(results) + + +__all__ = [ + "BASE_POSE_CHANNEL", + "FeedbackTerminalAcceptance", + "InFlightTrackingPolicy", + "JOINT_POSITION_CHANNEL", + "JointPositionTrackingEvaluator", + "JointPositionTrackingMetric", + "JointPositionTrackingProjector", + "JointPositionTrackingState", + "EndpointTrackingChannelBinding", + "EndpointTrackingFeedbackAddress", + "PlanningContextTrackingFeedbackProvider", + "PoseTrackingEvaluator", + "PoseTrackingMetric", + "PoseTrackingState", + "TerminalAcceptance", + "TimedTerminalAcceptance", + "TimedTrackingSequence", + "TrackingChannelId", + "TrackingCommandProjector", + "TrackingEvaluation", + "TrackingEvaluatorRegistry", + "TrackingFeedbackAddress", + "TrackingFeedbackBatch", + "TrackingFeedbackProvider", + "TrackingFeedbackProviderRegistry", + "TrackingFeedbackSourceRef", + "TrackingFrame", + "TrackingMetricCfg", + "TrackingMetricEvaluator", + "TrackingPolicy", + "TrackingProjectorRef", + "TrackingProjectorRegistry", + "TrackingRuntime", + "TrackingSetpoint", + "TrackingState", + "WHOLE_BODY_POSE_CHANNEL", + "WholeBodyPoseTrackingEvaluator", + "WholeBodyPoseTrackingMetric", + "WholeBodyPoseTrackingState", +] diff --git a/embodichain/lab/sim/atomic_actions/trajectory_ops.py b/embodichain/lab/sim/atomic_actions/trajectory_ops.py index 0154e8418..4593ab430 100644 --- a/embodichain/lab/sim/atomic_actions/trajectory_ops.py +++ b/embodichain/lab/sim/atomic_actions/trajectory_ops.py @@ -18,7 +18,6 @@ from __future__ import annotations -import numpy as np import torch from embodichain.lab.sim.planners import MoveType, PlanResult, PlanState @@ -30,29 +29,29 @@ def resolve_pose_target( target: torch.Tensor, *, - n_envs: int, + num_envs: int, device: torch.device | str, ) -> torch.Tensor: """Validate and copy an end-effector target onto the planning device.""" if not isinstance(target, torch.Tensor): raise TypeError( - f"target must be torch.Tensor of shape (4, 4), ({n_envs}, 4, 4), " - f"or ({n_envs}, n_waypoint, 4, 4)" + f"target must be torch.Tensor of shape (4, 4), ({num_envs}, 4, 4), " + f"or ({num_envs}, n_waypoint, 4, 4)" ) target = target.to(device=device, dtype=torch.float32).clone() if target.shape == (4, 4): - target = target.unsqueeze(0).repeat(n_envs, 1, 1) + target = target.unsqueeze(0).repeat(num_envs, 1, 1) if target.dim() == 3: - if target.shape != (n_envs, 4, 4): + if target.shape != (num_envs, 4, 4): raise ValueError( - f"target tensor must have shape (4, 4) or ({n_envs}, 4, 4), " + f"target tensor must have shape (4, 4) or ({num_envs}, 4, 4), " f"but got {target.shape}" ) elif target.dim() == 4: - if target.shape[0] != n_envs or target.shape[2:] != (4, 4): + if target.shape[0] != num_envs or target.shape[2:] != (4, 4): raise ValueError( "multi-waypoint target tensor must have shape " - f"({n_envs}, n_waypoint, 4, 4), but got {target.shape}" + f"({num_envs}, n_waypoint, 4, 4), but got {target.shape}" ) if target.shape[1] == 0: raise ValueError( @@ -61,8 +60,8 @@ def resolve_pose_target( ) else: raise ValueError( - f"target tensor must be (4, 4), ({n_envs}, 4, 4), or " - f"({n_envs}, n_waypoint, 4, 4), but got {target.shape}" + f"target tensor must be (4, 4), ({num_envs}, 4, 4), or " + f"({num_envs}, n_waypoint, 4, 4), but got {target.shape}" ) return target @@ -70,7 +69,7 @@ def resolve_pose_target( def resolve_joint_target( target_qpos: torch.Tensor, *, - n_envs: int, + num_envs: int, joint_dof: int, control_part: str, device: torch.device | str, @@ -79,23 +78,23 @@ def resolve_joint_target( if not isinstance(target_qpos, torch.Tensor): raise TypeError( f"target qpos for '{control_part}' must be a torch.Tensor with shape " - f"({joint_dof},), ({n_envs}, {joint_dof}), or " - f"({n_envs}, n_waypoint, {joint_dof})" + f"({joint_dof},), ({num_envs}, {joint_dof}), or " + f"({num_envs}, n_waypoint, {joint_dof})" ) target_qpos = target_qpos.to(device=device, dtype=torch.float32).clone() if target_qpos.shape == (joint_dof,): - target_qpos = target_qpos.unsqueeze(0).repeat(n_envs, 1) + target_qpos = target_qpos.unsqueeze(0).repeat(num_envs, 1) if target_qpos.dim() == 2: - if target_qpos.shape != (n_envs, joint_dof): + if target_qpos.shape != (num_envs, joint_dof): raise ValueError( f"target qpos for '{control_part}' must have shape ({joint_dof},) " - f"or ({n_envs}, {joint_dof}), but got {target_qpos.shape}" + f"or ({num_envs}, {joint_dof}), but got {target_qpos.shape}" ) elif target_qpos.dim() == 3: - if target_qpos.shape[0] != n_envs or target_qpos.shape[2] != joint_dof: + if target_qpos.shape[0] != num_envs or target_qpos.shape[2] != joint_dof: raise ValueError( f"multi-waypoint target qpos for '{control_part}' must have shape " - f"({n_envs}, n_waypoint, {joint_dof}), but got {target_qpos.shape}" + f"({num_envs}, n_waypoint, {joint_dof}), but got {target_qpos.shape}" ) if target_qpos.shape[1] == 0: raise ValueError( @@ -173,6 +172,76 @@ def translate_pose_world(pose: torch.Tensor, offset: torch.Tensor) -> torch.Tens return result +def axis_translation_keyframes( + start_pose: torch.Tensor, + end_pose: torch.Tensor, + axis: torch.Tensor, + *, + n_waypoints: int, +) -> torch.Tensor: + """Build exact Cartesian translation targets along one world-space axis. + + The returned targets exclude ``start_pose`` and include ``end_pose``. This + matches motion generation, where the observed start configuration is added + separately. Rotation remains fixed for the entire constrained segment. + + Args: + start_pose: Batched segment-start poses, shape ``(B, 4, 4)``. + end_pose: Batched segment-end poses, shape ``(B, 4, 4)``. + axis: Shared ``(3,)`` or batched ``(B, 3)`` world-space axis. + n_waypoints: Number of target poses, excluding the segment start. + + Returns: + Batched keyframes with shape ``(B, n_waypoints, 4, 4)``. + + Raises: + ValueError: If poses, axis, count, rotation, or displacement are invalid. + """ + if ( + start_pose.dim() != 3 + or start_pose.shape[1:] != (4, 4) + or end_pose.shape != start_pose.shape + ): + raise ValueError("start_pose and end_pose must have shape (B, 4, 4).") + if n_waypoints < 1: + raise ValueError("n_waypoints must be at least 1.") + axis = axis.to(device=start_pose.device, dtype=start_pose.dtype) + if axis.shape == (3,): + axis = axis.unsqueeze(0).expand(start_pose.shape[0], -1) + if axis.shape != (start_pose.shape[0], 3) or not torch.isfinite(axis).all(): + raise ValueError("axis must be finite with shape (3,) or (B, 3).") + axis_norm = torch.linalg.vector_norm(axis, dim=1, keepdim=True) + if torch.any(axis_norm <= 1.0e-6): + raise ValueError("axis must be non-zero.") + axis = axis / axis_norm + if not torch.allclose( + start_pose[:, :3, :3], + end_pose[:, :3, :3], + rtol=1.0e-5, + atol=1.0e-6, + ): + raise ValueError("Axis translation requires a fixed segment rotation.") + displacement = end_pose[:, :3, 3] - start_pose[:, :3, 3] + orthogonal = displacement - (displacement * axis).sum(dim=1, keepdim=True) * axis + if torch.any(torch.linalg.vector_norm(orthogonal, dim=1) > 1.0e-5): + raise ValueError("Segment displacement must be parallel to axis.") + + weights = torch.linspace( + 0.0, + 1.0, + n_waypoints + 1, + dtype=start_pose.dtype, + device=start_pose.device, + )[1:] + result = start_pose[:, None].expand(-1, n_waypoints, -1, -1).clone() + result[:, :, :3, 3] = torch.lerp( + start_pose[:, None, :3, 3], + end_pose[:, None, :3, 3], + weights[None, :, None], + ) + return result + + def split_three_segments( sample_count: int, hand_interp_steps: int, @@ -182,7 +251,7 @@ def split_three_segments( third_segment_name: str = "third", ) -> tuple[int, int, int]: """Split a sample budget into motion, hand, and motion segments.""" - first = int(np.round(sample_count - hand_interp_steps) * first_segment_ratio) + first = int(round((sample_count - hand_interp_steps) * first_segment_ratio)) if first < 2: raise ValueError( f"Not enough waypoints for {first_segment_name} trajectory. " @@ -251,7 +320,6 @@ def to_full_robot_trajectory( base_qpos: torch.Tensor, joint_ids: list[int], env_ids: torch.Tensor, - control_dt: float, ) -> tuple[torch.Tensor, TimedTrajectory]: """Embed a controlled-joint plan into a timed full-robot trajectory.""" positions = result.positions @@ -271,27 +339,18 @@ def embed_derivative(value: torch.Tensor | None) -> torch.Tensor | None: full[:, :, joint_ids] = value return full - duration: float | torch.Tensor | None = result.duration if result.dt is None: - duration_tensor = torch.as_tensor( - result.duration, - dtype=torch.float32, - device=base_qpos.device, - ) - if not bool((duration_tensor > 0.0).any().item()): - duration = None + raise ValueError("PlanResult must include explicit dt.") timed = TimedTrajectory.from_positions( full_positions, env_ids=env_ids, - control_dt=control_dt, velocities=embed_derivative(result.velocities), accelerations=embed_derivative(result.accelerations), dt=result.dt, - duration=duration, ) success = normalize_success_mask( result.success, - n_envs=base_qpos.shape[0], + num_envs=base_qpos.shape[0], device=base_qpos.device, name="PlanResult.success", ) @@ -299,6 +358,7 @@ def embed_derivative(value: torch.Tensor | None) -> torch.Tensor | None: __all__ = [ + "axis_translation_keyframes", "build_joint_plan_states", "build_pose_plan_states", "interpolate_hand_qpos", diff --git a/embodichain/lab/sim/atomic_actions/transports.py b/embodichain/lab/sim/atomic_actions/transports.py new file mode 100644 index 000000000..18b95178a --- /dev/null +++ b/embodichain/lab/sim/atomic_actions/transports.py @@ -0,0 +1,489 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Endpoint-command transport contracts and deterministic routing.""" + +from __future__ import annotations + +from collections.abc import Callable, Iterable, Mapping +import math +from types import MappingProxyType +from typing import TYPE_CHECKING, Protocol, runtime_checkable + +from .bindings import RuntimeEndpointTarget +from .runtime_commands import ( + EndpointCommand, + RuntimeCommandFrame, + RuntimeCommandPayload, +) + +if TYPE_CHECKING: + from .runner import CommandAcknowledgement + from .state import PlanningContext + + +def _validate_identifier(value: str, *, field_name: str) -> str: + """Validate and return one strict identifier.""" + if not isinstance(value, str) or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +def _validate_timeout(timeout: float) -> float: + """Validate and normalize one acknowledgement timeout.""" + if isinstance(timeout, bool) or not isinstance(timeout, (int, float)): + raise TypeError("timeout must be a real number.") + normalized = float(timeout) + if not math.isfinite(normalized) or normalized <= 0.0: + raise ValueError("timeout must be finite and greater than zero.") + return normalized + + +@runtime_checkable +class EndpointCommandTransport(Protocol): + """Backend that owns one kind of runtime endpoint command. + + Implementations own live simulator entities, device clients, or controller + handles. Runtime command values retain only immutable addressing and payload + data, so they remain independent of those process-owned resources. + """ + + @property + def transport_id(self) -> str: + """Return the exact identifier used to register this transport.""" + + @property + def payload_type(self) -> type[RuntimeCommandPayload]: + """Return the runtime payload type accepted by :meth:`send`.""" + + def send( + self, + frame: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Submit one transport-local command frame. + + Implementations must actively neutralize every inactive environment + row for every addressed target. Silently skipping an inactive row is + unsafe for persistent controllers such as base-velocity transports. + """ + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Hold transport-local targets at their observed state.""" + + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + """Cancel outstanding commands for transport-local targets.""" + + +class EndpointCommandRouter: + """Route generic endpoint operations to exact registered transports. + + The router implements :class:`~.runner.CommandSink` structurally while + avoiding a module-load dependency on ``runner``. Acknowledgement types are + imported only when an operation is executed, which keeps the transport + boundary safe to import while the runner imports this module. + + Args: + transports: Either an exact ``transport_id -> transport`` mapping or an + iterable of transports from which that mapping is built. Mapping + keys must exactly equal each value's declared ``transport_id``. + + Raises: + TypeError: If a registration does not implement the transport contract. + ValueError: If an identifier is invalid, a mapping key is not exact, or + the same transport identifier is registered more than once. + """ + + def __init__( + self, + transports: ( + Mapping[str, EndpointCommandTransport] | Iterable[EndpointCommandTransport] + ), + ) -> None: + registrations = self._registrations(transports) + registered: dict[str, EndpointCommandTransport] = {} + payload_types: dict[str, type[RuntimeCommandPayload]] = {} + for map_key, transport in registrations: + if not isinstance(transport, EndpointCommandTransport): + raise TypeError( + "Registered values must implement EndpointCommandTransport." + ) + transport_id = _validate_identifier( + transport.transport_id, + field_name="EndpointCommandTransport.transport_id", + ) + if map_key is not None and map_key != transport_id: + raise ValueError( + f"Transport mapping key {map_key!r} must exactly match declared " + f"transport_id {transport_id!r}." + ) + if transport_id in registered: + raise ValueError( + f"Endpoint transport {transport_id!r} is registered more than once." + ) + payload_type = transport.payload_type + if not isinstance(payload_type, type) or not issubclass( + payload_type, RuntimeCommandPayload + ): + raise TypeError( + f"Transport {transport_id!r} payload_type must be a " + "RuntimeCommandPayload subclass." + ) + registered[transport_id] = transport + payload_types[transport_id] = payload_type + self._transports: Mapping[str, EndpointCommandTransport] = MappingProxyType( + registered + ) + self._payload_types: Mapping[str, type[RuntimeCommandPayload]] = ( + MappingProxyType(payload_types) + ) + + @staticmethod + def _registrations( + transports: ( + Mapping[str, EndpointCommandTransport] | Iterable[EndpointCommandTransport] + ), + ) -> tuple[tuple[str | None, EndpointCommandTransport], ...]: + """Normalize mapping and iterable registration forms.""" + if isinstance(transports, Mapping): + registrations: list[tuple[str | None, EndpointCommandTransport]] = [] + for key, transport in transports.items(): + _validate_identifier(key, field_name="Transport mapping keys") + registrations.append((key, transport)) + return tuple(registrations) + if isinstance(transports, (str, bytes)): + raise TypeError("transports must be a mapping or iterable of transports.") + try: + return tuple((None, transport) for transport in transports) + except TypeError as exc: + raise TypeError( + "transports must be a mapping or iterable of transports." + ) from exc + + @property + def transports(self) -> Mapping[str, EndpointCommandTransport]: + """Return the immutable exact transport registry.""" + return self._transports + + def send( + self, + frame: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Route one synchronized command frame by transport identifier. + + Dispatch is preflighted before any transport is called. An unknown + transport or incompatible payload therefore rejects the whole frame + without creating a partially dispatched operation. + + Args: + frame: Generic runtime command frame to split by transport. + timeout: Maximum acknowledgement latency for each transport. + + Returns: + Aggregated acknowledgement. It is accepted only when every + addressed transport accepts its local frame. + """ + if not isinstance(frame, RuntimeCommandFrame): + raise TypeError("frame must be a RuntimeCommandFrame.") + normalized_timeout = _validate_timeout(timeout) + grouped: dict[str, list[EndpointCommand]] = {} + for command in frame.commands: + grouped.setdefault(command.transport_id, []).append(command) + + unknown = tuple( + transport_id + for transport_id in grouped + if transport_id not in self._transports + ) + if unknown: + return self._unknown_acknowledgement("send", unknown) + + incompatibilities: list[str] = [] + for transport_id, commands in grouped.items(): + payload_type = self._payload_types[transport_id] + for command in commands: + if not isinstance(command.payload, payload_type): + incompatibilities.append( + f"transport {transport_id!r} expects " + f"{payload_type.__name__}, got " + f"{type(command.payload).__name__} for target " + f"{command.target.target_id!r}" + ) + if incompatibilities: + return self._rejected_acknowledgement( + "send rejected: " + "; ".join(incompatibilities) + ) + + acknowledgements: list[tuple[str, CommandAcknowledgement]] = [] + for transport_id, commands in grouped.items(): + subframe = RuntimeCommandFrame( + commands=tuple(commands), + active_mask=frame.active_mask, + env_ids=frame.env_ids, + hold_duration=frame.hold_duration, + ) + transport = self._transports[transport_id] + acknowledgement = self._invoke_transport( + transport_id, + "send", + lambda transport=transport, subframe=subframe: transport.send( + subframe, + timeout=normalized_timeout, + ), + ) + acknowledgements.append((transport_id, acknowledgement)) + return self._aggregate_acknowledgements("send", acknowledgements) + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Route an observed-state hold request by target transport. + + Args: + targets: Runtime destinations to hold. + context: Fresh observation used by each transport to form its hold. + timeout: Maximum acknowledgement latency for each transport. + + Returns: + Aggregated acknowledgement. It is accepted only when every + addressed transport accepts its hold. + """ + normalized_timeout = _validate_timeout(timeout) + grouped = self._group_targets(targets) + unknown = tuple( + transport_id + for transport_id in grouped + if transport_id not in self._transports + ) + if unknown: + return self._unknown_acknowledgement("hold", unknown) + + acknowledgements: list[tuple[str, CommandAcknowledgement]] = [] + for transport_id, local_targets in grouped.items(): + transport = self._transports[transport_id] + acknowledgement = self._invoke_transport( + transport_id, + "hold", + lambda transport=transport, local_targets=local_targets: transport.hold( + local_targets, + context, + timeout=normalized_timeout, + ), + ) + acknowledgements.append((transport_id, acknowledgement)) + return self._aggregate_acknowledgements("hold", acknowledgements) + + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + """Route cancellation by target transport. + + Args: + targets: Runtime destinations whose outstanding commands are + cancelled. + timeout: Maximum acknowledgement latency for each transport. + + Returns: + Aggregated acknowledgement. It is accepted only when every + addressed transport accepts cancellation. + """ + normalized_timeout = _validate_timeout(timeout) + grouped = self._group_targets(targets) + unknown = tuple( + transport_id + for transport_id in grouped + if transport_id not in self._transports + ) + if unknown: + return self._unknown_acknowledgement("cancel", unknown) + + acknowledgements: list[tuple[str, CommandAcknowledgement]] = [] + for transport_id, local_targets in grouped.items(): + transport = self._transports[transport_id] + acknowledgement = self._invoke_transport( + transport_id, + "cancel", + lambda transport=transport, local_targets=local_targets: transport.cancel( + local_targets, + timeout=normalized_timeout, + ), + ) + acknowledgements.append((transport_id, acknowledgement)) + return self._aggregate_acknowledgements("cancel", acknowledgements) + + @staticmethod + def _group_targets( + targets: tuple[RuntimeEndpointTarget, ...], + ) -> dict[str, tuple[RuntimeEndpointTarget, ...]]: + """Validate, snapshot, and group runtime targets in first-use order.""" + if isinstance(targets, (str, bytes)): + raise TypeError( + "targets must be an iterable of RuntimeEndpointTarget values." + ) + try: + source_targets = tuple(targets) + except TypeError as exc: + raise TypeError( + "targets must be an iterable of RuntimeEndpointTarget values." + ) from exc + + grouped: dict[str, list[RuntimeEndpointTarget]] = {} + for target in source_targets: + if not isinstance(target, RuntimeEndpointTarget): + raise TypeError( + "targets values must be RuntimeEndpointTarget instances." + ) + snapshot = target.snapshot() + if type(snapshot) is not type(target) or snapshot is target: + raise TypeError( + "RuntimeEndpointTarget.snapshot() must return an independently " + "owned value of the same target type." + ) + transport_id = _validate_identifier( + snapshot.transport_id, + field_name="RuntimeEndpointTarget.transport_id", + ) + _validate_identifier( + snapshot.target_id, + field_name="RuntimeEndpointTarget.target_id", + ) + grouped.setdefault(transport_id, []).append(snapshot) + return { + transport_id: tuple(local_targets) + for transport_id, local_targets in grouped.items() + } + + @staticmethod + def _validate_acknowledgement( + transport_id: str, + operation: str, + acknowledgement: object, + ) -> CommandAcknowledgement: + """Require transports to return the runner acknowledgement value.""" + from .runner import CommandAcknowledgement + + if not isinstance(acknowledgement, CommandAcknowledgement): + raise TypeError( + f"Transport {transport_id!r} {operation}() must return " + f"CommandAcknowledgement, got {type(acknowledgement).__name__}." + ) + return acknowledgement + + @staticmethod + def _invoke_transport( + transport_id: str, + operation: str, + invoke: Callable[[], object], + ) -> CommandAcknowledgement: + """Convert one transport-local failure without blocking other transports.""" + from .runner import CommandAcknowledgement, CommandAckStatus + + try: + acknowledgement = invoke() + return EndpointCommandRouter._validate_acknowledgement( + transport_id, + operation, + acknowledgement, + ) + except Exception as exc: + return CommandAcknowledgement( + CommandAckStatus.REJECTED, + f"Transport {transport_id!r} {operation}() failed with " + f"{type(exc).__name__}: {exc}", + ) + + @staticmethod + def _aggregate_acknowledgements( + operation: str, + acknowledgements: list[tuple[str, CommandAcknowledgement]], + ) -> CommandAcknowledgement: + """Aggregate transport acknowledgements with deterministic precedence.""" + from .runner import CommandAcknowledgement, CommandAckStatus + + failures = [ + (transport_id, acknowledgement) + for transport_id, acknowledgement in acknowledgements + if not acknowledgement.accepted + ] + if not failures: + diagnostics = "; ".join( + f"{transport_id}: {acknowledgement.message}" + for transport_id, acknowledgement in acknowledgements + if acknowledgement.message + ) + return CommandAcknowledgement.accepted_ack(diagnostics) + + status = ( + CommandAckStatus.TIMED_OUT + if any( + acknowledgement.status is CommandAckStatus.TIMED_OUT + for _, acknowledgement in failures + ) + else CommandAckStatus.REJECTED + ) + diagnostics = "; ".join( + f"transport {transport_id!r} {acknowledgement.status.value}: " + f"{acknowledgement.message or 'no diagnostic'}" + for transport_id, acknowledgement in failures + ) + return CommandAcknowledgement( + status, + f"{operation} failed: {diagnostics}", + ) + + @staticmethod + def _unknown_acknowledgement( + operation: str, + transport_ids: tuple[str, ...], + ) -> CommandAcknowledgement: + """Build a rejection for unregistered exact transport identifiers.""" + identifiers = ", ".join(repr(transport_id) for transport_id in transport_ids) + return EndpointCommandRouter._rejected_acknowledgement( + f"{operation} rejected: no transport is registered for {identifiers}." + ) + + @staticmethod + def _rejected_acknowledgement(message: str) -> CommandAcknowledgement: + """Build one rejected runner acknowledgement without an import cycle.""" + from .runner import CommandAcknowledgement, CommandAckStatus + + return CommandAcknowledgement(CommandAckStatus.REJECTED, message) + + +__all__ = ["EndpointCommandRouter", "EndpointCommandTransport"] diff --git a/embodichain/lab/sim/objects/articulation.py b/embodichain/lab/sim/objects/articulation.py index 239258b13..d9128a149 100644 --- a/embodichain/lab/sim/objects/articulation.py +++ b/embodichain/lab/sim/objects/articulation.py @@ -1488,6 +1488,15 @@ def set_qf( data_type=ArticulationGPUAPIWriteType.JOINT_FORCE, ) + def get_qf(self) -> torch.Tensor: + """Get the current generalized efforts (qf) of the articulation. + + Returns: + torch.Tensor: Joint efforts with shape (N, dof), where N is the + number of environments. + """ + return self.body_data.qf + def get_qf_limits( self, joint_ids: Sequence[int] | torch.Tensor | None = None, diff --git a/embodichain/lab/sim/objects/rigid_object.py b/embodichain/lab/sim/objects/rigid_object.py index 5a4bd80a1..39185a8a0 100644 --- a/embodichain/lab/sim/objects/rigid_object.py +++ b/embodichain/lab/sim/objects/rigid_object.py @@ -502,12 +502,17 @@ def get_local_pose_cpu( """Helper function to get local pose on CPU.""" if to_matrix: pose = torch.as_tensor( - [entity.get_local_pose() for entity in entities], + np.array([entity.get_local_pose() for entity in entities]), + dtype=torch.float32, ) else: - xyzs = torch.as_tensor([entity.get_location() for entity in entities]) + xyzs = torch.as_tensor( + np.array([entity.get_location() for entity in entities]), + dtype=torch.float32, + ) quats = torch.as_tensor( - [entity.get_rotation_quat() for entity in entities] + np.array([entity.get_rotation_quat() for entity in entities]), + dtype=torch.float32, ) quats = convert_quat(quats, to="wxyz") pose = torch.cat((xyzs, quats), dim=-1) @@ -856,7 +861,9 @@ def get_inertia(self, env_ids: Sequence[int] | None = None) -> torch.Tensor: ) inertias.append(inertia) - return torch.as_tensor(inertias, dtype=torch.float32, device=self.device) + return torch.as_tensor( + np.array(inertias), dtype=torch.float32, device=self.device + ) def set_visual_material( self, @@ -1074,7 +1081,7 @@ def get_body_scale(self, env_ids: Sequence[int] | None = None) -> torch.Tensor: """ ids = env_ids if env_ids is not None else range(self.num_instances) return torch.as_tensor( - [self._entities[id].get_body_scale() for id in ids], + np.array([self._entities[id].get_body_scale() for id in ids]), dtype=torch.float32, device=self.device, ) diff --git a/embodichain/lab/sim/objects/robot.py b/embodichain/lab/sim/objects/robot.py index efbc7b8c4..7b8a1340e 100644 --- a/embodichain/lab/sim/objects/robot.py +++ b/embodichain/lab/sim/objects/robot.py @@ -775,7 +775,7 @@ def compute_fk( The output pose will be in the local arena frame. Args: - qpos (torch.Tensor | np.ndarray | None): Joint positions of the robot, (n_envs, num_joints). + qpos (torch.Tensor | np.ndarray | None): Joint positions of the robot, (num_envs, num_joints). name (str | None): The name of the control part to compute the FK for. If None, the default part is used. link_names (List[str] | None): The names of the links to compute the FK for. If None, all links are used. end_link_name (str | None): The name of the end link to compute the FK for. If None, the default end link is used. @@ -784,7 +784,7 @@ def compute_fk( to_matrix (bool): If True, returns the transformation in the form of a 4x4 matrix. Returns: - torch.Tensor: The forward kinematics result with shape (n_envs, 7) or (n_envs, 4, 4) if `to_matrix` is True. + torch.Tensor: The forward kinematics result with shape (num_envs, 7) or (num_envs, 4, 4) if `to_matrix` is True. """ local_env_ids = self._all_indices if env_ids is None else env_ids @@ -848,15 +848,15 @@ def compute_ik( The input pose should be in the local arena frame. Args: - pose (torch.Tensor): The end effector pose of the robot, (n_envs, 7) or (n_envs, 4, 4). - joint_seed (torch.Tensor | None): The joint positions to use as a seed for the IK computation, (n_envs, dof). + pose (torch.Tensor): The end effector pose of the robot, (num_envs, 7) or (num_envs, 4, 4). + joint_seed (torch.Tensor | None): The joint positions to use as a seed for the IK computation, (num_envs, dof). If None, the zero joint positions will be used as the seed. name (str | None): The name of the control part to compute the IK for. If None, the default part is used. env_ids (Sequence[int] | None): Environment indices to apply the positions. Defaults to all environments. return_all_solutions (bool): Whether to return all IK solutions or just the best one. Defaults to False. Returns: - Tuple[torch.Tensor, torch.Tensor] | None: The success Tensor with shape (n_envs, ) and qpos Tensor with shape (n_envs, max_results, dof), or None if solver not found. + Tuple[torch.Tensor, torch.Tensor] | None: The success Tensor with shape (num_envs, ) and qpos Tensor with shape (num_envs, max_results, dof), or None if solver not found. """ local_env_ids = self._all_indices if env_ids is None else env_ids @@ -926,13 +926,13 @@ def compute_batch_fk( The output pose will be in the local arena frame. Args: - qpos (torch.Tensor | np.ndarray | None): Joint positions of the robot, (n_envs, n_batch, num_joints). + qpos (torch.Tensor | np.ndarray | None): Joint positions of the robot, (num_envs, n_batch, num_joints). name (str | None): The name of the control part to compute the FK for. If None, the default part is used. env_ids (Sequence[int] | None): The environment ids to compute the FK for. If None, all environments are used. to_matrix (bool): If True, returns the transformation in the form of a 4x4 matrix. Returns: - torch.Tensor: The forward kinematics result with shape (n_envs, batch, 7) or (n_envs, batch, 4, 4) if `to_matrix` is True. + torch.Tensor: The forward kinematics result with shape (num_envs, batch, 7) or (num_envs, batch, 4, 4) if `to_matrix` is True. """ local_env_ids = self._all_indices if env_ids is None else env_ids if not self._solvers: @@ -992,15 +992,15 @@ def compute_batch_ik( The input pose should be in the local arena frame. Args: - pose (torch.Tensor): The end effector pose of the robot, (n_envs, n_batch, 7) or (n_envs, n_batch, 4, 4). - joint_seed (torch.Tensor | None): The joint positions to use as a seed for the IK computation, (n_envs, n_batch, dof). If None, the zero joint positions will be used as the seed. + pose (torch.Tensor): The end effector pose of the robot, (num_envs, n_batch, 7) or (num_envs, n_batch, 4, 4). + joint_seed (torch.Tensor | None): The joint positions to use as a seed for the IK computation, (num_envs, n_batch, dof). If None, the zero joint positions will be used as the seed. name (str | None): The name of the control part to compute the IK for. If None, the default part is used. env_ids (Sequence[int] | None): Environment indices to apply the positions. Defaults to all environments. Returns: Tuple[torch.Tensor, torch.Tensor]: - Success Tensor with shape (n_envs, n_batch) - Qpos Tensor with shape (n_envs, n_batch, dof). + Success Tensor with shape (num_envs, n_batch) + Qpos Tensor with shape (num_envs, n_batch, dof). """ local_env_ids = self._all_indices if env_ids is None else env_ids @@ -1042,7 +1042,7 @@ def compute_batch_ik( ) if pose.shape[-1] == 7 and pose.dim() == 3: - # Convert pose from (n_envs, n_batch, 7) to (n_envs * n_batch, 4, 4) + # Convert pose from (num_envs, n_batch, 7) to (num_envs * n_batch, 4, 4) pose_batch = pose.reshape(-1, 7) pos = pose_batch[:, :3] quat = pose_batch[:, 3:] @@ -1055,7 +1055,7 @@ def compute_batch_ik( pose_batch[:, :3, :3] = rot pose_batch[:, :3, 3] = pos else: - # Convert pose from (n_envs, n_batch, 4, 4) to (n_envs * n_batch, 4, 4) + # Convert pose from (num_envs, n_batch, 4, 4) to (num_envs * n_batch, 4, 4) pose_batch = pose.reshape(-1, 4, 4) # get xpos from link root diff --git a/embodichain/lab/sim/planners/base_planner.py b/embodichain/lab/sim/planners/base_planner.py index c6f0eecae..5dabfc930 100644 --- a/embodichain/lab/sim/planners/base_planner.py +++ b/embodichain/lab/sim/planners/base_planner.py @@ -21,14 +21,21 @@ import functools from abc import ABC, abstractmethod from collections.abc import Mapping -from dataclasses import MISSING +from dataclasses import MISSING, dataclass +from typing import Literal from embodichain.utils import logger from embodichain.utils import configclass from embodichain.lab.sim.sim_manager import SimulationManager from .utils import MoveType, PlanState, PlanResult -__all__ = ["BasePlannerCfg", "PlanOptions", "BasePlanner", "validate_plan_options"] +__all__ = [ + "BasePlannerCfg", + "CollisionWorldInfo", + "PlanOptions", + "BasePlanner", + "validate_plan_options", +] @configclass @@ -45,6 +52,57 @@ class PlanOptions: pass +@dataclass(frozen=True, slots=True) +class CollisionWorldInfo: + """Describe one planner's collision-world integration contract. + + Args: + entity_ids: Every canonical entity ID represented in the planner world. + dynamic_entity_ids: Canonical IDs accepted for per-plan pose updates. + batch_mode: Whether the collision world is shared across environments or + instantiated per environment. ``None`` means the mode is irrelevant + or unspecified. + supports_updates: Whether the planner accepts per-plan dynamic poses via + :meth:`BasePlanner.with_collision_world`. + """ + + entity_ids: tuple[str, ...] = () + dynamic_entity_ids: tuple[str, ...] = () + batch_mode: Literal["shared", "per_env"] | None = None + supports_updates: bool = False + + def __post_init__(self) -> None: + for field_name, entity_ids in ( + ("entity_ids", self.entity_ids), + ("dynamic_entity_ids", self.dynamic_entity_ids), + ): + if not isinstance(entity_ids, tuple) or not all( + isinstance(entity_id, str) + and entity_id + and entity_id == entity_id.strip() + for entity_id in entity_ids + ): + raise TypeError( + f"{field_name} must be a tuple of non-empty strings without " + "outer whitespace." + ) + if len(set(entity_ids)) != len(entity_ids): + raise ValueError(f"{field_name} must contain unique IDs.") + + unknown_dynamic_ids = sorted( + set(self.dynamic_entity_ids).difference(self.entity_ids) + ) + if unknown_dynamic_ids: + raise ValueError( + "dynamic_entity_ids must be a subset of entity_ids; unknown=" + f"{unknown_dynamic_ids}." + ) + if self.batch_mode not in (None, "shared", "per_env"): + raise ValueError("batch_mode must be 'shared', 'per_env', or None.") + if not isinstance(self.supports_updates, bool): + raise TypeError("supports_updates must be a bool.") + + def _infer_batch_size(target_states: list[PlanState]) -> int | None: """Return the leading batch dim B of the first tensor found in target_states, or None if none.""" for s in target_states: @@ -178,6 +236,14 @@ def __init__(self, cfg: BasePlannerCfg): supports_collision_world_updates: bool = False """Whether per-plan dynamic obstacle poses can update the collision world.""" + supports_joint_trajectory_validation: bool = False + """Whether exact joint samples can be checked against bounds/collisions.""" + + @property + def collision_world_info(self) -> CollisionWorldInfo | None: + """Return the planner's collision-world contract, if it has one.""" + return None + def supports_move_type(self, move_type: MoveType) -> bool: """Return whether the planner accepts a movement target type directly. @@ -225,8 +291,8 @@ def with_collision_world( ) -> PlanOptions: """Attach dynamic obstacle poses to backend planning options. - The base planner does not consume a collision world. Backends declaring - :attr:`supports_collision_world_updates` override this method. + The base planner does not consume a collision world. Backends whose + :attr:`collision_world_info` enables updates override this method. Args: options: Backend-specific options to enrich. @@ -237,6 +303,35 @@ def with_collision_world( """ return options + def validate_joint_trajectory( + self, + trajectory: torch.Tensor, + *, + control_part: str, + obstacle_poses: Mapping[str, torch.Tensor] | None = None, + ) -> torch.Tensor: + """Validate exact joint samples without replacing their path. + + Backends that implement this contract must evaluate every supplied + sample against joint bounds, self-collision, and their configured world + collision model. They return a boolean mask with shape ``(B, T)``. + + Args: + trajectory: Simulator-order joint samples with shape ``(B, T, D)``. + control_part: Robot control part whose ordered joints form ``D``. + obstacle_poses: Optional current dynamic-obstacle world poses. + + Returns: + Per-environment, per-sample validity mask. + + Raises: + NotImplementedError: Always for the base planner. + """ + del trajectory, control_part, obstacle_poses + raise NotImplementedError( + f"{type(self).__name__} does not validate exact joint trajectories." + ) + @validate_plan_options @abstractmethod def plan( @@ -264,7 +359,12 @@ def plan( accelerations. Populated by planners that compute dynamics; may be ``None`` for planners that do not. - dt: torch.Tensor ``(B, N)``, per-point time deltas - - duration: torch.Tensor ``(B,)``, total trajectory duration per env + - duration: derived torch.Tensor ``(B,)``, total trajectory + duration per env + + Returning ``positions`` without ``dt`` raises at + :class:`PlanResult` construction. ``duration`` is always derived + from ``dt.sum(dim=1)``. """ logger.log_error("Subclasses must implement plan() method", NotImplementedError) diff --git a/embodichain/lab/sim/planners/curobo/curobo_planner.py b/embodichain/lab/sim/planners/curobo/curobo_planner.py index f9cf5fce5..283146bec 100644 --- a/embodichain/lab/sim/planners/curobo/curobo_planner.py +++ b/embodichain/lab/sim/planners/curobo/curobo_planner.py @@ -51,9 +51,11 @@ from embodichain.lab.sim.planners.base_planner import ( BasePlanner, BasePlannerCfg, + CollisionWorldInfo, PlanOptions, validate_plan_options, ) +from embodichain.lab.sim.planners.curobo.curobo_yaml import _named_rigid_objects from embodichain.lab.sim.planners.utils import MoveType, PlanResult, PlanState if TYPE_CHECKING: @@ -136,6 +138,13 @@ def __deepcopy__(self, memo: dict) -> "_RigidObjectRefList": # noqa: ARG002 return _RigidObjectRefList(self) +class _RigidObjectRefMapping(dict): + """Registry IDs mapped to live objects without deepcopying their handles.""" + + def __deepcopy__(self, memo: dict) -> "_RigidObjectRefMapping": # noqa: ARG002 + return _RigidObjectRefMapping(self) + + @configclass class CuroboWorldCfg: """Static collision-world configuration for the cuRobo backend. @@ -144,16 +153,19 @@ class CuroboWorldCfg: meshes (see :attr:`rigid_objects`); there is no external scene-YAML path. """ - rigid_objects: list[RigidObject] | None = None - """Live :class:`RigidObject` obstacles to bake into the auto-generated world YAML. + rigid_objects: list[RigidObject] | Mapping[str, RigidObject] | None = None + """Live :class:`RigidObject` obstacles to bake into the generated world YAML. The adapter reads each object's mesh (``get_vertices`` / ``get_triangles``) and world pose (``get_local_pose``) and writes a cuRobo V2 scene YAML (cached - on disk by content hash). Poses are written in the cuRobo world/base frame, - so this is exact when the robot base sits at the simulator world origin. For - obstacles that move or live in an offset base frame, also list their names in - :attr:`dynamic_obstacle_names` to update poses at plan time. ``None`` yields an - initially empty collision world. + on disk by content hash). A mapping is the registry-backed path: its keys are + authoritative obstacle IDs even when they differ from ``RigidObject.uid``. + The list form remains available for advanced callers and derives names from + ``uid`` (or ``obstacle_`` when absent). Poses are written in the cuRobo + world/base frame, so this is exact when the robot base sits at the simulator + world origin. For obstacles that move or live in an offset base frame, also + list their canonical names in :attr:`dynamic_obstacle_names` to update poses + at plan time. ``None`` yields an initially empty collision world. """ obstacle_representation: str = "sphere" @@ -178,7 +190,7 @@ class CuroboWorldCfg: """ dynamic_obstacle_names: list[str] = [] - """Obstacle names whose poses may be updated between plans.""" + """Canonical obstacle IDs whose poses may be updated between plans.""" multi_env: bool = False """Whether cuRobo allocates one collision-world instance per environment. @@ -211,10 +223,65 @@ class CuroboWorldCfg: """ def __post_init__(self) -> None: + if isinstance(self.dynamic_obstacle_names, (str, bytes)): + raise TypeError( + "dynamic_obstacle_names must be an iterable of obstacle IDs, " + "not a string." + ) + try: + dynamic_names = list(self.dynamic_obstacle_names) + except TypeError as exc: + raise TypeError( + "dynamic_obstacle_names must be an iterable of obstacle IDs." + ) from exc + if not all( + isinstance(name, str) and name and name == name.strip() + for name in dynamic_names + ): + raise ValueError( + "dynamic_obstacle_names must contain unique non-empty names " + "without outer whitespace." + ) + if len(set(dynamic_names)) != len(dynamic_names): + raise ValueError( + "dynamic_obstacle_names must contain unique non-empty names " + "without outer whitespace." + ) + + if self.rigid_objects is not None and not isinstance( + self.rigid_objects, + (list, Mapping), + ): + raise TypeError("rigid_objects must be a list, mapping, or None.") + named_rigid_objects = _named_rigid_objects(self.rigid_objects) + rigid_names = [name for name, _ in named_rigid_objects] + if not all( + isinstance(name, str) and name and name == name.strip() + for name in rigid_names + ): + raise ValueError( + "CuroboWorldCfg.rigid_objects must have non-empty string obstacle " + "IDs without outer whitespace." + ) + if len(set(rigid_names)) != len(rigid_names): + raise ValueError( + "CuroboWorldCfg.rigid_objects must have unique obstacle names." + ) + missing = set(dynamic_names).difference(rigid_names) + if missing: + raise ValueError( + "dynamic_obstacle_names reference objects not present in " + f"rigid_objects: {sorted(missing)}." + ) + self.dynamic_obstacle_names = dynamic_names + # Wrap live RigidObjects so the @configclass field-deepcopy (run right # after this by custom_post_init) shares references instead of trying to # pickle non-pickleable C++ dexsim handles held by each RigidObject. - if self.rigid_objects is not None and not isinstance( + if isinstance(self.rigid_objects, Mapping): + if not isinstance(self.rigid_objects, _RigidObjectRefMapping): + self.rigid_objects = _RigidObjectRefMapping(self.rigid_objects) + elif self.rigid_objects is not None and not isinstance( self.rigid_objects, _RigidObjectRefList ): self.rigid_objects = _RigidObjectRefList(self.rigid_objects) @@ -417,7 +484,7 @@ class CuroboPlanOptions(PlanOptions): """EmbodiChain control-part name to plan for.""" dynamic_obstacle_poses: dict[str, torch.Tensor] | None = None - """Per-obstacle world poses ``(B, 4, 4)`` keyed by configured name.""" + """World poses ``(B, 4, 4)`` keyed by canonical dynamic-obstacle ID.""" max_attempts: int | None = None """Per-plan override of ``CuroboPlannerCfg.max_attempts``.""" @@ -464,8 +531,8 @@ def _validate_dynamic_obstacles( """Validate dynamic-obstacle pose names and shapes. Args: - poses: Mapping of obstacle name -> pose tensor. ``None`` is a no-op. - allowed_names: Obstacle names declared in :class:`CuroboWorldCfg`. + poses: Mapping of canonical obstacle ID -> pose tensor. ``None`` is a no-op. + allowed_names: Canonical IDs declared in :class:`CuroboWorldCfg`. Raises: ValueError: If a name is not configured, or a pose is not ``(B, 4, 4)``. @@ -649,6 +716,7 @@ def _require_curobo(log_level: str = "error") -> "Any": try: planner_mod = importlib.import_module("curobo.motion_planner") batch_mod = importlib.import_module("curobo.batch_motion_planner") + collision_mod = importlib.import_module("curobo.collision_checking") types_mod = importlib.import_module("curobo.types") except ModuleNotFoundError as exc: raise ImportError( @@ -663,6 +731,8 @@ def _require_curobo(log_level: str = "error") -> "Any": MotionPlanner=planner_mod.MotionPlanner, MotionPlannerCfg=planner_mod.MotionPlannerCfg, BatchMotionPlanner=batch_mod.BatchMotionPlanner, + RobotCollisionChecker=collision_mod.RobotCollisionChecker, + RobotCollisionCheckerCfg=collision_mod.RobotCollisionCheckerCfg, JointState=types_mod.JointState, Pose=types_mod.Pose, GoalToolPose=types_mod.GoalToolPose, @@ -750,6 +820,7 @@ class CuroboPlanner(BasePlanner): supported_move_types = frozenset({MoveType.EEF_MOVE, MoveType.JOINT_MOVE}) supports_collision_world_updates = True + supports_joint_trajectory_validation = True @property def preserve_plan_samples(self) -> bool: @@ -761,6 +832,18 @@ def preserve_plan_samples(self) -> bool: """ return self.cfg.preserve_plan_samples + @property + def collision_world_info(self) -> CollisionWorldInfo: + """Return the configured collision-world integration contract.""" + return CollisionWorldInfo( + entity_ids=tuple( + name for name, _ in _named_rigid_objects(self.cfg.world.rigid_objects) + ), + dynamic_entity_ids=tuple(self.cfg.world.dynamic_obstacle_names), + batch_mode="per_env" if self.cfg.world.multi_env else "shared", + supports_updates=True, + ) + def __init__(self, cfg: CuroboPlannerCfg) -> None: super().__init__(cfg) self.cfg: CuroboPlannerCfg = cfg @@ -888,6 +971,105 @@ def prepare_backend( "use_cuda_graph": backend.use_cuda_graph, } + def validate_joint_trajectory( + self, + trajectory: torch.Tensor, + *, + control_part: str, + obstacle_poses: Mapping[str, torch.Tensor] | None = None, + ) -> torch.Tensor: + """Validate every supplied joint sample with cuRobo collision models. + + The samples are not replanned or replaced. They are mapped from the + simulator's control-part order into the exact cuRobo model, then checked + against joint bounds, self-collision, and the live world collision + checker. Calling the configuration validator once per horizon sample + works around cuRobo 0.8's configuration-only ``validate`` contract while + retaining batched environments. + """ + if ( + not isinstance(trajectory, torch.Tensor) + or not trajectory.is_floating_point() + or trajectory.dim() != 3 + or 0 in trajectory.shape + or not bool(torch.isfinite(trajectory).all().item()) + ): + raise ValueError("trajectory must be finite floating shape (B, T, D).") + batch_size, horizon, dof = trajectory.shape + backend = self._get_backend( + control_part, + batch_size, + MoveType.JOINT_MOVE, + ) + if dof != len(backend.sim_joint_names): + raise ValueError( + f"Trajectory for {control_part!r} has {dof} joints, expected " + f"{len(backend.sim_joint_names)}." + ) + + dynamic_ids = tuple(self.cfg.world.dynamic_obstacle_names) + if dynamic_ids and obstacle_poses is None: + raise ValueError( + "Live dynamic-obstacle poses are required for exact trajectory " + "validation." + ) + poses = None if obstacle_poses is None else dict(obstacle_poses) + if poses is not None: + _validate_dynamic_obstacles(poses, list(dynamic_ids)) + missing = sorted(set(dynamic_ids).difference(poses)) + extra = sorted(set(poses).difference(dynamic_ids)) + if missing or extra: + raise ValueError( + "Dynamic collision obstacle IDs do not match the planner " + f"configuration; missing={missing}, extra={extra}." + ) + if poses: + base_pose_inv = pose_inv(self._get_sim_base_pose(backend, batch_size)) + self.update_dynamic_obstacles( + poses, + backend, + base_pose_inv, + ) + + if backend.collision_checker is None: + checker_cfg = self._bindings.RobotCollisionCheckerCfg.load_from_config( + robot_config=backend.profile.robot_config_path, + scene_collision_checker=(backend.planner.scene_collision_checker), + device_cfg=self._bindings.DeviceCfg(device=self._curobo_device), + num_envs=batch_size, + collision_activation_distance=self.cfg.collision_activation_distance, + ) + backend.collision_checker = self._bindings.RobotCollisionChecker( + checker_cfg + ) + + self._to_curobo_joint_state(trajectory[:, 0], backend) + assert backend.sim_to_curobo_col_idx is not None + curobo_trajectory = trajectory.to( + device=self._curobo_device, + dtype=torch.float32, + ).index_select(-1, backend.sim_to_curobo_col_idx) + env_query_idx = ( + torch.arange(batch_size, device=self._curobo_device, dtype=torch.int32) + if self.cfg.world.multi_env + else None + ) + samples: list[torch.Tensor] = [] + device_context = ( + torch.cuda.device(self._curobo_device) + if self._curobo_device.type == "cuda" + else nullcontext() + ) + with device_context: + for sample_index in range(horizon): + sample = curobo_trajectory[:, sample_index : sample_index + 1] + valid = backend.collision_checker.validate( + sample, + env_query_idx=env_query_idx, + ) + samples.append(valid[:, 0].to(torch.bool)) + return torch.stack(samples, dim=1).to(trajectory.device) + def with_collision_world( self, options: PlanOptions, @@ -1638,8 +1820,7 @@ def _world_yaml_cache_key(self, world_cfg: CuroboWorldCfg) -> str: hasher.update(str(auto.surface_radius).encode("utf-8")) hasher.update(str(auto.iterations).encode("utf-8")) hasher.update(str(auto.collision_sphere_buffer).encode("utf-8")) - for idx, obj in enumerate(world_cfg.rigid_objects or []): - name = getattr(obj, "uid", None) or f"obstacle_{idx}" + for name, obj in _named_rigid_objects(world_cfg.rigid_objects): hasher.update(name.encode("utf-8")) vertices = obj.get_vertices(env_ids=[0], scale=True)[0] faces = obj.get_triangles(env_ids=[0])[0] @@ -2002,12 +2183,10 @@ def _assemble_result( else: positions[b, :1] = start[b] positions[b, 1:] = start[b] - duration = dt.sum(dim=1) return PlanResult( success=alive.to(self.device), positions=positions.to(self.device), dt=dt.to(self.device), - duration=duration.to(self.device), ) # ------------------------------------------------------------------ @@ -2219,8 +2398,8 @@ def update_dynamic_obstacles( """Update named dynamic obstacle poses on cached cuRobo collision worlds. Args: - poses: Mapping of obstacle name -> ``(B, 4, 4)`` world pose. ``None`` - is a no-op. + poses: Mapping of canonical obstacle ID -> ``(B, 4, 4)`` world pose. + ``None`` is a no-op. backend: Specific cached backend to update. If ``None``, updates all cached backends. sim_base_pose_inv: Precomputed inverse of the live sim base pose for @@ -2348,6 +2527,7 @@ class _CuroboBackend: batch_size: int use_cuda_graph: bool planning_mode: MoveType + collision_checker: "Any | None" = None # Lazily-built device-tensor caches for the shared post-processing. The # cuRobo joint order and the profile's fixed transforms are stable for a # planner's life, so these are built once on first use and reused across diff --git a/embodichain/lab/sim/planners/curobo/curobo_yaml.py b/embodichain/lab/sim/planners/curobo/curobo_yaml.py index 1b24eec68..65015695e 100644 --- a/embodichain/lab/sim/planners/curobo/curobo_yaml.py +++ b/embodichain/lab/sim/planners/curobo/curobo_yaml.py @@ -28,7 +28,8 @@ from __future__ import annotations -from typing import TYPE_CHECKING, Sequence +from collections.abc import Mapping, Sequence +from typing import TYPE_CHECKING import torch @@ -41,6 +42,18 @@ __all__ = ["generate_curobo_robot_yaml", "generate_curobo_world_yaml"] +def _named_rigid_objects( + rigid_objects: Sequence[RigidObject] | Mapping[str, RigidObject] | None, +) -> list[tuple[str, RigidObject]]: + """Return canonical obstacle names paired with their live objects.""" + if isinstance(rigid_objects, Mapping): + return list(rigid_objects.items()) + return [ + (getattr(obj, "uid", None) or f"obstacle_{index}", obj) + for index, obj in enumerate(rigid_objects or ()) + ] + + def _parse_mimic_joint_names(urdf_path: str) -> set[str]: """Return the names of URDF joints that mimic another joint. @@ -539,7 +552,7 @@ def _mesh_to_obstacle_entry( def generate_curobo_world_yaml( - rigid_objects: Sequence[RigidObject], + rigid_objects: Sequence[RigidObject] | Mapping[str, RigidObject], output_path: str, *, representation: str = "cuboid", @@ -552,7 +565,7 @@ def generate_curobo_world_yaml( collision_sphere_buffer: float = 0.0, device: str = "cuda:0", ) -> str: - """Generate a cuRobo V2 scene (world) YAML from a sequence of ``RigidObject``. + """Generate a cuRobo V2 scene (world) YAML from live ``RigidObject`` handles. Each object's mesh (``get_vertices`` / ``get_triangles``) and world pose (``get_local_pose``) are converted into cuRobo obstacle entries under a single @@ -568,7 +581,9 @@ def generate_curobo_world_yaml( :meth:`~embodichain.lab.sim.planners.curobo.curobo_planner.CuroboPlanner.update_dynamic_obstacles`. Args: - rigid_objects: ``RigidObject`` instances to bake into the collision world. + rigid_objects: Objects to bake into the collision world. Mapping keys are + authoritative obstacle IDs. A sequence derives each name from the + object's ``uid`` (or ``obstacle_`` when absent). output_path: Destination YAML file path. representation: ``"cuboid"`` (default, AABB->OBB, no CUDA), ``"mesh"`` (exact triangle mesh, no CUDA), or ``"sphere"`` (cuRobo sphere fit, @@ -594,29 +609,42 @@ def generate_curobo_world_yaml( import yaml - rigid_objects = list(rigid_objects) - if not rigid_objects: + registry_backed = isinstance(rigid_objects, Mapping) + named_rigid_objects = _named_rigid_objects(rigid_objects) + if not named_rigid_objects: raise ValueError("rigid_objects must contain at least one RigidObject.") data: dict[str, dict[str, object]] = {} used_names: set[str] = set() - for idx, obj in enumerate(rigid_objects): - name = getattr(obj, "uid", None) or f"obstacle_{idx}" + for name, obj in named_rigid_objects: + if not isinstance(name, str) or not name or name != name.strip(): + raise ValueError( + "Obstacle IDs must be non-empty strings without outer whitespace." + ) if name in used_names: raise ValueError( - f"Duplicate obstacle name {name!r}; RigidObject uids must be unique." + f"Duplicate obstacle name {name!r}; obstacle IDs must be unique." ) used_names.add(name) vertices = obj.get_vertices(env_ids=[env_id], scale=True)[0] faces = obj.get_triangles(env_ids=[env_id])[0] - pose = obj.get_local_pose(to_matrix=False)[env_id] - - if vertices is None or faces is None or vertices.numel() == 0: + if ( + vertices is None + or faces is None + or vertices.numel() == 0 + or faces.numel() == 0 + ): + if registry_backed: + raise ValueError( + f"Registry-backed obstacle {name!r} has no mesh geometry; " + "the declared collision world cannot omit it." + ) logger.log_warning( f"RigidObject {name!r} has no mesh geometry; skipping collision export." ) continue + pose = obj.get_local_pose(to_matrix=False)[env_id] entries = _mesh_to_obstacle_entry( name, diff --git a/embodichain/lab/sim/planners/motion_generator.py b/embodichain/lab/sim/planners/motion_generator.py index e602dd6dc..a15c224b8 100644 --- a/embodichain/lab/sim/planners/motion_generator.py +++ b/embodichain/lab/sim/planners/motion_generator.py @@ -16,6 +16,7 @@ from __future__ import annotations +import math from collections.abc import Mapping from copy import deepcopy from dataclasses import MISSING @@ -27,6 +28,7 @@ from embodichain.lab.sim.planners import ( BasePlannerCfg, + CollisionWorldInfo, PlanOptions, BasePlanner, ToppraPlanner, @@ -105,6 +107,9 @@ class MotionGenOptions: - The pre-interpolation only works for PlanState with MoveType.EEF_MOVE or MoveType.JOINT_MOVE. """ + interpolation_dt: float | None = None + """Explicit waypoint interval for deterministic interpolation.""" + interpolate_nums: int | list[int] = 10 """Number of interpolation points to generate between each pair of waypoints. @@ -113,6 +118,13 @@ class MotionGenOptions: is_linear: bool = False """If True, use cartesian linear interpolation, else joint space""" + preserve_cartesian_samples: bool = False + """Treat Cartesian targets as exact output samples and solve each with IK. + + This constrained mode requires exactly ``sample_count - 1`` target states; + the observed start configuration supplies the first output sample. + """ + interpolate_position_step: float = 0.002 """Step size for interpolation. If is_linear is True, this is the step size in Cartesian space (meters). If is_linear is False, this is the step size in joint space (radians).""" @@ -133,6 +145,15 @@ def __post_init__(self) -> None: raise ValueError("velocity_limit must be greater than zero when set.") if self.acceleration_limit is not None and self.acceleration_limit <= 0.0: raise ValueError("acceleration_limit must be greater than zero when set.") + if self.interpolation_dt is not None: + if isinstance(self.interpolation_dt, bool) or not isinstance( + self.interpolation_dt, (int, float) + ): + raise TypeError("interpolation_dt must be a real number or None.") + if not math.isfinite(self.interpolation_dt) or self.interpolation_dt <= 0.0: + raise ValueError( + "interpolation_dt must be finite and greater than zero when set." + ) class MotionGenerator: @@ -159,6 +180,16 @@ def __init__(self, cfg: MotionGenCfg) -> None: self.robot = self.planner.robot self.device = self.robot.device + @property + def collision_world_info(self) -> CollisionWorldInfo | None: + """Return the selected planner's collision-world contract.""" + info = self.planner.collision_world_info + if info is not None and not isinstance(info, CollisionWorldInfo): + raise TypeError( + "Planner.collision_world_info must be a CollisionWorldInfo or None." + ) + return info + @property def supports_dynamic_collision_world(self) -> bool: """Whether the planner accepts per-plan dynamic obstacle poses. @@ -166,7 +197,56 @@ def supports_dynamic_collision_world(self) -> bool: Returns: ``True`` when the selected planner supports collision-world updates. """ - return getattr(self.planner, "supports_collision_world_updates", False) is True + info = self.collision_world_info + return info is not None and info.supports_updates + + @property + def supports_joint_trajectory_validation(self) -> bool: + """Whether the backend checks exact joint samples for collisions.""" + return ( + getattr( + self.planner, + "supports_joint_trajectory_validation", + False, + ) + is True + ) + + @property + def dynamic_collision_entity_ids(self) -> tuple[str, ...]: + """Return canonical dynamic-obstacle IDs declared by the planner.""" + info = self.collision_world_info + return () if info is None else info.dynamic_entity_ids + + @property + def collision_world_entity_ids(self) -> tuple[str, ...]: + """Return every canonical entity ID in the planner collision world.""" + info = self.collision_world_info + return () if info is None else info.entity_ids + + @staticmethod + def _validate_collision_pose_keys( + poses: Mapping[object, object], + *, + field_name: str, + ) -> set[str]: + """Validate exact canonical IDs on one obstacle-pose mapping.""" + entity_ids = tuple(poses) + if not all( + isinstance(entity_id, str) and entity_id and entity_id == entity_id.strip() + for entity_id in entity_ids + ): + raise TypeError( + f"{field_name} keys must be non-empty strings without outer " + "whitespace." + ) + return set(entity_ids) + + @property + def collision_world_batch_mode(self) -> Literal["shared", "per_env"] | None: + """Return the backend's dynamic collision-world batch-sharing mode.""" + info = self.collision_world_info + return None if info is None else info.batch_mode def bind_collision_world( self, @@ -186,21 +266,126 @@ def bind_collision_world( Raises: ValueError: If the selected planner cannot consume dynamic obstacles. """ - if not self.supports_dynamic_collision_world: + info = self.collision_world_info + if info is None or not info.supports_updates: logger.log_error( f"{type(self.planner).__name__} does not support dynamic " "collision-world updates.", ValueError, ) + assert info is not None + configured_ids = info.dynamic_entity_ids + received_ids = tuple(obstacle_poses) + if not all( + isinstance(entity_id, str) and entity_id and entity_id == entity_id.strip() + for entity_id in received_ids + ): + raise TypeError( + "obstacle_poses keys must be non-empty strings without outer " + "whitespace." + ) + missing = sorted(set(configured_ids).difference(received_ids)) + extra = sorted(set(received_ids).difference(configured_ids)) + if missing or extra: + logger.log_error( + "Dynamic collision obstacle IDs do not match the planner " + f"configuration; missing={missing}, extra={extra}.", + ValueError, + ) options = ( deepcopy(plan_opts) if plan_opts is not None else self.planner.default_plan_options() ) - return self.planner.with_collision_world( + existing_poses = getattr(options, "dynamic_obstacle_poses", None) + if existing_poses is not None: + if not isinstance(existing_poses, Mapping): + raise TypeError( + "plan_opts.dynamic_obstacle_poses must be a mapping or None." + ) + existing_ids = self._validate_collision_pose_keys( + existing_poses, + field_name="plan_opts.dynamic_obstacle_poses", + ) + existing_extra = sorted(existing_ids.difference(configured_ids)) + if existing_extra: + raise ValueError( + "Caller planning options contain dynamic collision IDs that " + f"are not configured by the planner: {existing_extra}." + ) + bound = self.planner.with_collision_world( options, obstacle_poses=obstacle_poses, ) + if hasattr(bound, "dynamic_obstacle_poses"): + bound_poses = bound.dynamic_obstacle_poses + if bound_poses is None: + bound_ids: set[str] = set() + elif not isinstance(bound_poses, Mapping): + raise TypeError("Bound dynamic_obstacle_poses must be a mapping.") + else: + bound_ids = self._validate_collision_pose_keys( + bound_poses, + field_name="Bound dynamic_obstacle_poses", + ) + bound_missing = sorted(set(configured_ids).difference(bound_ids)) + bound_extra = sorted(bound_ids.difference(configured_ids)) + if bound_missing or bound_extra: + raise ValueError( + "Bound dynamic collision obstacle IDs do not match the planner " + f"configuration; missing={bound_missing}, extra={bound_extra}." + ) + return bound + + def validate_joint_trajectory( + self, + trajectory: torch.Tensor, + *, + control_part: str, + obstacle_poses: Mapping[str, torch.Tensor] | None = None, + ) -> torch.Tensor: + """Check exact joint samples through the selected planner backend. + + Args: + trajectory: Simulator-order joint samples with shape ``(B, T, D)``. + control_part: Robot control part whose ordered joints form ``D``. + obstacle_poses: Optional live dynamic-obstacle poses. + + Returns: + Boolean validity mask with shape ``(B, T)`` on the trajectory device. + """ + if not self.supports_joint_trajectory_validation: + raise ValueError( + f"Planner {type(self.planner).__name__} does not support exact " + "joint-trajectory collision validation." + ) + if not isinstance(trajectory, torch.Tensor): + raise TypeError("trajectory must be a torch.Tensor.") + if ( + not trajectory.is_floating_point() + or trajectory.dim() != 3 + or 0 in trajectory.shape + or not bool(torch.isfinite(trajectory).all().item()) + ): + raise ValueError( + "trajectory must be finite floating shape (B, T, D) with " + "non-zero dimensions." + ) + if type(control_part) is not str or not control_part: + raise ValueError("control_part must be a non-empty string.") + validity = self.planner.validate_joint_trajectory( + trajectory, + control_part=control_part, + obstacle_poses=obstacle_poses, + ) + if not isinstance(validity, torch.Tensor): + raise TypeError("Planner.validate_joint_trajectory() must return a tensor.") + if validity.dtype != torch.bool or validity.shape != trajectory.shape[:2]: + raise ValueError( + "Planner.validate_joint_trajectory() must return bool shape " + f"{tuple(trajectory.shape[:2])}." + ) + return validity.to(trajectory.device).clone() def resolve_plan_options( self, @@ -307,9 +492,13 @@ def generate( names = sorted(move_type.name for move_type in move_types) raise ValueError(f"All target states must share move_type; got {names}.") move_type = target_states[0].move_type - use_interpolation = options.strategy == "ik_interp" or ( - move_type is MoveType.JOINT_MOVE - and not self.planner.supports_move_type(MoveType.JOINT_MOVE) + use_interpolation = ( + options.preserve_cartesian_samples + or options.strategy == "ik_interp" + or ( + move_type is MoveType.JOINT_MOVE + and not self.planner.supports_move_type(MoveType.JOINT_MOVE) + ) ) if use_interpolation: raw_result = self._generate_ik_interpolation(target_states, options) @@ -419,6 +608,8 @@ def _generate_ik_interpolation( raise ValueError("IK interpolation requires start_qpos.") if options.sample_count is None: raise ValueError("IK interpolation requires sample_count.") + if options.interpolation_dt is None: + raise ValueError("IK interpolation requires explicit interpolation_dt.") start_qpos = options.start_qpos if start_qpos.dim() == 1: start_qpos = start_qpos.unsqueeze(0) @@ -451,9 +642,16 @@ def _generate_ik_interpolation( interp_num=options.sample_count, device=device, ) + dt = self._uniform_dt( + batch_size=batch_size, + waypoint_count=positions.shape[1], + step_dt=options.interpolation_dt, + device=device, + ) return PlanResult( success=torch.ones(batch_size, dtype=torch.bool, device=device), positions=positions, + dt=dt, ) if move_type is not MoveType.EEF_MOVE: @@ -486,7 +684,7 @@ def _generate_ik_interpolation( ) step_success = normalize_success_mask( step_success, - n_envs=batch_size, + num_envs=batch_size, device=device, name=f"IK success for target state {index}", ) @@ -514,14 +712,52 @@ def _generate_ik_interpolation( [start_qpos.unsqueeze(1), torch.stack(solved_waypoints, dim=1)], dim=1, ) - positions = interpolate_with_distance( - trajectory=keyframes, - interp_num=options.sample_count, - device=device, - ) + if options.preserve_cartesian_samples: + if keyframes.shape[1] != options.sample_count: + raise ValueError( + "Linear Cartesian targets must provide sample_count - 1 " + "keyframes so every output sample is IK-grounded; got " + f"{len(target_states)} targets for sample_count " + f"{options.sample_count}." + ) + positions = keyframes + else: + positions = interpolate_with_distance( + trajectory=keyframes, + interp_num=options.sample_count, + device=device, + ) held = start_qpos.unsqueeze(1).expand_as(positions) positions = torch.where(success[:, None, None], positions, held) - return PlanResult(success=success, positions=positions) + dt = self._uniform_dt( + batch_size=batch_size, + waypoint_count=positions.shape[1], + step_dt=options.interpolation_dt, + device=device, + ) + return PlanResult( + success=success, + positions=positions, + dt=dt, + ) + + @staticmethod + def _uniform_dt( + *, + batch_size: int, + waypoint_count: int, + step_dt: float, + device: torch.device, + ) -> torch.Tensor: + """Return explicit uniform arrival intervals for interpolation.""" + dt = torch.zeros( + (batch_size, waypoint_count), + dtype=torch.float32, + device=device, + ) + if waypoint_count > 1: + dt[:, 1:] = step_dt + return dt def _normalize_plan_result( self, @@ -550,7 +786,7 @@ def _normalize_plan_result( success = normalize_success_mask( result.success, - n_envs=batch_size, + num_envs=batch_size, device=device, name="MotionGenerator PlanResult.success", ) @@ -577,6 +813,21 @@ def _normalize_plan_result( if not torch.isfinite(positions).all(): raise ValueError("MotionGenerator returned non-finite positions.") + dt = result.dt + if not isinstance(dt, torch.Tensor): + raise ValueError( + "MotionGenerator planner results with positions require explicit dt." + ) + if dt.shape != positions.shape[:2]: + raise ValueError( + "MotionGenerator dt must match positions batch and sample " + f"dimensions, got {tuple(dt.shape)} and " + f"{tuple(positions.shape[:2])}." + ) + if dt.device != device or not torch.isfinite(dt).all() or (dt < 0).any(): + raise ValueError("MotionGenerator returned invalid time deltas.") + raw_duration = dt.sum(dim=1) + resampled = False preserve_samples = getattr(self.planner, "preserve_plan_samples", False) is True if ( @@ -590,6 +841,13 @@ def _normalize_plan_result( device=device, ) resampled = True + dt = torch.zeros( + positions.shape[:2], + dtype=result.dt.dtype, + device=device, + ) + if positions.shape[1] > 1: + dt[:, 1:] = raw_duration[:, None] / (positions.shape[1] - 1) def normalize_derivative( value: torch.Tensor | None, @@ -610,22 +868,6 @@ def normalize_derivative( velocities = normalize_derivative(result.velocities, "velocities") accelerations = normalize_derivative(result.accelerations, "accelerations") - dt = None if resampled else result.dt - if dt is not None: - if not isinstance(dt, torch.Tensor): - raise TypeError("MotionGenerator dt must be a torch.Tensor.") - if dt.shape != positions.shape[:2]: - raise ValueError( - "MotionGenerator dt must match positions batch and sample " - f"dimensions, got {tuple(dt.shape)} and " - f"{tuple(positions.shape[:2])}." - ) - if dt.device != device or not torch.isfinite(dt).all() or (dt < 0).any(): - raise ValueError("MotionGenerator returned invalid time deltas.") - duration: float | torch.Tensor = dt.sum(dim=1) - else: - duration = result.duration - if start_qpos is not None and not success.all(): held = ( start_qpos.to(dtype=positions.dtype).unsqueeze(1).expand_as(positions) @@ -651,7 +893,6 @@ def normalize_derivative( velocities=velocities, accelerations=accelerations, dt=dt, - duration=duration, ) def _runtime_device(self) -> torch.device: diff --git a/embodichain/lab/sim/planners/neural_planner.py b/embodichain/lab/sim/planners/neural_planner.py index ed1083f3b..f5c3d8c90 100644 --- a/embodichain/lab/sim/planners/neural_planner.py +++ b/embodichain/lab/sim/planners/neural_planner.py @@ -468,7 +468,8 @@ def plan( dtype=torch.float32, device=self.device, ) - dt = dt.unsqueeze(0).expand(b, -1) + dt = dt.unsqueeze(0).expand(b, -1).clone() + dt[:, 0] = 0.0 positions_t = positions_t.permute(1, 0, 2) xpos_t = xpos_t.permute(1, 0, 2, 3) velocities_t, accelerations_t = self._compute_vel_acc_via_finite_diff( @@ -482,11 +483,6 @@ def plan( accelerations=accelerations_t, xpos_list=xpos_t, dt=dt, - duration=torch.full( - (b,), - float(max(positions_t.shape[1] - 1, 0) * self.cfg.dt), - device=self.device, - ), ) def _parse_waypoints( diff --git a/embodichain/lab/sim/planners/toppra_planner.py b/embodichain/lab/sim/planners/toppra_planner.py index 3cc866738..5d612348c 100644 --- a/embodichain/lab/sim/planners/toppra_planner.py +++ b/embodichain/lab/sim/planners/toppra_planner.py @@ -74,7 +74,7 @@ def _toppra_solve_one_env( Returns: dict with ``positions`` ``(N_b, DOF)``, ``velocities``, ``accelerations``, - ``dt`` ``(N_b,)``, ``success`` bool, ``n`` int, ``duration`` float. + ``dt`` ``(N_b,)``, ``success`` bool, and ``n`` int. """ dofs = waypoints.shape[1] vlims, alims = _build_constraint_arrays(vel_constraint, acc_constraint, dofs) @@ -107,7 +107,6 @@ def _toppra_solve_one_env( "dt": np.array([0.0, 0.0], dtype=np.float32), "success": True, "n": 2, - "duration": 0.0, } ss = np.linspace(0.0, 1.0, len(waypoints)) @@ -149,7 +148,6 @@ def _toppra_solve_one_env( "dt": dt, "success": True, "n": len(ts), - "duration": duration, } @@ -162,7 +160,6 @@ def _empty_failure(dofs: int) -> dict: "dt": np.array([0.0, 0.0], dtype=np.float32), "success": False, "n": 2, - "duration": 0.0, } @@ -518,7 +515,6 @@ def _assemble_batched_result(self, results: list[dict], dofs: int) -> PlanResult velocities = np.zeros((b, max_n, dofs), dtype=np.float32) accelerations = np.zeros((b, max_n, dofs), dtype=np.float32) dt = np.zeros((b, max_n), dtype=np.float32) - duration = np.zeros((b,), dtype=np.float32) success = np.zeros((b,), dtype=bool) for i, r in enumerate(results): n = r["n"] @@ -526,7 +522,6 @@ def _assemble_batched_result(self, results: list[dict], dofs: int) -> PlanResult velocities[i, :n] = r["velocities"] accelerations[i, :n] = r["accelerations"] dt[i, :n] = r["dt"] - duration[i] = r["duration"] success[i] = r["success"] # tail-pad: repeat final waypoint for held-pose rows if n < max_n: @@ -539,5 +534,4 @@ def _assemble_batched_result(self, results: list[dict], dofs: int) -> PlanResult velocities=torch.as_tensor(velocities, device=self.device), accelerations=torch.as_tensor(accelerations, device=self.device), dt=torch.as_tensor(dt, device=self.device), - duration=torch.as_tensor(duration, device=self.device), ) diff --git a/embodichain/lab/sim/planners/utils.py b/embodichain/lab/sim/planners/utils.py index 1913449eb..63732bf76 100644 --- a/embodichain/lab/sim/planners/utils.py +++ b/embodichain/lab/sim/planners/utils.py @@ -41,15 +41,15 @@ def normalize_success_mask( success: bool | torch.Tensor, *, - n_envs: int, + num_envs: int, device: torch.device | str, name: str, ) -> torch.Tensor: - """Normalize a scalar or batched success value to ``(n_envs,)``. + """Normalize a scalar or batched success value to ``(num_envs,)``. Args: success: Scalar success or a boolean/binary-integer tensor. - n_envs: Required batch size. + num_envs: Required batch size. device: Device of the resulting tensor. name: Human-readable value name used in validation errors. @@ -58,13 +58,22 @@ def normalize_success_mask( Raises: TypeError: If ``success`` is neither boolean nor binary integer data. - ValueError: If a tensor does not match the required batch shape. + ValueError: If a tensor does not match the required batch shape or a + CUDA device is requested while CUDA is unavailable. """ resolved_device = torch.device(device) - if resolved_device.type == "cuda" and resolved_device.index is None: - resolved_device = torch.device(f"cuda:{torch.cuda.current_device()}") + if resolved_device.type == "cuda": + if not torch.cuda.is_available(): + raise ValueError( + "CUDA device requested for success-mask normalization, but " + "torch.cuda.is_available() is False." + ) + if resolved_device.index is None: + resolved_device = torch.device(f"cuda:{torch.cuda.current_device()}") if isinstance(success, bool): - return torch.full((n_envs,), success, dtype=torch.bool, device=resolved_device) + return torch.full( + (num_envs,), success, dtype=torch.bool, device=resolved_device + ) if not isinstance(success, torch.Tensor): raise TypeError( f"{name} must be a bool or torch.Tensor, got {type(success).__name__}." @@ -87,10 +96,10 @@ def normalize_success_mask( ) success = success.to(dtype=torch.bool) if success.dim() == 0 or success.shape == (1,): - success = success.reshape(1).expand(n_envs) - if success.shape != (n_envs,): + success = success.reshape(1).expand(num_envs) + if success.shape != (num_envs,): raise ValueError( - f"{name} must have shape ({n_envs},), got {tuple(success.shape)}." + f"{name} must have shape ({num_envs},), got {tuple(success.shape)}." ) return success.clone() @@ -175,7 +184,12 @@ class MoveType(Enum): @dataclass class PlanResult: - r"""Data class representing the result of a motion plan (env-batched).""" + r"""Data class representing the result of a motion plan (env-batched). + + A result that contains joint positions must also contain per-sample ``dt``. + Per-environment :attr:`duration` is derived from those intervals. Failed + plans may omit all trajectory fields by leaving ``positions`` as ``None``. + """ success: bool | torch.Tensor = False """Per-env success, shape ``(B,)`` bool tensor (or scalar bool).""" @@ -195,8 +209,32 @@ class PlanResult: dt: torch.Tensor | None = None """Per-env time deltas, shape ``(B, N)``.""" - duration: float | torch.Tensor = 0.0 - """Per-env total duration, shape ``(B,)``.""" + def __post_init__(self) -> None: + """Validate the explicit trajectory-timing contract.""" + if self.positions is None: + if self.dt is not None: + raise ValueError("PlanResult timing requires positions.") + return + if not isinstance(self.positions, torch.Tensor) or self.positions.dim() != 3: + raise ValueError("PlanResult.positions must have shape (B, N, DOF).") + batch_size, waypoint_count, _ = self.positions.shape + if not isinstance(self.dt, torch.Tensor): + raise ValueError( + "PlanResult with positions requires explicit dt with shape (B, N)." + ) + if self.dt.shape != (batch_size, waypoint_count): + raise ValueError( + "PlanResult.dt must match positions batch and waypoint dimensions." + ) + if self.dt.device != self.positions.device: + raise ValueError("PlanResult.dt and positions must share a device.") + if not torch.isfinite(self.dt).all() or (self.dt < 0).any(): + raise ValueError("PlanResult.dt must contain finite non-negative values.") + + @property + def duration(self) -> torch.Tensor | None: + """Return per-environment duration derived from :attr:`dt`.""" + return None if self.dt is None else self.dt.sum(dim=1) def is_all_success(self) -> bool: """Return True only when every env succeeded.""" diff --git a/embodichain/lab/sim/sim_manager.py b/embodichain/lab/sim/sim_manager.py index 6b71f2adf..5f4fc6977 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -213,6 +213,16 @@ class _WindowRecordState: loop_handle: object | None = None +@dataclass(frozen=True) +class _AxisMarkerGroup: + """Native axis handles and their backend-neutral display dimensions.""" + + handles: tuple[MeshObject, ...] + arena_index: int + axis_length: float + axis_radius: float + + class SimulationManager: r"""Global Embodied AI simulation manager. @@ -245,6 +255,7 @@ def __new__(cls, sim_config: SimulationManagerCfg = SimulationManagerCfg()): instance = super(SimulationManager, cls).__new__(cls) # Store sim_config in the instance for use in __init__ or elsewhere instance.sim_config = sim_config + instance._is_constructed = False cls._instances[n_instance] = instance return instance @@ -326,7 +337,7 @@ def __init__( self._gizmos: Dict[str, object] = dict() # Store active gizmos # marker management - self._markers: Dict[str, MeshObject] = dict() + self._markers: dict[str, _AxisMarkerGroup] = {} self._rigid_objects: Dict[str, RigidObject] = dict() self._constraints: Dict[str, RigidConstraint] = dict() @@ -368,6 +379,8 @@ def __init__( if sim_config.headless is False: self._window = self._world.get_windows() + self._is_constructed = True + @classmethod def get_instance(cls, instance_id: int = 0) -> SimulationManager: """Get the instance of SimulationManager by id. @@ -2369,7 +2382,12 @@ def draw_marker( # # Create point markers # pass - self._markers[name] = (marker_handles, cfg.arena_index) + self._markers[name] = _AxisMarkerGroup( + handles=tuple(marker_handles), + arena_index=cfg.arena_index, + axis_length=cfg.axis_len, + axis_radius=cfg.axis_size, + ) if self.is_physics_manually_update: self.update(step=1) @@ -2388,9 +2406,9 @@ def remove_marker(self, name: str) -> bool: logger.log_warning(f"Marker {name} not found.") return False try: - env = self.get_env(self._markers[name][1]) - marker_handles, arena_index = self._markers[name] - for marker_handle in marker_handles: + marker_group = self._markers[name] + env = self.get_env(marker_group.arena_index) + for marker_handle in marker_group.handles: if marker_handle is not None: env.remove_actor(marker_handle.get_name()) self._markers.pop(name) @@ -2399,6 +2417,25 @@ def remove_marker(self, name: str) -> bool: logger.log_warning(f"Failed to remove marker {name}: {str(e)}") return False + def get_axis_marker_items( + self, + ) -> tuple[tuple[str, tuple[MeshObject, ...], float, float], ...]: + """Return active axes for backend-neutral visualization. + + Returns: + Tuples containing the marker name, native handles, axis length, and + axis radius for each active marker group. + """ + return tuple( + ( + name, + group.handles, + group.axis_length, + group.axis_radius, + ) + for name, group in self._markers.items() + ) + def add_custom_window_control(self, controls: list[ObjectManipulator]) -> None: """Add one or more custom window input controls. @@ -3083,6 +3120,12 @@ def _deferred_destroy(self) -> None: self.stop_window_record() self.wait_window_record_saves() + # Stop the render loop before releasing scene resources. Vulkan window + # presentation may otherwise continue acquiring swapchain images while + # Env::Clean tears down render objects used by the in-flight frame. + if getattr(self, "is_window_opened", False): + self.close_window() + import sys, gc self.clean_materials() diff --git a/embodichain/lab/sim/skills/__init__.py b/embodichain/lab/sim/skills/__init__.py new file mode 100644 index 000000000..2760ce102 --- /dev/null +++ b/embodichain/lab/sim/skills/__init__.py @@ -0,0 +1,413 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Semantic-skill integration contracts built on the atomic-action core.""" + +from __future__ import annotations + +from .calls import ( + DeclarativeValue, + HandOver, + OperateArticulation, + Pick, + Place, + PlaceRelationTarget, + RegisteredSemanticCall, + SemanticCallCatalog, + SemanticCallDescriptor, + SemanticCallSpec, + SemanticPose, + builtin_semantic_call_catalog, +) +from .compiler import ( + AnalyzedSemanticCall, + ContainerRelationTargetGrounder, + GroundedHeldObjectGuard, + GroundedPhaseEffectGate, + GroundedSemanticCall, + HandOverPoseProvider, + HandOverPoseTargets, + HeldObjectGuardBaseline, + RegisteredSemanticLowerer, + RelationTargetGrounder, + SemanticEffectDependency, + SemanticHandOverTarget, + SemanticLowering, + SemanticObjectTarget, + SemanticRelationTarget, + SemanticSkillCompiler, + SemanticWorkflow, + SupportSurfaceRelationTargetGrounder, +) +from .effects import ( + ArticulationJointStateExpectation, + BinaryEffectClause, + BinaryEffectEvidenceBatch, + BinaryEvidenceKind, + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + CompositeEffectMonitor, + CompositeEffectMonitorCfg, + CompositeEffectMonitorFactory, + ControlPartEvidenceAddress, + CoordinatedHeldObjectCleanupExpectation, + EffectClause, + EffectEvidenceAddress, + EffectEvidenceBatch, + EffectEvidenceSourceRef, + EffectExpectationDecision, + EffectMonitor, + EffectMonitorDecision, + EffectMonitorFactory, + EffectMonitorParam, + EffectMonitorRef, + EffectMonitorRegistry, + EffectStateExpectation, + FORCE_EFFECT_CHANNEL, + HeldObjectRelation, + HeldObjectStateExpectation, + JOINT_STATE_EFFECT_CHANNEL, + JointStateEffectClause, + JointStateEvidenceBatch, + POSE_RELATION_EFFECT_CHANNEL, + PoseRelationClause, + PoseRelationEvidenceBatch, + PoseRelationExpectation, + ScalarEffectClause, + ScalarEffectEvidenceBatch, + ScalarEvidenceKind, + ScalarExpectation, + SemanticEffectKind, + SemanticEffectSpec, + SymbolicStateDomain, + SymbolicStateKey, +) +from .evidence import ( + ArticulationJointObservationCallback, + BinaryEffectEvidenceQuery, + BinaryEffectObservation, + BinaryObservationCallback, + ControlPartRobotEvidenceSource, + ControlPartSimulationEvidenceProvider, + EffectEvidenceCollectionContext, + EffectEvidenceCollector, + EffectEvidenceProvider, + EffectEvidenceProviderRegistry, + EffectEvidenceQuery, + EffectEvidenceQueryValue, + JointStateEvidenceQuery, + JointStateObservation, + PoseRelationEvidenceQuery, + ScalarEffectEvidenceQuery, + ScalarEffectObservation, + ScalarObservationCallback, + SceneArticulationEvidenceProvider, + build_effect_evidence_queries, +) +from .integration import ( + BoundSemanticCall, + BoundSemanticIntegration, + LinkedSemanticCall, + PathPart, + SceneEntityManifest, + SceneManifest, + SemanticDiagnostic, + SemanticIntegrationManifest, + SemanticValidationError, +) +from .parallel import ( + ParallelBarrierUpdate, + ParallelBranchPlan, + ParallelConflictError, + ParallelStateConflictError, + ParallelTimingError, + ParallelTimingPolicy, + align_parallel_commands, + merge_parallel_effects, + resolve_parallel_barrier, + validate_parallel_claims, +) +from .parallel_runtime import ( + ParallelBranchStaticAnalysis, + ParallelBranchRuntime, + ParallelCommandSafetyValidator, + ParallelLaneCommandSink, + ParallelRuntimeBranch, + ParallelSafetyError, + ParallelSkillResult, + ParallelSkillRuntime, + analyze_parallel_branches, +) +from .profiles import ( + AmbiguousSkillBindingError, + BoundRobotSkillProfile, + ControlPartEndpoint, + ControlPartEndpointAdapter, + EndpointResolution, + ProfileValidationError, + ResolvedResourceEndpoint, + ResolvedRobotResource, + ResolvedSkillBinding, + ResourceBinding, + ResourceClaim, + ResourceEndpoint, + ResourceEndpointAdapter, + RobotResource, + RobotSkillProfile, + SkillPolicyPreset, + UnsupportedSkillError, + WorkflowRecoveryPolicy, +) +from .scene import ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + ContainerAffordance, + PLACEMENT_TARGET_AFFORDANCE_REVISION, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, + AmbiguousSceneAffordanceError, + ArticulationJointEvidenceAddress, + GRASP_AFFORDANCE_CAPABILITY, + PLACE_IN_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, + RegistrySceneProvider, + SceneAffordanceRef, + SceneArticulationJointStateProvider, + SceneArticulationRef, + SceneCollisionRole, + SceneCollisionWorldMode, + SceneDynamics, + SceneEntityRef, + SceneEntityMetadata, + SceneEntityRegistration, + SceneEntityStateProvider, + SceneGeometryProvider, + SceneLinkRef, + SceneObjectRef, + SceneRegistry, + SupportSurfaceAffordance, + UnsupportedSceneAffordanceError, +) +from .runtime import ( + AtomicSkills, + EffectEvidenceCollectorPort, + ResolvedCorePolicyTrace, + SkillCallTrace, + SkillEndpointBindingTrace, + SkillEndpointTrackingChannelTrace, + SkillEffectTrace, + SkillFailure, + SkillPlanAttemptTrace, + SkillResult, + SkillRuntime, + SkillRuntimeProvider, + SkillScene, + SkillStatus, + SkillWorkflowRecoveryRole, + SkillWorkflowRecoveryTrace, + task_state_to_metadata, +) + +__all__ = [ + "ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY", + "AmbiguousSceneAffordanceError", + "AmbiguousSkillBindingError", + "AnalyzedSemanticCall", + "ArticulationJointEvidenceAddress", + "ArticulationJointObservationCallback", + "ArticulationJointStateExpectation", + "AtomicSkills", + "BinaryEffectClause", + "BinaryEffectEvidenceBatch", + "BinaryEffectEvidenceQuery", + "BinaryEffectObservation", + "BinaryEvidenceKind", + "BinaryObservationCallback", + "BoundSemanticCall", + "BoundSemanticIntegration", + "BoundRobotSkillProfile", + "ControlPartEndpoint", + "ControlPartEndpointAdapter", + "ControlPartEvidenceAddress", + "ControlPartRobotEvidenceSource", + "ControlPartSimulationEvidenceProvider", + "ContainerAffordance", + "ContainerRelationTargetGrounder", + "CoordinatedHeldObjectCleanupExpectation", + "COMPOSITE_EFFECT_MONITOR_ID", + "COMPOSITE_EFFECT_MONITOR_REVISION", + "CONTACT_EFFECT_CHANNEL", + "CONSTRAINT_EFFECT_CHANNEL", + "CONTROL_PART_EVIDENCE_PROVIDER_ID", + "CONTROL_PART_EVIDENCE_PROVIDER_REVISION", + "CompositeEffectMonitor", + "CompositeEffectMonitorCfg", + "CompositeEffectMonitorFactory", + "DeclarativeValue", + "EndpointResolution", + "EffectClause", + "EffectEvidenceAddress", + "EffectEvidenceBatch", + "EffectEvidenceCollectionContext", + "EffectEvidenceCollector", + "EffectEvidenceCollectorPort", + "EffectEvidenceProvider", + "EffectEvidenceProviderRegistry", + "EffectEvidenceQuery", + "EffectEvidenceQueryValue", + "EffectEvidenceSourceRef", + "EffectExpectationDecision", + "EffectMonitor", + "EffectMonitorDecision", + "EffectMonitorFactory", + "EffectMonitorParam", + "EffectMonitorRef", + "EffectMonitorRegistry", + "EffectStateExpectation", + "FORCE_EFFECT_CHANNEL", + "GRASP_AFFORDANCE_CAPABILITY", + "GroundedHeldObjectGuard", + "GroundedPhaseEffectGate", + "GroundedSemanticCall", + "HeldObjectRelation", + "HeldObjectStateExpectation", + "HandOver", + "HandOverPoseProvider", + "HandOverPoseTargets", + "HeldObjectGuardBaseline", + "LinkedSemanticCall", + "JOINT_STATE_EFFECT_CHANNEL", + "JointStateEffectClause", + "JointStateEvidenceBatch", + "JointStateEvidenceQuery", + "JointStateObservation", + "POSE_RELATION_EFFECT_CHANNEL", + "PLACE_IN_AFFORDANCE_CAPABILITY", + "PLACE_ON_AFFORDANCE_CAPABILITY", + "PLACEMENT_TARGET_AFFORDANCE_REVISION", + "PathPart", + "OperateArticulation", + "ParallelBarrierUpdate", + "ParallelBranchStaticAnalysis", + "ParallelBranchRuntime", + "ParallelCommandSafetyValidator", + "ParallelBranchPlan", + "ParallelConflictError", + "ParallelStateConflictError", + "ParallelTimingError", + "ParallelTimingPolicy", + "ParallelLaneCommandSink", + "ParallelRuntimeBranch", + "ParallelSafetyError", + "ParallelSkillResult", + "ParallelSkillRuntime", + "analyze_parallel_branches", + "Pick", + "Place", + "PlaceRelationTarget", + "PoseRelationClause", + "PoseRelationEvidenceBatch", + "PoseRelationEvidenceQuery", + "PoseRelationExpectation", + "ProfileValidationError", + "RegistrySceneProvider", + "ResolvedRobotResource", + "ResolvedResourceEndpoint", + "ResolvedSkillBinding", + "ResourceBinding", + "ResourceClaim", + "ResourceEndpoint", + "ResourceEndpointAdapter", + "ResolvedCorePolicyTrace", + "RegisteredSemanticCall", + "RegisteredSemanticLowerer", + "RelationTargetGrounder", + "RobotResource", + "RobotSkillProfile", + "SceneAffordanceRef", + "SceneArticulationJointStateProvider", + "SceneArticulationEvidenceProvider", + "SceneArticulationRef", + "SceneCollisionRole", + "SceneCollisionWorldMode", + "SceneDynamics", + "SceneEntityRef", + "SceneEntityMetadata", + "SceneEntityRegistration", + "SceneEntityManifest", + "SceneEntityStateProvider", + "SceneGeometryProvider", + "SceneLinkRef", + "SceneObjectRef", + "SceneRegistry", + "SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID", + "SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION", + "SceneManifest", + "ScalarEffectClause", + "ScalarEffectEvidenceBatch", + "ScalarEffectEvidenceQuery", + "ScalarEffectObservation", + "ScalarEvidenceKind", + "ScalarExpectation", + "ScalarObservationCallback", + "SemanticCallCatalog", + "SemanticCallDescriptor", + "SemanticCallSpec", + "SemanticDiagnostic", + "SemanticEffectDependency", + "SemanticEffectKind", + "SemanticEffectSpec", + "SymbolicStateDomain", + "SymbolicStateKey", + "SupportSurfaceAffordance", + "SupportSurfaceRelationTargetGrounder", + "SemanticHandOverTarget", + "SemanticIntegrationManifest", + "SemanticLowering", + "SemanticObjectTarget", + "SemanticPose", + "SemanticRelationTarget", + "SemanticSkillCompiler", + "SemanticValidationError", + "SemanticWorkflow", + "SkillPolicyPreset", + "SkillCallTrace", + "SkillEndpointBindingTrace", + "SkillEndpointTrackingChannelTrace", + "SkillEffectTrace", + "SkillFailure", + "SkillPlanAttemptTrace", + "SkillResult", + "SkillRuntime", + "SkillRuntimeProvider", + "SkillScene", + "SkillStatus", + "SkillWorkflowRecoveryRole", + "SkillWorkflowRecoveryTrace", + "task_state_to_metadata", + "UnsupportedSkillError", + "WorkflowRecoveryPolicy", + "UnsupportedSceneAffordanceError", + "build_effect_evidence_queries", + "align_parallel_commands", + "builtin_semantic_call_catalog", + "merge_parallel_effects", + "resolve_parallel_barrier", + "validate_parallel_claims", +] diff --git a/embodichain/lab/sim/skills/calls.py b/embodichain/lab/sim/skills/calls.py new file mode 100644 index 000000000..21377b28d --- /dev/null +++ b/embodichain/lab/sim/skills/calls.py @@ -0,0 +1,927 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Immutable, robot-independent semantic call specifications.""" + +from __future__ import annotations + +from collections.abc import Iterable, Mapping +from dataclasses import dataclass, field, fields +import math +import re +from types import MappingProxyType +from typing import ClassVar, TypeAlias + +import torch + +from embodichain.utils.math import matrix_from_quat +from embodichain.lab.sim.atomic_actions import ( + DisjointResourceSlots, + DisjointSlotEndpoints, + SkillBindingContract, + SkillDescriptor, + SkillEndpointRequirement, + SkillResourceSlot, +) + +from .scene import ( + SceneAffordanceRef, + SceneArticulationRef, + SceneEntityRef, + SceneLinkRef, + SceneObjectRef, +) + + +def _validate_identifier(value: str, *, field_name: str) -> str: + """Return one exact, non-empty identifier. + + Args: + value: Candidate identifier. + field_name: Diagnostic field name. + + Returns: + The validated input value. + + Raises: + ValueError: If the value is empty or has outer whitespace. + """ + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +def _validate_registered_call_id(value: str) -> str: + """Validate one lowercase, multi-segment extension identifier.""" + _validate_identifier(value, field_name="registered semantic call ID") + if re.fullmatch(r"[a-z][a-z0-9_]*(?:\.[a-z][a-z0-9_]*)+", value) is None: + raise ValueError( + "Registered semantic call IDs must contain two or more lowercase " + "identifier segments separated by single dots." + ) + return value + + +def _snapshot_resources(values: Mapping[str, str]) -> Mapping[str, str]: + """Validate and own a generic slot-to-resource mapping.""" + if not isinstance(values, Mapping): + raise TypeError("resources must be a mapping from slot IDs to resource IDs.") + resources: dict[str, str] = {} + for slot_id, resource_id in values.items(): + _validate_identifier(slot_id, field_name="resource slot IDs") + _validate_identifier(resource_id, field_name="resource IDs") + resources[slot_id] = resource_id + return MappingProxyType(resources) + + +def _validate_static_binding_contract( + contract: SkillBindingContract, + *, + field_name: str, +) -> None: + """Reject runtime-bearing subclasses anywhere in a binding contract.""" + if type(contract) is not SkillBindingContract: + raise TypeError(f"{field_name} must be exactly SkillBindingContract.") + if type(contract.slots) is not tuple or type(contract.constraints) is not tuple: + raise TypeError(f"{field_name} must contain exact immutable tuples.") + for slot in contract.slots: + if type(slot) is not SkillResourceSlot: + raise TypeError( + f"{field_name}.slots must contain exact SkillResourceSlot values." + ) + _validate_identifier(slot.slot_id, field_name=f"{field_name} slot IDs") + if type(slot.endpoints) is not tuple or type(slot.constraints) is not tuple: + raise TypeError(f"{field_name}.slots must contain exact immutable tuples.") + for endpoint in slot.endpoints: + if type(endpoint) is not SkillEndpointRequirement: + raise TypeError( + f"{field_name}.slots.endpoints must contain exact " + "SkillEndpointRequirement values." + ) + _validate_identifier( + endpoint.endpoint_id, + field_name=f"{field_name} endpoint IDs", + ) + if type(endpoint.capabilities) is not frozenset: + raise TypeError( + f"{field_name} endpoint capabilities must be exact frozensets." + ) + for capability in endpoint.capabilities: + _validate_identifier( + capability, + field_name=f"{field_name} endpoint capabilities", + ) + if type(endpoint.required_commands) is not MappingProxyType: + raise TypeError( + f"{field_name} required commands must be an immutable snapshot." + ) + for command_name, command_type in endpoint.required_commands.items(): + _validate_identifier( + command_name, + field_name=f"{field_name} required command names", + ) + if not isinstance(command_type, type): + raise TypeError( + f"{field_name} required command contracts must be class " + "objects." + ) + for constraint in slot.constraints: + if type(constraint) is not DisjointSlotEndpoints: + raise TypeError( + f"{field_name}.slots.constraints must contain exact " + "DisjointSlotEndpoints values." + ) + if type(constraint.endpoint_ids) is not tuple: + raise TypeError( + f"{field_name} endpoint constraints must contain exact tuples." + ) + for endpoint_id in constraint.endpoint_ids: + _validate_identifier( + endpoint_id, + field_name=f"{field_name} constrained endpoint IDs", + ) + for constraint in contract.constraints: + if type(constraint) is not DisjointResourceSlots: + raise TypeError( + f"{field_name}.constraints must contain exact " + "DisjointResourceSlots values." + ) + if type(constraint.slots) is not tuple: + raise TypeError( + f"{field_name} resource constraints must contain exact tuples." + ) + for slot_id in constraint.slots: + _validate_identifier( + slot_id, + field_name=f"{field_name} constrained slot IDs", + ) + + +def _validate_static_skill_descriptor( + descriptor: SkillDescriptor, + *, + field_name: str, +) -> None: + """Validate one exact, provider-free atomic target descriptor.""" + if type(descriptor) is not SkillDescriptor: + raise TypeError(f"{field_name} must be exactly SkillDescriptor.") + _validate_identifier(descriptor.skill_id, field_name=f"{field_name}.skill_id") + if type(descriptor.agent_visible) is not bool: + raise TypeError(f"{field_name}.agent_visible must be exactly bool.") + if type(descriptor.goal_type) is tuple: + if not descriptor.goal_type or not all( + type(goal_type) is type for goal_type in descriptor.goal_type + ): + raise TypeError(f"{field_name}.goal_type must contain exact class objects.") + elif type(descriptor.goal_type) is not type: + raise TypeError( + f"{field_name}.goal_type must be an exact class or tuple of classes." + ) + if type(descriptor.options_type) is not type: + raise TypeError(f"{field_name}.options_type must be an exact class object.") + if descriptor.binding_contract is None: + raise TypeError(f"{field_name}.binding_contract must be declared.") + _validate_static_binding_contract( + descriptor.binding_contract, + field_name=f"{field_name}.binding_contract", + ) + + +@dataclass(frozen=True, slots=True, init=False, eq=False) +class SemanticPose: + """Object-space pose expressed as position and a WXYZ quaternion. + + The value owns normalized tensor snapshots and never exposes its internal + tensors directly. A single pose or an environment batch is accepted. + + Args: + position: Shape ``(3,)`` or ``(B, 3)``. + quaternion_wxyz: Shape ``(4,)`` or ``(B, 4)``. Finite, non-zero + quaternions are normalized at construction. + """ + + _position: torch.Tensor = field(repr=False) + _quaternion_wxyz: torch.Tensor = field(repr=False) + + def __init__( + self, + position: torch.Tensor | tuple[float, float, float] | list[float], + quaternion_wxyz: torch.Tensor | tuple[float, float, float, float] | list[float], + ) -> None: + position_tensor = torch.as_tensor(position, dtype=torch.float32) + quaternion_tensor = torch.as_tensor(quaternion_wxyz, dtype=torch.float32) + if position_tensor.dim() not in (1, 2) or position_tensor.shape[-1] != 3: + raise ValueError("position must have shape (3,) or (B, 3).") + if quaternion_tensor.dim() not in (1, 2) or quaternion_tensor.shape[-1] != 4: + raise ValueError("quaternion_wxyz must have shape (4,) or (B, 4).") + if position_tensor.dim() != quaternion_tensor.dim(): + raise ValueError( + "position and quaternion_wxyz must both be unbatched or batched." + ) + if position_tensor.dim() == 2 and ( + position_tensor.shape[0] != quaternion_tensor.shape[0] + ): + raise ValueError("position and quaternion_wxyz batch sizes must match.") + if position_tensor.dim() == 2 and position_tensor.shape[0] == 0: + raise ValueError("SemanticPose batches must contain at least one pose.") + if not torch.isfinite(position_tensor).all(): + raise ValueError("position must contain only finite values.") + if not torch.isfinite(quaternion_tensor).all(): + raise ValueError("quaternion_wxyz must contain only finite values.") + norms = torch.linalg.vector_norm(quaternion_tensor, dim=-1, keepdim=True) + if torch.any(norms <= torch.finfo(torch.float32).eps): + raise ValueError("quaternion_wxyz must be non-zero.") + object.__setattr__(self, "_position", position_tensor.clone()) + object.__setattr__( + self, + "_quaternion_wxyz", + (quaternion_tensor / norms).clone(), + ) + + @property + def position(self) -> torch.Tensor: + """Return an independent position tensor.""" + return self._position.clone() + + @property + def quaternion_wxyz(self) -> torch.Tensor: + """Return an independent normalized quaternion tensor.""" + return self._quaternion_wxyz.clone() + + @property + def batch_size(self) -> int | None: + """Return the explicit batch size, or ``None`` for one broadcast pose.""" + return None if self._position.dim() == 1 else self._position.shape[0] + + def snapshot(self) -> SemanticPose: + """Return an independently owned pose value.""" + return SemanticPose(self._position, self._quaternion_wxyz) + + def to_matrix(self) -> torch.Tensor: + """Convert the semantic pose to a homogeneous transform. + + Returns: + Shape ``(4, 4)`` for an unbatched pose or ``(B, 4, 4)`` for a + batched pose. + """ + quaternion = self._quaternion_wxyz + was_unbatched = quaternion.dim() == 1 + if was_unbatched: + quaternion = quaternion.unsqueeze(0) + position = self._position.unsqueeze(0) + else: + position = self._position + output = torch.eye( + 4, + dtype=quaternion.dtype, + device=quaternion.device, + ).repeat(quaternion.shape[0], 1, 1) + output[:, :3, :3] = matrix_from_quat(quaternion) + output[:, :3, 3] = position + return output[0] if was_unbatched else output + + def to_metadata(self) -> dict[str, object]: + """Return the pose as deterministic JSON-safe semantic data.""" + return { + "position": self._position.detach().cpu().tolist(), + "quaternion_wxyz": self._quaternion_wxyz.detach().cpu().tolist(), + } + + +def _call_value_to_metadata(value: DeclarativeValue | object) -> object: + """Serialize one already validated semantic-call payload value.""" + if value is None or type(value) in (bool, int, float, str): + return value + if isinstance(value, SceneEntityRef): + return { + "entity_type": type(value).__name__, + "entity_id": value.entity_id, + } + if type(value) is SemanticPose: + return value.to_metadata() + if isinstance(value, Mapping): + return { + key: _call_value_to_metadata(nested) + for key, nested in sorted(value.items()) + } + if isinstance(value, tuple): + return [_call_value_to_metadata(nested) for nested in value] + raise TypeError( + f"Unsupported validated semantic-call metadata value {type(value).__name__}." + ) + + +@dataclass(frozen=True, slots=True, kw_only=True, eq=False) +class SemanticCallSpec: + """Base value contract shared by every declarative semantic call. + + Args: + resources: Optional skill-local slot to robot-resource overrides. + """ + + call_kind: ClassVar[str] = "semantic" + + resources: Mapping[str, str] = field(default_factory=dict) + + def __post_init__(self) -> None: + object.__setattr__(self, "resources", _snapshot_resources(self.resources)) + + @property + def semantic_id(self) -> str: + """Return the stable catalog identifier for this call.""" + return self.call_kind + + def to_metadata(self) -> dict[str, object]: + """Return this semantic call as deterministic JSON-safe data.""" + arguments = { + data_field.name: _call_value_to_metadata(getattr(self, data_field.name)) + for data_field in fields(self) + if data_field.name != "resources" + } + return { + "semantic_id": self.semantic_id, + "call_kind": self.call_kind, + "call_type": type(self).__name__, + "resources": _call_value_to_metadata(self.resources), + "arguments": arguments, + } + + +@dataclass(frozen=True, slots=True, eq=False) +class Pick(SemanticCallSpec): + """Pick one registered object using an optional explicit grasp affordance. + + Args: + object: Authoritative semantic object reference. + grasp: Optional explicit grasp affordance. Omission requests deterministic + registry selection. + resources: Optional skill-local resource overrides. + """ + + call_kind: ClassVar[str] = "pick" + + object: SceneObjectRef + grasp: SceneAffordanceRef | None = None + + def __post_init__(self) -> None: + SemanticCallSpec.__post_init__(self) + if type(self.object) is not SceneObjectRef: + raise TypeError("Pick.object must be a SceneObjectRef.") + if self.grasp is not None and type(self.grasp) is not SceneAffordanceRef: + raise TypeError("Pick.grasp must be a SceneAffordanceRef or None.") + + +PlaceRelationTarget: TypeAlias = SceneObjectRef | SceneAffordanceRef + + +@dataclass(frozen=True, slots=True, eq=False) +class Place(SemanticCallSpec): + """Place a held object at exactly one semantic destination. + + Args: + object: Authoritative held-object reference. + at: Absolute object-space pose. + on: Object or affordance supporting an ``on`` relation. + inside: Object or affordance supporting an ``inside`` relation. + resources: Optional skill-local resource overrides. + """ + + call_kind: ClassVar[str] = "place" + + object: SceneObjectRef + at: SemanticPose | None = None + on: PlaceRelationTarget | None = None + inside: PlaceRelationTarget | None = None + + def __post_init__(self) -> None: + SemanticCallSpec.__post_init__(self) + if type(self.object) is not SceneObjectRef: + raise TypeError("Place.object must be a SceneObjectRef.") + destinations = { + "at": self.at, + "on": self.on, + "inside": self.inside, + } + selected = [name for name, value in destinations.items() if value is not None] + if len(selected) != 1: + raise ValueError( + "Place requires exactly one of at, on, or inside; selected " + f"{selected}." + ) + if self.at is not None: + if type(self.at) is not SemanticPose: + raise TypeError("Place.at must be a SemanticPose or None.") + object.__setattr__(self, "at", self.at.snapshot()) + for field_name in ("on", "inside"): + target = getattr(self, field_name) + if target is not None and type(target) not in ( + SceneObjectRef, + SceneAffordanceRef, + ): + raise TypeError( + f"Place.{field_name} must be a SceneObjectRef, " + "SceneAffordanceRef, or None." + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class HandOver(SemanticCallSpec): + """Transfer a held object to another robot resource. + + Args: + object: Authoritative held-object reference. + final_target: Optional final object-space delivery pose. + resources: Optional ``source`` and ``destination`` resource overrides. + """ + + call_kind: ClassVar[str] = "hand_over" + + object: SceneObjectRef + final_target: SemanticPose | None = None + + def __post_init__(self) -> None: + SemanticCallSpec.__post_init__(self) + if type(self.object) is not SceneObjectRef: + raise TypeError("HandOver.object must be a SceneObjectRef.") + if self.final_target is not None: + if type(self.final_target) is not SemanticPose: + raise TypeError("HandOver.final_target must be a SemanticPose or None.") + object.__setattr__( + self, + "final_target", + self.final_target.snapshot(), + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class OperateArticulation(SemanticCallSpec): + """Operate one registered articulation through a typed handle affordance. + + Select either a named affordance target or an explicit absolute joint + position plus handle-relative displacement. Grounding captures the current + live joint position as the source of that declared stroke. Recovery + replans then combine the latest handle pose and joint position to execute + only the remaining signed displacement. + + Args: + articulation: Authoritative articulation reference. + handle: Optional explicit operation affordance. Omission requests the + capability-scoped default registered on the articulation. + target: Optional target name registered by the affordance. + target_position: Explicit absolute desired joint position. + target_displacement: Explicit full signed operation displacement from + the joint position and handle pose captured during grounding. + resources: Optional skill-local resource overrides. + """ + + call_kind: ClassVar[str] = "operate_articulation" + + articulation: SceneArticulationRef + handle: SceneAffordanceRef | None = None + target: str | None = None + target_position: float | None = None + target_displacement: float | None = None + + def __post_init__(self) -> None: + SemanticCallSpec.__post_init__(self) + if type(self.articulation) is not SceneArticulationRef: + raise TypeError( + "OperateArticulation.articulation must be a SceneArticulationRef." + ) + if self.handle is not None and type(self.handle) is not SceneAffordanceRef: + raise TypeError( + "OperateArticulation.handle must be a SceneAffordanceRef or None." + ) + named = self.target is not None + explicit_position = self.target_position is not None + explicit_displacement = self.target_displacement is not None + if named: + _validate_identifier( + self.target, + field_name="OperateArticulation.target", + ) + if explicit_position or explicit_displacement: + raise ValueError( + "OperateArticulation.target is mutually exclusive with " + "target_position and target_displacement." + ) + return + if not (explicit_position and explicit_displacement): + raise ValueError( + "OperateArticulation requires either target or the explicit " + "target_position and target_displacement pair." + ) + for field_name in ("target_position", "target_displacement"): + value = getattr(self, field_name) + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise TypeError( + f"OperateArticulation.{field_name} must be a finite scalar." + ) + normalized = float(value) + if not math.isfinite(normalized): + raise ValueError(f"OperateArticulation.{field_name} must be finite.") + object.__setattr__(self, field_name, normalized) + + +DeclarativeValue: TypeAlias = ( + None + | bool + | int + | float + | str + | SceneEntityRef + | SemanticPose + | tuple["DeclarativeValue", ...] + | Mapping[str, "DeclarativeValue"] +) + + +def _snapshot_declarative_value( + value: object, + *, + path: str, + _active: set[int] | None = None, + _budget: list[int] | None = None, + _depth: int = 0, +) -> DeclarativeValue: + """Recursively own a bounded, acyclic, non-executable payload.""" + if _active is None: + _active = set() + if _budget is None: + _budget = [4096] + if _depth > 32: + raise ValueError(f"{path} exceeds the maximum declarative depth of 32.") + _budget[0] -= 1 + if _budget[0] < 0: + raise ValueError(f"{path} exceeds the maximum declarative node count.") + if value is None or type(value) in (bool, int, str): + return value + if type(value) is float: + if not math.isfinite(value): + raise ValueError(f"{path} must be finite.") + return value + if type(value) in ( + SceneEntityRef, + SceneObjectRef, + SceneArticulationRef, + SceneLinkRef, + SceneAffordanceRef, + ): + return value + if type(value) is SemanticPose: + snapshot = value.snapshot() + if type(snapshot) is not SemanticPose or snapshot is value: + raise TypeError( + f"{path}.snapshot() must return an independent SemanticPose." + ) + return snapshot + if type(value) in (dict, MappingProxyType): + container_id = id(value) + if container_id in _active: + raise ValueError(f"{path} contains a cyclic declarative mapping.") + _active.add(container_id) + try: + snapshot: dict[str, DeclarativeValue] = {} + for key, nested in value.items(): + _validate_identifier(key, field_name=f"{path} keys") + snapshot[key] = _snapshot_declarative_value( + nested, + path=f"{path}.{key}", + _active=_active, + _budget=_budget, + _depth=_depth + 1, + ) + return MappingProxyType(snapshot) + finally: + _active.remove(container_id) + if type(value) in (tuple, list): + container_id = id(value) + if container_id in _active: + raise ValueError(f"{path} contains a cyclic declarative sequence.") + _active.add(container_id) + try: + return tuple( + _snapshot_declarative_value( + nested, + path=f"{path}[{index}]", + _active=_active, + _budget=_budget, + _depth=_depth + 1, + ) + for index, nested in enumerate(value) + ) + finally: + _active.remove(container_id) + raise TypeError( + f"{path} contains non-declarative {type(value).__name__}; callables, " + "classes, modules, tensors, and live objects are not allowed." + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class RegisteredSemanticCall(SemanticCallSpec): + """Safe value payload for a catalog-registered semantic extension. + + Args: + call_id: Stable extension identifier discovered in a semantic catalog. + arguments: Nested declarative data. Executable or live values are + rejected at construction. + resources: Optional skill-local resource overrides. + """ + + call_kind: ClassVar[str] = "registered" + + call_id: str + arguments: Mapping[str, DeclarativeValue] = field(default_factory=dict) + + def __post_init__(self) -> None: + SemanticCallSpec.__post_init__(self) + _validate_registered_call_id(self.call_id) + if type(self.arguments) not in (dict, MappingProxyType): + raise TypeError( + "RegisteredSemanticCall.arguments must be an exact dict or " + "immutable mapping proxy." + ) + object.__setattr__( + self, + "arguments", + _snapshot_declarative_value( + self.arguments, + path="RegisteredSemanticCall.arguments", + ), + ) + + @property + def semantic_id(self) -> str: + """Return the registered extension identifier.""" + return self.call_id + + +@dataclass(frozen=True, slots=True) +class SemanticCallDescriptor: + """Static catalog metadata for one semantic call kind. + + Args: + call_id: Stable semantic call identifier. + spec_type: Exact public call value type. + schema_version: Explicit configuration payload schema version. + target_descriptor: Exact atomic goal/options/resource contract. It is + inferred and non-overridable for curated calls and required for + registered extensions. + """ + + call_id: str + spec_type: type[SemanticCallSpec] + schema_version: int = 1 + target_descriptor: SkillDescriptor | None = None + + def __post_init__(self) -> None: + _validate_identifier(self.call_id, field_name="SemanticCallDescriptor.call_id") + if self.spec_type not in ( + Pick, + Place, + HandOver, + OperateArticulation, + RegisteredSemanticCall, + ): + raise TypeError( + "spec_type must be exactly Pick, Place, HandOver, " + "OperateArticulation, or RegisteredSemanticCall; extensions use " + "the registered payload contract rather than executable call " + "subclasses." + ) + if not isinstance(self.schema_version, int) or isinstance( + self.schema_version, bool + ): + raise TypeError("schema_version must be an integer.") + if self.schema_version != 1: + raise ValueError( + "Unsupported semantic call schema_version " + f"{self.schema_version}; supported versions are [1]." + ) + if self.spec_type is not RegisteredSemanticCall and ( + self.call_id != self.spec_type.call_kind + ): + raise ValueError( + f"Descriptor ID {self.call_id!r} must match " + f"{self.spec_type.__name__}.call_kind " + f"{self.spec_type.call_kind!r}." + ) + if self.spec_type is not RegisteredSemanticCall: + expected = _builtin_call_target(self.spec_type) + if ( + self.target_descriptor is not None + and self.target_descriptor != expected + ): + raise ValueError( + f"Built-in semantic call {self.call_id!r} must target skill " + f"{expected.skill_id!r} with its exact curated descriptor. " + "Use RegisteredSemanticCall for extensions." + ) + object.__setattr__(self, "target_descriptor", expected) + else: + if self.target_descriptor is None: + raise TypeError( + "Registered semantic descriptors require target_descriptor." + ) + _validate_static_skill_descriptor( + self.target_descriptor, + field_name="SemanticCallDescriptor.target_descriptor", + ) + if ( + not self.target_descriptor.agent_visible + or self.target_descriptor.binding_contract is None + ): + raise ValueError( + "Registered target_descriptor must be agent-visible and " + "declare a binding contract." + ) + if self.spec_type is RegisteredSemanticCall and self.call_id in { + Pick.call_kind, + Place.call_kind, + HandOver.call_kind, + OperateArticulation.call_kind, + RegisteredSemanticCall.call_kind, + }: + raise ValueError( + f"Registered semantic call ID {self.call_id!r} is reserved." + ) + if self.spec_type is RegisteredSemanticCall: + _validate_registered_call_id(self.call_id) + + @property + def skill_id(self) -> str: + """Return the atomic skill ID from the canonical target descriptor.""" + assert self.target_descriptor is not None + return self.target_descriptor.skill_id + + @property + def binding_contract(self) -> SkillBindingContract: + """Return the resource contract from the canonical target descriptor.""" + assert self.target_descriptor is not None + contract = self.target_descriptor.binding_contract + assert contract is not None + return contract + + +@dataclass(frozen=True, slots=True, init=False) +class SemanticCallCatalog: + """Immutable discovery catalog separated from engine installation.""" + + _descriptors: Mapping[str, SemanticCallDescriptor] + + def __init__( + self, + descriptors: Iterable[SemanticCallDescriptor], + ) -> None: + if isinstance(descriptors, (str, bytes)): + raise TypeError("descriptors must be an iterable of descriptors.") + try: + supplied = tuple(descriptors) + except TypeError as exc: + raise TypeError("descriptors must be an iterable of descriptors.") from exc + normalized: dict[str, SemanticCallDescriptor] = {} + for descriptor in supplied: + if type(descriptor) is not SemanticCallDescriptor: + raise TypeError( + "descriptors must contain exact SemanticCallDescriptor values." + ) + if descriptor.call_id in normalized: + raise ValueError(f"Duplicate semantic call ID {descriptor.call_id!r}.") + normalized[descriptor.call_id] = descriptor + object.__setattr__( + self, + "_descriptors", + MappingProxyType(normalized), + ) + + @property + def descriptors(self) -> Mapping[str, SemanticCallDescriptor]: + """Return immutable descriptors keyed by exact semantic ID.""" + return self._descriptors + + def discover( + self, + call: str | SemanticCallSpec, + ) -> SemanticCallDescriptor: + """Discover metadata without installing or executing an implementation. + + Args: + call: Exact semantic ID or a call value. + + Returns: + Matching immutable descriptor. + + Raises: + KeyError: If the exact call ID is unknown. + TypeError: If the call type disagrees with its descriptor. + """ + if type(call) is str: + call_id = _validate_identifier(call, field_name="semantic call ID") + call_value = None + elif type(call) in ( + Pick, + Place, + HandOver, + OperateArticulation, + RegisteredSemanticCall, + ): + call_id = call.semantic_id + call_value = call + else: + raise TypeError( + "call must be an exact semantic call ID or supported call value." + ) + descriptor = self._descriptors.get(call_id) + if descriptor is None: + raise KeyError( + f"Unknown semantic call {call_id!r}; available calls are " + f"{sorted(self._descriptors)}." + ) + if call_value is not None and type(call_value) is not descriptor.spec_type: + raise TypeError( + f"Semantic call {call_id!r} expects " + f"{descriptor.spec_type.__name__}, got " + f"{type(call_value).__name__}." + ) + return descriptor + + def with_descriptor( + self, + descriptor: SemanticCallDescriptor, + ) -> SemanticCallCatalog: + """Return a new catalog containing one additional descriptor.""" + return SemanticCallCatalog((*self._descriptors.values(), descriptor)) + + +def _builtin_call_target( + spec_type: type[SemanticCallSpec], +) -> SkillDescriptor: + """Return the non-overridable atomic target for one curated call type.""" + from embodichain.lab.sim.atomic_actions.primitives.hand_over import ( + HandOver as HandOverAction, + ) + from embodichain.lab.sim.atomic_actions.primitives.operate_articulation import ( + OperateArticulation as OperateArticulationAction, + ) + from embodichain.lab.sim.atomic_actions.primitives.pick_up import PickUp + from embodichain.lab.sim.atomic_actions.primitives.place import Place as PlaceAction + + targets = { + Pick: PickUp.descriptor(), + Place: PlaceAction.descriptor(), + HandOver: HandOverAction.descriptor(), + OperateArticulation: OperateArticulationAction.descriptor(), + } + try: + return targets[spec_type] + except KeyError as exc: + raise TypeError(f"Unsupported curated call type {spec_type!r}.") from exc + + +def builtin_semantic_call_catalog() -> SemanticCallCatalog: + """Build the curated catalog for installed manipulation primitives. + + Returns: + A fresh immutable catalog. Atomic implementations remain uninstalled; + callers bind them to an engine through the separate runtime path. + """ + descriptors = tuple( + SemanticCallDescriptor( + call_id=spec_type.call_kind, + spec_type=spec_type, + ) + for spec_type in (Pick, Place, HandOver, OperateArticulation) + ) + return SemanticCallCatalog(descriptors) + + +__all__ = [ + "DeclarativeValue", + "HandOver", + "OperateArticulation", + "Pick", + "Place", + "PlaceRelationTarget", + "RegisteredSemanticCall", + "SemanticCallCatalog", + "SemanticCallDescriptor", + "SemanticCallSpec", + "SemanticPose", + "builtin_semantic_call_catalog", +] diff --git a/embodichain/lab/sim/skills/compiler.py b/embodichain/lab/sim/skills/compiler.py new file mode 100644 index 000000000..34680d85c --- /dev/null +++ b/embodichain/lab/sim/skills/compiler.py @@ -0,0 +1,2686 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Static workflow analysis and JIT semantic-call lowering.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +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 + +import torch + +from embodichain.lab.sim.atomic_actions import ( + ActionControlOverrides, + ActionInvocation, + ActionOptions, + Affordance, + ArticulationOperationAffordance, + GraspGoal, + HandOverOptions, + HeldObjectState, + PickUpOptions, + PlaceGoal, + PlaceOptions, + OperateArticulationGoal, + OperateArticulationOptions, + PhaseEffectGateRequirement, + PlanningContext, + PoseGoalValue, + SceneArticulationOperationGeometry, + SceneEntityPose, +) +from .calls import ( + HandOver, + OperateArticulation, + Pick, + Place, + RegisteredSemanticCall, + SemanticCallSpec, + SemanticPose, +) +from .effects import ( + ArticulationJointStateExpectation, + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + POSE_RELATION_EFFECT_CHANNEL, + BinaryEffectClause, + BinaryEvidenceKind, + CompositeEffectMonitorFactory, + CoordinatedHeldObjectCleanupExpectation, + EffectClause, + EffectMonitor, + EffectMonitorRef, + EffectMonitorRegistry, + EffectEvidenceSourceRef, + EffectStateExpectation, + HeldObjectRelation, + HeldObjectStateExpectation, + JointStateEffectClause, + PoseRelationClause, + PoseRelationExpectation, + ScalarEffectClause, + ScalarExpectation, + SemanticEffectKind, + SemanticEffectSpec, + SymbolicStateKey, +) +from .integration import ( + BoundSemanticCall, + BoundSemanticIntegration, + PathPart, + SemanticDiagnostic, + SemanticValidationError, +) +from .scene import ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + ContainerAffordance, + PLACEMENT_TARGET_AFFORDANCE_REVISION, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, + ArticulationJointEvidenceAddress, + PLACE_IN_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, + SceneAffordanceRef, + SceneObjectRef, + SupportSurfaceAffordance, +) + +OptionT = TypeVar("OptionT", bound=ActionOptions) + + +def _validate_identifier(value: str, *, field_name: str) -> str: + """Return one exact non-empty identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +def _diagnostic( + code: str, + path: tuple[PathPart, ...], + message: str, + candidates: tuple[str, ...] = (), +) -> SemanticValidationError: + """Build one pathful semantic compiler error.""" + return SemanticValidationError(SemanticDiagnostic(code, path, message, candidates)) + + +@dataclass(frozen=True, slots=True) +class SemanticRelationTarget: + """Statically selected relation affordance awaiting typed grounding.""" + + capability: str + affordance: SceneAffordanceRef + payload_type: type[Affordance] + payload_revision: str + + def __post_init__(self) -> None: + _validate_identifier(self.capability, field_name="relation capability") + if type(self.affordance) is not SceneAffordanceRef: + raise TypeError("affordance must be exactly SceneAffordanceRef.") + if not isinstance(self.payload_type, type) or not issubclass( + self.payload_type, Affordance + ): + raise TypeError("payload_type must be an Affordance subclass.") + _validate_identifier( + self.payload_revision, + field_name="relation payload_revision", + ) + + @property + def grounder_key(self) -> tuple[str, type[Affordance], str]: + """Return the exact typed/versioned grounder lookup key.""" + return self.capability, self.payload_type, self.payload_revision + + +class RelationTargetGrounder(ABC): + """Shared implementation that converts one relation into object pose.""" + + capability: ClassVar[str] + affordance_type: ClassVar[type[Affordance]] + affordance_revision: ClassVar[str] + + @abstractmethod + def ground( + self, + relation: SemanticRelationTarget, + *, + affordance: Affordance, + context: PlanningContext, + ) -> PoseGoalValue: + """Return an object-space target from current state and typed payload. + + Args: + relation: Statically selected relation metadata. + affordance: Owned exact-type affordance payload. + context: Latest immutable planning observation. + + Returns: + Direct or scene-relative desired object pose. + """ + + +class SupportSurfaceRelationTargetGrounder(RelationTargetGrounder): + """Ground a declared support target frame to a late-bound object pose.""" + + capability: ClassVar[str] = PLACE_ON_AFFORDANCE_CAPABILITY + affordance_type: ClassVar[type[Affordance]] = SupportSurfaceAffordance + affordance_revision: ClassVar[str] = PLACEMENT_TARGET_AFFORDANCE_REVISION + + def ground( + self, + relation: SemanticRelationTarget, + *, + affordance: Affordance, + context: PlanningContext, + ) -> SceneEntityPose: + """Return the current support-relative target frame.""" + del context + if type(affordance) is not SupportSurfaceAffordance: + raise TypeError("affordance must be exactly SupportSurfaceAffordance.") + return SceneEntityPose( + relation.affordance.entity_id, + minimum_confidence=affordance.minimum_confidence, + ) + + +class ContainerRelationTargetGrounder(RelationTargetGrounder): + """Ground a declared container target frame to a late-bound object pose.""" + + capability: ClassVar[str] = PLACE_IN_AFFORDANCE_CAPABILITY + affordance_type: ClassVar[type[Affordance]] = ContainerAffordance + affordance_revision: ClassVar[str] = PLACEMENT_TARGET_AFFORDANCE_REVISION + + def ground( + self, + relation: SemanticRelationTarget, + *, + affordance: Affordance, + context: PlanningContext, + ) -> SceneEntityPose: + """Return the current container-relative target frame.""" + del context + if type(affordance) is not ContainerAffordance: + raise TypeError("affordance must be exactly ContainerAffordance.") + return SceneEntityPose( + relation.affordance.entity_id, + minimum_confidence=affordance.minimum_confidence, + ) + + +@dataclass(frozen=True, slots=True) +class SemanticObjectTarget: + """One object-space look-ahead target. + + Relation targets remain late-bound and require an explicitly installed + typed/versioned grounder. Handover targets defer to the + embodiment-selected provider and are used only for workflow look-ahead. + """ + + value: ( + SemanticPose | SceneEntityPose | SemanticRelationTarget | SemanticHandOverTarget + ) + + def __post_init__(self) -> None: + if type(self.value) in (SemanticPose, SceneEntityPose): + object.__setattr__(self, "value", self.value.snapshot()) + elif type(self.value) not in ( + SemanticRelationTarget, + SemanticHandOverTarget, + ): + raise TypeError( + "value must be exactly SemanticPose, SceneEntityPose, " + "SemanticRelationTarget, or SemanticHandOverTarget." + ) + + @property + def pose(self) -> SemanticPose | SceneEntityPose | None: + """Return the direct pose variant, when selected.""" + if type(self.value) in (SemanticPose, SceneEntityPose): + return self.value # type: ignore[return-value] + return None + + @property + def relation(self) -> SemanticRelationTarget | None: + """Return the relation variant, when selected.""" + return self.value if type(self.value) is SemanticRelationTarget else None + + @property + def handover(self) -> SemanticHandOverTarget | None: + """Return the deferred handover variant, when selected.""" + return self.value if type(self.value) is SemanticHandOverTarget else None + + +@dataclass(frozen=True, slots=True) +class SemanticHandOverTarget: + """Deferred middle pose selected by one named embodiment provider.""" + + provider_id: str + bound: BoundSemanticCall + + def __post_init__(self) -> None: + _validate_identifier(self.provider_id, field_name="handover provider_id") + if type(self.bound) is not BoundSemanticCall: + raise TypeError("bound must be exactly BoundSemanticCall.") + if type(self.bound.linked.call) is not HandOver: + raise TypeError("bound must contain an exact HandOver call.") + + +@dataclass(frozen=True, slots=True) +class SemanticEffectDependency: + """A consumer's verified-held-state dependency on an earlier call.""" + + producer_index: int | None + consumer_index: int + object: SceneObjectRef + + def __post_init__(self) -> None: + if self.producer_index is not None and ( + type(self.producer_index) is not int or self.producer_index < 0 + ): + raise ValueError("producer_index must be non-negative or None.") + if type(self.consumer_index) is not int or self.consumer_index < 0: + raise ValueError("consumer_index must be non-negative.") + if self.producer_index is not None and ( + self.producer_index >= self.consumer_index + ): + raise ValueError("producer_index must precede consumer_index.") + if type(self.object) is not SceneObjectRef: + raise TypeError("object must be exactly SceneObjectRef.") + + +@dataclass(frozen=True, slots=True) +class AnalyzedSemanticCall: + """One statically linked call plus workflow-derived lowering metadata.""" + + index: int + bound: BoundSemanticCall + effect_kind: SemanticEffectKind + symbolic_writes: frozenset[SymbolicStateKey] = frozenset() + opaque_symbolic_effect: bool = False + effect_monitor_ref: EffectMonitorRef | None = None + downstream_object_targets: tuple[SemanticObjectTarget, ...] = () + requires_verified_held_object: bool = False + requires_fresh_observation: bool = True + + def __post_init__(self) -> None: + if type(self.index) is not int or self.index < 0: + raise ValueError("index must be a non-negative integer.") + if type(self.bound) is not BoundSemanticCall: + raise TypeError("bound must be exactly BoundSemanticCall.") + if not isinstance(self.effect_kind, SemanticEffectKind): + raise TypeError("effect_kind must be a SemanticEffectKind.") + if type(self.symbolic_writes) is not frozenset or not all( + type(write) is SymbolicStateKey for write in self.symbolic_writes + ): + raise TypeError( + "symbolic_writes must be an exact frozenset of " + "SymbolicStateKey values." + ) + if type(self.opaque_symbolic_effect) is not bool: + raise TypeError("opaque_symbolic_effect must be a bool.") + if self.opaque_symbolic_effect and self.symbolic_writes: + raise ValueError( + "Opaque symbolic effects cannot also claim inferred exact keys." + ) + if self.effect_monitor_ref is not None: + if not isinstance(self.effect_monitor_ref, EffectMonitorRef): + raise TypeError( + "effect_monitor_ref must be an EffectMonitorRef or None." + ) + object.__setattr__( + self, + "effect_monitor_ref", + self.effect_monitor_ref.snapshot(), + ) + targets = tuple(self.downstream_object_targets) + if not all(type(target) is SemanticObjectTarget for target in targets): + raise TypeError( + "downstream_object_targets must contain exact " + "SemanticObjectTarget values." + ) + object.__setattr__(self, "downstream_object_targets", targets) + if type(self.requires_verified_held_object) is not bool: + raise TypeError("requires_verified_held_object must be a bool.") + if type(self.requires_fresh_observation) is not bool: + raise TypeError("requires_fresh_observation must be a bool.") + + @property + def call(self) -> SemanticCallSpec: + """Return the canonical linked semantic call.""" + return self.bound.linked.call + + +@dataclass(frozen=True, slots=True) +class SemanticWorkflow: + """Immutable result of static workflow analysis.""" + + workflow_id: str + calls: tuple[AnalyzedSemanticCall, ...] + effect_dependencies: tuple[SemanticEffectDependency, ...] = () + _compiler_id: str = field(repr=False, compare=False, default="") + + def __post_init__(self) -> None: + _validate_identifier(self.workflow_id, field_name="workflow_id") + calls = tuple(self.calls) + if not calls: + raise ValueError("SemanticWorkflow requires at least one call.") + if not all(type(call) is AnalyzedSemanticCall for call in calls): + raise TypeError("calls must contain exact AnalyzedSemanticCall values.") + if tuple(call.index for call in calls) != tuple(range(len(calls))): + raise ValueError("SemanticWorkflow call indices must be contiguous.") + dependencies = tuple(self.effect_dependencies) + if not all( + type(dependency) is SemanticEffectDependency for dependency in dependencies + ): + raise TypeError( + "effect_dependencies must contain exact " + "SemanticEffectDependency values." + ) + _validate_identifier(self._compiler_id, field_name="compiler_id") + object.__setattr__(self, "calls", calls) + object.__setattr__(self, "effect_dependencies", dependencies) + + +@dataclass(frozen=True, slots=True) +class SemanticLowering: + """Registered-lowerer output wrapped by compiler-owned invocation policy.""" + + goal: object + skill_options: ActionOptions | None = None + control_overrides: ActionControlOverrides = field( + default_factory=ActionControlOverrides + ) + + def __post_init__(self) -> None: + if self.skill_options is not None and not isinstance( + self.skill_options, ActionOptions + ): + raise TypeError("skill_options must be an ActionOptions or None.") + if type(self.control_overrides) is not ActionControlOverrides: + raise TypeError("control_overrides must be exactly ActionControlOverrides.") + + +class RegisteredSemanticLowerer(ABC): + """Explicitly installed implementation for one registered call ID.""" + + call_id: ClassVar[str] + schema_version: ClassVar[int] + + @abstractmethod + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + option_template: ActionOptions, + ) -> SemanticLowering: + """Lower a registered value with one owned typed option template. + + The lowerer must return :class:`SemanticLowering` with + ``skill_options=None``. The supplied template is an owned read-only + input for goal grounding; the selected policy preset remains the sole + owner of action options. + """ + + +@dataclass(frozen=True, slots=True) +class HandOverPoseTargets: + """Embodiment-owned object-space poses needed by the core handover skill.""" + + middle: SemanticObjectTarget + final: SemanticObjectTarget + + def __post_init__(self) -> None: + if type(self.middle) is not SemanticObjectTarget: + raise TypeError("middle must be exactly SemanticObjectTarget.") + if type(self.final) is not SemanticObjectTarget: + raise TypeError("final must be exactly SemanticObjectTarget.") + + +class HandOverPoseProvider(ABC): + """Integration extension that selects robot-appropriate handover poses.""" + + provider_id: ClassVar[str] + + @abstractmethod + def resolve( + self, + call: HandOver, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> HandOverPoseTargets: + """Return middle and final object-space targets for one handover. + + Args: + call: Canonical handover semantic value. + context: Latest immutable planning observation. + bound: Engine/profile-bound handover call. + + Returns: + Embodiment-appropriate middle and final object targets. + """ + + +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) +class GroundedPhaseEffectGate: + """One independently monitored physical-effect segment-entry gate. + + Args: + gate_id: Invocation-local stable gate identity. + segment_name: Named trajectory segment blocked by the gate. + effect_spec: Single-expectation physical observation contract. + effect_monitor: Fresh monitor instance owned only by this gate. + retry_action: Whether contradiction may retry the enclosing action. + """ + + gate_id: str + segment_name: str + effect_spec: SemanticEffectSpec + effect_monitor: EffectMonitor = field(repr=False, compare=False) + retry_action: bool = True + + def __post_init__(self) -> None: + _validate_identifier(self.gate_id, field_name="gate_id") + _validate_identifier(self.segment_name, field_name="segment_name") + if not isinstance(self.effect_spec, SemanticEffectSpec): + raise TypeError("effect_spec must be a SemanticEffectSpec.") + physical_ids = {clause.expectation_id for clause in self.effect_spec.clauses} + if ( + len(self.effect_spec.state_expectations) != 1 + or len(physical_ids) != 1 + or next(iter(physical_ids)) + != self.effect_spec.state_expectations[0].expectation_id + ): + raise ValueError( + "A phase-effect gate must own exactly one physically observed " + "state expectation." + ) + if not isinstance(self.effect_monitor, EffectMonitor): + raise TypeError("effect_monitor must be an EffectMonitor.") + if type(self.retry_action) is not bool: + raise TypeError("retry_action must be a bool.") + object.__setattr__(self, "effect_spec", self.effect_spec.snapshot()) + + @property + def requirement(self) -> PhaseEffectGateRequirement: + """Return the core-owned blocking requirement for this monitor.""" + return PhaseEffectGateRequirement( + gate_id=self.gate_id, + segment_name=self.segment_name, + ) + + +@dataclass(frozen=True, slots=True, init=False) +class GroundedSemanticCall: + """Call lowered from the latest observed context.""" + + analyzed: AnalyzedSemanticCall + invocation: ActionInvocation + effect_spec: SemanticEffectSpec | None + effect_monitor: EffectMonitor | None = field(repr=False, compare=False) + effect_guards: tuple[GroundedHeldObjectGuard, ...] + effect_gates: tuple[GroundedPhaseEffectGate, ...] + _eligible_mask: torch.Tensor = field(repr=False, compare=False) + + def __init__(self, *args: object, **kwargs: object) -> None: + """Reject construction outside :class:`SemanticSkillCompiler`.""" + del args, kwargs + raise TypeError( + "GroundedSemanticCall values are created by " + "SemanticSkillCompiler.ground()." + ) + + @classmethod + def _create( + cls, + *, + analyzed: AnalyzedSemanticCall, + invocation: ActionInvocation, + effect_spec: SemanticEffectSpec | None, + effect_monitor: EffectMonitor | None, + effect_guards: tuple[GroundedHeldObjectGuard, ...], + effect_gates: tuple[GroundedPhaseEffectGate, ...], + eligible_mask: torch.Tensor, + ) -> GroundedSemanticCall: + """Create one compiler-owned grounded result.""" + instance = object.__new__(cls) + object.__setattr__(instance, "analyzed", analyzed) + 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, "effect_gates", tuple(effect_gates)) + object.__setattr__(instance, "_eligible_mask", eligible_mask.clone()) + instance.__post_init__() + return instance + + def __post_init__(self) -> None: + if type(self.analyzed) is not AnalyzedSemanticCall: + raise TypeError("analyzed must be exactly AnalyzedSemanticCall.") + if type(self.invocation) is not ActionInvocation: + raise TypeError("invocation must be exactly ActionInvocation.") + if self.invocation.skill_id != self.analyzed.bound.linked.descriptor.skill_id: + raise ValueError("invocation skill_id must match the analyzed call.") + if (self.effect_spec is None) != (self.effect_monitor is None): + raise ValueError( + "effect_spec and effect_monitor must either both be set or both be None." + ) + if self.effect_spec is not None: + if not isinstance(self.effect_spec, SemanticEffectSpec): + raise TypeError("effect_spec must be a SemanticEffectSpec or None.") + if not isinstance(self.effect_monitor, EffectMonitor): + raise TypeError("effect_monitor must be an EffectMonitor or None.") + if self.effect_spec.semantic_id != self.analyzed.call.semantic_id: + raise ValueError( + "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) + gates = tuple(self.effect_gates) + if not all(type(value) is GroundedPhaseEffectGate for value in gates): + raise TypeError( + "effect_gates must contain exact GroundedPhaseEffectGate values." + ) + gate_ids = [value.gate_id for value in gates] + gate_segments = [value.segment_name for value in gates] + if len(set(gate_ids)) != len(gate_ids): + raise ValueError("Grounded phase-effect gate IDs must be unique.") + if len(set(gate_segments)) != len(gate_segments): + raise ValueError( + "At most one grounded phase-effect gate may block each segment." + ) + if tuple(value.requirement for value in gates) != ( + self.invocation.phase_effect_gates + ): + raise ValueError( + "Grounded phase-effect gates must match invocation requirements." + ) + if gates and self.effect_spec is None: + raise ValueError("Phase-effect gates require a terminal effect spec.") + object.__setattr__(self, "effect_gates", gates) + 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: + raise ValueError("eligible_mask must be a one-dimensional bool tensor.") + if self._eligible_mask.numel() == 0: + raise ValueError("eligible_mask must contain at least one environment.") + object.__setattr__(self, "_eligible_mask", self._eligible_mask.clone()) + + @property + def eligible_mask(self) -> torch.Tensor: + """Return an owned mask that the execution session must preserve.""" + return self._eligible_mask.clone() + + +class SemanticSkillCompiler: + """Analyze semantic workflows and JIT-lower exactly one call at a time.""" + + def __init__( + self, + integration: BoundSemanticIntegration, + *, + registered_lowerers: Iterable[RegisteredSemanticLowerer] = (), + relation_grounders: Iterable[RelationTargetGrounder] = (), + handover_pose_providers: Iterable[HandOverPoseProvider] = (), + effect_monitor_registry: EffectMonitorRegistry | None = None, + ) -> None: + """Install immutable semantic lowering and grounding registries. + + Args: + integration: Exact live scene, engine, and robot-profile binding. + registered_lowerers: Explicit implementations for registered calls. + relation_grounders: Exact capability/payload/revision dispatch entries. + handover_pose_providers: Named embodiment-owned handover providers. + effect_monitor_registry: Versioned semantic-effect monitor factories. + """ + if type(integration) is not BoundSemanticIntegration: + raise TypeError("integration must be exactly BoundSemanticIntegration.") + if isinstance(registered_lowerers, (str, bytes)): + raise TypeError("registered_lowerers must be an iterable of lowerers.") + try: + supplied_lowerers = tuple(registered_lowerers) + except TypeError as exc: + raise TypeError( + "registered_lowerers must be an iterable of lowerers." + ) from exc + lowerers: dict[str, RegisteredSemanticLowerer] = {} + for lowerer in supplied_lowerers: + if not isinstance(lowerer, RegisteredSemanticLowerer): + raise TypeError( + "registered_lowerers must contain RegisteredSemanticLowerer " + "instances." + ) + call_id = _validate_identifier( + getattr(type(lowerer), "call_id", None), + field_name="RegisteredSemanticLowerer.call_id", + ) + if call_id in lowerers: + raise ValueError(f"Duplicate registered lowerer {call_id!r}.") + try: + descriptor = integration.manifest.call_catalog.discover(call_id) + except KeyError as exc: + raise ValueError( + f"Lowerer {call_id!r} has no registered semantic descriptor." + ) from exc + if descriptor.spec_type is not RegisteredSemanticCall: + raise ValueError( + f"Lowerer {call_id!r} cannot replace curated call semantics." + ) + schema_version = getattr(type(lowerer), "schema_version", None) + if type(schema_version) is not int or ( + schema_version != descriptor.schema_version + ): + raise ValueError( + f"Lowerer {call_id!r} schema_version must exactly match " + f"descriptor version {descriptor.schema_version}." + ) + lowerers[call_id] = lowerer + if isinstance(relation_grounders, (str, bytes)): + raise TypeError("relation_grounders must be an iterable of grounders.") + try: + supplied_grounders = tuple(relation_grounders) + except TypeError as exc: + raise TypeError( + "relation_grounders must be an iterable of grounders." + ) from exc + normalized_grounders: dict[ + tuple[str, type[Affordance], str], RelationTargetGrounder + ] = {} + for grounder in supplied_grounders: + if not isinstance(grounder, RelationTargetGrounder): + raise TypeError( + "relation_grounders must contain RelationTargetGrounder " + "instances." + ) + grounder_type = type(grounder) + capability = _validate_identifier( + getattr(grounder_type, "capability", None), + field_name="RelationTargetGrounder.capability", + ) + affordance_type = getattr(grounder_type, "affordance_type", None) + if not isinstance(affordance_type, type) or not issubclass( + affordance_type, Affordance + ): + raise TypeError( + "RelationTargetGrounder.affordance_type must be an " + "Affordance subclass." + ) + revision = _validate_identifier( + getattr(grounder_type, "affordance_revision", None), + field_name="RelationTargetGrounder.affordance_revision", + ) + key = (capability, affordance_type, revision) + if key in normalized_grounders: + raise ValueError(f"Duplicate relation grounder key {key!r}.") + normalized_grounders[key] = grounder + if isinstance(handover_pose_providers, (str, bytes)): + raise TypeError("handover_pose_providers must be an iterable of providers.") + try: + supplied_handover_providers = tuple(handover_pose_providers) + except TypeError as exc: + raise TypeError( + "handover_pose_providers must be an iterable of providers." + ) from exc + normalized_handover_providers: dict[str, HandOverPoseProvider] = {} + for provider in supplied_handover_providers: + if not isinstance(provider, HandOverPoseProvider): + raise TypeError( + "handover_pose_providers must contain " + "HandOverPoseProvider instances." + ) + provider_id = _validate_identifier( + getattr(type(provider), "provider_id", None), + field_name="HandOverPoseProvider.provider_id", + ) + if provider_id in normalized_handover_providers: + raise ValueError(f"Duplicate handover pose provider {provider_id!r}.") + normalized_handover_providers[provider_id] = provider + self._integration = integration + self._compiler_id = uuid4().hex + self._registered_lowerers = MappingProxyType(lowerers) + self._relation_grounders = MappingProxyType(normalized_grounders) + self._handover_pose_providers = MappingProxyType(normalized_handover_providers) + selected_monitor_registry = ( + EffectMonitorRegistry((CompositeEffectMonitorFactory(),)) + if effect_monitor_registry is None + else effect_monitor_registry + ) + if not isinstance(selected_monitor_registry, EffectMonitorRegistry): + raise TypeError( + "effect_monitor_registry must be an EffectMonitorRegistry or None." + ) + self._effect_monitor_registry = selected_monitor_registry + + @property + def integration(self) -> BoundSemanticIntegration: + """Return the exact live integration used for linking and grounding.""" + return self._integration + + @property + def registered_lowerers(self) -> Mapping[str, RegisteredSemanticLowerer]: + """Return installed registered-call lowerers by stable call ID.""" + return self._registered_lowerers + + @property + def relation_grounders( + self, + ) -> Mapping[tuple[str, type[Affordance], str], RelationTargetGrounder]: + """Return exact typed/versioned relation grounders.""" + return self._relation_grounders + + @property + def handover_pose_providers(self) -> Mapping[str, HandOverPoseProvider]: + """Return installed handover pose providers by stable provider ID.""" + return self._handover_pose_providers + + @property + def effect_monitor_registry(self) -> EffectMonitorRegistry: + """Return the immutable versioned effect-monitor factory registry.""" + return self._effect_monitor_registry + + def analyze( + self, + calls: Iterable[SemanticCallSpec], + *, + workflow_id: str = "semantic_workflow", + path: tuple[PathPart, ...] = ("workflow",), + ) -> SemanticWorkflow: + """Statically link calls and infer look-ahead/effect dependencies. + + Args: + calls: Ordered exact semantic call values. + workflow_id: Stable caller-selected workflow identifier. + path: Root diagnostic path. + + Returns: + Factory-owned provider-free workflow analysis. + + Raises: + SemanticValidationError: If linking, grounding capabilities, or + object-state flow are invalid. + """ + _validate_identifier(workflow_id, field_name="workflow_id") + self._assert_current(path=("integration", "robot_profile")) + if isinstance(calls, (str, bytes)): + raise TypeError("calls must be an iterable of semantic call values.") + try: + supplied = tuple(calls) + except TypeError as exc: + raise TypeError( + "calls must be an iterable of semantic call values." + ) from exc + if not supplied: + raise ValueError("Semantic workflow requires at least one call.") + allowed_types = ( + Pick, + Place, + HandOver, + OperateArticulation, + RegisteredSemanticCall, + ) + if not all(type(call) in allowed_types for call in supplied): + raise TypeError("calls must contain exact supported semantic call values.") + + bound_calls: list[BoundSemanticCall] = [] + effect_kinds: list[SemanticEffectKind] = [] + dependencies: list[SemanticEffectDependency] = [] + latest_holder: dict[str, tuple[int, str]] = {} + for index, call in enumerate(supplied): + if type(call) is RegisteredSemanticCall and ( + call.call_id not in self._registered_lowerers + ): + raise _diagnostic( + "semantic_lowerer_not_installed", + (*path, index, "kind"), + f"Registered semantic call {call.call_id!r} has no explicitly " + "installed compiler lowerer.", + tuple(self._registered_lowerers), + ) + call = self._inherit_held_resource(call, latest_holder) + bound = self._integration.link_call( + call, + path=(*path, index, "call"), + ) + bound_calls.append(bound) + call = bound.linked.call + if type(call) is HandOver: + self._require_handover_pose_provider( + call, + path=(*path, index, "call"), + ) + if type(call) is Place and call.at is None: + target = self._relation_target(bound) + assert target.relation is not None + destination_metadata = self._integration.manifest.scene.lookup( + target.relation.affordance, + expected_type=SceneAffordanceRef, + ) + if destination_metadata.parent == call.object: + raise _diagnostic( + "place_self_reference", + (*path, index, "call", "destination"), + f"Object {call.object.entity_id!r} cannot be placed in a " + "relation to its own affordance.", + ) + self._require_relation_grounder( + target.relation, + path=(*path, index, "call", "destination"), + ) + if type(call) is Pick: + previous = latest_holder.get(call.object.entity_id) + if previous is not None: + raise _diagnostic( + "invalid_object_state_flow", + (*path, index, "call", "object"), + f"Object {call.object.entity_id!r} is already acquired by " + f"call {previous[0]} without an intervening release.", + ) + effect_kind = SemanticEffectKind.ATTACH + latest_holder[call.object.entity_id] = ( + index, + bound.binding.resource_ids["primary"], + ) + elif type(call) is Place: + producer = latest_holder.get(call.object.entity_id) + selected_resource = bound.binding.resource_ids["primary"] + if producer is not None and producer[1] != selected_resource: + raise _diagnostic( + "held_resource_mismatch", + (*path, index, "call", "resources", "primary"), + f"Place selects resource {selected_resource!r}, but the " + f"verified producer selects {producer[1]!r}.", + (producer[1],), + ) + effect_kind = SemanticEffectKind.RELEASE + dependencies.append( + SemanticEffectDependency( + producer_index=None if producer is None else producer[0], + consumer_index=index, + object=call.object, + ) + ) + latest_holder.pop(call.object.entity_id, None) + elif type(call) is HandOver: + producer = latest_holder.get(call.object.entity_id) + source_resource = bound.binding.resource_ids["source"] + if producer is not None and producer[1] != source_resource: + raise _diagnostic( + "held_resource_mismatch", + (*path, index, "call", "resources", "source"), + f"HandOver selects source {source_resource!r}, but the " + f"verified producer selects {producer[1]!r}.", + (producer[1],), + ) + effect_kind = SemanticEffectKind.TRANSFER + dependencies.append( + SemanticEffectDependency( + producer_index=None if producer is None else producer[0], + consumer_index=index, + object=call.object, + ) + ) + latest_holder[call.object.entity_id] = ( + index, + bound.binding.resource_ids["destination"], + ) + elif type(call) is OperateArticulation: + effect_kind = SemanticEffectKind.ARTICULATION + else: + effect_kind = SemanticEffectKind.REGISTERED + # A registered extension has no declarative state-flow contract + # in Version 1. Treat it as an opaque effect boundary. + latest_holder.clear() + effect_kinds.append(effect_kind) + + analyzed: list[AnalyzedSemanticCall] = [] + for index, (bound, effect_kind) in enumerate( + zip(bound_calls, effect_kinds, strict=True) + ): + call = bound.linked.call + requires_held = type(call) in (Place, HandOver) + downstream_targets = ( + self._downstream_targets(index, bound_calls) + if type(call) is Pick + else () + ) + effect_monitor_ref = self._effect_monitor_ref( + bound, + effect_kind, + path=(*path, index, "effect_monitor"), + ) + symbolic_writes, opaque_symbolic_effect = self._static_symbolic_writes( + bound, + path=(*path, index, "call"), + ) + analyzed.append( + AnalyzedSemanticCall( + index=index, + bound=bound, + effect_kind=effect_kind, + symbolic_writes=symbolic_writes, + opaque_symbolic_effect=opaque_symbolic_effect, + effect_monitor_ref=effect_monitor_ref, + downstream_object_targets=downstream_targets, + requires_verified_held_object=requires_held, + ) + ) + return SemanticWorkflow( + workflow_id=workflow_id, + calls=tuple(analyzed), + effect_dependencies=tuple(dependencies), + _compiler_id=self._compiler_id, + ) + + @staticmethod + def _inherit_held_resource( + call: SemanticCallSpec, + latest_holder: Mapping[str, tuple[int, str]], + ) -> SemanticCallSpec: + """Fill an omitted consumer slot from the workflow's known holder.""" + if type(call) is Place: + slot_id = "primary" + elif type(call) is HandOver: + slot_id = "source" + else: + return call + holder = latest_holder.get(call.object.entity_id) + if holder is None or slot_id in call.resources: + return call + resources = dict(call.resources) + resources[slot_id] = holder[1] + return replace(call, resources=resources) + + def _static_symbolic_writes( + self, + bound: BoundSemanticCall, + *, + path: tuple[PathPart, ...], + ) -> tuple[frozenset[SymbolicStateKey], bool]: + """Return exact provider-free ``TaskState`` keys for one linked call. + + Curated calls own these contracts. Registered calls remain an opaque + physical-effect boundary until their public descriptor grows an + explicit static-effect contract; lowering arguments are never guessed. + Conditional coordinated-held cleanup is likewise omitted because its + exact pair keys depend on the verified input ``TaskState``. + """ + call = bound.linked.call + if type(call) in (Pick, Place): + return ( + frozenset( + { + SymbolicStateKey.held_object( + self._participant_task_state_key( + bound, + slot_id="primary", + path=(*path, "resources", "primary"), + ) + ) + } + ), + False, + ) + if type(call) is HandOver: + return ( + frozenset( + SymbolicStateKey.held_object( + self._participant_task_state_key( + bound, + slot_id=slot_id, + path=(*path, "resources", slot_id), + ) + ) + for slot_id in ("source", "destination") + ), + False, + ) + if type(call) is OperateArticulation: + handle_ref = bound.linked.affordances.get("handle") + if handle_ref is None: + raise AssertionError( + "Linked articulation call lacks an operation affordance." + ) + registration = self._integration.scene_registry.lookup( + handle_ref, + expected_type=SceneAffordanceRef, + ) + affordance = registration.affordance + if ( + type(affordance) is not ArticulationOperationAffordance + or ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY + not in registration.affordance_capabilities + ): + raise _diagnostic( + "invalid_articulation_affordance", + (*path, "handle"), + f"Handle {handle_ref.entity_id!r} must expose an exact " + "ArticulationOperationAffordance payload and the articulation " + "operation capability.", + ) + return ( + frozenset( + { + SymbolicStateKey.articulation_joint( + call.articulation.entity_id, + affordance.joint_id, + ) + } + ), + False, + ) + if type(call) is RegisteredSemanticCall: + return frozenset(), True + raise AssertionError(f"Unsupported linked call {type(call).__name__}.") + + @staticmethod + def _participant_task_state_key( + bound: BoundSemanticCall, + *, + slot_id: str, + path: tuple[PathPart, ...], + ) -> str: + """Resolve the exact held-object key shared by participant endpoints.""" + resource = bound.binding.resources.get(slot_id) + if resource is None: + raise _diagnostic( + "missing_effect_resource", + path, + f"Held-object effects require bound resource slot {slot_id!r}.", + tuple(bound.binding.resources), + ) + motion_endpoint = resource.endpoints.get("motion") + grasp_endpoint = resource.endpoints.get("grasp") + if motion_endpoint is None or grasp_endpoint is None: + raise _diagnostic( + "missing_effect_endpoint", + (*path, "endpoints"), + "Held-object effects require bound motion and grasp endpoints.", + tuple(resource.endpoints), + ) + task_state_key = motion_endpoint.task_state_key + assert isinstance(task_state_key, str) + if grasp_endpoint.task_state_key != task_state_key: + raise _diagnostic( + "effect_state_key_mismatch", + (*path, "task_state_key"), + "Motion and grasp endpoints for one participant must share one " + "logical task-state key.", + ) + return task_state_key + + def ground( + self, + workflow: SemanticWorkflow, + call_index: int, + context: PlanningContext, + *, + eligible_mask: torch.Tensor | None = None, + revision: int = 0, + path: tuple[PathPart, ...] = ("workflow",), + ) -> GroundedSemanticCall: + """Lower one analyzed call from the latest immutable observation. + + Args: + workflow: Workflow created by this compiler. + call_index: Zero-based call index to lower. + context: Latest immutable planning observation. + eligible_mask: Rows still eligible to execute this call. + revision: Monotonic revision for re-grounding the same invocation. + path: Root diagnostic path. + + Returns: + Invocation and execution eligibility. + + Raises: + SemanticValidationError: If workflow ownership, live integration, + grounding, or verified state is invalid. + """ + if type(workflow) is not SemanticWorkflow: + raise TypeError("workflow must be exactly SemanticWorkflow.") + if type(call_index) is not int or not 0 <= call_index < len(workflow.calls): + raise IndexError(f"call_index {call_index!r} is outside the workflow.") + if type(context) is not PlanningContext: + raise TypeError("context must be exactly PlanningContext.") + if type(revision) is not int or revision < 0: + raise ValueError("revision must be a non-negative integer.") + self._assert_workflow_current(workflow, path=path) + self._integration.engine._validate_context(context) + eligible = self._normalize_eligible_mask(eligible_mask, context) + analyzed = workflow.calls[call_index] + call = analyzed.call + if type(call) is Pick: + lowering = self._lower_pick(analyzed, context) + elif type(call) is Place: + lowering = self._lower_place(analyzed, context, eligible, path=path) + elif type(call) is HandOver: + lowering = self._lower_handover(analyzed, context, eligible, path=path) + elif type(call) is OperateArticulation: + lowering = self._lower_operate_articulation( + analyzed, + context, + path=path, + ) + elif type(call) is RegisteredSemanticCall: + lowering = self._lower_registered(analyzed, context, path=path) + else: # pragma: no cover - exact workflow construction prevents this + raise AssertionError(f"Unsupported analyzed call {type(call).__name__}.") + + bound = analyzed.bound + if lowering.skill_options is None: + raise AssertionError( + "Semantic lowering must resolve a non-None action-options value." + ) + invocation = ActionInvocation( + skill_id=bound.linked.descriptor.skill_id, + goal=lowering.goal, + binding=bound.binding.action_binding, + motion_policy=bound.preset.motion_policy, + tracking_policy=bound.preset.tracking_policy, + recovery_policy=bound.preset.recovery_policy, + skill_options=lowering.skill_options, + control_overrides=lowering.control_overrides, + invocation_id=f"{workflow.workflow_id}:{call_index}", + revision=revision, + ) + effect_spec = self._ground_effect_spec( + analyzed, + invocation, + context, + path=(*path, call_index, "effect"), + ) + effect_monitor: EffectMonitor | None = None + if effect_spec is not None and analyzed.effect_monitor_ref is not None: + try: + effect_monitor = self._effect_monitor_registry.create( + effect_spec, + analyzed.effect_monitor_ref, + ) + except (KeyError, TypeError, ValueError) as exc: + raise _diagnostic( + "effect_monitor_creation_failed", + (*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"), + ) + effect_gates = self._ground_phase_effect_gates( + analyzed, + effect_spec, + path=(*path, call_index, "effect_gates"), + ) + if effect_gates: + invocation = replace( + invocation, + phase_effect_gates=tuple(value.requirement for value in effect_gates), + ) + return GroundedSemanticCall._create( + analyzed=analyzed, + invocation=invocation, + effect_spec=effect_spec, + effect_monitor=effect_monitor, + effect_guards=effect_guards, + effect_gates=effect_gates, + eligible_mask=eligible, + ) + + def _assert_current(self, *, path: tuple[PathPart, ...]) -> None: + """Reject a compiler after engine profile/catalog ownership changes.""" + engine = self._integration.engine + if engine.skill_profile is not self._integration.robot_profile: + raise _diagnostic( + "semantic_profile_stale", + path, + "The engine's canonical robot profile changed after compiler " + "construction.", + ) + try: + _ = self._integration.robot_profile.skills + except RuntimeError as exc: + raise _diagnostic( + "semantic_catalog_stale", + path, + str(exc), + ) from exc + + def _assert_workflow_current( + self, + workflow: SemanticWorkflow, + *, + path: tuple[PathPart, ...], + ) -> None: + """Ensure a workflow belongs to this still-current engine revision.""" + self._assert_current(path=("integration", "robot_profile")) + if workflow._compiler_id != self._compiler_id: + raise _diagnostic( + "semantic_program_stale", + path, + "The workflow belongs to a different compiler/grounder registry.", + ) + + @staticmethod + def _normalize_eligible_mask( + eligible_mask: torch.Tensor | None, + context: PlanningContext, + ) -> torch.Tensor: + """Return one owned per-row eligibility mask.""" + if eligible_mask is None: + return torch.ones( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ) + if not isinstance(eligible_mask, torch.Tensor): + raise TypeError("eligible_mask must be a torch.Tensor or None.") + if eligible_mask.dtype != torch.bool or eligible_mask.shape != ( + context.batch_size, + ): + raise ValueError( + "eligible_mask must be a bool tensor matching the context batch." + ) + if eligible_mask.device != context.robot.qpos.device: + raise ValueError("eligible_mask must use the context device.") + return eligible_mask.clone() + + def _validate_context(self, context: PlanningContext) -> None: + """Require the grounding observation to match the bound engine batch.""" + engine = self._integration.engine + if context.robot.robot_dof != engine.robot.dof: + raise ValueError( + "PlanningContext robot_dof must match the compiler engine, " + f"got {context.robot.robot_dof} and {engine.robot.dof}." + ) + engine_qpos = engine.robot.get_qpos() + if context.batch_size != int(engine_qpos.shape[0]): + raise ValueError( + "PlanningContext batch size must match the compiler engine, " + f"got {context.batch_size} and {engine_qpos.shape[0]}." + ) + if context.robot.qpos.device != engine.device: + raise ValueError("PlanningContext and compiler engine must share a device.") + + def _effect_monitor_ref( + self, + bound: BoundSemanticCall, + effect_kind: SemanticEffectKind, + *, + path: tuple[PathPart, ...], + ) -> EffectMonitorRef | None: + """Resolve one preset-owned exact monitor reference without creating it.""" + semantic_id = bound.linked.call.semantic_id + monitor_ref = bound.preset.effect_monitors.get(semantic_id) + if monitor_ref is None: + if type(bound.linked.call) in ( + Pick, + Place, + HandOver, + OperateArticulation, + ): + raise _diagnostic( + "missing_effect_monitor", + path, + f"Semantic call {semantic_id!r} requires an effect monitor " + f"for its {effect_kind.value!r} postcondition.", + tuple(bound.preset.effect_monitors), + ) + return None + if type(bound.linked.call) is RegisteredSemanticCall: + raise _diagnostic( + "registered_effect_contract_not_installed", + path, + f"Registered semantic call {semantic_id!r} selects an effect " + "monitor but no declarative effect-contract grounder is " + "installed.", + ) + try: + self._effect_monitor_registry.validate_ref(monitor_ref) + except KeyError as exc: + available = tuple( + f"{monitor_id}@{revision}" + for monitor_id, revision in self._effect_monitor_registry.factories + ) + raise _diagnostic( + "effect_monitor_not_installed", + path, + f"Effect monitor {monitor_ref.monitor_id!r} revision " + f"{monitor_ref.revision!r} is not installed.", + available, + ) from exc + except (TypeError, ValueError) as exc: + raise _diagnostic( + "invalid_effect_monitor_config", + path, + f"Effect monitor {monitor_ref.monitor_id!r} revision " + f"{monitor_ref.revision!r} has invalid configuration: {exc}", + ) from exc + return monitor_ref.snapshot() + + def _downstream_targets( + self, + pick_index: int, + bound_calls: list[BoundSemanticCall], + ) -> tuple[SemanticObjectTarget, ...]: + """Propagate object targets until the picked object is released.""" + pick = bound_calls[pick_index].linked.call + assert type(pick) is Pick + object_id = pick.object.entity_id + targets: list[SemanticObjectTarget] = [] + for call_index, bound in enumerate( + bound_calls[pick_index + 1 :], + start=pick_index + 1, + ): + call = bound.linked.call + if type(call) is RegisteredSemanticCall: + break + call_object = getattr(call, "object", None) + if type(call_object) is not SceneObjectRef or ( + call_object.entity_id != object_id + ): + continue + if type(call) is Pick: + break + if type(call) is HandOver: + provider_id, _ = self._require_handover_pose_provider( + call, + path=("workflow", call_index, "call"), + ) + targets.append( + SemanticObjectTarget( + SemanticHandOverTarget( + provider_id=provider_id, + bound=bound, + ) + ) + ) + break + if type(call) is Place: + if call.at is not None: + targets.append(SemanticObjectTarget(call.at)) + else: + targets.append(self._relation_target(bound)) + break + return tuple(targets) + + def _lower_pick( + self, + analyzed: AnalyzedSemanticCall, + context: PlanningContext, + ) -> SemanticLowering: + """Lower object-centric pickup and its downstream look-ahead.""" + call = analyzed.call + assert type(call) is Pick + grasp_ref = analyzed.bound.linked.affordances.get("grasp") + if grasp_ref is None: + raise AssertionError("Linked pick call lacks a grasp affordance.") + semantics = self._integration.scene_registry.object_semantics( + call.object, + affordance=grasp_ref, + ) + option_template = self._action_option_template(analyzed, PickUpOptions) + return SemanticLowering( + goal=GraspGoal(semantics=semantics), + skill_options=replace( + option_template, + downstream_object_target_poses=tuple( + self._ground_object_target(target, context) + for target in analyzed.downstream_object_targets + ), + ), + ) + + def _lower_place( + self, + analyzed: AnalyzedSemanticCall, + context: PlanningContext, + eligible: torch.Tensor, + *, + path: tuple[PathPart, ...], + ) -> SemanticLowering: + """Convert an object-space place target using verified held state.""" + call = analyzed.call + assert type(call) is Place + task_state_key, held = self._require_held_object( + analyzed, + context, + eligible, + slot_id="primary", + path=(*path, analyzed.index, "call"), + ) + del task_state_key + if call.at is not None: + object_target = self._broadcast_pose( + call.at.to_matrix(), + context, + name="Place.at", + ) + xpos: PoseGoalValue = torch.bmm(object_target, held.object_to_eef) + else: + object_target = self._ground_object_target( + self._relation_target(analyzed.bound), + context, + ) + xpos = self._compose_object_to_eef( + object_target, held.object_to_eef, context + ) + return SemanticLowering( + goal=PlaceGoal(xpos=xpos), + skill_options=self._action_option_template(analyzed, PlaceOptions), + ) + + def _lower_handover( + self, + analyzed: AnalyzedSemanticCall, + context: PlanningContext, + eligible: torch.Tensor, + *, + path: tuple[PathPart, ...], + ) -> SemanticLowering: + """Lower handover through an explicitly installed embodiment provider.""" + call = analyzed.call + assert type(call) is HandOver + self._require_held_object( + analyzed, + context, + eligible, + slot_id="source", + path=(*path, analyzed.index, "call"), + ) + _, provider = self._require_handover_pose_provider( + call, + path=(*path, analyzed.index, "call"), + ) + targets = self._resolve_handover_targets( + provider, + call, + context=context, + bound=analyzed.bound, + ) + grasp_ref = analyzed.bound.linked.affordances.get("receiver_grasp") + if grasp_ref is None: + raise AssertionError("Linked handover lacks receiver grasp affordance.") + semantics = self._integration.scene_registry.object_semantics( + call.object, + affordance=grasp_ref, + ) + middle = self._ground_object_target(targets.middle, context) + final_target = ( + SemanticObjectTarget(call.final_target) + if call.final_target is not None + else targets.final + ) + final = self._ground_object_target(final_target, context) + option_template = self._action_option_template(analyzed, HandOverOptions) + return SemanticLowering( + goal=GraspGoal(semantics=semantics), + skill_options=replace( + option_template, + middle_object_pose=middle, + final_object_pose=final, + ), + ) + + def _lower_operate_articulation( + self, + analyzed: AnalyzedSemanticCall, + context: PlanningContext, + *, + path: tuple[PathPart, ...], + ) -> SemanticLowering: + """Ground one handle operation from the latest scene snapshot.""" + call = analyzed.call + assert type(call) is OperateArticulation + handle_ref = analyzed.bound.linked.affordances.get("handle") + if handle_ref is None: + raise AssertionError( + "Linked articulation call lacks an operation affordance." + ) + registration = self._integration.scene_registry.lookup( + handle_ref, + expected_type=SceneAffordanceRef, + ) + affordance = registration.affordance + if ( + type(affordance) is not ArticulationOperationAffordance + or ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY + not in registration.affordance_capabilities + ): + raise _diagnostic( + "invalid_articulation_affordance", + (*path, analyzed.index, "call", "handle"), + f"Handle {handle_ref.entity_id!r} must expose an exact " + "ArticulationOperationAffordance payload and the articulation " + "operation capability.", + ) + + if call.target is not None: + try: + resolved_target = affordance.resolve_target(call.target) + except KeyError as exc: + raise _diagnostic( + "unknown_articulation_target", + (*path, analyzed.index, "call", "target"), + f"Handle {handle_ref.entity_id!r} has no semantic target " + f"{call.target!r}.", + tuple(affordance.semantic_targets), + ) from exc + target_position = resolved_target.target_position + displacement = resolved_target.displacement + else: + assert call.target_position is not None + assert call.target_displacement is not None + target_position = call.target_position + displacement = call.target_displacement + + try: + handle_state = context.scene.entities[handle_ref.entity_id] + except KeyError as exc: + raise _diagnostic( + "missing_handle_observation", + (*path, analyzed.index, "call", "handle"), + f"The current planning snapshot has no pose for handle " + f"{handle_ref.entity_id!r}.", + ) from exc + try: + self._broadcast_pose( + handle_state.pose, + context, + name=f"handle {handle_ref.entity_id!r}", + ) + except (TypeError, ValueError) as exc: + raise _diagnostic( + "articulation_grounding_failed", + (*path, analyzed.index, "call", "handle"), + f"Could not ground articulation handle geometry: {exc}", + ) from exc + joint_address = call.articulation.entity_id, affordance.joint_id + observed_joint = context.scene.get_articulation_joint_state(*joint_address) + if observed_joint is None: + raise _diagnostic( + "missing_articulation_joint_observation", + (*path, analyzed.index, "call", "articulation"), + "Recovery-safe articulation grounding requires a live " + "ObservedArticulationJointState for " + f"{joint_address!r} in the current scene snapshot.", + ) + try: + source_position = self._broadcast_joint_position( + observed_joint.position, + context, + name=f"articulation joint {joint_address!r}", + ) + except (TypeError, ValueError) as exc: + raise _diagnostic( + "invalid_articulation_joint_observation", + (*path, analyzed.index, "call", "articulation"), + f"Could not use live articulation joint state: {exc}", + ) from exc + if observed_joint.valid_mask is not None: + valid = observed_joint.valid_mask.to(device=context.robot.qpos.device) + if bool((~valid).any()): + rows = (~valid).nonzero(as_tuple=False).flatten().tolist() + raise _diagnostic( + "invalid_articulation_joint_observation", + (*path, analyzed.index, "call", "articulation"), + "Live articulation joint state is unavailable for planning " + f"rows {rows}.", + ) + target = torch.full( + (context.batch_size, 1), + target_position, + dtype=context.robot.qpos.dtype, + device=context.robot.qpos.device, + ) + return SemanticLowering( + goal=OperateArticulationGoal( + articulation_id=call.articulation.entity_id, + joint_id=affordance.joint_id, + geometry=SceneArticulationOperationGeometry( + handle_pose=SceneEntityPose(handle_ref.entity_id), + approach_offset=affordance.approach_offset, + contact_offset=affordance.contact_offset, + operation_offset=affordance.operation_offset, + retract_offset=affordance.retract_offset, + operation_axis=affordance.operation_axis, + position_scale=affordance.position_scale, + ), + source_position=source_position, + target_position=target, + target_displacement=displacement, + ), + skill_options=self._action_option_template( + analyzed, + OperateArticulationOptions, + ), + ) + + def _lower_registered( + self, + analyzed: AnalyzedSemanticCall, + context: PlanningContext, + *, + path: tuple[PathPart, ...], + ) -> SemanticLowering: + """Invoke one explicitly installed registered-call lowerer.""" + call = analyzed.call + assert type(call) is RegisteredSemanticCall + lowerer = self._registered_lowerers.get(call.call_id) + if lowerer is None: + raise _diagnostic( + "semantic_lowerer_not_installed", + (*path, analyzed.index, "call", "kind"), + f"No lowerer is installed for {call.call_id!r}.", + tuple(self._registered_lowerers), + ) + descriptor = analyzed.bound.linked.descriptor + target = descriptor.target_descriptor + assert target is not None + option_template = self._action_option_template( + analyzed, + target.options_type, + ) + lowering = lowerer.lower( + call, + context=context, + bound=analyzed.bound, + option_template=deepcopy(option_template), + ) + if type(lowering) is not SemanticLowering: + raise TypeError( + "RegisteredSemanticLowerer.lower() must return exactly " + "SemanticLowering." + ) + expected_goal_types = ( + target.goal_type + if isinstance(target.goal_type, tuple) + else (target.goal_type,) + ) + if type(lowering.goal) not in expected_goal_types: + raise TypeError( + f"Lowerer {call.call_id!r} produced {type(lowering.goal).__name__}; " + f"target skill {target.skill_id!r} expects {target.goal_type!r}." + ) + if lowering.skill_options is not None: + raise TypeError( + f"Lowerer {call.call_id!r} must not return skill_options; " + "the selected policy preset owns action options." + ) + return replace(lowering, skill_options=deepcopy(option_template)) + + @staticmethod + def _action_option_template( + analyzed: AnalyzedSemanticCall, + expected_type: type[OptionT], + ) -> OptionT: + """Return one owned exact template selected by semantic call ID.""" + semantic_id = analyzed.call.semantic_id + try: + template = analyzed.bound.preset.action_option_template(semantic_id) + except KeyError as exc: # pragma: no cover - static linking owns this check + raise AssertionError( + f"Linked call {semantic_id!r} has no action-option template." + ) from exc + if type(template) is not expected_type: + raise AssertionError( + f"Linked call {semantic_id!r} has {type(template).__name__}; " + f"expected exact {expected_type.__name__}." + ) + return template + + def _ground_effect_spec( + self, + analyzed: AnalyzedSemanticCall, + invocation: ActionInvocation, + context: PlanningContext, + *, + path: tuple[PathPart, ...], + ) -> SemanticEffectSpec | None: + """Ground typed symbolic state and raw-evidence clauses.""" + if analyzed.effect_monitor_ref is None: + return None + call = analyzed.call + state_expectations: list[EffectStateExpectation] = [] + clauses: list[EffectClause] = [] + if type(call) is Pick: + expectation, grounded_clauses = self._ground_held_effect( + analyzed, + expectation_id="destination", + relation=HeldObjectRelation.ATTACHED, + slot_id="primary", + object_id=call.object.entity_id, + context=context, + path=(*path, "state_expectations", "destination"), + ) + state_expectations.append(expectation) + clauses.extend(grounded_clauses) + state_expectations.extend( + self._coordinated_cleanup_expectations( + context, + task_state_keys=(expectation.task_state_key,), + ) + ) + elif type(call) is Place: + expectation, grounded_clauses = self._ground_held_effect( + analyzed, + expectation_id="source", + relation=HeldObjectRelation.DETACHED, + slot_id="primary", + object_id=call.object.entity_id, + context=context, + path=(*path, "state_expectations", "source"), + ) + state_expectations.append(expectation) + clauses.extend(grounded_clauses) + state_expectations.extend( + self._coordinated_cleanup_expectations( + context, + task_state_keys=(expectation.task_state_key,), + ) + ) + elif type(call) is HandOver: + source, source_clauses = self._ground_held_effect( + analyzed, + expectation_id="source", + relation=HeldObjectRelation.DETACHED, + slot_id="source", + object_id=call.object.entity_id, + context=context, + path=(*path, "state_expectations", "source"), + ) + destination, destination_clauses = self._ground_held_effect( + analyzed, + expectation_id="destination", + relation=HeldObjectRelation.ATTACHED, + slot_id="destination", + object_id=call.object.entity_id, + context=context, + path=(*path, "state_expectations", "destination"), + ) + state_expectations.extend((source, destination)) + clauses.extend((*source_clauses, *destination_clauses)) + elif type(call) is OperateArticulation: + goal = invocation.goal + if type(goal) is not OperateArticulationGoal: + raise AssertionError( + "OperateArticulation lowering produced an incompatible goal." + ) + expectation = ArticulationJointStateExpectation( + expectation_id="joint", + articulation_id=goal.articulation_id, + joint_id=goal.joint_id, + target_position=goal.target_position, + ) + source = EffectEvidenceSourceRef( + provider_id=SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, + revision=SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, + address=ArticulationJointEvidenceAddress( + articulation_id=goal.articulation_id, + joint_id=goal.joint_id, + ), + ) + state_expectations.append(expectation) + clauses.append( + JointStateEffectClause( + clause_id="joint.position", + expectation_id=expectation.expectation_id, + source=source, + target_position=goal.target_position, + ) + ) + else: # pragma: no cover - exact workflow construction prevents this + raise AssertionError(f"Unsupported analyzed call {type(call).__name__}.") + return SemanticEffectSpec( + semantic_id=call.semantic_id, + effect_kind=analyzed.effect_kind, + skill_id=invocation.skill_id, + invocation_id=invocation.invocation_id, + invocation_revision=invocation.revision, + env_ids=context.env_ids, + state_expectations=tuple(state_expectations), + clauses=tuple(clauses), + ) + + def _ground_phase_effect_gates( + self, + analyzed: AnalyzedSemanticCall, + effect_spec: SemanticEffectSpec | None, + *, + path: tuple[PathPart, ...], + ) -> tuple[GroundedPhaseEffectGate, ...]: + """Create blocking acquisition/release gates for built-in semantics.""" + monitor_ref = analyzed.effect_monitor_ref + if effect_spec is None or monitor_ref is None: + return () + call = analyzed.call + if type(call) is Pick: + definitions = (("destination_acquired", "lift", "destination"),) + elif type(call) is Place: + definitions = (("source_released", "retract", "source"),) + elif type(call) is HandOver: + definitions = (("destination_acquired", "release", "destination"),) + else: + return () + + gates: list[GroundedPhaseEffectGate] = [] + for gate_id, segment_name, expectation_id in definitions: + gate_spec = self._single_held_expectation_effect_spec( + effect_spec, + expectation_id=expectation_id, + ) + try: + monitor = self._effect_monitor_registry.create( + gate_spec, + monitor_ref, + ) + except (KeyError, TypeError, ValueError) as exc: + raise _diagnostic( + "effect_gate_monitor_creation_failed", + (*path, gate_id), + f"Could not create phase-effect gate monitor: {exc}", + ) from exc + gates.append( + GroundedPhaseEffectGate( + gate_id=gate_id, + segment_name=segment_name, + effect_spec=gate_spec, + effect_monitor=monitor, + retry_action=True, + ) + ) + return tuple(gates) + + @staticmethod + def _single_held_expectation_effect_spec( + terminal_spec: SemanticEffectSpec, + *, + expectation_id: str, + ) -> SemanticEffectSpec: + """Project one terminal held relation into an independent gate spec.""" + expectation = terminal_spec.state_expectation(expectation_id) + if type(expectation) is not HeldObjectStateExpectation: + raise TypeError("Phase-effect gates require held-object expectations.") + clauses = tuple( + clause + for clause in terminal_spec.clauses + if clause.expectation_id == expectation_id + ) + if not clauses: + raise ValueError( + f"Held-object expectation {expectation_id!r} has no physical clauses." + ) + effect_kind = ( + SemanticEffectKind.ATTACH + if expectation.relation is HeldObjectRelation.ATTACHED + else SemanticEffectKind.RELEASE + ) + return SemanticEffectSpec( + semantic_id=terminal_spec.semantic_id, + effect_kind=effect_kind, + 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=(expectation,), + clauses=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, + *, + task_state_keys: tuple[str, ...], + ) -> tuple[CoordinatedHeldObjectCleanupExpectation, ...]: + """Declare the exact coordinated relations a primitive must remove.""" + related = set(task_state_keys) + return tuple( + CoordinatedHeldObjectCleanupExpectation( + expectation_id=f"cleanup:{resources[0]}:{resources[1]}", + task_state_keys=resources, + ) + for resources in context.task.coordinated_held_objects + if not set(resources).isdisjoint(related) + ) + + @staticmethod + def _effect_source( + sources: Mapping[str, EffectEvidenceSourceRef], + channel: str, + *, + path: tuple[PathPart, ...], + ) -> EffectEvidenceSourceRef: + """Resolve one exact endpoint-owned observation source.""" + source = sources.get(channel) + if source is None: + raise _diagnostic( + "missing_effect_source", + (*path, "effect_sources", channel), + f"The endpoint does not expose required effect channel {channel!r}.", + tuple(sources), + ) + return source.snapshot() + + def _ground_held_effect( + self, + analyzed: AnalyzedSemanticCall, + *, + expectation_id: str, + relation: HeldObjectRelation, + slot_id: str, + object_id: str, + context: PlanningContext, + path: tuple[PathPart, ...], + ) -> tuple[HeldObjectStateExpectation, tuple[EffectClause, ...]]: + """Bind one held-object state relation to generic endpoint sources.""" + resource = analyzed.bound.binding.resources[slot_id] + motion_endpoint = resource.endpoints.get("motion") + grasp_endpoint = resource.endpoints.get("grasp") + if motion_endpoint is None or grasp_endpoint is None: + raise _diagnostic( + "missing_effect_endpoint", + (*path, "endpoints"), + "Held-object effects require bound motion and grasp endpoints.", + tuple(resource.endpoints), + ) + task_state_key = motion_endpoint.task_state_key + assert isinstance(task_state_key, str) + if grasp_endpoint.task_state_key != task_state_key: + raise _diagnostic( + "effect_state_key_mismatch", + (*path, "task_state_key"), + "Motion and grasp endpoints for one participant must share one " + "logical task-state key.", + ) + baseline: torch.Tensor | None = None + if relation is HeldObjectRelation.DETACHED: + held = context.task.get_held_object(task_state_key) + if held is None or held.semantics.entity_id != object_id: + raise _diagnostic( + "verified_held_object_required", + (*path, "baseline"), + f"Detached relation requires verified object {object_id!r} " + f"held under logical state key {task_state_key!r}.", + ) + baseline = held.object_to_eef + state_expectation = HeldObjectStateExpectation( + expectation_id=expectation_id, + relation=relation, + object_id=object_id, + slot_id=slot_id, + resource_id=resource.resource_id, + task_state_key=task_state_key, + ) + pose_source = self._effect_source( + motion_endpoint.effect_sources, + POSE_RELATION_EFFECT_CHANNEL, + path=(*path, "motion"), + ) + binary_channel = ( + CONSTRAINT_EFFECT_CHANNEL + if CONSTRAINT_EFFECT_CHANNEL in grasp_endpoint.effect_sources + else CONTACT_EFFECT_CHANNEL + ) + binary_source = self._effect_source( + grasp_endpoint.effect_sources, + binary_channel, + path=(*path, "grasp"), + ) + pose_clause = PoseRelationClause( + clause_id=f"{expectation_id}.pose", + expectation_id=expectation_id, + source=pose_source, + expectation=( + PoseRelationExpectation.MATCHED + if relation is HeldObjectRelation.ATTACHED + else PoseRelationExpectation.SEPARATED + ), + baseline_object_to_endpoint=baseline, + ) + binary_kind = ( + BinaryEvidenceKind.CONSTRAINT + if binary_channel == CONSTRAINT_EFFECT_CHANNEL + else BinaryEvidenceKind.CONTACT + ) + binary_clause = BinaryEffectClause( + clause_id=f"{expectation_id}.{binary_kind.value}", + expectation_id=expectation_id, + source=binary_source, + evidence_kind=binary_kind, + expected=relation is HeldObjectRelation.ATTACHED, + ) + return state_expectation, (pose_clause, binary_clause) + + def _relation_target( + self, + bound: BoundSemanticCall, + ) -> SemanticObjectTarget: + """Describe a linked placement relation without observing providers.""" + call = bound.linked.call + assert type(call) is Place and call.at is None + capability = ( + PLACE_ON_AFFORDANCE_CAPABILITY + if call.on is not None + else PLACE_IN_AFFORDANCE_CAPABILITY + ) + affordance_ref = bound.linked.affordances.get("destination") + if affordance_ref is None: + raise AssertionError("Linked relation place lacks destination affordance.") + metadata = self._integration.manifest.scene.lookup( + affordance_ref, + expected_type=SceneAffordanceRef, + ) + if ( + metadata.affordance_payload_type is None + or metadata.affordance_revision is None + ): + raise AssertionError( + "Capability-bearing relation affordance lacks payload metadata." + ) + return SemanticObjectTarget( + SemanticRelationTarget( + capability=capability, + affordance=affordance_ref, + payload_type=metadata.affordance_payload_type, + payload_revision=metadata.affordance_revision, + ) + ) + + def _require_relation_grounder( + self, + relation: SemanticRelationTarget | None, + *, + path: tuple[PathPart, ...], + ) -> RelationTargetGrounder: + """Resolve one exact relation grounder or fail during static analysis.""" + assert relation is not None + grounder = self._relation_grounders.get(relation.grounder_key) + if grounder is None: + candidates = tuple( + f"{capability}:{payload_type.__name__}:{revision}" + for capability, payload_type, revision in self._relation_grounders + ) + raise _diagnostic( + "relation_grounder_not_installed", + path, + "No relation target grounder is installed for " + f"{relation.capability!r}, {relation.payload_type.__name__}, " + f"revision {relation.payload_revision!r}.", + candidates, + ) + return grounder + + def _ground_object_target( + self, + target: SemanticObjectTarget, + context: PlanningContext, + ) -> PoseGoalValue: + """Ground a direct pose or dispatch one typed relation grounder.""" + if type(target.pose) is SemanticPose: + return target.pose.to_matrix() + if type(target.pose) is SceneEntityPose: + return target.pose + deferred_handover = target.handover + if deferred_handover is not None: + call = deferred_handover.bound.linked.call + assert type(call) is HandOver + provider_id, provider = self._require_handover_pose_provider( + call, + path=("handover", "provider"), + ) + if provider_id != deferred_handover.provider_id: + raise _diagnostic( + "semantic_program_stale", + ("handover", "provider"), + "The profile-selected handover provider changed after " + "workflow analysis.", + ) + targets = self._resolve_handover_targets( + provider, + call, + context=context, + bound=deferred_handover.bound, + ) + return self._ground_object_target(targets.middle, context) + relation = target.relation + assert relation is not None + grounder = self._require_relation_grounder( + relation, + path=("relation", relation.affordance.entity_id), + ) + registration = self._integration.scene_registry.lookup( + relation.affordance, + expected_type=SceneAffordanceRef, + ) + affordance = registration.affordance + assert affordance is not None + if ( + type(affordance) is not relation.payload_type + or registration.affordance_revision != relation.payload_revision + or relation.capability not in registration.affordance_capabilities + ): + raise TypeError( + "Semantic relation target does not match the exact live " + "affordance type, capability, and revision." + ) + pose_goal = grounder.ground( + relation, + affordance=affordance, + context=context, + ) + if type(pose_goal) is not SceneEntityPose and not isinstance( + pose_goal, torch.Tensor + ): + raise TypeError( + "RelationTargetGrounder.ground() must return a torch.Tensor or " + "exact SceneEntityPose." + ) + return pose_goal + + def _require_handover_pose_provider( + self, + call: HandOver, + *, + path: tuple[PathPart, ...], + ) -> tuple[str, HandOverPoseProvider]: + """Resolve the profile-selected named handover grounding provider.""" + provider_id = ( + self._integration.robot_profile.source_profile.grounding_providers.get( + call.semantic_id + ) + ) + if provider_id is None: + raise _diagnostic( + "handover_grounding_unconfigured", + path, + "The robot profile must select a named grounding provider for " + f"semantic call {call.semantic_id!r}.", + tuple(self._handover_pose_providers), + ) + provider = self._handover_pose_providers.get(provider_id) + if provider is None: + raise _diagnostic( + "handover_grounding_provider_not_installed", + path, + f"Robot profile selects handover provider {provider_id!r}, but " + "the compiler did not install it.", + tuple(self._handover_pose_providers), + ) + return provider_id, provider + + @staticmethod + def _resolve_handover_targets( + provider: HandOverPoseProvider, + call: HandOver, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> HandOverPoseTargets: + """Run one provider and reject recursive deferred target values.""" + targets = provider.resolve(call, context=context, bound=bound) + if type(targets) is not HandOverPoseTargets: + raise TypeError( + "HandOverPoseProvider.resolve() must return exactly " + "HandOverPoseTargets." + ) + if targets.middle.handover is not None or targets.final.handover is not None: + raise TypeError( + "HandOverPoseProvider targets cannot recursively defer to another " + "handover provider." + ) + return targets + + def _compose_object_to_eef( + self, + object_target: PoseGoalValue, + object_to_eef: torch.Tensor, + context: PlanningContext, + ) -> PoseGoalValue: + """Compose a relation-grounded object target with verified held state.""" + if isinstance(object_target, torch.Tensor): + return torch.bmm( + self._broadcast_pose(object_target, context, name="relation target"), + object_to_eef, + ) + relative = object_target.relative_pose + if relative is None: + composed = object_to_eef.clone() + else: + composed = torch.bmm( + self._broadcast_pose(relative, context, name="relation offset"), + object_to_eef, + ) + return SceneEntityPose( + object_target.entity_id, + relative_pose=composed, + minimum_confidence=object_target.minimum_confidence, + ) + + def _require_held_object( + self, + analyzed: AnalyzedSemanticCall, + context: PlanningContext, + eligible: torch.Tensor, + *, + slot_id: str, + path: tuple[PathPart, ...], + ) -> tuple[str, HeldObjectState]: + """Resolve the logical participant key and verify held-object identity.""" + resource = analyzed.bound.binding.resources[slot_id] + endpoint = resource.endpoints.get("motion") + if endpoint is None: + raise _diagnostic( + "missing_effect_endpoint", + (*path, "resources", slot_id, "motion"), + "The semantic lowerer requires a bound motion endpoint.", + tuple(resource.endpoints), + ) + task_state_key = endpoint.task_state_key + assert isinstance(task_state_key, str) + held = context.task.get_held_object(task_state_key) + call_object = getattr(analyzed.call, "object", None) + assert type(call_object) is SceneObjectRef + if held is None or held.semantics.entity_id != call_object.entity_id: + raise _diagnostic( + "verified_held_object_required", + (*path, "object"), + f"Call requires verified object {call_object.entity_id!r} held by " + f"logical state key {task_state_key!r}.", + ) + assert held.env_mask is not None + missing = eligible & ~held.env_mask + if missing.any(): + missing_env_ids = tuple( + str(value) + for value in context.env_ids[missing].detach().to("cpu").tolist() + ) + raise _diagnostic( + "verified_held_object_required", + (*path, "object"), + f"Object {call_object.entity_id!r} is not verified as held in " + "every eligible environment.", + missing_env_ids, + ) + return task_state_key, held + + @staticmethod + def _broadcast_pose( + pose: torch.Tensor, + context: PlanningContext, + *, + name: str, + ) -> torch.Tensor: + """Move and broadcast one object-space pose to the planning batch.""" + pose = pose.to(device=context.robot.qpos.device, dtype=torch.float32) + if pose.shape == (4, 4): + return pose.unsqueeze(0).expand(context.batch_size, -1, -1).clone() + if pose.shape != (context.batch_size, 4, 4): + raise ValueError( + f"{name} must have shape (4, 4) or " f"({context.batch_size}, 4, 4)." + ) + return pose.clone() + + @staticmethod + def _broadcast_joint_position( + position: torch.Tensor, + context: PlanningContext, + *, + name: str, + ) -> torch.Tensor: + """Move and broadcast one scalar articulation joint observation.""" + if not isinstance(position, torch.Tensor): + raise TypeError(f"{name} position must be a torch.Tensor.") + if not position.is_floating_point() or not torch.isfinite(position).all(): + raise ValueError(f"{name} position must be a finite floating tensor.") + position = position.to( + device=context.robot.qpos.device, + dtype=context.robot.qpos.dtype, + ) + if position.shape == (1,): + return position.unsqueeze(0).expand(context.batch_size, -1).clone() + if position.shape != (context.batch_size, 1): + raise ValueError( + f"{name} position must have shape (1,) or " + f"({context.batch_size}, 1)." + ) + return position.clone() + + +__all__ = [ + "AnalyzedSemanticCall", + "ContainerRelationTargetGrounder", + "GroundedHeldObjectGuard", + "GroundedPhaseEffectGate", + "GroundedSemanticCall", + "HandOverPoseProvider", + "HandOverPoseTargets", + "HeldObjectGuardBaseline", + "RelationTargetGrounder", + "RegisteredSemanticLowerer", + "SemanticEffectDependency", + "SemanticEffectKind", + "SemanticHandOverTarget", + "SemanticLowering", + "SemanticObjectTarget", + "SemanticRelationTarget", + "SemanticSkillCompiler", + "SemanticWorkflow", + "SupportSurfaceRelationTargetGrounder", +] diff --git a/embodichain/lab/sim/skills/effects.py b/embodichain/lab/sim/skills/effects.py new file mode 100644 index 000000000..7bcbb4636 --- /dev/null +++ b/embodichain/lab/sim/skills/effects.py @@ -0,0 +1,2426 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Backend-neutral semantic-effect contracts, evidence, and monitors.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from collections.abc import Hashable, Iterable, Mapping +from copy import deepcopy +from dataclasses import dataclass, field, fields, is_dataclass +from enum import Enum +import math +from types import MappingProxyType +from typing import ClassVar, TypeAlias + +import torch + +from embodichain.lab.sim.atomic_actions.execution import EffectVerificationRequest +from embodichain.lab.sim.atomic_actions.state import ( + ArticulationJointState, + HeldObjectState, +) + +EffectMonitorParam: TypeAlias = ( + None + | bool + | int + | float + | str + | tuple["EffectMonitorParam", ...] + | Mapping[str, "EffectMonitorParam"] +) +"""Recursively immutable, non-executable monitor configuration value.""" + +COMPOSITE_EFFECT_MONITOR_ID = "builtin.composite_effect" +"""Stable ID of the built-in typed-clause monitor.""" + +COMPOSITE_EFFECT_MONITOR_REVISION = "1" +"""Exact behavior/configuration revision of the built-in monitor.""" + +CONTROL_PART_EVIDENCE_PROVIDER_ID = "builtin.control_part" +"""Stable provider ID used by generic control-part evidence addresses.""" + +CONTROL_PART_EVIDENCE_PROVIDER_REVISION = "1" +"""Exact contract revision of control-part evidence addresses.""" + +POSE_RELATION_EFFECT_CHANNEL = "pose_relation" +CONTACT_EFFECT_CHANNEL = "contact" +CONSTRAINT_EFFECT_CHANNEL = "constraint" +FORCE_EFFECT_CHANNEL = "force" +JOINT_STATE_EFFECT_CHANNEL = "joint_state" + +_EFFECT_CHANNELS = frozenset( + { + POSE_RELATION_EFFECT_CHANNEL, + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + FORCE_EFFECT_CHANNEL, + JOINT_STATE_EFFECT_CHANNEL, + } +) +_SE3_BASE_ATOL = 1.0e-5 +_SE3_EPS_MULTIPLIER = 10.0 + + +def _metadata_value(value: object) -> object: + """Convert one typed effect value to deterministic JSON-safe data.""" + if value is None or type(value) in (bool, int, str): + return value + if type(value) is float: + return value if math.isfinite(value) else None + if isinstance(value, Enum): + return value.value + if isinstance(value, torch.Tensor): + return _metadata_value(value.detach().cpu().tolist()) + if isinstance(value, Mapping): + return { + str(key): _metadata_value(nested) + for key, nested in sorted(value.items(), key=lambda item: str(item[0])) + } + if isinstance(value, (tuple, list)): + return [_metadata_value(nested) for nested in value] + if is_dataclass(value) and not isinstance(value, type): + return { + "type": f"{type(value).__module__}.{type(value).__qualname__}", + **{ + data_field.name: _metadata_value(getattr(value, data_field.name)) + for data_field in fields(value) + }, + } + return {"type": f"{type(value).__module__}.{type(value).__qualname__}"} + + +def _validate_identifier(value: str, *, field_name: str) -> str: + """Return one exact non-empty identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +def _snapshot_declarative_value( + value: object, + *, + path: str, + active: set[int] | None = None, + budget: list[int] | None = None, + depth: int = 0, +) -> EffectMonitorParam: + """Own one bounded, acyclic, non-executable declarative value.""" + if active is None: + active = set() + if budget is None: + budget = [4096] + if depth > 32: + raise ValueError(f"{path} exceeds the maximum declarative depth of 32.") + budget[0] -= 1 + if budget[0] < 0: + raise ValueError(f"{path} exceeds the maximum declarative node count.") + if value is None or type(value) in (bool, int, str): + return value + if type(value) is float: + if not math.isfinite(value): + raise ValueError(f"{path} must be finite.") + return value + if type(value) in (dict, MappingProxyType): + container_id = id(value) + if container_id in active: + raise ValueError(f"{path} contains a cyclic mapping.") + active.add(container_id) + try: + snapshot: dict[str, EffectMonitorParam] = {} + for key, nested in value.items(): + _validate_identifier(key, field_name=f"{path} keys") + snapshot[key] = _snapshot_declarative_value( + nested, + path=f"{path}.{key}", + active=active, + budget=budget, + depth=depth + 1, + ) + return MappingProxyType(snapshot) + finally: + active.remove(container_id) + if type(value) in (tuple, list): + container_id = id(value) + if container_id in active: + raise ValueError(f"{path} contains a cyclic sequence.") + active.add(container_id) + try: + return tuple( + _snapshot_declarative_value( + nested, + path=f"{path}[{index}]", + active=active, + budget=budget, + depth=depth + 1, + ) + for index, nested in enumerate(value) + ) + finally: + active.remove(container_id) + raise TypeError( + f"{path} contains non-declarative {type(value).__name__}; callables, " + "classes, tensors, and live objects are not allowed." + ) + + +def _snapshot_monitor_params( + values: Mapping[str, EffectMonitorParam], +) -> Mapping[str, EffectMonitorParam]: + """Validate and own a monitor-parameter mapping.""" + if type(values) not in (dict, MappingProxyType): + raise TypeError( + "EffectMonitorRef.params must be an exact dict or mapping proxy." + ) + snapshot = _snapshot_declarative_value(values, path="EffectMonitorRef.params") + assert isinstance(snapshot, Mapping) + return snapshot + + +def _validate_pose_batch( + value: torch.Tensor, + *, + field_name: str, + valid_mask: torch.Tensor | None = None, +) -> torch.Tensor: + """Validate and own unbatched or batched proper SE(3) transforms.""" + if not isinstance(value, torch.Tensor): + raise TypeError(f"{field_name} must be a torch.Tensor.") + if value.shape != (4, 4) and ( + value.dim() != 3 or value.shape[0] == 0 or value.shape[-2:] != (4, 4) + ): + raise ValueError(f"{field_name} must have shape (4, 4) or (B, 4, 4).") + if not value.is_floating_point(): + raise TypeError(f"{field_name} must use a floating-point dtype.") + poses = value.unsqueeze(0) if value.dim() == 2 else value + if valid_mask is not None: + if not isinstance(valid_mask, torch.Tensor): + raise TypeError("valid_mask must be a torch.Tensor.") + if valid_mask.dtype != torch.bool or valid_mask.shape != (poses.shape[0],): + raise ValueError("valid_mask must be a bool tensor with shape (B,).") + if valid_mask.device != poses.device: + raise ValueError("valid_mask and poses must share a device.") + poses = poses[valid_mask] + if poses.numel() == 0: + return value.clone() + if not torch.isfinite(poses).all(): + raise ValueError(f"{field_name} must contain only finite values.") + tolerance = max( + _SE3_BASE_ATOL, + _SE3_EPS_MULTIPLIER * float(torch.finfo(value.dtype).eps), + ) + checked = poses.to(dtype=torch.float64) + expected_bottom = checked.new_tensor((0.0, 0.0, 0.0, 1.0)) + if not torch.isclose( + checked[:, 3, :], + expected_bottom.expand(checked.shape[0], -1), + atol=tolerance, + rtol=0.0, + ).all(): + raise ValueError( + f"{field_name} must contain SE(3) transforms with homogeneous " + "bottom row [0, 0, 0, 1]." + ) + rotations = checked[:, :3, :3] + gram = rotations.transpose(-1, -2) @ rotations + identity = torch.eye(3, dtype=checked.dtype, device=checked.device).expand_as(gram) + if not torch.isclose(gram, identity, atol=tolerance, rtol=0.0).all(): + raise ValueError( + f"{field_name} must contain SE(3) transforms with orthonormal rotations." + ) + determinants = torch.linalg.det(rotations) + if not torch.isclose( + determinants, + torch.ones_like(determinants), + atol=tolerance, + rtol=0.0, + ).all(): + raise ValueError( + f"{field_name} must contain SE(3) transforms with rotation " + "determinant +1." + ) + return value.clone() + + +class SemanticEffectKind(str, Enum): + """Trace-level semantic effect category; clause types define behavior.""" + + ATTACH = "attach" + RELEASE = "release" + TRANSFER = "transfer" + ARTICULATION = "articulation" + REGISTERED = "registered" + + +class SymbolicStateDomain(str, Enum): + """Typed mapping domains owned by :class:`~atomic_actions.TaskState`.""" + + HELD_OBJECT = "held_object" + COORDINATED_HELD_OBJECT = "coordinated_held_object" + ARTICULATION_JOINT = "articulation_joint" + + +@dataclass(frozen=True, slots=True) +class SymbolicStateKey: + """Provider-free key for one exact symbolic ``TaskState`` write. + + The domain makes otherwise similar string and pair addresses impossible to + conflate during static parallel analysis. This contract intentionally + describes only exact keys; dynamic or opaque effects must not manufacture + a guessed key. + """ + + domain: SymbolicStateDomain + address: tuple[str, ...] + + def __post_init__(self) -> None: + if not isinstance(self.domain, SymbolicStateDomain): + raise TypeError("domain must be a SymbolicStateDomain.") + address = tuple(self.address) + expected_size = 1 if self.domain is SymbolicStateDomain.HELD_OBJECT else 2 + if len(address) != expected_size: + raise ValueError( + f"{self.domain.value} symbolic keys require exactly " + f"{expected_size} address component(s)." + ) + for component in address: + _validate_identifier( + component, + field_name=f"{self.domain.value} symbolic key components", + ) + object.__setattr__(self, "address", address) + + @classmethod + def held_object(cls, task_state_key: str) -> SymbolicStateKey: + """Build one held-object mapping key.""" + return cls(SymbolicStateDomain.HELD_OBJECT, (task_state_key,)) + + @classmethod + def coordinated_held_object( + cls, + first_task_state_key: str, + second_task_state_key: str, + ) -> SymbolicStateKey: + """Build one ordered coordinated-held-object mapping key.""" + return cls( + SymbolicStateDomain.COORDINATED_HELD_OBJECT, + (first_task_state_key, second_task_state_key), + ) + + @classmethod + def articulation_joint( + cls, + articulation_id: str, + joint_id: str, + ) -> SymbolicStateKey: + """Build one articulation-joint mapping key.""" + return cls( + SymbolicStateDomain.ARTICULATION_JOINT, + (articulation_id, joint_id), + ) + + @property + def rendered(self) -> str: + """Return a deterministic domain-qualified diagnostic form.""" + return f"{self.domain.value}[{', '.join(repr(item) for item in self.address)}]" + + +@dataclass(frozen=True, slots=True) +class EffectMonitorRef: + """Versioned, declarative reference to an effect-monitor factory.""" + + monitor_id: str + revision: str + params: Mapping[str, EffectMonitorParam] = field(default_factory=dict) + + def __post_init__(self) -> None: + _validate_identifier(self.monitor_id, field_name="EffectMonitorRef.monitor_id") + _validate_identifier(self.revision, field_name="EffectMonitorRef.revision") + object.__setattr__(self, "params", _snapshot_monitor_params(self.params)) + + def snapshot(self) -> EffectMonitorRef: + """Return an independently owned declarative reference.""" + return EffectMonitorRef(self.monitor_id, self.revision, self.params) + + def to_metadata(self) -> dict[str, object]: + """Return a deterministic JSON-safe monitor selection.""" + return { + "monitor_id": self.monitor_id, + "revision": self.revision, + "params": _metadata_value(self.params), + } + + +class EffectEvidenceAddress(ABC): + """Immutable observation address, deliberately separate from command targets.""" + + @property + @abstractmethod + def address_fingerprint(self) -> Hashable: + """Return a stable, hashable physical observation address.""" + + def snapshot(self) -> EffectEvidenceAddress: + """Return an independently owned address of the exact same type.""" + return deepcopy(self) + + +@dataclass(frozen=True, slots=True) +class ControlPartEvidenceAddress(EffectEvidenceAddress): + """Provider-neutral robot control-part observation address.""" + + control_part: str + channel: str + + def __post_init__(self) -> None: + _validate_identifier( + self.control_part, + field_name="ControlPartEvidenceAddress.control_part", + ) + _validate_identifier( + self.channel, field_name="ControlPartEvidenceAddress.channel" + ) + if self.channel not in _EFFECT_CHANNELS: + raise ValueError( + f"Unknown control-part effect channel {self.channel!r}; expected " + f"one of {sorted(_EFFECT_CHANNELS)}." + ) + + @property + def address_fingerprint(self) -> Hashable: + """Return the channel-scoped control-part observation address.""" + return type(self), self.control_part, self.channel + + +@dataclass(frozen=True, slots=True) +class EffectEvidenceSourceRef: + """Versioned provider route plus one immutable observation address.""" + + provider_id: str + revision: str + address: EffectEvidenceAddress + + def __post_init__(self) -> None: + _validate_identifier( + self.provider_id, + field_name="EffectEvidenceSourceRef.provider_id", + ) + _validate_identifier( + self.revision, + field_name="EffectEvidenceSourceRef.revision", + ) + if not isinstance(self.address, EffectEvidenceAddress): + raise TypeError( + "EffectEvidenceSourceRef.address must be an EffectEvidenceAddress." + ) + snapshot = self.address.snapshot() + if type(snapshot) is not type(self.address) or snapshot is self.address: + raise TypeError( + "EffectEvidenceAddress.snapshot() must return an independently " + "owned address of the same exact type." + ) + try: + source_fingerprint = self.address.address_fingerprint + snapshot_fingerprint = snapshot.address_fingerprint + hash(source_fingerprint) + hash(snapshot_fingerprint) + except TypeError as exc: + raise TypeError( + "EffectEvidenceAddress.address_fingerprint must be hashable." + ) from exc + if snapshot_fingerprint != source_fingerprint: + raise ValueError( + "EffectEvidenceAddress.snapshot() must preserve its fingerprint." + ) + object.__setattr__(self, "address", snapshot) + + @property + def source_fingerprint(self) -> Hashable: + """Return the provider-scoped source address fingerprint.""" + return ( + self.provider_id, + self.revision, + type(self.address), + self.address.address_fingerprint, + ) + + def snapshot(self) -> EffectEvidenceSourceRef: + """Return an independently owned source reference.""" + return EffectEvidenceSourceRef( + self.provider_id, + self.revision, + self.address, + ) + + def to_metadata(self) -> dict[str, object]: + """Return the versioned physical observation address as JSON-safe data.""" + return { + "provider_id": self.provider_id, + "revision": self.revision, + "address": _metadata_value(self.address), + } + + +class HeldObjectRelation(str, Enum): + """Expected symbolic held-object state at an effect boundary.""" + + ATTACHED = "attached" + DETACHED = "detached" + + +@dataclass(frozen=True, slots=True) +class HeldObjectStateExpectation: + """Typed individual held-object postcondition.""" + + expectation_id: str + relation: HeldObjectRelation + object_id: str + slot_id: str + resource_id: str + task_state_key: str + + def __post_init__(self) -> None: + for field_name in ( + "expectation_id", + "object_id", + "slot_id", + "resource_id", + "task_state_key", + ): + _validate_identifier( + getattr(self, field_name), + field_name=f"HeldObjectStateExpectation.{field_name}", + ) + if not isinstance(self.relation, HeldObjectRelation): + raise TypeError("relation must be a HeldObjectRelation.") + + def snapshot(self) -> HeldObjectStateExpectation: + """Return an independently constructed state expectation.""" + return HeldObjectStateExpectation( + self.expectation_id, + self.relation, + self.object_id, + self.slot_id, + self.resource_id, + self.task_state_key, + ) + + +@dataclass(frozen=True, slots=True) +class CoordinatedHeldObjectCleanupExpectation: + """Typed removal of one coordinated held-object relation.""" + + expectation_id: str + task_state_keys: tuple[str, str] + + def __post_init__(self) -> None: + _validate_identifier( + self.expectation_id, + field_name="CoordinatedHeldObjectCleanupExpectation.expectation_id", + ) + keys = tuple(self.task_state_keys) + if len(keys) != 2: + raise ValueError("task_state_keys must contain exactly two keys.") + for key in keys: + _validate_identifier(key, field_name="coordinated task-state keys") + if keys[0] == keys[1]: + raise ValueError("Coordinated task-state keys must be distinct.") + object.__setattr__(self, "task_state_keys", keys) + + def snapshot(self) -> CoordinatedHeldObjectCleanupExpectation: + """Return an independently constructed cleanup expectation.""" + return CoordinatedHeldObjectCleanupExpectation( + self.expectation_id, + self.task_state_keys, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class ArticulationJointStateExpectation: + """Future-compatible symbolic articulation-joint postcondition.""" + + expectation_id: str + articulation_id: str + joint_id: str + target_position: torch.Tensor + + def __post_init__(self) -> None: + for field_name in ("expectation_id", "articulation_id", "joint_id"): + _validate_identifier( + getattr(self, field_name), + field_name=f"ArticulationJointStateExpectation.{field_name}", + ) + target = self.target_position + if not isinstance(target, torch.Tensor) or target.dim() not in (1, 2): + raise ValueError("target_position must have shape (J,) or (B, J).") + if target.numel() == 0 or not target.is_floating_point(): + raise TypeError("target_position must be a non-empty floating tensor.") + if not torch.isfinite(target).all(): + raise ValueError("target_position must be finite.") + object.__setattr__(self, "target_position", target.clone()) + + def snapshot(self) -> ArticulationJointStateExpectation: + """Return an independently owned articulation expectation.""" + return ArticulationJointStateExpectation( + self.expectation_id, + self.articulation_id, + self.joint_id, + self.target_position, + ) + + +EffectStateExpectation: TypeAlias = ( + HeldObjectStateExpectation + | CoordinatedHeldObjectCleanupExpectation + | ArticulationJointStateExpectation +) + + +class PoseRelationExpectation(str, Enum): + """Expected relationship to a grounded pose baseline.""" + + MATCHED = "matched" + SEPARATED = "separated" + + +class BinaryEvidenceKind(str, Enum): + """Raw boolean evidence channel.""" + + CONTACT = "contact" + CONSTRAINT = "constraint" + + +class ScalarEvidenceKind(str, Enum): + """Raw scalar physical evidence channel.""" + + FORCE = "force" + WRENCH = "wrench" + + +class ScalarExpectation(str, Enum): + """Expected high/low magnitude band for scalar evidence.""" + + PRESENT = "present" + ABSENT = "absent" + + +def _validate_clause_identity( + clause_id: str, + expectation_id: str, + source: EffectEvidenceSourceRef, +) -> EffectEvidenceSourceRef: + """Validate common clause identity and own its source.""" + _validate_identifier(clause_id, field_name="effect clause_id") + _validate_identifier(expectation_id, field_name="effect expectation_id") + if not isinstance(source, EffectEvidenceSourceRef): + raise TypeError("effect clause source must be an EffectEvidenceSourceRef.") + return source.snapshot() + + +@dataclass(frozen=True, slots=True, eq=False) +class PoseRelationClause: + """Object-to-endpoint pose condition with monitor-owned tolerances.""" + + clause_id: str + expectation_id: str + source: EffectEvidenceSourceRef + expectation: PoseRelationExpectation + baseline_object_to_endpoint: torch.Tensor | None = None + + def __post_init__(self) -> None: + object.__setattr__( + self, + "source", + _validate_clause_identity( + self.clause_id, + self.expectation_id, + self.source, + ), + ) + if not isinstance(self.expectation, PoseRelationExpectation): + raise TypeError("expectation must be a PoseRelationExpectation.") + baseline = self.baseline_object_to_endpoint + if self.expectation is PoseRelationExpectation.SEPARATED: + if baseline is None: + raise ValueError("A separated pose clause requires a baseline.") + object.__setattr__( + self, + "baseline_object_to_endpoint", + _validate_pose_batch( + baseline, + field_name="PoseRelationClause.baseline_object_to_endpoint", + ), + ) + elif baseline is not None: + raise ValueError( + "A matched pose clause obtains its baseline from the expected " + "held-object StateDelta and must not embed one." + ) + + def snapshot(self) -> PoseRelationClause: + """Return an independently owned pose clause.""" + return PoseRelationClause( + self.clause_id, + self.expectation_id, + self.source, + self.expectation, + self.baseline_object_to_endpoint, + ) + + +@dataclass(frozen=True, slots=True) +class BinaryEffectClause: + """Raw contact or constraint-state condition.""" + + clause_id: str + expectation_id: str + source: EffectEvidenceSourceRef + evidence_kind: BinaryEvidenceKind + expected: bool + + def __post_init__(self) -> None: + object.__setattr__( + self, + "source", + _validate_clause_identity( + self.clause_id, + self.expectation_id, + self.source, + ), + ) + if not isinstance(self.evidence_kind, BinaryEvidenceKind): + raise TypeError("evidence_kind must be a BinaryEvidenceKind.") + if type(self.expected) is not bool: + raise TypeError("expected must be a bool.") + + def snapshot(self) -> BinaryEffectClause: + """Return an independently owned binary clause.""" + return BinaryEffectClause( + self.clause_id, + self.expectation_id, + self.source, + self.evidence_kind, + self.expected, + ) + + +@dataclass(frozen=True, slots=True) +class ScalarEffectClause: + """Raw force/wrench magnitude condition with monitor-owned thresholds.""" + + clause_id: str + expectation_id: str + source: EffectEvidenceSourceRef + evidence_kind: ScalarEvidenceKind + expectation: ScalarExpectation + + def __post_init__(self) -> None: + object.__setattr__( + self, + "source", + _validate_clause_identity( + self.clause_id, + self.expectation_id, + self.source, + ), + ) + if not isinstance(self.evidence_kind, ScalarEvidenceKind): + raise TypeError("evidence_kind must be a ScalarEvidenceKind.") + if not isinstance(self.expectation, ScalarExpectation): + raise TypeError("expectation must be a ScalarExpectation.") + + def snapshot(self) -> ScalarEffectClause: + """Return an independently owned scalar clause.""" + return ScalarEffectClause( + self.clause_id, + self.expectation_id, + self.source, + self.evidence_kind, + self.expectation, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class JointStateEffectClause: + """Raw articulation/robot joint-position target condition.""" + + clause_id: str + expectation_id: str + source: EffectEvidenceSourceRef + target_position: torch.Tensor + + def __post_init__(self) -> None: + object.__setattr__( + self, + "source", + _validate_clause_identity( + self.clause_id, + self.expectation_id, + self.source, + ), + ) + target = self.target_position + if not isinstance(target, torch.Tensor) or target.dim() not in (1, 2): + raise ValueError("target_position must have shape (J,) or (B, J).") + if target.numel() == 0 or not target.is_floating_point(): + raise TypeError("target_position must be a non-empty floating tensor.") + if not torch.isfinite(target).all(): + raise ValueError("target_position must be finite.") + object.__setattr__(self, "target_position", target.clone()) + + def snapshot(self) -> JointStateEffectClause: + """Return an independently owned joint-state clause.""" + return JointStateEffectClause( + self.clause_id, + self.expectation_id, + self.source, + self.target_position, + ) + + +EffectClause: TypeAlias = ( + PoseRelationClause + | BinaryEffectClause + | ScalarEffectClause + | JointStateEffectClause +) +_STATE_EXPECTATION_TYPES = ( + HeldObjectStateExpectation, + CoordinatedHeldObjectCleanupExpectation, + ArticulationJointStateExpectation, +) +_CLAUSE_TYPES = ( + PoseRelationClause, + BinaryEffectClause, + ScalarEffectClause, + JointStateEffectClause, +) + + +@dataclass(frozen=True, slots=True, eq=False) +class SemanticEffectSpec: + """Grounded typed physical clauses and symbolic postconditions for one call.""" + + semantic_id: str + effect_kind: SemanticEffectKind + skill_id: str + invocation_id: str | None + invocation_revision: int + env_ids: torch.Tensor + state_expectations: tuple[EffectStateExpectation, ...] + clauses: tuple[EffectClause, ...] + + def __post_init__(self) -> None: + _validate_identifier( + self.semantic_id, + field_name="SemanticEffectSpec.semantic_id", + ) + _validate_identifier(self.skill_id, field_name="SemanticEffectSpec.skill_id") + if not isinstance(self.effect_kind, SemanticEffectKind): + raise TypeError("effect_kind must be a SemanticEffectKind.") + if self.invocation_id is not None: + _validate_identifier( + self.invocation_id, + field_name="SemanticEffectSpec.invocation_id", + ) + if type(self.invocation_revision) is not int or self.invocation_revision < 0: + raise ValueError("invocation_revision must be a non-negative integer.") + if not isinstance(self.env_ids, torch.Tensor): + raise TypeError("env_ids must be a torch.Tensor.") + if self.env_ids.dtype != torch.long or self.env_ids.dim() != 1: + raise ValueError("env_ids must be a one-dimensional torch.long tensor.") + if self.env_ids.numel() == 0: + raise ValueError("env_ids must contain at least one environment ID.") + if torch.unique(self.env_ids).numel() != self.env_ids.numel(): + raise ValueError("env_ids must be unique.") + object.__setattr__(self, "env_ids", self.env_ids.clone()) + + expectations = tuple(self.state_expectations) + if not expectations or not all( + type(value) in _STATE_EXPECTATION_TYPES for value in expectations + ): + raise TypeError( + "state_expectations must contain exact typed state expectations." + ) + expectation_ids = [value.expectation_id for value in expectations] + if len(set(expectation_ids)) != len(expectation_ids): + raise ValueError("State expectation IDs must be unique.") + held_keys = [ + value.task_state_key + for value in expectations + if type(value) is HeldObjectStateExpectation + ] + if len(set(held_keys)) != len(held_keys): + raise ValueError("Held-object task-state keys must be unique.") + cleanup_keys = [ + value.task_state_keys + for value in expectations + if type(value) is CoordinatedHeldObjectCleanupExpectation + ] + if len(set(cleanup_keys)) != len(cleanup_keys): + raise ValueError("Coordinated cleanup keys must be unique.") + + clauses = tuple(self.clauses) + if not clauses or not all(type(value) in _CLAUSE_TYPES for value in clauses): + raise TypeError("clauses must contain exact typed effect clauses.") + clause_ids = [value.clause_id for value in clauses] + if len(set(clause_ids)) != len(clause_ids): + raise ValueError("Effect clause IDs must be unique.") + unknown_expectations = {value.expectation_id for value in clauses}.difference( + expectation_ids + ) + if unknown_expectations: + raise ValueError( + "Effect clauses reference unknown state expectations: " + f"{sorted(unknown_expectations)}." + ) + uncovered = set(expectation_ids).difference( + value.expectation_id for value in clauses + ) + uncovered.difference_update( + value.expectation_id + for value in expectations + if type(value) is CoordinatedHeldObjectCleanupExpectation + ) + if uncovered: + raise ValueError( + "Every physical state expectation needs at least one clause; " + f"missing {sorted(uncovered)}." + ) + for value in expectations: + if ( + type(value) is ArticulationJointStateExpectation + and value.target_position.dim() == 2 + and value.target_position.shape[0] != self.env_ids.numel() + ): + raise ValueError( + "Batched articulation targets must match env_ids length." + ) + for value in clauses: + if type(value) is PoseRelationClause: + baseline = value.baseline_object_to_endpoint + if baseline is not None and baseline.dim() == 3: + if baseline.shape[0] != self.env_ids.numel(): + raise ValueError( + "Batched pose baselines must match env_ids length." + ) + if baseline.device != self.env_ids.device: + raise ValueError( + "Batched pose baselines and env_ids must share a device." + ) + elif ( + type(value) is JointStateEffectClause + and value.target_position.dim() == 2 + and value.target_position.shape[0] != self.env_ids.numel() + ): + raise ValueError("Batched joint targets must match env_ids length.") + + held_relations = { + value.relation + for value in expectations + if type(value) is HeldObjectStateExpectation + } + if self.effect_kind is SemanticEffectKind.ATTACH and held_relations != { + HeldObjectRelation.ATTACHED + }: + raise ValueError("An attach effect requires only attached state.") + if self.effect_kind is SemanticEffectKind.RELEASE and held_relations != { + HeldObjectRelation.DETACHED + }: + raise ValueError("A release effect requires only detached state.") + if self.effect_kind is SemanticEffectKind.TRANSFER and held_relations != { + HeldObjectRelation.ATTACHED, + HeldObjectRelation.DETACHED, + }: + raise ValueError("A transfer effect requires attached and detached state.") + if self.effect_kind is SemanticEffectKind.ARTICULATION and not any( + type(value) is ArticulationJointStateExpectation for value in expectations + ): + raise ValueError( + "An articulation effect requires an articulation-joint expectation." + ) + + object.__setattr__( + self, + "state_expectations", + tuple(value.snapshot() for value in expectations), + ) + object.__setattr__( + self, + "clauses", + tuple(value.snapshot() for value in clauses), + ) + + def snapshot(self) -> SemanticEffectSpec: + """Return an independently owned grounded effect contract.""" + return SemanticEffectSpec( + semantic_id=self.semantic_id, + effect_kind=self.effect_kind, + skill_id=self.skill_id, + invocation_id=self.invocation_id, + invocation_revision=self.invocation_revision, + env_ids=self.env_ids, + state_expectations=self.state_expectations, + clauses=self.clauses, + ) + + def to_metadata(self) -> dict[str, object]: + """Return this grounded effect contract as deterministic JSON-safe data.""" + return { + "semantic_id": self.semantic_id, + "effect_kind": self.effect_kind.value, + "skill_id": self.skill_id, + "invocation_id": self.invocation_id, + "invocation_revision": self.invocation_revision, + "env_ids": _metadata_value(self.env_ids), + "state_expectations": [ + _metadata_value(value) for value in self.state_expectations + ], + "clauses": [_metadata_value(value) for value in self.clauses], + } + + def state_expectation(self, expectation_id: str) -> EffectStateExpectation: + """Return an owned state expectation by effect-local ID.""" + for value in self.state_expectations: + if value.expectation_id == expectation_id: + return value.snapshot() + raise KeyError(f"Unknown effect state expectation {expectation_id!r}.") + + def validate_request(self, request: EffectVerificationRequest) -> None: + """Validate execution identity and typed symbolic postconditions.""" + if not isinstance(request, EffectVerificationRequest): + raise TypeError("request must be an EffectVerificationRequest.") + if request.skill_id != self.skill_id: + raise ValueError("Effect request skill_id does not match the spec.") + if request.invocation_id != self.invocation_id: + raise ValueError("Effect request invocation_id does not match the spec.") + if request.invocation_revision != self.invocation_revision: + raise ValueError( + "Effect request invocation_revision does not match the spec." + ) + if request.env_mask.shape != self.env_ids.shape: + raise ValueError("Effect request row count does not match spec env_ids.") + if request.env_mask.device != self.env_ids.device: + raise ValueError( + "Effect request mask and spec env_ids must share a device." + ) + + held_expectations = { + value.task_state_key: value + for value in self.state_expectations + if type(value) is HeldObjectStateExpectation + } + expected_held = request.expected_effects.held_object_updates + if set(expected_held) != set(held_expectations): + raise ValueError( + "Effect request held-object updates must exactly match typed " + "state expectation keys." + ) + for task_state_key, expectation in held_expectations.items(): + candidate = expected_held[task_state_key] + if expectation.relation is HeldObjectRelation.DETACHED: + if candidate is not None: + raise ValueError( + f"Detached expectation {expectation.expectation_id!r} must " + "remove its held-object state." + ) + continue + if not isinstance(candidate, HeldObjectState): + raise ValueError( + f"Attached expectation {expectation.expectation_id!r} requires " + "a HeldObjectState postcondition." + ) + if candidate.semantics.entity_id != expectation.object_id: + raise ValueError( + f"Attached expectation {expectation.expectation_id!r} targets " + "the wrong canonical object." + ) + if candidate.object_to_eef.device != request.env_mask.device: + raise ValueError( + "Attached postcondition poses and request rows must share a device." + ) + if ( + candidate.object_to_eef.dim() == 3 + and candidate.object_to_eef.shape[0] != self.env_ids.numel() + ): + raise ValueError( + "Batched attached postcondition poses must match spec env_ids." + ) + _validate_pose_batch( + candidate.object_to_eef, + field_name=( + f"Attached expectation {expectation.expectation_id!r} pose" + ), + ) + if candidate.env_mask is not None: + if ( + candidate.env_mask.shape != request.env_mask.shape + or candidate.env_mask.device != request.env_mask.device + ): + raise ValueError( + "Attached postcondition masks must match request rows and device." + ) + if (request.env_mask & ~candidate.env_mask).any(): + raise ValueError( + "Attached postconditions must cover every requested row." + ) + + cleanup_expectations = { + value.task_state_keys + for value in self.state_expectations + if type(value) is CoordinatedHeldObjectCleanupExpectation + } + expected_cleanup = request.expected_effects.coordinated_held_object_updates + if set(expected_cleanup) != cleanup_expectations: + raise ValueError( + "Effect request coordinated updates must exactly match typed " + "cleanup expectations." + ) + if any(value is not None for value in expected_cleanup.values()): + raise ValueError( + "Coordinated held-object cleanup expectations may only remove state." + ) + + articulation_expectations = { + (value.articulation_id, value.joint_id): value + for value in self.state_expectations + if type(value) is ArticulationJointStateExpectation + } + articulation_updates = request.expected_effects.articulation_joint_updates + if set(articulation_updates) != set(articulation_expectations): + raise ValueError( + "Articulation-joint updates must exactly match typed state " + "expectations." + ) + for key, expectation in articulation_expectations.items(): + candidate = articulation_updates[key] + if not isinstance(candidate, ArticulationJointState): + raise ValueError( + f"Articulation expectation {expectation.expectation_id!r} " + "requires an ArticulationJointState postcondition." + ) + if candidate.position.device != request.env_mask.device: + raise ValueError( + "Articulation postconditions and request rows must share a device." + ) + if candidate.position.dim() == 2: + if candidate.position.shape[0] != self.env_ids.numel(): + raise ValueError( + "Batched articulation postconditions must match spec env_ids." + ) + positions = candidate.position + else: + positions = candidate.position.unsqueeze(0).expand( + self.env_ids.numel(), -1 + ) + target = expectation.target_position + if target.device != positions.device or target.dtype != positions.dtype: + raise ValueError( + "Articulation postconditions must match target device and dtype." + ) + if target.dim() == 1: + target = target.unsqueeze(0).expand(self.env_ids.numel(), -1) + if positions.shape != target.shape or not torch.equal( + positions[request.env_mask], + target[request.env_mask], + ): + raise ValueError( + f"Articulation expectation {expectation.expectation_id!r} " + "postcondition does not match its target position." + ) + if candidate.env_mask is not None: + if ( + candidate.env_mask.shape != request.env_mask.shape + or candidate.env_mask.device != request.env_mask.device + ): + raise ValueError( + "Articulation postcondition masks must match request rows " + "and device." + ) + if (request.env_mask & ~candidate.env_mask).any(): + raise ValueError( + "Articulation postconditions must cover every requested row." + ) + + +def _validate_evidence_common( + *, + evidence_id: str, + valid: torch.Tensor, + acquisition_errors: tuple[str | None, ...], + timestamp: float, + env_ids: torch.Tensor, + observation_revision: int, + batch_size: int, + device: torch.device, +) -> tuple[torch.Tensor, tuple[str | None, ...], float, torch.Tensor]: + """Validate and own fields shared by every raw evidence batch.""" + _validate_identifier(evidence_id, field_name="effect evidence_id") + if not isinstance(valid, torch.Tensor): + raise TypeError("valid must be a torch.Tensor.") + if valid.dtype != torch.bool or valid.shape != (batch_size,): + raise ValueError("valid must be a bool tensor with shape (B,).") + if valid.device != device: + raise ValueError("valid and evidence payload must share a device.") + if not isinstance(env_ids, torch.Tensor): + raise TypeError("env_ids must be a torch.Tensor.") + if env_ids.dtype != torch.long or env_ids.shape != (batch_size,): + raise ValueError("env_ids must be a torch.long tensor with shape (B,).") + if env_ids.device != device: + raise ValueError("env_ids and evidence payload must share a device.") + if torch.unique(env_ids).numel() != env_ids.numel(): + raise ValueError("Evidence env_ids must be unique.") + errors = tuple(acquisition_errors) + if len(errors) != batch_size: + raise ValueError("acquisition_errors must contain one entry per row.") + for row, (row_valid, error) in enumerate(zip(valid.tolist(), errors)): + if row_valid and error is not None: + raise ValueError(f"Valid evidence row {row} must not carry an error.") + if not row_valid and ( + type(error) is not str or not error or error != error.strip() + ): + raise ValueError(f"Invalid evidence row {row} requires a non-empty error.") + if not isinstance(timestamp, (int, float)) or isinstance(timestamp, bool): + raise TypeError("timestamp must be a number.") + normalized_timestamp = float(timestamp) + if not math.isfinite(normalized_timestamp) or normalized_timestamp < 0.0: + raise ValueError("timestamp must be finite and non-negative.") + if type(observation_revision) is not int or observation_revision < 0: + raise ValueError("observation_revision must be a non-negative integer.") + return valid.clone(), errors, normalized_timestamp, env_ids.clone() + + +@dataclass(frozen=True, slots=True, eq=False) +class PoseRelationEvidenceBatch: + """Raw object-to-endpoint transform observations.""" + + evidence_id: str + object_to_endpoint: torch.Tensor + valid: torch.Tensor + acquisition_errors: tuple[str | None, ...] + timestamp: float + env_ids: torch.Tensor + observation_revision: int + + def __post_init__(self) -> None: + poses = self.object_to_endpoint + if not isinstance(poses, torch.Tensor) or ( + poses.dim() != 3 or poses.shape[0] == 0 or poses.shape[-2:] != (4, 4) + ): + raise ValueError("object_to_endpoint must have shape (B, 4, 4).") + if not poses.is_floating_point(): + raise TypeError("object_to_endpoint must use a floating-point dtype.") + valid, errors, timestamp, env_ids = _validate_evidence_common( + evidence_id=self.evidence_id, + valid=self.valid, + acquisition_errors=self.acquisition_errors, + timestamp=self.timestamp, + env_ids=self.env_ids, + observation_revision=self.observation_revision, + batch_size=poses.shape[0], + device=poses.device, + ) + object.__setattr__( + self, + "object_to_endpoint", + _validate_pose_batch( + poses, + field_name="Valid pose-relation evidence", + valid_mask=valid, + ), + ) + object.__setattr__(self, "valid", valid) + object.__setattr__(self, "acquisition_errors", errors) + object.__setattr__(self, "timestamp", timestamp) + object.__setattr__(self, "env_ids", env_ids) + + def snapshot(self) -> PoseRelationEvidenceBatch: + """Return an independently owned evidence batch.""" + return PoseRelationEvidenceBatch( + self.evidence_id, + self.object_to_endpoint, + self.valid, + self.acquisition_errors, + self.timestamp, + self.env_ids, + self.observation_revision, + ) + + def to_metadata(self) -> dict[str, object]: + """Return raw pose evidence as JSON-safe trace metadata.""" + return _evidence_metadata( + self, + payload={"object_to_endpoint": self.object_to_endpoint}, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class BinaryEffectEvidenceBatch: + """Raw per-row contact or constraint-state observations.""" + + evidence_id: str + evidence_kind: BinaryEvidenceKind + values: torch.Tensor + valid: torch.Tensor + acquisition_errors: tuple[str | None, ...] + timestamp: float + env_ids: torch.Tensor + observation_revision: int + + def __post_init__(self) -> None: + if not isinstance(self.evidence_kind, BinaryEvidenceKind): + raise TypeError("evidence_kind must be a BinaryEvidenceKind.") + values = self.values + if not isinstance(values, torch.Tensor): + raise TypeError("values must be a torch.Tensor.") + if values.dtype != torch.bool or values.dim() != 1 or values.numel() == 0: + raise ValueError("binary evidence values must have bool shape (B,).") + valid, errors, timestamp, env_ids = _validate_evidence_common( + evidence_id=self.evidence_id, + valid=self.valid, + acquisition_errors=self.acquisition_errors, + timestamp=self.timestamp, + env_ids=self.env_ids, + observation_revision=self.observation_revision, + batch_size=values.shape[0], + device=values.device, + ) + object.__setattr__(self, "values", values.clone()) + object.__setattr__(self, "valid", valid) + object.__setattr__(self, "acquisition_errors", errors) + object.__setattr__(self, "timestamp", timestamp) + object.__setattr__(self, "env_ids", env_ids) + + def snapshot(self) -> BinaryEffectEvidenceBatch: + """Return an independently owned evidence batch.""" + return BinaryEffectEvidenceBatch( + self.evidence_id, + self.evidence_kind, + self.values, + self.valid, + self.acquisition_errors, + self.timestamp, + self.env_ids, + self.observation_revision, + ) + + def to_metadata(self) -> dict[str, object]: + """Return raw binary evidence as JSON-safe trace metadata.""" + return _evidence_metadata( + self, + payload={ + "evidence_kind": self.evidence_kind.value, + "values": self.values, + }, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class ScalarEffectEvidenceBatch: + """Raw per-row force or wrench-magnitude observations.""" + + evidence_id: str + evidence_kind: ScalarEvidenceKind + values: torch.Tensor + valid: torch.Tensor + acquisition_errors: tuple[str | None, ...] + timestamp: float + env_ids: torch.Tensor + observation_revision: int + + def __post_init__(self) -> None: + if not isinstance(self.evidence_kind, ScalarEvidenceKind): + raise TypeError("evidence_kind must be a ScalarEvidenceKind.") + values = self.values + if not isinstance(values, torch.Tensor): + raise TypeError("values must be a torch.Tensor.") + if values.dim() != 1 or values.numel() == 0 or not values.is_floating_point(): + raise ValueError("scalar evidence values must have floating shape (B,).") + valid, errors, timestamp, env_ids = _validate_evidence_common( + evidence_id=self.evidence_id, + valid=self.valid, + acquisition_errors=self.acquisition_errors, + timestamp=self.timestamp, + env_ids=self.env_ids, + observation_revision=self.observation_revision, + batch_size=values.shape[0], + device=values.device, + ) + if not torch.isfinite(values[valid]).all(): + raise ValueError("Valid scalar evidence values must be finite.") + object.__setattr__(self, "values", values.clone()) + object.__setattr__(self, "valid", valid) + object.__setattr__(self, "acquisition_errors", errors) + object.__setattr__(self, "timestamp", timestamp) + object.__setattr__(self, "env_ids", env_ids) + + def snapshot(self) -> ScalarEffectEvidenceBatch: + """Return an independently owned evidence batch.""" + return ScalarEffectEvidenceBatch( + self.evidence_id, + self.evidence_kind, + self.values, + self.valid, + self.acquisition_errors, + self.timestamp, + self.env_ids, + self.observation_revision, + ) + + def to_metadata(self) -> dict[str, object]: + """Return raw scalar evidence as JSON-safe trace metadata.""" + return _evidence_metadata( + self, + payload={ + "evidence_kind": self.evidence_kind.value, + "values": self.values, + }, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class JointStateEvidenceBatch: + """Raw per-row joint position/velocity observations.""" + + evidence_id: str + positions: torch.Tensor + velocities: torch.Tensor | None + valid: torch.Tensor + acquisition_errors: tuple[str | None, ...] + timestamp: float + env_ids: torch.Tensor + observation_revision: int + + def __post_init__(self) -> None: + positions = self.positions + if not isinstance(positions, torch.Tensor): + raise TypeError("positions must be a torch.Tensor.") + if ( + positions.dim() != 2 + or positions.shape[0] == 0 + or positions.shape[1] == 0 + or not positions.is_floating_point() + ): + raise ValueError("positions must have non-empty floating shape (B, J).") + valid, errors, timestamp, env_ids = _validate_evidence_common( + evidence_id=self.evidence_id, + valid=self.valid, + acquisition_errors=self.acquisition_errors, + timestamp=self.timestamp, + env_ids=self.env_ids, + observation_revision=self.observation_revision, + batch_size=positions.shape[0], + device=positions.device, + ) + if not torch.isfinite(positions[valid]).all(): + raise ValueError("Valid joint positions must be finite.") + velocities = self.velocities + if velocities is not None: + if not isinstance(velocities, torch.Tensor): + raise TypeError("velocities must be a torch.Tensor or None.") + if ( + velocities.shape != positions.shape + or velocities.device != positions.device + ): + raise ValueError("velocities must match positions shape and device.") + if not velocities.is_floating_point(): + raise TypeError("velocities must use a floating-point dtype.") + if not torch.isfinite(velocities[valid]).all(): + raise ValueError("Valid joint velocities must be finite.") + object.__setattr__(self, "positions", positions.clone()) + object.__setattr__( + self, + "velocities", + None if velocities is None else velocities.clone(), + ) + object.__setattr__(self, "valid", valid) + object.__setattr__(self, "acquisition_errors", errors) + object.__setattr__(self, "timestamp", timestamp) + object.__setattr__(self, "env_ids", env_ids) + + def snapshot(self) -> JointStateEvidenceBatch: + """Return an independently owned evidence batch.""" + return JointStateEvidenceBatch( + self.evidence_id, + self.positions, + self.velocities, + self.valid, + self.acquisition_errors, + self.timestamp, + self.env_ids, + self.observation_revision, + ) + + def to_metadata(self) -> dict[str, object]: + """Return raw joint-state evidence as JSON-safe trace metadata.""" + return _evidence_metadata( + self, + payload={ + "positions": self.positions, + "velocities": self.velocities, + }, + ) + + +def _evidence_metadata( + batch: EffectEvidenceBatch, + *, + payload: Mapping[str, object], +) -> dict[str, object]: + """Serialize fields shared by all raw physical-evidence batches.""" + return { + "evidence_id": batch.evidence_id, + **{key: _metadata_value(value) for key, value in payload.items()}, + "valid_mask": _metadata_value(batch.valid), + "acquisition_errors": list(batch.acquisition_errors), + "timestamp": batch.timestamp, + "env_ids": _metadata_value(batch.env_ids), + "observation_revision": batch.observation_revision, + } + + +EffectEvidenceBatch: TypeAlias = ( + PoseRelationEvidenceBatch + | BinaryEffectEvidenceBatch + | ScalarEffectEvidenceBatch + | JointStateEvidenceBatch +) +_EVIDENCE_TYPES = ( + PoseRelationEvidenceBatch, + BinaryEffectEvidenceBatch, + ScalarEffectEvidenceBatch, + JointStateEvidenceBatch, +) + + +@dataclass(frozen=True, slots=True, eq=False) +class EffectExpectationDecision: + """Per-row outcome for one physical state expectation. + + Rows absent from both ``satisfied_mask`` and ``contradicted_mask`` remain + unresolved. ``inverse_satisfied_mask`` is deliberately stronger than + contradiction: it requires every clause in the expectation group to have + reached its explicit inverse band for the configured consecutive-sample + window. This distinction lets failure reconciliation retain a relation + only from complete inverse evidence rather than from one contradictory + clause. + """ + + expectation_id: str + satisfied_mask: torch.Tensor + contradicted_mask: torch.Tensor + inverse_satisfied_mask: torch.Tensor + + def __post_init__(self) -> None: + _validate_identifier( + self.expectation_id, + field_name="EffectExpectationDecision.expectation_id", + ) + for field_name in ( + "satisfied_mask", + "contradicted_mask", + "inverse_satisfied_mask", + ): + value = getattr(self, field_name) + if not isinstance(value, torch.Tensor): + raise TypeError(f"{field_name} must be a torch.Tensor.") + if value.dtype != torch.bool or value.dim() != 1: + raise ValueError(f"{field_name} must be a one-dimensional bool tensor.") + masks = ( + self.satisfied_mask, + self.contradicted_mask, + self.inverse_satisfied_mask, + ) + if any(value.shape != masks[0].shape for value in masks[1:]): + raise ValueError("Expectation decision masks must have equal shapes.") + if any(value.device != masks[0].device for value in masks[1:]): + raise ValueError("Expectation decision masks must use the same device.") + if (self.satisfied_mask & self.contradicted_mask).any(): + raise ValueError("satisfied_mask and contradicted_mask must not overlap.") + if (self.inverse_satisfied_mask & ~self.contradicted_mask).any(): + raise ValueError( + "inverse_satisfied_mask must be a subset of contradicted_mask." + ) + object.__setattr__(self, "satisfied_mask", self.satisfied_mask.clone()) + object.__setattr__( + self, + "contradicted_mask", + self.contradicted_mask.clone(), + ) + object.__setattr__( + self, + "inverse_satisfied_mask", + self.inverse_satisfied_mask.clone(), + ) + + def snapshot(self) -> EffectExpectationDecision: + """Return an independently owned expectation outcome.""" + return EffectExpectationDecision( + expectation_id=self.expectation_id, + satisfied_mask=self.satisfied_mask, + contradicted_mask=self.contradicted_mask, + inverse_satisfied_mask=self.inverse_satisfied_mask, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class EffectMonitorDecision: + """Uncorrelated aggregate and per-expectation monitor decision. + + When ``expectation_decisions`` is non-empty, the aggregate masks are + authoritative reductions of that current observation: success is the + conjunction of every satisfied mask and failure is the union of every + contradicted mask. This prevents callers from combining expectation + outcomes observed on different ticks. + """ + + success_mask: torch.Tensor + failure_mask: torch.Tensor + expectation_decisions: tuple[EffectExpectationDecision, ...] = () + + def __post_init__(self) -> None: + for field_name in ("success_mask", "failure_mask"): + value = getattr(self, field_name) + if not isinstance(value, torch.Tensor): + raise TypeError(f"{field_name} must be a torch.Tensor.") + if value.dtype != torch.bool or value.dim() != 1: + raise ValueError(f"{field_name} must be a one-dimensional bool tensor.") + if self.success_mask.shape != self.failure_mask.shape: + raise ValueError("Decision masks must have equal shapes.") + if self.success_mask.device != self.failure_mask.device: + raise ValueError("Decision masks must use the same device.") + if (self.success_mask & self.failure_mask).any(): + raise ValueError("Decision masks must not overlap.") + expectation_decisions = tuple(self.expectation_decisions) + if not all( + type(value) is EffectExpectationDecision for value in expectation_decisions + ): + raise TypeError( + "expectation_decisions must contain exact " + "EffectExpectationDecision values." + ) + expectation_ids = [value.expectation_id for value in expectation_decisions] + if len(set(expectation_ids)) != len(expectation_ids): + raise ValueError("Expectation decision IDs must be unique.") + if expectation_decisions: + for value in expectation_decisions: + if value.satisfied_mask.shape != self.success_mask.shape: + raise ValueError( + "Expectation and aggregate decision masks must have " + "equal shapes." + ) + if value.satisfied_mask.device != self.success_mask.device: + raise ValueError( + "Expectation and aggregate decision masks must use the " + "same device." + ) + expected_success = torch.ones_like(self.success_mask) + expected_failure = torch.zeros_like(self.failure_mask) + for value in expectation_decisions: + expected_success &= value.satisfied_mask + expected_failure |= value.contradicted_mask + if not torch.equal(self.success_mask, expected_success): + raise ValueError( + "success_mask must equal the conjunction of expectation " + "satisfied masks." + ) + if not torch.equal(self.failure_mask, expected_failure): + raise ValueError( + "failure_mask must equal the union of expectation " + "contradicted masks." + ) + object.__setattr__(self, "success_mask", self.success_mask.clone()) + object.__setattr__(self, "failure_mask", self.failure_mask.clone()) + object.__setattr__( + self, + "expectation_decisions", + tuple(value.snapshot() for value in expectation_decisions), + ) + + +class EffectMonitor(ABC): + """Stateful verifier owned by one grounded semantic call.""" + + @property + @abstractmethod + def spec(self) -> SemanticEffectSpec: + """Return an independently owned effect contract.""" + + @property + def resolved_params(self) -> Mapping[str, EffectMonitorParam]: + """Return resolved monitor thresholds for trace metadata. + + Custom monitors may override this property. The empty default keeps + third-party implementations source-compatible while built-ins expose + every effective threshold, including defaults omitted by configuration. + """ + return MappingProxyType({}) + + @abstractmethod + def observe( + self, + request: EffectVerificationRequest, + evidence: Mapping[str, EffectEvidenceBatch], + ) -> EffectMonitorDecision: + """Consume one synchronized raw observation and decide requested rows.""" + + +class EffectMonitorFactory(ABC): + """Versioned constructor for independent semantic-effect monitors.""" + + monitor_id: ClassVar[str] + revision: ClassVar[str] + + @abstractmethod + def validate_ref(self, ref: EffectMonitorRef) -> None: + """Validate one reference without providers or state creation.""" + + @abstractmethod + def create( + self, + spec: SemanticEffectSpec, + ref: EffectMonitorRef, + ) -> EffectMonitor: + """Create one independent monitor for ``spec`` and ``ref``.""" + + +def _same_tensor_value(left: torch.Tensor, right: torch.Tensor) -> bool: + """Return whether tensors have identical placement, type, shape, and value.""" + return ( + left.device == right.device + and left.dtype == right.dtype + and left.shape == right.shape + and torch.equal(left, right) + ) + + +def _same_source( + left: EffectEvidenceSourceRef, + right: EffectEvidenceSourceRef, +) -> bool: + return left.source_fingerprint == right.source_fingerprint + + +def _same_state_expectation( + left: EffectStateExpectation, + right: EffectStateExpectation, +) -> bool: + if type(left) is not type(right): + return False + if type(left) is HeldObjectStateExpectation: + assert type(right) is HeldObjectStateExpectation + return left == right + if type(left) is CoordinatedHeldObjectCleanupExpectation: + assert type(right) is CoordinatedHeldObjectCleanupExpectation + return left == right + assert type(left) is ArticulationJointStateExpectation + assert type(right) is ArticulationJointStateExpectation + return ( + left.expectation_id == right.expectation_id + and left.articulation_id == right.articulation_id + and left.joint_id == right.joint_id + and _same_tensor_value(left.target_position, right.target_position) + ) + + +def _same_clause(left: EffectClause, right: EffectClause) -> bool: + if type(left) is not type(right): + return False + if ( + left.clause_id != right.clause_id + or left.expectation_id != right.expectation_id + or not _same_source(left.source, right.source) + ): + return False + if type(left) is PoseRelationClause: + assert type(right) is PoseRelationClause + if left.expectation is not right.expectation: + return False + left_baseline = left.baseline_object_to_endpoint + right_baseline = right.baseline_object_to_endpoint + if left_baseline is None or right_baseline is None: + return left_baseline is None and right_baseline is None + return _same_tensor_value(left_baseline, right_baseline) + if type(left) is BinaryEffectClause: + assert type(right) is BinaryEffectClause + return ( + left.evidence_kind is right.evidence_kind + and left.expected is right.expected + ) + if type(left) is ScalarEffectClause: + assert type(right) is ScalarEffectClause + return ( + left.evidence_kind is right.evidence_kind + and left.expectation is right.expectation + ) + assert type(left) is JointStateEffectClause + assert type(right) is JointStateEffectClause + return _same_tensor_value(left.target_position, right.target_position) + + +def _same_effect_spec(left: SemanticEffectSpec, right: SemanticEffectSpec) -> bool: + """Return whether grounded typed effect specs are exactly equivalent.""" + return ( + left.semantic_id == right.semantic_id + and left.effect_kind is right.effect_kind + and left.skill_id == right.skill_id + and left.invocation_id == right.invocation_id + and left.invocation_revision == right.invocation_revision + and _same_tensor_value(left.env_ids, right.env_ids) + and len(left.state_expectations) == len(right.state_expectations) + and all( + _same_state_expectation(left_value, right_value) + for left_value, right_value in zip( + left.state_expectations, + right.state_expectations, + strict=True, + ) + ) + and len(left.clauses) == len(right.clauses) + and all( + _same_clause(left_value, right_value) + for left_value, right_value in zip( + left.clauses, + right.clauses, + strict=True, + ) + ) + ) + + +class EffectMonitorRegistry: + """Immutable exact-ID/revision registry of monitor factories.""" + + __slots__ = ("_factories",) + + def __init__(self, factories: Iterable[EffectMonitorFactory] = ()) -> None: + normalized: dict[tuple[str, str], EffectMonitorFactory] = {} + for factory in factories: + if not isinstance(factory, EffectMonitorFactory): + raise TypeError("factories must contain EffectMonitorFactory objects.") + monitor_id = _validate_identifier( + factory.monitor_id, + field_name="EffectMonitorFactory.monitor_id", + ) + revision = _validate_identifier( + factory.revision, + field_name="EffectMonitorFactory.revision", + ) + key = monitor_id, revision + if key in normalized: + raise ValueError(f"Duplicate effect-monitor factory {key!r}.") + normalized[key] = factory + self._factories = MappingProxyType(normalized) + + @property + def factories(self) -> Mapping[tuple[str, str], EffectMonitorFactory]: + """Return the immutable exact-key factory mapping.""" + return self._factories + + def resolve(self, ref: EffectMonitorRef) -> EffectMonitorFactory: + """Resolve the exact factory named by a declarative reference.""" + if not isinstance(ref, EffectMonitorRef): + raise TypeError("ref must be an EffectMonitorRef.") + key = ref.monitor_id, ref.revision + try: + return self._factories[key] + except KeyError as exc: + raise KeyError(f"Unknown effect-monitor factory {key!r}.") from exc + + def validate_ref(self, ref: EffectMonitorRef) -> None: + """Validate a reference provider-free through its exact factory.""" + self.resolve(ref).validate_ref(ref) + + def create( + self, + spec: SemanticEffectSpec, + ref: EffectMonitorRef, + ) -> EffectMonitor: + """Create one independent monitor through exact factory lookup.""" + if not isinstance(spec, SemanticEffectSpec): + raise TypeError("spec must be a SemanticEffectSpec.") + factory = self.resolve(ref) + factory.validate_ref(ref) + monitor = factory.create(spec, ref) + if not isinstance(monitor, EffectMonitor): + raise TypeError( + "EffectMonitorFactory.create() must return an EffectMonitor." + ) + monitor_spec = monitor.spec + if not isinstance(monitor_spec, SemanticEffectSpec): + raise TypeError("EffectMonitor.spec must be a SemanticEffectSpec.") + if monitor_spec is spec: + raise TypeError( + "EffectMonitor.spec must return an independently owned contract." + ) + if not _same_effect_spec(monitor_spec, spec): + raise ValueError( + "EffectMonitorFactory created a monitor for a different effect spec." + ) + return monitor + + +@dataclass(frozen=True, slots=True) +class CompositeEffectMonitorCfg: + """Strict hysteresis policy for typed pose/binary/scalar/joint clauses.""" + + attached_translation_threshold: float = 0.02 + attached_rotation_threshold: float = 0.20 + detached_translation_threshold: float = 0.05 + detached_rotation_threshold: float = 0.50 + force_absent_threshold: float = 0.20 + force_present_threshold: float = 1.00 + joint_success_tolerance: float = 0.02 + joint_failure_tolerance: float = 0.10 + consecutive_samples: int = 2 + + def __post_init__(self) -> None: + for field_name in ( + "attached_translation_threshold", + "attached_rotation_threshold", + "detached_translation_threshold", + "detached_rotation_threshold", + "force_absent_threshold", + "force_present_threshold", + "joint_success_tolerance", + "joint_failure_tolerance", + ): + value = getattr(self, field_name) + if not isinstance(value, (int, float)) or isinstance(value, bool): + raise TypeError(f"{field_name} must be a number.") + if not math.isfinite(float(value)) or value < 0.0: + raise ValueError(f"{field_name} must be finite and non-negative.") + object.__setattr__(self, field_name, float(value)) + if self.attached_translation_threshold >= self.detached_translation_threshold: + raise ValueError( + "attached_translation_threshold must be less than " + "detached_translation_threshold." + ) + if self.attached_rotation_threshold >= self.detached_rotation_threshold: + raise ValueError( + "attached_rotation_threshold must be less than " + "detached_rotation_threshold." + ) + if self.detached_rotation_threshold > math.pi: + raise ValueError("detached_rotation_threshold must not exceed pi.") + if self.force_absent_threshold >= self.force_present_threshold: + raise ValueError( + "force_absent_threshold must be less than force_present_threshold." + ) + if self.joint_success_tolerance >= self.joint_failure_tolerance: + raise ValueError( + "joint_success_tolerance must be less than joint_failure_tolerance." + ) + if type(self.consecutive_samples) is not int or self.consecutive_samples <= 0: + raise ValueError("consecutive_samples must be a positive integer.") + + @classmethod + def from_params( + cls, + params: Mapping[str, EffectMonitorParam], + ) -> CompositeEffectMonitorCfg: + """Decode strict declarative factory parameters.""" + allowed = { + "attached_translation_threshold", + "attached_rotation_threshold", + "detached_translation_threshold", + "detached_rotation_threshold", + "force_absent_threshold", + "force_present_threshold", + "joint_success_tolerance", + "joint_failure_tolerance", + "consecutive_samples", + } + unknown = set(params).difference(allowed) + if unknown: + raise ValueError( + f"Unknown composite effect monitor parameters: {sorted(unknown)}." + ) + return cls(**dict(params)) # type: ignore[arg-type] + + def to_metadata(self) -> dict[str, object]: + """Return every resolved hysteresis threshold as JSON-safe data.""" + return { + "attached_translation_threshold": self.attached_translation_threshold, + "attached_rotation_threshold": self.attached_rotation_threshold, + "detached_translation_threshold": self.detached_translation_threshold, + "detached_rotation_threshold": self.detached_rotation_threshold, + "force_absent_threshold": self.force_absent_threshold, + "force_present_threshold": self.force_present_threshold, + "joint_success_tolerance": self.joint_success_tolerance, + "joint_failure_tolerance": self.joint_failure_tolerance, + "consecutive_samples": self.consecutive_samples, + } + + +def _pose_errors( + observed: torch.Tensor, + baseline: torch.Tensor, +) -> tuple[float, float]: + baseline = baseline.to(device=observed.device, dtype=observed.dtype) + translation = torch.linalg.vector_norm(observed[:3, 3] - baseline[:3, 3]) + relative_rotation = baseline[:3, :3].transpose(0, 1) @ observed[:3, :3] + cosine = torch.clamp((torch.trace(relative_rotation) - 1.0) * 0.5, -1.0, 1.0) + rotation = torch.acos(cosine) + return float(translation.item()), float(rotation.item()) + + +class CompositeEffectMonitor(EffectMonitor): + """Stateful conjunction monitor over typed physical evidence clauses.""" + + def __init__( + self, + spec: SemanticEffectSpec, + cfg: CompositeEffectMonitorCfg, + ) -> None: + if not isinstance(spec, SemanticEffectSpec): + raise TypeError("spec must be a SemanticEffectSpec.") + if not isinstance(cfg, CompositeEffectMonitorCfg): + raise TypeError("cfg must be a CompositeEffectMonitorCfg.") + self._spec = spec.snapshot() + self._cfg = cfg + self._attempt_generation: int | None = None + self._active_env_ids: frozenset[int] = frozenset() + self._success_counts: dict[tuple[str, int], int] = {} + self._failure_counts: dict[tuple[str, int], int] = {} + self._inverse_success_counts: dict[tuple[str, int], int] = {} + self._last_observations: dict[int, tuple[float, int]] = {} + + @property + def spec(self) -> SemanticEffectSpec: + """Return an independently owned effect contract.""" + return self._spec.snapshot() + + @property + def resolved_params(self) -> Mapping[str, EffectMonitorParam]: + """Return all effective typed-clause thresholds, including defaults.""" + return MappingProxyType(self._cfg.to_metadata()) + + def _prepare_request(self, request: EffectVerificationRequest) -> None: + self._spec.validate_request(request) + active_env_ids = frozenset( + int(value) + for value in self._spec.env_ids[request.env_mask].detach().cpu().tolist() + ) + if self._attempt_generation != request.attempt_generation: + self._attempt_generation = request.attempt_generation + self._active_env_ids = active_env_ids + self._success_counts.clear() + self._failure_counts.clear() + self._inverse_success_counts.clear() + self._last_observations.clear() + return + if not active_env_ids.issubset(self._active_env_ids): + raise ValueError( + "An effect-verification request may only shrink within one " + "attempt_generation." + ) + self._active_env_ids = active_env_ids + self._success_counts = { + key: count + for key, count in self._success_counts.items() + if key[1] in active_env_ids + } + self._failure_counts = { + key: count + for key, count in self._failure_counts.items() + if key[1] in active_env_ids + } + self._inverse_success_counts = { + key: count + for key, count in self._inverse_success_counts.items() + if key[1] in active_env_ids + } + self._last_observations = { + env_id: observation + for env_id, observation in self._last_observations.items() + if env_id in active_env_ids + } + + @staticmethod + def _validate_evidence_type( + clause: EffectClause, + batch: EffectEvidenceBatch, + ) -> None: + if type(clause) is PoseRelationClause: + if type(batch) is not PoseRelationEvidenceBatch: + raise TypeError("PoseRelationClause requires pose evidence.") + return + if type(clause) is BinaryEffectClause: + if type(batch) is not BinaryEffectEvidenceBatch: + raise TypeError("BinaryEffectClause requires binary evidence.") + if batch.evidence_kind is not clause.evidence_kind: + raise ValueError("Binary evidence kind does not match its clause.") + return + if type(clause) is ScalarEffectClause: + if type(batch) is not ScalarEffectEvidenceBatch: + raise TypeError("ScalarEffectClause requires scalar evidence.") + if batch.evidence_kind is not clause.evidence_kind: + raise ValueError("Scalar evidence kind does not match its clause.") + return + if type(batch) is not JointStateEvidenceBatch: + raise TypeError("JointStateEffectClause requires joint-state evidence.") + + def _normalize_evidence( + self, + evidence: Mapping[str, EffectEvidenceBatch], + *, + requested_at: float, + deadline: float, + ) -> tuple[Mapping[str, EffectEvidenceBatch], tuple[int, ...]]: + if not isinstance(evidence, Mapping): + raise TypeError("evidence must be a mapping.") + clause_by_id = {value.clause_id: value for value in self._spec.clauses} + if set(evidence) != set(clause_by_id): + raise ValueError("Evidence keys must exactly match effect clause IDs.") + normalized: dict[str, EffectEvidenceBatch] = {} + first: EffectEvidenceBatch | None = None + for clause_id, batch in evidence.items(): + if type(batch) not in _EVIDENCE_TYPES: + raise TypeError( + "evidence values must be typed effect evidence batches." + ) + if batch.evidence_id != clause_id: + raise ValueError("Evidence keys must match batch evidence_id values.") + self._validate_evidence_type(clause_by_id[clause_id], batch) + if batch.timestamp < requested_at: + raise ValueError("Effect evidence must not predate the request.") + if batch.timestamp > deadline: + raise ValueError( + "Effect evidence must not exceed the request deadline." + ) + if first is None: + first = batch + elif ( + batch.timestamp != first.timestamp + or batch.observation_revision != first.observation_revision + or not torch.equal(batch.env_ids, first.env_ids) + ): + raise ValueError( + "All effect evidence must share timestamp, observation_revision, " + "and env_ids." + ) + normalized[clause_id] = batch.snapshot() + assert first is not None + known_env_ids = set(self._spec.env_ids.detach().cpu().tolist()) + observed_env_ids = tuple(int(value) for value in first.env_ids.cpu().tolist()) + if not set(observed_env_ids).issubset(known_env_ids): + raise ValueError("Evidence contains env_ids outside the effect spec.") + missing = self._active_env_ids.difference(observed_env_ids) + if missing: + expectation_ids = {clause.expectation_id for clause in self._spec.clauses} + for env_id in missing: + for expectation_id in expectation_ids: + key = (expectation_id, env_id) + self._success_counts[key] = 0 + self._failure_counts[key] = 0 + self._inverse_success_counts[key] = 0 + raise ValueError( + "Evidence must cover every active request env_id exactly once; " + f"missing {sorted(missing)}. Acquisition failures must be explicit " + "valid=False rows." + ) + return MappingProxyType(normalized), observed_env_ids + + def _pose_baseline( + self, + clause: PoseRelationClause, + request: EffectVerificationRequest, + spec_row: int, + ) -> torch.Tensor: + baseline = clause.baseline_object_to_endpoint + if baseline is None: + expectation = self._spec.state_expectation(clause.expectation_id) + if type(expectation) is not HeldObjectStateExpectation: + raise ValueError( + "A request-derived pose baseline requires a held-object " + "state expectation." + ) + candidate = request.expected_effects.held_object_updates[ + expectation.task_state_key + ] + assert isinstance(candidate, HeldObjectState) + baseline = candidate.object_to_eef + return baseline if baseline.dim() == 2 else baseline[spec_row] + + def _classify_clause( + self, + clause: EffectClause, + batch: EffectEvidenceBatch, + *, + evidence_row: int, + spec_row: int, + request: EffectVerificationRequest, + ) -> int: + """Return 1 expected, -1 contradicted, or 0 unresolved.""" + if not bool(batch.valid[evidence_row].item()): + return 0 + if type(clause) is PoseRelationClause: + assert type(batch) is PoseRelationEvidenceBatch + observed = batch.object_to_endpoint[evidence_row] + baseline = self._pose_baseline(clause, request, spec_row) + translation_error, rotation_error = _pose_errors(observed, baseline) + matched = ( + translation_error <= self._cfg.attached_translation_threshold + and rotation_error <= self._cfg.attached_rotation_threshold + ) + separated = ( + translation_error >= self._cfg.detached_translation_threshold + or rotation_error >= self._cfg.detached_rotation_threshold + ) + if clause.expectation is PoseRelationExpectation.MATCHED: + return 1 if matched else (-1 if separated else 0) + return 1 if separated else (-1 if matched else 0) + if type(clause) is BinaryEffectClause: + assert type(batch) is BinaryEffectEvidenceBatch + return ( + 1 if bool(batch.values[evidence_row].item()) is clause.expected else -1 + ) + if type(clause) is ScalarEffectClause: + assert type(batch) is ScalarEffectEvidenceBatch + magnitude = abs(float(batch.values[evidence_row].item())) + present = magnitude >= self._cfg.force_present_threshold + absent = magnitude <= self._cfg.force_absent_threshold + if clause.expectation is ScalarExpectation.PRESENT: + return 1 if present else (-1 if absent else 0) + return 1 if absent else (-1 if present else 0) + assert type(clause) is JointStateEffectClause + assert type(batch) is JointStateEvidenceBatch + target = clause.target_position + if target.dim() == 2: + target = target[spec_row] + observed = batch.positions[evidence_row] + if target.shape != observed.shape: + raise ValueError("Joint evidence width does not match its clause target.") + error = float(torch.max(torch.abs(observed - target)).item()) + if error <= self._cfg.joint_success_tolerance: + return 1 + if error >= self._cfg.joint_failure_tolerance: + return -1 + return 0 + + def observe( + self, + request: EffectVerificationRequest, + evidence: Mapping[str, EffectEvidenceBatch], + ) -> EffectMonitorDecision: + """Update typed-clause hysteresis and decide current request rows.""" + self._prepare_request(request) + batches, observed_env_ids = self._normalize_evidence( + evidence, + requested_at=request.requested_at, + deadline=request.deadline, + ) + spec_rows = { + int(env_id): row + for row, env_id in enumerate(self._spec.env_ids.detach().cpu().tolist()) + } + request_rows = { + int(env_id): row + for row, env_id in enumerate(self._spec.env_ids.detach().cpu().tolist()) + if bool(request.env_mask[row].item()) + } + first_batch = next(iter(batches.values())) + observation_token = ( + first_batch.timestamp, + first_batch.observation_revision, + ) + for env_id in self._active_env_ids: + previous = self._last_observations.get(env_id) + if previous is None: + continue + if observation_token[0] < previous[0]: + raise ValueError( + "Evidence timestamps must be monotonic for every active env_id." + ) + if observation_token[1] < previous[1]: + raise ValueError( + "Evidence observation_revision values must be monotonic for " + "every active env_id." + ) + + clauses_by_expectation: dict[str, list[EffectClause]] = {} + for clause in self._spec.clauses: + clauses_by_expectation.setdefault(clause.expectation_id, []).append(clause) + physical_expectation_ids = tuple( + expectation.expectation_id + for expectation in self._spec.state_expectations + if expectation.expectation_id in clauses_by_expectation + ) + satisfied_masks = { + expectation_id: torch.zeros_like(request.env_mask) + for expectation_id in physical_expectation_ids + } + contradicted_masks = { + expectation_id: torch.zeros_like(request.env_mask) + for expectation_id in physical_expectation_ids + } + inverse_satisfied_masks = { + expectation_id: torch.zeros_like(request.env_mask) + for expectation_id in physical_expectation_ids + } + + for evidence_row, env_id in enumerate(observed_env_ids): + request_row = request_rows.get(env_id) + if request_row is None: + continue + if self._last_observations.get(env_id) == observation_token: + continue + self._last_observations[env_id] = observation_token + spec_row = spec_rows[env_id] + for expectation_id in physical_expectation_ids: + classifications = [ + self._classify_clause( + clause, + batches[clause.clause_id], + evidence_row=evidence_row, + spec_row=spec_row, + request=request, + ) + for clause in clauses_by_expectation[expectation_id] + ] + group_expected = all(value == 1 for value in classifications) + group_contradicted = any(value == -1 for value in classifications) + group_inverse_satisfied = all(value == -1 for value in classifications) + key = (expectation_id, env_id) + if group_expected: + self._success_counts[key] = self._success_counts.get(key, 0) + 1 + else: + self._success_counts[key] = 0 + if group_contradicted: + self._failure_counts[key] = self._failure_counts.get(key, 0) + 1 + else: + self._failure_counts[key] = 0 + if group_inverse_satisfied: + self._inverse_success_counts[key] = ( + self._inverse_success_counts.get(key, 0) + 1 + ) + else: + self._inverse_success_counts[key] = 0 + if self._success_counts.get(key, 0) >= self._cfg.consecutive_samples: + satisfied_masks[expectation_id][request_row] = True + if self._failure_counts.get(key, 0) >= self._cfg.consecutive_samples: + contradicted_masks[expectation_id][request_row] = True + if ( + self._inverse_success_counts.get(key, 0) + >= self._cfg.consecutive_samples + ): + inverse_satisfied_masks[expectation_id][request_row] = True + + expectation_decisions = tuple( + EffectExpectationDecision( + expectation_id=expectation_id, + satisfied_mask=satisfied_masks[expectation_id] & request.env_mask, + contradicted_mask=( + contradicted_masks[expectation_id] & request.env_mask + ), + inverse_satisfied_mask=( + inverse_satisfied_masks[expectation_id] & request.env_mask + ), + ) + for expectation_id in physical_expectation_ids + ) + success_mask = request.env_mask.clone() + failure_mask = torch.zeros_like(request.env_mask) + for decision in expectation_decisions: + success_mask &= decision.satisfied_mask + failure_mask |= decision.contradicted_mask + return EffectMonitorDecision( + success_mask, + failure_mask, + expectation_decisions, + ) + + +class CompositeEffectMonitorFactory(EffectMonitorFactory): + """Factory for the built-in typed-clause monitor.""" + + monitor_id = COMPOSITE_EFFECT_MONITOR_ID + revision = COMPOSITE_EFFECT_MONITOR_REVISION + + def validate_ref(self, ref: EffectMonitorRef) -> None: + """Validate exact built-in selection and typed thresholds.""" + if not isinstance(ref, EffectMonitorRef): + raise TypeError("ref must be an EffectMonitorRef.") + if (ref.monitor_id, ref.revision) != (self.monitor_id, self.revision): + raise ValueError("EffectMonitorRef does not select this exact factory.") + CompositeEffectMonitorCfg.from_params(ref.params) + + def create( + self, + spec: SemanticEffectSpec, + ref: EffectMonitorRef, + ) -> CompositeEffectMonitor: + """Create one independently stateful typed-clause monitor.""" + if not isinstance(spec, SemanticEffectSpec): + raise TypeError("spec must be a SemanticEffectSpec.") + self.validate_ref(ref) + return CompositeEffectMonitor( + spec.snapshot(), + CompositeEffectMonitorCfg.from_params(ref.params), + ) + + +__all__ = [ + "ArticulationJointStateExpectation", + "BinaryEffectClause", + "BinaryEffectEvidenceBatch", + "BinaryEvidenceKind", + "COMPOSITE_EFFECT_MONITOR_ID", + "COMPOSITE_EFFECT_MONITOR_REVISION", + "CONTACT_EFFECT_CHANNEL", + "CONSTRAINT_EFFECT_CHANNEL", + "CONTROL_PART_EVIDENCE_PROVIDER_ID", + "CONTROL_PART_EVIDENCE_PROVIDER_REVISION", + "CompositeEffectMonitor", + "CompositeEffectMonitorCfg", + "CompositeEffectMonitorFactory", + "ControlPartEvidenceAddress", + "CoordinatedHeldObjectCleanupExpectation", + "EffectClause", + "EffectEvidenceAddress", + "EffectEvidenceBatch", + "EffectEvidenceSourceRef", + "EffectExpectationDecision", + "EffectMonitor", + "EffectMonitorDecision", + "EffectMonitorFactory", + "EffectMonitorParam", + "EffectMonitorRef", + "EffectMonitorRegistry", + "EffectStateExpectation", + "FORCE_EFFECT_CHANNEL", + "HeldObjectRelation", + "HeldObjectStateExpectation", + "JOINT_STATE_EFFECT_CHANNEL", + "JointStateEffectClause", + "JointStateEvidenceBatch", + "POSE_RELATION_EFFECT_CHANNEL", + "PoseRelationClause", + "PoseRelationEvidenceBatch", + "PoseRelationExpectation", + "ScalarEffectClause", + "ScalarEffectEvidenceBatch", + "ScalarEvidenceKind", + "ScalarExpectation", + "SemanticEffectKind", + "SemanticEffectSpec", + "SymbolicStateDomain", + "SymbolicStateKey", +] diff --git a/embodichain/lab/sim/skills/evidence.py b/embodichain/lab/sim/skills/evidence.py new file mode 100644 index 000000000..56fe7e94a --- /dev/null +++ b/embodichain/lab/sim/skills/evidence.py @@ -0,0 +1,1467 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Backend-neutral acquisition ports for typed semantic-effect evidence.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from collections.abc import Callable, Iterable, Mapping, Sequence +from dataclasses import dataclass +import math +from types import MappingProxyType +from typing import ClassVar, Protocol, TypeAlias, runtime_checkable + +import torch + +from embodichain.utils.math import pose_inv + +from ..atomic_actions import SceneProvider, SceneSnapshot +from .effects import ( + ArticulationJointStateExpectation, + BinaryEffectClause, + BinaryEffectEvidenceBatch, + BinaryEvidenceKind, + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + ControlPartEvidenceAddress, + CoordinatedHeldObjectCleanupExpectation, + EffectClause, + EffectEvidenceBatch, + EffectEvidenceSourceRef, + EffectStateExpectation, + FORCE_EFFECT_CHANNEL, + HeldObjectStateExpectation, + JOINT_STATE_EFFECT_CHANNEL, + JointStateEffectClause, + JointStateEvidenceBatch, + POSE_RELATION_EFFECT_CHANNEL, + PoseRelationClause, + PoseRelationEvidenceBatch, + ScalarEffectClause, + ScalarEffectEvidenceBatch, + ScalarEvidenceKind, + SemanticEffectSpec, +) +from .scene import ( + SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, + ArticulationJointEvidenceAddress, +) + +_EFFECT_EXPECTATION_TYPES = ( + HeldObjectStateExpectation, + CoordinatedHeldObjectCleanupExpectation, + ArticulationJointStateExpectation, +) +_EFFECT_BATCH_TYPES = ( + PoseRelationEvidenceBatch, + BinaryEffectEvidenceBatch, + ScalarEffectEvidenceBatch, + JointStateEvidenceBatch, +) + + +def _validate_identifier(value: str, *, field_name: str) -> str: + """Return one exact non-empty identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +@dataclass(frozen=True, slots=True, eq=False) +class EffectEvidenceCollectionContext: + """One synchronized acquisition tick shared by all effect clauses. + + Args: + timestamp: Non-negative backend observation time. + observation_revision: Monotonic revision chosen by the runtime port. + env_ids: Ordered environment correlation IDs to observe. + """ + + timestamp: float + observation_revision: int + env_ids: torch.Tensor + + def __post_init__(self) -> None: + if isinstance(self.timestamp, bool) or not isinstance( + self.timestamp, (int, float) + ): + raise TypeError("timestamp must be a number.") + timestamp = float(self.timestamp) + if not math.isfinite(timestamp) or timestamp < 0.0: + raise ValueError("timestamp must be finite and non-negative.") + if type(self.observation_revision) is not int or self.observation_revision < 0: + raise ValueError("observation_revision must be a non-negative integer.") + env_ids = self.env_ids + if not isinstance(env_ids, torch.Tensor): + raise TypeError("env_ids must be a torch.Tensor.") + if env_ids.dtype != torch.long or env_ids.dim() != 1 or env_ids.numel() == 0: + raise ValueError( + "env_ids must be a non-empty one-dimensional int64 tensor." + ) + if torch.unique(env_ids).numel() != env_ids.numel(): + raise ValueError("env_ids must be unique.") + object.__setattr__(self, "timestamp", timestamp) + object.__setattr__(self, "env_ids", env_ids.clone()) + + def snapshot(self) -> EffectEvidenceCollectionContext: + """Return an independently owned acquisition context.""" + return EffectEvidenceCollectionContext( + self.timestamp, + self.observation_revision, + self.env_ids, + ) + + +def _snapshot_expectation( + expectation: EffectStateExpectation, +) -> EffectStateExpectation: + """Validate and own one exact typed effect expectation.""" + if type(expectation) not in _EFFECT_EXPECTATION_TYPES: + raise TypeError("expectation must be an exact typed effect expectation.") + return expectation.snapshot() + + +class EffectEvidenceQuery(ABC): + """Typed request for the raw evidence of exactly one effect clause.""" + + @property + @abstractmethod + def evidence_id(self) -> str: + """Return the clause-local evidence identifier.""" + + @property + @abstractmethod + def source(self) -> EffectEvidenceSourceRef: + """Return an owned exact provider route and physical address.""" + + @property + @abstractmethod + def expectation(self) -> EffectStateExpectation: + """Return an owned symbolic expectation related to this query.""" + + @abstractmethod + def snapshot(self) -> EffectEvidenceQuery: + """Return an independently owned query of the exact same type.""" + + +def _validate_query( + clause: EffectClause, + expectation: EffectStateExpectation, +) -> EffectStateExpectation: + """Validate common clause/expectation correlation.""" + owned = _snapshot_expectation(expectation) + if clause.expectation_id != owned.expectation_id: + raise ValueError("Query clause and expectation IDs must match.") + return owned + + +@dataclass(frozen=True, slots=True, eq=False) +class PoseRelationEvidenceQuery(EffectEvidenceQuery): + """Query for an object's pose relative to a resource endpoint.""" + + clause: PoseRelationClause + _expectation: EffectStateExpectation + + def __post_init__(self) -> None: + if type(self.clause) is not PoseRelationClause: + raise TypeError("clause must be a PoseRelationClause.") + object.__setattr__(self, "clause", self.clause.snapshot()) + object.__setattr__( + self, + "_expectation", + _validate_query(self.clause, self._expectation), + ) + + @property + def evidence_id(self) -> str: + """Return the source clause ID.""" + return self.clause.clause_id + + @property + def source(self) -> EffectEvidenceSourceRef: + """Return an owned source route.""" + return self.clause.source.snapshot() + + @property + def expectation(self) -> EffectStateExpectation: + """Return an owned correlated expectation.""" + return self._expectation.snapshot() + + def snapshot(self) -> PoseRelationEvidenceQuery: + """Return an independently owned pose query.""" + return PoseRelationEvidenceQuery(self.clause, self._expectation) + + +@dataclass(frozen=True, slots=True, eq=False) +class BinaryEffectEvidenceQuery(EffectEvidenceQuery): + """Query for one raw contact or constraint boolean.""" + + clause: BinaryEffectClause + _expectation: EffectStateExpectation + + def __post_init__(self) -> None: + if type(self.clause) is not BinaryEffectClause: + raise TypeError("clause must be a BinaryEffectClause.") + object.__setattr__(self, "clause", self.clause.snapshot()) + object.__setattr__( + self, + "_expectation", + _validate_query(self.clause, self._expectation), + ) + + @property + def evidence_id(self) -> str: + """Return the source clause ID.""" + return self.clause.clause_id + + @property + def source(self) -> EffectEvidenceSourceRef: + """Return an owned source route.""" + return self.clause.source.snapshot() + + @property + def expectation(self) -> EffectStateExpectation: + """Return an owned correlated expectation.""" + return self._expectation.snapshot() + + def snapshot(self) -> BinaryEffectEvidenceQuery: + """Return an independently owned binary query.""" + return BinaryEffectEvidenceQuery(self.clause, self._expectation) + + +@dataclass(frozen=True, slots=True, eq=False) +class ScalarEffectEvidenceQuery(EffectEvidenceQuery): + """Query for one raw force or wrench magnitude.""" + + clause: ScalarEffectClause + _expectation: EffectStateExpectation + + def __post_init__(self) -> None: + if type(self.clause) is not ScalarEffectClause: + raise TypeError("clause must be a ScalarEffectClause.") + object.__setattr__(self, "clause", self.clause.snapshot()) + object.__setattr__( + self, + "_expectation", + _validate_query(self.clause, self._expectation), + ) + + @property + def evidence_id(self) -> str: + """Return the source clause ID.""" + return self.clause.clause_id + + @property + def source(self) -> EffectEvidenceSourceRef: + """Return an owned source route.""" + return self.clause.source.snapshot() + + @property + def expectation(self) -> EffectStateExpectation: + """Return an owned correlated expectation.""" + return self._expectation.snapshot() + + def snapshot(self) -> ScalarEffectEvidenceQuery: + """Return an independently owned scalar query.""" + return ScalarEffectEvidenceQuery(self.clause, self._expectation) + + +@dataclass(frozen=True, slots=True, eq=False) +class JointStateEvidenceQuery(EffectEvidenceQuery): + """Query for current joint positions and optional velocities.""" + + clause: JointStateEffectClause + _expectation: EffectStateExpectation + + def __post_init__(self) -> None: + if type(self.clause) is not JointStateEffectClause: + raise TypeError("clause must be a JointStateEffectClause.") + object.__setattr__(self, "clause", self.clause.snapshot()) + object.__setattr__( + self, + "_expectation", + _validate_query(self.clause, self._expectation), + ) + + @property + def evidence_id(self) -> str: + """Return the source clause ID.""" + return self.clause.clause_id + + @property + def source(self) -> EffectEvidenceSourceRef: + """Return an owned source route.""" + return self.clause.source.snapshot() + + @property + def expectation(self) -> EffectStateExpectation: + """Return an owned correlated expectation.""" + return self._expectation.snapshot() + + def snapshot(self) -> JointStateEvidenceQuery: + """Return an independently owned joint-state query.""" + return JointStateEvidenceQuery(self.clause, self._expectation) + + +EffectEvidenceQueryValue: TypeAlias = ( + PoseRelationEvidenceQuery + | BinaryEffectEvidenceQuery + | ScalarEffectEvidenceQuery + | JointStateEvidenceQuery +) +"""Closed set of typed clause queries accepted by evidence providers.""" + + +def build_effect_evidence_queries( + spec: SemanticEffectSpec, +) -> tuple[EffectEvidenceQueryValue, ...]: + """Build one independently owned typed query per effect clause. + + Args: + spec: Grounded semantic effect contract. + + Returns: + Queries in the contract's deterministic clause order. + """ + if not isinstance(spec, SemanticEffectSpec): + raise TypeError("spec must be a SemanticEffectSpec.") + queries: list[EffectEvidenceQueryValue] = [] + for clause in spec.clauses: + expectation = spec.state_expectation(clause.expectation_id) + if type(clause) is PoseRelationClause: + queries.append(PoseRelationEvidenceQuery(clause, expectation)) + elif type(clause) is BinaryEffectClause: + queries.append(BinaryEffectEvidenceQuery(clause, expectation)) + elif type(clause) is ScalarEffectClause: + queries.append(ScalarEffectEvidenceQuery(clause, expectation)) + elif type(clause) is JointStateEffectClause: + queries.append(JointStateEvidenceQuery(clause, expectation)) + else: + raise TypeError(f"Unsupported effect clause type {type(clause).__name__}.") + return tuple(queries) + + +class EffectEvidenceProvider(ABC): + """Versioned backend port that acquires a group of exact-source queries.""" + + provider_id: ClassVar[str] + revision: ClassVar[str] + + @abstractmethod + def collect( + self, + queries: tuple[EffectEvidenceQueryValue, ...], + context: EffectEvidenceCollectionContext, + ) -> Mapping[str, EffectEvidenceBatch]: + """Acquire one synchronized batch for every supplied query.""" + + +class EffectEvidenceProviderRegistry: + """Immutable exact-ID/revision registry of live evidence providers.""" + + __slots__ = ("_providers",) + + def __init__(self, providers: Iterable[EffectEvidenceProvider] = ()) -> None: + normalized: dict[tuple[str, str], EffectEvidenceProvider] = {} + for provider in providers: + if not isinstance(provider, EffectEvidenceProvider): + raise TypeError( + "providers must contain EffectEvidenceProvider instances." + ) + provider_id = _validate_identifier( + provider.provider_id, + field_name="EffectEvidenceProvider.provider_id", + ) + revision = _validate_identifier( + provider.revision, + field_name="EffectEvidenceProvider.revision", + ) + key = provider_id, revision + if key in normalized: + raise ValueError(f"Duplicate effect-evidence provider {key!r}.") + normalized[key] = provider + self._providers = MappingProxyType(normalized) + + @property + def providers(self) -> Mapping[tuple[str, str], EffectEvidenceProvider]: + """Return the immutable exact-key provider mapping.""" + return self._providers + + def resolve(self, source: EffectEvidenceSourceRef) -> EffectEvidenceProvider: + """Resolve the exact provider selected by ``source``. + + Args: + source: Versioned evidence route from one effect clause. + + Returns: + Registered provider with the exact ID and revision. + + Raises: + KeyError: If no exact provider version is installed. + """ + if not isinstance(source, EffectEvidenceSourceRef): + raise TypeError("source must be an EffectEvidenceSourceRef.") + key = source.provider_id, source.revision + try: + return self._providers[key] + except KeyError as exc: + raise KeyError( + f"Unknown effect-evidence provider {key!r}; exact versions are " + "required." + ) from exc + + +def _expected_batch_type(query: EffectEvidenceQueryValue) -> type[EffectEvidenceBatch]: + """Return the exact evidence batch type required by one query.""" + if type(query) is PoseRelationEvidenceQuery: + return PoseRelationEvidenceBatch + if type(query) is BinaryEffectEvidenceQuery: + return BinaryEffectEvidenceBatch + if type(query) is ScalarEffectEvidenceQuery: + return ScalarEffectEvidenceBatch + if type(query) is JointStateEvidenceQuery: + return JointStateEvidenceBatch + raise TypeError(f"Unsupported effect evidence query {type(query).__name__}.") + + +class EffectEvidenceCollector: + """Dispatch and normalize a synchronized observation for one effect spec.""" + + __slots__ = ("_registry",) + + def __init__(self, registry: EffectEvidenceProviderRegistry) -> None: + if not isinstance(registry, EffectEvidenceProviderRegistry): + raise TypeError("registry must be an EffectEvidenceProviderRegistry.") + self._registry = registry + + @property + def registry(self) -> EffectEvidenceProviderRegistry: + """Return the immutable provider registry.""" + return self._registry + + def collect( + self, + spec: SemanticEffectSpec, + *, + timestamp: float, + observation_revision: int, + env_ids: torch.Tensor | None = None, + ) -> Mapping[str, EffectEvidenceBatch]: + """Acquire and strictly synchronize evidence for every effect clause. + + Args: + spec: Grounded semantic effect contract. + timestamp: Backend observation time for this acquisition tick. + observation_revision: Runtime-owned observation revision. + env_ids: Optional ordered subset of ``spec.env_ids``. Acquisition + failures must remain present as rows with ``valid=False``. + + Returns: + Immutable mapping keyed exactly by effect clause ID. + """ + if not isinstance(spec, SemanticEffectSpec): + raise TypeError("spec must be a SemanticEffectSpec.") + selected_env_ids = spec.env_ids if env_ids is None else env_ids + context = EffectEvidenceCollectionContext( + timestamp, + observation_revision, + selected_env_ids, + ) + known_ids = set(spec.env_ids.detach().cpu().tolist()) + selected_ids = set(context.env_ids.detach().cpu().tolist()) + if not selected_ids.issubset(known_ids): + raise ValueError("env_ids must be a subset of the effect spec env_ids.") + + queries = build_effect_evidence_queries(spec) + groups: dict[ + tuple[str, str], + list[EffectEvidenceQueryValue], + ] = {} + for query in queries: + source = query.source + self._registry.resolve(source) + groups.setdefault((source.provider_id, source.revision), []).append(query) + + batches: dict[str, EffectEvidenceBatch] = {} + for key, grouped_queries in groups.items(): + provider = self._registry.providers[key] + owned_queries = tuple(query.snapshot() for query in grouped_queries) + supplied = provider.collect(owned_queries, context.snapshot()) + if not isinstance(supplied, Mapping): + raise TypeError( + f"Effect-evidence provider {key!r} must return a mapping." + ) + expected_ids = {query.evidence_id for query in grouped_queries} + if set(supplied) != expected_ids: + raise ValueError( + f"Effect-evidence provider {key!r} must return exactly query " + f"IDs {sorted(expected_ids)}; got {sorted(supplied)}." + ) + for query in grouped_queries: + batch = supplied[query.evidence_id] + expected_type = _expected_batch_type(query) + if type(batch) is not expected_type: + raise TypeError( + f"Evidence {query.evidence_id!r} must be " + f"{expected_type.__name__}." + ) + if batch.evidence_id != query.evidence_id: + raise ValueError( + "Evidence mapping keys must match batch evidence_id values." + ) + if batch.timestamp != context.timestamp: + raise ValueError( + "Every evidence batch must use the collection timestamp." + ) + if batch.observation_revision != context.observation_revision: + raise ValueError( + "Every evidence batch must use the collection revision." + ) + if batch.env_ids.device != context.env_ids.device or not torch.equal( + batch.env_ids, context.env_ids + ): + raise ValueError( + "Every evidence batch must use the ordered collection env_ids." + ) + if type(query) is BinaryEffectEvidenceQuery: + assert type(batch) is BinaryEffectEvidenceBatch + if batch.evidence_kind is not query.clause.evidence_kind: + raise ValueError("Binary evidence kind must match its query.") + if type(query) is ScalarEffectEvidenceQuery: + assert type(batch) is ScalarEffectEvidenceBatch + if batch.evidence_kind is not query.clause.evidence_kind: + raise ValueError("Scalar evidence kind must match its query.") + batches[query.evidence_id] = batch.snapshot() + + expected_all = {query.evidence_id for query in queries} + if set(batches) != expected_all: + raise AssertionError("Evidence dispatch lost one or more effect clauses.") + return MappingProxyType(batches) + + +@dataclass(frozen=True, slots=True, eq=False) +class BinaryEffectObservation: + """Callback-owned raw binary values with explicit row validity.""" + + values: torch.Tensor + valid: torch.Tensor | None = None + acquisition_errors: tuple[str | None, ...] = () + + def __post_init__(self) -> None: + values = self.values + if not isinstance(values, torch.Tensor): + raise TypeError("values must be a torch.Tensor.") + if values.dtype != torch.bool or values.dim() != 1 or values.numel() == 0: + raise ValueError("values must have non-empty bool shape (B,).") + valid = torch.ones_like(values) if self.valid is None else self.valid + if ( + not isinstance(valid, torch.Tensor) + or valid.dtype != torch.bool + or valid.shape != values.shape + or valid.device != values.device + ): + raise ValueError("valid must match values shape, bool dtype, and device.") + errors = self.acquisition_errors or (None,) * values.shape[0] + _validate_observation_errors(valid, errors) + object.__setattr__(self, "values", values.clone()) + object.__setattr__(self, "valid", valid.clone()) + object.__setattr__(self, "acquisition_errors", tuple(errors)) + + +@dataclass(frozen=True, slots=True, eq=False) +class ScalarEffectObservation: + """Callback-owned raw scalar values with explicit row validity.""" + + values: torch.Tensor + valid: torch.Tensor | None = None + acquisition_errors: tuple[str | None, ...] = () + + def __post_init__(self) -> None: + values = self.values + if not isinstance(values, torch.Tensor): + raise TypeError("values must be a torch.Tensor.") + if not values.is_floating_point() or values.dim() != 1 or values.numel() == 0: + raise ValueError("values must have non-empty floating shape (B,).") + valid = ( + torch.ones_like(values, dtype=torch.bool) + if self.valid is None + else self.valid + ) + if ( + not isinstance(valid, torch.Tensor) + or valid.dtype != torch.bool + or valid.shape != values.shape + or valid.device != values.device + ): + raise ValueError("valid must match values shape, bool dtype, and device.") + if not torch.isfinite(values[valid]).all(): + raise ValueError("Valid scalar observations must be finite.") + errors = self.acquisition_errors or (None,) * values.shape[0] + _validate_observation_errors(valid, errors) + object.__setattr__(self, "values", values.clone()) + object.__setattr__(self, "valid", valid.clone()) + object.__setattr__(self, "acquisition_errors", tuple(errors)) + + +@dataclass(frozen=True, slots=True, eq=False) +class JointStateObservation: + """Callback-owned raw joint state with explicit row validity.""" + + positions: torch.Tensor + velocities: torch.Tensor | None = None + valid: torch.Tensor | None = None + acquisition_errors: tuple[str | None, ...] = () + + def __post_init__(self) -> None: + positions = self.positions + if not isinstance(positions, torch.Tensor): + raise TypeError("positions must be a torch.Tensor.") + if ( + not positions.is_floating_point() + or positions.dim() != 2 + or positions.shape[0] == 0 + or positions.shape[1] == 0 + ): + raise ValueError("positions must have non-empty floating shape (B, J).") + valid = ( + torch.ones(positions.shape[0], dtype=torch.bool, device=positions.device) + if self.valid is None + else self.valid + ) + if ( + not isinstance(valid, torch.Tensor) + or valid.dtype != torch.bool + or valid.shape != (positions.shape[0],) + or valid.device != positions.device + ): + raise ValueError("valid must have bool shape (B,) on the positions device.") + if not torch.isfinite(positions[valid]).all(): + raise ValueError("Valid joint positions must be finite.") + velocities = self.velocities + if velocities is not None: + if not isinstance(velocities, torch.Tensor): + raise TypeError("velocities must be a torch.Tensor or None.") + if ( + velocities.shape != positions.shape + or velocities.device != positions.device + ): + raise ValueError("velocities must match positions shape and device.") + if not velocities.is_floating_point(): + raise TypeError("velocities must use a floating-point dtype.") + if not torch.isfinite(velocities[valid]).all(): + raise ValueError("Valid joint velocities must be finite.") + errors = self.acquisition_errors or (None,) * positions.shape[0] + _validate_observation_errors(valid, errors) + object.__setattr__(self, "positions", positions.clone()) + object.__setattr__( + self, + "velocities", + None if velocities is None else velocities.clone(), + ) + object.__setattr__(self, "valid", valid.clone()) + object.__setattr__(self, "acquisition_errors", tuple(errors)) + + +def _validate_observation_errors( + valid: torch.Tensor, + errors: Sequence[str | None], +) -> None: + """Validate explicit per-row acquisition errors.""" + if len(errors) != valid.shape[0]: + raise ValueError("acquisition_errors must contain one entry per row.") + for row, (row_valid, error) in enumerate(zip(valid.tolist(), errors)): + if row_valid and error is not None: + raise ValueError(f"Valid observation row {row} must not carry an error.") + if not row_valid and ( + type(error) is not str or not error or error != error.strip() + ): + raise ValueError( + f"Invalid observation row {row} requires a non-empty error." + ) + + +BinaryObservationCallback: TypeAlias = Callable[ + [BinaryEffectEvidenceQuery, EffectEvidenceCollectionContext], + BinaryEffectObservation, +] +ScalarObservationCallback: TypeAlias = Callable[ + [ScalarEffectEvidenceQuery, EffectEvidenceCollectionContext], + ScalarEffectObservation, +] +ArticulationJointObservationCallback: TypeAlias = Callable[ + [JointStateEvidenceQuery, EffectEvidenceCollectionContext], + JointStateObservation, +] + + +class SceneArticulationEvidenceProvider(EffectEvidenceProvider): + """Typed adapter for scene-articulation joint-state observations. + + Integrations inject either a direct observer or a :class:`SceneProvider` + whose snapshot contains ``ObservedArticulationJointState`` values. + The adapter never discovers live simulator objects from an environment. + Repeated clauses share one synchronized snapshot and one sample per exact + physical address. + """ + + provider_id = SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID + revision = SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION + + def __init__( + self, + observer: ArticulationJointObservationCallback | None = None, + *, + scene_provider: SceneProvider | None = None, + ) -> None: + if (observer is None) == (scene_provider is None): + raise ValueError( + "Exactly one of observer or scene_provider must be supplied." + ) + if observer is not None and not callable(observer): + raise TypeError("observer must be callable or None.") + if scene_provider is not None and not isinstance(scene_provider, SceneProvider): + raise TypeError("scene_provider must implement SceneProvider or be None.") + self._observer = observer + self._scene_provider = scene_provider + + def collect( + self, + queries: tuple[EffectEvidenceQueryValue, ...], + context: EffectEvidenceCollectionContext, + ) -> Mapping[str, EffectEvidenceBatch]: + """Collect synchronized joint state for exact scene addresses.""" + if not isinstance(context, EffectEvidenceCollectionContext): + raise TypeError("context must be an EffectEvidenceCollectionContext.") + if not isinstance(queries, tuple) or not queries: + raise ValueError("queries must be a non-empty tuple.") + owned_queries = tuple(self._validate_query(query) for query in queries) + if len({query.evidence_id for query in owned_queries}) != len(owned_queries): + raise ValueError("queries must have unique evidence IDs.") + + observations: dict[object, JointStateObservation] = {} + batches: dict[str, JointStateEvidenceBatch] = {} + scene_snapshot: SceneSnapshot | None = None + if self._scene_provider is not None: + scene_snapshot = self._scene_provider.snapshot( + timestamp=context.timestamp, + env_ids=context.env_ids.clone(), + ) + if not isinstance(scene_snapshot, SceneSnapshot): + raise TypeError("scene_provider.snapshot() must return SceneSnapshot.") + if scene_snapshot.timestamp != context.timestamp: + raise ValueError( + "Scene snapshot timestamp must match the evidence tick." + ) + for query in owned_queries: + address = query.source.address + assert type(address) is ArticulationJointEvidenceAddress + fingerprint = address.address_fingerprint + observation = observations.get(fingerprint) + if observation is None: + supplied = ( + self._observe_scene_snapshot(query, context, scene_snapshot) + if scene_snapshot is not None + else self._observer(query.snapshot(), context.snapshot()) + ) + if not isinstance(supplied, JointStateObservation): + raise TypeError( + "Articulation observers must return JointStateObservation." + ) + observation = supplied + observations[fingerprint] = observation + self._validate_observation(query, observation, context) + assert observation.valid is not None + batches[query.evidence_id] = JointStateEvidenceBatch( + query.evidence_id, + observation.positions, + observation.velocities, + observation.valid, + observation.acquisition_errors, + context.timestamp, + context.env_ids, + context.observation_revision, + ) + return MappingProxyType(batches) + + @staticmethod + def _observe_scene_snapshot( + query: JointStateEvidenceQuery, + context: EffectEvidenceCollectionContext, + snapshot: SceneSnapshot, + ) -> JointStateObservation: + """Adapt one typed live scene joint into raw effect evidence.""" + address = query.source.address + assert type(address) is ArticulationJointEvidenceAddress + state = snapshot.get_articulation_joint_state( + address.articulation_id, + address.joint_id, + ) + batch_size = int(context.env_ids.numel()) + if state is None: + width = int(query.clause.target_position.shape[-1]) + return JointStateObservation( + positions=torch.zeros( + (batch_size, width), + dtype=query.clause.target_position.dtype, + device=context.env_ids.device, + ), + valid=torch.zeros( + batch_size, + dtype=torch.bool, + device=context.env_ids.device, + ), + acquisition_errors=( + f"Scene snapshot has no live articulation joint " + f"{(address.articulation_id, address.joint_id)!r}.", + ) + * batch_size, + ) + positions = state.position + if positions.dim() == 1: + positions = positions.unsqueeze(0).expand(batch_size, -1) + if positions.shape[0] != batch_size: + raise ValueError( + "Scene articulation observation rows must match context env_ids." + ) + positions = positions.to(device=context.env_ids.device) + valid = state.valid_mask + if valid is None: + valid = torch.ones( + batch_size, + dtype=torch.bool, + device=context.env_ids.device, + ) + else: + valid = valid.to(device=context.env_ids.device) + errors = tuple( + None if bool(row_valid) else "Scene articulation joint row is invalid." + for row_valid in valid.tolist() + ) + return JointStateObservation( + positions=positions, + valid=valid, + acquisition_errors=errors, + ) + + def _validate_query( + self, + query: EffectEvidenceQueryValue, + ) -> JointStateEvidenceQuery: + """Require one exact joint query and matching canonical address.""" + if type(query) is not JointStateEvidenceQuery: + raise TypeError( + "SceneArticulationEvidenceProvider accepts only " + "JointStateEvidenceQuery values." + ) + source = query.source + if (source.provider_id, source.revision) != ( + self.provider_id, + self.revision, + ): + raise ValueError("Query does not select this exact provider version.") + if type(source.address) is not ArticulationJointEvidenceAddress: + raise TypeError( + "Scene articulation evidence requires " + "ArticulationJointEvidenceAddress." + ) + expectation = query.expectation + if type(expectation) is not ArticulationJointStateExpectation: + raise TypeError( + "Scene articulation evidence requires an " + "ArticulationJointStateExpectation." + ) + if ( + expectation.articulation_id != source.address.articulation_id + or expectation.joint_id != source.address.joint_id + ): + raise ValueError( + "Articulation evidence address must exactly match its typed " + "state expectation." + ) + return query.snapshot() + + @staticmethod + def _validate_observation( + query: JointStateEvidenceQuery, + observation: JointStateObservation, + context: EffectEvidenceCollectionContext, + ) -> None: + """Require callback rows/device/width to match the synchronized query.""" + if observation.positions.shape[0] != context.env_ids.numel(): + raise ValueError( + "Articulation observation rows must match context env_ids." + ) + if observation.positions.device != context.env_ids.device: + raise ValueError( + "Articulation observations and context env_ids must share a device." + ) + target_width = int(query.clause.target_position.shape[-1]) + if observation.positions.shape[1] != target_width: + raise ValueError( + f"Joint observation width {observation.positions.shape[1]} does " + f"not match query target width {target_width}." + ) + + +@runtime_checkable +class ControlPartRobotEvidenceSource(Protocol): + """Minimal simulation robot API used by the built-in provider.""" + + def get_qpos(self, name: str | None = None, target: bool = False) -> torch.Tensor: + """Return current robot or control-part joint positions.""" + + def get_qvel(self, name: str | None = None, target: bool = False) -> torch.Tensor: + """Return current robot or control-part joint velocities.""" + + def compute_fk( + self, + qpos: torch.Tensor, + name: str | None = None, + env_ids: Sequence[int] | None = None, + to_matrix: bool = False, + ) -> torch.Tensor: + """Return the selected endpoint pose for current joint positions.""" + + +class ControlPartSimulationEvidenceProvider(EffectEvidenceProvider): + """Built-in simulation acquisition for control-part evidence addresses. + + Pose evidence is computed as ``inverse(object_pose) @ endpoint_pose`` from + one scene snapshot and :meth:`Robot.compute_fk`. Joint evidence reads the + control part's measured positions and velocities. Contact, constraint, + force, and wrench signals are backend-specific, so callers inject raw + observation callbacks. An omitted callback yields explicit invalid rows; + the effect monitor can then retry until its normal deadline. + """ + + provider_id = CONTROL_PART_EVIDENCE_PROVIDER_ID + revision = CONTROL_PART_EVIDENCE_PROVIDER_REVISION + + def __init__( + self, + robot: ControlPartRobotEvidenceSource, + *, + scene_provider: SceneProvider | None = None, + contact_observer: BinaryObservationCallback | None = None, + constraint_observer: BinaryObservationCallback | None = None, + force_observer: ScalarObservationCallback | None = None, + wrench_observer: ScalarObservationCallback | None = None, + ) -> None: + if not isinstance(robot, ControlPartRobotEvidenceSource): + raise TypeError("robot must implement ControlPartRobotEvidenceSource.") + if scene_provider is not None and not isinstance(scene_provider, SceneProvider): + raise TypeError("scene_provider must implement SceneProvider or be None.") + for name, callback in ( + ("contact_observer", contact_observer), + ("constraint_observer", constraint_observer), + ("force_observer", force_observer), + ("wrench_observer", wrench_observer), + ): + if callback is not None and not callable(callback): + raise TypeError(f"{name} must be callable or None.") + self._robot = robot + self._scene_provider = scene_provider + self._binary_observers = { + BinaryEvidenceKind.CONTACT: contact_observer, + BinaryEvidenceKind.CONSTRAINT: constraint_observer, + } + self._scalar_observers = { + ScalarEvidenceKind.FORCE: force_observer, + ScalarEvidenceKind.WRENCH: wrench_observer, + } + + def collect( + self, + queries: tuple[EffectEvidenceQueryValue, ...], + context: EffectEvidenceCollectionContext, + ) -> Mapping[str, EffectEvidenceBatch]: + """Acquire all supplied control-part queries at one observation tick.""" + if not isinstance(context, EffectEvidenceCollectionContext): + raise TypeError("context must be an EffectEvidenceCollectionContext.") + if not isinstance(queries, tuple) or not queries: + raise ValueError("queries must be a non-empty tuple.") + owned_queries = tuple( + self._validate_and_snapshot_query(query) for query in queries + ) + if len({query.evidence_id for query in owned_queries}) != len(owned_queries): + raise ValueError("queries must have unique evidence IDs.") + + pose_queries = tuple( + query for query in owned_queries if type(query) is PoseRelationEvidenceQuery + ) + scene_snapshot = self._capture_scene(pose_queries, context) + joint_cache: dict[str, tuple[torch.Tensor, torch.Tensor]] = {} + endpoint_cache: dict[str, torch.Tensor] = {} + results: dict[str, EffectEvidenceBatch] = {} + for query in owned_queries: + address = query.source.address + assert type(address) is ControlPartEvidenceAddress + if type(query) is PoseRelationEvidenceQuery: + results[query.evidence_id] = self._collect_pose( + query, + address, + context, + scene_snapshot, + joint_cache, + endpoint_cache, + ) + elif type(query) is BinaryEffectEvidenceQuery: + results[query.evidence_id] = self._collect_binary( + query, + address, + context, + ) + elif type(query) is ScalarEffectEvidenceQuery: + results[query.evidence_id] = self._collect_scalar( + query, + address, + context, + ) + elif type(query) is JointStateEvidenceQuery: + results[query.evidence_id] = self._collect_joint_state( + query, + address, + context, + joint_cache, + ) + else: + raise TypeError(f"Unsupported query type {type(query).__name__}.") + return MappingProxyType(results) + + def _validate_and_snapshot_query( + self, + query: EffectEvidenceQueryValue, + ) -> EffectEvidenceQueryValue: + """Require the exact built-in route and a control-part address.""" + if type(query) not in { + PoseRelationEvidenceQuery, + BinaryEffectEvidenceQuery, + ScalarEffectEvidenceQuery, + JointStateEvidenceQuery, + }: + raise TypeError("queries must contain exact typed evidence queries.") + source = query.source + if (source.provider_id, source.revision) != ( + self.provider_id, + self.revision, + ): + raise ValueError("Query does not select this exact provider version.") + if type(source.address) is not ControlPartEvidenceAddress: + raise TypeError( + "ControlPartSimulationEvidenceProvider requires " + "ControlPartEvidenceAddress values." + ) + return query.snapshot() + + def _capture_scene( + self, + queries: tuple[PoseRelationEvidenceQuery, ...], + context: EffectEvidenceCollectionContext, + ) -> SceneSnapshot | None: + """Capture one shared scene snapshot if pose queries need it.""" + if not queries or self._scene_provider is None: + return None + snapshot = self._scene_provider.snapshot( + timestamp=context.timestamp, + env_ids=context.env_ids.clone(), + ) + if not isinstance(snapshot, SceneSnapshot): + raise TypeError("scene_provider.snapshot() must return SceneSnapshot.") + if snapshot.timestamp != context.timestamp: + raise ValueError("Scene snapshot timestamp must match the evidence tick.") + return snapshot + + @staticmethod + def _require_channel( + address: ControlPartEvidenceAddress, + expected: str, + *, + evidence_id: str, + ) -> None: + """Reject clause/address channel mismatches before acquisition.""" + if address.channel != expected: + raise ValueError( + f"Evidence query {evidence_id!r} requires channel {expected!r}, " + f"not {address.channel!r}." + ) + + @staticmethod + def _select_rows( + value: torch.Tensor, context: EffectEvidenceCollectionContext + ) -> torch.Tensor: + """Select simulator rows addressed by the context's integer env IDs.""" + if not isinstance(value, torch.Tensor): + raise TypeError("Robot state accessors must return torch.Tensor values.") + if value.dim() != 2 or value.shape[0] == 0 or value.shape[1] == 0: + raise ValueError("Robot joint state must have non-empty shape (N, J).") + indices = context.env_ids.to(device=value.device) + if bool((indices < 0).any()) or int(indices.max().item()) >= value.shape[0]: + raise ValueError( + "The built-in simulation provider requires env_ids to address " + "valid simulator batch rows." + ) + selected = value.index_select(0, indices) + if selected.device != context.env_ids.device: + raise ValueError( + "Robot evidence and collection env_ids must share a device." + ) + return selected.clone() + + def _joint_state( + self, + control_part: str, + context: EffectEvidenceCollectionContext, + cache: dict[str, tuple[torch.Tensor, torch.Tensor]], + ) -> tuple[torch.Tensor, torch.Tensor]: + """Read and cache measured positions and velocities for one part.""" + cached = cache.get(control_part) + if cached is not None: + return cached[0].clone(), cached[1].clone() + qpos = self._select_rows( + self._robot.get_qpos(name=control_part, target=False), + context, + ) + qvel = self._select_rows( + self._robot.get_qvel(name=control_part, target=False), + context, + ) + if qvel.shape != qpos.shape or qvel.device != qpos.device: + raise ValueError("Robot qvel must match qpos shape and device.") + if not qpos.is_floating_point() or not qvel.is_floating_point(): + raise TypeError("Robot qpos and qvel must use floating-point dtypes.") + cache[control_part] = qpos.clone(), qvel.clone() + return qpos, qvel + + def _endpoint_pose( + self, + control_part: str, + context: EffectEvidenceCollectionContext, + joint_cache: dict[str, tuple[torch.Tensor, torch.Tensor]], + endpoint_cache: dict[str, torch.Tensor], + ) -> torch.Tensor: + """Compute and cache one control-part endpoint pose.""" + cached = endpoint_cache.get(control_part) + if cached is not None: + return cached.clone() + qpos, _ = self._joint_state(control_part, context, joint_cache) + pose = self._robot.compute_fk( + qpos=qpos, + name=control_part, + env_ids=context.env_ids.detach().cpu().tolist(), + to_matrix=True, + ) + if not isinstance(pose, torch.Tensor): + raise TypeError("robot.compute_fk() must return a torch.Tensor.") + if pose.shape != (context.env_ids.numel(), 4, 4): + raise ValueError("robot.compute_fk() must return shape (B, 4, 4).") + if pose.device != context.env_ids.device: + raise ValueError( + "Endpoint poses and collection env_ids must share a device." + ) + endpoint_cache[control_part] = pose.clone() + return pose + + @staticmethod + def _pose_entity_id(query: PoseRelationEvidenceQuery) -> str: + """Resolve the canonical scene entity observed by a pose relation.""" + expectation = query.expectation + if type(expectation) is HeldObjectStateExpectation: + return expectation.object_id + if type(expectation) is ArticulationJointStateExpectation: + return expectation.articulation_id + raise ValueError( + "Pose relation evidence requires an expectation with one canonical " + "scene entity." + ) + + def _collect_pose( + self, + query: PoseRelationEvidenceQuery, + address: ControlPartEvidenceAddress, + context: EffectEvidenceCollectionContext, + scene_snapshot: SceneSnapshot | None, + joint_cache: dict[str, tuple[torch.Tensor, torch.Tensor]], + endpoint_cache: dict[str, torch.Tensor], + ) -> PoseRelationEvidenceBatch: + """Collect object-to-endpoint transforms from scene and FK state.""" + self._require_channel( + address, + POSE_RELATION_EFFECT_CHANNEL, + evidence_id=query.evidence_id, + ) + if scene_snapshot is None: + return self._invalid_pose( + query.evidence_id, + context, + "No scene provider is configured for pose-relation evidence.", + ) + entity_id = self._pose_entity_id(query) + try: + state = scene_snapshot.entities[entity_id] + except KeyError as exc: + raise KeyError( + f"Pose evidence references missing scene entity {entity_id!r}." + ) from exc + object_pose = state.pose + batch_size = int(context.env_ids.numel()) + if object_pose.shape == (4, 4): + object_pose = object_pose.unsqueeze(0).expand(batch_size, -1, -1) + if object_pose.shape != (batch_size, 4, 4): + raise ValueError( + f"Scene entity {entity_id!r} pose must have shape (B, 4, 4)." + ) + endpoint_pose = self._endpoint_pose( + address.control_part, + context, + joint_cache, + endpoint_cache, + ) + object_pose = object_pose.to( + device=endpoint_pose.device, + dtype=endpoint_pose.dtype, + ) + relative = torch.bmm(pose_inv(object_pose), endpoint_pose) + valid = torch.full( + (batch_size,), + state.confidence > 0.0, + dtype=torch.bool, + device=relative.device, + ) + errors: tuple[str | None, ...] + if bool(valid.all()): + errors = (None,) * batch_size + else: + errors = ( + f"Scene entity {entity_id!r} has zero observation confidence.", + ) * batch_size + return PoseRelationEvidenceBatch( + query.evidence_id, + relative, + valid, + errors, + context.timestamp, + context.env_ids, + context.observation_revision, + ) + + def _collect_binary( + self, + query: BinaryEffectEvidenceQuery, + address: ControlPartEvidenceAddress, + context: EffectEvidenceCollectionContext, + ) -> BinaryEffectEvidenceBatch: + """Collect callback-provided contact or constraint state.""" + expected_channel = ( + CONTACT_EFFECT_CHANNEL + if query.clause.evidence_kind is BinaryEvidenceKind.CONTACT + else CONSTRAINT_EFFECT_CHANNEL + ) + self._require_channel(address, expected_channel, evidence_id=query.evidence_id) + callback = self._binary_observers[query.clause.evidence_kind] + if callback is None: + return self._invalid_binary( + query, + context, + f"No {query.clause.evidence_kind.value} observation callback is configured.", + ) + observation = callback(query.snapshot(), context.snapshot()) + if not isinstance(observation, BinaryEffectObservation): + raise TypeError( + "Binary observation callbacks must return BinaryEffectObservation." + ) + self._validate_callback_rows(observation.values, context) + assert observation.valid is not None + return BinaryEffectEvidenceBatch( + query.evidence_id, + query.clause.evidence_kind, + observation.values, + observation.valid, + observation.acquisition_errors, + context.timestamp, + context.env_ids, + context.observation_revision, + ) + + def _collect_scalar( + self, + query: ScalarEffectEvidenceQuery, + address: ControlPartEvidenceAddress, + context: EffectEvidenceCollectionContext, + ) -> ScalarEffectEvidenceBatch: + """Collect callback-provided force or wrench magnitude.""" + self._require_channel( + address, FORCE_EFFECT_CHANNEL, evidence_id=query.evidence_id + ) + callback = self._scalar_observers[query.clause.evidence_kind] + if callback is None: + return self._invalid_scalar( + query, + context, + f"No {query.clause.evidence_kind.value} observation callback is configured.", + ) + observation = callback(query.snapshot(), context.snapshot()) + if not isinstance(observation, ScalarEffectObservation): + raise TypeError( + "Scalar observation callbacks must return ScalarEffectObservation." + ) + self._validate_callback_rows(observation.values, context) + assert observation.valid is not None + return ScalarEffectEvidenceBatch( + query.evidence_id, + query.clause.evidence_kind, + observation.values, + observation.valid, + observation.acquisition_errors, + context.timestamp, + context.env_ids, + context.observation_revision, + ) + + def _collect_joint_state( + self, + query: JointStateEvidenceQuery, + address: ControlPartEvidenceAddress, + context: EffectEvidenceCollectionContext, + joint_cache: dict[str, tuple[torch.Tensor, torch.Tensor]], + ) -> JointStateEvidenceBatch: + """Collect measured control-part joint positions and velocities.""" + self._require_channel( + address, + JOINT_STATE_EFFECT_CHANNEL, + evidence_id=query.evidence_id, + ) + qpos, qvel = self._joint_state(address.control_part, context, joint_cache) + target_width = int(query.clause.target_position.shape[-1]) + if qpos.shape[1] != target_width: + raise ValueError( + f"Joint evidence width {qpos.shape[1]} does not match query target " + f"width {target_width}." + ) + batch_size = int(context.env_ids.numel()) + return JointStateEvidenceBatch( + query.evidence_id, + qpos, + qvel, + torch.ones(batch_size, dtype=torch.bool, device=qpos.device), + (None,) * batch_size, + context.timestamp, + context.env_ids, + context.observation_revision, + ) + + @staticmethod + def _validate_callback_rows( + values: torch.Tensor, + context: EffectEvidenceCollectionContext, + ) -> None: + """Require callback values to follow the synchronized context rows.""" + if values.shape != context.env_ids.shape: + raise ValueError("Observation callback rows must match context env_ids.") + if values.device != context.env_ids.device: + raise ValueError( + "Observation callback values and context env_ids must share a device." + ) + + @staticmethod + def _invalid_pose( + evidence_id: str, + context: EffectEvidenceCollectionContext, + message: str, + ) -> PoseRelationEvidenceBatch: + """Create explicit invalid rows for unavailable pose acquisition.""" + batch_size = int(context.env_ids.numel()) + poses = torch.eye( + 4, + dtype=torch.float32, + device=context.env_ids.device, + ).expand(batch_size, -1, -1) + return PoseRelationEvidenceBatch( + evidence_id, + poses, + torch.zeros(batch_size, dtype=torch.bool, device=context.env_ids.device), + (message,) * batch_size, + context.timestamp, + context.env_ids, + context.observation_revision, + ) + + @staticmethod + def _invalid_binary( + query: BinaryEffectEvidenceQuery, + context: EffectEvidenceCollectionContext, + message: str, + ) -> BinaryEffectEvidenceBatch: + """Create explicit invalid rows for an unavailable binary channel.""" + batch_size = int(context.env_ids.numel()) + return BinaryEffectEvidenceBatch( + query.evidence_id, + query.clause.evidence_kind, + torch.zeros(batch_size, dtype=torch.bool, device=context.env_ids.device), + torch.zeros(batch_size, dtype=torch.bool, device=context.env_ids.device), + (message,) * batch_size, + context.timestamp, + context.env_ids, + context.observation_revision, + ) + + @staticmethod + def _invalid_scalar( + query: ScalarEffectEvidenceQuery, + context: EffectEvidenceCollectionContext, + message: str, + ) -> ScalarEffectEvidenceBatch: + """Create explicit invalid rows for an unavailable scalar channel.""" + batch_size = int(context.env_ids.numel()) + return ScalarEffectEvidenceBatch( + query.evidence_id, + query.clause.evidence_kind, + torch.zeros(batch_size, dtype=torch.float32, device=context.env_ids.device), + torch.zeros(batch_size, dtype=torch.bool, device=context.env_ids.device), + (message,) * batch_size, + context.timestamp, + context.env_ids, + context.observation_revision, + ) + + +__all__ = [ + "ArticulationJointObservationCallback", + "BinaryEffectEvidenceQuery", + "BinaryEffectObservation", + "BinaryObservationCallback", + "ControlPartRobotEvidenceSource", + "ControlPartSimulationEvidenceProvider", + "EffectEvidenceCollectionContext", + "EffectEvidenceCollector", + "EffectEvidenceProvider", + "EffectEvidenceProviderRegistry", + "EffectEvidenceQuery", + "EffectEvidenceQueryValue", + "JointStateEvidenceQuery", + "JointStateObservation", + "PoseRelationEvidenceQuery", + "ScalarEffectEvidenceQuery", + "ScalarEffectObservation", + "ScalarObservationCallback", + "SceneArticulationEvidenceProvider", + "build_effect_evidence_queries", +] diff --git a/embodichain/lab/sim/skills/integration.py b/embodichain/lab/sim/skills/integration.py new file mode 100644 index 000000000..12f20d8be --- /dev/null +++ b/embodichain/lab/sim/skills/integration.py @@ -0,0 +1,1340 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Two-phase static and live semantic integration validation.""" + +from __future__ import annotations + +from collections.abc import Iterable, Mapping +from dataclasses import dataclass, field, replace +from types import MappingProxyType +from typing import TypeVar + +from embodichain.lab.sim.atomic_actions import ( + AtomicActionEngine, + DynamicCollisionMode, + DisjointResourceSlots, + DisjointSlotEndpoints, + HandOverOptions, + PickUpOptions, + SkillResourceSlot, +) + +from .calls import ( + HandOver, + OperateArticulation, + Pick, + Place, + RegisteredSemanticCall, + SemanticCallCatalog, + SemanticCallDescriptor, + SemanticCallSpec, +) +from .profiles import ( + BoundRobotSkillProfile, + ControlPartEndpoint, + ResolvedSkillBinding, + ResourceEndpoint, + ResourceEndpointAdapter, + RobotResource, + RobotSkillProfile, + SkillPolicyPreset, +) +from .scene import ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + GRASP_AFFORDANCE_CAPABILITY, + PLACE_IN_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, + SceneAffordanceRef, + SceneArticulationRef, + SceneCollisionWorldMode, + SceneEntityMetadata, + SceneEntityRef, + SceneLinkRef, + SceneObjectRef, + SceneRegistry, + _SceneMetadataIndex, +) + +PathPart = str | int +RefT = TypeVar("RefT", bound=SceneEntityRef) + + +def _validate_identifier(value: str, *, field_name: str) -> str: + """Return one exact, non-empty identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +def _render_path(path: tuple[PathPart, ...]) -> str: + """Render tuple path components in configuration notation.""" + output = "" + for part in path: + if isinstance(part, int): + output += f"[{part}]" + elif not output: + output = part + else: + output += f".{part}" + return output or "" + + +@dataclass(frozen=True, slots=True) +class SemanticDiagnostic: + """Structured deterministic semantic-integration diagnostic. + + Args: + code: Stable machine-readable failure code. + path: Complete configuration or program path. + message: Human-readable explanation. + candidates: Canonical candidate IDs, sorted when applicable. + """ + + code: str + path: tuple[PathPart, ...] + message: str + candidates: tuple[str, ...] = () + + def __post_init__(self) -> None: + _validate_identifier(self.code, field_name="SemanticDiagnostic.code") + if isinstance(self.path, (str, bytes)): + raise TypeError("SemanticDiagnostic.path must be a tuple of components.") + path = tuple(self.path) + if not all( + (isinstance(part, str) and part) + or (isinstance(part, int) and not isinstance(part, bool)) + for part in path + ): + raise ValueError("SemanticDiagnostic.path contains an invalid component.") + object.__setattr__(self, "path", path) + if not isinstance(self.message, str) or not self.message: + raise ValueError("SemanticDiagnostic.message must be non-empty.") + candidates = tuple(self.candidates) + if not all(isinstance(candidate, str) for candidate in candidates): + raise TypeError("SemanticDiagnostic.candidates must contain strings.") + object.__setattr__(self, "candidates", tuple(sorted(candidates))) + + @property + def rendered_path(self) -> str: + """Return the path in dotted/indexed notation.""" + return _render_path(self.path) + + +class SemanticValidationError(ValueError): + """Raise one structured error at a static or live integration boundary.""" + + def __init__(self, diagnostic: SemanticDiagnostic) -> None: + if not isinstance(diagnostic, SemanticDiagnostic): + raise TypeError("diagnostic must be a SemanticDiagnostic.") + self.diagnostic = diagnostic + super().__init__(f"{diagnostic.rendered_path}: {diagnostic.message}") + + +@dataclass(frozen=True, slots=True) +class SceneEntityManifest(SceneEntityMetadata): + """Static scene declaration sharing the canonical metadata value model.""" + + @classmethod + def from_metadata(cls, metadata: SceneEntityMetadata) -> SceneEntityManifest: + """Copy one canonical provider-free metadata value.""" + if not isinstance(metadata, SceneEntityMetadata): + raise TypeError("metadata must be a SceneEntityMetadata.") + return cls( + ref=metadata.ref, + aliases=metadata.aliases, + parent=metadata.parent, + native_name=metadata.native_name, + dynamics=metadata.dynamics, + collision_role=metadata.collision_role, + semantic_type=metadata.semantic_type, + affordance_capabilities=metadata.affordance_capabilities, + default_affordances=metadata.default_affordances, + affordance_payload_type=metadata.affordance_payload_type, + affordance_revision=metadata.affordance_revision, + relative_pose=metadata.relative_pose, + ) + + +@dataclass(frozen=True, slots=True, init=False) +class SceneManifest: + """Immutable provider-free scene catalog used before simulation starts.""" + + _entries: tuple[SceneEntityManifest, ...] + _index: _SceneMetadataIndex + collision_world_mode: SceneCollisionWorldMode | None + + def __init__( + self, + entries: Iterable[SceneEntityManifest] = (), + *, + collision_world_mode: SceneCollisionWorldMode | None = None, + ) -> None: + if isinstance(entries, (str, bytes)): + raise TypeError("entries must be an iterable of scene manifests.") + try: + supplied = tuple(entries) + except TypeError as exc: + raise TypeError("entries must be an iterable of scene manifests.") from exc + if not all(type(entry) is SceneEntityManifest for entry in supplied): + raise TypeError("entries must contain exact SceneEntityManifest values.") + if collision_world_mode is not None and not isinstance( + collision_world_mode, + SceneCollisionWorldMode, + ): + raise TypeError( + "collision_world_mode must be a SceneCollisionWorldMode or None." + ) + object.__setattr__(self, "_entries", supplied) + object.__setattr__(self, "_index", _SceneMetadataIndex(supplied)) + object.__setattr__(self, "collision_world_mode", collision_world_mode) + + @property + def entries(self) -> tuple[SceneEntityManifest, ...]: + """Return immutable provider-free entries in declaration order.""" + return self._entries + + @classmethod + def from_registry(cls, registry: SceneRegistry) -> SceneManifest: + """Project a live registry without observing any dynamic provider.""" + if not isinstance(registry, SceneRegistry): + raise TypeError("registry must be a SceneRegistry.") + return cls( + ( + SceneEntityManifest.from_metadata(metadata) + for metadata in registry.entity_metadata + ), + collision_world_mode=registry.collision_world_mode, + ) + + def resolve( + self, + identifier: str | SceneEntityRef, + *, + expected_type: type[RefT] = SceneEntityRef, + path: tuple[PathPart, ...] = (), + ) -> RefT: + """Resolve one canonical or alias reference with pathful diagnostics.""" + if isinstance(identifier, SceneEntityRef): + candidate_id = identifier.entity_id + supplied_type: type[SceneEntityRef] | None = type(identifier) + elif isinstance(identifier, str): + _validate_identifier(identifier, field_name="scene identifier") + candidate_id = self._index.aliases.get(identifier, identifier) + supplied_type = None + else: + raise SemanticValidationError( + SemanticDiagnostic( + "invalid_entity_reference", + path, + "Expected a scene identifier or typed scene reference.", + ) + ) + entry = self._index.by_id.get(candidate_id) + if entry is None: + raise SemanticValidationError( + SemanticDiagnostic( + "unknown_entity", + path, + f"Unknown scene entity {candidate_id!r}.", + tuple(self._index.by_id), + ) + ) + if supplied_type is not None and supplied_type is not type(entry.ref): + raise SemanticValidationError( + SemanticDiagnostic( + "entity_type_mismatch", + path, + f"Scene entity {candidate_id!r} is " + f"{type(entry.ref).__name__}, not {supplied_type.__name__}.", + ) + ) + if not isinstance(entry.ref, expected_type): + raise SemanticValidationError( + SemanticDiagnostic( + "entity_type_mismatch", + path, + f"Scene entity {candidate_id!r} is " + f"{type(entry.ref).__name__}, not {expected_type.__name__}.", + ) + ) + return entry.ref # type: ignore[return-value] + + def lookup( + self, + identifier: str | SceneEntityRef, + *, + expected_type: type[RefT] = SceneEntityRef, + path: tuple[PathPart, ...] = (), + ) -> SceneEntityManifest: + """Return one static entry after canonical typed resolution.""" + ref = self.resolve(identifier, expected_type=expected_type, path=path) + return self._index.by_id[ref.entity_id] # type: ignore[return-value] + + def resolve_affordance( + self, + parent: str | SceneEntityRef, + *, + capability: str, + explicit: str | SceneAffordanceRef | None = None, + path: tuple[PathPart, ...] = (), + ) -> SceneAffordanceRef: + """Resolve one affordance using the same strict rule as SceneRegistry.""" + parent_ref = self.resolve(parent, path=path) + _validate_identifier(capability, field_name="affordance capability") + candidates = self._index.affordances_by_parent_capability.get( + (parent_ref.entity_id, capability), + (), + ) + if explicit is not None: + selected = self.resolve( + explicit, + expected_type=SceneAffordanceRef, + path=path, + ) + entry = self._index.by_id[selected.entity_id] + if entry.parent != parent_ref: + raise SemanticValidationError( + SemanticDiagnostic( + "affordance_parent_mismatch", + path, + f"Affordance {selected.entity_id!r} is not a direct child " + f"of {parent_ref.entity_id!r}.", + tuple(candidate.entity_id for candidate in candidates), + ) + ) + if capability not in entry.affordance_capabilities: + raise SemanticValidationError( + SemanticDiagnostic( + "unsupported_affordance", + path, + f"Affordance {selected.entity_id!r} does not support " + f"{capability!r}.", + tuple(candidate.entity_id for candidate in candidates), + ) + ) + return selected + if not candidates: + raise SemanticValidationError( + SemanticDiagnostic( + "missing_affordance", + path, + f"Scene entity {parent_ref.entity_id!r} has no affordance for " + f"{capability!r}.", + ) + ) + if len(candidates) == 1: + return candidates[0] + parent_entry = self._index.by_id[parent_ref.entity_id] + default = parent_entry.default_affordances.get(capability) + if default is not None: + return default + raise SemanticValidationError( + SemanticDiagnostic( + "ambiguous_affordance", + path, + f"Multiple affordances support {capability!r}; configure a " + "scoped default or select one explicitly.", + tuple(candidate.entity_id for candidate in candidates), + ) + ) + + def validate_registry( + self, + registry: SceneRegistry, + *, + path: tuple[PathPart, ...] = ("integration", "scene_registry"), + ) -> None: + """Require a live registry to match this provider-free declaration.""" + try: + live = SceneManifest.from_registry(registry) + except Exception as exc: # noqa: BLE001 - normalize integration failures + raise SemanticValidationError( + SemanticDiagnostic( + "invalid_scene_registry", + path, + f"Could not project the live scene registry: {exc}", + ) + ) from exc + static_ids = set(self._index.by_id) + live_ids = set(live._index.by_id) + if static_ids != live_ids: + raise SemanticValidationError( + SemanticDiagnostic( + "scene_manifest_mismatch", + path, + "Live scene IDs differ from the static manifest; " + f"missing={sorted(static_ids - live_ids)}, " + f"extra={sorted(live_ids - static_ids)}.", + ) + ) + for entity_id in sorted(static_ids): + if self._index.by_id[entity_id] != live._index.by_id[entity_id]: + raise SemanticValidationError( + SemanticDiagnostic( + "scene_manifest_mismatch", + (*path, entity_id), + "Live scene metadata differs from the static manifest.", + ) + ) + if self.collision_world_mode is not live.collision_world_mode: + raise SemanticValidationError( + SemanticDiagnostic( + "scene_manifest_mismatch", + (*path, "collision_world_mode"), + "Live collision-world mode differs from the static manifest.", + ) + ) + + +@dataclass(frozen=True, slots=True) +class LinkedSemanticCall: + """Provider-free static link result for one semantic call.""" + + call: SemanticCallSpec + descriptor: SemanticCallDescriptor + preset_id: str + affordances: Mapping[str, SceneAffordanceRef] = field(default_factory=dict) + + def __post_init__(self) -> None: + if type(self.call) not in ( + Pick, + Place, + HandOver, + OperateArticulation, + RegisteredSemanticCall, + ): + raise TypeError("call must be an exact supported semantic call value.") + if type(self.descriptor) is not SemanticCallDescriptor: + raise TypeError("descriptor must be exactly SemanticCallDescriptor.") + if type(self.call) is not self.descriptor.spec_type or ( + self.call.semantic_id != self.descriptor.call_id + ): + raise ValueError( + "call type and semantic ID must match the linked descriptor." + ) + _validate_identifier(self.preset_id, field_name="LinkedSemanticCall.preset_id") + if not isinstance(self.affordances, Mapping): + raise TypeError("affordances must be a mapping.") + normalized: dict[str, SceneAffordanceRef] = {} + for role, affordance in self.affordances.items(): + _validate_identifier(role, field_name="affordance roles") + if type(affordance) is not SceneAffordanceRef: + raise TypeError("affordances values must be SceneAffordanceRef values.") + normalized[role] = affordance + object.__setattr__(self, "affordances", MappingProxyType(normalized)) + + +@dataclass(frozen=True, slots=True, init=False) +class BoundSemanticCall: + """Factory-owned call linked to one installed engine/profile combination.""" + + linked: LinkedSemanticCall + binding: ResolvedSkillBinding + preset: SkillPolicyPreset + _robot_profile: BoundRobotSkillProfile = field(repr=False, compare=False) + + def __init__(self, *args: object, **kwargs: object) -> None: + """Reject construction outside :class:`BoundSemanticIntegration`.""" + del args, kwargs + raise TypeError( + "BoundSemanticCall values are created by " + "BoundSemanticIntegration.link_call()." + ) + + @classmethod + def _create( + cls, + *, + linked: LinkedSemanticCall, + binding: ResolvedSkillBinding, + preset: SkillPolicyPreset, + robot_profile: BoundRobotSkillProfile, + ) -> BoundSemanticCall: + """Create and validate one engine/profile-owned result.""" + instance = object.__new__(cls) + object.__setattr__(instance, "linked", linked) + object.__setattr__(instance, "binding", binding) + object.__setattr__(instance, "preset", preset) + object.__setattr__(instance, "_robot_profile", robot_profile) + instance._validate() + return instance + + def _validate(self) -> None: + """Validate the static and live ownership links.""" + if not isinstance(self.linked, LinkedSemanticCall): + raise TypeError("linked must be a LinkedSemanticCall.") + if not isinstance(self.binding, ResolvedSkillBinding): + raise TypeError("binding must be a ResolvedSkillBinding.") + if not isinstance(self.preset, SkillPolicyPreset): + raise TypeError("preset must be a SkillPolicyPreset.") + if self.binding.skill_id != self.linked.descriptor.skill_id: + raise ValueError( + "binding skill_id must match the linked semantic descriptor." + ) + if self.preset.preset_id != self.linked.preset_id: + raise ValueError("preset ID must match the statically linked preset.") + if not isinstance(self._robot_profile, BoundRobotSkillProfile): + raise TypeError("robot_profile must be a BoundRobotSkillProfile.") + if ( + self.binding.action_binding.owner_id + != self._robot_profile.engine.binding_owner_id + ): + raise ValueError("binding belongs to a different action engine.") + + @property + def robot_profile(self) -> BoundRobotSkillProfile: + """Return the exact bound profile that produced this call.""" + return self._robot_profile + + +@dataclass(frozen=True, slots=True) +class SemanticIntegrationManifest: + """Static scene/profile/catalog declaration validated before execution. + + Args: + scene: Provider-free scene manifest. + robot_profile: Declarative robot resource/profile snapshot. + call_catalog: Discoverable semantic call descriptors. + runtime_preset: Optional integration-wide policy preset override. + """ + + scene: SceneManifest + robot_profile: RobotSkillProfile + call_catalog: SemanticCallCatalog + runtime_preset: str | None = None + + def __post_init__(self) -> None: + if type(self.scene) is not SceneManifest: + raise TypeError("scene must be exactly SceneManifest.") + if type(self.robot_profile) is not RobotSkillProfile: + raise TypeError("robot_profile must be exactly RobotSkillProfile.") + if type(self.call_catalog) is not SemanticCallCatalog: + raise TypeError("call_catalog must be exactly SemanticCallCatalog.") + known_semantic_ids = set(self.call_catalog.descriptors) + for preset_id, preset in self.robot_profile.presets.items(): + unknown_monitor_ids = sorted( + set(preset.effect_monitors).difference(known_semantic_ids) + ) + if unknown_monitor_ids: + semantic_id = unknown_monitor_ids[0] + raise SemanticValidationError( + SemanticDiagnostic( + "unknown_effect_monitor_call", + ( + "integration", + "robot_profile", + "presets", + preset_id, + "effect_monitors", + semantic_id, + ), + f"Effect monitor configuration references unknown semantic " + f"call {semantic_id!r}.", + tuple(self.call_catalog.descriptors), + ) + ) + unknown_option_ids = sorted( + set(preset.action_option_templates).difference(known_semantic_ids) + ) + if unknown_option_ids: + semantic_id = unknown_option_ids[0] + raise SemanticValidationError( + SemanticDiagnostic( + "unknown_action_option_call", + ( + "integration", + "robot_profile", + "presets", + preset_id, + "action_option_templates", + semantic_id, + ), + f"Action-option configuration references unknown semantic " + f"call {semantic_id!r}.", + tuple(self.call_catalog.descriptors), + ) + ) + for semantic_id, options in preset.action_option_templates.items(): + descriptor = self.call_catalog.descriptors[semantic_id] + target = descriptor.target_descriptor + assert target is not None + option_path = ( + "integration", + "robot_profile", + "presets", + preset_id, + "action_option_templates", + semantic_id, + ) + if type(options) is not target.options_type: + raise SemanticValidationError( + SemanticDiagnostic( + "incompatible_action_option_template", + option_path, + f"Semantic call {semantic_id!r} targets options type " + f"{target.options_type.__name__}, not " + f"{type(options).__name__}.", + (target.options_type.__name__,), + ) + ) + if semantic_id == Pick.call_kind: + assert type(options) is PickUpOptions + if options.downstream_object_target_poses: + raise SemanticValidationError( + SemanticDiagnostic( + "reserved_action_option_field", + (*option_path, "downstream_object_target_poses"), + "Pick downstream targets are compiler-owned and " + "the template field must be empty.", + ) + ) + if semantic_id == HandOver.call_kind: + assert type(options) is HandOverOptions + if options.middle_object_pose is not None: + raise SemanticValidationError( + SemanticDiagnostic( + "reserved_action_option_field", + (*option_path, "middle_object_pose"), + "HandOver middle_object_pose is compiler-owned and " + "the template field must be None.", + ) + ) + if options.final_object_pose is not None: + raise SemanticValidationError( + SemanticDiagnostic( + "reserved_action_option_field", + (*option_path, "final_object_pose"), + "HandOver final_object_pose is compiler-owned and " + "the template field must be None.", + ) + ) + if self.runtime_preset is not None: + _validate_identifier( + self.runtime_preset, + field_name="runtime_preset", + ) + if self.runtime_preset not in self.robot_profile.presets: + raise SemanticValidationError( + SemanticDiagnostic( + "unknown_preset", + ("integration", "runtime_preset"), + f"Unknown runtime preset {self.runtime_preset!r}.", + tuple(self.robot_profile.presets), + ) + ) + + def link_call( + self, + call: SemanticCallSpec, + *, + path: tuple[PathPart, ...] = ("call",), + ) -> LinkedSemanticCall: + """Resolve static refs, affordances, and declared resource structure. + + This method never observes scene providers, constructs an engine, + samples a grasp, or runs a planner. + """ + if not isinstance(call, SemanticCallSpec): + raise TypeError("call must be a SemanticCallSpec.") + try: + descriptor = self.call_catalog.discover(call) + except (KeyError, TypeError, ValueError) as exc: + raise SemanticValidationError( + SemanticDiagnostic( + "unknown_call", + (*path, "kind"), + str(exc), + tuple(self.call_catalog.descriptors), + ) + ) from exc + + affordances: dict[str, SceneAffordanceRef] = {} + if isinstance(call, Pick): + object_ref = self.scene.resolve( + call.object, + expected_type=SceneObjectRef, + path=(*path, "object"), + ) + grasp = self.scene.resolve_affordance( + object_ref, + capability=GRASP_AFFORDANCE_CAPABILITY, + explicit=call.grasp, + path=(*path, "grasp"), + ) + normalized_call: SemanticCallSpec = replace( + call, + object=object_ref, + grasp=grasp, + ) + affordances["grasp"] = grasp + elif isinstance(call, Place): + object_ref = self.scene.resolve( + call.object, + expected_type=SceneObjectRef, + path=(*path, "object"), + ) + replacements: dict[str, object] = {"object": object_ref} + if call.on is not None: + destination, affordance = self._link_relation( + call.on, + capability=PLACE_ON_AFFORDANCE_CAPABILITY, + path=(*path, "on"), + ) + replacements["on"] = destination + affordances["destination"] = affordance + elif call.inside is not None: + destination, affordance = self._link_relation( + call.inside, + capability=PLACE_IN_AFFORDANCE_CAPABILITY, + path=(*path, "inside"), + ) + replacements["inside"] = destination + affordances["destination"] = affordance + normalized_call = replace(call, **replacements) + elif isinstance(call, HandOver): + object_ref = self.scene.resolve( + call.object, + expected_type=SceneObjectRef, + path=(*path, "object"), + ) + grasp = self.scene.resolve_affordance( + object_ref, + capability=GRASP_AFFORDANCE_CAPABILITY, + path=(*path, "object", "handover_grasp"), + ) + normalized_call = replace(call, object=object_ref) + affordances["receiver_grasp"] = grasp + elif isinstance(call, OperateArticulation): + articulation_ref = self.scene.resolve( + call.articulation, + expected_type=SceneArticulationRef, + path=(*path, "articulation"), + ) + handle = self.scene.resolve_affordance( + articulation_ref, + capability=ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + explicit=call.handle, + path=(*path, "handle"), + ) + normalized_call = replace( + call, + articulation=articulation_ref, + handle=handle, + ) + affordances["handle"] = handle + elif isinstance(call, RegisteredSemanticCall): + normalized_call = replace( + call, + arguments=self._normalize_registered_arguments( + call.arguments, + path=(*path, "arguments"), + ), + ) + else: # defensive for future subclasses not represented by the catalog + raise SemanticValidationError( + SemanticDiagnostic( + "unsupported_call_type", + path, + f"No static linker exists for {type(call).__name__}.", + ) + ) + self._validate_declared_resources( + descriptor, + normalized_call.resources, + path=(*path, "resources"), + ) + preset_id = self._resolve_declared_preset( + descriptor, + path=(*path, "preset"), + ) + preset = self.robot_profile.presets[preset_id] + if descriptor.call_id not in preset.action_option_templates: + option_path = ( + "integration", + "robot_profile", + "presets", + preset_id, + "action_option_templates", + descriptor.call_id, + ) + raise SemanticValidationError( + SemanticDiagnostic( + "missing_action_option_template", + option_path, + f"Policy preset {preset_id!r} has no action-option template " + f"for semantic call {descriptor.call_id!r} selected at " + f"{_render_path(path)}.", + tuple(preset.action_option_templates), + ) + ) + return LinkedSemanticCall( + call=normalized_call, + descriptor=descriptor, + preset_id=preset_id, + affordances=affordances, + ) + + def _selects_preset(self, preset_id: str) -> bool: + """Return whether one preset is reachable through this integration. + + Args: + preset_id: Stable policy preset identifier. + + Returns: + ``True`` when the integration-wide override or at least one + catalogued target skill can resolve to ``preset_id`` through its + per-skill or profile-default selection. This is intentionally a + conservative integration-level check, not a concrete-program + reachability analysis. + """ + _validate_identifier(preset_id, field_name="preset_id") + if self.runtime_preset is not None: + return self.runtime_preset == preset_id + skill_ids = { + descriptor.skill_id for descriptor in self.call_catalog.descriptors.values() + } + return any( + self.robot_profile.skill_presets.get( + skill_id, + self.robot_profile.default_preset, + ) + == preset_id + for skill_id in skill_ids + ) + + def _resolve_declared_preset( + self, + descriptor: SemanticCallDescriptor, + *, + path: tuple[PathPart, ...], + ) -> str: + """Resolve the static integration/per-skill/profile preset ID.""" + preset_id = self.runtime_preset + if preset_id is None: + preset_id = self.robot_profile.skill_presets.get(descriptor.skill_id) + if preset_id is None: + preset_id = self.robot_profile.default_preset + if preset_id is None: + raise SemanticValidationError( + SemanticDiagnostic( + "missing_preset", + path, + f"No policy preset is configured for skill " + f"{descriptor.skill_id!r}.", + tuple(self.robot_profile.presets), + ) + ) + if preset_id not in self.robot_profile.presets: + raise SemanticValidationError( + SemanticDiagnostic( + "unknown_preset", + path, + f"Unknown policy preset {preset_id!r}.", + tuple(self.robot_profile.presets), + ) + ) + return preset_id + + def _normalize_registered_arguments( + self, + value: object, + *, + path: tuple[PathPart, ...], + ) -> object: + """Canonicalize every typed scene ref in a registered payload.""" + if type(value) in ( + SceneEntityRef, + SceneObjectRef, + SceneArticulationRef, + SceneLinkRef, + SceneAffordanceRef, + ): + return self.scene.resolve( + value, + expected_type=type(value), + path=path, + ) + # Other exact scene-ref variants are admitted by the call value + # contract and resolved through their exact runtime type here. + if isinstance(value, SceneEntityRef): + return self.scene.resolve( + value, + expected_type=type(value), + path=path, + ) + if isinstance(value, Mapping): + return MappingProxyType( + { + key: self._normalize_registered_arguments( + nested, + path=(*path, key), + ) + for key, nested in value.items() + } + ) + if isinstance(value, tuple): + return tuple( + self._normalize_registered_arguments( + nested, + path=(*path, index), + ) + for index, nested in enumerate(value) + ) + return value + + def _link_relation( + self, + target: SceneObjectRef | SceneAffordanceRef, + *, + capability: str, + path: tuple[PathPart, ...], + ) -> tuple[SceneObjectRef | SceneAffordanceRef, SceneAffordanceRef]: + """Normalize one placement relation and select its affordance.""" + if isinstance(target, SceneObjectRef): + parent = self.scene.resolve( + target, + expected_type=SceneObjectRef, + path=path, + ) + affordance = self.scene.resolve_affordance( + parent, + capability=capability, + path=path, + ) + return parent, affordance + explicit = self.scene.resolve( + target, + expected_type=SceneAffordanceRef, + path=path, + ) + entry = self.scene.lookup(explicit, path=path) + assert entry.parent is not None + affordance = self.scene.resolve_affordance( + entry.parent, + capability=capability, + explicit=explicit, + path=path, + ) + return explicit, affordance + + def _validate_declared_resources( + self, + descriptor: SemanticCallDescriptor, + selections: Mapping[str, str], + *, + path: tuple[PathPart, ...], + ) -> None: + """Validate resource IDs and obvious capability mismatches statically.""" + contract = descriptor.binding_contract + default = self.robot_profile.defaults.get(descriptor.skill_id) + if default is not None: + expected_slots = set(contract.slot_ids) + default_slots = set(default.resources) + unknown_default_resources = sorted( + set(default.resources.values()) - set(self.robot_profile.resources) + ) + if default_slots != expected_slots or unknown_default_resources: + raise SemanticValidationError( + SemanticDiagnostic( + "invalid_default_binding", + ( + "integration", + "robot_profile", + "defaults", + descriptor.skill_id, + ), + "Default resource binding must cover the exact skill slots " + "and reference known resources.", + contract.slot_ids, + ) + ) + unknown_slots = sorted(set(selections) - set(contract.slot_ids)) + if unknown_slots: + slot = unknown_slots[0] + raise SemanticValidationError( + SemanticDiagnostic( + "unknown_resource_slot", + (*path, slot), + f"Skill {descriptor.skill_id!r} has no resource slot {slot!r}.", + contract.slot_ids, + ) + ) + unknown_resources = sorted( + set(selections.values()) - set(self.robot_profile.resources) + ) + if unknown_resources: + unknown = unknown_resources[0] + slot = next(key for key, value in selections.items() if value == unknown) + raise SemanticValidationError( + SemanticDiagnostic( + "unknown_resource", + (*path, slot), + f"Unknown robot resource {unknown!r}.", + tuple(self.robot_profile.resources), + ) + ) + for slot in contract.slots: + selected = selections.get(slot.slot_id) + if default is not None: + default_resource = self.robot_profile.resources[ + default.resources[slot.slot_id] + ] + if not self._resource_declares_requirements(default_resource, slot): + raise SemanticValidationError( + SemanticDiagnostic( + "invalid_default_binding", + ( + "integration", + "robot_profile", + "defaults", + descriptor.skill_id, + slot.slot_id, + ), + f"Default resource {default_resource.resource_id!r} " + f"does not satisfy slot {slot.slot_id!r}.", + ) + ) + candidates = tuple( + resource + for resource in self.robot_profile.resources.values() + if (selected is None or resource.resource_id == selected) + and self._resource_declares_requirements(resource, slot) + ) + if not candidates: + code = ( + "unsupported_resource" + if selected is not None + else "unsupported_skill" + ) + raise SemanticValidationError( + SemanticDiagnostic( + code, + (*path, slot.slot_id), + f"No declared robot resource satisfies slot " + f"{slot.slot_id!r} for skill {descriptor.skill_id!r}.", + tuple(self.robot_profile.resources), + ) + ) + effective_selections: dict[str, str] = {} + if default is not None: + effective_selections.update(default.resources) + effective_selections.update(selections) + for constraint in contract.constraints: + if not isinstance(constraint, DisjointResourceSlots) or not all( + slot_id in effective_selections for slot_id in constraint.slots + ): + continue + resources = [ + self.robot_profile.resources[effective_selections[slot_id]] + for slot_id in constraint.slots + ] + leaf_sets = [ + self._declared_resource_leaves(resource) for resource in resources + ] + for index, left in enumerate(leaf_sets): + if any(left & right for right in leaf_sets[index + 1 :]): + raise SemanticValidationError( + SemanticDiagnostic( + "resource_claim_conflict", + path, + f"Selected resources for slots {list(constraint.slots)} " + "share declared physical leaves.", + tuple(resource.resource_id for resource in resources), + ) + ) + + def _declared_resource_leaves(self, resource: RobotResource) -> frozenset[str]: + """Return transitive leaves from the static profile resource DAG.""" + if not resource.members: + return frozenset({resource.resource_id}) + leaves: set[str] = set() + for member_id in resource.members: + leaves.update( + self._declared_resource_leaves(self.robot_profile.resources[member_id]) + ) + return frozenset(leaves) + + def _resource_declares_requirements( + self, + resource: RobotResource, + slot: SkillResourceSlot, + ) -> bool: + """Check provider-free endpoint declarations without physical binding.""" + endpoints: dict[str, ResourceEndpoint] = {} + for requirement in slot.endpoints: + endpoint = resource.endpoints.get(requirement.endpoint_id) + if endpoint is None or not requirement.capabilities.issubset( + endpoint.capabilities + ): + return False + if requirement.required_commands and isinstance( + endpoint, ControlPartEndpoint + ): + profile_id = endpoint.command_profile or endpoint.control_part + command_profile = self.robot_profile.command_profiles.get(profile_id) + if command_profile is None: + return False + if any( + not isinstance(command_profile.commands.get(name), command_type) + for name, command_type in requirement.required_commands.items() + ): + return False + endpoints[requirement.endpoint_id] = endpoint + # Adapter claims are unavailable before live binding. For the built-in + # endpoint, equal control parts are an exact static conflict. + for constraint in slot.constraints: + if not isinstance(constraint, DisjointSlotEndpoints): + continue + constrained = [endpoints[name] for name in constraint.endpoint_ids] + for index, left in enumerate(constrained): + if not isinstance(left, ControlPartEndpoint): + continue + if any( + isinstance(right, ControlPartEndpoint) + and left.control_part == right.control_part + for right in constrained[index + 1 :] + ): + return False + return True + + def bind( + self, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + *, + endpoint_adapters: ( + Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None + ) = None, + ) -> BoundSemanticIntegration: + """Validate live scene and robot bindings without observing or planning.""" + self.scene.validate_registry(scene_registry) + if not isinstance(engine, AtomicActionEngine): + raise TypeError("engine must be an AtomicActionEngine.") + self._validate_safe_dynamic_collision_policy( + scene_registry=scene_registry, + engine=engine, + ) + try: + bound_profile = engine.bind_skill_profile( + self.robot_profile, + endpoint_adapters=endpoint_adapters, + ) + except Exception as exc: # noqa: BLE001 - add semantic integration path + raise SemanticValidationError( + SemanticDiagnostic( + "robot_profile_binding_failed", + ("integration", "robot_profile"), + str(exc), + ) + ) from exc + return BoundSemanticIntegration( + manifest=self, + scene_registry=scene_registry, + robot_profile=bound_profile, + engine=engine, + ) + + def _validate_safe_dynamic_collision_policy( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + ) -> None: + """Fail before observation when selected safe planning cannot be strict.""" + if not scene_registry.dynamic_collision_entity_ids or not self._selects_preset( + "safe" + ): + return + preset = self.robot_profile.presets["safe"] + policy_path: tuple[PathPart, ...] = ( + "integration", + "robot_profile", + "presets", + "safe", + "motion_policy", + ) + if preset.motion_policy.strategy != "motion_gen": + raise SemanticValidationError( + SemanticDiagnostic( + "safe_dynamic_collision_unsupported", + (*policy_path, "strategy"), + "The 'safe' preset requires strategy='motion_gen' when the " + "scene registry declares dynamic collision entities.", + ("motion_gen",), + ) + ) + if ( + getattr( + engine.motion_generator, + "supports_dynamic_collision_world", + False, + ) + is not True + ): + raise SemanticValidationError( + SemanticDiagnostic( + "safe_dynamic_collision_unsupported", + (*policy_path, "dynamic_collision_mode"), + "The 'safe' preset requires an active planner with dynamic " + "collision-world support for the registered dynamic entities " + f"{scene_registry.dynamic_collision_entity_ids!r}.", + ) + ) + + +class BoundSemanticIntegration: + """Live-installed, still side-effect-free semantic integration link.""" + + def __init__( + self, + *, + manifest: SemanticIntegrationManifest, + scene_registry: SceneRegistry, + robot_profile: BoundRobotSkillProfile, + engine: AtomicActionEngine, + ) -> None: + if type(manifest) is not SemanticIntegrationManifest: + raise TypeError("manifest must be exactly SemanticIntegrationManifest.") + if not isinstance(scene_registry, SceneRegistry): + raise TypeError("scene_registry must be a SceneRegistry.") + if type(robot_profile) is not BoundRobotSkillProfile: + raise TypeError("robot_profile must be exactly BoundRobotSkillProfile.") + if not isinstance(engine, AtomicActionEngine): + raise TypeError("engine must be an AtomicActionEngine.") + manifest.scene.validate_registry(scene_registry) + manifest._validate_safe_dynamic_collision_policy( + scene_registry=scene_registry, + engine=engine, + ) + if robot_profile.engine is not engine: + raise ValueError("robot_profile belongs to a different engine.") + if engine.skill_profile is not robot_profile: + raise ValueError( + "robot_profile must be the canonical profile installed on engine." + ) + if robot_profile.source_profile is not manifest.robot_profile: + raise ValueError( + "robot_profile does not match the semantic integration manifest." + ) + self._manifest = manifest + self._scene_registry = scene_registry + self._robot_profile = robot_profile + self._engine = engine + + @property + def manifest(self) -> SemanticIntegrationManifest: + """Return the static integration declaration.""" + return self._manifest + + @property + def scene_registry(self) -> SceneRegistry: + """Return the validated live scene registry.""" + return self._scene_registry + + @property + def robot_profile(self) -> BoundRobotSkillProfile: + """Return the validated live robot profile.""" + return self._robot_profile + + @property + def engine(self) -> AtomicActionEngine: + """Return the engine whose used call targets are validated at link time.""" + return self._engine + + def link_call( + self, + call: SemanticCallSpec, + *, + path: tuple[PathPart, ...] = ("call",), + ) -> BoundSemanticCall: + """Resolve one call against exact installed skills, resources, and preset.""" + if self._engine.skill_profile is not self._robot_profile: + raise SemanticValidationError( + SemanticDiagnostic( + "semantic_profile_stale", + ("integration", "robot_profile"), + "The engine's canonical robot profile changed after this " + "semantic integration was bound.", + ) + ) + linked = self._manifest.link_call(call, path=path) + installed = self._engine.skills.get(linked.descriptor.skill_id) + if installed is None or installed != linked.descriptor.target_descriptor: + raise SemanticValidationError( + SemanticDiagnostic( + "semantic_skill_not_installed", + (*path, "kind"), + f"Installed engine skill {linked.descriptor.skill_id!r} is " + "missing or has a different goal/options/resource contract.", + tuple(self._engine.skills), + ) + ) + try: + binding = self._robot_profile.resolve( + linked.descriptor.skill_id, + linked.call.resources, + ) + preset = self._robot_profile.preset( + linked.preset_id, + skill_id=linked.descriptor.skill_id, + ) + except Exception as exc: # noqa: BLE001 - add complete call path + raise SemanticValidationError( + SemanticDiagnostic( + "semantic_binding_failed", + (*path, "resources"), + str(exc), + ) + ) from exc + if ( + linked.preset_id == "safe" + and self._scene_registry.dynamic_collision_entity_ids + ): + preset = SkillPolicyPreset( + preset_id=preset.preset_id, + schema_version=preset.schema_version, + motion_policy=replace( + preset.motion_policy, + dynamic_collision_mode=DynamicCollisionMode.REQUIRED, + ), + tracking_policy=preset.tracking_policy, + recovery_policy=preset.recovery_policy, + workflow_recovery_policy=preset.workflow_recovery_policy, + runner_cfg=preset.runner_cfg, + effect_monitors=preset.effect_monitors, + action_option_templates=preset.action_option_templates, + ) + return BoundSemanticCall._create( + linked=linked, + binding=binding, + preset=preset, + robot_profile=self._robot_profile, + ) + + +__all__ = [ + "BoundSemanticCall", + "LinkedSemanticCall", + "PathPart", + "SceneEntityManifest", + "SceneManifest", + "SemanticDiagnostic", + "SemanticIntegrationManifest", + "SemanticValidationError", +] diff --git a/embodichain/lab/sim/skills/parallel.py b/embodichain/lab/sim/skills/parallel.py new file mode 100644 index 000000000..fe6d06d54 --- /dev/null +++ b/embodichain/lab/sim/skills/parallel.py @@ -0,0 +1,354 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Deterministic resource, timing, and state contracts for parallel skills.""" + +from __future__ import annotations + +from dataclasses import dataclass +import math +from types import MappingProxyType +from typing import Mapping + +import torch + +from embodichain.lab.sim.atomic_actions import ( + RuntimeCommandFrame, + StateDelta, + TaskState, + TimedCommandSequence, +) + +from .profiles import ResourceClaim + + +def _validate_identifier(value: str, *, field_name: str) -> None: + """Validate one non-empty stable identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError(f"{field_name} must be a non-empty stable identifier.") + + +@dataclass(frozen=True, slots=True) +class ParallelTimingPolicy: + """Strict environment-grid policy for one parallel barrier. + + Version 2 deliberately rejects fractional frame durations. Padding repeats + the last controller target, which is a deterministic position/tool hold; + no interpolation is hidden inside the scheduler. + """ + + step_dt: float + tolerance: float = 1.0e-6 + + def __post_init__(self) -> None: + for field_name in ("step_dt", "tolerance"): + value = getattr(self, field_name) + if not isinstance(value, (int, float)) or isinstance(value, bool): + raise TypeError(f"{field_name} must be a number.") + value = float(value) + if not math.isfinite(value) or value <= 0.0: + raise ValueError(f"{field_name} must be finite and positive.") + object.__setattr__(self, field_name, value) + + +@dataclass(frozen=True, slots=True, eq=False) +class ParallelBranchPlan: + """One independently planned lane entering a common barrier.""" + + branch_id: str + claim: ResourceClaim + commands: TimedCommandSequence + expected_effects: StateDelta = StateDelta() + + def __post_init__(self) -> None: + _validate_identifier(self.branch_id, field_name="branch_id") + if not isinstance(self.claim, ResourceClaim): + raise TypeError("claim must be a ResourceClaim.") + if not isinstance(self.commands, TimedCommandSequence): + raise TypeError("commands must be a TimedCommandSequence.") + if not isinstance(self.expected_effects, StateDelta): + raise TypeError("expected_effects must be a StateDelta.") + object.__setattr__(self, "commands", self.commands.snapshot()) + object.__setattr__(self, "expected_effects", self.expected_effects.snapshot()) + + +class ParallelConflictError(ValueError): + """Raised before execution when parallel lanes claim overlapping resources.""" + + +class ParallelTimingError(ValueError): + """Raised when a command sequence cannot use the environment step grid.""" + + +class ParallelStateConflictError(ValueError): + """Raised when successful lanes update the same symbolic state row.""" + + +def validate_parallel_claims(branches: tuple[ParallelBranchPlan, ...]) -> None: + """Reject duplicate IDs and every pair of overlapping physical claims.""" + if not isinstance(branches, tuple) or len(branches) < 2: + raise ValueError("Parallel execution requires at least two branch plans.") + if not all(type(branch) is ParallelBranchPlan for branch in branches): + raise TypeError("branches must contain exact ParallelBranchPlan values.") + branch_ids = tuple(branch.branch_id for branch in branches) + if len(set(branch_ids)) != len(branch_ids): + raise ParallelConflictError("Parallel branch IDs must be unique.") + for index, left in enumerate(branches): + for right in branches[index + 1 :]: + if left.claim.conflicts_with(right.claim): + raise ParallelConflictError( + f"Parallel branches {left.branch_id!r} and " + f"{right.branch_id!r} have overlapping physical claims." + ) + + +def _validate_grid_frame( + branch_id: str, + frame_index: int, + frame: RuntimeCommandFrame, + policy: ParallelTimingPolicy, +) -> None: + """Require one frame to occupy exactly one environment control step.""" + durations = frame.hold_duration + expected = torch.full_like(durations, policy.step_dt) + if not torch.allclose(durations, expected, atol=policy.tolerance, rtol=0.0): + values = sorted({float(value) for value in durations.detach().cpu().tolist()}) + raise ParallelTimingError( + f"Parallel branch {branch_id!r} frame {frame_index} has durations " + f"{values}; every emitted frame must equal step_dt={policy.step_dt}." + ) + + +def align_parallel_commands( + branches: tuple[ParallelBranchPlan, ...], + policy: ParallelTimingPolicy, +) -> TimedCommandSequence: + """Merge disjoint lanes on one grid and hold-pad shorter trajectories. + + Each merged frame is a single transport transaction. Runtime frame + validation independently rejects duplicate destinations or joint overlap, + defending against an incorrect custom ``ResourceClaim`` implementation. + """ + if not isinstance(policy, ParallelTimingPolicy): + raise TypeError("policy must be a ParallelTimingPolicy.") + validate_parallel_claims(branches) + first = branches[0].commands + if any( + branch.commands.device != first.device + or not torch.equal(branch.commands.env_ids, first.env_ids) + for branch in branches[1:] + ): + raise ParallelTimingError( + "Parallel command sequences must share ordered env_ids and device." + ) + if any(branch.commands.frame_count == 0 for branch in branches): + raise ParallelTimingError( + "Parallel branches must emit at least one command frame." + ) + for branch in branches: + for frame_index, frame in enumerate(branch.commands.frames): + _validate_grid_frame(branch.branch_id, frame_index, frame, policy) + + frame_count = max(branch.commands.frame_count for branch in branches) + merged: list[RuntimeCommandFrame] = [] + for frame_index in range(frame_count): + lane_frames = tuple( + branch.commands.frames[min(frame_index, branch.commands.frame_count - 1)] + for branch in branches + ) + reference_mask = lane_frames[0].active_mask + if any( + not torch.equal(frame.active_mask, reference_mask) + for frame in lane_frames[1:] + ): + raise ParallelTimingError( + "Parallel lanes cannot merge different per-environment active " + f"masks at frame {frame_index}; RuntimeCommandFrame owns one " + "mask for every command in the transaction." + ) + merged.append( + RuntimeCommandFrame( + commands=tuple( + command for frame in lane_frames for command in frame.commands + ), + active_mask=reference_mask, + env_ids=first.env_ids, + hold_duration=torch.full( + (first.batch_size,), + policy.step_dt, + dtype=lane_frames[0].hold_duration.dtype, + device=first.device, + ), + ) + ) + return TimedCommandSequence(frames=tuple(merged), env_ids=first.env_ids) + + +def _delta_keys(delta: StateDelta) -> frozenset[tuple[str, object]]: + """Return domain-qualified symbolic keys written by one delta.""" + return frozenset( + [("held", key) for key in delta.held_object_updates] + + [("coordinated", key) for key in delta.coordinated_held_object_updates] + + [("articulation", key) for key in delta.articulation_joint_updates] + ) + + +def merge_parallel_effects( + state: TaskState, + effects: Mapping[str, tuple[StateDelta, torch.Tensor]], +) -> TaskState: + """Apply disjoint branch effects with deterministic row-local conflict checks. + + Args: + state: Verified task state before the barrier. + effects: Branch ID to ``(delta, verified_success_mask)``. + + Returns: + New verified task state after all non-conflicting updates. + """ + if not isinstance(state, TaskState): + raise TypeError("state must be a TaskState.") + if not isinstance(effects, Mapping) or not effects: + raise ValueError("effects must be a non-empty branch mapping.") + normalized: dict[str, tuple[StateDelta, torch.Tensor]] = {} + for branch_id, value in effects.items(): + _validate_identifier(branch_id, field_name="effect branch IDs") + if not isinstance(value, tuple) or len(value) != 2: + raise TypeError("effect entries must be (StateDelta, success_mask) pairs.") + delta, mask = value + if not isinstance(delta, StateDelta): + raise TypeError("effect deltas must be StateDelta values.") + if ( + not isinstance(mask, torch.Tensor) + or mask.dtype != torch.bool + or mask.shape != (state.batch_size,) + or mask.device != state.device + ): + raise ValueError("effect masks must match TaskState batch and device.") + normalized[branch_id] = delta.snapshot(), mask.clone() + + entries = tuple(normalized.items()) + for index, (left_id, (left_delta, left_mask)) in enumerate(entries): + for right_id, (right_delta, right_mask) in entries[index + 1 :]: + overlapping_keys = _delta_keys(left_delta) & _delta_keys(right_delta) + overlapping_rows = left_mask & right_mask + if overlapping_keys and overlapping_rows.any(): + raise ParallelStateConflictError( + f"Parallel effects {left_id!r} and {right_id!r} write " + f"the same symbolic keys on rows " + f"{overlapping_rows.nonzero().flatten().tolist()}." + ) + result = state + for branch_id in sorted(normalized): + delta, mask = normalized[branch_id] + result = delta.apply(result, mask) + return result + + +@dataclass(frozen=True, slots=True, eq=False) +class ParallelBarrierUpdate: + """Per-row barrier status after one synchronized lane observation.""" + + completed_mask: torch.Tensor + failure_mask: torch.Tensor + cancellation_masks: Mapping[str, torch.Tensor] + + def __post_init__(self) -> None: + if ( + not isinstance(self.completed_mask, torch.Tensor) + or self.completed_mask.dtype != torch.bool + or self.completed_mask.dim() != 1 + ): + raise ValueError("completed_mask must be a one-dimensional bool tensor.") + if ( + not isinstance(self.failure_mask, torch.Tensor) + or self.failure_mask.dtype != torch.bool + or self.failure_mask.shape != self.completed_mask.shape + or self.failure_mask.device != self.completed_mask.device + ): + raise ValueError("failure_mask must match completed_mask.") + cancellations: dict[str, torch.Tensor] = {} + for branch_id, mask in self.cancellation_masks.items(): + _validate_identifier(branch_id, field_name="cancellation branch IDs") + if ( + not isinstance(mask, torch.Tensor) + or mask.dtype != torch.bool + or mask.shape != self.completed_mask.shape + or mask.device != self.completed_mask.device + ): + raise ValueError("cancellation masks must match completed_mask.") + cancellations[branch_id] = mask.clone() + object.__setattr__(self, "completed_mask", self.completed_mask.clone()) + object.__setattr__(self, "failure_mask", self.failure_mask.clone()) + object.__setattr__( + self, + "cancellation_masks", + MappingProxyType(cancellations), + ) + + +def resolve_parallel_barrier( + *, + pending_masks: Mapping[str, torch.Tensor], + success_masks: Mapping[str, torch.Tensor], + failure_masks: Mapping[str, torch.Tensor], +) -> ParallelBarrierUpdate: + """Apply deterministic per-row fail-fast semantics at one barrier update.""" + branch_ids = tuple(pending_masks) + if ( + not branch_ids + or set(success_masks) != set(branch_ids) + or set(failure_masks) != set(branch_ids) + ): + raise ValueError( + "pending, success, and failure mappings must share branch IDs." + ) + reference = pending_masks[branch_ids[0]] + if not isinstance(reference, torch.Tensor): + raise TypeError("barrier masks must be torch.Tensor values.") + for mapping in (pending_masks, success_masks, failure_masks): + for mask in mapping.values(): + if ( + not isinstance(mask, torch.Tensor) + or mask.dtype != torch.bool + or mask.shape != reference.shape + or mask.device != reference.device + ): + raise ValueError("all barrier masks must share bool shape and device.") + failed = torch.stack(tuple(failure_masks.values()), dim=0).any(dim=0) + succeeded_all = torch.stack(tuple(success_masks.values()), dim=0).all(dim=0) + cancellations = { + branch_id: failed & pending_masks[branch_id] for branch_id in branch_ids + } + return ParallelBarrierUpdate( + completed_mask=succeeded_all | failed, + failure_mask=failed, + cancellation_masks=cancellations, + ) + + +__all__ = [ + "ParallelBarrierUpdate", + "ParallelBranchPlan", + "ParallelConflictError", + "ParallelStateConflictError", + "ParallelTimingError", + "ParallelTimingPolicy", + "align_parallel_commands", + "merge_parallel_effects", + "resolve_parallel_barrier", + "validate_parallel_claims", +] diff --git a/embodichain/lab/sim/skills/parallel_runtime.py b/embodichain/lab/sim/skills/parallel_runtime.py new file mode 100644 index 000000000..bbaf43e9c --- /dev/null +++ b/embodichain/lab/sim/skills/parallel_runtime.py @@ -0,0 +1,1529 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Branch-local semantic execution joined by one deterministic barrier.""" + +from __future__ import annotations + +from collections.abc import Hashable, Mapping +from copy import deepcopy +from dataclasses import dataclass, field +import math +from types import MappingProxyType +from typing import Protocol, runtime_checkable + +import torch + +from embodichain.lab.sim.atomic_actions import ( + CommandAcknowledgement, + CommandSink, + ExecutionClock, + ExecutionRunnerCfg, + PlanningContext, + RuntimeCommandFrame, + RuntimeEndpointTarget, + StateDelta, + TaskState, + TimedCommandSequence, +) + +from .calls import SemanticCallSpec +from .compiler import SemanticSkillCompiler +from .effects import SymbolicStateKey +from .integration import ( + PathPart, + SemanticDiagnostic, + SemanticValidationError, +) +from .parallel import ( + ParallelBranchPlan, + ParallelTimingPolicy, + align_parallel_commands, + merge_parallel_effects, + resolve_parallel_barrier, +) +from .profiles import ResourceClaim +from .runtime import SkillResult, SkillRuntime, SkillStatus, task_state_to_metadata + + +def _validate_identifier(value: str, *, field_name: str) -> None: + if type(value) is not str or not value or value != value.strip(): + raise ValueError(f"{field_name} must be a non-empty stable identifier.") + + +def _snapshot_target(target: RuntimeEndpointTarget) -> RuntimeEndpointTarget: + snapshot = target.snapshot() + if type(snapshot) is not type(target) or snapshot is target: + raise TypeError("Runtime target snapshots must be independent exact values.") + return snapshot + + +def _target_fingerprint(target: RuntimeEndpointTarget) -> Hashable: + """Return one validated target address and safe-hold fingerprint.""" + fingerprint = target.address_fingerprint + try: + hash(fingerprint) + except TypeError as exc: + raise TypeError( + "RuntimeEndpointTarget.address_fingerprint must be hashable." + ) from exc + return fingerprint + + +@runtime_checkable +class ParallelBranchRuntime(Protocol): + """Minimal branch-local runtime surface required by the coordinator.""" + + @property + def result(self) -> SkillResult: + """Return the current immutable branch result.""" + + def start( + self, + *calls: SemanticCallSpec, + workflow_id: str, + eligible_mask: torch.Tensor | None = None, + ) -> SkillResult: + """Start one branch-local semantic workflow.""" + + def step(self) -> SkillResult: + """Advance the branch by one due runtime cycle.""" + + def deactivate_rows( + self, + env_mask: torch.Tensor, + *, + reason: str, + ) -> SkillResult: + """Remove peer-failed rows while other rows continue.""" + + def cancel(self, reason: str) -> SkillResult: + """Cancel the complete branch and apply its safe stop.""" + + +@runtime_checkable +class ParallelCommandSafetyValidator(Protocol): + """Fail-closed physical-safety boundary for one merged command tick. + + Resource claims prevent controller arbitration conflicts but cannot prove + that independently generated robot motions are collision-free when + executed together. Environment integrations must install a validator + backed by their authoritative robot/collision model before parallel + commands can leave the coordinator. + """ + + def validate( + self, + *, + branch_frames: Mapping[str, RuntimeCommandFrame], + merged_frame: RuntimeCommandFrame, + ) -> None: + """Raise when the synchronized command is not physically safe.""" + + +class ParallelSafetyError(RuntimeError): + """Raised when physical parallel-command safety cannot be established.""" + + +class ParallelLaneCommandSink: + """Acknowledge one branch locally and expose its frame to a coordinator. + + The coordinator is the only object allowed to forward commands to the real + transport. A lane retains its last frame so shorter or temporarily waiting + branches use deterministic hold-last padding. + """ + + def __init__(self) -> None: + self._fresh_frame: RuntimeCommandFrame | None = None + self._last_frame: RuntimeCommandFrame | None = None + self._hold_requests: list[ + tuple[tuple[RuntimeEndpointTarget, ...], PlanningContext] + ] = [] + self._cancel_targets: tuple[RuntimeEndpointTarget, ...] = () + + @property + def last_frame(self) -> RuntimeCommandFrame | None: + """Return an owned hold-last frame, if this lane has sent one.""" + return None if self._last_frame is None else self._last_frame.snapshot() + + @property + def hold_request( + self, + ) -> tuple[tuple[RuntimeEndpointTarget, ...], PlanningContext | None]: + """Return all pending targets and their latest planning context.""" + targets: dict[Hashable, RuntimeEndpointTarget] = {} + context: PlanningContext | None = None + for requested, request_context in self._hold_requests: + for target in requested: + targets[_target_fingerprint(target)] = target + context = request_context + return ( + tuple(_snapshot_target(target) for target in targets.values()), + context, + ) + + @property + def cancel_targets(self) -> tuple[RuntimeEndpointTarget, ...]: + """Return target snapshots from the most recent cancel request.""" + return tuple(_snapshot_target(target) for target in self._cancel_targets) + + def send( + self, + command: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Capture exactly one fresh frame for the current coordinator tick.""" + del timeout + if not isinstance(command, RuntimeCommandFrame): + raise TypeError("command must be a RuntimeCommandFrame.") + if self._fresh_frame is not None: + raise RuntimeError( + "A parallel lane emitted multiple command frames before drain." + ) + self._fresh_frame = command.snapshot() + self._last_frame = command.snapshot() + return CommandAcknowledgement.accepted_ack("buffered by parallel lane") + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Capture a target-scoped hold; hold-last remains the grid command.""" + del timeout + if not isinstance(context, PlanningContext): + raise TypeError("context must be a PlanningContext.") + self._hold_requests.append( + ( + tuple(_snapshot_target(target) for target in targets), + context, + ) + ) + return CommandAcknowledgement.accepted_ack("buffered parallel hold") + + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + """Capture cancellation ownership for coordinator-level safe stop.""" + del timeout + self._fresh_frame = None + self._cancel_targets = tuple(_snapshot_target(target) for target in targets) + return CommandAcknowledgement.accepted_ack("buffered parallel cancel") + + def drain_frame(self) -> RuntimeCommandFrame | None: + """Consume the frame emitted since the previous coordinator step.""" + frame = self._fresh_frame + self._fresh_frame = None + return None if frame is None else frame.snapshot() + + def drain_hold_requests( + self, + ) -> tuple[tuple[tuple[RuntimeEndpointTarget, ...], PlanningContext], ...]: + """Consume every completion/safe hold buffered since the last tick.""" + requests = tuple( + ( + tuple(_snapshot_target(target) for target in targets), + context, + ) + for targets, context in self._hold_requests + ) + self._hold_requests.clear() + return requests + + +@dataclass(frozen=True, slots=True) +class ParallelRuntimeBranch: + """One semantic-call lane and its exclusive resource claim.""" + + branch_id: str + calls: tuple[SemanticCallSpec, ...] + claim: ResourceClaim + runtime: ParallelBranchRuntime = field(repr=False, compare=False) + command_sink: ParallelLaneCommandSink = field(repr=False, compare=False) + + def __post_init__(self) -> None: + _validate_identifier(self.branch_id, field_name="branch_id") + calls = tuple(self.calls) + if not calls or not all(isinstance(call, SemanticCallSpec) for call in calls): + raise TypeError("calls must contain SemanticCallSpec values.") + if not isinstance(self.claim, ResourceClaim): + raise TypeError("claim must be a ResourceClaim.") + if not isinstance(self.runtime, ParallelBranchRuntime): + raise TypeError("runtime must implement ParallelBranchRuntime.") + if type(self.command_sink) is not ParallelLaneCommandSink: + raise TypeError("command_sink must be ParallelLaneCommandSink.") + object.__setattr__(self, "calls", calls) + + +@dataclass(frozen=True, slots=True) +class ParallelBranchStaticAnalysis: + """Provider-free physical and symbolic claims for one semantic lane.""" + + branch_id: str + calls: tuple[SemanticCallSpec, ...] + claim: ResourceClaim + symbolic_writes: frozenset[SymbolicStateKey] + opaque_symbolic_call_indices: tuple[int, ...] + source_path: tuple[PathPart, ...] + + def __post_init__(self) -> None: + _validate_identifier(self.branch_id, field_name="branch_id") + calls = tuple(self.calls) + if not calls or not all(isinstance(call, SemanticCallSpec) for call in calls): + raise TypeError("calls must contain SemanticCallSpec values.") + if not isinstance(self.claim, ResourceClaim): + raise TypeError("claim must be a ResourceClaim.") + if type(self.symbolic_writes) is not frozenset or not all( + type(write) is SymbolicStateKey for write in self.symbolic_writes + ): + raise TypeError( + "symbolic_writes must be an exact frozenset of " + "SymbolicStateKey values." + ) + opaque_indices = tuple(self.opaque_symbolic_call_indices) + if not all( + type(index) is int and 0 <= index < len(calls) for index in opaque_indices + ): + raise ValueError( + "opaque_symbolic_call_indices must select branch call indices." + ) + if len(set(opaque_indices)) != len(opaque_indices): + raise ValueError("opaque_symbolic_call_indices must be unique.") + source_path = tuple(self.source_path) + if not source_path or not all( + (type(part) is str and bool(part)) or type(part) is int + for part in source_path + ): + raise ValueError("source_path must contain valid diagnostic components.") + object.__setattr__(self, "calls", calls) + object.__setattr__(self, "opaque_symbolic_call_indices", opaque_indices) + object.__setattr__(self, "source_path", source_path) + + +def analyze_parallel_branches( + compiler: SemanticSkillCompiler, + branch_calls: Mapping[str, tuple[SemanticCallSpec, ...]], + *, + workflow_id: str = "parallel_static_analysis", + branch_paths: Mapping[str, tuple[PathPart, ...]] | None = None, +) -> tuple[ParallelBranchStaticAnalysis, ...]: + """Reject overlapping physical claims and exact symbolic write keys. + + This is the canonical provider-free parallel preflight shared by the core + runtime factory and higher-level declarative frontends. Dynamic command + collision safety remains the responsibility of + :class:`ParallelCommandSafetyValidator`. + + Args: + compiler: Canonical semantic compiler owning the current integration. + branch_calls: Ordered branch IDs and their complete semantic calls. + workflow_id: Stable diagnostic prefix for branch workflows. + branch_paths: Optional exact source path for every supplied branch. + + Returns: + Ordered owned branch analyses with combined resource claims. + + Raises: + ValueError: If fewer than two branches are supplied or claims overlap. + SemanticValidationError: If branches write one exact symbolic key. + """ + if not isinstance(compiler, SemanticSkillCompiler): + raise TypeError("compiler must be a SemanticSkillCompiler.") + if not isinstance(branch_calls, Mapping) or len(branch_calls) < 2: + raise ValueError("branch_calls must contain at least two branches.") + _validate_identifier(workflow_id, field_name="workflow_id") + if branch_paths is not None: + if not isinstance(branch_paths, Mapping): + raise TypeError("branch_paths must be a mapping or None.") + if set(branch_paths) != set(branch_calls): + raise ValueError("branch_paths keys must exactly match branch_calls.") + + analyses: list[ParallelBranchStaticAnalysis] = [] + for branch_index, (branch_id, supplied_calls) in enumerate(branch_calls.items()): + _validate_identifier(branch_id, field_name="parallel branch IDs") + calls = tuple(supplied_calls) + if not calls or not all(isinstance(call, SemanticCallSpec) for call in calls): + raise TypeError( + "parallel branch calls must contain SemanticCallSpec values." + ) + source_path = ( + ("parallel", "branches", branch_index) + if branch_paths is None + else tuple(branch_paths[branch_id]) + ) + workflow = compiler.analyze( + calls, + workflow_id=f"{workflow_id}:{branch_index}:{branch_id}", + path=source_path, + ) + analyses.append( + ParallelBranchStaticAnalysis( + branch_id=branch_id, + calls=calls, + claim=ResourceClaim.combine( + tuple(call.bound.binding.claim for call in workflow.calls) + ), + symbolic_writes=frozenset( + write + for analyzed_call in workflow.calls + for write in analyzed_call.symbolic_writes + ), + opaque_symbolic_call_indices=tuple( + analyzed_call.index + for analyzed_call in workflow.calls + if analyzed_call.opaque_symbolic_effect + ), + source_path=source_path, + ) + ) + for index, left in enumerate(analyses): + for right in analyses[index + 1 :]: + if left.claim.conflicts_with(right.claim): + raise SemanticValidationError( + SemanticDiagnostic( + "parallel_resource_conflict", + right.source_path, + f"Parallel branches {left.branch_id!r} and " + f"{right.branch_id!r} have overlapping resource claims.", + (left.branch_id, right.branch_id), + ) + ) + shared_writes = left.symbolic_writes & right.symbolic_writes + if shared_writes: + conflict = min( + shared_writes, + key=lambda write: (write.domain.value, write.address), + ) + raise SemanticValidationError( + SemanticDiagnostic( + "parallel_symbolic_write_conflict", + right.source_path, + f"Parallel branches {left.branch_id!r} and " + f"{right.branch_id!r} both write symbolic TaskState key " + f"{conflict.rendered}.", + (left.branch_id, right.branch_id), + ) + ) + return tuple(analyses) + + +@dataclass(frozen=True, slots=True, eq=False) +class ParallelSkillResult: + """Owned coordinator status at one explicit barrier.""" + + status: SkillStatus + env_ids: torch.Tensor + success_mask: torch.Tensor + failure_mask: torch.Tensor + cancelled_mask: torch.Tensor + pending_mask: torch.Tensor + task_state: TaskState + branch_results: Mapping[str, SkillResult] + elapsed_steps: int + command_count: int + wait_duration: float = 0.0 + message: str | None = None + + def __post_init__(self) -> None: + if not isinstance(self.status, SkillStatus): + raise TypeError("status must be a SkillStatus.") + if ( + not isinstance(self.env_ids, torch.Tensor) + or self.env_ids.dtype != torch.long + or self.env_ids.dim() != 1 + ): + raise ValueError("env_ids must be a one-dimensional int64 tensor.") + batch_size = int(self.env_ids.numel()) + for field_name in ( + "success_mask", + "failure_mask", + "cancelled_mask", + "pending_mask", + ): + value = getattr(self, field_name) + if ( + not isinstance(value, torch.Tensor) + or value.dtype != torch.bool + or value.shape != (batch_size,) + or value.device != self.env_ids.device + ): + raise ValueError(f"{field_name} must match env_ids.") + if ( + (self.success_mask & (self.failure_mask | self.cancelled_mask)).any() + or (self.failure_mask & self.cancelled_mask).any() + or ( + self.pending_mask + & (self.success_mask | self.failure_mask | self.cancelled_mask) + ).any() + ): + raise ValueError("parallel result masks must be disjoint.") + if not isinstance(self.task_state, TaskState): + raise TypeError("task_state must be a TaskState.") + if ( + self.task_state.batch_size != batch_size + or self.task_state.device != self.env_ids.device + ): + raise ValueError("task_state must match env_ids.") + if type(self.elapsed_steps) is not int or self.elapsed_steps < 0: + raise ValueError("elapsed_steps must be non-negative.") + if type(self.command_count) is not int or self.command_count < 0: + raise ValueError("command_count must be non-negative.") + if not math.isfinite(self.wait_duration) or self.wait_duration < 0.0: + raise ValueError("wait_duration must be finite and non-negative.") + if self.message is not None and type(self.message) is not str: + raise TypeError("message must be a string or None.") + branches: dict[str, SkillResult] = {} + for branch_id, result in self.branch_results.items(): + _validate_identifier(branch_id, field_name="branch result IDs") + if not isinstance(result, SkillResult): + raise TypeError("branch_results values must be SkillResult values.") + branches[branch_id] = result + object.__setattr__(self, "env_ids", self.env_ids.clone()) + for field_name in ( + "success_mask", + "failure_mask", + "cancelled_mask", + "pending_mask", + ): + object.__setattr__(self, field_name, getattr(self, field_name).clone()) + object.__setattr__( + self, + "task_state", + TaskState( + batch_size=self.task_state.batch_size, + device=self.task_state.device, + held_objects=self.task_state.held_objects, + coordinated_held_objects=self.task_state.coordinated_held_objects, + articulation_joints=self.task_state.articulation_joints, + ), + ) + object.__setattr__(self, "branch_results", MappingProxyType(branches)) + + @property + def terminal(self) -> bool: + """Whether every row has left the barrier.""" + return self.status in { + SkillStatus.COMPLETED, + SkillStatus.FAILED, + SkillStatus.CANCELLED, + } + + def to_metadata(self) -> dict[str, object]: + """Return a fresh deterministic JSON-safe parallel barrier result.""" + return { + "schema_version": 1, + "kind": "parallel_skill_result", + "status": self.status.value, + "env_ids": self.env_ids.detach().cpu().tolist(), + "masks": { + "success": self.success_mask.detach().cpu().tolist(), + "failure": self.failure_mask.detach().cpu().tolist(), + "cancelled": self.cancelled_mask.detach().cpu().tolist(), + "pending": self.pending_mask.detach().cpu().tolist(), + }, + "task_state": task_state_to_metadata(self.task_state), + "branches": { + branch_id: result.to_metadata() + for branch_id, result in sorted(self.branch_results.items()) + }, + "elapsed_steps": self.elapsed_steps, + "command_count": self.command_count, + "wait_duration": self.wait_duration, + "message": self.message, + } + + +def _optional_tensor_equal( + left: torch.Tensor | None, right: torch.Tensor | None +) -> bool: + return (left is None and right is None) or ( + left is not None and right is not None and torch.equal(left, right) + ) + + +def _state_value_equal(left: object, right: object) -> bool: + if type(left) is not type(right): + return False + if left is None or right is None: + return left is right + if hasattr(left, "position"): + return torch.equal(left.position, right.position) and _optional_tensor_equal( + left.env_mask, + right.env_mask, + ) + if hasattr(left, "left_object_to_eef"): + return ( + left.semantics.entity_id == right.semantics.entity_id + and torch.equal(left.left_object_to_eef, right.left_object_to_eef) + and torch.equal(left.right_object_to_eef, right.right_object_to_eef) + and torch.equal(left.left_grasp_xpos, right.left_grasp_xpos) + and torch.equal(left.right_grasp_xpos, right.right_grasp_xpos) + and _optional_tensor_equal(left.env_mask, right.env_mask) + ) + return ( + left.semantics.entity_id == right.semantics.entity_id + and torch.equal(left.object_to_eef, right.object_to_eef) + and torch.equal(left.grasp_xpos, right.grasp_xpos) + and _optional_tensor_equal(left.env_mask, right.env_mask) + ) + + +def _mapping_delta( + before: Mapping[object, object], after: Mapping[object, object] +) -> dict: + updates: dict[object, object | None] = {} + for key in set(before) | set(after): + if key not in after: + updates[key] = None + elif key not in before or not _state_value_equal(before[key], after[key]): + updates[key] = after[key] + return updates + + +def _task_state_delta(before: TaskState, after: TaskState) -> StateDelta: + if before.batch_size != after.batch_size or before.device != after.device: + raise ValueError("Parallel branch TaskState changed batch or device.") + return StateDelta( + held_object_updates=_mapping_delta( + before.held_objects, + after.held_objects, + ), + coordinated_held_object_updates=_mapping_delta( + before.coordinated_held_objects, + after.coordinated_held_objects, + ), + articulation_joint_updates=_mapping_delta( + before.articulation_joints, + after.articulation_joints, + ), + ) + + +class ParallelSkillRuntime: + """Run independent JIT semantic lanes on one synchronized command grid. + + Schema v2 deliberately uses conservative barrier ownership: branches are + not assigned disjoint environment-row partitions, so two branches that + write the same symbolic key conflict for the complete started batch even + when their observed value masks happen to be disjoint. A future schema + may add explicit row partitioning before relaxing this invariant. + + A lane completion hold is forwarded as an explicit grid action. Other + lanes therefore receive deterministic hold-padding for that environment + step; a merged frame generated in the same coordinator cycle is retained + and dispatched only after the clock advances. Branch runners are not + stepped while that retained frame is being dispatched. This keeps the + physical order ``observed hold -> next command`` and limits every normal + coordinator step to one action-producing transport operation. + """ + + def __init__( + self, + branches: tuple[ParallelRuntimeBranch, ...], + command_sink: CommandSink, + clock: ExecutionClock, + timing_policy: ParallelTimingPolicy, + safety_validator: ParallelCommandSafetyValidator, + *, + timeout_steps: int, + failure_policy: str = "fail_fast", + runner_cfg: ExecutionRunnerCfg | None = None, + ) -> None: + if not isinstance(branches, tuple) or len(branches) < 2: + raise ValueError("ParallelSkillRuntime requires at least two branches.") + if not all(type(branch) is ParallelRuntimeBranch for branch in branches): + raise TypeError("branches must contain ParallelRuntimeBranch values.") + branch_ids = tuple(branch.branch_id for branch in branches) + if len(set(branch_ids)) != len(branch_ids): + raise ValueError("Parallel branch IDs must be unique.") + for index, left in enumerate(branches): + for right in branches[index + 1 :]: + if left.claim.conflicts_with(right.claim): + raise ValueError( + f"Parallel branches {left.branch_id!r} and " + f"{right.branch_id!r} have overlapping resource claims." + ) + if not isinstance(command_sink, CommandSink): + raise TypeError("command_sink must implement CommandSink.") + if not isinstance(clock, ExecutionClock): + raise TypeError("clock must implement ExecutionClock.") + if not isinstance(timing_policy, ParallelTimingPolicy): + raise TypeError("timing_policy must be ParallelTimingPolicy.") + if not isinstance(safety_validator, ParallelCommandSafetyValidator): + raise TypeError( + "safety_validator must implement ParallelCommandSafetyValidator; " + "resource claims alone do not establish collision safety." + ) + if type(timeout_steps) is not int or timeout_steps <= 0: + raise ValueError("timeout_steps must be positive.") + if failure_policy != "fail_fast": + raise ValueError("failure_policy must be exactly 'fail_fast'.") + if runner_cfg is not None and not isinstance(runner_cfg, ExecutionRunnerCfg): + raise TypeError("runner_cfg must be an ExecutionRunnerCfg or None.") + initial = branches[0].runtime.result + for branch in branches[1:]: + result = branch.runtime.result + if ( + result.env_ids.device != initial.env_ids.device + or not torch.equal(result.env_ids, initial.env_ids) + or result.task_state.batch_size != initial.task_state.batch_size + or result.task_state.device != initial.task_state.device + ): + raise ValueError( + "Parallel branch runtimes must share env_ids, batch, and device." + ) + if not _task_state_delta(initial.task_state, result.task_state).is_empty: + raise ValueError( + "Parallel branch runtimes must start from the same verified " + "TaskState barrier snapshot." + ) + self._branches = branches + self._command_sink = command_sink + self._clock = clock + self._timing_policy = timing_policy + self._safety_validator = safety_validator + self._runner_cfg = deepcopy(runner_cfg or ExecutionRunnerCfg()) + self._timeout_steps = timeout_steps + self._initial_state = initial.task_state + self._task_state = initial.task_state + self._env_ids = initial.env_ids + self._status = SkillStatus.IDLE + self._success = torch.zeros_like(initial.success_mask) + self._failure = torch.zeros_like(initial.failure_mask) + self._cancelled = torch.zeros_like(initial.cancelled_mask) + self._pending = torch.ones_like(initial.success_mask) + self._started_eligible = torch.zeros_like(initial.success_mask) + self._elapsed_steps = 0 + self._start_timestamp: float | None = None + self._command_count = 0 + self._wait_duration = 0.0 + self._message: str | None = None + self._force_mask_dispatch = False + self._terminal_stop_forwarded = False + self._held_target_fingerprints: set[Hashable] = set() + self._last_hold_context: PlanningContext | None = None + self._deferred_frame: RuntimeCommandFrame | None = None + self._deferred_lane_frames: dict[str, RuntimeCommandFrame] = {} + self._terminal_hold_pending = False + self._next_transport_at: float | None = None + + @classmethod + def from_template( + cls, + template_runtime: SkillRuntime, + branch_calls: Mapping[str, tuple[SemanticCallSpec, ...]], + command_sink: CommandSink, + timing_policy: ParallelTimingPolicy, + safety_validator: ParallelCommandSafetyValidator, + *, + timeout_steps: int, + failure_policy: str = "fail_fast", + runner_cfg: ExecutionRunnerCfg | None = None, + workflow_id: str = "parallel_static_analysis", + branch_paths: Mapping[str, tuple[PathPart, ...]] | None = None, + ) -> ParallelSkillRuntime: + """Analyze claims and derive independent lanes from one runtime. + + This factory deliberately accepts semantic calls instead of compiled + Gym-program types. It keeps the simulation runtime independent of the + higher-level configuration package while giving every frontend one + canonical resource-conflict and lane-construction path. + + Args: + template_runtime: Idle runtime providing shared compiler and ports. + branch_calls: Ordered branch ID to semantic-call sequence mapping. + command_sink: The sole outbound merged command sink. + timing_policy: Exact shared environment grid. + safety_validator: Required physical/collision safety gate for each + synchronized outbound command. + timeout_steps: Maximum environment steps at the barrier. + failure_policy: Row-local barrier failure policy. + runner_cfg: Shared command timeout, safe-stop, completion-hold, and + minimum-cycle policy selected by the runtime preset. + workflow_id: Stable prefix for provider-free claim analysis. + branch_paths: Optional exact source path for every branch. + + Returns: + A one-shot parallel runtime whose branches share no mutable runner + state. + """ + if not isinstance(template_runtime, SkillRuntime): + raise TypeError("template_runtime must be a SkillRuntime.") + if template_runtime.status is SkillStatus.RUNNING: + raise RuntimeError("template_runtime must not be running.") + branches: list[ParallelRuntimeBranch] = [] + for analysis in analyze_parallel_branches( + template_runtime.compiler, + branch_calls, + workflow_id=workflow_id, + branch_paths=branch_paths, + ): + lane_sink = ParallelLaneCommandSink() + lane_runtime = template_runtime.fork( + lane_sink, + task_state=template_runtime.task_state, + ) + branches.append( + ParallelRuntimeBranch( + branch_id=analysis.branch_id, + calls=analysis.calls, + claim=analysis.claim, + runtime=lane_runtime, + command_sink=lane_sink, + ) + ) + return cls( + tuple(branches), + command_sink, + template_runtime.clock, + timing_policy, + safety_validator, + timeout_steps=timeout_steps, + failure_policy=failure_policy, + runner_cfg=runner_cfg, + ) + + @property + def result(self) -> ParallelSkillResult: + """Return an owned barrier snapshot.""" + return ParallelSkillResult( + status=self._status, + env_ids=self._env_ids, + success_mask=self._success, + failure_mask=self._failure, + cancelled_mask=self._cancelled, + pending_mask=self._pending, + task_state=self._task_state, + branch_results={ + branch.branch_id: branch.runtime.result for branch in self._branches + }, + elapsed_steps=self._elapsed_steps, + command_count=self._command_count, + wait_duration=self._wait_duration, + message=self._message, + ) + + @property + def clock(self) -> ExecutionClock: + """Return the exact clock shared by the coordinator and every lane.""" + return self._clock + + @property + def branch_claims(self) -> Mapping[str, ResourceClaim]: + """Return immutable statically analyzed claims in branch order.""" + return MappingProxyType( + {branch.branch_id: branch.claim for branch in self._branches} + ) + + @property + def runner_cfg(self) -> ExecutionRunnerCfg: + """Return an owned copy of the coordinator transport policy.""" + return deepcopy(self._runner_cfg) + + def start( + self, + *, + workflow_id: str = "parallel_workflow", + eligible_mask: torch.Tensor | None = None, + ) -> ParallelSkillResult: + """Start all lanes from the same verified barrier state.""" + if self._status is not SkillStatus.IDLE: + raise RuntimeError("ParallelSkillRuntime instances are one-shot.") + _validate_identifier(workflow_id, field_name="workflow_id") + if eligible_mask is None: + eligible = torch.ones_like(self._pending) + else: + if ( + not isinstance(eligible_mask, torch.Tensor) + or eligible_mask.dtype != torch.bool + or eligible_mask.shape != self._pending.shape + or eligible_mask.device != self._pending.device + ): + raise ValueError("eligible_mask must match the parallel batch.") + eligible = eligible_mask.clone() + if not eligible.any(): + raise ValueError("eligible_mask must contain an active row.") + self._success.zero_() + self._failure.zero_() + self._cancelled.zero_() + self._pending = eligible.clone() + self._started_eligible = eligible.clone() + self._elapsed_steps = 0 + self._start_timestamp = self._read_clock() + self._command_count = 0 + self._wait_duration = 0.0 + self._message = None + self._force_mask_dispatch = False + self._terminal_stop_forwarded = False + self._held_target_fingerprints.clear() + self._last_hold_context = None + self._deferred_frame = None + self._deferred_lane_frames.clear() + self._terminal_hold_pending = False + self._next_transport_at = None + self._status = SkillStatus.RUNNING + started: list[ParallelRuntimeBranch] = [] + try: + for branch in self._branches: + branch.runtime.start( + *branch.calls, + workflow_id=f"{workflow_id}:{branch.branch_id}", + eligible_mask=eligible, + ) + started.append(branch) + except Exception as exc: + reason = "Parallel branch startup failed: " f"{type(exc).__name__}: {exc}" + for branch in started: + branch.runtime.cancel(reason) + self._failure = eligible.clone() + self._pending.zero_() + self._status = SkillStatus.FAILED + self._message = reason + return self.result + try: + self._sync_branch_identity() + self._update_barrier() + self._finish_if_complete() + except Exception as exc: + self._abort_coordinator("Parallel startup coordination failed", exc) + return self.result + + def step(self) -> ParallelSkillResult: + """Advance one deterministic coordinator state-machine transition.""" + if self._status is not SkillStatus.RUNNING: + return self.result + try: + self._update_elapsed_steps() + if self._elapsed_steps >= self._timeout_steps and ( + self._pending.any() or self._transport_flush_pending + ): + self._timeout_pending_rows() + self._finish_if_complete() + return self.result + transport_wait = self._remaining_transport_wait() + if transport_wait > 0.0: + self._wait_duration = transport_wait + return self.result + if self._deferred_frame is not None: + accepted = self._dispatch_deferred_frame() + if ( + accepted + and not self._pending.any() + and self._status is SkillStatus.RUNNING + and ( + self._runner_cfg.hold_on_completion + or bool((self._failure | self._cancelled).any().item()) + ) + ): + self._terminal_hold_pending = True + self._finish_if_complete() + return self.result + if self._terminal_hold_pending: + self._terminal_hold_pending = False + self._dispatch_requested_hold( + required=True, + include_last_targets=True, + ) + self._finish_if_complete() + return self.result + for branch in self._branches: + if not branch.runtime.result.terminal: + branch.runtime.step() + self._update_barrier() + self._dispatch_grid_frame() + self._finish_if_complete() + except Exception as exc: + self._abort_coordinator("Parallel coordinator step failed", exc) + return self.result + + def _timeout_pending_rows(self) -> None: + """Fail and safe-stop deadline-expired rows before another command.""" + timed_out = self._pending.clone() + if not timed_out.any() and self._transport_flush_pending: + timed_out = self._started_eligible.clone() + if not timed_out.any(): + return + self._failure |= timed_out + self._success &= ~timed_out + self._pending &= ~timed_out + self._deferred_frame = None + self._deferred_lane_frames.clear() + self._terminal_hold_pending = False + self._next_transport_at = None + self._message = f"Parallel barrier timed out after {self._timeout_steps} steps." + errors: list[str] = [] + for branch in self._branches: + if branch.runtime.result.terminal: + continue + try: + branch.runtime.cancel(self._message) + except Exception as exc: + errors.append(f"{branch.branch_id}: {type(exc).__name__}: {exc}") + stopped, stop_message = self._forward_safe_stop() + self._terminal_stop_forwarded = True + if not stopped and stop_message is not None: + errors.append(stop_message) + if errors: + self._message += " Safe stop errors: " + "; ".join(errors) + + def _read_clock(self) -> float: + """Read one finite non-negative timestamp from the shared clock.""" + now = float(self._clock.now()) + if not math.isfinite(now) or now < 0.0: + raise ValueError("ExecutionClock.now() must be finite and non-negative.") + return now + + def _update_elapsed_steps(self) -> None: + """Measure completed environment-grid intervals since start.""" + assert self._start_timestamp is not None + now = self._read_clock() + elapsed = now - self._start_timestamp + if elapsed < -self._timing_policy.tolerance: + raise RuntimeError("Parallel execution clock moved backwards.") + ratio = max(0.0, elapsed) / self._timing_policy.step_dt + tolerance = self._timing_policy.tolerance / self._timing_policy.step_dt + self._elapsed_steps = max( + self._elapsed_steps, + int(math.floor(ratio + tolerance)), + ) + + def _sync_branch_identity(self) -> None: + """Adopt and verify env IDs after every lane's first observation.""" + reference = self._branches[0].runtime.result + for branch in self._branches[1:]: + result = branch.runtime.result + if ( + result.env_ids.device != reference.env_ids.device + or not torch.equal(result.env_ids, reference.env_ids) + or result.task_state.batch_size != reference.task_state.batch_size + or result.task_state.device != reference.task_state.device + ): + raise ValueError( + "Parallel branch observations must share env_ids, batch, " + "and device." + ) + self._env_ids = reference.env_ids.clone() + + def cancel( + self, + reason: str = "Parallel workflow cancelled by caller.", + ) -> ParallelSkillResult: + """Cancel every lane and forward one target-scoped transport cancel.""" + if type(reason) is not str or not reason: + raise ValueError("reason must be a non-empty string.") + if self._status is not SkillStatus.RUNNING: + return self.result + had_transport_flush = self._transport_flush_pending + cancelled = self._pending.clone() + if not cancelled.any() and had_transport_flush: + cancelled = self._started_eligible.clone() + self._deferred_frame = None + self._deferred_lane_frames.clear() + self._terminal_hold_pending = False + self._next_transport_at = None + errors: list[str] = [] + for branch in self._branches: + try: + branch.runtime.cancel(reason) + except Exception as exc: + errors.append(f"{branch.branch_id}: {type(exc).__name__}: {exc}") + stopped, stop_message = self._forward_safe_stop() + self._terminal_stop_forwarded = True + if stop_message is not None: + errors.append(stop_message) + self._pending &= ~cancelled + self._success &= ~cancelled + merge_succeeded = self._merge_verified_state() + if errors or not stopped or not merge_succeeded: + self._failure |= cancelled + self._cancelled &= ~cancelled + self._status = SkillStatus.FAILED + if errors or not stopped: + stop_detail = "; ".join(errors) or "unknown safe-stop failure" + self._message = reason + " Safe stop failed: " + stop_detail + elif self._message is None: + self._message = reason + " Verified-state merge failed." + else: + self._cancelled |= cancelled + self._status = SkillStatus.CANCELLED + self._message = reason + self._wait_duration = 0.0 + return self.result + + @property + def _transport_flush_pending(self) -> bool: + """Whether a retained command or mandatory final hold is outstanding.""" + return self._deferred_frame is not None or self._terminal_hold_pending + + def _remaining_transport_wait(self) -> float: + """Return time until another normal grid action may be forwarded.""" + ready_at = self._next_transport_at + if ready_at is None: + return 0.0 + remaining = ready_at - self._read_clock() + if remaining <= self._timing_policy.tolerance: + self._next_transport_at = None + return 0.0 + return remaining + + def _record_transport_action(self) -> None: + """Arm the next physical grid boundary after one accepted action.""" + interval = max( + self._timing_policy.step_dt, + self._runner_cfg.minimum_cycle_time, + ) + self._next_transport_at = self._read_clock() + interval + self._wait_duration = interval + + def _update_barrier(self) -> None: + results = {branch.branch_id: branch.runtime.result for branch in self._branches} + pending = { + branch_id: ( + result.eligible_mask + & ~result.success_mask + & ~result.failure_mask + & ~result.cancelled_mask + ) + for branch_id, result in results.items() + } + update = resolve_parallel_barrier( + pending_masks=pending, + success_masks={ + branch_id: result.success_mask for branch_id, result in results.items() + }, + failure_masks={ + branch_id: result.failure_mask | result.cancelled_mask + for branch_id, result in results.items() + }, + ) + new_failure = update.failure_mask & ~self._failure + self._failure |= update.failure_mask + self._success |= update.completed_mask & ~update.failure_mask + self._pending &= ~update.completed_mask + if new_failure.any(): + self._force_mask_dispatch = True + reason = "A peer parallel branch failed for these environment rows." + for branch in self._branches: + mask = update.cancellation_masks[branch.branch_id] + if mask.any(): + branch.runtime.deactivate_rows(mask, reason=reason) + running = tuple(result for result in results.values() if not result.terminal) + if not running or any(result.wait_duration <= 0.0 for result in running): + self._wait_duration = 0.0 + else: + self._wait_duration = min(result.wait_duration for result in running) + + def _dispatch_grid_frame(self) -> None: + fresh: dict[str, RuntimeCommandFrame] = {} + for branch in self._branches: + frame = branch.command_sink.drain_frame() + if frame is not None: + if branch.runtime.result.terminal: + raise ParallelSafetyError( + f"Parallel branch {branch.branch_id!r} became terminal " + "while emitting a fresh command frame. A post-command " + "observation is required before a safe terminal hold." + ) + fresh[branch.branch_id] = frame + force_mask_dispatch = self._force_mask_dispatch + self._force_mask_dispatch = False + if not fresh and not force_mask_dispatch: + self._dispatch_requested_hold() + return + plans: list[ParallelBranchPlan] = [] + lane_frames: dict[str, RuntimeCommandFrame] = {} + requested_holds = { + _target_fingerprint(target) + for branch in self._branches + for target in branch.command_sink.hold_request[0] + } + for branch in self._branches: + frame = fresh.get(branch.branch_id) + is_fresh = frame is not None + if frame is None: + frame = branch.command_sink.last_frame + if frame is None: + continue + if not is_fresh: + commands = tuple( + command + for command in frame.commands + if _target_fingerprint(command.target) + not in self._held_target_fingerprints | requested_holds + ) + if not commands: + continue + frame = RuntimeCommandFrame( + commands=commands, + active_mask=frame.active_mask, + env_ids=frame.env_ids, + hold_duration=frame.hold_duration, + ) + frame = frame.with_active_mask(frame.active_mask & ~self._failure) + lane_frames[branch.branch_id] = frame.snapshot() + plans.append( + ParallelBranchPlan( + branch_id=branch.branch_id, + claim=branch.claim, + commands=TimedCommandSequence( + frames=(frame,), + env_ids=frame.env_ids, + ), + ) + ) + if not plans: + self._dispatch_requested_hold() + return + if len(plans) == 1: + frame = plans[0].commands.frames[0] + durations = frame.hold_duration + expected = torch.full_like( + durations, + self._timing_policy.step_dt, + ) + if not torch.allclose( + durations, + expected, + atol=self._timing_policy.tolerance, + rtol=0.0, + ): + raise ValueError( + "Parallel command frames must equal the environment step grid." + ) + merged = plans[0].commands + else: + merged = align_parallel_commands(tuple(plans), self._timing_policy) + frame = merged.frames[0] + if not frame.active_mask.any(): + self._dispatch_requested_hold(extra_targets=frame.targets) + return + + if self._has_unforwarded_hold_targets(): + self._deferred_frame = frame.snapshot() + self._deferred_lane_frames = { + branch_id: branch_frame.snapshot() + for branch_id, branch_frame in lane_frames.items() + } + if not self._dispatch_requested_hold(): + self._deferred_frame = None + self._deferred_lane_frames.clear() + return + + # Drain duplicate requests to refresh the latest synchronized context + # without producing another action, then send exactly one grid frame. + self._dispatch_requested_hold() + accepted = self._send_merged_frame(frame, lane_frames) + if ( + accepted + and not self._pending.any() + and self._status is SkillStatus.RUNNING + and ( + self._runner_cfg.hold_on_completion + or bool((self._failure | self._cancelled).any().item()) + ) + ): + self._terminal_hold_pending = True + + def _dispatch_deferred_frame(self) -> bool: + """Send a frame retained behind one explicit hold-padding step.""" + frame = self._deferred_frame + if frame is None: + raise RuntimeError("No deferred parallel frame is available.") + lane_frames = { + branch_id: branch_frame.snapshot() + for branch_id, branch_frame in self._deferred_lane_frames.items() + } + self._deferred_frame = None + self._deferred_lane_frames.clear() + return self._send_merged_frame(frame, lane_frames) + + def _send_merged_frame( + self, + frame: RuntimeCommandFrame, + lane_frames: Mapping[str, RuntimeCommandFrame], + ) -> bool: + """Validate and forward one active synchronized command frame.""" + try: + safety_result = self._safety_validator.validate( + branch_frames=MappingProxyType(dict(lane_frames)), + merged_frame=frame.snapshot(), + ) + except ParallelSafetyError: + raise + except Exception as exc: + raise ParallelSafetyError( + "Parallel command safety validation failed: " + f"{type(exc).__name__}: {exc}" + ) from exc + if safety_result is not None: + raise ParallelSafetyError( + "ParallelCommandSafetyValidator.validate() must return None." + ) + acknowledgement = self._command_sink.send( + frame, + timeout=self._runner_cfg.command_timeout, + ) + if not isinstance(acknowledgement, CommandAcknowledgement): + raise TypeError("CommandSink.send() returned an invalid value.") + if not acknowledgement.accepted: + self._fail_transport(acknowledgement.message) + return False + self._command_count += 1 + self._record_transport_action() + self._held_target_fingerprints.difference_update( + _target_fingerprint(target) for target in frame.targets + ) + return True + + def _has_unforwarded_hold_targets(self) -> bool: + """Whether lane requests contain a target not already physically held.""" + for branch in self._branches: + targets, _ = branch.command_sink.hold_request + if any( + _target_fingerprint(target) not in self._held_target_fingerprints + for target in targets + ): + return True + return False + + def _dispatch_requested_hold( + self, + *, + extra_targets: tuple[RuntimeEndpointTarget, ...] = (), + include_last_targets: bool = False, + required: bool = False, + ) -> bool: + """Forward every lane hold without dropping earlier call targets.""" + targets: dict[Hashable, RuntimeEndpointTarget] = { + _target_fingerprint(target): target for target in extra_targets + } + context: PlanningContext | None = None + for branch in self._branches: + for ( + branch_targets, + branch_context, + ) in branch.command_sink.drain_hold_requests(): + for target in branch_targets: + targets[_target_fingerprint(target)] = target + context = branch_context + self._last_hold_context = branch_context + if include_last_targets: + last_frame = branch.command_sink.last_frame + if last_frame is not None: + for target in last_frame.targets: + targets[_target_fingerprint(target)] = target + targets = { + key: target + for key, target in targets.items() + if key not in self._held_target_fingerprints + } + if not targets: + return True + if context is None: + context = self._last_hold_context + if context is None: + message = "Parallel hold targets have no synchronized planning context." + if required or targets: + self._fail_transport(message) + return False + acknowledgement = self._command_sink.hold( + tuple(targets.values()), + context, + timeout=self._runner_cfg.safe_stop_timeout, + ) + if not isinstance(acknowledgement, CommandAcknowledgement): + raise TypeError("CommandSink.hold() returned an invalid value.") + if not acknowledgement.accepted: + self._fail_transport(acknowledgement.message) + return False + self._held_target_fingerprints.update(targets) + self._last_hold_context = context + self._record_transport_action() + return True + + def _fail_transport(self, message: str) -> None: + self._deferred_frame = None + self._deferred_lane_frames.clear() + self._terminal_hold_pending = False + self._next_transport_at = None + failed = self._pending.clone() + if not failed.any(): + failed = self._started_eligible.clone() + self._failure |= failed + self._success &= ~failed + self._pending &= ~failed + self._message = "Parallel command transport rejected the merged operation." + if message: + self._message += f" {message}" + for branch in self._branches: + branch.runtime.cancel(self._message) + self._forward_safe_stop() + self._terminal_stop_forwarded = True + + def _forward_safe_stop(self) -> tuple[bool, str | None]: + """Forward lane-owned cancellation and hold once to the real sink.""" + targets: dict[Hashable, RuntimeEndpointTarget] = {} + context: PlanningContext | None = self._last_hold_context + for branch in self._branches: + for target in branch.command_sink.cancel_targets: + targets[_target_fingerprint(target)] = target + branch_targets, branch_context = branch.command_sink.hold_request + for target in branch_targets: + targets[_target_fingerprint(target)] = target + if branch_context is not None: + context = branch_context + last_frame = branch.command_sink.last_frame + if last_frame is not None: + for target in last_frame.targets: + targets[_target_fingerprint(target)] = target + if not targets: + return True, None + snapshots = tuple(targets.values()) + errors: list[str] = [] + try: + cancel_ack = self._command_sink.cancel( + snapshots, + timeout=self._runner_cfg.safe_stop_timeout, + ) + if not isinstance(cancel_ack, CommandAcknowledgement): + raise TypeError("CommandSink.cancel() returned an invalid value.") + if not cancel_ack.accepted: + errors.append(cancel_ack.message or "transport cancel was rejected") + except Exception as exc: + errors.append(f"cancel {type(exc).__name__}: {exc}") + if context is None: + errors.append("no planning context was available for final safe hold") + else: + try: + hold_ack = self._command_sink.hold( + snapshots, + context, + timeout=self._runner_cfg.safe_stop_timeout, + ) + if not isinstance(hold_ack, CommandAcknowledgement): + raise TypeError("CommandSink.hold() returned an invalid value.") + if not hold_ack.accepted: + errors.append(hold_ack.message or "transport hold was rejected") + except Exception as exc: + errors.append(f"hold {type(exc).__name__}: {exc}") + if not errors: + self._held_target_fingerprints.update( + _target_fingerprint(target) for target in snapshots + ) + self._last_hold_context = context + return (not errors), (None if not errors else "; ".join(errors)) + + def _abort_coordinator(self, prefix: str, exc: Exception) -> None: + """Convert an internal tick exception into a safe terminal failure.""" + self._deferred_frame = None + self._deferred_lane_frames.clear() + self._terminal_hold_pending = False + self._next_transport_at = None + reason = f"{prefix}: {type(exc).__name__}: {exc}" + failed = self._pending.clone() + if not failed.any(): + failed = self._started_eligible.clone() + self._failure |= failed + self._success &= ~failed + self._pending &= ~failed + errors: list[str] = [] + for branch in self._branches: + if branch.runtime.result.terminal: + continue + try: + branch.runtime.cancel(reason) + except Exception as cancel_exc: + errors.append( + f"{branch.branch_id}: {type(cancel_exc).__name__}: {cancel_exc}" + ) + stopped, stop_message = self._forward_safe_stop() + self._terminal_stop_forwarded = True + if not stopped and stop_message is not None: + errors.append(stop_message) + self._message = reason + if errors: + self._message += " Safe stop errors: " + "; ".join(errors) + self._merge_verified_state() + self._status = SkillStatus.FAILED + self._wait_duration = 0.0 + + def _merge_verified_state(self) -> bool: + """Merge every branch-local verified patch at a terminal barrier.""" + effects = { + branch.branch_id: ( + _task_state_delta( + self._initial_state, + branch.runtime.result.task_state, + ), + self._started_eligible, + ) + for branch in self._branches + } + try: + self._task_state = merge_parallel_effects(self._initial_state, effects) + except Exception as exc: + self._failure |= self._started_eligible + self._success.zero_() + merge_message = ( + "Parallel verified-state merge failed: " f"{type(exc).__name__}: {exc}" + ) + self._message = ( + merge_message + if self._message is None + else f"{self._message} {merge_message}" + ) + return False + return True + + def _finish_if_complete(self) -> None: + if self._pending.any(): + return + if self._deferred_frame is not None or self._terminal_hold_pending: + return + self._merge_verified_state() + if self._status is SkillStatus.RUNNING and not self._terminal_stop_forwarded: + terminal_failure = bool((self._failure | self._cancelled).any().item()) + require_hold = self._runner_cfg.hold_on_completion or terminal_failure + self._dispatch_requested_hold( + required=require_hold, + include_last_targets=require_hold, + ) + self._wait_duration = 0.0 + if self._failure.any(): + self._status = SkillStatus.FAILED + elif self._cancelled.any(): + self._status = SkillStatus.CANCELLED + else: + self._status = SkillStatus.COMPLETED + + +__all__ = [ + "analyze_parallel_branches", + "ParallelBranchRuntime", + "ParallelBranchStaticAnalysis", + "ParallelCommandSafetyValidator", + "ParallelLaneCommandSink", + "ParallelRuntimeBranch", + "ParallelSkillResult", + "ParallelSkillRuntime", + "ParallelSafetyError", +] diff --git a/embodichain/lab/sim/skills/profiles.py b/embodichain/lab/sim/skills/profiles.py new file mode 100644 index 000000000..5aef8053a --- /dev/null +++ b/embodichain/lab/sim/skills/profiles.py @@ -0,0 +1,2414 @@ +# ---------------------------------------------------------------------------- +# 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 robot resources, skill binding, and policy presets.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from copy import deepcopy +from dataclasses import dataclass, field, fields, is_dataclass +from enum import Enum +from itertools import product +from types import MappingProxyType +from typing import ClassVar, Mapping, TYPE_CHECKING + +import torch + +from embodichain.lab.sim.atomic_actions.bindings import ( + ActionBinding, + EndpointBinding, + JointPositionTarget, + RuntimeEndpointTarget, +) +from embodichain.lab.sim.atomic_actions.control import ( + ControlCommand, + ControlPartCommandProfile, + JointPositionCommand, +) +from embodichain.lab.sim.atomic_actions.core import SkillDescriptor +from embodichain.lab.sim.atomic_actions.invocation import ActionOptions +from embodichain.lab.sim.atomic_actions.policies import MotionPolicy, RecoveryPolicy +from embodichain.lab.sim.atomic_actions.tracking import ( + JOINT_POSITION_CHANNEL, + EndpointTrackingChannelBinding, + EndpointTrackingFeedbackAddress, + TrackingFeedbackSourceRef, + TrackingPolicy, + TrackingProjectorRef, +) +from embodichain.lab.sim.atomic_actions.requirements import ( + BATCH_INVERSE_KINEMATICS_CAPABILITY, + DisjointResourceSlots, + DisjointSlotEndpoints, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + INVERSE_KINEMATICS_CAPABILITY, + SkillBindingContract, + SkillResourceSlot, +) +from embodichain.lab.sim.atomic_actions.runner import ExecutionRunnerCfg +from .effects import ( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + FORCE_EFFECT_CHANNEL, + JOINT_STATE_EFFECT_CHANNEL, + POSE_RELATION_EFFECT_CHANNEL, + ControlPartEvidenceAddress, + EffectEvidenceSourceRef, + EffectMonitorRef, +) + +if TYPE_CHECKING: + from embodichain.lab.sim.atomic_actions.engine import AtomicActionEngine + + +class ProfileValidationError(ValueError): + """Raised when a robot skill profile disagrees with its engine or robot.""" + + +class UnsupportedSkillError(ValueError): + """Raised when no robot-resource assignment can satisfy a skill.""" + + +class AmbiguousSkillBindingError(ValueError): + """Raised when multiple assignments remain without a complete default.""" + + +_SOLVER_BACKED_CAPABILITIES = frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + INVERSE_KINEMATICS_CAPABILITY, + } +) + + +def _validate_identifier(value: str, *, field_name: str) -> str: + """Return one strict, whitespace-free identifier.""" + if not isinstance(value, str) or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty string without outer whitespace." + ) + return value + + +def _snapshot_graph_tokens( + value: object, + *, + path: str, + visited: set[int], +) -> set[tuple[object, ...]]: + """Collect identities for every mutable value and tensor storage. + + Immutable containers are traversed because they may retain mutable leaves. + Unknown opaque values fail closed: an action-options declaration must expose + its complete snapshot graph through dataclass fields and built-in containers. + """ + if value is None or type(value) in { + bool, + int, + float, + complex, + str, + bytes, + range, + slice, + torch.device, + torch.dtype, + }: + return set() + if isinstance(value, (Enum, type)): + return set() + + value_id = id(value) + if value_id in visited: + return set() + visited.add(value_id) + + if isinstance(value, torch.Tensor): + tokens: set[tuple[object, ...]] = {("object", value_id)} + storage = value.untyped_storage() + if storage.nbytes() > 0: + tokens.add( + ( + "tensor_storage", + value.device.type, + value.device.index, + storage.data_ptr(), + ) + ) + return tokens + if is_dataclass(value) and not isinstance(value, type): + tokens = {("object", value_id)} + for data_field in fields(value): + tokens.update( + _snapshot_graph_tokens( + getattr(value, data_field.name), + path=f"{path}.{data_field.name}", + visited=visited, + ) + ) + return tokens + if type(value) is dict: + tokens = {("object", value_id)} + for key, nested in value.items(): + tokens.update( + _snapshot_graph_tokens( + key, + path=f"{path}.", + visited=visited, + ) + ) + tokens.update( + _snapshot_graph_tokens( + nested, + path=f"{path}[{key!r}]", + visited=visited, + ) + ) + return tokens + if type(value) in {list, set, bytearray}: + tokens = {("object", value_id)} + for index, nested in enumerate(value): + tokens.update( + _snapshot_graph_tokens( + nested, + path=f"{path}[{index}]", + visited=visited, + ) + ) + return tokens + if type(value) in {tuple, frozenset}: + tokens = set() + for index, nested in enumerate(value): + tokens.update( + _snapshot_graph_tokens( + nested, + path=f"{path}[{index}]", + visited=visited, + ) + ) + return tokens + raise TypeError( + f"Action-options snapshot graph contains unsupported opaque value " + f"{type(value).__module__}.{type(value).__qualname__} at {path}." + ) + + +def _snapshot_action_options(options: ActionOptions) -> ActionOptions: + """Return one exact action-options snapshot with no mutable aliasing.""" + if not isinstance(options, ActionOptions): + raise TypeError( + "action_option_templates values must be ActionOptions instances." + ) + option_type = type(options) + dataclass_params = option_type.__dict__.get("__dataclass_params__") + dataclass_fields = option_type.__dict__.get("__dataclass_fields__") + if ( + dataclass_params is None + or dataclass_fields is None + or dataclass_params.frozen is not True + ): + raise TypeError( + "action_option_templates values must be exact frozen @dataclass " + "declarations, not inherited undecorated ActionOptions subclasses." + ) + if hasattr(options, "__dict__"): + raise TypeError("action_option_templates values must not carry __dict__ state.") + field_names = {data_field.name for data_field in fields(options)} + declared_slots: set[str] = set() + for base in option_type.__mro__: + slots = base.__dict__.get("__slots__", ()) + if isinstance(slots, str): + declared_slots.add(slots) + else: + declared_slots.update(slots) + opaque_slots = declared_slots.difference(field_names, {"__weakref__"}) + if opaque_slots: + raise TypeError( + "action_option_templates values must not carry non-dataclass " + f"slot state: {sorted(opaque_slots)}." + ) + snapshot = deepcopy(options) + if type(snapshot) is not option_type or snapshot is options: + raise TypeError( + "action_option_templates values must support independent deep-copy " + "snapshots of their exact type." + ) + source_tokens = _snapshot_graph_tokens( + options, + path=option_type.__name__, + visited=set(), + ) + snapshot_tokens = _snapshot_graph_tokens( + snapshot, + path=option_type.__name__, + visited=set(), + ) + if source_tokens.intersection(snapshot_tokens): + raise TypeError( + "action_option_templates values must support independently owned " + "snapshots without shared mutable objects or tensor storage." + ) + return snapshot + + +def _normalize_identifier_set( + values: frozenset[str], + *, + field_name: str, +) -> frozenset[str]: + """Validate one immutable set of identifiers.""" + if isinstance(values, (str, bytes)): + raise TypeError(f"{field_name} must be an iterable of strings, not a string.") + try: + normalized = frozenset(values) + except TypeError as exc: + raise TypeError(f"{field_name} must be an iterable of strings.") from exc + for value in normalized: + _validate_identifier(value, field_name=field_name) + return normalized + + +def _snapshot_endpoint_commands( + values: Mapping[str, ControlCommand], + *, + field_name: str, +) -> Mapping[str, ControlCommand]: + """Validate, snapshot, and freeze commands exposed by one endpoint.""" + if not isinstance(values, Mapping): + raise TypeError(f"{field_name} must be a mapping.") + snapshots: dict[str, ControlCommand] = {} + for command_name, command in values.items(): + _validate_identifier(command_name, field_name=f"{field_name} keys") + if not isinstance(command, ControlCommand): + raise TypeError(f"{field_name} values must be ControlCommand instances.") + snapshot = command.snapshot() + if type(snapshot) is not type(command) or snapshot is command: + raise TypeError( + f"{field_name}[{command_name!r}].snapshot() must return an " + "independently owned value of the same ControlCommand type." + ) + snapshots[command_name] = snapshot + return MappingProxyType(snapshots) + + +def _snapshot_effect_sources( + values: Mapping[str, EffectEvidenceSourceRef], + *, + field_name: str, +) -> Mapping[str, EffectEvidenceSourceRef]: + """Validate, own, and freeze endpoint observation sources by channel.""" + if not isinstance(values, Mapping): + raise TypeError(f"{field_name} must be a mapping.") + snapshots: dict[str, EffectEvidenceSourceRef] = {} + for channel, source in values.items(): + _validate_identifier(channel, field_name=f"{field_name} channel names") + if not isinstance(source, EffectEvidenceSourceRef): + raise TypeError( + f"{field_name} values must be EffectEvidenceSourceRef instances." + ) + snapshot = source.snapshot() + if snapshot is source: + raise TypeError( + f"{field_name}[{channel!r}].snapshot() must return an independent " + "source reference." + ) + if ( + isinstance(snapshot.address, ControlPartEvidenceAddress) + and snapshot.address.channel != channel + ): + raise ValueError( + f"{field_name}[{channel!r}] disagrees with its control-part " + f"address channel {snapshot.address.channel!r}." + ) + snapshots[channel] = snapshot + return MappingProxyType(snapshots) + + +def _snapshot_tracking_channels( + values: Mapping[str, EndpointTrackingChannelBinding], + *, + field_name: str, +) -> Mapping[str, EndpointTrackingChannelBinding]: + """Validate, own, and freeze endpoint tracking bindings by channel.""" + if not isinstance(values, Mapping): + raise TypeError(f"{field_name} must be a mapping.") + snapshots: dict[str, EndpointTrackingChannelBinding] = {} + for channel_id, binding in values.items(): + _validate_identifier(channel_id, field_name=f"{field_name} channel IDs") + if not isinstance(binding, EndpointTrackingChannelBinding): + raise TypeError( + f"{field_name} values must be EndpointTrackingChannelBinding " + "instances." + ) + if binding.channel_id != channel_id: + raise ValueError( + f"{field_name}[{channel_id!r}] disagrees with binding channel " + f"{binding.channel_id!r}." + ) + snapshot = binding.snapshot() + if snapshot is binding: + raise TypeError( + f"{field_name}[{channel_id!r}].snapshot() must return an " + "independent channel binding." + ) + snapshots[channel_id] = snapshot + return MappingProxyType(snapshots) + + +@dataclass(frozen=True, slots=True, kw_only=True) +class ResourceEndpoint(ABC): + """Extensible execution endpoint in a robot resource graph. + + Endpoint subclasses add controller-specific addressing data. Capabilities + stay on this common base so skill matching does not depend on any one + controller kind. + """ + + capabilities: frozenset[str] = frozenset() + """Open, namespaced capabilities provided by this exact endpoint.""" + + def __post_init__(self) -> None: + object.__setattr__( + self, + "capabilities", + _normalize_identifier_set( + self.capabilities, + field_name="ResourceEndpoint.capabilities", + ), + ) + + def snapshot(self) -> ResourceEndpoint: + """Return an independently owned endpoint declaration. + + Endpoint subclasses with payloads that cannot be deep-copied must + override this method and return a new value of their exact type. + """ + return deepcopy(self) + + +@dataclass(frozen=True, slots=True) +class ControlPartEndpoint(ResourceEndpoint): + """One named execution endpoint backed by a robot control part. + + Capabilities are explicit and never inferred from the endpoint name, joint + count, other endpoints, or composite resource members. + """ + + control_part: str + """Key from the bound robot's ``control_parts`` mapping.""" + + command_profile: str | None = None + """Optional generic command-profile ID; defaults to ``control_part``.""" + + def __post_init__(self) -> None: + ResourceEndpoint.__post_init__(self) + _validate_identifier( + self.control_part, + field_name="ControlPartEndpoint.control_part", + ) + if self.command_profile is not None: + _validate_identifier( + self.command_profile, + field_name="ControlPartEndpoint.command_profile", + ) + + +@dataclass(frozen=True, slots=True) +class EndpointResolution: + """Adapter-produced runtime destination and claim metadata for one endpoint.""" + + runtime_target: RuntimeEndpointTarget + """Typed immutable destination consumed by an endpoint command transport.""" + + task_state_key: str | None = None + """Optional symbolic state key; profile binding defaults to its resource ID.""" + + effect_sources: Mapping[str, EffectEvidenceSourceRef] = field(default_factory=dict) + """Provider-routed raw observation sources keyed by open channel ID.""" + + tracking_channels: Mapping[str, EndpointTrackingChannelBinding] = field( + default_factory=dict + ) + """Typed feedback source and desired-state projector by channel ID.""" + + command_profile_key: str | None = None + """Profile key that owns semantic commands for this endpoint, when any.""" + + requires_command_profile: bool = False + """Whether a missing ``command_profile_key`` entry invalidates binding.""" + + claim_tokens: frozenset[str] = frozenset() + """Adapter-defined physical/controller claims beyond robot joint IDs.""" + + joint_ids: tuple[int, ...] = () + """Ordered robot joint IDs controlled by the endpoint, when applicable.""" + + exclusive: bool = True + """Whether this execution endpoint must declare a physical claim.""" + + def __post_init__(self) -> None: + if not isinstance(self.runtime_target, RuntimeEndpointTarget): + raise TypeError( + "EndpointResolution.runtime_target must be a " "RuntimeEndpointTarget." + ) + target = self.runtime_target.snapshot() + if ( + type(target) is not type(self.runtime_target) + or target is self.runtime_target + ): + raise TypeError( + "RuntimeEndpointTarget.snapshot() must return an independently " + "owned value of the same target type." + ) + _validate_identifier( + target.transport_id, + field_name="RuntimeEndpointTarget.transport_id", + ) + _validate_identifier( + target.target_id, + field_name="RuntimeEndpointTarget.target_id", + ) + object.__setattr__(self, "runtime_target", target) + if self.task_state_key is not None: + _validate_identifier( + self.task_state_key, + field_name="EndpointResolution.task_state_key", + ) + object.__setattr__( + self, + "effect_sources", + _snapshot_effect_sources( + self.effect_sources, + field_name="EndpointResolution.effect_sources", + ), + ) + object.__setattr__( + self, + "tracking_channels", + _snapshot_tracking_channels( + self.tracking_channels, + field_name="EndpointResolution.tracking_channels", + ), + ) + if self.command_profile_key is not None: + _validate_identifier( + self.command_profile_key, + field_name="EndpointResolution.command_profile_key", + ) + if not isinstance(self.requires_command_profile, bool): + raise TypeError("requires_command_profile must be a bool.") + if self.requires_command_profile and self.command_profile_key is None: + raise ValueError( + "requires_command_profile needs a non-None command_profile_key." + ) + object.__setattr__( + self, + "claim_tokens", + _normalize_identifier_set( + self.claim_tokens, + field_name="EndpointResolution.claim_tokens", + ), + ) + joint_ids = tuple(self.joint_ids) + if not all( + isinstance(joint_id, int) + and not isinstance(joint_id, bool) + and joint_id >= 0 + for joint_id in joint_ids + ): + raise ValueError( + "EndpointResolution.joint_ids must be non-negative integers." + ) + if len(set(joint_ids)) != len(joint_ids): + raise ValueError("EndpointResolution.joint_ids must be unique.") + if isinstance(target, JointPositionTarget) and joint_ids != target.joint_ids: + raise ValueError( + "EndpointResolution.joint_ids must exactly match its " + "JointPositionTarget." + ) + object.__setattr__(self, "joint_ids", joint_ids) + if not isinstance(self.exclusive, bool): + raise TypeError("EndpointResolution.exclusive must be a bool.") + if self.exclusive and not joint_ids and not self.claim_tokens: + raise ValueError( + "An exclusive EndpointResolution must declare joint_ids or " + "claim_tokens." + ) + + +class ResourceEndpointAdapter(ABC): + """Resolve one endpoint kind without coupling profiles to its controller.""" + + adapter_id: ClassVar[str] + """Stable adapter identifier used in diagnostics and resolved metadata.""" + + endpoint_type: ClassVar[type[ResourceEndpoint]] + """Exact endpoint declaration type accepted by this adapter.""" + + runtime_transport_ids: ClassVar[frozenset[str]] + """Exact endpoint-command transport IDs this adapter may resolve.""" + + runtime_target_types: ClassVar[tuple[type[RuntimeEndpointTarget], ...]] + """Exact immutable runtime-target value types this adapter may resolve.""" + + tracking_feedback_source_keys: ClassVar[frozenset[tuple[str, str]]] + """Exact ``(provider_id, revision)`` tracking-feedback routes emitted.""" + + tracking_projector_keys: ClassVar[frozenset[tuple[str, str]]] + """Exact ``(projector_id, revision)`` desired-state routes emitted.""" + + effect_evidence_source_keys: ClassVar[frozenset[tuple[str, str]]] + """Exact ``(provider_id, revision)`` effect-evidence routes emitted.""" + + @abstractmethod + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: AtomicActionEngine, + ) -> EndpointResolution: + """Validate and resolve one endpoint against an action engine. + + Args: + endpoint: Endpoint declaration of :attr:`endpoint_type`. + engine: Engine whose robot, planner, and command profiles are bound. + + Returns: + Physical claims and supported lowering metadata. + """ + + +class ControlPartEndpointAdapter(ResourceEndpointAdapter): + """Resolve joint-backed :class:`ControlPartEndpoint` declarations.""" + + adapter_id: ClassVar[str] = "control_part" + endpoint_type: ClassVar[type[ResourceEndpoint]] = ControlPartEndpoint + runtime_transport_ids: ClassVar[frozenset[str]] = frozenset( + {JointPositionTarget.TRANSPORT_ID} + ) + runtime_target_types: ClassVar[tuple[type[RuntimeEndpointTarget], ...]] = ( + JointPositionTarget, + ) + tracking_feedback_source_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset( + {("planning_context.robot", "1")} + ) + tracking_projector_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset( + {("joint_position_payload", "1")} + ) + effect_evidence_source_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset( + { + ( + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + ) + } + ) + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: AtomicActionEngine, + ) -> EndpointResolution: + """Resolve a robot control part and verify its standard capabilities.""" + if not isinstance(endpoint, ControlPartEndpoint): + raise TypeError("ControlPartEndpointAdapter requires ControlPartEndpoint.") + control_parts = getattr(engine.robot, "control_parts", None) + if not isinstance(control_parts, Mapping): + raise ProfileValidationError( + "ControlPartEndpoint requires Robot.control_parts." + ) + if endpoint.control_part not in control_parts: + available = sorted(str(name) for name in control_parts) + raise ProfileValidationError( + f"ControlPartEndpoint references unknown control part " + f"{endpoint.control_part!r}; Robot.control_parts contains " + f"{available}." + ) + joint_ids = tuple(engine.robot.get_joint_ids(name=endpoint.control_part)) + if not joint_ids: + raise ProfileValidationError( + f"Control part {endpoint.control_part!r} contains no joints." + ) + if len(set(joint_ids)) != len(joint_ids): + raise ProfileValidationError( + f"Control part {endpoint.control_part!r} contains duplicate joint IDs." + ) + declared = endpoint.capabilities & _SOLVER_BACKED_CAPABILITIES + if declared: + get_solver = getattr(engine.robot, "get_solver", None) + if not callable(get_solver): + raise ProfileValidationError( + f"Control part {endpoint.control_part!r} declares solver-backed " + f"capabilities {sorted(declared)}, but the robot exposes no " + "get_solver()." + ) + try: + solver = get_solver(name=endpoint.control_part) + except Exception as exc: + raise ProfileValidationError( + f"Could not validate solver-backed capabilities for control " + f"part {endpoint.control_part!r}: {exc}" + ) from exc + if solver is None: + raise ProfileValidationError( + f"Control part {endpoint.control_part!r} declares solver-backed " + f"capabilities {sorted(declared)}, but has no configured solver." + ) + effect_channels = { + POSE_RELATION_EFFECT_CHANNEL, + JOINT_STATE_EFFECT_CHANNEL, + } + if GRASP_CAPABILITY in endpoint.capabilities: + effect_channels.update( + { + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + FORCE_EFFECT_CHANNEL, + } + ) + runtime_target = JointPositionTarget( + control_part=endpoint.control_part, + joint_ids=joint_ids, + ) + tracking_channel = EndpointTrackingChannelBinding( + JOINT_POSITION_CHANNEL, + TrackingFeedbackSourceRef( + "planning_context.robot", + "1", + EndpointTrackingFeedbackAddress( + runtime_target, + JOINT_POSITION_CHANNEL, + ), + ), + TrackingProjectorRef("joint_position_payload", "1"), + ) + return EndpointResolution( + runtime_target=runtime_target, + command_profile_key=( + endpoint.control_part + if endpoint.command_profile is None + else endpoint.command_profile + ), + requires_command_profile=endpoint.command_profile is not None, + effect_sources={ + channel: EffectEvidenceSourceRef( + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + ControlPartEvidenceAddress(endpoint.control_part, channel), + ) + for channel in sorted(effect_channels) + }, + tracking_channels={JOINT_POSITION_CHANNEL: tracking_channel}, + claim_tokens=frozenset({f"robot.control_part:{endpoint.control_part}"}), + joint_ids=joint_ids, + ) + + +@dataclass(frozen=True, slots=True) +class ResolvedResourceEndpoint: + """Endpoint declaration resolved by one registered adapter.""" + + endpoint: ResourceEndpoint + adapter_id: str + runtime_target: RuntimeEndpointTarget + task_state_key: str | None = None + effect_sources: Mapping[str, EffectEvidenceSourceRef] = field(default_factory=dict) + tracking_channels: Mapping[str, EndpointTrackingChannelBinding] = field( + default_factory=dict + ) + command_profile_key: str | None = None + requires_command_profile: bool = False + commands: Mapping[str, ControlCommand] = field(default_factory=dict) + claim_tokens: frozenset[str] = frozenset() + joint_ids: tuple[int, ...] = () + exclusive: bool = True + + def __post_init__(self) -> None: + if not isinstance(self.endpoint, ResourceEndpoint): + raise TypeError("endpoint must be a ResourceEndpoint.") + endpoint_snapshot = self.endpoint.snapshot() + if ( + type(endpoint_snapshot) is not type(self.endpoint) + or endpoint_snapshot is self.endpoint + ): + raise TypeError( + "endpoint.snapshot() must return an independently owned value of " + "the same endpoint type." + ) + object.__setattr__(self, "endpoint", endpoint_snapshot) + _validate_identifier( + self.adapter_id, + field_name="ResolvedResourceEndpoint.adapter_id", + ) + resolution = EndpointResolution( + runtime_target=self.runtime_target, + task_state_key=self.task_state_key, + effect_sources=self.effect_sources, + tracking_channels=self.tracking_channels, + command_profile_key=self.command_profile_key, + requires_command_profile=self.requires_command_profile, + claim_tokens=self.claim_tokens, + joint_ids=self.joint_ids, + exclusive=self.exclusive, + ) + object.__setattr__(self, "runtime_target", resolution.runtime_target) + resolved_state_key = ( + resolution.runtime_target.target_id + if resolution.task_state_key is None + else resolution.task_state_key + ) + object.__setattr__(self, "task_state_key", resolved_state_key) + object.__setattr__(self, "effect_sources", resolution.effect_sources) + object.__setattr__(self, "tracking_channels", resolution.tracking_channels) + object.__setattr__( + self, + "command_profile_key", + resolution.command_profile_key, + ) + object.__setattr__( + self, + "requires_command_profile", + resolution.requires_command_profile, + ) + object.__setattr__( + self, + "commands", + _snapshot_endpoint_commands( + self.commands, + field_name="ResolvedResourceEndpoint.commands", + ), + ) + object.__setattr__(self, "claim_tokens", resolution.claim_tokens) + object.__setattr__(self, "joint_ids", resolution.joint_ids) + object.__setattr__(self, "exclusive", resolution.exclusive) + + @property + def capabilities(self) -> frozenset[str]: + """Return capabilities declared by the source endpoint.""" + return self.endpoint.capabilities + + def conflicts_with(self, other: ResolvedResourceEndpoint) -> bool: + """Return whether two endpoints address overlapping physical channels.""" + if not isinstance(other, ResolvedResourceEndpoint): + raise TypeError("other must be a ResolvedResourceEndpoint.") + return bool( + ( + self.runtime_target.transport_id, + self.runtime_target.target_id, + ) + == ( + other.runtime_target.transport_id, + other.runtime_target.target_id, + ) + or self.claim_tokens & other.claim_tokens + or set(self.joint_ids) & set(other.joint_ids) + ) + + +def _normalize_endpoints( + values: Mapping[str, ResourceEndpoint], +) -> Mapping[str, ResourceEndpoint]: + """Validate and freeze resource endpoint declarations.""" + if not isinstance(values, Mapping): + raise TypeError("RobotResource.endpoints must be a mapping.") + normalized: dict[str, ResourceEndpoint] = {} + for endpoint_id, endpoint in values.items(): + _validate_identifier(endpoint_id, field_name="resource endpoint identifiers") + if not isinstance(endpoint, ResourceEndpoint): + raise TypeError( + "RobotResource.endpoints values must be ResourceEndpoint " "instances." + ) + snapshot = endpoint.snapshot() + if type(snapshot) is not type(endpoint) or snapshot is endpoint: + raise TypeError( + f"Endpoint {endpoint_id!r}.snapshot() must return an independently " + f"owned {type(endpoint).__name__}." + ) + normalized[endpoint_id] = snapshot + return MappingProxyType(normalized) + + +@dataclass(frozen=True, slots=True) +class RobotResource: + """Generic leaf or composite resource in one robot's resource DAG. + + A resource may expose any number of named endpoints. For example, one + manipulation participant may expose ``motion`` and ``grasp`` endpoints, + while a mobile base or whole-body controller may expose only ``motion``. + ``members`` describes physical claim composition and does not inherit + endpoint capabilities. + """ + + resource_id: str + endpoints: Mapping[str, ResourceEndpoint] = field(default_factory=dict) + members: tuple[str, ...] = () + + def __post_init__(self) -> None: + _validate_identifier(self.resource_id, field_name="RobotResource.resource_id") + object.__setattr__(self, "endpoints", _normalize_endpoints(self.endpoints)) + if isinstance(self.members, (str, bytes)): + raise TypeError( + "RobotResource.members must be an iterable of strings, not a string." + ) + try: + members = tuple(self.members) + except TypeError as exc: + raise TypeError( + "RobotResource.members must be an iterable of strings." + ) from exc + for member in members: + _validate_identifier(member, field_name="RobotResource.members") + if len(set(members)) != len(members): + raise ValueError("RobotResource.members must be unique.") + if self.resource_id in members: + raise ValueError("A robot resource cannot contain itself.") + if not members and not self.endpoints: + raise ValueError( + "A leaf RobotResource must expose at least one execution endpoint." + ) + object.__setattr__(self, "members", members) + + def snapshot(self) -> RobotResource: + """Return an independently owned resource declaration.""" + return RobotResource( + resource_id=self.resource_id, + endpoints=self.endpoints, + members=self.members, + ) + + +@dataclass(frozen=True, slots=True) +class ResourceBinding: + """Generic mapping from skill-local slots to robot resource IDs.""" + + resources: Mapping[str, str] + + def __post_init__(self) -> None: + if not isinstance(self.resources, Mapping): + raise TypeError("ResourceBinding.resources must be a mapping.") + normalized: dict[str, str] = {} + for slot_id, resource_id in self.resources.items(): + _validate_identifier(slot_id, field_name="ResourceBinding slot IDs") + _validate_identifier(resource_id, field_name="ResourceBinding resource IDs") + normalized[slot_id] = resource_id + object.__setattr__(self, "resources", MappingProxyType(normalized)) + + +@dataclass(frozen=True, slots=True) +class WorkflowRecoveryPolicy: + """Bound workflow-level recovery for curated semantic effect failures. + + The atomic action remains the owner of replans and whole-action retries. + This policy applies only after that action emits ``RECOVERY_REQUIRED`` and + returns control to :class:`~embodichain.lab.sim.skills.SkillRuntime`. + + Args: + max_recovery_attempts: Maximum recovery cycles for each environment row + at one semantic-call boundary. Zero disables workflow recovery. + """ + + max_recovery_attempts: int = 0 + + def __post_init__(self) -> None: + """Validate a finite, non-negative per-row recovery budget.""" + if type(self.max_recovery_attempts) is not int: + raise TypeError("max_recovery_attempts must be an integer.") + if not 0 <= self.max_recovery_attempts <= 100: + raise ValueError("max_recovery_attempts must be in [0, 100].") + + def snapshot(self) -> WorkflowRecoveryPolicy: + """Return an independently owned immutable policy.""" + return WorkflowRecoveryPolicy( + max_recovery_attempts=self.max_recovery_attempts, + ) + + +@dataclass(frozen=True, slots=True, init=False) +class SkillPolicyPreset: + """Versioned policies and typed semantic-call option templates.""" + + preset_id: str + schema_version: int + required_planner: str | None + """Optional planner backend required by this preset.""" + _motion_policy: MotionPolicy + _tracking_policy: TrackingPolicy + _recovery_policy: RecoveryPolicy + _workflow_recovery_policy: WorkflowRecoveryPolicy + _runner_cfg: ExecutionRunnerCfg + _effect_monitors: Mapping[str, EffectMonitorRef] + _action_option_templates: Mapping[str, ActionOptions] + + def __init__( + self, + preset_id: str, + *, + action_option_templates: Mapping[str, ActionOptions], + schema_version: int = 3, + motion_policy: MotionPolicy | None = None, + tracking_policy: TrackingPolicy | None = None, + recovery_policy: RecoveryPolicy | None = None, + workflow_recovery_policy: WorkflowRecoveryPolicy | None = None, + runner_cfg: ExecutionRunnerCfg | None = None, + effect_monitors: Mapping[str, EffectMonitorRef] | None = None, + required_planner: str | None = None, + ) -> None: + """Own one policy bundle without exposing mutable nested configuration.""" + _validate_identifier(preset_id, field_name="SkillPolicyPreset.preset_id") + if not isinstance(schema_version, int) or isinstance(schema_version, bool): + raise TypeError("SkillPolicyPreset.schema_version must be an integer.") + if schema_version != 3: + raise ValueError( + "Unsupported SkillPolicyPreset.schema_version " + f"{schema_version}; supported versions are [3]." + ) + if required_planner is not None: + _validate_identifier( + required_planner, + field_name="SkillPolicyPreset.required_planner", + ) + selected_motion = MotionPolicy() if motion_policy is None else motion_policy + selected_tracking = ( + TrackingPolicy.joint_position() + if tracking_policy is None + else tracking_policy + ) + selected_recovery = ( + RecoveryPolicy() if recovery_policy is None else recovery_policy + ) + selected_workflow_recovery = ( + WorkflowRecoveryPolicy() + if workflow_recovery_policy is None + else workflow_recovery_policy + ) + selected_runner = ExecutionRunnerCfg() if runner_cfg is None else runner_cfg + if not isinstance(selected_motion, MotionPolicy): + raise TypeError("motion_policy must be a MotionPolicy.") + if not isinstance(selected_tracking, TrackingPolicy): + raise TypeError("tracking_policy must be a TrackingPolicy.") + if not isinstance(selected_recovery, RecoveryPolicy): + raise TypeError("recovery_policy must be a RecoveryPolicy.") + if type(selected_workflow_recovery) is not WorkflowRecoveryPolicy: + raise TypeError( + "workflow_recovery_policy must be a WorkflowRecoveryPolicy." + ) + if not isinstance(selected_runner, ExecutionRunnerCfg): + raise TypeError("runner_cfg must be an ExecutionRunnerCfg.") + selected_effect_monitors = ( + { + semantic_id: EffectMonitorRef( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + ) + for semantic_id in ( + "pick", + "place", + "hand_over", + "operate_articulation", + ) + } + if effect_monitors is None + else effect_monitors + ) + if not isinstance(selected_effect_monitors, Mapping): + raise TypeError("effect_monitors must be a mapping or None.") + normalized_effect_monitors: dict[str, EffectMonitorRef] = {} + for semantic_id, monitor_ref in selected_effect_monitors.items(): + _validate_identifier( + semantic_id, + field_name="SkillPolicyPreset effect semantic IDs", + ) + if not isinstance(monitor_ref, EffectMonitorRef): + raise TypeError( + "effect_monitors values must be EffectMonitorRef instances." + ) + normalized_effect_monitors[semantic_id] = monitor_ref.snapshot() + if not isinstance(action_option_templates, Mapping): + raise TypeError("action_option_templates must be a mapping.") + normalized_action_option_templates: dict[str, ActionOptions] = {} + for semantic_id, options in action_option_templates.items(): + _validate_identifier( + semantic_id, + field_name="SkillPolicyPreset action-option semantic IDs", + ) + normalized_action_option_templates[semantic_id] = _snapshot_action_options( + options + ) + object.__setattr__(self, "preset_id", preset_id) + object.__setattr__(self, "schema_version", schema_version) + object.__setattr__(self, "required_planner", required_planner) + object.__setattr__(self, "_motion_policy", deepcopy(selected_motion)) + object.__setattr__(self, "_tracking_policy", deepcopy(selected_tracking)) + object.__setattr__(self, "_recovery_policy", deepcopy(selected_recovery)) + object.__setattr__( + self, + "_workflow_recovery_policy", + selected_workflow_recovery.snapshot(), + ) + object.__setattr__(self, "_runner_cfg", deepcopy(selected_runner)) + object.__setattr__( + self, + "_effect_monitors", + MappingProxyType(normalized_effect_monitors), + ) + object.__setattr__( + self, + "_action_option_templates", + MappingProxyType(normalized_action_option_templates), + ) + + @property + def motion_policy(self) -> MotionPolicy: + """Return an independently owned motion policy.""" + return deepcopy(self._motion_policy) + + @property + def recovery_policy(self) -> RecoveryPolicy: + """Return an independently owned recovery policy.""" + return deepcopy(self._recovery_policy) + + @property + def workflow_recovery_policy(self) -> WorkflowRecoveryPolicy: + """Return the bounded semantic-workflow recovery policy.""" + return self._workflow_recovery_policy.snapshot() + + @property + def tracking_policy(self) -> TrackingPolicy: + """Return independently owned endpoint-tracking settings.""" + return deepcopy(self._tracking_policy) + + @property + def runner_cfg(self) -> ExecutionRunnerCfg: + """Return an independently owned runner configuration.""" + return deepcopy(self._runner_cfg) + + @property + def effect_monitors(self) -> Mapping[str, EffectMonitorRef]: + """Return effect-monitor selections keyed by exact semantic call ID.""" + return MappingProxyType( + { + semantic_id: monitor_ref.snapshot() + for semantic_id, monitor_ref in self._effect_monitors.items() + } + ) + + @property + def action_option_templates(self) -> Mapping[str, ActionOptions]: + """Return owned option templates keyed by exact semantic call ID.""" + return MappingProxyType( + { + semantic_id: _snapshot_action_options(options) + for semantic_id, options in self._action_option_templates.items() + } + ) + + def action_option_template(self, semantic_id: str) -> ActionOptions: + """Return one owned template for an exact semantic call ID. + + Raises: + KeyError: If this preset does not declare the semantic call. + """ + _validate_identifier( + semantic_id, + field_name="SkillPolicyPreset action-option semantic ID", + ) + try: + template = self._action_option_templates[semantic_id] + except KeyError as exc: + raise KeyError( + f"Preset {self.preset_id!r} has no action-option template for " + f"semantic call {semantic_id!r}." + ) from exc + return _snapshot_action_options(template) + + def snapshot(self) -> SkillPolicyPreset: + """Return an independently owned preset value.""" + return SkillPolicyPreset( + preset_id=self.preset_id, + schema_version=self.schema_version, + motion_policy=self.motion_policy, + tracking_policy=self.tracking_policy, + recovery_policy=self.recovery_policy, + workflow_recovery_policy=self.workflow_recovery_policy, + runner_cfg=self.runner_cfg, + effect_monitors=self.effect_monitors, + action_option_templates=self.action_option_templates, + required_planner=self.required_planner, + ) + + +@dataclass(frozen=True, slots=True) +class ResourceClaim: + """Physical leaf and joint claim used for deterministic conflict checks.""" + + leaf_resource_ids: frozenset[str] + joint_ids: tuple[int, ...] + claim_tokens: frozenset[str] = frozenset() + + def __post_init__(self) -> None: + object.__setattr__( + self, + "leaf_resource_ids", + _normalize_identifier_set( + self.leaf_resource_ids, + field_name="ResourceClaim.leaf_resource_ids", + ), + ) + joint_ids = tuple(self.joint_ids) + if not all( + isinstance(joint_id, int) + and not isinstance(joint_id, bool) + and joint_id >= 0 + for joint_id in joint_ids + ): + raise ValueError("ResourceClaim.joint_ids must be non-negative integers.") + if tuple(sorted(set(joint_ids))) != joint_ids: + raise ValueError("ResourceClaim.joint_ids must be sorted and unique.") + object.__setattr__(self, "joint_ids", joint_ids) + object.__setattr__( + self, + "claim_tokens", + _normalize_identifier_set( + self.claim_tokens, + field_name="ResourceClaim.claim_tokens", + ), + ) + + def conflicts_with(self, other: ResourceClaim) -> bool: + """Return whether two claims overlap in a leaf or concrete joint.""" + if not isinstance(other, ResourceClaim): + raise TypeError("other must be a ResourceClaim.") + return bool( + self.leaf_resource_ids & other.leaf_resource_ids + or self.claim_tokens & other.claim_tokens + or set(self.joint_ids) & set(other.joint_ids) + ) + + @classmethod + def combine(cls, claims: tuple[ResourceClaim, ...]) -> ResourceClaim: + """Return the union of zero or more resource claims.""" + leaves: set[str] = set() + joints: set[int] = set() + tokens: set[str] = set() + for claim in claims: + if not isinstance(claim, ResourceClaim): + raise TypeError("claims values must be ResourceClaim instances.") + leaves.update(claim.leaf_resource_ids) + joints.update(claim.joint_ids) + tokens.update(claim.claim_tokens) + return cls( + frozenset(leaves), + tuple(sorted(joints)), + frozenset(tokens), + ) + + +@dataclass(frozen=True, slots=True) +class ResolvedRobotResource: + """Robot-validated resource with concrete endpoint joint IDs and claim.""" + + resource_id: str + endpoints: Mapping[str, ResolvedResourceEndpoint] + members: tuple[str, ...] + claim: ResourceClaim + + def __post_init__(self) -> None: + _validate_identifier( + self.resource_id, + field_name="ResolvedRobotResource.resource_id", + ) + if not isinstance(self.endpoints, Mapping): + raise TypeError("endpoints must be a mapping.") + normalized_endpoints: dict[str, ResolvedResourceEndpoint] = {} + for endpoint_id, endpoint in self.endpoints.items(): + _validate_identifier(endpoint_id, field_name="resolved endpoint IDs") + if not isinstance(endpoint, ResolvedResourceEndpoint): + raise TypeError( + "ResolvedRobotResource.endpoints values must be " + "ResolvedResourceEndpoint instances." + ) + normalized_endpoints[endpoint_id] = endpoint + object.__setattr__( + self, + "endpoints", + MappingProxyType(normalized_endpoints), + ) + if isinstance(self.members, (str, bytes)): + raise TypeError("members must be an iterable of resource IDs.") + members = tuple(self.members) + for member in members: + _validate_identifier(member, field_name="resolved resource members") + if len(set(members)) != len(members): + raise ValueError("Resolved resource members must be unique.") + object.__setattr__(self, "members", members) + if not isinstance(self.claim, ResourceClaim): + raise TypeError("claim must be a ResourceClaim.") + endpoint_joints = { + joint_id + for endpoint in normalized_endpoints.values() + for joint_id in endpoint.joint_ids + } + missing_claim_joints = sorted(endpoint_joints - set(self.claim.joint_ids)) + if missing_claim_joints: + raise ValueError( + "Resolved resource claim does not cover endpoint joints " + f"{missing_claim_joints}." + ) + endpoint_tokens = { + token + for endpoint in normalized_endpoints.values() + for token in endpoint.claim_tokens + } + missing_claim_tokens = sorted(endpoint_tokens - self.claim.claim_tokens) + if missing_claim_tokens: + raise ValueError( + "Resolved resource claim does not cover endpoint claim tokens " + f"{missing_claim_tokens}." + ) + if not members: + if self.claim.leaf_resource_ids != frozenset({self.resource_id}): + raise ValueError( + "A resolved leaf resource claim must contain exactly its own " + "resource ID." + ) + if set(self.claim.joint_ids) != endpoint_joints: + raise ValueError( + "A resolved leaf resource claim must contain exactly its " + "endpoint joints." + ) + if self.claim.claim_tokens != frozenset(endpoint_tokens): + raise ValueError( + "A resolved leaf resource claim must contain exactly its " + "endpoint claim tokens." + ) + + @property + def endpoint_joint_ids(self) -> Mapping[str, tuple[int, ...]]: + """Return ordered joint IDs for each resolved endpoint.""" + return MappingProxyType( + { + endpoint_id: endpoint.joint_ids + for endpoint_id, endpoint in self.endpoints.items() + } + ) + + +@dataclass(frozen=True, slots=True) +class ResolvedSkillBinding: + """One generic resource assignment lowered for the current action core.""" + + skill_id: str + resources: Mapping[str, ResolvedRobotResource] + action_binding: ActionBinding + claim: ResourceClaim + + def __post_init__(self) -> None: + _validate_identifier(self.skill_id, field_name="ResolvedSkillBinding.skill_id") + if not isinstance(self.resources, Mapping): + raise TypeError("resources must be a mapping.") + normalized: dict[str, ResolvedRobotResource] = {} + for slot_id, resource in self.resources.items(): + _validate_identifier(slot_id, field_name="resolved skill slot IDs") + if not isinstance(resource, ResolvedRobotResource): + raise TypeError( + "ResolvedSkillBinding.resources values must be " + "ResolvedRobotResource instances." + ) + normalized[slot_id] = resource + object.__setattr__(self, "resources", MappingProxyType(normalized)) + if not isinstance(self.action_binding, ActionBinding): + raise TypeError("action_binding must be an ActionBinding.") + if not isinstance(self.claim, ResourceClaim): + raise TypeError("claim must be a ResourceClaim.") + + @property + def resource_ids(self) -> Mapping[str, str]: + """Return the selected logical resource ID for each skill-local slot.""" + return MappingProxyType( + { + slot_id: resource.resource_id + for slot_id, resource in self.resources.items() + } + ) + + +def _normalize_resources( + values: Mapping[str, RobotResource], +) -> Mapping[str, RobotResource]: + """Validate profile resource ownership and mapping keys.""" + if not isinstance(values, Mapping): + raise TypeError("RobotSkillProfile.resources must be a mapping.") + normalized: dict[str, RobotResource] = {} + for resource_id, resource in values.items(): + _validate_identifier(resource_id, field_name="profile resource IDs") + if not isinstance(resource, RobotResource): + raise TypeError( + "RobotSkillProfile.resources values must be RobotResource instances." + ) + if resource_id != resource.resource_id: + raise ValueError( + f"Resource mapping key {resource_id!r} does not match " + f"RobotResource.resource_id {resource.resource_id!r}." + ) + normalized[resource_id] = resource.snapshot() + return MappingProxyType(normalized) + + +def _normalize_command_profiles( + values: Mapping[str, ControlPartCommandProfile], +) -> Mapping[str, ControlPartCommandProfile]: + """Own generic endpoint command-profile snapshots by stable profile ID.""" + if not isinstance(values, Mapping): + raise TypeError("command_profiles must be a mapping.") + normalized: dict[str, ControlPartCommandProfile] = {} + for profile_id, profile in values.items(): + _validate_identifier(profile_id, field_name="command profile IDs") + if not isinstance(profile, ControlPartCommandProfile): + raise TypeError( + "command_profiles values must be ControlPartCommandProfile instances." + ) + normalized[profile_id] = profile.snapshot() + return MappingProxyType(normalized) + + +def _normalize_defaults( + values: Mapping[str, ResourceBinding], +) -> Mapping[str, ResourceBinding]: + """Validate and freeze per-skill complete default bindings.""" + if not isinstance(values, Mapping): + raise TypeError("defaults must be a mapping.") + normalized: dict[str, ResourceBinding] = {} + for skill_id, binding in values.items(): + _validate_identifier(skill_id, field_name="default skill IDs") + if not isinstance(binding, ResourceBinding): + raise TypeError("defaults values must be ResourceBinding instances.") + normalized[skill_id] = binding + return MappingProxyType(normalized) + + +def _normalize_presets( + values: Mapping[str, SkillPolicyPreset], +) -> Mapping[str, SkillPolicyPreset]: + """Validate preset keys and own independent snapshots.""" + if not isinstance(values, Mapping): + raise TypeError("presets must be a mapping.") + normalized: dict[str, SkillPolicyPreset] = {} + for preset_id, preset in values.items(): + _validate_identifier(preset_id, field_name="preset IDs") + if not isinstance(preset, SkillPolicyPreset): + raise TypeError("presets values must be SkillPolicyPreset instances.") + if preset_id != preset.preset_id: + raise ValueError( + f"Preset mapping key {preset_id!r} does not match preset_id " + f"{preset.preset_id!r}." + ) + normalized[preset_id] = preset.snapshot() + return MappingProxyType(normalized) + + +def _normalize_named_mapping( + values: Mapping[str, str], + *, + field_name: str, +) -> Mapping[str, str]: + """Validate and freeze one identifier-to-identifier mapping.""" + if not isinstance(values, Mapping): + raise TypeError(f"{field_name} must be a mapping.") + normalized: dict[str, str] = {} + for key, value in values.items(): + _validate_identifier(key, field_name=f"{field_name} keys") + _validate_identifier(value, field_name=f"{field_name} values") + normalized[key] = value + return MappingProxyType(normalized) + + +def _normalize_endpoint_adapters( + values: Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None, +) -> Mapping[type[ResourceEndpoint], ResourceEndpointAdapter]: + """Install the built-in adapter plus exact-type endpoint extensions.""" + normalized: dict[type[ResourceEndpoint], ResourceEndpointAdapter] = { + ControlPartEndpoint: ControlPartEndpointAdapter() + } + if values is not None: + if not isinstance(values, Mapping): + raise TypeError("endpoint_adapters must be a mapping or None.") + for endpoint_type, adapter in values.items(): + if not isinstance(endpoint_type, type) or not issubclass( + endpoint_type, ResourceEndpoint + ): + raise TypeError( + "endpoint_adapters keys must be ResourceEndpoint subclasses." + ) + if endpoint_type is ControlPartEndpoint: + raise ValueError( + "The built-in ControlPartEndpoint adapter cannot be overridden; " + "declare a distinct ResourceEndpoint subtype for custom " + "controller semantics." + ) + if not isinstance(adapter, ResourceEndpointAdapter): + raise TypeError( + "endpoint_adapters values must be ResourceEndpointAdapter " + "instances." + ) + declared_endpoint_type = getattr(adapter, "endpoint_type", None) + if not isinstance(declared_endpoint_type, type) or not issubclass( + declared_endpoint_type, ResourceEndpoint + ): + raise TypeError( + f"Endpoint adapter {type(adapter).__name__} must declare a " + "ResourceEndpoint subclass as endpoint_type." + ) + if declared_endpoint_type is not endpoint_type: + raise ValueError( + f"Endpoint adapter {type(adapter).__name__} declares " + f"endpoint_type {declared_endpoint_type.__name__}, but is " + f"registered for {endpoint_type.__name__}." + ) + adapter_id = getattr(adapter, "adapter_id", None) + _validate_identifier( + adapter_id, + field_name="ResourceEndpointAdapter.adapter_id", + ) + normalized[endpoint_type] = adapter + adapter_ids = [ + getattr(adapter, "adapter_id", None) for adapter in normalized.values() + ] + if len(set(adapter_ids)) != len(adapter_ids): + raise ValueError("Installed ResourceEndpointAdapter IDs must be unique.") + return MappingProxyType(normalized) + + +@dataclass(frozen=True, slots=True) +class RobotSkillProfile: + """Reusable declarative skill integration for one robot embodiment.""" + + profile_id: str + resources: Mapping[str, RobotResource] + command_profiles: Mapping[str, ControlPartCommandProfile] = field( + default_factory=dict + ) + defaults: Mapping[str, ResourceBinding] = field(default_factory=dict) + presets: Mapping[str, SkillPolicyPreset] = field(default_factory=dict) + default_preset: str | None = None + skill_presets: Mapping[str, str] = field(default_factory=dict) + grounding_providers: Mapping[str, str] = field(default_factory=dict) + """Semantic call ID to embodiment-owned named grounding provider ID.""" + + def __post_init__(self) -> None: + _validate_identifier(self.profile_id, field_name="RobotSkillProfile.profile_id") + resources = _normalize_resources(self.resources) + object.__setattr__(self, "resources", resources) + object.__setattr__( + self, + "command_profiles", + _normalize_command_profiles(self.command_profiles), + ) + object.__setattr__(self, "defaults", _normalize_defaults(self.defaults)) + presets = _normalize_presets(self.presets) + object.__setattr__(self, "presets", presets) + if self.default_preset is not None: + _validate_identifier( + self.default_preset, + field_name="RobotSkillProfile.default_preset", + ) + if self.default_preset not in presets: + raise ValueError( + f"Unknown default preset {self.default_preset!r}; available " + f"presets are {sorted(presets)}." + ) + skill_presets = _normalize_named_mapping( + self.skill_presets, + field_name="skill_presets", + ) + unknown_presets = sorted(set(skill_presets.values()) - set(presets)) + if unknown_presets: + raise ValueError( + f"skill_presets references unknown presets {unknown_presets}." + ) + object.__setattr__(self, "skill_presets", skill_presets) + object.__setattr__( + self, + "grounding_providers", + _normalize_named_mapping( + self.grounding_providers, + field_name="grounding_providers", + ), + ) + self._validate_resource_graph(resources) + self.action_control_profiles() + + def action_control_profiles(self) -> Mapping[str, ControlPartCommandProfile]: + """Lower endpoint command profiles for the current action core. + + Returns: + Owned command profiles keyed by concrete robot control-part name. + + Raises: + ValueError: If two endpoint declarations assign non-equivalent + commands with the same semantic name to one control part. + """ + commands_by_control_part: dict[str, dict[str, ControlCommand]] = {} + for resource in self.resources.values(): + for endpoint in resource.endpoints.values(): + if type(endpoint) is not ControlPartEndpoint: + continue + profile_id = ( + endpoint.control_part + if endpoint.command_profile is None + else endpoint.command_profile + ) + profile = self.command_profiles.get(profile_id) + if profile is None: + continue + merged = commands_by_control_part.setdefault( + endpoint.control_part, + {}, + ) + for command_name, command in profile.commands.items(): + previous = merged.get(command_name) + if previous is not None and not previous.equivalent_to(command): + raise ValueError( + f"Control part {endpoint.control_part!r} receives " + f"non-equivalent {command_name!r} commands from profile " + f"{profile_id!r}." + ) + merged[command_name] = command + return MappingProxyType( + { + control_part: ControlPartCommandProfile(commands=commands) + for control_part, commands in commands_by_control_part.items() + } + ) + + @staticmethod + def _validate_resource_graph(resources: Mapping[str, RobotResource]) -> None: + """Reject unknown members and cycles in the resource DAG.""" + for resource in resources.values(): + unknown = sorted(set(resource.members) - set(resources)) + if unknown: + raise ValueError( + f"Robot resource {resource.resource_id!r} references unknown " + f"members {unknown}." + ) + + visiting: list[str] = [] + visited: set[str] = set() + + def visit(resource_id: str) -> None: + if resource_id in visited: + return + if resource_id in visiting: + cycle_start = visiting.index(resource_id) + cycle = visiting[cycle_start:] + [resource_id] + raise ValueError( + "Robot resource graph contains a cycle: " + " -> ".join(cycle) + ) + visiting.append(resource_id) + for member in resources[resource_id].members: + visit(member) + visiting.pop() + visited.add(resource_id) + + for resource_id in resources: + visit(resource_id) + + def bind( + self, + engine: AtomicActionEngine, + *, + endpoint_adapters: ( + Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None + ) = None, + ) -> BoundRobotSkillProfile: + """Validate this profile against one fully configured action engine. + + Args: + engine: Installed atomic-action engine for the target robot. + endpoint_adapters: Optional exact endpoint-type adapters. Explicit + entries extend the non-overridable built-in control-part adapter. + + Returns: + Robot-, engine-, and adapter-validated profile view. + """ + return BoundRobotSkillProfile( + self, + engine, + endpoint_adapters=endpoint_adapters, + ) + + +class BoundRobotSkillProfile: + """Robot- and engine-validated view of a :class:`RobotSkillProfile`.""" + + def __init__( + self, + profile: RobotSkillProfile, + engine: AtomicActionEngine, + *, + endpoint_adapters: ( + Mapping[type[ResourceEndpoint], ResourceEndpointAdapter] | None + ) = None, + ) -> None: + from embodichain.lab.sim.atomic_actions.engine import AtomicActionEngine + + if not isinstance(profile, RobotSkillProfile): + raise TypeError("profile must be a RobotSkillProfile.") + if not isinstance(engine, AtomicActionEngine): + raise TypeError("engine must be an AtomicActionEngine.") + self._profile = profile + self._engine = engine + self._endpoint_adapters = _normalize_endpoint_adapters(endpoint_adapters) + self._validate_presets() + self._resources = self._resolve_resources() + self._validate_engine_control_profiles() + self._validate_leaf_ownership() + self._skill_catalog_revision = engine.skill_catalog_revision + self._installed_skills = MappingProxyType(dict(engine.skills)) + self._validate_named_skill_configuration() + self._validate_defaults() + self._skills = MappingProxyType( + { + skill_id: descriptor + for skill_id, descriptor in self._installed_skills.items() + if self._assignments(descriptor.binding_contract, {}) + } + ) + + @property + def profile_id(self) -> str: + """Return the stable profile identifier.""" + return self._profile.profile_id + + @property + def engine(self) -> AtomicActionEngine: + """Return the exact action engine that owns this bound profile.""" + return self._engine + + @property + def source_profile(self) -> RobotSkillProfile: + """Return the immutable profile object used to create this binding.""" + return self._profile + + @property + def resources(self) -> Mapping[str, ResolvedRobotResource]: + """Return resolved generic robot resources keyed by logical ID.""" + return self._resources + + @property + def skills(self) -> Mapping[str, SkillDescriptor]: + """Return installed semantic skills fully supported by this profile.""" + self._assert_catalog_current() + return self._skills + + def preset( + self, + preset_id: str | None = None, + *, + skill_id: str | None = None, + ) -> SkillPolicyPreset: + """Resolve an explicit, per-skill, or profile-default policy preset.""" + selected = preset_id + if skill_id is not None: + descriptor = self._require_installed_skill(skill_id) + if skill_id not in self._skills: + raise UnsupportedSkillError( + self._unsupported_message( + skill_id, + descriptor.binding_contract, + {}, + ) + ) + if selected is None: + selected = self._profile.skill_presets.get(skill_id) + if selected is None: + selected = self._profile.default_preset + if selected is None: + raise KeyError( + "No policy preset was selected and no default is configured." + ) + try: + preset = self._profile.presets[selected] + except KeyError as exc: + raise KeyError( + f"Unknown policy preset {selected!r}; available presets are " + f"{sorted(self._profile.presets)}." + ) from exc + return preset.snapshot() + + def candidates( + self, + skill_id: str, + selections: Mapping[str, str] | None = None, + ) -> tuple[ResourceBinding, ...]: + """Return every valid complete resource assignment deterministically.""" + descriptor = self._require_installed_skill(skill_id) + normalized = self._normalize_selections(descriptor, selections) + return tuple( + ResourceBinding( + resources={ + slot_id: resource.resource_id + for slot_id, resource in assignment.items() + } + ) + for assignment in self._assignments( + descriptor.binding_contract, + normalized, + ) + ) + + def resolve( + self, + skill_id: str, + selections: Mapping[str, str] | None = None, + ) -> ResolvedSkillBinding: + """Resolve one skill with strict capability matching and disambiguation.""" + descriptor = self._require_installed_skill(skill_id) + normalized = self._normalize_selections(descriptor, selections) + contract = descriptor.binding_contract + assignments = self._assignments(contract, normalized) + if not assignments: + raise UnsupportedSkillError( + self._unsupported_message(skill_id, contract, normalized) + ) + if len(assignments) == 1: + assignment = assignments[0] + else: + default = self._profile.defaults.get(skill_id) + assignment = None + if default is not None: + selected_ids = dict(default.resources) + selected_ids.update(normalized) + for candidate in assignments: + if all( + candidate[slot_id].resource_id == resource_id + for slot_id, resource_id in selected_ids.items() + ): + assignment = candidate + break + if assignment is None: + rendered = [ + "{" + + ", ".join( + f"{slot}={resource.resource_id}" + for slot, resource in candidate.items() + ) + + "}" + for candidate in assignments + ] + raise AmbiguousSkillBindingError( + f"Skill {skill_id!r} has {len(assignments)} valid resource " + f"bindings: {rendered}. Configure a complete per-skill " + "default or provide enough explicit slot selections." + ) + return self._lower_binding(skill_id, contract, assignment) + + def _require_installed_skill(self, skill_id: str) -> SkillDescriptor: + """Return one installed explicit descriptor or fail at the right boundary.""" + self._assert_catalog_current() + _validate_identifier(skill_id, field_name="skill_id") + descriptor = self._installed_skills.get(skill_id) + if descriptor is None: + raise KeyError( + f"Skill {skill_id!r} is not an installed, agent-visible skill with " + "an explicit binding contract." + ) + return descriptor + + def _assert_catalog_current(self) -> None: + """Prevent stale contracts after engine registration or replacement.""" + if self._engine.skill_catalog_revision != self._skill_catalog_revision: + raise RuntimeError( + "AtomicActionEngine semantic skills changed after the robot skill " + "profile was bound; bind the profile again before discovery or " + "resolution." + ) + + def _normalize_selections( + self, + descriptor: SkillDescriptor, + selections: Mapping[str, str] | None, + ) -> Mapping[str, str]: + """Validate caller selections against one skill's local slots.""" + normalized = _normalize_named_mapping( + {} if selections is None else selections, + field_name="selections", + ) + contract = descriptor.binding_contract + assert contract is not None + unknown_slots = sorted(set(normalized) - set(contract.slot_ids)) + if unknown_slots: + raise ValueError( + f"Skill {descriptor.skill_id!r} selections contain unknown slots " + f"{unknown_slots}; expected a subset of {list(contract.slot_ids)}." + ) + unknown_resources = sorted(set(normalized.values()) - set(self._resources)) + if unknown_resources: + raise ValueError( + f"Selections reference unknown resources {unknown_resources}; " + f"available resources are {sorted(self._resources)}." + ) + return normalized + + def _validate_presets(self) -> None: + """Validate planner-pinned presets against the selected engine backend.""" + configured = self._engine.planning_services.planner_name + for preset in self._profile.presets.values(): + required = preset.required_planner + if required is not None and required != configured: + raise ProfileValidationError( + f"Preset {preset.preset_id!r} requires planner {required!r}, " + f"but this engine uses {configured!r}." + ) + + def _validate_engine_control_profiles(self) -> None: + """Require current-core endpoint commands to be installed on the engine.""" + engine_profiles = self._engine.control_profiles + try: + expected_control_profiles = self._profile.action_control_profiles() + except (TypeError, ValueError) as exc: + raise ProfileValidationError( + f"Could not lower profile commands to action control parts: {exc}" + ) from exc + for control_part, expected in expected_control_profiles.items(): + installed = engine_profiles.get(control_part) + if installed is None: + raise ProfileValidationError( + f"Profile command set for control part {control_part!r} is not " + "installed on the AtomicActionEngine." + ) + for command_name, command in expected.commands.items(): + installed_command = installed.commands.get(command_name) + if installed_command is None: + raise ProfileValidationError( + f"Engine control profile {control_part!r} is missing profile " + f"command {command_name!r}." + ) + if not command.equivalent_to(installed_command): + raise ProfileValidationError( + f"Engine command {control_part!r}.{command_name} is not " + "semantically equivalent to the profile-owned command." + ) + for resource in self._resources.values(): + for endpoint in resource.endpoints.values(): + if not endpoint.commands or not isinstance( + endpoint.runtime_target, + JointPositionTarget, + ): + continue + control_part = endpoint.runtime_target.control_part + installed = engine_profiles.get(control_part) + if installed is None: + raise ProfileValidationError( + f"Endpoint command profile " + f"{endpoint.command_profile_key!r} for control part " + f"{control_part!r} is not installed on the " + "AtomicActionEngine." + ) + for command_name, command in endpoint.commands.items(): + installed_command = installed.commands.get(command_name) + if installed_command is None: + raise ProfileValidationError( + f"Engine control profile {control_part!r} is missing " + f"profile command {command_name!r}." + ) + if not command.equivalent_to(installed_command): + raise ProfileValidationError( + f"Engine command {control_part!r}.{command_name} is " + "not semantically equivalent to the profile-owned " + "command." + ) + + def _resolve_resources(self) -> Mapping[str, ResolvedRobotResource]: + """Resolve adapter endpoints, graph closure, commands, and claims.""" + resolved_endpoints: dict[str, dict[str, ResolvedResourceEndpoint]] = {} + direct_joints: dict[str, set[int]] = {} + direct_tokens: dict[str, set[str]] = {} + for resource_id, resource in self._profile.resources.items(): + resource_endpoints: dict[str, ResolvedResourceEndpoint] = {} + for endpoint_id, endpoint in resource.endpoints.items(): + adapter = self._endpoint_adapters.get(type(endpoint)) + if adapter is None: + raise ProfileValidationError( + f"Resource {resource_id!r} endpoint {endpoint_id!r} uses " + f"unsupported endpoint type {type(endpoint).__name__}; " + "register a ResourceEndpointAdapter for that exact type." + ) + try: + resolution = adapter.resolve(endpoint, engine=self._engine) + except Exception as exc: + raise ProfileValidationError( + f"Endpoint adapter {adapter.adapter_id!r} failed for resource " + f"{resource_id!r} endpoint {endpoint_id!r}: {exc}" + ) from exc + if not isinstance(resolution, EndpointResolution): + raise ProfileValidationError( + f"Endpoint adapter {adapter.adapter_id!r} returned " + f"{type(resolution).__name__}, expected EndpointResolution." + ) + invalid_joint_ids = sorted( + joint_id + for joint_id in resolution.joint_ids + if joint_id >= self._engine.robot.dof + ) + if invalid_joint_ids: + raise ProfileValidationError( + f"Endpoint adapter {adapter.adapter_id!r} resolved resource " + f"{resource_id!r} endpoint {endpoint_id!r} to joint IDs " + f"{invalid_joint_ids} outside robot DOF " + f"{self._engine.robot.dof}." + ) + command_profile = ( + None + if resolution.command_profile_key is None + else self._profile.command_profiles.get( + resolution.command_profile_key + ) + ) + if resolution.requires_command_profile and command_profile is None: + raise ProfileValidationError( + f"Endpoint adapter {adapter.adapter_id!r} resolved resource " + f"{resource_id!r} endpoint {endpoint_id!r} to required " + f"command profile {resolution.command_profile_key!r}, but " + "the RobotSkillProfile does not define it." + ) + resource_endpoints[endpoint_id] = ResolvedResourceEndpoint( + endpoint=endpoint, + adapter_id=adapter.adapter_id, + runtime_target=resolution.runtime_target, + task_state_key=( + resource_id + if resolution.task_state_key is None + else resolution.task_state_key + ), + effect_sources=resolution.effect_sources, + tracking_channels=resolution.tracking_channels, + command_profile_key=resolution.command_profile_key, + requires_command_profile=resolution.requires_command_profile, + commands=( + {} if command_profile is None else command_profile.commands + ), + claim_tokens=resolution.claim_tokens, + joint_ids=resolution.joint_ids, + exclusive=resolution.exclusive, + ) + resolved_endpoints[resource_id] = resource_endpoints + direct_joints[resource_id] = { + joint_id + for endpoint in resource_endpoints.values() + for joint_id in endpoint.joint_ids + } + direct_tokens[resource_id] = { + token + for endpoint in resource_endpoints.values() + for token in endpoint.claim_tokens + } + + leaf_cache: dict[str, frozenset[str]] = {} + joint_cache: dict[str, frozenset[int]] = {} + token_cache: dict[str, frozenset[str]] = {} + + def resolve_claim( + resource_id: str, + ) -> tuple[frozenset[str], frozenset[int], frozenset[str]]: + cached_leaves = leaf_cache.get(resource_id) + if cached_leaves is not None: + return ( + cached_leaves, + joint_cache[resource_id], + token_cache[resource_id], + ) + resource = self._profile.resources[resource_id] + if not resource.members: + leaves = frozenset({resource_id}) + joints = frozenset(direct_joints[resource_id]) + tokens = frozenset(direct_tokens[resource_id]) + else: + leaves_set: set[str] = set() + member_joints: set[int] = set() + member_tokens: set[str] = set() + for member in resource.members: + member_leaves, nested_joints, nested_tokens = resolve_claim(member) + leaves_set.update(member_leaves) + member_joints.update(nested_joints) + member_tokens.update(nested_tokens) + uncovered = direct_joints[resource_id] - member_joints + if uncovered: + raise ProfileValidationError( + f"Composite resource {resource_id!r} endpoints control joints " + f"{sorted(uncovered)} not claimed by its members." + ) + leaves = frozenset(leaves_set) + joints = frozenset(member_joints | direct_joints[resource_id]) + tokens = frozenset(member_tokens | direct_tokens[resource_id]) + leaf_cache[resource_id] = leaves + joint_cache[resource_id] = joints + token_cache[resource_id] = tokens + return leaves, joints, tokens + + resolved: dict[str, ResolvedRobotResource] = {} + for resource_id, resource in self._profile.resources.items(): + leaves, joints, tokens = resolve_claim(resource_id) + resolved[resource_id] = ResolvedRobotResource( + resource_id=resource_id, + endpoints=resolved_endpoints[resource_id], + members=resource.members, + claim=ResourceClaim( + leaves, + tuple(sorted(joints)), + tokens, + ), + ) + self._validate_command_shapes(resolved_endpoints) + return MappingProxyType(resolved) + + def _validate_command_shapes( + self, + endpoints_by_resource: Mapping[str, Mapping[str, ResolvedResourceEndpoint]], + ) -> None: + """Validate profile joint commands against every referenced endpoint DOF.""" + checked: set[tuple[str, int]] = set() + for endpoints in endpoints_by_resource.values(): + for endpoint in endpoints.values(): + if not endpoint.commands: + continue + dof = len(endpoint.joint_ids) + profile_label = endpoint.command_profile_key or endpoint.adapter_id + key = (profile_label, dof) + if key in checked: + continue + checked.add(key) + for command_name, command in endpoint.commands.items(): + if not isinstance(command, JointPositionCommand): + continue + positions = command.positions + if positions.dim() != 1: + raise ProfileValidationError( + f"Profile command {profile_label!r}." + f"{command_name} must be one-dimensional and " + "broadcastable across environments; use invocation " + "overrides for per-environment commands." + ) + if positions.shape[-1] != dof: + raise ProfileValidationError( + f"Command {profile_label!r}.{command_name} has " + f"{positions.shape[-1]} joints, expected {dof}." + ) + + def _validate_leaf_ownership(self) -> None: + """Require physical leaves to own disjoint claims and runtime targets.""" + leaves = [ + resource for resource in self._resources.values() if not resource.members + ] + for index, left in enumerate(leaves): + for right in leaves[index + 1 :]: + overlapping_joints = sorted( + set(left.claim.joint_ids) & set(right.claim.joint_ids) + ) + overlapping_tokens = sorted( + left.claim.claim_tokens & right.claim.claim_tokens + ) + if overlapping_joints or overlapping_tokens: + raise ProfileValidationError( + f"Leaf resources {left.resource_id!r} and " + f"{right.resource_id!r} overlap on robot joints " + f"{overlapping_joints} or adapter claims " + f"{overlapping_tokens}. " + "Model one physical leaf and reference it from composites." + ) + left_targets = { + ( + endpoint.runtime_target.transport_id, + endpoint.runtime_target.target_id, + ) + for endpoint in left.endpoints.values() + } + right_targets = { + ( + endpoint.runtime_target.transport_id, + endpoint.runtime_target.target_id, + ) + for endpoint in right.endpoints.values() + } + overlapping_targets = sorted(left_targets & right_targets) + if overlapping_targets: + raise ProfileValidationError( + f"Leaf resources {left.resource_id!r} and " + f"{right.resource_id!r} share runtime targets " + f"{overlapping_targets}. Model one physical leaf and " + "reference it from composites." + ) + + def _validate_named_skill_configuration(self) -> None: + """Reject defaults and preset selections for absent semantic skills.""" + configured_skill_ids = set(self._profile.defaults) | set( + self._profile.skill_presets + ) + unknown = sorted(configured_skill_ids - set(self._installed_skills)) + if unknown: + raise ProfileValidationError( + f"Profile references skills not installed with explicit contracts: " + f"{unknown}." + ) + + def _validate_defaults(self) -> None: + """Require every configured default to be complete and currently valid.""" + for skill_id, default in self._profile.defaults.items(): + descriptor = self._installed_skills[skill_id] + contract = descriptor.binding_contract + assert contract is not None + expected = set(contract.slot_ids) + actual = set(default.resources) + if actual != expected: + raise ProfileValidationError( + f"Default binding for skill {skill_id!r} must cover exactly " + f"{sorted(expected)}; missing={sorted(expected - actual)}, " + f"extra={sorted(actual - expected)}." + ) + unknown_resources = sorted( + set(default.resources.values()) - set(self._resources) + ) + if unknown_resources: + raise ProfileValidationError( + f"Default binding for skill {skill_id!r} references unknown " + f"resources {unknown_resources}." + ) + assignments = self._assignments(contract, default.resources) + if len(assignments) != 1: + raise ProfileValidationError( + f"Default binding for skill {skill_id!r} does not satisfy its " + "capabilities, commands, endpoints, and resource constraints." + ) + + def _assignments( + self, + contract: SkillBindingContract | None, + selections: Mapping[str, str], + ) -> tuple[dict[str, ResolvedRobotResource], ...]: + """Enumerate valid complete assignments in declaration order.""" + if contract is None: + return () + if not contract.slots: + return ({},) + slot_candidates: list[tuple[ResolvedRobotResource, ...]] = [] + for slot in contract.slots: + selected = selections.get(slot.slot_id) + candidates = tuple( + resource + for resource in self._resources.values() + if (selected is None or resource.resource_id == selected) + and not self._rejection_reasons(resource, slot) + ) + if not candidates: + return () + slot_candidates.append(candidates) + assignments: list[dict[str, ResolvedRobotResource]] = [] + for combination in product(*slot_candidates): + assignment = { + slot.slot_id: resource + for slot, resource in zip(contract.slots, combination, strict=True) + } + if self._constraints_match(contract, assignment): + assignments.append(assignment) + return tuple(assignments) + + @staticmethod + def _constraints_match( + contract: SkillBindingContract, + assignment: Mapping[str, ResolvedRobotResource], + ) -> bool: + """Apply declared graph/claim constraints to one assignment.""" + for constraint in contract.constraints: + if isinstance(constraint, DisjointResourceSlots): + resources = [assignment[slot] for slot in constraint.slots] + for index, left in enumerate(resources): + if any( + left.claim.conflicts_with(right.claim) + for right in resources[index + 1 :] + ): + return False + return True + + def _unsupported_message( + self, + skill_id: str, + contract: SkillBindingContract | None, + selections: Mapping[str, str], + ) -> str: + """Render deterministic per-slot rejection reasons.""" + if contract is None: + return f"Skill {skill_id!r} has no explicit binding contract." + lines = [f"Skill {skill_id!r} has no compatible resource binding."] + every_slot_has_candidate = True + for slot in contract.slots: + selected = selections.get(slot.slot_id) + lines.append(f"slot {slot.slot_id!r}:") + slot_has_candidate = False + for resource in self._resources.values(): + if selected is not None and resource.resource_id != selected: + continue + reasons = self._rejection_reasons(resource, slot) + status = "compatible" if not reasons else "; ".join(reasons) + slot_has_candidate |= not reasons + lines.append(f" {resource.resource_id}: {status}") + every_slot_has_candidate &= slot_has_candidate + if contract.constraints and every_slot_has_candidate: + lines.append( + "All individually compatible combinations violate constraints." + ) + return "\n".join(lines) + + def _rejection_reasons( + self, + resource: ResolvedRobotResource, + slot: SkillResourceSlot, + ) -> tuple[str, ...]: + """Explain why one resource fails one slot requirement.""" + reasons: list[str] = [] + matched_endpoints: dict[str, ResolvedResourceEndpoint] = {} + for requirement in slot.endpoints: + endpoint = resource.endpoints.get(requirement.endpoint_id) + if endpoint is None: + reasons.append(f"missing endpoint {requirement.endpoint_id!r}") + continue + missing_capabilities = sorted( + requirement.capabilities - endpoint.capabilities + ) + if missing_capabilities: + reasons.append( + f"endpoint {requirement.endpoint_id!r} missing capabilities " + f"{missing_capabilities}" + ) + for command_name, command_type in requirement.required_commands.items(): + command = endpoint.commands.get(command_name) + if command is None: + reasons.append( + f"endpoint {requirement.endpoint_id!r} missing command " + f"{command_name!r}" + ) + elif not isinstance(command, command_type): + reasons.append( + f"command {command_name!r} is {type(command).__name__}, " + f"expected {command_type.__name__}" + ) + matched_endpoints[requirement.endpoint_id] = endpoint + for constraint in slot.constraints: + if not isinstance(constraint, DisjointSlotEndpoints): + continue + endpoint_ids = constraint.endpoint_ids + for index, left_id in enumerate(endpoint_ids): + left = matched_endpoints.get(left_id) + if left is None: + continue + for right_id in endpoint_ids[index + 1 :]: + right = matched_endpoints.get(right_id) + if right is None or not left.conflicts_with(right): + continue + overlapping_joints = sorted( + set(left.joint_ids) & set(right.joint_ids) + ) + overlapping_tokens = sorted(left.claim_tokens & right.claim_tokens) + shared_target = ( + ( + left.runtime_target.transport_id, + left.runtime_target.target_id, + ) + if ( + left.runtime_target.transport_id, + left.runtime_target.target_id, + ) + == ( + right.runtime_target.transport_id, + right.runtime_target.target_id, + ) + else None + ) + reasons.append( + f"endpoints {left_id!r} and {right_id!r} overlap on joints " + f"{overlapping_joints} or adapter claims " + f"{overlapping_tokens} or share runtime target " + f"{shared_target}" + ) + return tuple(reasons) + + def _lower_binding( + self, + skill_id: str, + contract: SkillBindingContract | None, + assignment: Mapping[str, ResolvedRobotResource], + ) -> ResolvedSkillBinding: + """Lower every required endpoint to one engine-owned action binding.""" + assert contract is not None + endpoints: list[EndpointBinding] = [] + for slot in contract.slots: + resource = assignment[slot.slot_id] + for requirement in slot.endpoints: + endpoint = resource.endpoints[requirement.endpoint_id] + endpoints.append( + EndpointBinding( + slot_id=slot.slot_id, + endpoint_id=requirement.endpoint_id, + resource_id=resource.resource_id, + adapter_id=endpoint.adapter_id, + target=endpoint.runtime_target, + task_state_key=endpoint.task_state_key, + tracking_channels=endpoint.tracking_channels, + capabilities=endpoint.capabilities, + commands=endpoint.commands, + claim_tokens=endpoint.claim_tokens, + joint_ids=endpoint.joint_ids, + ) + ) + return ResolvedSkillBinding( + skill_id=skill_id, + resources=assignment, + action_binding=ActionBinding( + owner_id=self._engine.binding_owner_id, + endpoints=tuple(endpoints), + ), + claim=ResourceClaim.combine( + tuple(resource.claim for resource in assignment.values()) + ), + ) + + +__all__ = [ + "AmbiguousSkillBindingError", + "BoundRobotSkillProfile", + "ControlPartEndpoint", + "ControlPartEndpointAdapter", + "EndpointResolution", + "ProfileValidationError", + "ResourceEndpoint", + "ResourceEndpointAdapter", + "ResolvedRobotResource", + "ResolvedResourceEndpoint", + "ResolvedSkillBinding", + "ResourceBinding", + "ResourceClaim", + "RobotResource", + "RobotSkillProfile", + "SkillPolicyPreset", + "UnsupportedSkillError", + "WorkflowRecoveryPolicy", +] diff --git a/embodichain/lab/sim/skills/runtime.py b/embodichain/lab/sim/skills/runtime.py new file mode 100644 index 000000000..aa529e874 --- /dev/null +++ b/embodichain/lab/sim/skills/runtime.py @@ -0,0 +1,3765 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Canonical execution service and convenience facade for semantic skills.""" + +from __future__ import annotations + +from collections import deque +from collections.abc import Iterable, Mapping +from dataclasses import dataclass, fields, is_dataclass, replace +from enum import Enum +import math +from types import MappingProxyType +from typing import Protocol, runtime_checkable + +import torch + +from ..atomic_actions.bindings import EndpointBinding +from ..atomic_actions.engine import AtomicActionEngine +from ..atomic_actions.effects import StateDelta +from ..atomic_actions.execution import ( + EffectExpectationResult, + EffectVerificationRequest, + EffectVerificationResult, + ExecutionEvent, + ExecutionEventKind, + ExecutionPlanAttempt, + HeldObjectGuardRequest, + HeldObjectGuardResult, + PhaseEffectGateRequest, + PhaseEffectGateResult, +) +from ..atomic_actions.plans import TrajectorySegment +from ..atomic_actions.policies import MotionPolicy, RecoveryPolicy +from ..atomic_actions.runner import ( + CommandSink, + ExecutionClock, + ExecutionRunner, + ExecutionRunnerCfg, + MonotonicExecutionClock, + ObservationProvider, + RunnerStatus, + RunnerStep, +) +from ..atomic_actions.state import HeldObjectState, PlanningContext, TaskState +from ..atomic_actions.tracking import ( + FeedbackTerminalAcceptance, + TimedTrackingSequence, + TrackingMetricCfg, + TrackingPolicy, +) +from .calls import HandOver, Pick, Place, SemanticCallSpec +from .compiler import ( + GroundedHeldObjectGuard, + GroundedPhaseEffectGate, + HeldObjectGuardBaseline, + SemanticSkillCompiler, +) +from .effects import ( + BinaryEffectEvidenceBatch, + EffectEvidenceBatch, + EffectExpectationDecision, + EffectMonitor, + EffectMonitorDecision, + EffectMonitorRef, + HeldObjectRelation, + HeldObjectStateExpectation, + JointStateEvidenceBatch, + PoseRelationEvidenceBatch, + ScalarEffectEvidenceBatch, + SemanticEffectSpec, +) +from .scene import ( + SceneAffordanceRef, + SceneArticulationRef, + SceneEntityRef, + SceneLinkRef, + SceneObjectRef, + SceneRegistry, +) +from .profiles import WorkflowRecoveryPolicy + + +def _snapshot_task_state(state: TaskState) -> TaskState: + """Return a tensor-owning snapshot of verified symbolic state.""" + return TaskState( + batch_size=state.batch_size, + device=state.device, + held_objects=state.held_objects, + coordinated_held_objects=state.coordinated_held_objects, + articulation_joints=state.articulation_joints, + ) + + +def _snapshot_event(event: ExecutionEvent) -> ExecutionEvent: + """Return an independently owned execution event.""" + return ExecutionEvent( + kind=event.kind, + timestamp=event.timestamp, + skill_id=event.skill_id, + invocation_id=event.invocation_id, + invocation_revision=event.invocation_revision, + invocation_index=event.invocation_index, + env_mask=event.env_mask, + message=event.message, + ) + + +def _metadata_value(value: object, *, depth: int = 0) -> object: + """Convert supported runtime diagnostics to deterministic JSON-safe data.""" + if depth > 16: + return {"type": f"{type(value).__module__}.{type(value).__qualname__}"} + if value is None or type(value) in (bool, int, str): + return value + if type(value) is float: + return value if math.isfinite(value) else None + if isinstance(value, Enum): + return value.value + if isinstance(value, torch.Tensor): + return _metadata_value(value.detach().cpu().tolist(), depth=depth + 1) + if isinstance(value, torch.device): + return str(value) + if isinstance(value, type): + return {"__type__": f"{value.__module__}.{value.__qualname__}"} + if isinstance(value, Mapping): + items = sorted(value.items(), key=lambda item: str(item[0])) + if all(type(key) is str and key and key == key.strip() for key, _ in items): + return { + key: _metadata_value(nested, depth=depth + 1) for key, nested in items + } + return { + "__entries__": [ + { + "key": _metadata_value(key, depth=depth + 1), + "value": _metadata_value(nested, depth=depth + 1), + } + for key, nested in items + ] + } + if isinstance(value, (tuple, list)): + return [_metadata_value(nested, depth=depth + 1) for nested in value] + if isinstance(value, (set, frozenset)): + return [ + _metadata_value(nested, depth=depth + 1) + for nested in sorted(value, key=str) + ] + return {"type": f"{type(value).__module__}.{type(value).__qualname__}"} + + +def _freeze_metadata_value(value: object) -> object: + """Recursively freeze already JSON-safe metadata for immutable traces.""" + if isinstance(value, dict): + return MappingProxyType( + {key: _freeze_metadata_value(nested) for key, nested in value.items()} + ) + if isinstance(value, list): + return tuple(_freeze_metadata_value(nested) for nested in value) + return value + + +def _snapshot_metadata_mapping(value: Mapping[str, object]) -> Mapping[str, object]: + """Own one JSON-safe string-keyed metadata mapping.""" + if not isinstance(value, Mapping): + raise TypeError("metadata must be a mapping.") + normalized = _metadata_value(value) + if not isinstance(normalized, dict): + raise TypeError("metadata normalization must produce a dict.") + return MappingProxyType(normalized) + + +def _event_to_metadata(event: ExecutionEvent) -> dict[str, object]: + """Serialize one execution/recovery event without exposing tensors.""" + return { + "kind": event.kind.value, + "timestamp": _metadata_value(event.timestamp), + "skill_id": event.skill_id, + "invocation_id": event.invocation_id, + "invocation_revision": event.invocation_revision, + "invocation_index": event.invocation_index, + "env_mask": _metadata_value(event.env_mask), + "message": event.message, + } + + +def task_state_to_metadata(state: TaskState) -> dict[str, object]: + """Return verified symbolic task state as deterministic JSON-safe data.""" + if not isinstance(state, TaskState): + raise TypeError("state must be a TaskState.") + held = [] + for resource_id, value in sorted(state.held_objects.items()): + held.append( + { + "resource_id": resource_id, + "object_id": value.semantics.entity_id, + "object_label": value.semantics.label, + "object_to_eef": _metadata_value(value.object_to_eef), + "grasp_xpos": _metadata_value(value.grasp_xpos), + "active_mask": _metadata_value(value.env_mask), + } + ) + coordinated = [] + for resource_ids, value in sorted(state.coordinated_held_objects.items()): + coordinated.append( + { + "resource_ids": list(resource_ids), + "object_id": value.semantics.entity_id, + "object_label": value.semantics.label, + "left_object_to_eef": _metadata_value(value.left_object_to_eef), + "right_object_to_eef": _metadata_value(value.right_object_to_eef), + "left_grasp_xpos": _metadata_value(value.left_grasp_xpos), + "right_grasp_xpos": _metadata_value(value.right_grasp_xpos), + "active_mask": _metadata_value(value.env_mask), + } + ) + articulations = [] + for (articulation_id, joint_id), value in sorted(state.articulation_joints.items()): + articulations.append( + { + "articulation_id": articulation_id, + "joint_id": joint_id, + "position": _metadata_value(value.position), + "active_mask": _metadata_value(value.env_mask), + } + ) + return { + "batch_size": state.batch_size, + "device": str(state.device), + "held_objects": held, + "coordinated_held_objects": coordinated, + "articulation_joints": articulations, + } + + +class SkillStatus(str, Enum): + """Lifecycle state of one semantic workflow run.""" + + IDLE = "idle" + RUNNING = "running" + COMPLETED = "completed" + FAILED = "failed" + CANCELLED = "cancelled" + + +class SkillWorkflowRecoveryRole(str, Enum): + """Role of one real semantic call inside workflow recovery.""" + + RETRY_RETAINED = "retry_retained" + REACQUIRE = "reacquire" + RETRY_REACQUIRED = "retry_reacquired" + + +@dataclass(frozen=True, slots=True) +class SkillEndpointTrackingChannelTrace: + """Stable provider and projector route for one endpoint feedback channel.""" + + channel_id: str + provider_id: str + provider_revision: str + projector_id: str + projector_revision: str + feedback_address_type: str + address_fingerprint: object + route_fingerprint: object + + def __post_init__(self) -> None: + for name in ( + "channel_id", + "provider_id", + "provider_revision", + "projector_id", + "projector_revision", + "feedback_address_type", + ): + if type(getattr(self, name)) is not str or not getattr(self, name): + raise ValueError(f"{name} must be a non-empty string.") + object.__setattr__( + self, + "address_fingerprint", + _freeze_metadata_value(_metadata_value(self.address_fingerprint)), + ) + object.__setattr__( + self, + "route_fingerprint", + _freeze_metadata_value(_metadata_value(self.route_fingerprint)), + ) + + def to_metadata(self) -> dict[str, object]: + """Return the exact immutable tracking route without live objects.""" + return { + "channel_id": self.channel_id, + "feedback_source": { + "provider_id": self.provider_id, + "revision": self.provider_revision, + "address_type": self.feedback_address_type, + "address_fingerprint": _metadata_value(self.address_fingerprint), + }, + "projector": { + "projector_id": self.projector_id, + "revision": self.projector_revision, + }, + "route_fingerprint": _metadata_value(self.route_fingerprint), + } + + +@dataclass(frozen=True, slots=True) +class SkillEndpointBindingTrace: + """JSON-safe typed projection of one resolved execution endpoint.""" + + slot_id: str + endpoint_id: str + resource_id: str + adapter_id: str + transport_id: str + target_id: str + target_type: str + task_state_key: str + capabilities: tuple[str, ...] + command_ids: tuple[str, ...] + tracking_channels: tuple[SkillEndpointTrackingChannelTrace, ...] + claim_tokens: tuple[str, ...] + joint_ids: tuple[int, ...] + + def __post_init__(self) -> None: + for name in ( + "slot_id", + "endpoint_id", + "resource_id", + "adapter_id", + "transport_id", + "target_id", + "target_type", + "task_state_key", + ): + value = getattr(self, name) + if type(value) is not str or not value: + raise ValueError(f"{name} must be a non-empty string.") + for name in ("capabilities", "command_ids", "claim_tokens"): + values = tuple(getattr(self, name)) + if tuple(sorted(set(values))) != values or not all( + type(value) is str and value for value in values + ): + raise ValueError(f"{name} must contain sorted unique identifiers.") + object.__setattr__(self, name, values) + tracking_channels = tuple(self.tracking_channels) + if not all( + type(value) is SkillEndpointTrackingChannelTrace + for value in tracking_channels + ): + raise TypeError( + "tracking_channels must contain exact " + "SkillEndpointTrackingChannelTrace values." + ) + channel_ids = tuple(value.channel_id for value in tracking_channels) + if tuple(sorted(set(channel_ids))) != channel_ids: + raise ValueError( + "tracking_channels must use sorted unique channel identifiers." + ) + object.__setattr__(self, "tracking_channels", tracking_channels) + joint_ids = tuple(self.joint_ids) + if len(set(joint_ids)) != len(joint_ids) or not all( + type(value) is int and value >= 0 for value in joint_ids + ): + raise ValueError("joint_ids must contain unique non-negative integers.") + object.__setattr__(self, "joint_ids", joint_ids) + + @classmethod + def from_binding(cls, binding: EndpointBinding) -> SkillEndpointBindingTrace: + """Project one owned endpoint binding without retaining its target.""" + if not isinstance(binding, EndpointBinding): + raise TypeError("binding must be an EndpointBinding.") + target = binding.target + return cls( + slot_id=binding.slot_id, + endpoint_id=binding.endpoint_id, + resource_id=binding.resource_id, + adapter_id=binding.adapter_id, + transport_id=target.transport_id, + target_id=target.target_id, + target_type=f"{type(target).__module__}.{type(target).__qualname__}", + task_state_key=binding.task_state_key, + capabilities=tuple(sorted(binding.capabilities)), + command_ids=tuple(sorted(binding.commands)), + tracking_channels=tuple( + SkillEndpointTrackingChannelTrace( + channel_id=channel_id, + provider_id=channel.source.provider_id, + provider_revision=channel.source.revision, + projector_id=channel.projector.projector_id, + projector_revision=channel.projector.revision, + feedback_address_type=( + f"{type(channel.source.address).__module__}." + f"{type(channel.source.address).__qualname__}" + ), + address_fingerprint=(channel.source.address.address_fingerprint), + route_fingerprint=channel.route_fingerprint, + ) + for channel_id, channel in sorted(binding.tracking_channels.items()) + ), + claim_tokens=tuple(sorted(binding.claim_tokens)), + joint_ids=binding.joint_ids, + ) + + def to_metadata(self) -> dict[str, object]: + """Return stable endpoint, resource, adapter, and transport metadata.""" + return { + "slot_id": self.slot_id, + "endpoint_id": self.endpoint_id, + "resource_id": self.resource_id, + "adapter_id": self.adapter_id, + "transport_id": self.transport_id, + "target_id": self.target_id, + "target_type": self.target_type, + "task_state_key": self.task_state_key, + "capabilities": list(self.capabilities), + "command_ids": list(self.command_ids), + "tracking_channels": [ + channel.to_metadata() for channel in self.tracking_channels + ], + "claim_tokens": list(self.claim_tokens), + "joint_ids": list(self.joint_ids), + } + + +def _motion_policy_to_metadata(policy: MotionPolicy) -> dict[str, object]: + """Serialize one owned core motion policy without retaining planner objects.""" + plan_options = policy.plan_opts + options_metadata: object = None + if plan_options is not None: + values = ( + plan_options.to_dict() + if callable(getattr(plan_options, "to_dict", None)) + else None + ) + options_metadata = { + "type": f"{type(plan_options).__module__}.{type(plan_options).__qualname__}", + "values": _metadata_value(values), + } + return { + "strategy": policy.strategy, + "sample_count": policy.sample_count, + "dynamic_collision_mode": policy.dynamic_collision_mode.value, + "plan_options": options_metadata, + } + + +def _recovery_policy_to_metadata(policy: RecoveryPolicy) -> dict[str, object]: + """Serialize all bounded-recovery settings.""" + return { + "max_replans": policy.max_replans, + "max_action_retries": policy.max_action_retries, + "goal_translation_threshold": _metadata_value( + policy.goal_translation_threshold + ), + "goal_rotation_threshold": _metadata_value(policy.goal_rotation_threshold), + "action_timeout": _metadata_value(policy.action_timeout), + } + + +def _tracking_metric_to_metadata(metric: TrackingMetricCfg) -> dict[str, object]: + """Serialize one exact typed metric and its unit-preserving tolerances.""" + parameters = ( + { + value.name: _metadata_value(getattr(metric, value.name)) + for value in fields(metric) + } + if is_dataclass(metric) + else {} + ) + return { + "metric_id": metric.metric_id, + "revision": metric.revision, + "channel_id": metric.channel_id, + "type": f"{type(metric).__module__}.{type(metric).__qualname__}", + "parameters": parameters, + } + + +def _tracking_policy_to_metadata(policy: TrackingPolicy) -> dict[str, object]: + """Serialize independent in-flight and terminal tracking contracts.""" + in_flight = policy.in_flight + terminal = policy.terminal + return { + "in_flight": ( + None + if in_flight is None + else { + "metrics": [ + _tracking_metric_to_metadata(metric) for metric in in_flight.metrics + ], + "consecutive_violations": in_flight.consecutive_violations, + "grace_period": _metadata_value(in_flight.grace_period), + } + ), + "terminal": ( + { + "mode": "feedback", + "metrics": [ + _tracking_metric_to_metadata(metric) for metric in terminal.metrics + ], + "settle_timeout": _metadata_value(terminal.settle_timeout), + "consecutive_acceptances": terminal.consecutive_acceptances, + } + if isinstance(terminal, FeedbackTerminalAcceptance) + else { + "mode": "timed", + "settle_duration": _metadata_value(terminal.settle_duration), + } + ), + } + + +def _tracking_sequence_to_metadata( + sequence: TimedTrackingSequence | None, +) -> dict[str, object] | None: + """Serialize the provider/projector shape of one plan-owned contract.""" + if sequence is None: + return None + first_frame = None if not sequence.frames else sequence.frames[0] + return { + "env_ids": _metadata_value(sequence.env_ids), + "frame_count": sequence.frame_count, + "setpoints": [ + { + "endpoint": list(setpoint.endpoint_key), + "channel_id": setpoint.binding.channel_id, + "state_type": ( + f"{type(setpoint.desired).__module__}." + f"{type(setpoint.desired).__qualname__}" + ), + "feedback_source": { + "provider_id": setpoint.binding.source.provider_id, + "revision": setpoint.binding.source.revision, + "address_fingerprint": _metadata_value( + setpoint.binding.source.address.address_fingerprint + ), + }, + "projector": { + "projector_id": setpoint.binding.projector.projector_id, + "revision": setpoint.binding.projector.revision, + }, + "route_fingerprint": _metadata_value( + setpoint.binding.route_fingerprint + ), + } + for setpoint in (() if first_frame is None else first_frame.setpoints) + ], + } + + +@dataclass(frozen=True, slots=True) +class ResolvedCorePolicyTrace: + """Resolved preset, core policies, and execution binding for one plan.""" + + profile_id: str + preset_id: str + preset_schema_version: int + motion_policy: MotionPolicy + tracking_policy: TrackingPolicy + recovery_policy: RecoveryPolicy + endpoints: tuple[SkillEndpointBindingTrace, ...] + + def __post_init__(self) -> None: + for name in ("profile_id", "preset_id"): + value = getattr(self, name) + if type(value) is not str or not value: + raise ValueError(f"{name} must be a non-empty string.") + if ( + type(self.preset_schema_version) is not int + or self.preset_schema_version < 1 + ): + raise ValueError("preset_schema_version must be a positive integer.") + if not isinstance(self.motion_policy, MotionPolicy): + raise TypeError("motion_policy must be a MotionPolicy.") + if not isinstance(self.tracking_policy, TrackingPolicy): + raise TypeError("tracking_policy must be a TrackingPolicy.") + if not isinstance(self.recovery_policy, RecoveryPolicy): + raise TypeError("recovery_policy must be a RecoveryPolicy.") + endpoints = tuple(self.endpoints) + if not all(type(value) is SkillEndpointBindingTrace for value in endpoints): + raise TypeError( + "endpoints must contain exact SkillEndpointBindingTrace values." + ) + keys = tuple((value.slot_id, value.endpoint_id) for value in endpoints) + if len(set(keys)) != len(keys): + raise ValueError("endpoints must use unique slot/endpoint keys.") + object.__setattr__(self, "motion_policy", replace(self.motion_policy)) + object.__setattr__(self, "tracking_policy", self.tracking_policy.snapshot()) + object.__setattr__(self, "recovery_policy", replace(self.recovery_policy)) + object.__setattr__(self, "endpoints", endpoints) + + @classmethod + def from_resolved_binding( + cls, + *, + profile_id: str, + preset_id: str, + preset_schema_version: int, + motion_policy: MotionPolicy, + tracking_policy: TrackingPolicy, + recovery_policy: RecoveryPolicy, + endpoints: Iterable[EndpointBinding], + ) -> ResolvedCorePolicyTrace: + """Project one resolved preset and action binding to a trace.""" + return cls( + profile_id=profile_id, + preset_id=preset_id, + preset_schema_version=preset_schema_version, + motion_policy=motion_policy, + tracking_policy=tracking_policy, + recovery_policy=recovery_policy, + endpoints=tuple( + SkillEndpointBindingTrace.from_binding(endpoint) + for endpoint in endpoints + ), + ) + + def snapshot(self) -> ResolvedCorePolicyTrace: + """Return an independently owned core-policy and binding trace.""" + return ResolvedCorePolicyTrace( + profile_id=self.profile_id, + preset_id=self.preset_id, + preset_schema_version=self.preset_schema_version, + motion_policy=self.motion_policy, + tracking_policy=self.tracking_policy, + recovery_policy=self.recovery_policy, + endpoints=self.endpoints, + ) + + def to_metadata(self) -> dict[str, object]: + """Return deterministic policy and endpoint-binding metadata.""" + return { + "profile_id": self.profile_id, + "preset": { + "preset_id": self.preset_id, + "schema_version": self.preset_schema_version, + }, + "motion_policy": _motion_policy_to_metadata(self.motion_policy), + "tracking_policy": _tracking_policy_to_metadata(self.tracking_policy), + "recovery_policy": _recovery_policy_to_metadata(self.recovery_policy), + "endpoints": [endpoint.to_metadata() for endpoint in self.endpoints], + } + + +@dataclass(frozen=True, slots=True, eq=False) +class SkillPlanAttemptTrace: + """Compact, typed trace of one installed action-plan generation. + + ``scene_dependency_monitor_until`` preserves the plan's per-entity exclusive + waypoint cutoff: an entity is monitored only while the current waypoint index + is smaller than its configured value. + """ + + attempt_generation: int + trigger: str + planned_at: float + invocation_index: int + planned_mask: torch.Tensor + action_retry_counts: tuple[int, ...] + replan_counts: tuple[int, ...] + skill_id: str + invocation_id: str | None + invocation_revision: int + plan_success_mask: torch.Tensor + command_frame_count: int + trajectory_segments: tuple[TrajectorySegment, ...] + planned_scene_version: int + planned_collision_world_revision: tuple[int, ...] + scene_dependencies: tuple[str, ...] + scene_dependency_monitor_until: Mapping[str, int] + collision_world_sensitive: bool + replannable: bool + tracking_policy: TrackingPolicy + tracking: TimedTrackingSequence | None + effect_verification_kind: str | None + resolved_core_policy: ResolvedCorePolicyTrace + planner_backend: str + planner_messages: tuple[str, ...] + planner_metadata: Mapping[str, object] + + def __post_init__(self) -> None: + if type(self.attempt_generation) is not int or self.attempt_generation < 0: + raise ValueError("attempt_generation must be non-negative.") + if type(self.trigger) is not str or not self.trigger: + raise ValueError("trigger must be a non-empty string.") + if not math.isfinite(self.planned_at) or self.planned_at < 0.0: + raise ValueError("planned_at must be finite and non-negative.") + if type(self.invocation_index) is not int or self.invocation_index < 0: + raise ValueError("invocation_index must be non-negative.") + for name in ("planned_mask", "plan_success_mask"): + value = getattr(self, name) + if ( + not isinstance(value, torch.Tensor) + or value.dtype != torch.bool + or value.dim() != 1 + ): + raise ValueError(f"{name} must be a one-dimensional bool tensor.") + if self.planned_mask.shape != self.plan_success_mask.shape: + raise ValueError("Plan-attempt masks must have equal shapes.") + if self.planned_mask.device != self.plan_success_mask.device: + raise ValueError("Plan-attempt masks must share a device.") + batch_size = int(self.planned_mask.numel()) + retries = tuple(self.action_retry_counts) + replans = tuple(self.replan_counts) + if len(retries) != batch_size or len(replans) != batch_size: + raise ValueError("Recovery counters must contain one value per row.") + if any(type(value) is not int or value < 0 for value in (*retries, *replans)): + raise ValueError("Recovery counters must be non-negative integers.") + 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.") + if type(self.invocation_revision) is not int or self.invocation_revision < 0: + raise ValueError("invocation_revision must be non-negative.") + if type(self.command_frame_count) is not int or self.command_frame_count < 0: + raise ValueError("command_frame_count must be non-negative.") + segments = tuple(self.trajectory_segments) + if not all(type(value) is TrajectorySegment for value in segments): + raise TypeError( + "trajectory_segments must contain TrajectorySegment values." + ) + if ( + type(self.planned_scene_version) is not int + or self.planned_scene_version < 0 + ): + raise ValueError("planned_scene_version must be non-negative.") + collision_revisions = tuple(self.planned_collision_world_revision) + if len(collision_revisions) != batch_size or any( + type(value) is not int or value < 0 for value in collision_revisions + ): + raise ValueError( + "planned_collision_world_revision must contain one non-negative " + "integer per row." + ) + dependencies = tuple(self.scene_dependencies) + if len(set(dependencies)) != len(dependencies) or not all( + type(value) is str and value for value in dependencies + ): + raise ValueError("scene_dependencies must contain unique identifiers.") + if not isinstance(self.scene_dependency_monitor_until, Mapping): + raise TypeError("scene_dependency_monitor_until must be a mapping.") + monitor_until = dict(self.scene_dependency_monitor_until) + if not set(monitor_until).issubset(dependencies): + raise ValueError( + "scene_dependency_monitor_until keys must be scene dependencies." + ) + for entity_id, waypoint_index in monitor_until.items(): + if ( + type(entity_id) is not str + or not entity_id + or type(waypoint_index) is not int + or not 0 <= waypoint_index <= self.command_frame_count + ): + raise ValueError( + "scene_dependency_monitor_until must map non-empty entity IDs " + "to waypoint indices within the command sequence." + ) + if type(self.collision_world_sensitive) is not bool: + raise TypeError("collision_world_sensitive must be a bool.") + if type(self.replannable) is not bool: + raise TypeError("replannable must be a bool.") + if not isinstance(self.tracking_policy, TrackingPolicy): + raise TypeError("tracking_policy must be a TrackingPolicy.") + if self.tracking is not None and not isinstance( + self.tracking, TimedTrackingSequence + ): + raise TypeError("tracking must be a TimedTrackingSequence or None.") + if self.effect_verification_kind is not None and ( + type(self.effect_verification_kind) is not str + or not self.effect_verification_kind + ): + raise ValueError("effect_verification_kind must be non-empty or None.") + if type(self.resolved_core_policy) is not ResolvedCorePolicyTrace: + raise TypeError( + "resolved_core_policy must be exactly ResolvedCorePolicyTrace." + ) + if type(self.planner_backend) is not str or not self.planner_backend: + raise ValueError("planner_backend must be a non-empty string.") + messages = tuple(self.planner_messages) + if not all(type(value) is str for value in messages): + raise TypeError("planner_messages must contain strings.") + object.__setattr__(self, "planned_mask", self.planned_mask.clone()) + object.__setattr__(self, "plan_success_mask", self.plan_success_mask.clone()) + object.__setattr__(self, "action_retry_counts", retries) + object.__setattr__(self, "replan_counts", replans) + object.__setattr__(self, "trajectory_segments", segments) + object.__setattr__( + self, + "planned_collision_world_revision", + collision_revisions, + ) + object.__setattr__(self, "scene_dependencies", dependencies) + object.__setattr__( + self, + "scene_dependency_monitor_until", + MappingProxyType(monitor_until), + ) + object.__setattr__(self, "tracking_policy", self.tracking_policy.snapshot()) + if self.tracking is not None: + object.__setattr__(self, "tracking", self.tracking.snapshot()) + object.__setattr__( + self, + "resolved_core_policy", + self.resolved_core_policy.snapshot(), + ) + object.__setattr__(self, "planner_messages", messages) + object.__setattr__( + self, + "planner_metadata", + _snapshot_metadata_mapping(self.planner_metadata), + ) + + @classmethod + def from_execution_attempt( + cls, + attempt: ExecutionPlanAttempt, + *, + profile_id: str, + preset_id: str, + preset_schema_version: int, + ) -> SkillPlanAttemptTrace: + """Project one session-owned plan attempt to compact trace metadata.""" + if not isinstance(attempt, ExecutionPlanAttempt): + raise TypeError("attempt must be an ExecutionPlanAttempt.") + plan = attempt.plan + request = attempt.request + return cls( + attempt_generation=attempt.attempt_generation, + trigger=attempt.event_kind.value, + planned_at=attempt.planned_at, + invocation_index=attempt.invocation_index, + planned_mask=attempt.planned_mask, + action_retry_counts=attempt.action_retry_counts, + replan_counts=attempt.replan_counts, + skill_id=plan.skill_id, + invocation_id=plan.invocation_id, + invocation_revision=plan.invocation_revision, + plan_success_mask=plan.plan_success, + command_frame_count=plan.commands.frame_count, + trajectory_segments=plan.segments, + planned_scene_version=plan.planned_scene_version, + planned_collision_world_revision=plan.planned_collision_world_revision, + scene_dependencies=plan.scene_dependencies, + scene_dependency_monitor_until=plan.scene_dependency_monitor_until, + collision_world_sensitive=plan.collision_world_sensitive, + replannable=plan.replannable, + tracking_policy=plan.tracking_policy, + tracking=plan.tracking, + effect_verification_kind=( + None + if plan.effect_verification is None + else plan.effect_verification.kind + ), + resolved_core_policy=ResolvedCorePolicyTrace.from_resolved_binding( + profile_id=profile_id, + preset_id=preset_id, + preset_schema_version=preset_schema_version, + motion_policy=request.motion_policy, + tracking_policy=request.tracking_policy, + recovery_policy=request.recovery_policy, + endpoints=request.binding.endpoints, + ), + planner_backend=plan.diagnostics.backend, + planner_messages=plan.diagnostics.messages, + planner_metadata=plan.diagnostics.metadata, + ) + + def snapshot(self) -> SkillPlanAttemptTrace: + """Return an independently owned compact plan-attempt trace.""" + return SkillPlanAttemptTrace( + attempt_generation=self.attempt_generation, + trigger=self.trigger, + planned_at=self.planned_at, + invocation_index=self.invocation_index, + planned_mask=self.planned_mask, + action_retry_counts=self.action_retry_counts, + replan_counts=self.replan_counts, + skill_id=self.skill_id, + invocation_id=self.invocation_id, + invocation_revision=self.invocation_revision, + plan_success_mask=self.plan_success_mask, + command_frame_count=self.command_frame_count, + trajectory_segments=self.trajectory_segments, + planned_scene_version=self.planned_scene_version, + planned_collision_world_revision=self.planned_collision_world_revision, + scene_dependencies=self.scene_dependencies, + scene_dependency_monitor_until=self.scene_dependency_monitor_until, + collision_world_sensitive=self.collision_world_sensitive, + replannable=self.replannable, + tracking_policy=self.tracking_policy, + tracking=self.tracking, + effect_verification_kind=self.effect_verification_kind, + resolved_core_policy=self.resolved_core_policy, + planner_backend=self.planner_backend, + planner_messages=self.planner_messages, + planner_metadata=self.planner_metadata, + ) + + def to_metadata(self) -> dict[str, object]: + """Return one plan generation as deterministic JSON-safe data.""" + return { + "attempt_generation": self.attempt_generation, + "trigger": self.trigger, + "planned_at": self.planned_at, + "invocation_index": self.invocation_index, + "planned_mask": _metadata_value(self.planned_mask), + "recovery_counters": { + "action_retries": list(self.action_retry_counts), + "replans": list(self.replan_counts), + }, + "skill_id": self.skill_id, + "invocation_id": self.invocation_id, + "invocation_revision": self.invocation_revision, + "plan_success_mask": _metadata_value(self.plan_success_mask), + "command_frame_count": self.command_frame_count, + "trajectory_segments": [ + { + "name": segment.name, + "start": segment.start, + "stop": segment.stop, + "waypoint_count": segment.waypoint_count, + } + for segment in self.trajectory_segments + ], + "planned_scene_version": self.planned_scene_version, + "planned_collision_world_revision": list( + self.planned_collision_world_revision + ), + "scene_dependencies": list(self.scene_dependencies), + "scene_dependency_monitor_until": { + entity_id: self.scene_dependency_monitor_until[entity_id] + for entity_id in sorted(self.scene_dependency_monitor_until) + }, + "collision_world_sensitive": self.collision_world_sensitive, + "replannable": self.replannable, + "tracking_policy": _tracking_policy_to_metadata(self.tracking_policy), + "tracking_contract": _tracking_sequence_to_metadata(self.tracking), + "effect_verification_kind": self.effect_verification_kind, + "resolved_core_policy": self.resolved_core_policy.to_metadata(), + "planner_diagnostics": { + "backend": self.planner_backend, + "messages": list(self.planner_messages), + "metadata": _metadata_value(self.planner_metadata), + }, + } + + +@dataclass(frozen=True, slots=True, eq=False) +class SkillEffectTrace: + """One monitor decision correlated with an atomic verification boundary.""" + + call_index: int + verification_id: int + observation_revision: int + timestamp: float + success_mask: torch.Tensor + failure_mask: torch.Tensor + expectation_decisions: tuple[EffectExpectationDecision, ...] + effect_spec: SemanticEffectSpec + monitor_id: str + monitor_revision: str | None + 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 + gate_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: + raise ValueError("call_index must be a non-negative integer.") + 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.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", + "phase_effect_gate", + }: + raise ValueError( + "boundary_kind must be 'terminal', 'in_flight_guard', or " + "'phase_effect_gate'." + ) + for name in ("guard_id", "gate_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.gate_id is not None + or self.segment_name is not None + ): + raise ValueError( + "Terminal effect traces cannot declare segment-boundary metadata." + ) + elif self.boundary_kind == "in_flight_guard" and ( + self.guard_id is None + or self.gate_id is not None + or self.segment_name is None + ): + raise ValueError( + "In-flight guard traces require only guard_id and segment_name." + ) + elif self.boundary_kind == "phase_effect_gate" and ( + self.gate_id is None + or self.guard_id is not None + or self.segment_name is None + ): + raise ValueError( + "Phase-effect gate traces require only gate_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"): + 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.success_mask.shape != self.failure_mask.shape: + raise ValueError("Effect trace masks must have equal shapes.") + if self.success_mask.device != self.failure_mask.device: + raise ValueError("Effect trace masks must share a device.") + if (self.success_mask & self.failure_mask).any(): + raise ValueError("Effect trace masks must not overlap.") + if not isinstance(self.effect_spec, SemanticEffectSpec): + raise TypeError("effect_spec must be a SemanticEffectSpec.") + expectation_decisions = tuple(self.expectation_decisions) + if not all( + type(value) is EffectExpectationDecision for value in expectation_decisions + ): + raise TypeError( + "expectation_decisions must contain exact " + "EffectExpectationDecision values." + ) + for value in expectation_decisions: + if value.satisfied_mask.shape != self.success_mask.shape: + raise ValueError( + "Expectation and aggregate trace masks must have equal shapes." + ) + if value.satisfied_mask.device != self.success_mask.device: + raise ValueError( + "Expectation and aggregate trace masks must share a device." + ) + physical_ids = tuple( + expectation.expectation_id + for expectation in self.effect_spec.state_expectations + if any( + clause.expectation_id == expectation.expectation_id + for clause in self.effect_spec.clauses + ) + ) + outcome_ids = tuple(value.expectation_id for value in expectation_decisions) + if outcome_ids != physical_ids: + raise ValueError( + "Effect trace must contain one ordered outcome for every " + f"physical expectation; expected={physical_ids}, " + f"got={outcome_ids}." + ) + if expectation_decisions: + expected_success = torch.ones_like(self.success_mask) + expected_failure = torch.zeros_like(self.failure_mask) + for value in expectation_decisions: + expected_success &= value.satisfied_mask + expected_failure |= value.contradicted_mask + if not torch.equal(self.success_mask, expected_success): + raise ValueError( + "success_mask must equal the conjunction of expectation " + "trace outcomes." + ) + if not torch.equal(self.failure_mask, expected_failure): + raise ValueError( + "failure_mask must equal the union of expectation trace " + "outcomes." + ) + if type(self.monitor_id) is not str or not self.monitor_id: + raise ValueError("monitor_id must be a non-empty string.") + if self.monitor_revision is not None and ( + type(self.monitor_revision) is not str or not self.monitor_revision + ): + raise ValueError("monitor_revision must be non-empty or None.") + evidence_types = ( + PoseRelationEvidenceBatch, + BinaryEffectEvidenceBatch, + ScalarEffectEvidenceBatch, + JointStateEvidenceBatch, + ) + evidence: dict[str, EffectEvidenceBatch] = {} + for evidence_id, batch in self.evidence.items(): + if type(evidence_id) is not str or not evidence_id: + raise ValueError("evidence keys must be non-empty strings.") + if type(batch) not in evidence_types: + raise TypeError("evidence values must be exact evidence batches.") + if batch.evidence_id != evidence_id: + raise ValueError("evidence keys must match batch evidence_id values.") + evidence[evidence_id] = batch.snapshot() + object.__setattr__(self, "success_mask", self.success_mask.clone()) + object.__setattr__(self, "failure_mask", self.failure_mask.clone()) + object.__setattr__( + self, + "expectation_decisions", + tuple(value.snapshot() for value in expectation_decisions), + ) + object.__setattr__(self, "effect_spec", self.effect_spec.snapshot()) + object.__setattr__( + self, + "configured_monitor_params", + _snapshot_metadata_mapping(self.configured_monitor_params), + ) + object.__setattr__( + self, + "resolved_monitor_params", + _snapshot_metadata_mapping(self.resolved_monitor_params), + ) + object.__setattr__(self, "evidence", MappingProxyType(evidence)) + + def snapshot(self) -> SkillEffectTrace: + """Return an independently owned trace.""" + return SkillEffectTrace( + call_index=self.call_index, + verification_id=self.verification_id, + observation_revision=self.observation_revision, + timestamp=self.timestamp, + success_mask=self.success_mask, + failure_mask=self.failure_mask, + expectation_decisions=self.expectation_decisions, + effect_spec=self.effect_spec, + monitor_id=self.monitor_id, + monitor_revision=self.monitor_revision, + 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, + gate_id=self.gate_id, + segment_name=self.segment_name, + ) + + def to_metadata(self) -> dict[str, object]: + """Return monitor contract, evidence, thresholds, and decision metadata.""" + metadata = { + "call_index": self.call_index, + "verification_id": self.verification_id, + "observation_revision": self.observation_revision, + "timestamp": self.timestamp, + "effect_spec": self.effect_spec.to_metadata(), + "monitor": { + "monitor_id": self.monitor_id, + "revision": self.monitor_revision, + "configured_params": _metadata_value(self.configured_monitor_params), + "resolved_params": _metadata_value(self.resolved_monitor_params), + }, + "evidence": { + evidence_id: batch.to_metadata() + for evidence_id, batch in sorted(self.evidence.items()) + }, + "decision": { + "success_mask": _metadata_value(self.success_mask), + "failure_mask": _metadata_value(self.failure_mask), + "expectations": [ + { + "expectation_id": value.expectation_id, + "satisfied_mask": _metadata_value(value.satisfied_mask), + "contradicted_mask": _metadata_value(value.contradicted_mask), + "inverse_satisfied_mask": _metadata_value( + value.inverse_satisfied_mask + ), + } + for value in self.expectation_decisions + ], + }, + } + 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, + } + ) + elif self.boundary_kind == "phase_effect_gate": + metadata["boundary"].update( + { + "gate_id": self.gate_id, + "segment_name": self.segment_name, + } + ) + return metadata + + +@dataclass(frozen=True, slots=True, eq=False) +class SkillFailure: + """Per-environment semantic workflow failure.""" + + call_index: int + semantic_id: str + env_mask: torch.Tensor + message: str + + def __post_init__(self) -> None: + if type(self.call_index) is not int or self.call_index < 0: + raise ValueError("call_index must be a non-negative integer.") + if type(self.semantic_id) is not str or not self.semantic_id: + raise ValueError("semantic_id 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 type(self.message) is not str or not self.message: + raise ValueError("message must be a non-empty string.") + object.__setattr__(self, "env_mask", self.env_mask.clone()) + + def snapshot(self) -> SkillFailure: + """Return an independently owned failure.""" + return SkillFailure( + call_index=self.call_index, + semantic_id=self.semantic_id, + env_mask=self.env_mask, + message=self.message, + ) + + def to_metadata(self) -> dict[str, object]: + """Return one row-local failure as JSON-safe data.""" + return { + "call_index": self.call_index, + "semantic_id": self.semantic_id, + "env_mask": _metadata_value(self.env_mask), + "message": self.message, + } + + +@dataclass(frozen=True, slots=True, eq=False) +class SkillCallTrace: + """Terminal trace for exactly one semantic call and execution session.""" + + call_index: int + semantic_id: str + call_metadata: Mapping[str, object] + skill_id: str + invocation_id: str | None + invocation_revision: int + status: RunnerStatus + entered_mask: torch.Tensor + completed_mask: torch.Tensor + failed_mask: torch.Tensor + command_count: int + resolved_core_policy: ResolvedCorePolicyTrace + plan_attempts: tuple[SkillPlanAttemptTrace, ...] + events: tuple[ExecutionEvent, ...] = () + effects: tuple[SkillEffectTrace, ...] = () + + def __post_init__(self) -> None: + if type(self.call_index) is not int or self.call_index < 0: + raise ValueError("call_index must be a non-negative integer.") + for name in ("semantic_id", "skill_id"): + value = getattr(self, name) + if type(value) is not str or not value: + raise ValueError(f"{name} must be a non-empty string.") + normalized_call = _snapshot_metadata_mapping(self.call_metadata) + if normalized_call.get("semantic_id") != self.semantic_id: + raise ValueError("call_metadata semantic_id must match semantic_id.") + 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.") + if self.invocation_revision < 0: + raise ValueError("invocation_revision must be non-negative.") + if not isinstance(self.status, RunnerStatus): + raise TypeError("status must be a RunnerStatus.") + if self.status is RunnerStatus.RUNNING: + raise ValueError("A terminal call trace cannot have running status.") + for name in ("entered_mask", "completed_mask", "failed_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 not ( + self.entered_mask.shape + == self.completed_mask.shape + == self.failed_mask.shape + ): + raise ValueError("Call trace masks must have equal shapes.") + if not ( + self.entered_mask.device + == self.completed_mask.device + == self.failed_mask.device + ): + raise ValueError("Call trace masks must share a device.") + if (self.completed_mask & ~self.entered_mask).any(): + raise ValueError("completed_mask must be a subset of entered_mask.") + if (self.failed_mask & ~self.entered_mask).any(): + raise ValueError("failed_mask must be a subset of entered_mask.") + if (self.completed_mask & self.failed_mask).any(): + raise ValueError("completed_mask and failed_mask must not overlap.") + if type(self.command_count) is not int or self.command_count < 0: + raise ValueError("command_count must be non-negative.") + if type(self.resolved_core_policy) is not ResolvedCorePolicyTrace: + raise TypeError( + "resolved_core_policy must be exactly ResolvedCorePolicyTrace." + ) + attempts = tuple(self.plan_attempts) + if not all(type(attempt) is SkillPlanAttemptTrace for attempt in attempts): + raise TypeError("plan_attempts must contain SkillPlanAttemptTrace values.") + if attempts: + generations = tuple(attempt.attempt_generation for attempt in attempts) + if generations != tuple( + range(generations[0], generations[0] + len(attempts)) + ): + raise ValueError( + "plan_attempts must use contiguous ordered generations." + ) + if attempts[-1].skill_id != self.skill_id: + raise ValueError("The active plan-attempt skill must match skill_id.") + elif self.status is not RunnerStatus.FAILED or self.command_count != 0: + raise ValueError( + "Only a preparation failure with no commands may omit plan_attempts." + ) + object.__setattr__(self, "entered_mask", self.entered_mask.clone()) + object.__setattr__(self, "completed_mask", self.completed_mask.clone()) + object.__setattr__(self, "failed_mask", self.failed_mask.clone()) + object.__setattr__(self, "call_metadata", normalized_call) + object.__setattr__( + self, + "resolved_core_policy", + self.resolved_core_policy.snapshot(), + ) + object.__setattr__( + self, + "plan_attempts", + tuple(attempt.snapshot() for attempt in attempts), + ) + object.__setattr__( + self, + "events", + tuple(_snapshot_event(event) for event in self.events), + ) + object.__setattr__( + self, + "effects", + tuple(effect.snapshot() for effect in self.effects), + ) + + def snapshot(self) -> SkillCallTrace: + """Return an independently owned call trace.""" + return SkillCallTrace( + call_index=self.call_index, + semantic_id=self.semantic_id, + call_metadata=self.call_metadata, + skill_id=self.skill_id, + invocation_id=self.invocation_id, + invocation_revision=self.invocation_revision, + status=self.status, + entered_mask=self.entered_mask, + completed_mask=self.completed_mask, + failed_mask=self.failed_mask, + command_count=self.command_count, + resolved_core_policy=self.resolved_core_policy, + plan_attempts=self.plan_attempts, + events=self.events, + effects=self.effects, + ) + + @property + def active_plan(self) -> SkillPlanAttemptTrace: + """Return the final installed plan generation as an owned trace.""" + if not self.plan_attempts: + raise RuntimeError("This call failed before an action plan was installed.") + return self.plan_attempts[-1].snapshot() + + def to_metadata(self) -> dict[str, object]: + """Return one semantic call, recovery history, and effects as JSON-safe data.""" + attempts = [attempt.to_metadata() for attempt in self.plan_attempts] + return { + "call_index": self.call_index, + "semantic_id": self.semantic_id, + "call": _metadata_value(self.call_metadata), + "skill_id": self.skill_id, + "invocation_id": self.invocation_id, + "invocation_revision": self.invocation_revision, + "status": self.status.value, + "masks": { + "entered": _metadata_value(self.entered_mask), + "completed": _metadata_value(self.completed_mask), + "failed": _metadata_value(self.failed_mask), + }, + "command_count": self.command_count, + "active_plan_attempt_generation": ( + None + if not self.plan_attempts + else self.plan_attempts[-1].attempt_generation + ), + "resolved_core_policy": self.resolved_core_policy.to_metadata(), + "plan_attempts": attempts, + "events": [_event_to_metadata(event) for event in self.events], + "effects": [effect.to_metadata() for effect in self.effects], + } + + +@dataclass(frozen=True, slots=True, eq=False) +class SkillWorkflowRecoveryTrace: + """One auditable real semantic call within bounded workflow recovery. + + Args: + recovery_id: Monotonic runtime-local trace identifier. + trigger_call_index: Original workflow call held at the shared barrier. + trigger_semantic_id: Semantic ID of the original failed call. + attempt_index: One-based per-row recovery-cycle index. + max_recovery_attempts: Configured per-row recovery-cycle budget. + role: Whether this call retries, re-acquires, or retries after pickup. + source_resource_id: Resolved robot resource that owns the source object. + source_task_state_key: Verified held-object state key for that resource. + entered_mask: Rows that entered this real recovery call. + completed_mask: Entered rows that completed this call. + failed_mask: Entered rows that failed this call. + call: Nested semantic-call trace, or ``None`` when preparation failed. + message: Optional terminal or preparation diagnostic. + """ + + recovery_id: int + trigger_call_index: int + trigger_semantic_id: str + attempt_index: int + max_recovery_attempts: int + role: SkillWorkflowRecoveryRole + source_resource_id: str + source_task_state_key: str + entered_mask: torch.Tensor + completed_mask: torch.Tensor + failed_mask: torch.Tensor + call: SkillCallTrace | None + message: str | None = None + + def __post_init__(self) -> None: + if type(self.recovery_id) is not int or self.recovery_id < 0: + raise ValueError("recovery_id must be a non-negative integer.") + if type(self.trigger_call_index) is not int or self.trigger_call_index < 0: + raise ValueError("trigger_call_index must be non-negative.") + for name in ( + "trigger_semantic_id", + "source_resource_id", + "source_task_state_key", + ): + value = getattr(self, name) + if type(value) is not str or not value: + raise ValueError(f"{name} must be a non-empty string.") + if type(self.attempt_index) is not int or self.attempt_index <= 0: + raise ValueError("attempt_index must be a positive integer.") + if ( + type(self.max_recovery_attempts) is not int + or self.max_recovery_attempts <= 0 + or self.attempt_index > self.max_recovery_attempts + ): + raise ValueError( + "max_recovery_attempts must cover the positive attempt_index." + ) + if not isinstance(self.role, SkillWorkflowRecoveryRole): + raise TypeError("role must be a SkillWorkflowRecoveryRole.") + for name in ("entered_mask", "completed_mask", "failed_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 not ( + self.entered_mask.shape + == self.completed_mask.shape + == self.failed_mask.shape + ) or not ( + self.entered_mask.device + == self.completed_mask.device + == self.failed_mask.device + ): + raise ValueError("Workflow-recovery masks must share shape and device.") + if (self.completed_mask & self.failed_mask).any(): + raise ValueError("Recovery completion and failure masks cannot overlap.") + if ((self.completed_mask | self.failed_mask) & ~self.entered_mask).any(): + raise ValueError("Recovery outcomes must be subsets of entered_mask.") + if self.call is not None: + if type(self.call) is not SkillCallTrace: + raise TypeError("call must be a SkillCallTrace or None.") + if not torch.equal(self.call.entered_mask, self.entered_mask): + raise ValueError("Recovery call entered_mask must match the trace.") + if not torch.equal(self.call.completed_mask, self.completed_mask): + raise ValueError("Recovery call completed_mask must match the trace.") + if not torch.equal(self.call.failed_mask, self.failed_mask): + raise ValueError("Recovery call failed_mask must match the trace.") + elif not torch.equal(self.failed_mask, self.entered_mask): + raise ValueError( + "A recovery preparation failure must fail every entered row." + ) + if self.message is not None and ( + type(self.message) is not str or not self.message + ): + raise ValueError("message must be a non-empty string or None.") + for name in ("entered_mask", "completed_mask", "failed_mask"): + object.__setattr__(self, name, getattr(self, name).clone()) + if self.call is not None: + object.__setattr__(self, "call", self.call.snapshot()) + + def snapshot(self) -> SkillWorkflowRecoveryTrace: + """Return an independently owned workflow-recovery trace.""" + return SkillWorkflowRecoveryTrace( + recovery_id=self.recovery_id, + trigger_call_index=self.trigger_call_index, + trigger_semantic_id=self.trigger_semantic_id, + attempt_index=self.attempt_index, + max_recovery_attempts=self.max_recovery_attempts, + role=self.role, + source_resource_id=self.source_resource_id, + source_task_state_key=self.source_task_state_key, + entered_mask=self.entered_mask, + completed_mask=self.completed_mask, + failed_mask=self.failed_mask, + call=self.call, + message=self.message, + ) + + def to_metadata(self) -> dict[str, object]: + """Return deterministic JSON-safe workflow-recovery metadata.""" + return { + "recovery_id": self.recovery_id, + "trigger_call_index": self.trigger_call_index, + "trigger_semantic_id": self.trigger_semantic_id, + "attempt_index": self.attempt_index, + "max_recovery_attempts": self.max_recovery_attempts, + "role": self.role.value, + "source_resource_id": self.source_resource_id, + "source_task_state_key": self.source_task_state_key, + "masks": { + "entered": _metadata_value(self.entered_mask), + "completed": _metadata_value(self.completed_mask), + "failed": _metadata_value(self.failed_mask), + }, + "call": None if self.call is None else self.call.to_metadata(), + "message": self.message, + } + + +@dataclass(frozen=True, slots=True, eq=False) +class _WorkflowRecoveryWorkItem: + """One cohort scheduled at a shared semantic-call recovery barrier.""" + + role: SkillWorkflowRecoveryRole + call: SemanticCallSpec + env_mask: torch.Tensor + attempt_index: int + + def __post_init__(self) -> None: + if not isinstance(self.role, SkillWorkflowRecoveryRole): + raise TypeError("role must be a SkillWorkflowRecoveryRole.") + if not isinstance(self.call, SemanticCallSpec): + raise TypeError("call must be a SemanticCallSpec.") + if ( + not isinstance(self.env_mask, torch.Tensor) + or self.env_mask.dtype != torch.bool + or self.env_mask.dim() != 1 + or not self.env_mask.any() + ): + raise ValueError( + "env_mask must be a non-empty one-dimensional bool tensor." + ) + if type(self.attempt_index) is not int or self.attempt_index <= 0: + raise ValueError("attempt_index must be a positive integer.") + object.__setattr__(self, "env_mask", self.env_mask.clone()) + + +@dataclass(slots=True) +class _WorkflowRecoveryBarrier: + """Mutable per-call barrier while failed rows recover and rejoin.""" + + trigger_call_index: int + trigger_call: SemanticCallSpec + policy: WorkflowRecoveryPolicy + source_resource_id: str + source_task_state_key: str + entered_mask: torch.Tensor + success_mask: torch.Tensor + final_failure_mask: torch.Tensor + attempt_counts: torch.Tensor + work_items: deque[_WorkflowRecoveryWorkItem] + failure_messages: list[str] + + +@dataclass(frozen=True, slots=True) +class _FinishedCallAttempt: + """Internal terminal projection of one execution session.""" + + trace: SkillCallTrace + completed_mask: torch.Tensor + failed_mask: torch.Tensor + status: RunnerStatus + message: str | None + + +@dataclass(frozen=True, slots=True) +class _WorkflowRecoveryTrigger: + """Resolved workflow policy and source identity for one original call.""" + + policy: WorkflowRecoveryPolicy + source_resource_id: str + source_task_state_key: str + + +@dataclass(frozen=True, slots=True, eq=False) +class SkillResult: + """Immutable workflow snapshot returned by sync and step-wise execution.""" + + status: SkillStatus + workflow_id: str | None + current_call_index: int | None + env_ids: torch.Tensor + success_mask: torch.Tensor + failure_mask: torch.Tensor + cancelled_mask: torch.Tensor + eligible_mask: torch.Tensor + task_state: TaskState + events: tuple[ExecutionEvent, ...] = () + calls: tuple[SkillCallTrace, ...] = () + effects: tuple[SkillEffectTrace, ...] = () + workflow_recoveries: tuple[SkillWorkflowRecoveryTrace, ...] = () + failures: tuple[SkillFailure, ...] = () + wait_duration: float = 0.0 + message: str | None = None + + def __post_init__(self) -> None: + if not isinstance(self.status, SkillStatus): + raise TypeError("status must be a SkillStatus.") + if self.workflow_id is not None and ( + type(self.workflow_id) is not str or not self.workflow_id + ): + raise ValueError("workflow_id must be a non-empty string or None.") + if self.current_call_index is not None and ( + type(self.current_call_index) is not int or self.current_call_index < 0 + ): + raise ValueError("current_call_index must be non-negative or None.") + if not isinstance(self.env_ids, torch.Tensor): + raise TypeError("env_ids must be a torch.Tensor.") + if self.env_ids.dtype != torch.long or self.env_ids.dim() != 1: + raise ValueError("env_ids must be a one-dimensional torch.long tensor.") + if self.env_ids.numel() == 0: + raise ValueError("env_ids must contain at least one environment.") + if torch.unique(self.env_ids).numel() != self.env_ids.numel(): + raise ValueError("env_ids must be unique.") + batch_size = int(self.env_ids.numel()) + for name in ( + "success_mask", + "failure_mask", + "cancelled_mask", + "eligible_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.shape != (batch_size,): + raise ValueError(f"{name} must be bool with shape ({batch_size},).") + if value.device != self.env_ids.device: + raise ValueError(f"{name} and env_ids must share a device.") + if (self.success_mask & (self.failure_mask | self.cancelled_mask)).any(): + raise ValueError("Successful rows cannot also fail or be cancelled.") + if (self.failure_mask & self.cancelled_mask).any(): + raise ValueError("Failed and cancelled masks must not overlap.") + if (self.eligible_mask & (self.failure_mask | self.cancelled_mask)).any(): + raise ValueError("Eligible rows cannot also fail or be cancelled.") + if (self.success_mask & ~self.eligible_mask).any(): + raise ValueError("success_mask must be a subset of eligible_mask.") + if not isinstance(self.task_state, TaskState): + raise TypeError("task_state must be a TaskState.") + if self.task_state.batch_size != batch_size: + raise ValueError("task_state batch size must match env_ids.") + if self.task_state.device != self.env_ids.device: + raise ValueError("task_state and env_ids must share a device.") + if not math.isfinite(self.wait_duration) or self.wait_duration < 0.0: + raise ValueError("wait_duration must be finite and non-negative.") + if self.message is not None and type(self.message) is not str: + raise TypeError("message must be a string or None.") + if not all( + type(recovery) is SkillWorkflowRecoveryTrace + for recovery in self.workflow_recoveries + ): + raise TypeError( + "workflow_recoveries must contain SkillWorkflowRecoveryTrace values." + ) + object.__setattr__(self, "env_ids", self.env_ids.clone()) + for name in ( + "success_mask", + "failure_mask", + "cancelled_mask", + "eligible_mask", + ): + object.__setattr__(self, name, getattr(self, name).clone()) + object.__setattr__(self, "task_state", _snapshot_task_state(self.task_state)) + object.__setattr__( + self, + "events", + tuple(_snapshot_event(event) for event in self.events), + ) + object.__setattr__( + self, + "calls", + tuple(call.snapshot() for call in self.calls), + ) + object.__setattr__( + self, + "effects", + tuple(effect.snapshot() for effect in self.effects), + ) + object.__setattr__( + self, + "workflow_recoveries", + tuple(recovery.snapshot() for recovery in self.workflow_recoveries), + ) + object.__setattr__( + self, + "failures", + tuple(failure.snapshot() for failure in self.failures), + ) + + @property + def terminal(self) -> bool: + """Whether the workflow no longer accepts execution steps.""" + return self.status in { + SkillStatus.COMPLETED, + SkillStatus.FAILED, + SkillStatus.CANCELLED, + } + + def to_metadata(self) -> dict[str, object]: + """Return a fresh deterministic JSON-safe workflow result. + + Core recovery remains represented by the ordered + :class:`ExecutionEvent` stream and each call's plan-attempt history. + Workflow re-acquisition additionally appears in + ``workflow_recoveries``. The returned object owns only Python scalars, + lists, and dictionaries and can be serialized with + ``json.dumps(..., allow_nan=False)``. + """ + return { + "schema_version": 2, + "kind": "skill_result", + "status": self.status.value, + "workflow_id": self.workflow_id, + "current_call_index": self.current_call_index, + "env_ids": _metadata_value(self.env_ids), + "masks": { + "success": _metadata_value(self.success_mask), + "failure": _metadata_value(self.failure_mask), + "cancelled": _metadata_value(self.cancelled_mask), + "eligible": _metadata_value(self.eligible_mask), + }, + "task_state": task_state_to_metadata(self.task_state), + "events": [_event_to_metadata(event) for event in self.events], + "calls": [call.to_metadata() for call in self.calls], + "effects": [effect.to_metadata() for effect in self.effects], + "workflow_recoveries": [ + recovery.to_metadata() for recovery in self.workflow_recoveries + ], + "failures": [failure.to_metadata() for failure in self.failures], + "wait_duration": self.wait_duration, + "message": self.message, + } + + +@runtime_checkable +class EffectEvidenceCollectorPort(Protocol): + """Minimal collector surface consumed by :class:`SkillRuntime`.""" + + def collect( + self, + spec: SemanticEffectSpec, + *, + timestamp: float, + observation_revision: int, + env_ids: torch.Tensor | None = None, + ) -> Mapping[str, EffectEvidenceBatch]: + """Acquire synchronized raw evidence for one grounded effect.""" + + +@runtime_checkable +class SkillRuntimeProvider(Protocol): + """Explicit environment adapter installed for :meth:`AtomicSkills.from_env`.""" + + def create_skill_runtime(self, *, preset: str) -> SkillRuntime: + """Build a fully connected semantic runtime for this environment.""" + + +class _PrimedObservationProvider: + """Return a JIT-grounding observation once before delegating fresh reads.""" + + def __init__( + self, + context: PlanningContext, + delegate: ObservationProvider, + ) -> None: + self._context: PlanningContext | None = context + self._delegate = delegate + + def observe(self, task_state: TaskState) -> PlanningContext: + """Reuse the grounding snapshot for the session's first due cycle.""" + context = self._context + if context is None: + return self._delegate.observe(task_state) + self._context = None + return PlanningContext( + robot=context.robot, + task=task_state, + scene=context.scene, + env_ids=context.env_ids, + control_dt=context.control_dt, + ) + + +class SkillRuntime: + """JIT-ground and execute semantic calls through one runner per call. + + Static workflow analysis occurs once in :meth:`start`. Each call then gets + a fresh observation, one grounded invocation, one execution session, and + one :class:`ExecutionRunner`. Verified task state and row eligibility cross + call barriers; execution sessions never do. + """ + + def __init__( + self, + compiler: SemanticSkillCompiler, + observation_provider: ObservationProvider, + command_sink: CommandSink, + evidence_collector: EffectEvidenceCollectorPort, + *, + task_state: TaskState | None = None, + clock: ExecutionClock | None = None, + runner_cfg: ExecutionRunnerCfg | None = None, + ) -> None: + if not isinstance(compiler, SemanticSkillCompiler): + raise TypeError("compiler must be a SemanticSkillCompiler.") + if not isinstance(observation_provider, ObservationProvider): + raise TypeError("observation_provider must implement ObservationProvider.") + if not isinstance(command_sink, CommandSink): + raise TypeError("command_sink must implement CommandSink.") + if not isinstance(evidence_collector, EffectEvidenceCollectorPort): + raise TypeError( + "evidence_collector must implement EffectEvidenceCollectorPort." + ) + if clock is not None and not isinstance(clock, ExecutionClock): + raise TypeError("clock must implement ExecutionClock.") + if runner_cfg is not None and not isinstance(runner_cfg, ExecutionRunnerCfg): + raise TypeError("runner_cfg must be an ExecutionRunnerCfg or None.") + integration = compiler.integration + engine = integration.engine + if not isinstance(engine, AtomicActionEngine): + raise TypeError( + "compiler.integration.engine must be an AtomicActionEngine." + ) + initial_task = ( + engine.initial_context().task if task_state is None else task_state + ) + if not isinstance(initial_task, TaskState): + raise TypeError("task_state must be a TaskState or None.") + if initial_task.device != engine.device: + raise ValueError("task_state and compiler engine must share a device.") + + self._compiler = compiler + self._engine = engine + self._observation_provider = observation_provider + self._command_sink = command_sink + self._evidence_collector = evidence_collector + self._clock = clock or MonotonicExecutionClock() + self._runner_cfg = runner_cfg or ExecutionRunnerCfg() + self._task_state = _snapshot_task_state(initial_task) + self._env_ids = torch.arange( + self._task_state.batch_size, + dtype=torch.long, + device=self._task_state.device, + ) + self._has_observed_env_ids = False + self._status = SkillStatus.IDLE + self._workflow: object | None = None + self._workflow_id: str | None = None + self._calls: tuple[SemanticCallSpec, ...] = () + self._execution_prefix_length = 0 + self._current_call_index: int | None = None + self._runner: ExecutionRunner | None = None + self._grounded: object | None = None + self._active_call: SemanticCallSpec | None = None + self._active_recovery_item: _WorkflowRecoveryWorkItem | None = None + self._recovery_barrier: _WorkflowRecoveryBarrier | None = None + self._call_entered_mask = torch.zeros( + self._task_state.batch_size, + dtype=torch.bool, + device=self._task_state.device, + ) + self._eligible = torch.ones_like(self._call_entered_mask) + self._success = torch.zeros_like(self._eligible) + self._failed = torch.zeros_like(self._eligible) + self._cancelled = torch.zeros_like(self._eligible) + self._events: list[ExecutionEvent] = [] + self._call_traces: list[SkillCallTrace] = [] + self._effect_traces: list[SkillEffectTrace] = [] + self._workflow_recovery_traces: list[SkillWorkflowRecoveryTrace] = [] + self._failures: list[SkillFailure] = [] + self._call_event_offset = 0 + self._call_effect_offset = 0 + self._observation_revision = 0 + self._next_guard_verification_id = 0 + self._next_gate_verification_id = 0 + self._next_workflow_recovery_id = 0 + self._wait_duration = 0.0 + self._message: str | None = None + + @classmethod + def from_components( + cls, + compiler: SemanticSkillCompiler, + observation_provider: ObservationProvider, + command_sink: CommandSink, + evidence_collector: EffectEvidenceCollectorPort, + *, + task_state: TaskState | None = None, + clock: ExecutionClock | None = None, + runner_cfg: ExecutionRunnerCfg | None = None, + ) -> SkillRuntime: + """Construct the canonical runtime from explicit reusable ports.""" + return cls( + compiler, + observation_provider, + command_sink, + evidence_collector, + task_state=task_state, + clock=clock, + runner_cfg=runner_cfg, + ) + + @property + def compiler(self) -> SemanticSkillCompiler: + """Return the installed semantic compiler.""" + return self._compiler + + @property + def clock(self) -> ExecutionClock: + """Return the shared execution clock used by this runtime. + + Parallel coordinators use the same clock for every derived lane so a + branch cannot advance independently of the environment step grid. + """ + return self._clock + + @property + def scene_registry(self) -> SceneRegistry: + """Return the authoritative semantic scene registry.""" + return self._compiler.integration.scene_registry + + @property + def task_state(self) -> TaskState: + """Return an owned snapshot of persistent verified task state.""" + return _snapshot_task_state(self._task_state) + + def fork( + self, + command_sink: CommandSink, + *, + task_state: TaskState | None = None, + ) -> SkillRuntime: + """Create an independent execution lane from the same runtime ports. + + The derived runtime shares the immutable compiler integration, + observation/evidence providers, clock, and runner policy, but owns its + workflow, runner, masks, and verified task state. Its command sink is + supplied explicitly so a parallel coordinator can buffer commands + until all lanes have reached the same environment tick. + + Args: + command_sink: Lane-local command sink. + task_state: Optional verified barrier state. The current owned + task state is used when omitted. + + Returns: + A new idle semantic runtime for one independent lane. + """ + if not isinstance(command_sink, CommandSink): + raise TypeError("command_sink must implement CommandSink.") + initial_state = self.task_state if task_state is None else task_state + if not isinstance(initial_state, TaskState): + raise TypeError("task_state must be a TaskState or None.") + return SkillRuntime( + self._compiler, + self._observation_provider, + command_sink, + self._evidence_collector, + task_state=initial_state, + clock=self._clock, + runner_cfg=self._runner_cfg, + ) + + @property + def status(self) -> SkillStatus: + """Return the current workflow status.""" + return self._status + + @property + def result(self) -> SkillResult: + """Return an immutable snapshot of the current workflow.""" + return SkillResult( + status=self._status, + workflow_id=self._workflow_id, + current_call_index=self._current_call_index, + env_ids=self._env_ids, + success_mask=self._success, + failure_mask=self._failed, + cancelled_mask=self._cancelled, + eligible_mask=self._eligible, + task_state=self._task_state, + events=tuple(self._events), + calls=tuple(self._call_traces), + effects=tuple(self._effect_traces), + workflow_recoveries=tuple(self._workflow_recovery_traces), + failures=tuple(self._failures), + wait_duration=self._wait_duration, + message=self._message, + ) + + def start( + self, + *calls: SemanticCallSpec | Iterable[SemanticCallSpec], + workflow_id: str = "semantic_workflow", + eligible_mask: torch.Tensor | None = None, + execution_prefix_length: int | None = None, + ) -> SkillResult: + """Analyze once and prepare the first call without blocking on motion. + + Args: + *calls: Complete ordered semantic analysis window. Calls after the + execution prefix participate in static look-ahead but are not + grounded or executed by this run. + workflow_id: Stable workflow identifier used in diagnostics. + eligible_mask: Optional row-local execution eligibility. + execution_prefix_length: Number of leading calls to execute. When + omitted, the complete analysis window is executed. + + Returns: + Immutable initial runtime result. + """ + if self._status is SkillStatus.RUNNING: + raise RuntimeError("A semantic workflow is already running.") + normalized = self._normalize_calls(calls) + if type(workflow_id) is not str or not workflow_id: + raise ValueError("workflow_id must be a non-empty string.") + prefix_length = self._normalize_execution_prefix_length( + execution_prefix_length, + call_count=len(normalized), + ) + workflow = self._compiler.analyze(normalized, workflow_id=workflow_id) + self._reset_workflow( + normalized, + workflow, + workflow_id=workflow_id, + eligible_mask=eligible_mask, + execution_prefix_length=prefix_length, + ) + try: + self._prepare_call(0) + except Exception as exc: # noqa: BLE001 - return one uniform result + self._fail_preparation(0, exc) + return self.result + + def step(self) -> SkillResult: + """Advance the current call by at most one due runner cycle.""" + if self._status is not SkillStatus.RUNNING: + return self.result + runner = self._require_runner() + grounded = self._require_grounded() + monitor = getattr(grounded, "effect_monitor", None) + verifier = self._effect_verifier if monitor is not None else None + guards = tuple(getattr(grounded, "effect_guards", ())) + guard_verifier = self._held_object_guard_verifier if guards else None + gates = tuple(getattr(grounded, "effect_gates", ())) + gate_verifier = self._phase_effect_gate_verifier if gates else None + runner_step = runner.step( + effect_verifier=verifier, + phase_effect_gate_verifier=gate_verifier, + held_object_guard_verifier=guard_verifier, + ) + self._consume_runner_step(runner_step) + if ( + runner_step.status is RunnerStatus.RUNNING + and runner_step.tick is not None + and runner_step.tick.pending_effect is not None + and monitor is None + ): + self._abort( + "The atomic plan requested effect verification, but the grounded " + "semantic call did not install an effect monitor." + ) + return self.result + if ( + runner_step.status is RunnerStatus.RUNNING + and runner_step.tick is not None + and runner_step.tick.pending_phase_effect_gate is not None + and not gates + ): + self._abort( + "The atomic invocation requested a phase-effect gate, but the " + "grounded semantic call did not install its monitor." + ) + return self.result + if runner_step.status is RunnerStatus.RUNNING: + return self.result + recovery_item = self._active_recovery_item + trigger = ( + self._workflow_recovery_trigger() + if recovery_item is None and self._active_call_requires_workflow_recovery() + else None + ) + finished = self._finish_active_call(runner_step) + if recovery_item is None: + self._call_traces.append(finished.trace) + self._handle_original_call_finished(finished, trigger=trigger) + else: + self._handle_recovery_call_finished(recovery_item, finished) + return self.result + + def run( + self, + *calls: SemanticCallSpec | Iterable[SemanticCallSpec], + workflow_id: str = "semantic_workflow", + eligible_mask: torch.Tensor | None = None, + execution_prefix_length: int | None = None, + max_steps: int = 100_000, + ) -> SkillResult: + """Synchronously execute an analyzed semantic-call prefix.""" + if type(max_steps) is not int or max_steps <= 0: + raise ValueError("max_steps must be a positive integer.") + result = self.start( + *calls, + workflow_id=workflow_id, + eligible_mask=eligible_mask, + execution_prefix_length=execution_prefix_length, + ) + for _ in range(max_steps): + if result.terminal: + return result + if result.wait_duration > 0.0: + self._clock.sleep(result.wait_duration) + result = self.step() + self._abort(f"Semantic runtime exceeded max_steps={max_steps}.") + return self.result + + def cancel( + self, reason: str = "Semantic workflow cancelled by caller." + ) -> SkillResult: + """Cancel the active runner and inherit its cancel-then-hold behavior.""" + if type(reason) is not str or not reason: + raise ValueError("reason must be a non-empty string.") + if self._status is not SkillStatus.RUNNING: + return self.result + pending = self._eligible.clone() + recovery_item = self._active_recovery_item + runner_step = self._require_runner().cancel(reason) + self._consume_runner_step(runner_step) + self._message = runner_step.message or reason + finished = self._finish_active_call(runner_step) + if recovery_item is None: + self._call_traces.append(finished.trace) + else: + self._append_workflow_recovery_trace( + recovery_item, + call=finished.trace, + completed_mask=finished.completed_mask, + failed_mask=finished.failed_mask, + message=finished.message, + ) + self._status = ( + SkillStatus.CANCELLED + if runner_step.status is RunnerStatus.CANCELLED + else SkillStatus.FAILED + ) + if self._status is SkillStatus.FAILED: + self._failed |= pending + self._cancelled &= ~pending + else: + self._cancelled |= pending + self._eligible &= ~pending + self._recovery_barrier = None + self._current_call_index = None + self._wait_duration = 0.0 + return self.result + + def deactivate_rows( + self, + env_mask: torch.Tensor, + *, + reason: str, + ) -> SkillResult: + """Cancel selected rows while the remaining shared call keeps running. + + This is the row-local cancellation boundary used by a parallel + fail-fast coordinator. The active runner remains the sole owner of + controller neutralization and effect-request correlation. + + Args: + env_mask: Rows to remove permanently from this workflow. + reason: Human-readable cancellation reason. + + Returns: + Updated immutable workflow result. + """ + if self._status is not SkillStatus.RUNNING: + return self.result + if not isinstance(env_mask, torch.Tensor): + raise TypeError("env_mask must be a torch.Tensor.") + if ( + env_mask.dtype != torch.bool + or env_mask.shape != self._eligible.shape + or env_mask.device != self._eligible.device + ): + raise ValueError( + "env_mask must be bool and match the runtime batch/device." + ) + if type(reason) is not str or not reason: + raise ValueError("reason must be a non-empty string.") + changed = env_mask & self._eligible + self._require_runner().deactivate_rows( + changed, + reason=reason, + ) + self._cancelled |= changed + self._eligible &= ~changed + barrier = self._recovery_barrier + if barrier is not None and changed.any(): + barrier.success_mask &= ~changed + retained_items: deque[_WorkflowRecoveryWorkItem] = deque() + for item in barrier.work_items: + retained = item.env_mask & ~changed + if retained.any(): + retained_items.append( + _WorkflowRecoveryWorkItem( + role=item.role, + call=item.call, + env_mask=retained, + attempt_index=item.attempt_index, + ) + ) + barrier.work_items = retained_items + if not self._eligible.any(): + recovery_item = self._active_recovery_item + runner_step = self._require_runner().cancel(reason) + self._consume_runner_step(runner_step) + finished = self._finish_active_call(runner_step) + if recovery_item is None: + self._call_traces.append(finished.trace) + else: + self._append_workflow_recovery_trace( + recovery_item, + call=finished.trace, + completed_mask=finished.completed_mask, + failed_mask=finished.failed_mask, + message=finished.message, + ) + self._status = ( + SkillStatus.CANCELLED + if runner_step.status is RunnerStatus.CANCELLED + else SkillStatus.FAILED + ) + if self._status is SkillStatus.FAILED: + failed = self._call_entered_mask & ~self._cancelled + self._failed |= failed + self._recovery_barrier = None + self._current_call_index = None + self._wait_duration = 0.0 + return self.result + + def adopt_verified_task_state(self, task_state: TaskState) -> SkillResult: + """Install a verified state snapshot between independent workflows. + + Parallel coordinators use this explicit barrier operation after + deterministically merging branch-local effects. Running workflows + cannot replace their runner-owned state. + """ + if self._status is SkillStatus.RUNNING: + raise RuntimeError("Cannot replace task state while a workflow is running.") + if not isinstance(task_state, TaskState): + raise TypeError("task_state must be a TaskState.") + if ( + task_state.batch_size != self._task_state.batch_size + or task_state.device != self._task_state.device + ): + raise ValueError("task_state must match the runtime batch and device.") + self._task_state = _snapshot_task_state(task_state) + return self.result + + @property + def _has_next_call(self) -> bool: + assert self._current_call_index is not None + return self._current_call_index + 1 < self._execution_prefix_length + + @staticmethod + def _normalize_execution_prefix_length( + value: int | None, + *, + call_count: int, + ) -> int: + """Normalize a non-empty execution prefix inside one analysis window.""" + if value is None: + return call_count + if type(value) is not int: + raise TypeError("execution_prefix_length must be an integer or None.") + if not 1 <= value <= call_count: + raise ValueError( + "execution_prefix_length must be in " f"[1, {call_count}], got {value}." + ) + return value + + def _normalize_calls( + self, + supplied: tuple[SemanticCallSpec | Iterable[SemanticCallSpec], ...], + ) -> tuple[SemanticCallSpec, ...]: + """Normalize varargs and one explicit iterable to the same compiler path.""" + if len(supplied) == 1 and not isinstance(supplied[0], SemanticCallSpec): + candidate = supplied[0] + if isinstance(candidate, (str, bytes)): + raise TypeError("calls must contain SemanticCallSpec values.") + try: + calls = tuple(candidate) + except TypeError as exc: + raise TypeError( + "A single run argument must be a SemanticCallSpec or iterable." + ) from exc + else: + calls = tuple(supplied) + if not calls: + raise ValueError("A semantic workflow requires at least one call.") + if not all(isinstance(call, SemanticCallSpec) for call in calls): + raise TypeError("calls must contain SemanticCallSpec values.") + return calls + + def _reset_workflow( + self, + calls: tuple[SemanticCallSpec, ...], + workflow: object, + *, + workflow_id: str, + eligible_mask: torch.Tensor | None, + execution_prefix_length: int, + ) -> None: + """Reset per-run state while retaining verified symbolic state.""" + if eligible_mask is None: + eligible = torch.ones( + self._task_state.batch_size, + dtype=torch.bool, + device=self._task_state.device, + ) + else: + if not isinstance(eligible_mask, torch.Tensor): + raise TypeError("eligible_mask must be a torch.Tensor or None.") + if eligible_mask.dtype != torch.bool or eligible_mask.shape != ( + self._task_state.batch_size, + ): + raise ValueError( + "eligible_mask must be bool with shape " + f"({self._task_state.batch_size},)." + ) + eligible = eligible_mask.to(self._task_state.device).clone() + if not eligible.any(): + raise ValueError("eligible_mask must contain at least one active row.") + self._workflow = workflow + self._workflow_id = workflow_id + self._calls = calls + self._execution_prefix_length = execution_prefix_length + self._current_call_index = 0 + self._runner = None + self._grounded = None + self._active_call = None + self._active_recovery_item = None + self._recovery_barrier = None + self._eligible = eligible + self._success = torch.zeros_like(eligible) + self._failed = torch.zeros_like(eligible) + self._cancelled = torch.zeros_like(eligible) + self._events = [] + self._call_traces = [] + self._effect_traces = [] + self._workflow_recovery_traces = [] + self._failures = [] + self._call_event_offset = 0 + self._call_effect_offset = 0 + self._observation_revision = 0 + self._next_guard_verification_id = 0 + self._next_gate_verification_id = 0 + self._next_workflow_recovery_id = 0 + self._wait_duration = 0.0 + self._message = None + self._status = SkillStatus.RUNNING + + def _observe_for_grounding(self) -> PlanningContext: + """Capture and normalize one fresh context for JIT lowering.""" + context = self._observation_provider.observe(self._task_state) + if not isinstance(context, PlanningContext): + raise TypeError( + "ObservationProvider.observe() must return PlanningContext." + ) + normalized = PlanningContext( + robot=context.robot, + task=self._task_state, + scene=context.scene, + env_ids=context.env_ids, + control_dt=context.control_dt, + ) + if normalized.batch_size != self._task_state.batch_size: + raise ValueError( + "Observation batch size changed during semantic execution." + ) + if normalized.robot.qpos.device != self._task_state.device: + raise ValueError("Observation and verified TaskState must share a device.") + if self._has_observed_env_ids: + if normalized.env_ids.device != self._env_ids.device or not torch.equal( + normalized.env_ids, + self._env_ids, + ): + raise ValueError( + "Observation env_ids must remain stable across call barriers." + ) + else: + self._env_ids = normalized.env_ids.clone() + self._has_observed_env_ids = True + return normalized + + def _prepare_call(self, call_index: int) -> None: + """Freshly ground and create exactly one session and runner.""" + assert self._workflow is not None + self._prepare_grounded_call( + self._workflow, + analysis_call_index=call_index, + workflow_call_index=call_index, + call=self._calls[call_index], + active_mask=self._eligible, + recovery_item=None, + ) + + def _prepare_recovery_work_item( + self, + item: _WorkflowRecoveryWorkItem, + ) -> None: + """Analyze and ground one real recovery call with fresh observation.""" + barrier = self._require_recovery_barrier() + suffix = self._calls[barrier.trigger_call_index :] + analysis_calls = ( + (item.call, *suffix) + if item.role is SkillWorkflowRecoveryRole.REACQUIRE + else suffix + ) + workflow = self._compiler.analyze( + analysis_calls, + workflow_id=( + f"{self._workflow_id}:workflow_recovery:" + f"{self._next_workflow_recovery_id}" + ), + ) + self._prepare_grounded_call( + workflow, + analysis_call_index=0, + workflow_call_index=barrier.trigger_call_index, + call=item.call, + active_mask=item.env_mask, + recovery_item=item, + ) + + def _prepare_grounded_call( + self, + workflow: object, + *, + analysis_call_index: int, + workflow_call_index: int, + call: SemanticCallSpec, + active_mask: torch.Tensor, + recovery_item: _WorkflowRecoveryWorkItem | None, + ) -> None: + """Install one original or recovery semantic call in a fresh session.""" + context = self._observe_for_grounding() + grounded = self._compiler.ground( + workflow, + analysis_call_index, + context, + eligible_mask=active_mask, + ) + invocation = getattr(grounded, "invocation", 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", ())) + effect_gates = tuple(getattr(grounded, "effect_gates", ())) + if invocation is None: + raise TypeError("Semantic compiler ground() must return an invocation.") + if not isinstance(grounded_eligible, torch.Tensor) or not torch.equal( + grounded_eligible, + active_mask, + ): + raise ValueError("Grounded call must preserve runtime eligibility.") + if (effect_spec is None) != (effect_monitor is None): + raise ValueError( + "Grounded effect_spec and effect_monitor must be set together." + ) + if effect_spec is not None: + if not isinstance(effect_spec, SemanticEffectSpec): + raise TypeError("Grounded effect_spec must be a SemanticEffectSpec.") + if not isinstance(effect_monitor, EffectMonitor): + raise TypeError("Grounded effect_monitor must be an EffectMonitor.") + if effect_spec.env_ids.device != context.env_ids.device or not torch.equal( + effect_spec.env_ids, + 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.") + if not all(type(value) is GroundedPhaseEffectGate for value in effect_gates): + raise TypeError( + "Grounded effect_gates must contain exact " + "GroundedPhaseEffectGate values." + ) + if effect_gates and effect_spec is None: + raise ValueError("Grounded phase-effect gates require an effect spec.") + + self._grounded = grounded + session = self._engine.start( + (invocation,), + context, + eligible_mask=active_mask, + ) + primed = _PrimedObservationProvider(context, self._observation_provider) + runner = ExecutionRunner( + session, + primed, + self._command_sink, + clock=self._clock, + cfg=self._runner_cfg, + ) + self._current_call_index = workflow_call_index + self._runner = runner + self._active_call = call + self._active_recovery_item = recovery_item + self._call_entered_mask = active_mask.clone() + self._call_event_offset = len(self._events) + self._call_effect_offset = len(self._effect_traces) + self._wait_duration = 0.0 + + def _effect_verifier( + self, + context: PlanningContext, + request: EffectVerificationRequest, + ) -> EffectVerificationResult: + """Collect raw evidence and feed the grounded call's monitor.""" + grounded = self._require_grounded() + spec = getattr(grounded, "effect_spec", None) + monitor = getattr(grounded, "effect_monitor", None) + if not isinstance(spec, SemanticEffectSpec) or not isinstance( + monitor, + EffectMonitor, + ): + raise RuntimeError( + "The active atomic plan requested effect verification, but its " + "semantic call has no grounded effect monitor." + ) + if request.skill_id != spec.skill_id: + raise ValueError("Effect request skill_id does not match the effect spec.") + if request.invocation_id != spec.invocation_id: + raise ValueError( + "Effect request invocation_id does not match the effect spec." + ) + 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, + ) + expectation_decisions = self._validated_expectation_decisions( + spec, + decision, + ) + invalidation_mask, retry_mask = self._terminal_failure_policy( + grounded, + decision.failure_mask, + expectation_decisions, + ) + return EffectVerificationResult( + verification_id=request.verification_id, + success_mask=decision.success_mask, + failure_mask=decision.failure_mask, + invalidation_mask=invalidation_mask, + retry_mask=retry_mask, + expectation_results=tuple( + EffectExpectationResult( + expectation_id=value.expectation_id, + satisfied_mask=value.satisfied_mask, + contradicted_mask=value.contradicted_mask, + inverse_satisfied_mask=value.inverse_satisfied_mask, + ) + for value in expectation_decisions + ), + ) + + @staticmethod + def _validated_expectation_decisions( + spec: SemanticEffectSpec, + decision: EffectMonitorDecision, + ) -> tuple[EffectExpectationDecision, ...]: + """Require one current-observation outcome per physical expectation.""" + physical_ids = tuple( + expectation.expectation_id + for expectation in spec.state_expectations + if any( + clause.expectation_id == expectation.expectation_id + for clause in spec.clauses + ) + ) + outcomes = tuple(decision.expectation_decisions) + if not outcomes and len(physical_ids) == 1: + outcomes = ( + EffectExpectationDecision( + expectation_id=physical_ids[0], + satisfied_mask=decision.success_mask, + contradicted_mask=decision.failure_mask, + inverse_satisfied_mask=torch.zeros_like(decision.failure_mask), + ), + ) + outcome_ids = tuple(value.expectation_id for value in outcomes) + if outcome_ids != physical_ids: + raise ValueError( + "Effect monitor must return one ordered outcome for every " + f"physical expectation; expected={physical_ids}, got={outcome_ids}." + ) + return outcomes + + @staticmethod + def _terminal_failure_policy( + grounded: object, + failure_mask: torch.Tensor, + expectation_decisions: tuple[EffectExpectationDecision, ...], + ) -> tuple[torch.Tensor, torch.Tensor]: + """Select fail-closed invalidation and safe local retry rows.""" + call = getattr(getattr(grounded, "analyzed", None), "call", None) + invalidation = failure_mask.clone() + retry = failure_mask.clone() + if type(call) is Pick: + return invalidation, retry + if type(call) is Place: + source = next( + value + for value in expectation_decisions + if value.expectation_id == "source" + ) + retained = failure_mask & source.inverse_satisfied_mask + return failure_mask & ~retained, retained + if type(call) is HandOver: + source = next( + value + for value in expectation_decisions + if value.expectation_id == "source" + ) + retained = failure_mask & source.inverse_satisfied_mask + return failure_mask & ~retained, torch.zeros_like(failure_mask) + return invalidation, retry + + def _phase_effect_gate_verifier( + self, + context: PlanningContext, + request: PhaseEffectGateRequest, + ) -> PhaseEffectGateResult: + """Observe one blocking segment-entry effect on a fresh due cycle.""" + grounded = self._require_grounded() + gates = tuple(getattr(grounded, "effect_gates", ())) + matches = tuple(value for value in gates if value.gate_id == request.gate_id) + if len(matches) != 1: + raise RuntimeError( + f"Grounded call must own exactly one phase-effect gate " + f"{request.gate_id!r}." + ) + gate = matches[0] + if gate.segment_name != request.segment_name: + raise ValueError( + "Phase-effect gate request segment does not match its grounded " + "monitor." + ) + session = self._require_runner().session + monitor_request = EffectVerificationRequest( + verification_id=self._next_gate_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=request.requested_at, + deadline=request.deadline, + env_mask=request.env_mask, + expected_effects=self._phase_effect_gate_expected_effects( + gate, + session.active_plan.expected_effects, + ), + ) + self._next_gate_verification_id += 1 + decision = self._observe_effect_monitor( + context, + monitor_request, + spec=gate.effect_spec, + monitor=gate.effect_monitor, + boundary_kind="phase_effect_gate", + gate_id=gate.gate_id, + segment_name=gate.segment_name, + ) + return PhaseEffectGateResult( + verification_id=request.verification_id, + gate_id=request.gate_id, + attempt_generation=request.attempt_generation, + invocation_index=request.invocation_index, + next_waypoint_index=request.next_waypoint_index, + success_mask=decision.success_mask, + failure_mask=decision.failure_mask, + retry_mask=( + decision.failure_mask + if gate.retry_action + else torch.zeros_like(decision.failure_mask) + ), + message=( + f"Physical evidence contradicted gate {gate.gate_id!r} before " + f"segment {gate.segment_name!r}." + if decision.failure_mask.any() + else "" + ), + ) + + @staticmethod + def _phase_effect_gate_expected_effects( + gate: GroundedPhaseEffectGate, + action_effects: StateDelta, + ) -> StateDelta: + """Project the action-owned held relation required by one gate.""" + expectation = gate.effect_spec.state_expectations[0] + if type(expectation) is not HeldObjectStateExpectation: + raise TypeError("Built-in phase-effect gates require held-object state.") + key = expectation.task_state_key + if key not in action_effects.held_object_updates: + raise ValueError(f"Active action does not declare gate state key {key!r}.") + candidate = action_effects.held_object_updates[key] + if expectation.relation is HeldObjectRelation.ATTACHED: + if not isinstance(candidate, HeldObjectState): + raise ValueError( + f"Attached gate {gate.gate_id!r} requires an action-owned " + "HeldObjectState candidate." + ) + elif candidate is not None: + raise ValueError( + f"Detached gate {gate.gate_id!r} requires an action-owned removal." + ) + return StateDelta(held_object_updates={key: candidate}) + + 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, + gate_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)] + evidence = self._evidence_collector.collect( + spec, + timestamp=context.robot.timestamp, + observation_revision=observation_revision, + env_ids=selected_env_ids, + ) + observed = monitor.observe(request, evidence) + expectation_decisions = self._validated_expectation_decisions( + spec, + observed, + ) + decision = EffectMonitorDecision( + success_mask=observed.success_mask, + failure_mask=observed.failure_mask, + expectation_decisions=expectation_decisions, + ) + analyzed = getattr(grounded, "analyzed", None) + monitor_ref = getattr(analyzed, "effect_monitor_ref", None) + if monitor_ref is not None and not isinstance(monitor_ref, EffectMonitorRef): + raise TypeError("Grounded effect monitor reference must be typed.") + if monitor_ref is None: + monitor_id = f"{type(monitor).__module__}.{type(monitor).__qualname__}" + monitor_revision = None + configured_monitor_params: Mapping[str, object] = {} + else: + monitor_id = monitor_ref.monitor_id + monitor_revision = monitor_ref.revision + configured_monitor_params = monitor_ref.params + resolved_monitor_params = monitor.resolved_params + if not isinstance(resolved_monitor_params, Mapping): + raise TypeError("EffectMonitor.resolved_params must return a mapping.") + trace = SkillEffectTrace( + call_index=self._require_call_index(), + verification_id=request.verification_id, + observation_revision=observation_revision, + timestamp=context.robot.timestamp, + success_mask=decision.success_mask, + failure_mask=decision.failure_mask, + expectation_decisions=decision.expectation_decisions, + effect_spec=spec, + monitor_id=monitor_id, + monitor_revision=monitor_revision, + configured_monitor_params=configured_monitor_params, + resolved_monitor_params=resolved_monitor_params, + evidence=evidence, + boundary_kind=boundary_kind, + guard_id=guard_id, + gate_id=gate_id, + segment_name=segment_name, + ) + self._effect_traces.append(trace) + return decision + + def _consume_runner_step(self, runner_step: RunnerStep) -> None: + """Merge one runner update into workflow-level traces.""" + self._wait_duration = runner_step.wait_duration + if runner_step.tick is not None: + self._task_state = _snapshot_task_state(runner_step.tick.task_state) + self._events.extend( + _snapshot_event(event) for event in runner_step.tick.events + ) + if runner_step.message: + self._message = runner_step.message + + def _finish_active_call(self, runner_step: RunnerStep) -> _FinishedCallAttempt: + """Project one terminal session without deciding workflow eligibility.""" + runner = self._require_runner() + grounded = self._require_grounded() + call_index = self._require_call_index() + call = self._active_call + if not isinstance(call, SemanticCallSpec): + raise RuntimeError("No semantic call is associated with the active runner.") + self._task_state = _snapshot_task_state(runner.session.task_state) + after = runner.session.eligible_mask + invocation = getattr(grounded, "invocation") + if runner_step.status is RunnerStatus.COMPLETED: + completed = self._call_entered_mask & after + failed = self._call_entered_mask & ~after & ~self._cancelled + elif runner_step.status is RunnerStatus.CANCELLED: + completed = torch.zeros_like(self._call_entered_mask) + failed = torch.zeros_like(self._call_entered_mask) + else: + completed = torch.zeros_like(self._call_entered_mask) + failed = self._call_entered_mask & ~self._cancelled + plan_attempts = tuple( + SkillPlanAttemptTrace.from_execution_attempt( + attempt, + profile_id=grounded.analyzed.bound.robot_profile.profile_id, + preset_id=grounded.analyzed.bound.preset.preset_id, + preset_schema_version=grounded.analyzed.bound.preset.schema_version, + ) + for attempt in runner.session.plan_attempts + ) + trace = SkillCallTrace( + call_index=call_index, + semantic_id=call.semantic_id, + call_metadata=call.to_metadata(), + skill_id=invocation.skill_id, + invocation_id=invocation.invocation_id, + invocation_revision=invocation.revision, + status=runner_step.status, + entered_mask=self._call_entered_mask, + completed_mask=completed, + failed_mask=failed, + command_count=runner_step.command_count, + resolved_core_policy=plan_attempts[-1].resolved_core_policy, + plan_attempts=plan_attempts, + events=tuple(self._events[self._call_event_offset :]), + effects=tuple(self._effect_traces[self._call_effect_offset :]), + ) + self._runner = None + self._grounded = None + self._active_call = None + self._active_recovery_item = None + return _FinishedCallAttempt( + trace=trace, + completed_mask=completed, + failed_mask=failed, + status=runner_step.status, + message=runner_step.message, + ) + + def _workflow_recovery_trigger(self) -> _WorkflowRecoveryTrigger | None: + """Resolve preset policy and the failed call's physical source endpoint.""" + call = self._active_call + if type(call) is Place: + source_slot = "primary" + elif type(call) is HandOver: + source_slot = "source" + else: + return None + grounded = self._require_grounded() + preset = grounded.analyzed.bound.preset + policy = preset.workflow_recovery_policy + if type(policy) is not WorkflowRecoveryPolicy: + raise TypeError("Grounded preset workflow_recovery_policy must be exact.") + if policy.max_recovery_attempts == 0: + return None + endpoints = tuple( + endpoint + for endpoint in grounded.invocation.binding.endpoints + if endpoint.slot_id == source_slot + ) + resource_ids = {endpoint.resource_id for endpoint in endpoints} + task_state_keys = {endpoint.task_state_key for endpoint in endpoints} + if not endpoints or len(resource_ids) != 1 or len(task_state_keys) != 1: + raise RuntimeError( + f"Workflow recovery requires one physical source resource and " + f"task-state key for slot {source_slot!r}." + ) + return _WorkflowRecoveryTrigger( + policy=policy, + source_resource_id=next(iter(resource_ids)), + source_task_state_key=next(iter(task_state_keys)), + ) + + def _active_call_requires_workflow_recovery(self) -> bool: + """Whether the active call emitted a row-local external-recovery hand-off.""" + entered = self._call_entered_mask + return any( + event.kind is ExecutionEventKind.RECOVERY_REQUIRED + and bool((event.env_mask.to(entered.device) & entered).any().item()) + for event in self._events[self._call_event_offset :] + ) + + @staticmethod + def _recovery_required_mask(trace: SkillCallTrace) -> torch.Tensor: + """Return failed rows explicitly handed to workflow recovery by core.""" + required = torch.zeros_like(trace.failed_mask) + for event in trace.events: + if event.kind is ExecutionEventKind.RECOVERY_REQUIRED: + required |= event.env_mask.to(required.device) + return required & trace.failed_mask + + def _handle_original_call_finished( + self, + finished: _FinishedCallAttempt, + *, + trigger: _WorkflowRecoveryTrigger | None, + ) -> None: + """Either advance one call barrier or start bounded row-local recovery.""" + if finished.status is RunnerStatus.CANCELLED: + cancelled = finished.trace.entered_mask & self._eligible + self._cancelled |= cancelled + self._eligible &= ~cancelled + self._finish_workflow_terminal() + return + recovery_required = self._recovery_required_mask(finished.trace) + recoverable = ( + torch.zeros_like(recovery_required) + if trigger is None + else recovery_required + ) + if not recoverable.any(): + self._complete_original_call_barrier( + success_mask=finished.completed_mask, + failure_mask=finished.failed_mask, + message=finished.message, + ) + return + assert trigger is not None + permanent_failure = finished.failed_mask & ~recoverable + call_index = self._require_call_index() + barrier = _WorkflowRecoveryBarrier( + trigger_call_index=call_index, + trigger_call=self._calls[call_index], + policy=trigger.policy, + source_resource_id=trigger.source_resource_id, + source_task_state_key=trigger.source_task_state_key, + entered_mask=finished.trace.entered_mask.clone(), + success_mask=finished.completed_mask.clone(), + final_failure_mask=permanent_failure.clone(), + attempt_counts=torch.zeros_like( + finished.trace.entered_mask, + dtype=torch.long, + ), + work_items=deque(), + failure_messages=( + [finished.message or "Semantic call failed for some rows."] + if permanent_failure.any() + else [] + ), + ) + self._recovery_barrier = barrier + self._eligible = (barrier.success_mask | recoverable) & ~self._cancelled + self._failed |= permanent_failure + self._schedule_recovery_cycle(recoverable) + self._start_next_recovery_work_item_or_finish() + + def _handle_recovery_call_finished( + self, + item: _WorkflowRecoveryWorkItem, + finished: _FinishedCallAttempt, + ) -> None: + """Update one recovery cohort and retain the shared call barrier.""" + barrier = self._require_recovery_barrier() + self._append_workflow_recovery_trace( + item, + call=finished.trace, + completed_mask=finished.completed_mask, + failed_mask=finished.failed_mask, + message=finished.message, + ) + if finished.status is RunnerStatus.CANCELLED: + cancelled = item.env_mask & self._eligible + self._cancelled |= cancelled + self._eligible &= ~cancelled + elif item.role is SkillWorkflowRecoveryRole.REACQUIRE: + if finished.completed_mask.any(): + barrier.work_items.append( + _WorkflowRecoveryWorkItem( + role=SkillWorkflowRecoveryRole.RETRY_REACQUIRED, + call=barrier.trigger_call, + env_mask=finished.completed_mask, + attempt_index=item.attempt_index, + ) + ) + if finished.failed_mask.any(): + self._schedule_recovery_cycle(finished.failed_mask) + else: + barrier.success_mask |= finished.completed_mask + if finished.failed_mask.any(): + recovery_required = self._recovery_required_mask(finished.trace) + permanent = finished.failed_mask & ~recovery_required + self._record_permanent_recovery_failure( + permanent, + finished.message + or "The retried semantic call failed without a recovery hand-off.", + ) + self._schedule_recovery_cycle(recovery_required) + self._start_next_recovery_work_item_or_finish() + + def _schedule_recovery_cycle(self, requested_mask: torch.Tensor) -> None: + """Consume one per-row budget and enqueue retained/reacquire cohorts.""" + barrier = self._require_recovery_barrier() + requested = requested_mask & self._eligible & ~self._cancelled + allowed = requested & ( + barrier.attempt_counts < barrier.policy.max_recovery_attempts + ) + exhausted = requested & ~allowed + self._record_permanent_recovery_failure( + exhausted, + "Workflow recovery exhausted its per-row attempt budget.", + ) + if not allowed.any(): + return + barrier.attempt_counts[allowed] += 1 + for attempt_index in range(1, barrier.policy.max_recovery_attempts + 1): + cohort = allowed & (barrier.attempt_counts == attempt_index) + if not cohort.any(): + continue + retained = self._retained_source_mask(cohort) + reacquire = cohort & ~retained + if retained.any(): + barrier.work_items.append( + _WorkflowRecoveryWorkItem( + role=SkillWorkflowRecoveryRole.RETRY_RETAINED, + call=barrier.trigger_call, + env_mask=retained, + attempt_index=attempt_index, + ) + ) + if reacquire.any(): + barrier.work_items.append( + _WorkflowRecoveryWorkItem( + role=SkillWorkflowRecoveryRole.REACQUIRE, + call=self._reacquisition_call(barrier), + env_mask=reacquire, + attempt_index=attempt_index, + ) + ) + + def _retained_source_mask(self, env_mask: torch.Tensor) -> torch.Tensor: + """Return rows whose reconciled symbolic state proves source retention.""" + barrier = self._require_recovery_barrier() + held = self._task_state.get_held_object(barrier.source_task_state_key) + if not isinstance(held, HeldObjectState): + return torch.zeros_like(env_mask) + trigger_object = getattr(barrier.trigger_call, "object", None) + object_id = getattr(trigger_object, "entity_id", None) + if held.semantics.entity_id != object_id: + return torch.zeros_like(env_mask) + active = ( + torch.ones_like(env_mask) + if held.env_mask is None + else held.env_mask.to(env_mask.device) + ) + return env_mask & active + + def _reacquisition_call(self, barrier: _WorkflowRecoveryBarrier) -> Pick: + """Derive a real Pick using the failed call's resolved source resource.""" + trigger_object = getattr(barrier.trigger_call, "object", None) + if type(trigger_object) is not SceneObjectRef: + raise TypeError("Curated workflow recovery requires a SceneObjectRef.") + grasp: SceneAffordanceRef | None = None + for candidate in reversed(self._calls[: barrier.trigger_call_index]): + if ( + type(candidate) is Pick + and candidate.object.entity_id == trigger_object.entity_id + ): + grasp = candidate.grasp + break + return Pick( + object=SceneObjectRef(trigger_object.entity_id), + grasp=(None if grasp is None else SceneAffordanceRef(grasp.entity_id)), + resources={"primary": barrier.source_resource_id}, + ) + + def _start_next_recovery_work_item_or_finish(self) -> None: + """Start the next non-empty cohort or close the recovered call barrier.""" + barrier = self._require_recovery_barrier() + while barrier.work_items: + queued = barrier.work_items.popleft() + active = queued.env_mask & self._eligible & ~self._cancelled + if not active.any(): + continue + item = _WorkflowRecoveryWorkItem( + role=queued.role, + call=queued.call, + env_mask=active, + attempt_index=queued.attempt_index, + ) + try: + self._prepare_recovery_work_item(item) + except Exception as exc: # noqa: BLE001 - row-local recovery failure + message = ( + f"Could not prepare workflow recovery call " + f"{item.call.semantic_id!r}: {type(exc).__name__}: {exc}" + ) + self._append_workflow_recovery_trace( + item, + call=None, + completed_mask=torch.zeros_like(item.env_mask), + failed_mask=item.env_mask, + message=message, + ) + self._record_permanent_recovery_failure(item.env_mask, message) + self._runner = None + self._grounded = None + self._active_call = None + self._active_recovery_item = None + continue + return + self._finish_recovery_barrier() + + def _append_workflow_recovery_trace( + self, + item: _WorkflowRecoveryWorkItem, + *, + call: SkillCallTrace | None, + completed_mask: torch.Tensor, + failed_mask: torch.Tensor, + message: str | None, + ) -> None: + """Append one immutable recovery-call trace with stable correlation.""" + barrier = self._require_recovery_barrier() + self._workflow_recovery_traces.append( + SkillWorkflowRecoveryTrace( + recovery_id=self._next_workflow_recovery_id, + trigger_call_index=barrier.trigger_call_index, + trigger_semantic_id=barrier.trigger_call.semantic_id, + attempt_index=item.attempt_index, + max_recovery_attempts=barrier.policy.max_recovery_attempts, + role=item.role, + source_resource_id=barrier.source_resource_id, + source_task_state_key=barrier.source_task_state_key, + entered_mask=item.env_mask, + completed_mask=completed_mask, + failed_mask=failed_mask, + call=call, + message=message, + ) + ) + self._next_workflow_recovery_id += 1 + + def _record_permanent_recovery_failure( + self, + env_mask: torch.Tensor, + message: str, + ) -> None: + """Remove exhausted rows while leaving other recovery cohorts active.""" + if not env_mask.any(): + return + barrier = self._require_recovery_barrier() + barrier.final_failure_mask |= env_mask + barrier.failure_messages.append(message) + self._failed |= env_mask + self._eligible &= ~env_mask + + def _finish_recovery_barrier(self) -> None: + """Rejoin recovered rows and advance the original program counter once.""" + barrier = self._require_recovery_barrier() + unresolved = ( + barrier.entered_mask + & ~barrier.success_mask + & ~barrier.final_failure_mask + & ~self._cancelled + ) + if unresolved.any(): + self._record_permanent_recovery_failure( + unresolved, + "Workflow recovery ended with unresolved rows.", + ) + success = barrier.success_mask & ~self._cancelled + failure = barrier.final_failure_mask & ~self._cancelled + message = ( + None + if not failure.any() + else "; ".join(dict.fromkeys(barrier.failure_messages)) + ) + self._recovery_barrier = None + self._complete_original_call_barrier( + success_mask=success, + failure_mask=failure, + message=message, + ) + + def _complete_original_call_barrier( + self, + *, + success_mask: torch.Tensor, + failure_mask: torch.Tensor, + message: str | None, + ) -> None: + """Commit final row outcomes and advance exactly one original call.""" + call_index = self._require_call_index() + call = self._calls[call_index] + failure = failure_mask & ~self._cancelled + self._failed |= failure + self._eligible = success_mask & ~self._failed & ~self._cancelled + if failure.any(): + failure_message = message or "Semantic call failed for these rows." + self._failures.append( + SkillFailure( + call_index=call_index, + semantic_id=call.semantic_id, + env_mask=failure, + message=failure_message, + ) + ) + self._message = failure_message + elif self._workflow_recovery_traces: + self._message = None + if self._eligible.any() and self._has_next_call: + next_index = call_index + 1 + try: + self._prepare_call(next_index) + except Exception as exc: # noqa: BLE001 - preserve workflow trace + self._fail_preparation(next_index, exc) + elif self._eligible.any(): + self._success = self._eligible.clone() + self._status = SkillStatus.COMPLETED + self._current_call_index = None + self._wait_duration = 0.0 + else: + self._finish_workflow_terminal() + + def _finish_workflow_terminal(self) -> None: + """Choose one terminal status from final row-local outcomes.""" + self._status = ( + SkillStatus.CANCELLED + if self._cancelled.any() and not self._failed.any() + else SkillStatus.FAILED + ) + self._current_call_index = None + self._wait_duration = 0.0 + + def _fail_preparation(self, call_index: int, exc: Exception) -> None: + """Convert a post-barrier grounding failure to a terminal result.""" + failed = self._eligible.clone() + self._failed |= failed + self._eligible &= ~failed + semantic_id = self._calls[call_index].semantic_id + message = ( + f"Could not prepare semantic call {call_index} ({semantic_id!r}): " + f"{type(exc).__name__}: {exc}" + ) + self._failures.append(SkillFailure(call_index, semantic_id, failed, message)) + self._append_preparation_failure_trace(call_index, failed) + self._message = message + self._status = SkillStatus.FAILED + self._current_call_index = None + self._runner = None + self._grounded = None + self._active_call = None + self._active_recovery_item = None + self._recovery_barrier = None + self._wait_duration = 0.0 + + def _append_preparation_failure_trace( + self, + call_index: int, + failed_mask: torch.Tensor, + ) -> None: + """Record statically resolved policy choices when planning never starts.""" + grounded = self._grounded + analyzed = getattr(grounded, "analyzed", None) + invocation = getattr(grounded, "invocation", None) + if analyzed is None: + workflow_calls = getattr(self._workflow, "calls", ()) + if call_index < len(workflow_calls): + analyzed = workflow_calls[call_index] + bound = getattr(analyzed, "bound", None) + if bound is None: + return + try: + profile = bound.robot_profile + preset = bound.preset + action_binding = ( + bound.binding.action_binding + if invocation is None + else invocation.binding + ) + resolved = ResolvedCorePolicyTrace.from_resolved_binding( + profile_id=profile.profile_id, + preset_id=preset.preset_id, + preset_schema_version=preset.schema_version, + motion_policy=( + preset.motion_policy + if invocation is None + else invocation.motion_policy + ), + tracking_policy=( + preset.tracking_policy + if invocation is None + else invocation.tracking_policy + ), + recovery_policy=( + preset.recovery_policy + if invocation is None + else invocation.recovery_policy + ), + endpoints=action_binding.endpoints, + ) + skill_id = bound.linked.descriptor.skill_id + except (AttributeError, TypeError, ValueError): + return + self._call_traces.append( + SkillCallTrace( + call_index=call_index, + semantic_id=self._calls[call_index].semantic_id, + call_metadata=self._calls[call_index].to_metadata(), + skill_id=skill_id, + invocation_id=( + None if invocation is None else invocation.invocation_id + ), + invocation_revision=(0 if invocation is None else invocation.revision), + status=RunnerStatus.FAILED, + entered_mask=failed_mask, + completed_mask=torch.zeros_like(failed_mask), + failed_mask=failed_mask, + command_count=0, + resolved_core_policy=resolved, + plan_attempts=(), + ) + ) + + def _abort(self, reason: str) -> None: + """Safe-stop the active runner and mark remaining rows failed.""" + if self._runner is not None: + recovery_item = self._active_recovery_item + safe_stop_step = self._runner.cancel(reason) + runner_step = replace( + safe_stop_step, + status=RunnerStatus.FAILED, + message=reason, + ) + self._consume_runner_step(runner_step) + finished = self._finish_active_call(runner_step) + if recovery_item is None: + self._call_traces.append(finished.trace) + elif self._recovery_barrier is not None: + self._append_workflow_recovery_trace( + recovery_item, + call=finished.trace, + completed_mask=finished.completed_mask, + failed_mask=finished.failed_mask, + message=reason, + ) + failed = self._eligible.clone() + self._failed |= failed + self._eligible &= ~failed + if failed.any() and self._calls: + call_index = min( + self._current_call_index or 0, + len(self._calls) - 1, + ) + self._failures.append( + SkillFailure( + call_index, + self._calls[call_index].semantic_id, + failed, + reason, + ) + ) + self._message = reason + self._status = SkillStatus.FAILED + self._recovery_barrier = None + self._current_call_index = None + self._wait_duration = 0.0 + + def _require_runner(self) -> ExecutionRunner: + if self._runner is None: + raise RuntimeError("No semantic call runner is active.") + return self._runner + + def _require_grounded(self) -> object: + if self._grounded is None: + raise RuntimeError("No grounded semantic call is active.") + return self._grounded + + def _require_recovery_barrier(self) -> _WorkflowRecoveryBarrier: + if self._recovery_barrier is None: + raise RuntimeError("No workflow-recovery barrier is active.") + return self._recovery_barrier + + def _require_call_index(self) -> int: + if self._current_call_index is None: + raise RuntimeError("No semantic call is active.") + return self._current_call_index + + +class SkillScene: + """Typed convenience lookup surface backed by one immutable registry.""" + + def __init__(self, registry: SceneRegistry) -> None: + if not isinstance(registry, SceneRegistry): + raise TypeError("registry must be a SceneRegistry.") + self._registry = registry + + @property + def registry(self) -> SceneRegistry: + """Return the authoritative scene registry.""" + return self._registry + + def entity(self, identifier: str | SceneEntityRef) -> SceneEntityRef: + """Resolve any registered semantic entity.""" + return self._registry.resolve(identifier) + + def object(self, identifier: str | SceneObjectRef) -> SceneObjectRef: + """Resolve a registered semantic object.""" + return self._registry.resolve(identifier, expected_type=SceneObjectRef) + + def articulation( + self, + identifier: str | SceneArticulationRef, + ) -> SceneArticulationRef: + """Resolve a registered articulation.""" + return self._registry.resolve(identifier, expected_type=SceneArticulationRef) + + def link(self, identifier: str | SceneLinkRef) -> SceneLinkRef: + """Resolve a registered articulation link.""" + return self._registry.resolve(identifier, expected_type=SceneLinkRef) + + def affordance( + self, + identifier: str | SceneAffordanceRef, + ) -> SceneAffordanceRef: + """Resolve a registered semantic affordance.""" + return self._registry.resolve(identifier, expected_type=SceneAffordanceRef) + + +class AtomicSkills: + """Small application-facing facade over :class:`SkillRuntime`.""" + + def __init__(self, runtime: SkillRuntime) -> None: + if not isinstance(runtime, SkillRuntime): + raise TypeError("runtime must be a SkillRuntime.") + self._runtime = runtime + self._scene = SkillScene(runtime.scene_registry) + + @classmethod + def from_components( + cls, + compiler: SemanticSkillCompiler, + observation_provider: ObservationProvider, + command_sink: CommandSink, + evidence_collector: EffectEvidenceCollectorPort, + *, + task_state: TaskState | None = None, + clock: ExecutionClock | None = None, + runner_cfg: ExecutionRunnerCfg | None = None, + ) -> AtomicSkills: + """Build a facade from explicit compiler and runtime ports.""" + return cls( + SkillRuntime.from_components( + compiler, + observation_provider, + command_sink, + evidence_collector, + task_state=task_state, + clock=clock, + runner_cfg=runner_cfg, + ) + ) + + @classmethod + def from_env(cls, env: object, *, preset: str = "safe") -> AtomicSkills: + """Build through an explicitly installed environment integration adapter. + + The method deliberately does not inspect generic environment attributes + for robots, scenes, controllers, or managers. An environment integration + must implement :class:`SkillRuntimeProvider` and own those decisions. + """ + if type(preset) is not str or not preset: + raise ValueError("preset must be a non-empty string.") + if not isinstance(env, SkillRuntimeProvider): + raise TypeError( + "Environment has no semantic-skill integration adapter. Install " + "SkillRuntimeProvider.create_skill_runtime(*, preset=...) or use " + "AtomicSkills.from_components(...) with explicit ports." + ) + runtime = env.create_skill_runtime(preset=preset) + if not isinstance(runtime, SkillRuntime): + raise TypeError( + "SkillRuntimeProvider.create_skill_runtime() must return " + "SkillRuntime." + ) + return cls(runtime) + + @property + def runtime(self) -> SkillRuntime: + """Return the canonical runtime for advanced step-wise use.""" + return self._runtime + + @property + def scene(self) -> SkillScene: + """Return typed semantic scene lookup helpers.""" + return self._scene + + @property + def result(self) -> SkillResult: + """Return the current immutable runtime result.""" + return self._runtime.result + + def start( + self, + *calls: SemanticCallSpec | Iterable[SemanticCallSpec], + workflow_id: str = "semantic_workflow", + eligible_mask: torch.Tensor | None = None, + execution_prefix_length: int | None = None, + ) -> SkillResult: + """Start non-blocking semantic execution without exposing sessions.""" + return self._runtime.start( + *calls, + workflow_id=workflow_id, + eligible_mask=eligible_mask, + execution_prefix_length=execution_prefix_length, + ) + + def step(self) -> SkillResult: + """Advance non-blocking execution by one due runner cycle.""" + return self._runtime.step() + + def run( + self, + *calls: SemanticCallSpec | Iterable[SemanticCallSpec], + workflow_id: str = "semantic_workflow", + eligible_mask: torch.Tensor | None = None, + execution_prefix_length: int | None = None, + max_steps: int = 100_000, + ) -> SkillResult: + """Synchronously execute calls without exposing core runtime objects.""" + return self._runtime.run( + *calls, + workflow_id=workflow_id, + eligible_mask=eligible_mask, + execution_prefix_length=execution_prefix_length, + max_steps=max_steps, + ) + + def cancel( + self, reason: str = "Semantic workflow cancelled by caller." + ) -> SkillResult: + """Cancel and safe-stop the active semantic workflow.""" + return self._runtime.cancel(reason) + + +__all__ = [ + "AtomicSkills", + "EffectEvidenceCollectorPort", + "ResolvedCorePolicyTrace", + "SkillCallTrace", + "SkillEndpointBindingTrace", + "SkillEndpointTrackingChannelTrace", + "SkillEffectTrace", + "SkillFailure", + "SkillPlanAttemptTrace", + "SkillResult", + "SkillRuntime", + "SkillRuntimeProvider", + "SkillScene", + "SkillStatus", + "SkillWorkflowRecoveryRole", + "SkillWorkflowRecoveryTrace", + "task_state_to_metadata", +] diff --git a/embodichain/lab/sim/skills/scene.py b/embodichain/lab/sim/skills/scene.py new file mode 100644 index 000000000..16dc7ee4e --- /dev/null +++ b/embodichain/lab/sim/skills/scene.py @@ -0,0 +1,2248 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Authoritative scene identity and registration value contracts.""" + +from __future__ import annotations + +from collections.abc import Hashable, Iterable, Iterator, Mapping +from copy import deepcopy +from dataclasses import dataclass, field, fields, is_dataclass, replace +from enum import Enum +import math +from types import MappingProxyType +from typing import Any, Protocol, TYPE_CHECKING, TypeVar, runtime_checkable + +import torch + +from embodichain.lab.sim.common import BatchEntity +from embodichain.lab.sim.atomic_actions import ( + Affordance, + AntipodalAffordance, + ArticulationOperationAffordance, + EntityState, + ObjectSemantics, + ObservedArticulationJointState, + SceneProvider, + SceneSnapshot, +) +from .effects import EffectEvidenceAddress + +if TYPE_CHECKING: + from embodichain.lab.sim.planners import MotionGenerator + from embodichain.lab.sim.sim_manager import SimulationManager + + +RefT = TypeVar("RefT", bound="SceneEntityRef") + +GRASP_AFFORDANCE_CAPABILITY = "affordance.grasp" +"""Capability for an affordance usable by object pickup or handover.""" + +ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY = "affordance.articulation.operation" +"""Capability for a typed handle-driven articulation operation.""" + +PLACE_ON_AFFORDANCE_CAPABILITY = "affordance.place.on" +"""Capability for an affordance that defines an ``on`` placement relation.""" + +PLACE_IN_AFFORDANCE_CAPABILITY = "affordance.place.in" +"""Capability for an affordance that defines an ``inside`` placement relation.""" + +PLACEMENT_TARGET_AFFORDANCE_REVISION = "1" +"""Schema revision for built-in support/container object-target frames.""" + +SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID = "builtin.scene_articulation" +"""Stable route for explicitly injected articulation-joint observations.""" + +SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION = "1" +"""Exact contract revision for articulation-joint evidence addresses.""" + + +@dataclass(frozen=True, slots=True) +class ArticulationJointEvidenceAddress(EffectEvidenceAddress): + """Canonical scene articulation and joint observation address.""" + + articulation_id: str + joint_id: str + + def __post_init__(self) -> None: + _validate_identifier(self.articulation_id, "articulation_id") + _validate_identifier(self.joint_id, "joint_id") + + @property + def address_fingerprint(self) -> Hashable: + """Return the exact provider-independent joint address.""" + return type(self), self.articulation_id, self.joint_id + + +class UnsupportedSceneAffordanceError(ValueError): + """Raised when a parent has no affordance for a required capability.""" + + +class AmbiguousSceneAffordanceError(ValueError): + """Raised when compatible affordances lack one explicitly scoped default.""" + + +@dataclass +class SupportSurfaceAffordance(Affordance): + """Typed target frame for placing an object's origin on a support surface. + + The registered affordance pose is the desired object pose, expressed + relative to its parent scene entity. The optional confidence threshold is + enforced whenever that late-bound target pose is resolved. + + Args: + minimum_confidence: Minimum confidence accepted while resolving the + late-bound target pose. + """ + + minimum_confidence: float = 0.0 + + def __post_init__(self) -> None: + if isinstance(self.minimum_confidence, bool) or not isinstance( + self.minimum_confidence, + (int, float), + ): + raise TypeError("minimum_confidence must be a number.") + self.minimum_confidence = float(self.minimum_confidence) + if not 0.0 <= self.minimum_confidence <= 1.0: + raise ValueError("minimum_confidence must be in [0, 1].") + + +@dataclass +class ContainerAffordance(Affordance): + """Typed target frame for placing an object's origin inside a container. + + The registered affordance pose is the desired object pose, expressed + relative to its parent scene entity. The optional confidence threshold is + enforced whenever that late-bound target pose is resolved. + + Args: + minimum_confidence: Minimum confidence accepted while resolving the + late-bound target pose. + """ + + minimum_confidence: float = 0.0 + + def __post_init__(self) -> None: + if isinstance(self.minimum_confidence, bool) or not isinstance( + self.minimum_confidence, + (int, float), + ): + raise TypeError("minimum_confidence must be a number.") + self.minimum_confidence = float(self.minimum_confidence) + if not 0.0 <= self.minimum_confidence <= 1.0: + raise ValueError("minimum_confidence must be in [0, 1].") + + +def _validate_identifier(value: str, name: str) -> None: + """Validate an exact, non-empty identifier without normalizing it.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError(f"{name} must be a non-empty string without outer whitespace.") + + +def _normalize_affordance_capabilities( + values: Iterable[str], +) -> frozenset[str]: + """Validate one open set of namespaced affordance capabilities.""" + if isinstance(values, (str, bytes)): + raise TypeError( + "affordance_capabilities must be an iterable of strings, not a string." + ) + try: + capabilities = frozenset(values) + except TypeError as exc: + raise TypeError( + "affordance_capabilities must be an iterable of strings." + ) from exc + for capability in capabilities: + _validate_identifier(capability, "affordance capability") + return capabilities + + +def _normalize_default_affordances( + values: Mapping[str, SceneAffordanceRef], +) -> Mapping[str, SceneAffordanceRef]: + """Validate and own a capability-scoped default-affordance mapping.""" + if not isinstance(values, Mapping): + raise TypeError("default_affordances must be a mapping.") + defaults: dict[str, SceneAffordanceRef] = {} + for capability, affordance_ref in values.items(): + _validate_identifier(capability, "default affordance capability") + if type(affordance_ref) is not SceneAffordanceRef: + raise TypeError( + "default_affordances values must be SceneAffordanceRef instances." + ) + defaults[capability] = affordance_ref + return MappingProxyType(defaults) + + +@dataclass(frozen=True, slots=True) +class SceneEntityRef: + """Typed reference to one authoritative scene-registry entity. + + Args: + entity_id: Globally stable canonical registry identifier. + """ + + entity_id: str + """Globally stable authoritative registry identifier.""" + + def __post_init__(self) -> None: + _validate_identifier(self.entity_id, "entity_id") + + +@dataclass(frozen=True, slots=True) +class SceneObjectRef(SceneEntityRef): + """Reference to one object registered in the semantic scene.""" + + +@dataclass(frozen=True, slots=True) +class SceneArticulationRef(SceneEntityRef): + """Reference to one articulation registered in the semantic scene.""" + + +@dataclass(frozen=True, slots=True) +class SceneLinkRef(SceneEntityRef): + """Reference to one registered articulation link.""" + + +@dataclass(frozen=True, slots=True) +class SceneAffordanceRef(SceneEntityRef): + """Reference to one registered interaction affordance.""" + + +class SceneDynamics(str, Enum): + """Physical mobility classification owned by a scene registration.""" + + UNKNOWN = "unknown" + STATIC = "static" + KINEMATIC = "kinematic" + DYNAMIC = "dynamic" + + +class SceneCollisionRole(str, Enum): + """How an entity participates in the planner collision world.""" + + NONE = "none" + STATIC = "static" + DYNAMIC = "dynamic" + + +class SceneCollisionWorldMode(str, Enum): + """Batch-sharing policy for a dynamic planner collision world.""" + + SHARED = "shared" + PER_ENV = "per_env" + + +@dataclass(frozen=True, slots=True) +class SceneEntityMetadata: + """Provider-free semantic metadata projected from one registration. + + Args: + ref: Canonical typed entity reference. + aliases: Boundary aliases, compared as an order-independent set. + parent: Canonical parent for links and affordances. + native_name: Backend-local child name. + dynamics: Physical mobility classification. + collision_role: Planner collision classification. + semantic_type: Optional application semantic type. + affordance_capabilities: Open capabilities of an affordance. + default_affordances: Capability-scoped direct-child defaults. + affordance_payload_type: Exact registered affordance value type. + affordance_revision: Integrator-owned payload revision or fingerprint. + relative_pose: Flattened parent-relative 4x4 pose, when declared. + """ + + ref: SceneEntityRef + aliases: tuple[str, ...] = () + parent: SceneEntityRef | None = None + native_name: str | None = None + dynamics: SceneDynamics = SceneDynamics.UNKNOWN + collision_role: SceneCollisionRole = SceneCollisionRole.NONE + semantic_type: str | None = None + affordance_capabilities: frozenset[str] = frozenset() + default_affordances: Mapping[str, SceneAffordanceRef] = field(default_factory=dict) + affordance_payload_type: type[Affordance] | None = None + affordance_revision: str | None = None + relative_pose: tuple[float, ...] | None = None + + def __post_init__(self) -> None: + allowed_ref_types = { + SceneEntityRef, + SceneObjectRef, + SceneArticulationRef, + SceneLinkRef, + SceneAffordanceRef, + } + if type(self.ref) not in allowed_ref_types: + raise TypeError("SceneEntityMetadata.ref must be a SceneEntityRef.") + if isinstance(self.aliases, (str, bytes)): + raise TypeError("SceneEntityMetadata.aliases must be an iterable.") + aliases = tuple(sorted(set(self.aliases))) + for alias in aliases: + _validate_identifier(alias, "scene alias") + object.__setattr__(self, "aliases", aliases) + if self.parent is not None and type(self.parent) not in allowed_ref_types: + raise TypeError("SceneEntityMetadata.parent must be a SceneEntityRef.") + if self.native_name is not None: + _validate_identifier(self.native_name, "native_name") + if not isinstance(self.dynamics, SceneDynamics): + raise TypeError("SceneEntityMetadata.dynamics must be SceneDynamics.") + if not isinstance(self.collision_role, SceneCollisionRole): + raise TypeError( + "SceneEntityMetadata.collision_role must be SceneCollisionRole." + ) + if self.semantic_type is not None: + _validate_identifier(self.semantic_type, "semantic_type") + object.__setattr__( + self, + "affordance_capabilities", + _normalize_affordance_capabilities(self.affordance_capabilities), + ) + object.__setattr__( + self, + "default_affordances", + _normalize_default_affordances(self.default_affordances), + ) + if self.affordance_payload_type is not None and ( + not isinstance(self.affordance_payload_type, type) + or not issubclass(self.affordance_payload_type, Affordance) + ): + raise TypeError( + "affordance_payload_type must be an Affordance subclass or None." + ) + if self.affordance_revision is not None: + _validate_identifier(self.affordance_revision, "affordance_revision") + if self.relative_pose is not None: + relative_pose = tuple(float(value) for value in self.relative_pose) + if len(relative_pose) != 16 or not all( + math.isfinite(value) for value in relative_pose + ): + raise ValueError( + "SceneEntityMetadata.relative_pose must contain 16 finite values." + ) + object.__setattr__(self, "relative_pose", relative_pose) + self._validate_topology() + + def _validate_topology(self) -> None: + """Apply the typed topology contract without requiring live providers.""" + if isinstance(self.ref, (SceneObjectRef, SceneArticulationRef)): + if self.parent is not None or self.native_name is not None: + raise ValueError( + "Object and articulation metadata cannot declare a parent " + "or native_name." + ) + if self.affordance_capabilities or self.affordance_payload_type is not None: + raise ValueError( + "Object and articulation metadata cannot declare affordance " + "payload capabilities." + ) + if self.affordance_revision is not None or self.relative_pose is not None: + raise ValueError( + "Object and articulation metadata cannot declare affordance " + "revision or relative_pose." + ) + return + if isinstance(self.ref, SceneLinkRef): + if not isinstance(self.parent, SceneArticulationRef) or ( + self.native_name is None + ): + raise ValueError( + "Link metadata requires an articulation parent and native_name." + ) + if self.affordance_capabilities or self.affordance_payload_type is not None: + raise ValueError( + "Link metadata cannot declare affordance payload capabilities." + ) + if self.affordance_revision is not None or self.relative_pose is not None: + raise ValueError( + "Link metadata cannot declare affordance revision or relative_pose." + ) + return + if isinstance(self.ref, SceneAffordanceRef): + if ( + not isinstance( + self.parent, + (SceneObjectRef, SceneArticulationRef, SceneLinkRef), + ) + or self.native_name is None + ): + raise ValueError( + "Affordance metadata requires an object, articulation, or link " + "parent and native_name." + ) + if self.affordance_payload_type is None: + raise ValueError( + "Affordance metadata requires affordance_payload_type." + ) + if self.default_affordances: + raise ValueError( + "Affordance metadata cannot declare default_affordances." + ) + if self.affordance_capabilities and self.affordance_revision is None: + raise ValueError( + "Capability-bearing affordance metadata requires an explicit " + "affordance_revision." + ) + if ( + GRASP_AFFORDANCE_CAPABILITY in self.affordance_capabilities + and not issubclass(self.affordance_payload_type, AntipodalAffordance) + ): + raise TypeError( + f"{GRASP_AFFORDANCE_CAPABILITY!r} requires an " + "AntipodalAffordance payload." + ) + if ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY + in self.affordance_capabilities + and not issubclass( + self.affordance_payload_type, + ArticulationOperationAffordance, + ) + ): + raise TypeError( + f"{ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY!r} requires " + "an ArticulationOperationAffordance payload." + ) + return + if self.parent is not None or self.native_name is not None: + raise ValueError("Generic scene metadata cannot declare a parent.") + if self.affordance_capabilities or self.affordance_payload_type is not None: + raise ValueError( + "Generic scene metadata cannot declare affordance capabilities." + ) + if self.default_affordances: + raise ValueError( + "Generic scene metadata cannot declare default_affordances." + ) + if self.affordance_revision is not None or self.relative_pose is not None: + raise ValueError( + "Generic scene metadata cannot declare affordance revision or pose." + ) + + @classmethod + def from_registration( + cls, + registration: SceneEntityRegistration, + ) -> SceneEntityMetadata: + """Project semantic metadata without copying a live payload/provider.""" + relative_pose = registration.relative_pose + return cls( + ref=registration.ref, + aliases=registration.aliases, + parent=registration.parent, + native_name=registration.native_name, + dynamics=registration.dynamics, + collision_role=registration.collision_role, + semantic_type=registration.semantic_type, + affordance_capabilities=registration.affordance_capabilities, + default_affordances=registration.default_affordances, + affordance_payload_type=( + None + if registration.affordance is None + else type(registration.affordance) + ), + affordance_revision=registration.affordance_revision, + relative_pose=( + None + if relative_pose is None + else tuple(relative_pose.detach().cpu().reshape(-1).tolist()) + ), + ) + + +@dataclass(frozen=True, slots=True, init=False) +class _SceneMetadataIndex: + """Shared identity and affordance index for static and live scene catalogs.""" + + entries: tuple[SceneEntityMetadata, ...] + by_id: Mapping[str, SceneEntityMetadata] + aliases: Mapping[str, str] + affordances_by_parent_capability: Mapping[ + tuple[str, str], tuple[SceneAffordanceRef, ...] + ] + + def __init__(self, entries: Iterable[SceneEntityMetadata] = ()) -> None: + try: + supplied = tuple(entries) + except TypeError as exc: + raise TypeError("entries must be an iterable of scene metadata.") from exc + if not all(isinstance(entry, SceneEntityMetadata) for entry in supplied): + raise TypeError("entries must contain SceneEntityMetadata values.") + + by_id: dict[str, SceneEntityMetadata] = {} + for entry in supplied: + entity_id = entry.ref.entity_id + if entity_id in by_id: + raise ValueError(f"Duplicate canonical scene entity ID {entity_id!r}.") + by_id[entity_id] = entry + + aliases: dict[str, str] = {} + canonical_ids = set(by_id) + for entry in supplied: + canonical_id = entry.ref.entity_id + for alias in entry.aliases: + if alias in canonical_ids: + raise ValueError( + f"Scene alias {alias!r} collides with canonical entity ID " + f"{alias!r}." + ) + previous = aliases.get(alias) + if previous is not None: + raise ValueError( + f"Scene alias {alias!r} is ambiguous between canonical " + f"IDs {previous!r} and {canonical_id!r}." + ) + aliases[alias] = canonical_id + + self._validate_relationships(supplied, by_id) + affordances = self._index_affordances(supplied) + object.__setattr__(self, "entries", supplied) + object.__setattr__(self, "by_id", MappingProxyType(by_id)) + object.__setattr__(self, "aliases", MappingProxyType(aliases)) + object.__setattr__( + self, + "affordances_by_parent_capability", + MappingProxyType(affordances), + ) + + @staticmethod + def _validate_relationships( + entries: tuple[SceneEntityMetadata, ...], + by_id: Mapping[str, SceneEntityMetadata], + ) -> None: + """Validate canonical parents, native members, and scoped defaults.""" + native_members: dict[tuple[type[SceneEntityRef], str, str], str] = {} + for entry in entries: + parent = entry.parent + if parent is None: + continue + if parent.entity_id == entry.ref.entity_id: + raise ValueError( + f"Scene entity {entry.ref.entity_id!r} cannot parent itself." + ) + parent_entry = by_id.get(parent.entity_id) + if parent_entry is None: + raise ValueError( + f"Scene entity {entry.ref.entity_id!r} references " + f"unregistered parent {parent.entity_id!r}." + ) + if type(parent_entry.ref) is not type(parent): + raise TypeError( + f"Parent {parent.entity_id!r} is registered as " + f"{type(parent_entry.ref).__name__}, not " + f"{type(parent).__name__}." + ) + if isinstance(entry.ref, (SceneLinkRef, SceneAffordanceRef)): + assert entry.native_name is not None + member_key = ( + type(entry.ref), + parent.entity_id, + entry.native_name, + ) + previous = native_members.get(member_key) + if previous is not None: + raise ValueError( + f"{type(entry.ref).__name__} parent {parent.entity_id!r} " + f"and native_name {entry.native_name!r} are already " + f"registered as canonical ID {previous!r}." + ) + native_members[member_key] = entry.ref.entity_id + + for entry in entries: + for capability, default_ref in entry.default_affordances.items(): + default_entry = by_id.get(default_ref.entity_id) + if default_entry is None: + raise ValueError( + f"Scene entity {entry.ref.entity_id!r} declares unknown " + f"default affordance {default_ref.entity_id!r} for " + f"capability {capability!r}." + ) + if not isinstance(default_entry.ref, SceneAffordanceRef): + raise TypeError( + f"Default affordance {default_ref.entity_id!r} is " + f"registered as {type(default_entry.ref).__name__}, not " + "SceneAffordanceRef." + ) + if default_entry.parent != entry.ref: + actual_parent = default_entry.parent + raise ValueError( + f"Default affordance {default_ref.entity_id!r} is not a " + f"direct child of {entry.ref.entity_id!r}; its parent is " + f"{None if actual_parent is None else actual_parent.entity_id!r}." + ) + if capability not in default_entry.affordance_capabilities: + raise ValueError( + f"Default affordance {default_ref.entity_id!r} does not " + f"declare capability {capability!r}." + ) + + @staticmethod + def _index_affordances( + entries: tuple[SceneEntityMetadata, ...], + ) -> dict[tuple[str, str], tuple[SceneAffordanceRef, ...]]: + """Build deterministic parent/capability reverse lookup entries.""" + mutable: dict[tuple[str, str], list[SceneAffordanceRef]] = {} + for entry in entries: + if not isinstance(entry.ref, SceneAffordanceRef): + continue + assert entry.parent is not None + for capability in entry.affordance_capabilities: + mutable.setdefault((entry.parent.entity_id, capability), []).append( + entry.ref + ) + return { + key: tuple(sorted(refs, key=lambda ref: ref.entity_id)) + for key, refs in mutable.items() + } + + def resolve( + self, + identifier: str | SceneEntityRef, + *, + expected_type: type[RefT] = SceneEntityRef, + ) -> RefT: + """Resolve one canonical ID, alias, or typed reference.""" + if not isinstance(expected_type, type) or not issubclass( + expected_type, + SceneEntityRef, + ): + raise TypeError("expected_type must be a SceneEntityRef subclass.") + if isinstance(identifier, SceneEntityRef): + canonical_id = identifier.entity_id + supplied_ref: SceneEntityRef | None = identifier + elif isinstance(identifier, str): + _validate_identifier(identifier, "identifier") + canonical_id = self.aliases.get(identifier, identifier) + supplied_ref = None + else: + raise TypeError("identifier must be a string or SceneEntityRef.") + + entry = self.by_id.get(canonical_id) + if entry is None: + raise KeyError(f"Unknown scene entity {identifier!r}.") + canonical_ref = entry.ref + if supplied_ref is not None and type(supplied_ref) is not type(canonical_ref): + raise TypeError( + f"Scene entity {canonical_id!r} is registered as " + f"{type(canonical_ref).__name__}, not " + f"{type(supplied_ref).__name__}." + ) + if not isinstance(canonical_ref, expected_type): + raise TypeError( + f"Scene entity {canonical_id!r} is " + f"{type(canonical_ref).__name__}, not {expected_type.__name__}." + ) + return canonical_ref # type: ignore[return-value] + + def affordances( + self, + parent: str | SceneEntityRef, + *, + capability: str, + ) -> tuple[SceneAffordanceRef, ...]: + """Return compatible direct-child affordances without selecting one.""" + parent_ref = self.resolve(parent) + _validate_identifier(capability, "affordance capability") + return self.affordances_by_parent_capability.get( + (parent_ref.entity_id, capability), + (), + ) + + def resolve_affordance( + self, + parent: str | SceneEntityRef, + *, + capability: str, + explicit: str | SceneAffordanceRef | None = None, + ) -> SceneAffordanceRef: + """Select one compatible affordance using strict scoped defaults.""" + parent_ref = self.resolve(parent) + candidates = self.affordances(parent_ref, capability=capability) + if explicit is not None: + try: + selected = self.resolve(explicit, expected_type=SceneAffordanceRef) + except (KeyError, TypeError, ValueError) as exc: + raise UnsupportedSceneAffordanceError( + f"Explicit affordance {explicit!r} is not a registered " + "SceneAffordanceRef." + ) from exc + entry = self.by_id[selected.entity_id] + if entry.parent != parent_ref: + raise UnsupportedSceneAffordanceError( + f"Affordance {selected.entity_id!r} is not a direct child of " + f"{parent_ref.entity_id!r}." + ) + if capability not in entry.affordance_capabilities: + raise UnsupportedSceneAffordanceError( + f"Affordance {selected.entity_id!r} does not support " + f"capability {capability!r}." + ) + return selected + if not candidates: + raise UnsupportedSceneAffordanceError( + f"Scene entity {parent_ref.entity_id!r} has no affordance for " + f"capability {capability!r}." + ) + if len(candidates) == 1: + return candidates[0] + default = self.by_id[parent_ref.entity_id].default_affordances.get(capability) + if default is not None: + return self.resolve(default, expected_type=SceneAffordanceRef) + raise AmbiguousSceneAffordanceError( + f"Scene entity {parent_ref.entity_id!r} has multiple affordances for " + f"capability {capability!r}: " + f"{[candidate.entity_id for candidate in candidates]}. Configure " + "default_affordances for this parent and capability or select one " + "explicitly." + ) + + +@runtime_checkable +class SceneEntityStateProvider(Protocol): + """Observe one registered entity for an ordered environment batch.""" + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + """Return the entity state whose rows follow ``env_ids``. + + Args: + timestamp: Observation timestamp supplied by the integration. + env_ids: Stable ordered environment correlation IDs. + + Returns: + Current pose and confidence for the registered entity. + """ + + +@runtime_checkable +class SceneArticulationJointStateProvider(Protocol): + """Observe canonical joints for one registered scene articulation.""" + + def observe_joints( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> Mapping[str, ObservedArticulationJointState]: + """Return live joint observations whose rows follow ``env_ids``.""" + + +@runtime_checkable +class SceneGeometryProvider(Protocol): + """Provide one entity's planner-facing collision geometry descriptor.""" + + def get_geometry(self) -> object: + """Return the planner-facing geometry descriptor. + + Returns: + Backend-consumable geometry or a live simulation entity. + """ + + +@dataclass(frozen=True, slots=True, eq=False) +class SceneEntityRegistration: + """Immutable integration metadata for one authoritative scene entity. + + Parent relationships, simulator-native names, pose sources, geometry, and + affordances belong to the registry registration rather than the lightweight + reference copied into semantic calls. + + Args: + ref: Canonical typed reference. + state_provider: Optional dynamic pose/confidence source. + joint_state_provider: Optional live articulation-joint source. + aliases: External names normalized at the registry boundary. + parent: Canonical parent for a link or affordance. + native_name: Backend-local member name under ``parent``. + dynamics: Physical mobility classification. + geometry_provider: Planner-facing collision geometry source. + collision_role: Static, dynamic, or no planner collision role. + semantic_type: Optional application semantic type. + affordance: Affordance value for an affordance registration. + affordance_capabilities: Open semantic operations supported by an + affordance registration. + default_affordances: Capability-to-child mapping owned by a parent + object, articulation, or link registration. + affordance_revision: Stable integrator-owned revision or fingerprint for + capability-bearing affordance payload data. + relative_pose: Optional parent-relative affordance transform. + """ + + ref: SceneEntityRef + """Canonical typed reference owned by the registry.""" + + state_provider: SceneEntityStateProvider | None = None + """Explicit dynamic pose/confidence source.""" + + aliases: tuple[str, ...] = () + """External or legacy names normalized once at the registry boundary.""" + + parent: SceneEntityRef | None = None + """Canonical parent reference for a link or affordance.""" + + native_name: str | None = None + """Backend-local link or affordance name under ``parent``.""" + + dynamics: SceneDynamics = SceneDynamics.UNKNOWN + """Static, kinematic, dynamic, or unknown mobility classification.""" + + geometry_provider: SceneGeometryProvider | None = None + """Collision geometry source required for planner collision roles.""" + + collision_role: SceneCollisionRole = SceneCollisionRole.NONE + """Static/dynamic planner-obstacle role, or ``none``.""" + + semantic_type: str | None = None + """Optional application semantic type such as ``container`` or ``tool``.""" + + affordance: Affordance | None = None + """Affordance value owned by a :class:`SceneAffordanceRef` registration.""" + + affordance_capabilities: frozenset[str] = frozenset() + """Open semantic capabilities declared by an affordance registration.""" + + default_affordances: Mapping[str, SceneAffordanceRef] = field(default_factory=dict) + """Capability-scoped child affordances selected when multiple are valid.""" + + affordance_revision: str | None = None + """Stable payload revision required by capability-bearing affordances.""" + + relative_pose: torch.Tensor | None = None + """Optional parent-relative pose when no explicit state provider exists.""" + + joint_state_provider: SceneArticulationJointStateProvider | None = None + """Explicit live joint source for an articulation registration.""" + + def __post_init__(self) -> None: + if type(self.ref) not in { + SceneEntityRef, + SceneObjectRef, + SceneArticulationRef, + SceneLinkRef, + SceneAffordanceRef, + }: + raise TypeError("ref must be a SceneEntityRef.") + if self.state_provider is not None and not isinstance( + self.state_provider, + SceneEntityStateProvider, + ): + raise TypeError("state_provider must implement SceneEntityStateProvider.") + if self.joint_state_provider is not None and not isinstance( + self.joint_state_provider, + SceneArticulationJointStateProvider, + ): + raise TypeError( + "joint_state_provider must implement " + "SceneArticulationJointStateProvider." + ) + + if isinstance(self.aliases, (str, bytes)): + raise TypeError("aliases must be an iterable of identifiers, not a string.") + try: + aliases = tuple(self.aliases) + except TypeError as exc: + raise TypeError("aliases must be an iterable of identifiers.") from exc + for alias in aliases: + _validate_identifier(alias, "alias") + aliases = tuple(alias for alias in aliases if alias != self.ref.entity_id) + if len(set(aliases)) != len(aliases): + raise ValueError("aliases must be unique.") + object.__setattr__(self, "aliases", aliases) + + if self.parent is not None and type(self.parent) not in { + SceneEntityRef, + SceneObjectRef, + SceneArticulationRef, + SceneLinkRef, + SceneAffordanceRef, + }: + raise TypeError("parent must be a SceneEntityRef or None.") + if self.native_name is not None: + _validate_identifier(self.native_name, "native_name") + if not isinstance(self.dynamics, SceneDynamics): + raise TypeError("dynamics must be a SceneDynamics value.") + if not isinstance(self.collision_role, SceneCollisionRole): + raise TypeError("collision_role must be a SceneCollisionRole value.") + if self.geometry_provider is not None and not isinstance( + self.geometry_provider, + SceneGeometryProvider, + ): + raise TypeError("geometry_provider must implement SceneGeometryProvider.") + if self.semantic_type is not None: + _validate_identifier(self.semantic_type, "semantic_type") + if self.affordance is not None and not isinstance(self.affordance, Affordance): + raise TypeError("affordance must be an Affordance or None.") + object.__setattr__( + self, + "affordance_capabilities", + _normalize_affordance_capabilities(self.affordance_capabilities), + ) + object.__setattr__( + self, + "default_affordances", + _normalize_default_affordances(self.default_affordances), + ) + if self.affordance_revision is not None: + _validate_identifier(self.affordance_revision, "affordance_revision") + if self.relative_pose is not None: + if not isinstance(self.relative_pose, torch.Tensor): + raise TypeError("relative_pose must be a torch.Tensor or None.") + if self.relative_pose.shape != (4, 4): + raise ValueError("relative_pose must have shape (4, 4).") + object.__setattr__(self, "relative_pose", self.relative_pose.clone()) + if self.state_provider is not None and self.relative_pose is not None: + raise ValueError( + "state_provider and relative_pose are mutually exclusive pose sources." + ) + + self._validate_reference_contract() + SceneEntityMetadata.from_registration(self) + if ( + self.collision_role is not SceneCollisionRole.NONE + and self.geometry_provider is None + ): + raise ValueError( + f"Collision entity {self.ref.entity_id!r} requires geometry_provider." + ) + + def _validate_reference_contract(self) -> None: + """Validate fields whose meaning follows from the typed ref.""" + if isinstance(self.ref, (SceneObjectRef, SceneArticulationRef)): + if self.parent is not None: + raise ValueError( + "Object and articulation registrations cannot have a parent." + ) + if self.native_name is not None: + raise ValueError( + "Object and articulation registrations cannot have native_name." + ) + if self.state_provider is None: + raise ValueError( + "Object and articulation registrations require state_provider." + ) + if self.relative_pose is not None: + raise ValueError( + "Object and articulation registrations cannot use relative_pose." + ) + if self.affordance is not None: + raise ValueError( + "Affordance values require a SceneAffordanceRef registration." + ) + if self.affordance_capabilities: + raise ValueError( + "affordance_capabilities require a SceneAffordanceRef " + "registration." + ) + if ( + isinstance(self.ref, SceneObjectRef) + and self.joint_state_provider is not None + ): + raise ValueError( + "joint_state_provider requires a SceneArticulationRef " + "registration." + ) + return + + if isinstance(self.ref, SceneLinkRef): + if self.joint_state_provider is not None: + raise ValueError( + "joint_state_provider requires a SceneArticulationRef " + "registration." + ) + if ( + not isinstance(self.parent, SceneArticulationRef) + or self.native_name is None + ): + raise ValueError("Link registrations require parent and native_name.") + if self.state_provider is None: + raise ValueError("Link registrations require state_provider.") + if self.relative_pose is not None: + raise ValueError("Link registrations cannot use relative_pose.") + if self.affordance is not None: + raise ValueError( + "Affordance values require a SceneAffordanceRef registration." + ) + if self.affordance_capabilities: + raise ValueError( + "affordance_capabilities require a SceneAffordanceRef " + "registration." + ) + return + + if isinstance(self.ref, SceneAffordanceRef): + if self.joint_state_provider is not None: + raise ValueError( + "joint_state_provider requires a SceneArticulationRef " + "registration." + ) + if ( + not isinstance( + self.parent, + (SceneObjectRef, SceneArticulationRef, SceneLinkRef), + ) + or self.native_name is None + ): + raise ValueError( + "Affordance registrations require parent and native_name." + ) + if self.affordance is None: + raise ValueError("Affordance registrations require affordance.") + if self.state_provider is None and self.relative_pose is None: + raise ValueError( + "Affordance registrations require state_provider or relative_pose." + ) + if self.default_affordances: + raise ValueError( + "An affordance registration cannot declare default_affordances." + ) + return + + if self.parent is not None or self.native_name is not None: + raise ValueError("Generic entity registrations cannot declare a parent.") + if self.joint_state_provider is not None: + raise ValueError( + "joint_state_provider requires a SceneArticulationRef registration." + ) + if self.state_provider is None: + raise ValueError("Generic entity registrations require state_provider.") + if self.affordance_capabilities: + raise ValueError( + "affordance_capabilities require a SceneAffordanceRef registration." + ) + if self.default_affordances: + raise ValueError( + "Only object, articulation, or link registrations may declare " + "default_affordances." + ) + + +def _copy_registration( + registration: SceneEntityRegistration, +) -> SceneEntityRegistration: + """Copy registry metadata without cloning live providers or entities.""" + relative_pose = registration.relative_pose + return replace( + registration, + affordance=_copy_affordance(registration.affordance), + relative_pose=relative_pose.clone() if relative_pose is not None else None, + ) + + +def _copy_affordance(affordance: Affordance | None) -> Affordance | None: + """Own mutable affordance metadata while preserving live entity handles.""" + if affordance is None: + return None + memo: dict[int, object] = {} + visited: set[int] = set() + + def visit(value: object) -> None: + value_id = id(value) + if value_id in visited: + return + visited.add(value_id) + if isinstance(value, BatchEntity): + memo[value_id] = value + return + if is_dataclass(value) and not isinstance(value, type): + for data_field in fields(value): + nested = getattr(value, data_field.name) + if data_field.name == "_generator" and nested is not None: + memo[id(nested)] = None + else: + visit(nested) + return + if isinstance(value, Mapping): + for key, nested in value.items(): + visit(key) + visit(nested) + return + if isinstance(value, (list, tuple, set, frozenset)): + for nested in value: + visit(nested) + + visit(affordance) + try: + copied = deepcopy(affordance, memo) + except Exception as exc: # noqa: BLE001 - normalize opaque metadata failures + raise TypeError( + f"Affordance {type(affordance).__name__} must contain copyable " + "registry metadata." + ) from exc + if copied is affordance or type(copied) is not type(affordance): + raise TypeError( + f"Affordance {type(affordance).__name__} must deepcopy to a distinct " + "value of the exact same type." + ) + return copied + + +@dataclass(frozen=True, slots=True, eq=False, init=False) +class SceneRegistry: + """Immutable authoritative catalog of semantic scene entities. + + Canonical identifiers occupy one flat, globally unique namespace. Aliases + are accepted only at lookup and integration boundaries and always resolve + to a canonical typed reference before they leave the registry. + + Args: + registrations: Complete scene registrations. The iterable is copied and + cannot be extended after construction. + collision_world_mode: Explicit dynamic-collision batch policy. It may be + omitted for a single environment, which resolves to ``shared``. A + multi-environment dynamic world must select a mode explicitly. + """ + + _registrations: tuple[SceneEntityRegistration, ...] = field(repr=False) + _registrations_by_id: Mapping[str, SceneEntityRegistration] = field(repr=False) + _metadata_index: _SceneMetadataIndex = field(repr=False) + _collision_world_entity_ids: tuple[str, ...] = field(repr=False) + _dynamic_collision_entity_ids: tuple[str, ...] = field(repr=False) + _static_collision_entity_ids: tuple[str, ...] = field(repr=False) + collision_world_mode: SceneCollisionWorldMode | None + + def __init__( + self, + registrations: Iterable[SceneEntityRegistration] = (), + *, + collision_world_mode: SceneCollisionWorldMode | None = None, + ) -> None: + if collision_world_mode is not None and not isinstance( + collision_world_mode, + SceneCollisionWorldMode, + ): + raise TypeError( + "collision_world_mode must be a SceneCollisionWorldMode or None." + ) + try: + supplied = tuple(registrations) + except TypeError as exc: + raise TypeError("registrations must be an iterable.") from exc + if not all(isinstance(item, SceneEntityRegistration) for item in supplied): + raise TypeError( + "registrations must contain SceneEntityRegistration values." + ) + owned = tuple(_copy_registration(item) for item in supplied) + entity_metadata = tuple( + SceneEntityMetadata.from_registration(item) for item in owned + ) + metadata_index = _SceneMetadataIndex(entity_metadata) + by_id = {registration.ref.entity_id: registration for registration in owned} + object.__setattr__(self, "_registrations", owned) + object.__setattr__( + self, + "_registrations_by_id", + MappingProxyType(by_id), + ) + object.__setattr__(self, "_metadata_index", metadata_index) + object.__setattr__( + self, + "_collision_world_entity_ids", + tuple( + item.ref.entity_id + for item in owned + if item.collision_role is not SceneCollisionRole.NONE + ), + ) + object.__setattr__( + self, + "_dynamic_collision_entity_ids", + tuple( + item.ref.entity_id + for item in owned + if item.collision_role is SceneCollisionRole.DYNAMIC + ), + ) + object.__setattr__( + self, + "_static_collision_entity_ids", + tuple( + item.ref.entity_id + for item in owned + if item.collision_role is SceneCollisionRole.STATIC + ), + ) + object.__setattr__(self, "collision_world_mode", collision_world_mode) + + @property + def registrations(self) -> tuple[SceneEntityRegistration, ...]: + """Return structurally independent registration values.""" + return tuple(_copy_registration(item) for item in self._registrations) + + @property + def entity_metadata(self) -> tuple[SceneEntityMetadata, ...]: + """Return provider-free metadata without copying affordance payloads.""" + return self._metadata_index.entries + + @property + def entity_refs(self) -> tuple[SceneEntityRef, ...]: + """Return canonical typed references in registration order.""" + return tuple(item.ref for item in self._registrations) + + @property + def aliases(self) -> Mapping[str, str]: + """Return the immutable alias-to-canonical-ID index.""" + return self._metadata_index.aliases + + @property + def collision_world_entity_ids(self) -> tuple[str, ...]: + """Return every canonical ID represented in the planner world.""" + return self._collision_world_entity_ids + + @property + def dynamic_collision_entity_ids(self) -> tuple[str, ...]: + """Return canonical IDs whose planner poses update dynamically.""" + return self._dynamic_collision_entity_ids + + @property + def static_collision_entity_ids(self) -> tuple[str, ...]: + """Return canonical IDs baked into the static planner world.""" + return self._static_collision_entity_ids + + def __len__(self) -> int: + return len(self._registrations) + + def __iter__(self) -> Iterator[SceneEntityRef]: + return iter(self.entity_refs) + + def __getitem__( + self, + identifier: str | SceneEntityRef, + ) -> SceneEntityRegistration: + return self.lookup(identifier) + + def resolve( + self, + identifier: str | SceneEntityRef, + *, + expected_type: type[RefT] = SceneEntityRef, + ) -> RefT: + """Resolve a canonical ID or alias to a typed canonical reference. + + Args: + identifier: Canonical ID, alias, or already typed canonical ref. + expected_type: Required reference class for typed lookup. + + Returns: + Registry-owned canonical reference. + + Raises: + KeyError: If the canonical ID or alias is unknown. + TypeError: If the supplied or resolved reference has the wrong type. + """ + return self._metadata_index.resolve( + identifier, + expected_type=expected_type, + ) + + def lookup( + self, + identifier: str | SceneEntityRef, + *, + expected_type: type[RefT] = SceneEntityRef, + ) -> SceneEntityRegistration: + """Return an owned registration after canonical typed resolution. + + Args: + identifier: Canonical ID, alias, or typed canonical reference. + expected_type: Required reference class. + + Returns: + A structurally independent copy of the matching registration. + """ + ref = self.resolve(identifier, expected_type=expected_type) + return _copy_registration(self._registrations_by_id[ref.entity_id]) + + def affordances( + self, + parent: str | SceneEntityRef, + *, + capability: str, + ) -> tuple[SceneAffordanceRef, ...]: + """Return compatible direct-child affordances without selecting one. + + Args: + parent: Canonical ID, alias, or typed parent reference. + capability: Required open affordance capability. + + Returns: + Compatible canonical references sorted by canonical ID. + """ + return self._metadata_index.affordances( + parent, + capability=capability, + ) + + def resolve_affordance( + self, + parent: str | SceneEntityRef, + *, + capability: str, + explicit: str | SceneAffordanceRef | None = None, + ) -> SceneAffordanceRef: + """Select one compatible affordance with strict scoped-default rules. + + Args: + parent: Entity that directly owns the affordance. + capability: Required semantic affordance capability. + explicit: Optional explicit affordance ID or typed reference. + + Returns: + One canonical compatible affordance reference. + + Raises: + UnsupportedSceneAffordanceError: If no compatible affordance exists + or an explicit affordance has the wrong parent/capability. + AmbiguousSceneAffordanceError: If multiple candidates exist without + a scoped default. + """ + return self._metadata_index.resolve_affordance( + parent, + capability=capability, + explicit=explicit, + ) + + def object_semantics( + self, + object_ref: str | SceneObjectRef, + *, + affordance: str | SceneAffordanceRef, + ) -> ObjectSemantics: + """Build one owned atomic-action semantic snapshot. + + Args: + object_ref: Canonical object ID, alias, or typed reference. + affordance: Registered direct-child affordance for the object. + + Returns: + Object semantics with an owned affordance payload and canonical ID. + + Raises: + ValueError: If the affordance does not belong to the object. + """ + canonical_object = self.resolve( + object_ref, + expected_type=SceneObjectRef, + ) + object_registration = self._registrations_by_id[canonical_object.entity_id] + affordance_registration = self.lookup( + affordance, + expected_type=SceneAffordanceRef, + ) + if affordance_registration.parent != canonical_object: + raise ValueError( + f"Affordance {affordance_registration.ref.entity_id!r} is not a " + f"direct child of object {canonical_object.entity_id!r}." + ) + payload = affordance_registration.affordance + if payload is None: + raise AssertionError("Affordance registration lost its payload.") + return ObjectSemantics( + affordance=payload, + geometry={}, + properties={}, + label=object_registration.semantic_type or "none", + entity_id=canonical_object.entity_id, + ) + + def make_scene_provider( + self, + *, + translation_threshold: float = 1.0e-4, + rotation_threshold: float = 1.0e-3, + batch_size: int | None = None, + ) -> RegistrySceneProvider: + """Create an independent provider without planner cross-validation. + + This factory is intended for perception and direct-core consumers. The + canonical planning path must use :meth:`make_planning_scene_provider` + so planner IDs, capabilities, and collision-world mode cannot drift. + + Args: + translation_threshold: Accumulated translation needed to publish a + material scene change. + rotation_threshold: Accumulated rotation needed to publish a + material scene change. + batch_size: Optional fixed integration batch size. Supplying it + validates the collision-world mode immediately and binds the + provider to that row count. + + Returns: + A new provider with independent revisions and published baselines. + """ + return RegistrySceneProvider( + self, + translation_threshold=translation_threshold, + rotation_threshold=rotation_threshold, + batch_size=batch_size, + ) + + def make_planning_scene_provider( + self, + motion_generator: MotionGenerator, + *, + batch_size: int, + translation_threshold: float = 1.0e-4, + rotation_threshold: float = 1.0e-3, + ) -> RegistrySceneProvider: + """Create a provider after complete planner/registry validation. + + Args: + motion_generator: Motion generator that will consume dynamic poses. + batch_size: Number of execution environments. + translation_threshold: Accumulated translation needed to publish a + material scene change. + rotation_threshold: Accumulated rotation needed to publish a + material scene change. + + Returns: + A new independently stateful, planner-validated scene provider. + """ + provider = self.make_scene_provider( + translation_threshold=translation_threshold, + rotation_threshold=rotation_threshold, + batch_size=batch_size, + ) + self.validate_collision_integration( + motion_generator, + batch_size=batch_size, + scene_provider=provider, + ) + return provider + + def collision_geometry_by_id( + self, + role: SceneCollisionRole | None = None, + ) -> Mapping[str, object]: + """Materialize planner geometry under canonical registry IDs. + + Args: + role: Optional exact collision-role filter. Without a filter, all + static and dynamic collision registrations are included. + Registrations whose role is :attr:`SceneCollisionRole.NONE` + never enter the planner collision world. + + Returns: + Fresh immutable canonical-ID-to-geometry mapping. + """ + if role is not None and not isinstance(role, SceneCollisionRole): + raise TypeError("role must be a SceneCollisionRole or None.") + geometry: dict[str, object] = {} + for registration in self._registrations: + provider = registration.geometry_provider + if provider is None: + continue + if role is None: + if registration.collision_role is SceneCollisionRole.NONE: + continue + elif registration.collision_role is not role: + continue + entity_id = registration.ref.entity_id + descriptor = provider.get_geometry() + if descriptor is None: + raise ValueError( + f"Collision geometry provider for scene entity " + f"{entity_id!r} returned None." + ) + geometry[entity_id] = descriptor + return MappingProxyType(geometry) + + def validate_collision_integration( + self, + motion_generator: MotionGenerator, + *, + batch_size: int, + scene_provider: SceneProvider | None = None, + ) -> SceneCollisionWorldMode | None: + """Validate registry/planner agreement before dynamic planning. + + Args: + motion_generator: Motion generator whose planner consumes obstacles. + batch_size: Number of execution environments. + scene_provider: Optional external perception or hardware provider. + Its concrete ``collision_entity_ids`` must agree exactly with + the registry and planner declarations. + + Returns: + Effective dynamic collision mode, or ``None`` without dynamic IDs. + """ + effective_mode = self.resolve_collision_world_mode(batch_size=batch_size) + try: + planner_info = motion_generator.collision_world_info + if planner_info is None: + planner_dynamic_ids = () + planner_world_ids = () + supports_updates = False + planner_mode = None + else: + planner_dynamic_ids = planner_info.dynamic_entity_ids + planner_world_ids = planner_info.entity_ids + supports_updates = planner_info.supports_updates + planner_mode = planner_info.batch_mode + except AttributeError as exc: + raise TypeError( + "motion_generator must expose collision_world_info." + ) from exc + planner_dynamic_ids = self._validate_integration_ids( + planner_dynamic_ids, + field_name="motion_generator.dynamic_collision_entity_ids", + ) + planner_world_ids = self._validate_integration_ids( + planner_world_ids, + field_name="motion_generator.collision_world_entity_ids", + ) + registry_dynamic_ids = set(self.dynamic_collision_entity_ids) + planner_dynamic_id_set = set(planner_dynamic_ids) + if registry_dynamic_ids != planner_dynamic_id_set: + raise ValueError( + "Dynamic collision entity mismatch: registry missing from planner " + f"{sorted(registry_dynamic_ids - planner_dynamic_id_set)}, planner " + "missing from registry " + f"{sorted(planner_dynamic_id_set - registry_dynamic_ids)}. Planner IDs " + "must use authoritative registry IDs, not aliases." + ) + registry_world_ids = set(self.collision_world_entity_ids) + planner_world_id_set = set(planner_world_ids) + if registry_world_ids != planner_world_id_set: + raise ValueError( + "Collision world entity mismatch: registry missing from planner " + f"{sorted(registry_world_ids - planner_world_id_set)}, planner " + "missing from registry " + f"{sorted(planner_world_id_set - registry_world_ids)}. Planner IDs " + "must use authoritative registry IDs, not aliases." + ) + if scene_provider is not None: + if not isinstance(scene_provider, SceneProvider): + raise TypeError("scene_provider must implement SceneProvider.") + provider_ids = getattr(scene_provider, "collision_entity_ids", None) + provider_ids = self._validate_integration_ids( + provider_ids, + field_name="scene_provider.collision_entity_ids", + ) + provider_id_set = set(provider_ids) + if registry_dynamic_ids != provider_id_set: + raise ValueError( + "Dynamic collision entity mismatch: registry missing from " + "provider " + f"{sorted(registry_dynamic_ids - provider_id_set)}, provider " + "missing from registry " + f"{sorted(provider_id_set - registry_dynamic_ids)}. Provider IDs " + "must use authoritative registry IDs, not aliases." + ) + collision_geometry = self.collision_geometry_by_id() + if set(collision_geometry) != registry_world_ids: + raise ValueError( + "Collision geometry IDs do not match authoritative registry " + f"world IDs {sorted(registry_world_ids)}." + ) + if not registry_dynamic_ids: + return None + if supports_updates is not True: + raise ValueError( + "The selected motion generator does not support dynamic collision " + f"updates required by {sorted(registry_dynamic_ids)}." + ) + assert effective_mode is not None + if planner_mode != effective_mode.value: + raise ValueError( + "Dynamic collision world mode mismatch: registry requires " + f"{effective_mode.value!r}, planner declares {planner_mode!r}." + ) + return effective_mode + + @staticmethod + def _validate_integration_ids( + value: object, + *, + field_name: str, + ) -> tuple[str, ...]: + """Validate one canonical collision-ID declaration at a boundary.""" + if not isinstance(value, tuple) or not all( + isinstance(entity_id, str) and entity_id and entity_id == entity_id.strip() + for entity_id in value + ): + raise TypeError( + f"{field_name} must be a tuple of non-empty canonical IDs " + "without outer whitespace." + ) + if len(set(value)) != len(value): + raise ValueError(f"{field_name} must contain unique IDs.") + return value + + def resolve_collision_world_mode( + self, + *, + batch_size: int, + ) -> SceneCollisionWorldMode | None: + """Resolve the configured collision mode for an execution batch. + + Args: + batch_size: Number of execution environments. + + Returns: + The effective mode, or ``None`` when no dynamic collision entity is + registered. + """ + return self._effective_collision_world_mode(batch_size) + + def _effective_collision_world_mode( + self, + batch_size: int, + ) -> SceneCollisionWorldMode | None: + """Resolve E without reading any live state or planner integration.""" + if isinstance(batch_size, bool) or not isinstance(batch_size, int): + raise TypeError("batch_size must be an integer.") + if batch_size <= 0: + raise ValueError("batch_size must be positive.") + if not self.dynamic_collision_entity_ids: + return None + if self.collision_world_mode is not None: + return self.collision_world_mode + if batch_size == 1: + return SceneCollisionWorldMode.SHARED + raise ValueError( + "Multi-environment dynamic collision requires an explicit " + "collision_world_mode of 'shared' or 'per_env'." + ) + + @classmethod + def from_simulation( + cls, + simulation: SimulationManager, + *, + rigid_objects: Mapping[str, str] | None = None, + articulations: Mapping[str, str] | None = None, + collision_roles: Mapping[str, SceneCollisionRole] | None = None, + geometry_providers: Mapping[str, SceneGeometryProvider] | None = None, + collision_world_mode: SceneCollisionWorldMode | None = None, + ) -> SceneRegistry: + """Opt explicitly selected simulation entities into a registry. + + ``rigid_objects`` and ``articulations`` map authoritative registry IDs + to simulation UIDs. UIDs become aliases automatically; unlisted + simulation entities are never imported. Collision participation + defaults to :attr:`SceneCollisionRole.NONE`. + + Args: + simulation: Simulation manager used only for explicit UID lookup. + rigid_objects: Canonical object IDs mapped to simulation UIDs. + articulations: Canonical articulation IDs mapped to simulation UIDs. + collision_roles: Optional collision roles keyed by canonical ID. + geometry_providers: Optional geometry overrides keyed by canonical + ID. Selected rigid objects otherwise expose their live handles. + collision_world_mode: Optional dynamic collision batch-sharing mode. + + Returns: + Immutable registry containing only the explicitly selected entities. + """ + object_ids = cls._normalize_simulation_mapping( + rigid_objects, + name="rigid_objects", + ) + articulation_ids = cls._normalize_simulation_mapping( + articulations, + name="articulations", + ) + duplicate_ids = set(object_ids).intersection(articulation_ids) + if duplicate_ids: + raise ValueError( + "Simulation registry IDs must be globally unique across entity " + f"types: {sorted(duplicate_ids)}." + ) + all_ids = set(object_ids).union(articulation_ids) + roles = dict(collision_roles or {}) + geometry = dict(geometry_providers or {}) + for mapping_name, values in ( + ("collision_roles", roles), + ("geometry_providers", geometry), + ): + unknown = set(values).difference(all_ids) + if unknown: + raise KeyError( + f"{mapping_name} reference unselected registry IDs: " + f"{sorted(unknown)}." + ) + + registrations: list[SceneEntityRegistration] = [] + for registry_id, uid in object_ids.items(): + entity = cls._get_simulation_entity( + simulation, + getter_name="get_rigid_object", + registry_id=registry_id, + uid=uid, + ) + registrations.append( + SceneEntityRegistration( + ref=SceneObjectRef(registry_id), + state_provider=_SimulationEntityStateProvider(entity), + aliases=(() if uid == registry_id else (uid,)), + geometry_provider=geometry.get( + registry_id, + _SimulationEntityGeometryProvider(entity), + ), + collision_role=roles.get( + registry_id, + SceneCollisionRole.NONE, + ), + ) + ) + for registry_id, uid in articulation_ids.items(): + entity = cls._get_simulation_entity( + simulation, + getter_name="get_articulation", + registry_id=registry_id, + uid=uid, + ) + registrations.append( + SceneEntityRegistration( + ref=SceneArticulationRef(registry_id), + state_provider=_SimulationEntityStateProvider(entity), + joint_state_provider=( + _SimulationArticulationJointStateProvider(entity) + ), + aliases=(() if uid == registry_id else (uid,)), + geometry_provider=geometry.get(registry_id), + collision_role=roles.get( + registry_id, + SceneCollisionRole.NONE, + ), + ) + ) + return cls( + registrations, + collision_world_mode=collision_world_mode, + ) + + @staticmethod + def _normalize_simulation_mapping( + mapping: Mapping[str, str] | None, + *, + name: str, + ) -> dict[str, str]: + if mapping is None: + return {} + if not isinstance(mapping, Mapping): + raise TypeError(f"{name} must be a mapping from registry ID to UID.") + normalized = dict(mapping) + for registry_id, uid in normalized.items(): + _validate_identifier(registry_id, f"{name} registry ID") + _validate_identifier(uid, f"{name} UID") + return normalized + + @staticmethod + def _get_simulation_entity( + simulation: SimulationManager, + *, + getter_name: str, + registry_id: str, + uid: str, + ) -> Any: + getter = getattr(simulation, getter_name, None) + if not callable(getter): + raise TypeError(f"simulation must provide {getter_name}().") + entity = getter(uid) + if entity is None: + raise KeyError( + f"Simulation UID {uid!r} selected for registry entity " + f"{registry_id!r} was not found." + ) + return entity + + +@dataclass(frozen=True, slots=True) +class _SimulationEntityStateProvider: + """Read poses from one explicitly selected simulation entity.""" + + entity: Any + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp, env_ids + pose = self.entity.get_local_pose(to_matrix=True) + if not isinstance(pose, torch.Tensor): + raise TypeError("Simulation entity get_local_pose() must return a tensor.") + return EntityState(pose) + + +@dataclass(frozen=True, slots=True) +class _SimulationArticulationJointStateProvider: + """Read named measured qpos from one selected simulation articulation.""" + + entity: Any + + def observe_joints( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> Mapping[str, ObservedArticulationJointState]: + del timestamp + qpos = self.entity.get_qpos(target=False) + if not isinstance(qpos, torch.Tensor): + raise TypeError("Simulation articulation get_qpos() must return a tensor.") + if qpos.dim() != 2 or qpos.shape[0] == 0 or qpos.shape[1] == 0: + raise ValueError( + "Simulation articulation qpos must have non-empty shape (N, J)." + ) + joint_names = tuple(self.entity.joint_names) + if len(joint_names) != qpos.shape[1]: + raise ValueError( + "Simulation articulation joint_names must match qpos width." + ) + for joint_name in joint_names: + _validate_identifier(joint_name, "simulation articulation joint name") + if len(set(joint_names)) != len(joint_names): + raise ValueError("Simulation articulation joint_names must be unique.") + indices = env_ids.to(device=qpos.device) + if bool((indices < 0).any()) or int(indices.max().item()) >= qpos.shape[0]: + raise ValueError( + "Simulation scene env_ids must address valid articulation rows." + ) + selected = qpos.index_select(0, indices) + return MappingProxyType( + { + joint_name: ObservedArticulationJointState( + selected[:, index : index + 1] + ) + for index, joint_name in enumerate(joint_names) + } + ) + + +@dataclass(frozen=True, slots=True) +class _SimulationEntityGeometryProvider: + """Expose a selected live rigid object as planner geometry input.""" + + entity: Any + + def get_geometry(self) -> object: + return self.entity + + +class RegistrySceneProvider(SceneProvider): + """Stateful scene provider derived from an immutable registry. + + Instances are created by :meth:`SceneRegistry.make_scene_provider`; each + instance owns its revision counters and material-pose baselines. + + Args: + registry: Immutable catalog that owns entity registrations. + translation_threshold: Accumulated translation needed to publish a + material scene change. + rotation_threshold: Accumulated rotation needed to publish a material + scene change. + batch_size: Optional fixed execution batch size. Factory-created + planning providers bind this value before their first observation. + """ + + def __init__( + self, + registry: SceneRegistry, + *, + translation_threshold: float, + rotation_threshold: float, + batch_size: int | None = None, + ) -> None: + if not isinstance(registry, SceneRegistry): + raise TypeError("registry must be a SceneRegistry.") + for name, value in ( + ("translation_threshold", translation_threshold), + ("rotation_threshold", rotation_threshold), + ): + if ( + isinstance(value, bool) + or not isinstance(value, (int, float)) + or not math.isfinite(float(value)) + or value < 0.0 + ): + raise ValueError(f"{name} must be finite and non-negative.") + self.registry = registry + self.translation_threshold = float(translation_threshold) + self.rotation_threshold = float(rotation_threshold) + self.collision_entity_ids = registry.dynamic_collision_entity_ids + self._expected_batch_size = batch_size + self._last_timestamp: float | None = None + self._env_ids: torch.Tensor | None = None + self._published_poses: dict[str, torch.Tensor] = {} + self._published_confidences: dict[str, float] = {} + self._published_joint_positions: dict[tuple[str, str], torch.Tensor] = {} + self._published_joint_validity: dict[tuple[str, str], torch.Tensor] = {} + self._scene_version = 0 + self._collision_revisions: list[int] = [] + self._effective_collision_world_mode = ( + registry.resolve_collision_world_mode(batch_size=batch_size) + if batch_size is not None + else None + ) + + @property + def collision_world_mode(self) -> SceneCollisionWorldMode | None: + """Return the configured or first-snapshot-resolved collision mode.""" + return ( + self._effective_collision_world_mode + if self._effective_collision_world_mode is not None + else self.registry.collision_world_mode + ) + + def snapshot( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> SceneSnapshot: + """Observe all canonical entities and advance material revisions. + + Args: + timestamp: Non-negative monotonic observation timestamp. + env_ids: Stable ordered correlation IDs for every environment row. + + Returns: + An immutable snapshot keyed only by canonical registry IDs. + """ + if ( + isinstance(timestamp, bool) + or not isinstance(timestamp, (int, float)) + or not math.isfinite(float(timestamp)) + or timestamp < 0.0 + ): + raise ValueError("timestamp must be finite and non-negative.") + if self._last_timestamp is not None and timestamp < self._last_timestamp: + raise ValueError("Scene provider timestamps must be monotonic.") + if ( + not isinstance(env_ids, torch.Tensor) + or env_ids.dtype != torch.long + or env_ids.dim() != 1 + or env_ids.numel() == 0 + ): + raise ValueError("env_ids must be a non-empty 1D int64 tensor.") + if torch.unique(env_ids).numel() != env_ids.numel(): + raise ValueError("env_ids must be unique.") + + batch_size = int(env_ids.numel()) + if ( + self._expected_batch_size is not None + and batch_size != self._expected_batch_size + ): + raise ValueError( + "Scene provider batch size must remain equal to its configured " + f"batch_size={self._expected_batch_size}; got {batch_size}." + ) + effective_mode = self.registry.resolve_collision_world_mode( + batch_size=batch_size + ) + stable_ids = env_ids.detach().to("cpu") + if self._env_ids is None: + self._env_ids = stable_ids.clone() + self._collision_revisions = [0] * batch_size + self._effective_collision_world_mode = effective_mode + elif not torch.equal(stable_ids, self._env_ids): + raise ValueError("Scene provider env_ids must remain stable and ordered.") + + states = self._observe_states( + timestamp=float(timestamp), + env_ids=env_ids, + ) + articulation_joints = self._observe_articulation_joints( + timestamp=float(timestamp), + env_ids=env_ids, + ) + poses = {entity_id: state.pose for entity_id, state in states.items()} + confidences = { + entity_id: state.confidence for entity_id, state in states.items() + } + if self._published_poses: + changed_by_entity = { + entity_id: self._pose_change_mask( + self._published_poses[entity_id], + current_pose, + ) + for entity_id, current_pose in poses.items() + } + confidence_changed = any( + confidences[entity_id] != self._published_confidences[entity_id] + for entity_id in confidences + ) + joint_changed = self._joint_observations_changed(articulation_joints) + if ( + confidence_changed + or joint_changed + or any(changed.any().item() for changed in changed_by_entity.values()) + ): + self._scene_version += 1 + collision_changed = torch.zeros(batch_size, dtype=torch.bool) + for entity_id in self.collision_entity_ids: + collision_changed |= changed_by_entity[entity_id] + for row in collision_changed.nonzero(as_tuple=False).flatten().tolist(): + self._collision_revisions[row] += 1 + + for entity_id, changed in changed_by_entity.items(): + if changed.any(): + published_pose = self._published_poses[entity_id] + changed_on_published_device = changed.to(published_pose.device) + current_pose = poses[entity_id].to( + device=published_pose.device, + dtype=published_pose.dtype, + ) + published_pose[changed_on_published_device] = current_pose[ + changed_on_published_device + ] + self._published_confidences = confidences.copy() + else: + self._published_poses = { + entity_id: pose.clone() for entity_id, pose in poses.items() + } + self._published_confidences = confidences.copy() + self._store_joint_baseline(articulation_joints) + + self._last_timestamp = float(timestamp) + return SceneSnapshot( + timestamp=float(timestamp), + version=self._scene_version, + entities=states, + collision_world_revision=tuple(self._collision_revisions), + collision_entity_ids=self.collision_entity_ids, + articulation_joints=articulation_joints, + ) + + def _observe_articulation_joints( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> dict[tuple[str, str], ObservedArticulationJointState]: + """Observe every explicitly registered articulation-joint provider.""" + batch_size = int(env_ids.numel()) + observed: dict[tuple[str, str], ObservedArticulationJointState] = {} + for registration in self.registry._registrations: + provider = registration.joint_state_provider + if provider is None: + continue + assert isinstance(registration.ref, SceneArticulationRef) + supplied = provider.observe_joints( + timestamp=timestamp, + env_ids=env_ids.clone(), + ) + if not isinstance(supplied, Mapping): + raise TypeError( + f"Joint provider for {registration.ref.entity_id!r} must " + "return a mapping." + ) + for joint_id, state in supplied.items(): + _validate_identifier(joint_id, "joint provider joint_id") + if not isinstance(state, ObservedArticulationJointState): + raise TypeError( + f"Joint provider for {registration.ref.entity_id!r} must " + "return ObservedArticulationJointState values." + ) + key = registration.ref.entity_id, joint_id + observed[key] = self._normalize_joint_observation( + state, + batch_size=batch_size, + address=key, + ) + return observed + + @staticmethod + def _normalize_joint_observation( + state: ObservedArticulationJointState, + *, + batch_size: int, + address: tuple[str, str], + ) -> ObservedArticulationJointState: + """Broadcast one live joint observation to the scene batch.""" + position = state.position + if position.dim() == 1: + position = position.unsqueeze(0).expand(batch_size, -1).clone() + elif position.shape[0] != batch_size: + raise ValueError( + f"Articulation joint {address!r} observation must have {batch_size} " + "rows." + ) + valid = state.valid_mask + if valid is None: + valid = torch.ones( + batch_size, + dtype=torch.bool, + device=position.device, + ) + return ObservedArticulationJointState(position, valid) + + def _joint_observations_changed( + self, + states: Mapping[tuple[str, str], ObservedArticulationJointState], + ) -> bool: + """Update live joint baselines and report any material value change.""" + changed = set(states) != set(self._published_joint_positions) + if not changed: + for key, state in states.items(): + previous_position = self._published_joint_positions[key] + previous_validity = self._published_joint_validity[key] + current_position = state.position.to( + device=previous_position.device, + dtype=previous_position.dtype, + ) + assert state.valid_mask is not None + current_validity = state.valid_mask.to(previous_validity.device) + if not torch.equal( + current_position, previous_position + ) or not torch.equal( + current_validity, + previous_validity, + ): + changed = True + break + self._store_joint_baseline(states) + return changed + + def _store_joint_baseline( + self, + states: Mapping[tuple[str, str], ObservedArticulationJointState], + ) -> None: + """Own the current live joint values used for scene revisioning.""" + self._published_joint_positions = { + key: state.position.clone() for key, state in states.items() + } + self._published_joint_validity = {} + for key, state in states.items(): + assert state.valid_mask is not None + self._published_joint_validity[key] = state.valid_mask.clone() + + def _observe_states( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> dict[str, EntityState]: + """Observe explicit sources before deriving relative affordance poses.""" + batch_size = int(env_ids.numel()) + states: dict[str, EntityState] = {} + relative_registrations: list[SceneEntityRegistration] = [] + for registration in self.registry._registrations: + entity_id = registration.ref.entity_id + if registration.state_provider is None: + relative_registrations.append(registration) + continue + state = registration.state_provider.observe( + timestamp=timestamp, + env_ids=env_ids.clone(), + ) + if not isinstance(state, EntityState): + raise TypeError( + f"State provider for {entity_id!r} must return EntityState." + ) + states[entity_id] = EntityState( + self._normalize_pose(state.pose, batch_size, entity_id), + confidence=state.confidence, + ) + + for registration in relative_registrations: + entity_id = registration.ref.entity_id + assert registration.parent is not None + assert registration.relative_pose is not None + parent_state = states[registration.parent.entity_id] + relative_pose = registration.relative_pose.to( + device=parent_state.pose.device, + dtype=parent_state.pose.dtype, + ) + pose = torch.matmul(parent_state.pose, relative_pose) + states[entity_id] = EntityState( + pose, + confidence=parent_state.confidence, + ) + return states + + @staticmethod + def _normalize_pose( + pose: torch.Tensor, + batch_size: int, + entity_id: str, + ) -> torch.Tensor: + if pose.shape == (4, 4): + return pose.unsqueeze(0).expand(batch_size, -1, -1).clone() + if pose.shape != (batch_size, 4, 4): + raise ValueError( + f"Scene entity {entity_id!r} pose must have shape (4, 4) or " + f"({batch_size}, 4, 4)." + ) + return pose.clone() + + def _pose_change_mask( + self, + previous: torch.Tensor, + current: torch.Tensor, + ) -> torch.Tensor: + """Return CPU rows changed against the last material publication.""" + current = current.to(device=previous.device, dtype=previous.dtype) + translation = torch.linalg.vector_norm( + current[:, :3, 3] - previous[:, :3, 3], + dim=1, + ) + relative_rotation = torch.bmm( + previous[:, :3, :3].transpose(1, 2), + current[:, :3, :3], + ) + cosine = ( + (relative_rotation.diagonal(dim1=1, dim2=2).sum(dim=1) - 1.0) / 2.0 + ).clamp(-1.0, 1.0) + rotation = torch.acos(cosine) + return ( + ( + (translation > self.translation_threshold) + | (rotation > self.rotation_threshold) + ) + .detach() + .to("cpu") + ) + + +__all__ = [ + "ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY", + "ArticulationJointEvidenceAddress", + "AmbiguousSceneAffordanceError", + "ContainerAffordance", + "GRASP_AFFORDANCE_CAPABILITY", + "PLACEMENT_TARGET_AFFORDANCE_REVISION", + "PLACE_IN_AFFORDANCE_CAPABILITY", + "PLACE_ON_AFFORDANCE_CAPABILITY", + "RegistrySceneProvider", + "SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID", + "SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION", + "SceneArticulationJointStateProvider", + "SceneAffordanceRef", + "SceneArticulationRef", + "SceneCollisionRole", + "SceneCollisionWorldMode", + "SceneDynamics", + "SceneEntityRef", + "SceneEntityMetadata", + "SceneEntityRegistration", + "SceneEntityStateProvider", + "SceneGeometryProvider", + "SceneLinkRef", + "SceneObjectRef", + "SceneRegistry", + "SupportSurfaceAffordance", + "UnsupportedSceneAffordanceError", +] 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/lab/sim/workspace/analyzer.py b/embodichain/lab/sim/workspace/analyzer.py index c2decb5af..292aae18d 100644 --- a/embodichain/lab/sim/workspace/analyzer.py +++ b/embodichain/lab/sim/workspace/analyzer.py @@ -14,6 +14,8 @@ # limitations under the License. # ---------------------------------------------------------------------------- +from __future__ import annotations + import time import torch import numpy as np @@ -962,7 +964,7 @@ def compute_workspace_points( for batch_start in pbar: batch_end = min(batch_start + batch_size, num_samples) - # Reshape to (n_envs=1, batch_size, num_joints) for compute_batch_fk + # Reshape to (num_envs=1, batch_size, num_joints) for compute_batch_fk qpos_batch = joint_configs[batch_start:batch_end].unsqueeze(0) try: diff --git a/embodichain/lab/sim/workspace/samplers/__init__.py b/embodichain/lab/sim/workspace/samplers/__init__.py index 388a0d007..56061e720 100644 --- a/embodichain/lab/sim/workspace/samplers/__init__.py +++ b/embodichain/lab/sim/workspace/samplers/__init__.py @@ -14,9 +14,11 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Workspace configuration samplers deriving from ``BaseSampler``. +"""Workspace sampling strategies deriving from ``BaseSampler``. -Built-in samplers: uniform, random, Halton, Sobol, and Latin-hypercube, plus a ``SamplerFactory`` and ``create_sampler`` helper. +The package exports uniform, random, Halton, Sobol, and Latin-hypercube +samplers. ``SamplerFactory`` and ``create_sampler`` additionally construct the +Gaussian and importance strategies. """ from embodichain.lab.sim.workspace.samplers.base_sampler import ( diff --git a/embodichain/lab/sim/workspace/samplers/sampler_factory.py b/embodichain/lab/sim/workspace/samplers/sampler_factory.py index 64d762d84..ab19d179c 100644 --- a/embodichain/lab/sim/workspace/samplers/sampler_factory.py +++ b/embodichain/lab/sim/workspace/samplers/sampler_factory.py @@ -61,9 +61,8 @@ class SamplerFactory: the sampling strategy. It uses the singleton pattern to ensure only one instance exists throughout the application. - The factory comes pre-registered with built-in samplers: - - UNIFORM: UniformSampler - - RANDOM: RandomSampler + The factory comes pre-registered with the strategies defined by + ``SamplingStrategy``. Additional samplers can be registered using register_sampler(). @@ -159,11 +158,8 @@ def create_sampler( strategy: The sampling strategy to use. Can be a SamplingStrategy enum or a string identifier. If None, defaults to RANDOM. **kwargs: Additional keyword arguments to pass to the sampler constructor. - Common arguments include: - - seed: Random seed for reproducibility - - samples_per_dim: For UniformSampler - - device: PyTorch device for tensor operations - Note: constraint parameter is temporarily disabled + Common options include ``seed``, ``device``, and + ``samples_per_dim`` for ``UniformSampler``. Returns: An instance of the requested sampler. @@ -177,7 +173,8 @@ def create_sampler( >>> sampler = factory.create_sampler(SamplingStrategy.UNIFORM, seed=42) >>> sampler = factory.create_sampler("random", seed=123) - Note: Constraint-based sampling examples are temporarily disabled + Note: + Constraint-based sampling is temporarily disabled. """ # Default to RANDOM if no strategy specified if strategy is None: @@ -267,7 +264,8 @@ def create_sampler( >>> sampler = create_sampler(SamplingStrategy.UNIFORM, seed=42) >>> sampler = create_sampler("random", seed=123) - Note: Constraint-based sampling is temporarily disabled + Note: + Constraint-based sampling is temporarily disabled. """ factory = SamplerFactory() return factory.create_sampler(strategy, **kwargs) diff --git a/embodichain/lab/sim/workspace/visualizers/point_cloud_visualizer.py b/embodichain/lab/sim/workspace/visualizers/point_cloud_visualizer.py index d35910705..a67623e86 100644 --- a/embodichain/lab/sim/workspace/visualizers/point_cloud_visualizer.py +++ b/embodichain/lab/sim/workspace/visualizers/point_cloud_visualizer.py @@ -54,13 +54,14 @@ def __init__( Args: backend: Visualization backend ('sim_manager', 'viser', 'open3d', - 'matplotlib', or 'data'). - Defaults to 'sim_manager'. 'data' backend returns raw data without visualization. - 'sim_manager' backend uses simulation environment for visualization. + 'matplotlib', or 'data'). Defaults to 'sim_manager'. The 'data' + backend returns raw data without visualization. point_size: Size of points in visualization. Defaults to 2.0. config: Optional configuration dictionary. Defaults to None. - sim_manager: SimulationManager instance for 'sim_manager' backend. Defaults to None. - control_part_name: Control part name for naming the point cloud. Defaults to None. + sim_manager: SimulationManager instance for the 'sim_manager' or + 'viser' backend. Defaults to None. + control_part_name: Control part name used to name the point cloud. + Defaults to None. """ super().__init__(backend, config) self.point_size = point_size diff --git a/embodichain/lab/sim/workspace/visualizers/visualizer_factory.py b/embodichain/lab/sim/workspace/visualizers/visualizer_factory.py index 0d879ec6f..ecb177d58 100644 --- a/embodichain/lab/sim/workspace/visualizers/visualizer_factory.py +++ b/embodichain/lab/sim/workspace/visualizers/visualizer_factory.py @@ -160,12 +160,9 @@ def create_visualizer( viz_type: The visualization type to use. Can be a VisualizationType enum or a string identifier. If None, defaults to POINT_CLOUD. **kwargs: Additional keyword arguments to pass to the visualizer constructor. - Common arguments include: - - backend: Visualization backend ('open3d', 'matplotlib', 'data') - 'data' backend returns processed data without visualization - - voxel_size: For VoxelVisualizer - - sphere_radius: For SphereVisualizer - - point_size: For PointCloudVisualizer + Common options include ``backend``, ``voxel_size`` for + ``VoxelVisualizer``, ``sphere_radius`` for ``SphereVisualizer``, + and ``point_size`` for ``PointCloudVisualizer``. Returns: An instance of the requested visualizer. diff --git a/embodichain/lab/visualization/scene_exporter.py b/embodichain/lab/visualization/scene_exporter.py index cf77446b8..31d12c027 100644 --- a/embodichain/lab/visualization/scene_exporter.py +++ b/embodichain/lab/visualization/scene_exporter.py @@ -32,6 +32,7 @@ CameraImageFrame, CameraSpec, DynamicMeshUpdate, + FrameOverlay, GizmoSpec, GizmoState, JointControlProvider, @@ -702,9 +703,54 @@ def _append_articulations( ) ) + def _capture_axis_marker_overlays( + self, reserved_frame_ids: set[str] | None = None + ) -> tuple[FrameOverlay, ...]: + """Capture native simulation axes as Viser coordinate-frame overlays. + + Args: + reserved_frame_ids: Caller-owned frame IDs that generated markers + must not replace. + """ + get_axis_marker_items = getattr(self._sim, "get_axis_marker_items", None) + if get_axis_marker_items is None: + return () + + frames: list[FrameOverlay] = [] + used_frame_ids = set(reserved_frame_ids or ()) + for marker_name, handles, axis_length, axis_radius in get_axis_marker_items(): + for index, handle in enumerate(handles): + position, wxyz = pose_to_position_wxyz(handle.get_world_pose()) + base_id = f"marker:{marker_name}:{index}" + overlay_id = base_id + suffix = 1 + while overlay_id in used_frame_ids: + overlay_id = f"{base_id}#{suffix}" + suffix += 1 + used_frame_ids.add(overlay_id) + frames.append( + FrameOverlay( + overlay_id=overlay_id, + position=position, + wxyz=wxyz, + axes_length=axis_length, + axes_radius=axis_radius, + # Native handles report hidden in headless mode even + # though draw_marker() requested a visible marker. + visible=True, + ) + ) + return tuple(frames) + def _prepare_overlays(self, overlays: SceneOverlays | None) -> SceneOverlays: + reserved_frame_ids = ( + {frame.overlay_id for frame in overlays.frames} + if overlays is not None + else None + ) + marker_frames = self._capture_axis_marker_overlays(reserved_frame_ids) if overlays is None: - return SceneOverlays() + return SceneOverlays(frames=marker_frames) point_clouds: list[PointCloudOverlay] = [] for point_cloud in overlays.point_clouds: point_count = point_cloud.points.shape[0] @@ -732,7 +778,7 @@ def _prepare_overlays(self, overlays: SceneOverlays | None) -> SceneOverlays: ) ) return SceneOverlays( - frames=overlays.frames, + frames=marker_frames + overlays.frames, trajectories=overlays.trajectories, targets=overlays.targets, point_clouds=tuple(point_clouds), diff --git a/embodichain/learning/rl/algo/grpo.py b/embodichain/learning/rl/algo/grpo.py index 3dcacf20e..b97379a3c 100644 --- a/embodichain/learning/rl/algo/grpo.py +++ b/embodichain/learning/rl/algo/grpo.py @@ -68,21 +68,21 @@ def _compute_step_returns_and_mask( self, rewards: torch.Tensor, dones: torch.Tensor ) -> tuple[torch.Tensor, torch.Tensor]: """Compute discounted returns and valid-step mask over `[N, T]` rollout.""" - n_envs, t_steps = rewards.shape + num_envs, t_steps = rewards.shape seq_mask = torch.ones( - (n_envs, t_steps), dtype=torch.float32, device=self.device + (num_envs, t_steps), dtype=torch.float32, device=self.device ) step_returns = torch.zeros( - (n_envs, t_steps), dtype=torch.float32, device=self.device + (num_envs, t_steps), dtype=torch.float32, device=self.device ) - alive = torch.ones(n_envs, dtype=torch.float32, device=self.device) + alive = torch.ones(num_envs, dtype=torch.float32, device=self.device) for t in range(t_steps): seq_mask[:, t] = alive if self.cfg.truncate_at_first_done: alive = alive * (~dones[:, t]).float() - running_return = torch.zeros(n_envs, dtype=torch.float32, device=self.device) + running_return = torch.zeros(num_envs, dtype=torch.float32, device=self.device) for t in reversed(range(t_steps)): running_return = ( rewards[:, t] + self.cfg.gamma * running_return * (~dones[:, t]).float() @@ -95,11 +95,11 @@ def _compute_step_group_advantages( self, step_returns: torch.Tensor, seq_mask: torch.Tensor ) -> torch.Tensor: """Normalize per-step returns within each environment group.""" - n_envs, t_steps = step_returns.shape + num_envs, t_steps = step_returns.shape group_size = self.cfg.group_size - returns_grouped = step_returns.view(n_envs // group_size, group_size, t_steps) - mask_grouped = seq_mask.view(n_envs // group_size, group_size, t_steps) + returns_grouped = step_returns.view(num_envs // group_size, group_size, t_steps) + mask_grouped = seq_mask.view(num_envs // group_size, group_size, t_steps) valid_count = mask_grouped.sum(dim=1, keepdim=True) valid_count_safe = torch.clamp(valid_count, min=1.0) @@ -112,7 +112,7 @@ def _compute_step_group_advantages( group_std = torch.sqrt(group_var) advantages = (returns_grouped - group_mean) / (group_std + self.cfg.eps) - return advantages.view(n_envs, t_steps) * seq_mask + return advantages.view(num_envs, t_steps) * seq_mask def update(self, rollout: TensorDict) -> Dict[str, float]: rollout = rollout.clone() diff --git a/embodichain/toolkits/graspkit/pg_grasp/antipodal_generator.py b/embodichain/toolkits/graspkit/pg_grasp/antipodal_generator.py index 0eb4fa588..c53d92613 100644 --- a/embodichain/toolkits/graspkit/pg_grasp/antipodal_generator.py +++ b/embodichain/toolkits/graspkit/pg_grasp/antipodal_generator.py @@ -14,8 +14,11 @@ # limitations under the License. # ---------------------------------------------------------------------------- +from __future__ import annotations + import os import argparse +from collections.abc import Callable import open3d as o3d import time import torch @@ -44,6 +47,7 @@ Path.home() / ".cache" / "embodichain" / "grasp_annotator_cache" ) GRASP_ANNOTATOR_CACHE_DIR.mkdir(parents=True, exist_ok=True) +VERSION_TAG = "v0.0.1" __all__ = ["GraspGenerator", "GraspGeneratorCfg"] @@ -419,7 +423,7 @@ def _get_cache_dir(self, vertices: torch.Tensor, triangles: torch.Tensor): face_bytes = triangles.to("cpu").numpy().tobytes() md5_hash = hashlib.md5(vert_bytes + face_bytes).hexdigest() cache_path = os.path.join( - GRASP_ANNOTATOR_CACHE_DIR, f"antipodal_cache_{md5_hash}.npy" + GRASP_ANNOTATOR_CACHE_DIR, f"antipodal_cache_{VERSION_TAG}_{md5_hash}.npy" ) return cache_path @@ -612,6 +616,9 @@ def get_valid_grasp_poses( approach_direction: torch.Tensor, object_part: str = "center", visualize_collision: bool = False, + pose_cost_fn: ( + Callable[[torch.Tensor, torch.Tensor], torch.Tensor] | None + ) = None, ): if self._hit_point_pairs is None: logger.log_warning( @@ -658,6 +665,7 @@ def get_valid_grasp_poses( approach_direction=approach_direction, mesh_vert_transformed=mesh_vert_transformed, visualize_collision=visualize_collision, + pose_cost_fn=pose_cost_fn, ) def get_dual_arm_valid_grasp_poses( @@ -761,6 +769,9 @@ def _filter_valid_grasp_poses( mesh_vert_transformed: torch.Tensor, object_pose: torch.Tensor, visualize_collision: bool = False, + pose_cost_fn: ( + Callable[[torch.Tensor, torch.Tensor], torch.Tensor] | None + ) = None, ): grasp_x = F.normalize(hit_points_ - origin_points_, dim=-1) cos_angle = torch.clamp((grasp_x * approach_direction).sum(dim=-1), -1.0, 1.0) @@ -849,6 +860,17 @@ def _filter_valid_grasp_poses( center_cost = center_distance / center_distance.max() length_cost = 1 - valid_open_lengths / valid_open_lengths.max() total_cost = 0.2 * angle_cost + 0.2 * length_cost + 0.6 * center_cost + if pose_cost_fn is not None: + adjusted_cost = pose_cost_fn(valid_grasp_poses, total_cost) + if adjusted_cost.shape != total_cost.shape: + logger.log_error( + "pose_cost_fn must preserve the grasp cost shape.", + ValueError, + ) + total_cost = adjusted_cost.to( + device=total_cost.device, + dtype=total_cost.dtype, + ) n_valid = valid_grasp_poses.shape[0] if n_valid == 0: diff --git a/embodichain/toolkits/graspkit/pg_grasp/antipodal_sampler.py b/embodichain/toolkits/graspkit/pg_grasp/antipodal_sampler.py index 0ea90416f..47f6f7b98 100644 --- a/embodichain/toolkits/graspkit/pg_grasp/antipodal_sampler.py +++ b/embodichain/toolkits/graspkit/pg_grasp/antipodal_sampler.py @@ -91,11 +91,14 @@ def sample(self, vertices: torch.Tensor, faces: torch.Tensor) -> torch.Tensor: ray_origin = ( sample_points - 2.0 * max_range * ray_direc ) # ray origin in the other side of the mesh - # casting + ray_origin_2 = sample_points - 2.0 * self.cfg.max_length * ray_direc + all_ray_origin = torch.cat([ray_origin, ray_origin_2], dim=0) + all_ray_direc = torch.cat([ray_direc, ray_direc], dim=0) + all_surface_origin = torch.cat([sample_points, sample_points], dim=0) return self._get_raycast_result( - ray_origin, - ray_direc, - surface_origin=sample_points, + all_ray_origin, + all_ray_direc, + surface_origin=all_surface_origin, ) def _sample_surface_by_fibonacci_raycast( diff --git a/embodichain/utils/__init__.py b/embodichain/utils/__init__.py index f3dd6ba62..fd680446c 100644 --- a/embodichain/utils/__init__.py +++ b/embodichain/utils/__init__.py @@ -20,6 +20,15 @@ """ from .configclass import configclass, is_configclass +from .config_paths import resolve_config_path + +__all__ = [ + "GLOBAL_SEED", + "configclass", + "is_configclass", + "resolve_config_path", + "set_seed", +] GLOBAL_SEED = 1024 diff --git a/embodichain/utils/cfg.py b/embodichain/utils/cfg.py index fd9138027..70e5204f1 100644 --- a/embodichain/utils/cfg.py +++ b/embodichain/utils/cfg.py @@ -45,10 +45,10 @@ def _flatten_dict( """Traverse a dictionary and return all keys including nested ones. Args: - src (Dict): an instance of :class:`Dict`. - prefix (str | None, optional): [description]. Defaults to prefix. - sep (str | None, optional): [description]. Defaults to sep. - dct (Dict | None, optional): [description]. Defaults to {}. + src: Dictionary to flatten. + prefix: Prefix added to each flattened key. + sep: Separator between nested key components. + dct: Reserved compatibility argument. Returns: Dict: flatten dictionary with all keys. @@ -481,50 +481,43 @@ def _called_with_cfg(*args, **kwargs): def configurable(init_func=None, *, from_config=None): - """ - Decorate a function or a class's method so that it can be called - with a :class:`CfgNode` object using a :func:`from_config` function that translates - :class:`CfgNode` to arguments. + """Decorate a callable so it can receive a :class:`CfgNode`. + + The associated ``from_config`` callable translates a configuration object + into explicit keyword arguments. + + Args: + init_func: A class's ``__init__`` method. The class must have a + ``from_config`` classmethod that takes ``cfg`` as + the first argument. + from_config: Translation function for decorated functions or methods. + It must take ``cfg`` as its first argument. + Examples: - :: - # Usage 1: Decorator on __init__: - class A: - @configurable - def __init__(self, a, b=2, c=3): - pass - @classmethod - def from_config(cls, cfg): # 'cfg' must be the first argument - # Returns kwargs to be passed to __init__ - return {"a": cfg.A, "b": cfg.B} - a1 = A(a=1, b=2) # regular construction - a2 = A(cfg) # construct with a cfg - a3 = A(cfg, b=3, c=4) # construct with extra overwrite - - # Usage 2: Decorator on any function. Needs an extra from_config argument: - @configurable(from_config=lambda cfg: {"a": cfg.A, "b": cfg.B}) - def a_func(a, b=2, c=3): - pass - a1 = a_func(a=1, b=2) # regular call - a2 = a_func(cfg) # call with a cfg - a3 = a_func(cfg, b=3, c=4) # call with extra overwrite - - # Usage 3: Decorator on any method of class. Needs an extra from_config argument: - class A: + Decorate a class constructor that supplies its own translator: + + .. code-block:: python + + class A: + @configurable + def __init__(self, a, b=2): + pass + + @classmethod + def from_config(cls, cfg): + return {"a": cfg.A, "b": cfg.B} + + instance = A(cfg) + + Pass a translator directly when decorating a function: + + .. code-block:: python + @configurable(from_config=lambda cfg: {"a": cfg.A, "b": cfg.B}) - def a_func(self, a, b=2, c=3): + def a_func(a, b=2): pass - insA = A() - cfg = CfgNode.load_cfg('{"A": "2", "B": "3"}') - a1 = insA.a_func(a=1, b=2) # regular call - a2 = insA.a_func(cfg) # call with a cfg - a3 = insA.a_func(cfg, b=3, c=4) # call with extra overwrite - Args: - init_func (callable): a class's ``__init__`` method in usage 1. The - class must have a ``from_config`` classmethod which takes `cfg` as - the first argument. - from_config (callable): the from_config function in usage 2 and 3. It must take `cfg` - as its first argument. + result = a_func(cfg) """ if init_func is not None: assert ( diff --git a/embodichain/utils/config_paths.py b/embodichain/utils/config_paths.py new file mode 100644 index 000000000..e97346c98 --- /dev/null +++ b/embodichain/utils/config_paths.py @@ -0,0 +1,56 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Stable path resolution for user and packaged configuration files.""" + +from __future__ import annotations + +from pathlib import Path + +__all__ = ["resolve_config_path"] + + +def resolve_config_path(path: str | Path) -> Path: + """Resolve one configuration path without opening the target file. + + Existing, absolute, and ordinary relative paths preserve their normal + filesystem meaning. Repository-style paths below + ``embodichain_tasks/configs`` are redirected to the packaged task-config + resource so the same configuration reference works from an installed + wheel. + + Args: + path: User path or repository-style official-task configuration path. + + Returns: + Expanded filesystem path, resolved through the packaged task resource + only when the input uses the official-task configuration prefix. + + Raises: + TypeError: If ``path`` is not path-like. + """ + resolved_path = Path(path).expanduser() + if resolved_path.exists() or resolved_path.is_absolute(): + return resolved_path + + task_prefix = ("embodichain_tasks", "configs") + if resolved_path.parts[: len(task_prefix)] != task_prefix: + return resolved_path + + from embodichain_tasks.configs import get_config_path + + relative_path = Path(*resolved_path.parts[len(task_prefix) :]) + return get_config_path(relative_path) diff --git a/embodichain/utils/img_utils.py b/embodichain/utils/img_utils.py index b8307bbe2..fc4614158 100644 --- a/embodichain/utils/img_utils.py +++ b/embodichain/utils/img_utils.py @@ -26,7 +26,7 @@ def batched_mask_to_box(masks: torch.Tensor) -> torch.Tensor: Returns: torch.Tensor: A tensor of shape (..., 4) containing the bounding boxes - in XYXY format. + in XYXY format. """ # torch.max below raises an error on empty inputs, just skip in this case if torch.numel(masks) == 0: @@ -70,55 +70,23 @@ def batched_mask_to_box(masks: torch.Tensor) -> torch.Tensor: def gen_disp_colormap(inputs, normalize=True, torch_transpose=True): - """ - Generate an RGB visualization using the "plasma" colormap for 2D/3D/4D scalar image inputs. - - This utility maps scalar image(s) to an RGB colormap suitable for display or further processing. - It accepts either a NumPy array or a torch.Tensor (torch tensors are detached, moved to CPU and - converted to NumPy). The matplotlib "plasma" colormap with 256 entries is used. - - Parameters - - inputs (numpy.ndarray or torch.Tensor): - Scalar image data with one of the following dimensionalities: - * 2D: (H, W) -> a single image - * 3D: (N, H, W) -> a batch of N single-channel images - * 4D: (N, C, H, W) -> a batch with channel dimension; expected C==1 (first channel used) - The function will convert torch.Tensor input to numpy internally. - - normalize (bool, default True): - If True, input values are linearly scaled to [0, 1] using (x - min) / (max - min). - If the input is constant (min == max), a small divisor (1e5) is used to avoid division - by zero, which effectively maps values near 0. If False, values are assumed to already be - in the [0, 1] range (no scaling is performed). - - torch_transpose (bool, default True): - Controls the output channel ordering to match common PyTorch conventions: - * If True: outputs are transposed to channel-first form: - - 2D input -> (3, H, W) - - 3D input -> (N, 3, H, W) - - 4D input -> (N, 3, H, W) (uses the first channel) - * If False: outputs keep channel-last ordering: - - 2D input -> (H, W, 3) - - 3D input -> (N, H, W, 3) - - 4D input -> (N, H, W, 3) - - Returns - - numpy.ndarray: - RGB image(s) with float values in [0, 1]. The exact output shape depends on the input - dimensionality and the value of torch_transpose (see above). The alpha channel produced by - the colormap is discarded; only the RGB channels are returned. - - Notes and behavior - - The function uses matplotlib.pyplot.get_cmap("plasma", 256). - - For 4D inputs the code selects the first channel (index 0) before applying the colormap. - - Inputs with dimensionality other than 2, 3, or 4 are not supported and will likely raise - an error or produce unintended results. - - This function is non-destructive: it returns a new NumPy array and does not modify the input. - - Typical use cases: visualizing depth maps, single-channel activation maps, or other scalar - images as colored RGB images for inspection or logging. - - Examples - - 2D array (H, W) -> returns (3, H, W) if torch_transpose=True - - 3D array (N, H, W) -> returns (N, 3, H, W) if torch_transpose=True - - 4D array (N, 1, H, W) -> returns (N, 3, H, W) if torch_transpose=True + """Generate a color visualization with the ``plasma`` colormap. + + Args: + inputs: NumPy array or tensor with shape ``(H, W)``, ``(N, H, W)``, + or ``(N, C, H, W)``. Four-dimensional inputs use the first channel. + normalize: Whether to scale the input linearly to ``[0, 1]`` before + applying the colormap. Defaults to True. + torch_transpose: Whether to transpose the generated array to + channel-first order. Defaults to True. + + Returns: + A NumPy array containing the colormapped values. Its shape depends on + the input dimensionality and ``torch_transpose``. + + Note: + Tensor inputs are detached and moved to CPU. The returned array is a + new value and does not modify the input. """ import matplotlib.pyplot as plt import torch diff --git a/embodichain/utils/logger.py b/embodichain/utils/logger.py index e4509aba5..9011c0439 100644 --- a/embodichain/utils/logger.py +++ b/embodichain/utils/logger.py @@ -14,11 +14,60 @@ # limitations under the License. # ---------------------------------------------------------------------------- -import logging +from __future__ import annotations -logging.basicConfig( - level=logging.INFO, format="%(asctime)s %(message)s", datefmt="%H:%M:%S" -) +import logging +import time +from typing import NoReturn + +__all__ = [ + "decorate_str_color", + "format_message", + "log_debug", + "log_error", + "log_info", + "log_warning", + "logger", + "set_log_level", +] + +_LOG_FORMAT = "%(asctime)s.%(msecs)03d UTC │ %(message)s" +_DATE_FORMAT = "%Y-%m-%d %H:%M:%S" +_RESET_COLOR = "\033[0m" +_COLOR_CODES = { + "red": "\033[91m", + "green": "\033[92m", + "yellow": "\033[93m", + "blue": "\033[94m", + "purple": "\033[95m", + "cyan": "\033[96m", + "orange": "\033[33m", + "white": "\033[97m", +} +_DEFAULT_LEVEL_COLORS = { + "DEBUG": "cyan", + "INFO": "green", + "WARNING": "yellow", + "ERROR": "red", +} + + +class _UTCFormatter(logging.Formatter): + """Format logging timestamps in UTC.""" + + converter = time.gmtime + + def format(self, record: logging.LogRecord) -> str: + """Format a record, optionally omitting the standard log prefix.""" + if getattr(record, "embodichain_plain", False): + return record.getMessage() + return super().format(record) + + +_DEFAULT_FORMATTER = _UTCFormatter(_LOG_FORMAT, datefmt=_DATE_FORMAT) +_DEFAULT_HANDLER = logging.StreamHandler() +_DEFAULT_HANDLER.setFormatter(_DEFAULT_FORMATTER) +logging.basicConfig(level=logging.INFO, handlers=[_DEFAULT_HANDLER]) # Create a custom logger logger = logging.getLogger(__name__) @@ -27,48 +76,111 @@ logger.setLevel(logging.INFO) -def decorate_str_color(msg: str, color: str): - """Decorate a string with a specific color.""" - color_map = { - "red": "\033[91m", - "green": "\033[92m", - "yellow": "\033[93m", - "blue": "\033[94m", - "purple": "\033[95m", - "cyan": "\033[96m", - "orange": "\033[33m", - "white": "\033[97m", - } - return f"{color_map.get(color, '')}{msg}\033[0m" if color else msg - - -def set_log_level(level: str): - """Set the logging level.""" - level = level.upper() - assert level in ["DEBUG", "INFO", "WARNING", "ERROR"], "Invalid log level" - logger.setLevel(getattr(logging, level)) +def decorate_str_color(msg: str, color: str | None) -> str: + """Decorate a string with an ANSI color. + Args: + msg: Text to decorate. + color: Supported color name, or ``None`` to disable coloring. -def format_message(level: str, message: str): - """Format the log message with a consistent prefix.""" - return f"[EmbodiChain {level}]: {message}" + Returns: + The decorated text, including an ANSI reset sequence when colored. + """ + return f"{_COLOR_CODES.get(color, '')}{msg}{_RESET_COLOR}" if color else msg -def log_info(message, color=None): - """Log an info message.""" - logger.info(decorate_str_color(format_message("INFO", message), color)) +def set_log_level(level: str) -> None: + """Set the EmbodiChain logging level. - -def log_debug(message, color="blue"): - """Log a debug message.""" - logger.debug(decorate_str_color(format_message("DEBUG", message), color)) - - -def log_warning(message): - """Log a warning message.""" - logger.warning(decorate_str_color(format_message("WARNING", message), "purple")) + Args: + level: One of ``DEBUG``, ``INFO``, ``WARNING``, or ``ERROR``. + """ + level = level.upper() + assert level in ["DEBUG", "INFO", "WARNING", "ERROR"], "Invalid log level" + logger.setLevel(getattr(logging, level)) -def log_error(message, error_type=RuntimeError): - """Log an error message.""" - raise error_type(decorate_str_color(format_message("ERROR", message), "red")) +def format_message( + level: str, + message: object, + color: str | None = None, +) -> str: + """Format a log message using aligned, optionally colored columns. + + Args: + level: Logging level displayed in the first message column. + message: Log message payload. + color: Supported color name for the level, or ``None`` for no color. + + Returns: + A formatted message containing the level, component, and payload. + """ + decorated_level = decorate_str_color(f"{level:<7}", color) + return f"{decorated_level} │ EmbodiChain │ {message}" + + +def log_info( + message: object, + color: str | None = _DEFAULT_LEVEL_COLORS["INFO"], + *, + prefix: bool = True, +) -> None: + """Log an info message. + + Args: + message: Log message payload. + color: Level color override, or ``None`` to disable coloring. + prefix: Whether to include the timestamp, level, and component columns. + """ + if not prefix: + logger.info(message, extra={"embodichain_plain": True}) + return + logger.info(format_message("INFO", message, color)) + + +def log_debug( + message: object, + color: str | None = _DEFAULT_LEVEL_COLORS["DEBUG"], +) -> None: + """Log a debug message. + + Args: + message: Log message payload. + color: Level color override, or ``None`` to disable coloring. + """ + logger.debug(format_message("DEBUG", message, color)) + + +def log_warning( + message: object, + color: str | None = _DEFAULT_LEVEL_COLORS["WARNING"], +) -> None: + """Log a warning message. + + Args: + message: Log message payload. + color: Level and message color override, or ``None`` to disable coloring. + """ + logger.warning( + format_message("WARNING", decorate_str_color(str(message), color), color) + ) + + +def log_error( + message: object, + error_type: type[Exception] = RuntimeError, + color: str | None = _DEFAULT_LEVEL_COLORS["ERROR"], +) -> NoReturn: + """Raise an exception with an error-formatted message. + + Args: + message: Error message payload. + error_type: Exception class to raise. + color: Level and message color override, or ``None`` to disable coloring. + + Raises: + Exception: An instance of ``error_type`` containing the formatted message. + """ + raise error_type( + format_message("ERROR", decorate_str_color(str(message), color), color) + ) diff --git a/embodichain/utils/utility.py b/embodichain/utils/utility.py index 2c6b3cd1d..e44c5633f 100644 --- a/embodichain/utils/utility.py +++ b/embodichain/utils/utility.py @@ -31,6 +31,7 @@ from pathlib import Path from typing import Any, Dict, List, Tuple, Callable +from embodichain.utils.config_paths import resolve_config_path as _resolve_config_path from embodichain.utils.string import callable_to_string @@ -375,22 +376,6 @@ def _config_format_from_path(path: str | Path) -> str: ) -def _resolve_config_path(path: str | Path) -> Path: - """Resolve repository-style official-task paths from an installed wheel.""" - resolved_path = Path(path).expanduser() - if resolved_path.exists() or resolved_path.is_absolute(): - return resolved_path - - task_prefix = ("embodichain_tasks", "configs") - if resolved_path.parts[: len(task_prefix)] != task_prefix: - return resolved_path - - from embodichain_tasks.configs import get_config_path - - relative_path = Path(*resolved_path.parts[len(task_prefix) :]) - return get_config_path(relative_path) - - def load_config(path: str | Path) -> Dict[str, Any]: """Load a gym or agent config file into a dictionary. diff --git a/embodichain_tasks/README.md b/embodichain_tasks/README.md index 0d909ddd1..96c0a5556 100644 --- a/embodichain_tasks/README.md +++ b/embodichain_tasks/README.md @@ -62,9 +62,10 @@ Importing `embodichain_tasks` recursively imports every sub-package, which triggers each task's `@register_env` decorator and registers it in the gymnasium registry. The unified CLI calls `discover_task_packages()` (from `embodichain.lab.gym.utils.registration`) at startup, which imports this -package via its entry point. See -`docs/superpowers/specs/2026-07-07-task-env-refactor-design.md` for the full -design. +package via its entry point. See the +[task-package discovery utilities](../embodichain/lab/gym/utils/registration.py) +and the [official task package initializer](embodichain_tasks/__init__.py) for +the implementation. ## Extending with your own tasks diff --git a/embodichain_tasks/configs/expert_program/multi_segments/repeated_cube_pick_place.yaml b/embodichain_tasks/configs/expert_program/multi_segments/repeated_cube_pick_place.yaml new file mode 100644 index 000000000..107236109 --- /dev/null +++ b/embodichain_tasks/configs/expert_program/multi_segments/repeated_cube_pick_place.yaml @@ -0,0 +1,46 @@ +schema_version: 1 +program_id: repeated_cube_pick_place + +integration: + robot_profile: ur5_parallel_gripper_v1 + scene_registry: multi_segments_cube_v1 + runtime_preset: safe + +targets: + drop_pose: + kind: cyclic_pose + values: + - position: [-0.40, 0.48, 0.10] + quaternion_wxyz: [1.0, 0.0, 0.0, 0.0] + - position: [-0.42, -0.08, 0.10] + quaternion_wxyz: [1.0, 0.0, 0.0, 0.0] + +program: + kind: repeat + count: 3 + body: + kind: segment + name: move_cube + steps: + kind: sequence + items: + - kind: invoke + call: + kind: pick + object: cube + - kind: invoke + call: + kind: place + object: cube + at: + kind: target_ref + target: drop_pose + post: + - kind: wait_stable + entity: cube + preset: rigid_object + validators: + - kind: object_near_target + object: cube + target: drop_pose + position_tolerance: 0.12 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/expert_program/tableware/open_drawer.json b/embodichain_tasks/configs/expert_program/tableware/open_drawer.json new file mode 100644 index 000000000..9bd54f210 --- /dev/null +++ b/embodichain_tasks/configs/expert_program/tableware/open_drawer.json @@ -0,0 +1,23 @@ +{ + "schema_version": 1, + "program_id": "open_drawer", + "integration": { + "robot_profile": "cobot_magic_right_manipulator_v1", + "scene_registry": "open_drawer_v1", + "runtime_preset": "safe" + }, + "targets": {}, + "program": { + "kind": "segment", + "name": "open_drawer", + "steps": { + "kind": "invoke", + "call": { + "kind": "operate_articulation", + "articulation": "drawer", + "handle": "drawer_handle", + "target": "open" + } + } + } +} 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/configs/gym/multi_segments/cube_pick_place.json b/embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json index 6543d8fa1..cbb4ba140 100644 --- a/embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json +++ b/embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json @@ -1,5 +1,6 @@ { "id": "MultiSegmentsCubePickPlace-v1", + "expert_program_path": "../../expert_program/multi_segments/repeated_cube_pick_place.yaml", "max_episodes": 1, "max_episode_steps": 1200, "num_envs": 1, @@ -9,6 +10,24 @@ }, "env": { "sim_steps_per_control": 4, + "events": { + "settle_cube_on_reset": { + "func": "wait_for_dynamic_objects_to_settle", + "mode": "reset", + "params": { + "entity_cfgs": [ + { + "uid": "cube" + } + ], + "min_steps": 10, + "max_steps": 120, + "check_interval_steps": 2, + "required_stable_checks": 3, + "timeout_behavior": "raise" + } + } + }, "dataset": { "lerobot": { "func": "LeRobotRecorder", @@ -31,22 +50,7 @@ } } }, - "extensions": { - "num_cycles": 3, - "place_positions": [ - [-0.40, 0.48, 0.10], - [-0.42, -0.08, 0.10] - ], - "grasp_samples": 10000, - "force_reannotate": false, - "grasp_hold_steps": 45, - "settle_min_steps": 15, - "settle_max_steps": 80, - "settle_stable_steps": 5, - "linear_velocity_threshold": 0.03, - "angular_velocity_threshold": 0.20, - "place_position_tolerance": 0.12 - } + "extensions": {} }, "robot": { "class_type": "URRobot", diff --git a/embodichain_tasks/configs/gym/open_drawer/cobot_magic_3cam.json b/embodichain_tasks/configs/gym/open_drawer/cobot_magic_3cam.json index 60ab001f8..100fc9c21 100644 --- a/embodichain_tasks/configs/gym/open_drawer/cobot_magic_3cam.json +++ b/embodichain_tasks/configs/gym/open_drawer/cobot_magic_3cam.json @@ -1,5 +1,6 @@ { "id": "OpenDrawer-v1", + "expert_program_path": "../../expert_program/tableware/open_drawer.json", "max_episodes": 3, "max_episode_steps": 300, "env": { diff --git a/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py b/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py index 7d076b322..4ea67de33 100644 --- a/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py +++ b/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py @@ -14,25 +14,51 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Repeated cube pick-and-place task using lazy demonstration segments. +"""Declarative repeated cube pick-and-place environment. -Each segment plans one complete ``PickUp -> Place -> settle`` cycle. The outer -segment generator resumes only after the previous segment has executed and its -free-falling cube has settled. Consequently, the next pickup always plans from -the cube pose currently measured in simulation instead of a pose predicted -before the episode started. +The task declares its simulation identities and robot resources, while the +packaged Expert Program defines the three semantic pick/place cycles. Shared +Expert Program components own motion generation, execution, settling, and +validation; extending the cycle count or destinations requires config only. """ from __future__ import annotations -from collections.abc import Iterable, Sequence -from functools import partial -from typing import TYPE_CHECKING, Any +from pathlib import Path +from typing import Any -import torch - -from embodichain.lab.gym.envs import DemoSegment, EmbodiedEnv, EmbodiedEnvCfg +from embodichain.lab.gym.envs import EmbodiedEnv, EmbodiedEnvCfg +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.envs.expert_program import ( + AntipodalGraspAffordanceBinding, + ControlPartCommandPreset, + ControlPartEndpointBinding, + ControlPartResourceBinding, + ExpertProgramCfg, + ExpertProgramEnvironmentAdapter, + ExpertProgramEnvironmentMixin, + SimulationRigidObjectBinding, + SimulationExpertProgramRegistration, + SimulationRobotSkillProfileBinding, + SimulationSceneBinding, + create_simulation_expert_program_adapter, + load_expert_program, +) 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, + MotionPolicy, + PickUpOptions, + PlaceOptions, + RecoveryPolicy, + TrackingPolicy, +) from embodichain.lab.sim.cfg import ( LightCfg, RigidBodyAttributesCfg, @@ -40,21 +66,32 @@ ) from embodichain.lab.sim.robots import URRobotCfg from embodichain.lab.sim.shapes import CubeCfg -from embodichain.utils import logger - -if TYPE_CHECKING: - from embodichain.lab.sim.atomic_actions import AtomicActionEngine, ObjectSemantics - from embodichain.lab.sim.objects import RigidObject +from embodichain.lab.sim.skills import SceneCollisionRole, SceneDynamics +from embodichain.lab.sim.skills.profiles import ( + SkillPolicyPreset, + WorkflowRecoveryPolicy, +) +from embodichain.toolkits.graspkit.pg_grasp import ( + AntipodalSamplerCfg, + GraspGeneratorCfg, + GripperCollisionCfg, +) +from embodichain_tasks.configs import get_config_path -__all__ = ["MultiSegmentsCubePickPlaceEnv"] +__all__ = [ + "MultiSegmentsCubePickPlaceEnv", + "CUBE_EXPERT_PROGRAM_REGISTRATION", + "create_cube_robot_profile_binding", + "create_cube_scene_binding", +] CUBE_UID = "cube" CUBE_SIZE = 0.05 -DEFAULT_NUM_CYCLES = 3 -DEFAULT_GRASP_HOLD_STEPS = 45 -DEFAULT_PLACE_POSITIONS = ( - (-0.40, 0.48, 0.10), - (-0.42, -0.08, 0.10), +CUBE_SCENE_REGISTRY_ID = "multi_segments_cube_v1" +CUBE_ROBOT_PROFILE_ID = "ur5_parallel_gripper_v1" +CUBE_GRASP_AFFORDANCE_ID = "cube_antipodal_grasp" +CUBE_EXPERT_PROGRAM_PATH = Path( + "expert_program/multi_segments/repeated_cube_pick_place.yaml" ) GRIPPER_URDF_PATH = "DH_PGI_140_80/DH_PGI_140_80.urdf" @@ -64,11 +101,12 @@ GRIPPER_FINGER_LENGTH = 0.12 GRIPPER_ROOT_Z_WIDTH = 0.096 GRIPPER_Y_THICKNESS = 0.040 -DEFAULT_GRIPPER_CLOSE_QPOS = 0.024 +GRIPPER_OPEN_QPOS = 0.0 +GRIPPER_GRASP_QPOS = 0.024 def _create_default_robot_cfg() -> URRobotCfg: - """Create the UR5 and parallel-gripper setup used by atomic-action demos.""" + """Create the UR5 scene embodiment used by the declarative task.""" return URRobotCfg.from_dict( { "robot_type": "ur5", @@ -81,19 +119,11 @@ def _create_default_robot_cfg() -> URRobotCfg: }, ], }, - "control_parts": { - "hand": [GRIPPER_HAND_JOINT_PATTERN], - }, + "control_parts": {"hand": [GRIPPER_HAND_JOINT_PATTERN]}, "drive_pros": { - "stiffness": { - GRIPPER_HAND_JOINT_PATTERN: 1e3, - }, - "damping": { - GRIPPER_HAND_JOINT_PATTERN: 1e2, - }, - "max_effort": { - GRIPPER_HAND_JOINT_PATTERN: 1e4, - }, + "stiffness": {GRIPPER_HAND_JOINT_PATTERN: 1e3}, + "damping": {GRIPPER_HAND_JOINT_PATTERN: 1e2}, + "max_effort": {GRIPPER_HAND_JOINT_PATTERN: 1e4}, }, "solver_cfg": { "arm": { @@ -110,8 +140,18 @@ def _create_default_robot_cfg() -> URRobotCfg: ) +def _load_default_expert_program() -> ExpertProgramCfg: + """Decode the packaged semantic program for direct instantiation.""" + program = load_expert_program( + get_config_path(CUBE_EXPERT_PROGRAM_PATH), + validation_context=CUBE_EXPERT_PROGRAM_REGISTRATION.catalog, + ) + CUBE_EXPERT_PROGRAM_REGISTRATION.catalog.preflight(program) + return program + + def _create_default_env_cfg() -> EmbodiedEnvCfg: - """Create a directly-instantiable default task configuration.""" + """Create a directly-instantiable task configuration.""" cfg = EmbodiedEnvCfg() cfg.max_episode_steps = 1200 cfg.robot = _create_default_robot_cfg() @@ -141,449 +181,169 @@ def _create_default_env_cfg() -> EmbodiedEnvCfg: init_pos=(-0.42, -0.08, 0.5 * CUBE_SIZE), ) ] - cfg.extensions = { - "num_cycles": DEFAULT_NUM_CYCLES, - "place_positions": [list(position) for position in DEFAULT_PLACE_POSITIONS], - "grasp_samples": 10000, - "force_reannotate": False, - "grasp_hold_steps": DEFAULT_GRASP_HOLD_STEPS, - "settle_min_steps": 15, - "settle_max_steps": 80, - "settle_stable_steps": 5, - "linear_velocity_threshold": 0.03, - "angular_velocity_threshold": 0.20, - "place_position_tolerance": 0.12, - } - return cfg - - -@register_env("MultiSegmentsCubePickPlace-v1", max_episode_steps=1200) -class MultiSegmentsCubePickPlaceEnv(EmbodiedEnv): - """Repeatedly pick up and freely place one cube. - - The demonstration planner is intentionally lazy. It yields one complete - pick/place cycle at a time, waits for that cycle to execute and settle, and - only then reads the cube pose and plans the following cycle. - """ - - PICK_SAMPLE_INTERVAL = 120 - PLACE_SAMPLE_INTERVAL = 120 - HAND_INTERP_STEPS = 12 - - def __init__(self, cfg: EmbodiedEnvCfg | None = None, **kwargs: Any) -> None: - if cfg is None: - cfg = _create_default_env_cfg() - - extensions = getattr(cfg, "extensions", {}) or {} - self.num_cycles = int(extensions.get("num_cycles", DEFAULT_NUM_CYCLES)) - self.place_positions = self._validate_place_positions( - extensions.get("place_positions", DEFAULT_PLACE_POSITIONS) - ) - self.grasp_samples = int(extensions.get("grasp_samples", 10000)) - self.force_reannotate = bool(extensions.get("force_reannotate", False)) - self.grasp_hold_steps = int( - extensions.get("grasp_hold_steps", DEFAULT_GRASP_HOLD_STEPS) - ) - self.settle_min_steps = int(extensions.get("settle_min_steps", 15)) - self.settle_max_steps = int(extensions.get("settle_max_steps", 80)) - self.settle_stable_steps = int(extensions.get("settle_stable_steps", 5)) - self.linear_velocity_threshold = float( - extensions.get("linear_velocity_threshold", 0.03) - ) - self.angular_velocity_threshold = float( - extensions.get("angular_velocity_threshold", 0.20) - ) - self.place_position_tolerance = float( - extensions.get("place_position_tolerance", 0.12) - ) - self._validate_settings() - - super().__init__(cfg, **kwargs) - - # ``EmbodiedEnv`` exposes extension values as instance attributes. - # Re-normalize them because that binding intentionally preserves the - # JSON-native list/scalar types supplied by the launcher. - self.num_cycles = int(self.num_cycles) - self.place_positions = self._validate_place_positions(self.place_positions) - self.grasp_samples = int(self.grasp_samples) - self.force_reannotate = bool(self.force_reannotate) - self.grasp_hold_steps = int(self.grasp_hold_steps) - self.settle_min_steps = int(self.settle_min_steps) - self.settle_max_steps = int(self.settle_max_steps) - self.settle_stable_steps = int(self.settle_stable_steps) - self.linear_velocity_threshold = float(self.linear_velocity_threshold) - self.angular_velocity_threshold = float(self.angular_velocity_threshold) - self.place_position_tolerance = float(self.place_position_tolerance) - self._validate_settings() - - cube = self.sim.get_rigid_object(CUBE_UID) - if cube is None: - raise RuntimeError(f"Task requires a rigid object with uid {CUBE_UID!r}.") - self._cube: RigidObject = cube - self._completed_cycles = 0 - self._planned_cycle_count = 0 - self._last_target_position: torch.Tensor | None = None - self._initialize_atomic_actions() - - @staticmethod - def _validate_place_positions( - positions: Sequence[Sequence[float]], - ) -> tuple[tuple[float, float, float], ...]: - """Validate and normalize release positions from task configuration.""" - normalized = tuple( - tuple(float(value) for value in position) for position in positions - ) - if not normalized or any(len(position) != 3 for position in normalized): - raise ValueError("place_positions must contain at least one XYZ position.") - return normalized - - def _validate_settings(self) -> None: - """Validate task settings before allocating a simulation.""" - if self.num_cycles < 1: - raise ValueError("num_cycles must be at least 1.") - if self.grasp_samples < 1: - raise ValueError("grasp_samples must be at least 1.") - if self.grasp_hold_steps < 0: - raise ValueError("grasp_hold_steps must be non-negative.") - if not 0 <= self.settle_min_steps <= self.settle_max_steps: - raise ValueError( - "settle_min_steps must be non-negative and no larger than " - "settle_max_steps." - ) - if self.settle_stable_steps < 1: - raise ValueError("settle_stable_steps must be at least 1.") - if self.linear_velocity_threshold < 0 or self.angular_velocity_threshold < 0: - raise ValueError("Velocity thresholds must be non-negative.") - if self.place_position_tolerance <= 0: - raise ValueError("place_position_tolerance must be positive.") - - def _initialize_atomic_actions(self) -> None: - """Create the motion generator, action engine and cube semantics.""" - from embodichain.lab.sim.atomic_actions import ( - AtomicActionEngine, - ControlPartCommandProfile, - ) - from embodichain.lab.sim.planners import ( - MotionGenCfg, - MotionGenerator, - ToppraPlannerCfg, - ) - - hand_limits = self.robot.get_qpos_limits(name="hand")[0].to( - device=self.device, dtype=torch.float32 - ) - hand_open_qpos = hand_limits[:, 0] - hand_close_qpos = torch.clamp( - torch.full_like(hand_limits[:, 1], DEFAULT_GRIPPER_CLOSE_QPOS), - min=hand_limits[:, 0], - max=hand_limits[:, 1], - ) - motion_generator = MotionGenerator( - cfg=MotionGenCfg(planner_cfg=ToppraPlannerCfg(robot_uid=self.robot.uid)) - ) - self._action_engine: AtomicActionEngine = AtomicActionEngine( - motion_generator, - control_profiles={ - "hand": ControlPartCommandProfile.joint_positions( - open=hand_open_qpos, - grasp=hand_close_qpos, - ) + cfg.extensions = {} + cfg.events = { + "settle_cube_on_reset": EventCfg( + func=wait_for_dynamic_objects_to_settle, + mode="reset", + params={ + "entity_cfgs": [SceneEntityCfg(uid=CUBE_UID)], + "min_steps": 10, + "max_steps": 120, + "check_interval_steps": 2, + "required_stable_checks": 3, + "timeout_behavior": "raise", }, ) - self._cube_semantics: ObjectSemantics = self._create_cube_semantics() + } + cfg.expert_program = _load_default_expert_program() + return cfg - def _create_cube_semantics(self) -> ObjectSemantics: - """Create reusable antipodal semantics for the task cube.""" - from embodichain.lab.sim.atomic_actions import ( - AntipodalAffordance, - ObjectSemantics, - ) - from embodichain.toolkits.graspkit.pg_grasp.antipodal_generator import ( - AntipodalSamplerCfg, - GraspGeneratorCfg, - ) - from embodichain.toolkits.graspkit.pg_grasp.gripper_collision_checker import ( - GripperCollisionCfg, - ) - vertices = self._cube.get_vertices(env_ids=[0], scale=True)[0] - triangles = self._cube.get_triangles(env_ids=[0])[0] - return ObjectSemantics( - label=CUBE_UID, - geometry={}, - affordance=AntipodalAffordance( - mesh_vertices=vertices, - mesh_triangles=triangles, - 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, - ), +def create_cube_scene_binding( + *, + grasp_samples: int = 10000, + force_reannotate: bool = False, +) -> SimulationSceneBinding: + """Declare the cube and its exact 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=CUBE_SCENE_REGISTRY_ID, + rigid_objects=( + SimulationRigidObjectBinding( + entity_id=CUBE_UID, + simulation_uid=CUBE_UID, + dynamics=SceneDynamics.DYNAMIC, + collision_role=SceneCollisionRole.NONE, + semantic_type="cube", + default_grasp_affordance=CUBE_GRASP_AFFORDANCE_ID, + ), + ), + antipodal_grasps=( + AntipodalGraspAffordanceBinding( + entity_id=CUBE_GRASP_AFFORDANCE_ID, + object_id=CUBE_UID, + native_name="cube_mesh_antipodal", + revision="cube-antipodal-v1", generator_cfg=GraspGeneratorCfg( viser_port=11801, antipodal_sampler_cfg=AntipodalSamplerCfg( - n_sample=self.grasp_samples, + n_sample=grasp_samples, max_length=GRIPPER_MAX_OPEN_WIDTH, min_length=0.005, ), is_partial_annotate=False, is_filter_ground_collision=False, ), - force_reannotate=self.force_reannotate, - ), - entity=self._cube, - ) - - def create_demo_segments( - self, *, num_cycles: int | None = None, **kwargs: Any - ) -> Iterable[DemoSegment]: - """Lazily plan repeated cube pick-and-place segments. - - Args: - num_cycles: Optional per-rollout override for the configured cycle count. - **kwargs: Reserved for future expert-planning options. - - Yields: - One :class:`DemoSegment` for every pickup/place cycle. - """ - del kwargs - cycle_count = self.num_cycles if num_cycles is None else int(num_cycles) - if cycle_count < 1: - raise ValueError("num_cycles must be at least 1.") - - self._completed_cycles = 0 - self._planned_cycle_count = cycle_count - self._last_target_position = None - for cycle_index in range(cycle_count): - target_position = torch.tensor( - self.place_positions[cycle_index % len(self.place_positions)], - dtype=torch.float32, - device=self.device, - ) - plan_success, actions, source_pose = self._plan_pick_place_cycle( - target_position - ) - self._last_target_position = target_position - source_position = source_pose[:, :3, 3].detach().cpu().tolist() - logger.log_info( - f"Planned cube pick/place segment {cycle_index + 1}/{cycle_count} " - f"from {source_position} to {target_position.detach().cpu().tolist()}." - ) - yield DemoSegment( - actions=actions, - name=f"cube_pick_place_{cycle_index + 1}", - target_uid=CUBE_UID, - instruction=( - "Pick up the cube from its current settled pose and freely " - f"place it at target {cycle_index + 1}." - ), - metadata={ - "cycle_index": cycle_index, - "cycle_count": cycle_count, - "planning_success": plan_success.detach().cpu().tolist(), - "planned_source_poses": source_pose.detach().cpu().tolist(), - "target_position": target_position.detach().cpu().tolist(), - "free_fall_settle": True, - }, - validator=partial( - self._validate_cycle, - plan_success.detach().clone(), - target_position.detach().clone(), + 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, ), - ) - # Execution and validation happen while the generator is suspended at - # ``yield``. Advancing to the next iteration therefore means that the - # cube has already reached its new, measured scene pose. - self._completed_cycles = cycle_index + 1 + force_reannotate=force_reannotate, + ), + ), + ) - def _plan_pick_place_cycle( - self, target_position: torch.Tensor - ) -> tuple[torch.Tensor, Iterable[torch.Tensor], torch.Tensor]: - """Plan one pickup/place cycle from the cube's current measured pose.""" - from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, - GraspGoal, - MotionPolicy, - PickUpOptions, - PlaceGoal, - PlaceOptions, - ) - source_pose = self._cube.get_local_pose(to_matrix=True).to( - device=self.device, dtype=torch.float32 - ) - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ) - pick_compiled = self._action_engine.compile( - ( - ActionInvocation( - skill_id="pick_up", - goal=GraspGoal(self._cube_semantics), - binding=binding, - motion_policy=MotionPolicy(sample_count=self.PICK_SAMPLE_INTERVAL), - skill_options=PickUpOptions( - pre_grasp_distance=0.15, - lift_height=0.16, - hand_interp_steps=self.HAND_INTERP_STEPS, +def create_cube_robot_profile_binding() -> SimulationRobotSkillProfileBinding: + """Declare the UR5 arm and parallel-gripper semantic resource.""" + motion_capabilities = frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } + ) + return SimulationRobotSkillProfileBinding( + profile_id=CUBE_ROBOT_PROFILE_ID, + resources=( + ControlPartResourceBinding( + resource_id="manipulator", + endpoints=( + ControlPartEndpointBinding( + endpoint_id="motion", + control_part="arm", + capabilities=motion_capabilities, ), - ), - ) - ) - pick_success = pick_compiled.plan_success - pick_trajectory = pick_compiled.trajectory.positions - picked_context = pick_compiled.projected_context - held = picked_context.get_held_object("arm") - if held is None or not bool(pick_success.all().item()): - trajectory = self._ensure_nonempty_trajectory(pick_trajectory) - return ( - torch.zeros_like(pick_success, dtype=torch.bool), - self._iter_cycle_actions(trajectory, clear_dynamics_step=None), - source_pose, - ) - - pick_trajectory, clear_dynamics_step = self._insert_grasp_hold(pick_trajectory) - desired_cube_pose = source_pose.clone() - desired_cube_pose[:, :3, 3] = target_position.unsqueeze(0).expand( - self.num_envs, -1 - ) - place_eef_pose = torch.bmm(desired_cube_pose, held.object_to_eef) - place_compiled = self._action_engine.compile( - ( - ActionInvocation( - skill_id="place", - goal=PlaceGoal(place_eef_pose), - binding=binding, - motion_policy=MotionPolicy(sample_count=self.PLACE_SAMPLE_INTERVAL), - skill_options=PlaceOptions( - lift_height=0.14, - hand_interp_steps=self.HAND_INTERP_STEPS, + ControlPartEndpointBinding( + endpoint_id="grasp", + control_part="hand", + capabilities=frozenset({GRASP_CAPABILITY}), + command_preset="parallel_gripper", ), ), ), - picked_context, - ) - place_success = place_compiled.plan_success - place_trajectory = place_compiled.trajectory.positions - trajectory = self._ensure_nonempty_trajectory( - torch.cat((pick_trajectory, place_trajectory), dim=1) - ) - return ( - pick_success & place_success, - self._iter_cycle_actions(trajectory, clear_dynamics_step), - source_pose, - ) - - def _insert_grasp_hold( - self, pick_trajectory: torch.Tensor - ) -> tuple[torch.Tensor, int]: - """Hold the closed command at the grasp pose before beginning the lift.""" - close_end_step = min( - int(round(self.PICK_SAMPLE_INTERVAL - self.HAND_INTERP_STEPS) * 0.6) - + self.HAND_INTERP_STEPS, - pick_trajectory.shape[1], - ) - if self.grasp_hold_steps == 0: - return pick_trajectory, close_end_step - - grasp_hold = pick_trajectory[:, close_end_step - 1 : close_end_step, :].repeat( - 1, self.grasp_hold_steps, 1 - ) - augmented = torch.cat( - ( - pick_trajectory[:, :close_end_step, :], - grasp_hold, - pick_trajectory[:, close_end_step:, :], + ), + command_presets=( + ControlPartCommandPreset( + preset_id="parallel_gripper", + control_part="hand", + commands={ + "open": (GRIPPER_OPEN_QPOS,), + "grasp": (GRIPPER_GRASP_QPOS,), + }, ), - dim=1, - ) - return augmented, close_end_step + self.grasp_hold_steps - - def _ensure_nonempty_trajectory(self, trajectory: torch.Tensor) -> torch.Tensor: - """Return at least one hold command so planning failure is recordable.""" - if trajectory.shape[1] > 0: - return trajectory - return self.robot.get_qpos().clone().unsqueeze(1) + ), + defaults={ + "pick_up": {"primary": "manipulator"}, + "place": {"primary": "manipulator"}, + }, + presets=( + SkillPolicyPreset( + "safe", + action_option_templates={ + "pick": PickUpOptions(), + "place": PlaceOptions(), + }, + motion_policy=MotionPolicy(sample_count=100), + recovery_policy=RecoveryPolicy(), + workflow_recovery_policy=WorkflowRecoveryPolicy( + max_recovery_attempts=2, + ), + tracking_policy=TrackingPolicy.joint_position( + in_flight_max_abs_error=0.08, + terminal_max_abs_error=0.08, + ), + ), + ), + default_preset="safe", + ) - def _iter_cycle_actions( - self, - trajectory: torch.Tensor, - clear_dynamics_step: int | None, - ) -> Iterable[torch.Tensor]: - """Replay a planned trajectory, then hold until the cube is stable.""" - for step_index, action in enumerate(trajectory.unbind(dim=1), start=1): - yield action - if clear_dynamics_step is not None and step_index == clear_dynamics_step: - # Match the pickup tutorial: clear residual object velocity just - # after gripper closure and before the lift phase. - self._cube.clear_dynamics() - hold_action = trajectory[:, -1].clone() - stable_steps = 0 - for settle_step in range(self.settle_max_steps): - yield hold_action - if settle_step + 1 < self.settle_min_steps: - continue - if bool(self._cube_is_stable().all().item()): - stable_steps += 1 - if stable_steps >= self.settle_stable_steps: - break - else: - stable_steps = 0 +CUBE_EXPERT_PROGRAM_REGISTRATION = SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(), + robot_profile_binding=create_cube_robot_profile_binding(), +) - def _cube_is_stable(self) -> torch.Tensor: - """Return whether cube linear and angular speeds are below thresholds.""" - linear_speed = torch.linalg.vector_norm(self._cube.body_data.lin_vel, dim=-1) - angular_speed = torch.linalg.vector_norm(self._cube.body_data.ang_vel, dim=-1) - return (linear_speed <= self.linear_velocity_threshold) & ( - angular_speed <= self.angular_velocity_threshold - ) - def _cube_settled_near(self, target_position: torch.Tensor) -> torch.Tensor: - """Validate that the cube settled near a release target after free fall.""" - cube_position = self._cube.get_local_pose(to_matrix=True)[:, :3, 3] - target_position = target_position.to( - device=cube_position.device, dtype=cube_position.dtype - ) - xy_error = torch.linalg.vector_norm( - cube_position[:, :2] - target_position[None, :2], dim=-1 - ) - valid_height = (cube_position[:, 2] >= -0.01) & ( - cube_position[:, 2] <= target_position[2] + CUBE_SIZE - ) - return ( - (xy_error <= self.place_position_tolerance) - & valid_height - & self._cube_is_stable() - ) +@register_env( + "MultiSegmentsCubePickPlace-v1", + max_episode_steps=1200, + expert_program_registration=CUBE_EXPERT_PROGRAM_REGISTRATION, +) +class MultiSegmentsCubePickPlaceEnv(ExpertProgramEnvironmentMixin, EmbodiedEnv): + """Repeatedly pick and place a cube from a semantic config program.""" - def _validate_cycle( - self, plan_success: torch.Tensor, target_position: torch.Tensor - ) -> torch.Tensor: - """Combine motion-planning and post-free-fall validation.""" - return plan_success.to(device=self.device, dtype=torch.bool) & ( - self._cube_settled_near(target_position) + 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=CUBE_EXPERT_PROGRAM_REGISTRATION, ) - def is_task_success(self, **kwargs: Any) -> torch.Tensor: - """Return success after all lazy segments have executed and validated. - - Args: - **kwargs: Reserved for task-evaluation options. - - Returns: - One success flag per parallel environment. - """ - del kwargs - if ( - self._planned_cycle_count < 1 - or self._completed_cycles < self._planned_cycle_count - or self._last_target_position is None - ): - return torch.zeros(self.num_envs, dtype=torch.bool, device=self.device) - return self._cube_settled_near(self._last_target_position) + @property + def expert_program_adapter(self) -> ExpertProgramEnvironmentAdapter: + """Return the shared adapter assembled for this environment.""" + return self._expert_program_adapter 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/blocks_ranking_rgb.py b/embodichain_tasks/embodichain_tasks/tableware/blocks_ranking_rgb.py index 5e09bac89..152de4380 100644 --- a/embodichain_tasks/embodichain_tasks/tableware/blocks_ranking_rgb.py +++ b/embodichain_tasks/embodichain_tasks/tableware/blocks_ranking_rgb.py @@ -211,7 +211,6 @@ def _plan_block_segment( ) -> tuple[torch.Tensor, Iterable[torch.Tensor], torch.Tensor]: """Plan an atomic PickUp followed by Place for one block.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, GraspGoal, MotionPolicy, @@ -233,9 +232,19 @@ def _plan_block_segment( source_pose[:, :3, :3], local_grasp_offset.unsqueeze(-1) ).squeeze(-1) grasp_pose[:, :3, 3] = source_pose[:, :3, 3] + world_grasp_offset - binding = ActionBinding( - manipulators={"primary": arm}, - end_effectors={"primary": hand}, + endpoints = { + "primary": { + "motion": arm, + "grasp": hand, + } + } + pick_binding = self._action_engine.bind_control_parts( + "pick_up", + endpoints, + ) + place_binding = self._action_engine.bind_control_parts( + "place", + endpoints, ) pick_compiled = self._action_engine.compile( ( @@ -245,7 +254,7 @@ def _plan_block_segment( self._object_semantics[uid], grasp_xpos=grasp_pose, ), - binding=binding, + binding=pick_binding, motion_policy=MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( pre_grasp_distance=0.12, @@ -253,7 +262,8 @@ def _plan_block_segment( hand_interp_steps=HAND_INTERP_STEPS, ), ), - ) + ), + self._action_engine.initial_context(control_dt=self.step_dt), ) pick_success = pick_compiled.plan_success pick_trajectory = pick_compiled.trajectory.positions @@ -277,7 +287,7 @@ def _plan_block_segment( ActionInvocation( skill_id="place", goal=PlaceGoal(place_eef_pose), - binding=binding, + binding=place_binding, motion_policy=MotionPolicy(sample_count=PLACE_SAMPLE_INTERVAL), skill_options=PlaceOptions( lift_height=0.15, 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..4f8d3a77a --- /dev/null +++ b/embodichain_tasks/embodichain_tasks/tableware/hand_over.py @@ -0,0 +1,509 @@ +# ---------------------------------------------------------------------------- +# 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, + WorkflowRecoveryPolicy, +) +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), + workflow_recovery_policy=WorkflowRecoveryPolicy( + max_recovery_attempts=2, + ), + 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/embodichain_tasks/embodichain_tasks/tableware/open_drawer.py b/embodichain_tasks/embodichain_tasks/tableware/open_drawer.py index 3b4cbdc09..e645e0ba4 100644 --- a/embodichain_tasks/embodichain_tasks/tableware/open_drawer.py +++ b/embodichain_tasks/embodichain_tasks/tableware/open_drawer.py @@ -14,232 +14,229 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Expert demonstration environment for opening a drawer.""" +"""Declarative expert environment for opening a sliding drawer. + +The task owns only scene and embodiment declarations. The packaged Expert +Program selects the semantic ``operate_articulation`` skill and its named +``open`` target; shared runtime components generate and execute all motion. +""" from __future__ import annotations from typing import Any -import torch - from embodichain.lab.gym.envs import EmbodiedEnv, EmbodiedEnvCfg +from embodichain.lab.gym.envs.expert_program import ( + ArticulationOperationAffordanceBinding, + ArticulationOperationTargetBinding, + ControlPartCommandPreset, + ControlPartEndpointBinding, + ControlPartResourceBinding, + ExpertProgramEnvironmentAdapter, + ExpertProgramEnvironmentMixin, + SimulationArticulationBinding, + SimulationArticulationLinkBinding, + SimulationExpertProgramRegistration, + SimulationRobotSkillProfileBinding, + SimulationSceneBinding, + create_simulation_expert_program_adapter, +) from embodichain.lab.gym.utils.registration import register_env -from embodichain.lab.sim.planners import ( - MotionGenCfg, - MotionGenerator, - MotionGenOptions, - MoveType, - PlanResult, - PlanState, - ToppraPlannerCfg, - ToppraPlanOptions, - TrajectorySampleMethod, +from embodichain.lab.sim.atomic_actions import ( + CARTESIAN_POSE_CAPABILITY, + GRASP_CAPABILITY, + JOINT_POSITION_CAPABILITY, + OperateArticulationOptions, +) +from embodichain.lab.sim.skills import SceneCollisionRole, SceneDynamics +from embodichain.lab.sim.skills.profiles import SkillPolicyPreset + +__all__ = [ + "OpenDrawerEnv", + "OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION", + "create_open_drawer_robot_profile_binding", + "create_open_drawer_scene_binding", +] + +DRAWER_SCENE_REGISTRY_ID = "open_drawer_v1" +DRAWER_ROBOT_PROFILE_ID = "cobot_magic_right_manipulator_v1" +DRAWER_UID = "drawer" +DRAWER_HANDLE_LINK_ID = "drawer_handle_link" +DRAWER_HANDLE_AFFORDANCE_ID = "drawer_handle" +DRAWER_NATIVE_HANDLE_LINK = "handle_xpos" +DRAWER_NATIVE_SLIDE_JOINT = "slide_rails" +DRAWER_OPEN_POSITION = 0.11 +DRAWER_OPEN_DISPLACEMENT = 0.11 + +# Rotation from the drawer handle frame to the historical right-arm TCP frame. +_HANDLE_POSE_OFFSET = ( + -0.023958006, + -0.999453075, + -0.022793945, + 0.0, + 0.999712744, + -0.023966955, + 0.000119456, + 0.0, + -0.000665692, + -0.022784535, + 0.999740177, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, ) -from embodichain.lab.sim.utility.action_utils import interpolate_with_nums - -__all__ = ["OpenDrawerEnv"] - - -def _require_plan_positions(result: PlanResult, *, phase: str) -> torch.Tensor: - """Return a successful single-environment trajectory. - - Args: - result: Motion-planning result to validate. - phase: Human-readable planning phase for error reporting. - - Returns: - Joint positions for the task's single environment. - - Raises: - RuntimeError: If planning failed or returned no joint positions. - """ - if not result.is_all_success(): - raise RuntimeError(f"Motion planning failed during {phase}.") - if result.positions is None: - raise RuntimeError( - f"Motion planning returned no joint positions during {phase}." - ) - return result.positions[0] - - -@register_env("OpenDrawer-v1", max_episode_steps=300) -class OpenDrawerEnv(EmbodiedEnv): - """Open a sliding drawer with the right arm of a CobotMagic robot.""" - - def __init__(self, cfg: EmbodiedEnvCfg, **kwargs: Any) -> None: - """Initialize the environment and its TOPPRA motion generator. - Args: - cfg: Declarative environment configuration. - **kwargs: Additional arguments forwarded to :class:`EmbodiedEnv`. - """ - super().__init__(cfg, **kwargs) - self.motion_gen = MotionGenerator( - cfg=MotionGenCfg( - planner_cfg=ToppraPlannerCfg( - robot_uid=self.robot.uid, - ) - ) - ) - self.eef_open = self.robot.get_qpos_limits(name="right_eef")[:, :, 1] - self.eef_close = self.robot.get_qpos_limits(name="right_eef")[:, :, 0] - - def _generate_eef_motion( - self, num_steps: int = 10, *, opening: bool = True - ) -> torch.Tensor: - """Interpolate the right gripper between its closed and open limits. - - Args: - num_steps: Number of trajectory samples. - opening: Whether to open rather than close the gripper. - - Returns: - Gripper joint trajectory with shape ``(num_steps, eef_dof)``. - """ - if num_steps < 2: - raise ValueError("num_steps must be at least 2.") - - current_qpos = self.eef_close if opening else self.eef_open - target_qpos = self.eef_open if opening else self.eef_close - return interpolate_with_nums( - torch.stack([current_qpos, target_qpos], dim=1), - interp_nums=[num_steps - 1], - device=self.device, - ).squeeze(0) - - def create_demo_action_list(self, *args: Any, **kwargs: Any) -> torch.Tensor: - """Generate an expert trajectory that grasps and pulls the drawer handle. - - The demonstration is defined for the single-environment CobotMagic task - configuration and consists of four phases: move to the start pose, - approach the handle, close the gripper, and pull the drawer open. - - Returns: - Joint-position actions with shape ``(num_steps, action_dof)``. - - Raises: - ValueError: If the environment contains more than one arena. - RuntimeError: If any motion-planning phase fails. - """ - if self.num_envs != 1: - raise ValueError( - "OpenDrawerEnv expert demonstrations currently require num_envs=1." - ) - - qpos_start = torch.tensor( - [[0.0, 2.06, -0.75, 0.0, -1.20, 1.6]], - dtype=torch.float32, - device=self.device, - ) - - options_to_start = MotionGenOptions( - control_part="right_arm", - is_interpolate=True, - start_qpos=self.robot.get_qpos("right_arm")[0], - plan_opts=ToppraPlanOptions( - sample_method=TrajectorySampleMethod.QUANTITY, - sample_interval=50, +def _translation_pose(x: float, y: float, z: float) -> tuple[float, ...]: + """Return a flattened identity-rotation pose with one translation.""" + return ( + 1.0, + 0.0, + 0.0, + x, + 0.0, + 1.0, + 0.0, + y, + 0.0, + 0.0, + 1.0, + z, + 0.0, + 0.0, + 0.0, + 1.0, + ) + + +def create_open_drawer_scene_binding() -> SimulationSceneBinding: + """Declare the exact native drawer identities used by the semantic task.""" + approach = _translation_pose(-0.00442594, -0.00050044, -0.10508996) + contact = _translation_pose(-0.00442594, -0.00050041, 0.00491005) + retract = _translation_pose(-0.00442594, -0.00050044, -0.00508996) + return SimulationSceneBinding( + registry_id=DRAWER_SCENE_REGISTRY_ID, + articulations=( + SimulationArticulationBinding( + entity_id=DRAWER_UID, + simulation_uid=DRAWER_UID, + dynamics=SceneDynamics.DYNAMIC, + collision_role=SceneCollisionRole.NONE, + semantic_type="sliding_drawer", + default_operation_affordance=DRAWER_HANDLE_AFFORDANCE_ID, ), - ) - plan_to_start_result = self.motion_gen.generate( - target_states=[ - PlanState.single(move_type=MoveType.JOINT_MOVE, qpos=qpos_start[0]) - ], - options=options_to_start, - ) - plan_to_start = _require_plan_positions( - plan_to_start_result, phase="move to start" - ) - - xpos_begin = self.robot.compute_fk( - name="right_arm", qpos=qpos_start, to_matrix=True - )[0] - xpos_mid = xpos_begin.clone() - xpos_mid[0, 3] += 0.11 - - options_to_handle = MotionGenOptions( - control_part="right_arm", - is_interpolate=True, - is_linear=True, - start_qpos=qpos_start[0], - plan_opts=ToppraPlanOptions( - sample_method=TrajectorySampleMethod.QUANTITY, - sample_interval=50, + ), + links=( + SimulationArticulationLinkBinding( + entity_id=DRAWER_HANDLE_LINK_ID, + articulation_id=DRAWER_UID, + native_link_name=DRAWER_NATIVE_HANDLE_LINK, + dynamics=SceneDynamics.DYNAMIC, + semantic_type="drawer_handle_link", ), - ) - plan_to_handle_result = self.motion_gen.generate( - target_states=[ - PlanState.single(move_type=MoveType.EEF_MOVE, xpos=xpos) - for xpos in (xpos_begin, xpos_mid) - ], - options=options_to_handle, - ) - plan_to_handle = _require_plan_positions( - plan_to_handle_result, phase="handle approach" - ) - - options_leave_handle = MotionGenOptions( - control_part="right_arm", - is_interpolate=True, - is_linear=True, - start_qpos=plan_to_handle[-1], - plan_opts=ToppraPlanOptions( - sample_method=TrajectorySampleMethod.QUANTITY, - sample_interval=50, + ), + articulation_operations=( + ArticulationOperationAffordanceBinding( + entity_id=DRAWER_HANDLE_AFFORDANCE_ID, + articulation_id=DRAWER_UID, + link_id=DRAWER_HANDLE_LINK_ID, + joint_id=DRAWER_NATIVE_SLIDE_JOINT, + revision="open-drawer-v1", + semantic_targets={ + "open": ArticulationOperationTargetBinding( + target_position=DRAWER_OPEN_POSITION, + displacement=DRAWER_OPEN_DISPLACEMENT, + ), + }, + handle_pose_offset=_HANDLE_POSE_OFFSET, + approach_offset=approach, + contact_offset=contact, + operation_offset=contact, + retract_offset=retract, + operation_axis=(0.0, 0.0, -1.0), + position_scale=1.0, ), - ) - plan_leave_handle_result = self.motion_gen.generate( - target_states=[ - PlanState.single(move_type=MoveType.EEF_MOVE, xpos=xpos) - for xpos in (xpos_mid, xpos_begin) - ], - options=options_leave_handle, - ) - plan_leave_handle = _require_plan_positions( - plan_leave_handle_result, phase="drawer pull" - ) - - num_grasp_steps = 20 - eef_grasp_motion = self._generate_eef_motion( - num_steps=num_grasp_steps, opening=False - ) - - len_to_start = plan_to_start.shape[0] - len_to_handle = plan_to_handle.shape[0] - len_leave_handle = plan_leave_handle.shape[0] - total_len = len_to_start + len_to_handle + num_grasp_steps + len_leave_handle - trajectory = torch.zeros( - (total_len, self.robot.dof), - dtype=torch.float32, - device=self.device, - ) + ), + ) + + +def create_open_drawer_robot_profile_binding() -> SimulationRobotSkillProfileBinding: + """Declare the CobotMagic right-arm and right-gripper skill resource.""" + return SimulationRobotSkillProfileBinding( + profile_id=DRAWER_ROBOT_PROFILE_ID, + resources=( + ControlPartResourceBinding( + resource_id="right_manipulator", + endpoints=( + ControlPartEndpointBinding( + endpoint_id="motion", + control_part="right_arm", + capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + JOINT_POSITION_CAPABILITY, + } + ), + ), + ControlPartEndpointBinding( + endpoint_id="interaction", + control_part="right_eef", + capabilities=frozenset({GRASP_CAPABILITY}), + command_preset="right_parallel_gripper", + ), + ), + ), + ), + command_presets=( + ControlPartCommandPreset( + preset_id="right_parallel_gripper", + control_part="right_eef", + commands={ + "open": (0.05, 0.05), + "grasp": (0.0, 0.0), + }, + ), + ), + defaults={ + "operate_articulation": {"primary": "right_manipulator"}, + }, + presets=( + SkillPolicyPreset( + "safe", + action_option_templates={ + "operate_articulation": OperateArticulationOptions(), + }, + ), + ), + default_preset="safe", + ) - right_arm_ids = self.robot.get_joint_ids("right_arm") - right_eef_ids = self.robot.get_joint_ids("right_eef") - idx = 0 - trajectory[idx : idx + len_to_start, right_arm_ids] = plan_to_start - trajectory[idx : idx + len_to_start, right_eef_ids] = self._generate_eef_motion( - num_steps=len_to_start, opening=True - ) - idx += len_to_start +OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION = SimulationExpertProgramRegistration( + scene_binding=create_open_drawer_scene_binding(), + robot_profile_binding=create_open_drawer_robot_profile_binding(), +) - trajectory[idx : idx + len_to_handle, right_arm_ids] = plan_to_handle - trajectory[idx : idx + len_to_handle, right_eef_ids] = self.eef_open.expand( - len_to_handle, -1 - ) - idx += len_to_handle - trajectory[idx : idx + num_grasp_steps, right_arm_ids] = ( - plan_to_handle[-1].unsqueeze(0).expand(num_grasp_steps, -1) - ) - trajectory[idx : idx + num_grasp_steps, right_eef_ids] = eef_grasp_motion - idx += num_grasp_steps +@register_env( + "OpenDrawer-v1", + max_episode_steps=300, + expert_program_registration=OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION, +) +class OpenDrawerEnv(ExpertProgramEnvironmentMixin, EmbodiedEnv): + """Open a drawer through a configured semantic Expert Program.""" - trajectory[idx : idx + len_leave_handle, right_arm_ids] = plan_leave_handle - trajectory[idx : idx + len_leave_handle, right_eef_ids] = self.eef_close.expand( - len_leave_handle, -1 + def __init__(self, cfg: EmbodiedEnvCfg, **kwargs: Any) -> None: + """Initialize the configured scene without task-level motion code.""" + super().__init__(cfg, **kwargs) + self._expert_program_adapter = create_simulation_expert_program_adapter( + self, + registration=OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION, ) - return trajectory[:, self.active_joint_ids] + @property + def expert_program_adapter(self) -> ExpertProgramEnvironmentAdapter: + """Return the shared adapter assembled for this environment.""" + return self._expert_program_adapter diff --git a/embodichain_tasks/embodichain_tasks/tableware/stack_blocks_two.py b/embodichain_tasks/embodichain_tasks/tableware/stack_blocks_two.py index 9001f0c73..7e6021d31 100644 --- a/embodichain_tasks/embodichain_tasks/tableware/stack_blocks_two.py +++ b/embodichain_tasks/embodichain_tasks/tableware/stack_blocks_two.py @@ -133,7 +133,6 @@ def _plan_stack( ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor]: """Plan PickUp then Place while threading the held-object state.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, GraspGoal, MotionPolicy, @@ -156,9 +155,19 @@ def _plan_stack( grasp_pose[:, :3, 3] = source_pose[:, :3, 3] + torch.tensor( GRASP_OFFSET, dtype=torch.float32, device=self.device ) - binding = ActionBinding( - manipulators={"primary": CONTROL_PART}, - end_effectors={"primary": HAND_CONTROL_PART}, + endpoints = { + "primary": { + "motion": CONTROL_PART, + "grasp": HAND_CONTROL_PART, + } + } + pick_binding = self._action_engine.bind_control_parts( + "pick_up", + endpoints, + ) + place_binding = self._action_engine.bind_control_parts( + "place", + endpoints, ) pick_compiled = self._action_engine.compile( ( @@ -168,7 +177,7 @@ def _plan_stack( self._stack_block_semantics, grasp_xpos=grasp_pose, ), - binding=binding, + binding=pick_binding, motion_policy=MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( pre_grasp_distance=0.12, @@ -176,7 +185,8 @@ def _plan_stack( hand_interp_steps=HAND_INTERP_STEPS, ), ), - ) + ), + self._action_engine.initial_context(control_dt=self.step_dt), ) pick_success = pick_compiled.plan_success pick_trajectory = pick_compiled.trajectory.positions @@ -201,7 +211,7 @@ def _plan_stack( ActionInvocation( skill_id="place", goal=PlaceGoal(place_eef_pose), - binding=binding, + binding=place_binding, motion_policy=MotionPolicy(sample_count=PLACE_SAMPLE_INTERVAL), skill_options=PlaceOptions( lift_height=0.10, diff --git a/examples/sim/demo/grasp_cup_to_caffe.py b/examples/sim/demo/grasp_cup_to_caffe.py index a36f68925..dde7cbaae 100644 --- a/examples/sim/demo/grasp_cup_to_caffe.py +++ b/examples/sim/demo/grasp_cup_to_caffe.py @@ -257,7 +257,7 @@ def create_trajectory( caffe (Robot): The caffe object. Returns: - torch.Tensor: Interpolated trajectory of shape [n_envs, n_waypoint, dof]. + torch.Tensor: Interpolated trajectory of shape [num_envs, n_waypoint, dof]. """ right_arm_ids = robot.get_joint_ids("right_arm") hand_open_qpos = torch.tensor( @@ -274,7 +274,7 @@ def create_trajectory( cup_position = cup.get_local_pose(to_matrix=True)[:, :3, 3] # grasp cup waypoint generation - rest_right_qpos = robot.get_qpos()[:, right_arm_ids] # [n_envs, dof] + rest_right_qpos = robot.get_qpos()[:, right_arm_ids] # [num_envs, dof] right_arm_xpos = robot.compute_fk( qpos=rest_right_qpos, name="right_arm", to_matrix=True ) @@ -324,7 +324,7 @@ def create_trajectory( pose=place_down_pose, joint_seed=place_up_qpos, name="right_arm" ) - n_envs = sim.num_envs + num_envs = sim.num_envs # combine hand and arm trajectory arm_trajectory = torch.cat( @@ -344,21 +344,21 @@ def create_trajectory( ) hand_trajectory = torch.cat( [ - hand_open_qpos[None, None, :].repeat(n_envs, 1, 1), - hand_open_qpos[None, None, :].repeat(n_envs, 1, 1), - hand_open_qpos[None, None, :].repeat(n_envs, 1, 1), - hand_close_qpos[None, None, :].repeat(n_envs, 1, 1), - hand_close_qpos[None, None, :].repeat(n_envs, 1, 1), - hand_close_qpos[None, None, :].repeat(n_envs, 1, 1), - hand_close_qpos[None, None, :].repeat(n_envs, 1, 1), - hand_open_qpos[None, None, :].repeat(n_envs, 1, 1), - hand_open_qpos[None, None, :].repeat(n_envs, 1, 1), - hand_open_qpos[None, None, :].repeat(n_envs, 1, 1), + hand_open_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_open_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_open_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_close_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_close_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_close_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_close_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_open_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_open_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_open_qpos[None, None, :].repeat(num_envs, 1, 1), ], dim=1, ) all_trajectory = torch.cat([arm_trajectory, hand_trajectory], dim=-1) - # trajetory with shape [n_envs, n_waypoint, dof] + # trajetory with shape [num_envs, n_waypoint, dof] interp_trajectory = interpolate_with_distance( trajectory=all_trajectory, interp_num=150, device=sim.device ) @@ -377,7 +377,7 @@ def run_simulation( cup (RigidObject): The cup object. caffe (Robot): The caffe object. """ - # [n_envs, n_waypoint, dof] + # [num_envs, n_waypoint, dof] interp_trajectory = create_trajectory(sim, robot, cup, caffe) right_arm_ids = robot.get_joint_ids("right_arm") diff --git a/examples/sim/demo/pick_up_cloth.py b/examples/sim/demo/pick_up_cloth.py index e8183cab8..7184ea6de 100644 --- a/examples/sim/demo/pick_up_cloth.py +++ b/examples/sim/demo/pick_up_cloth.py @@ -193,7 +193,7 @@ def create_cloth(sim: SimulationManager): def get_grasp_traj(sim: SimulationManager, robot: Robot, grasp_xpos: torch.Tensor): - n_envs = sim.num_envs + num_envs = sim.num_envs rest_arm_qpos = robot.get_qpos("arm") approach_xpos = grasp_xpos.clone() @@ -222,12 +222,12 @@ def get_grasp_traj(sim: SimulationManager, robot: Robot, grasp_xpos: torch.Tenso ) hand_trajectory = torch.cat( [ - hand_open_qpos[None, None, :].repeat(n_envs, 1, 1), - hand_open_qpos[None, None, :].repeat(n_envs, 1, 1), - hand_open_qpos[None, None, :].repeat(n_envs, 1, 1), - hand_close_qpos[None, None, :].repeat(n_envs, 1, 1), - hand_close_qpos[None, None, :].repeat(n_envs, 1, 1), - hand_close_qpos[None, None, :].repeat(n_envs, 1, 1), + hand_open_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_open_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_open_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_close_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_close_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_close_qpos[None, None, :].repeat(num_envs, 1, 1), ], dim=1, ) diff --git a/examples/sim/planners/curobo_planner.py b/examples/sim/planners/curobo_planner.py index b3105f248..908f8619a 100644 --- a/examples/sim/planners/curobo_planner.py +++ b/examples/sim/planners/curobo_planner.py @@ -59,7 +59,6 @@ visualization_cfg_from_args, ) from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, @@ -749,9 +748,12 @@ def main() -> None: ) ) engine = AtomicActionEngine(motion_generator) - binding = ActionBinding(manipulators={"primary": control_part}) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": control_part}}, + ) motion_policy = MotionPolicy( - motion_source="motion_gen", + strategy="motion_gen", plan_opts=CuroboPlanOptions( dynamic_obstacle_poses=( obstacle_poses if use_independent_worlds else None diff --git a/scripts/benchmark/atomic_action/move_end_effector_benchmark.py b/scripts/benchmark/atomic_action/move_end_effector_benchmark.py index 0f2f1de48..7630f9641 100644 --- a/scripts/benchmark/atomic_action/move_end_effector_benchmark.py +++ b/scripts/benchmark/atomic_action/move_end_effector_benchmark.py @@ -121,7 +121,6 @@ def _run_case( """Run one MoveEndEffector case.""" torch = ensure_torch() from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, EndEffectorPoseGoal, MotionPolicy, @@ -129,6 +128,10 @@ def _run_case( reset_robot(robot, initial_qpos) target_pose = _make_pose(sim.device, pose_case.xyz) + binding = atomic_engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "arm"}}, + ) elapsed, mem_delta, peak_gpu, result = timed_call( lambda: atomic_engine.compile( @@ -136,7 +139,7 @@ def _run_case( ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(xpos=target_pose), - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=binding, motion_policy=MotionPolicy(sample_count=MOVE_SAMPLE_INTERVAL), ), ) diff --git a/scripts/benchmark/atomic_action/move_held_object_benchmark.py b/scripts/benchmark/atomic_action/move_held_object_benchmark.py index 0f66c5b9d..8d88560b0 100644 --- a/scripts/benchmark/atomic_action/move_held_object_benchmark.py +++ b/scripts/benchmark/atomic_action/move_held_object_benchmark.py @@ -175,7 +175,6 @@ def _prepare_held_state( ): """Run PickUp precondition outside the timed MoveHeldObject block.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -212,22 +211,26 @@ def _prepare_held_state( move_position = obj_pose[0, :3, 3].clone() move_position[2] = 0.36 move_target = make_pre_pick_eef_pose(robot, move_position) - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, + move_binding = atomic_engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "arm"}}, + ) + pick_binding = atomic_engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "arm", "grasp": "hand"}}, ) result = atomic_engine.compile( ( ActionInvocation( "move_end_effector", EndEffectorPoseGoal(xpos=move_target), - binding, + move_binding, MotionPolicy(sample_count=MOVE_SAMPLE_INTERVAL), ), ActionInvocation( "pick_up", GraspGoal(semantics=semantics), - binding, + pick_binding, MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( approach_direction=resolve_pickup_approach_direction( @@ -238,7 +241,8 @@ def _prepare_held_state( hand_interp_steps=HAND_INTERP_STEPS, ), ), - ) + ), + atomic_engine.initial_context(control_dt=sim.sim_config.physics_dt), ) is_success = bool(result.plan_success.all().item()) traj = result.trajectory.positions @@ -269,7 +273,6 @@ def _run_case( ): """Run one MoveHeldObject benchmark case.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -319,16 +322,17 @@ def _run_case( }, ) target_pose = _make_object_target_pose(sim.device, case.xyz) + binding = atomic_engine.bind_control_parts( + "move_held_object", + {"primary": {"motion": "arm", "grasp": "hand"}}, + ) elapsed, mem_delta, peak_gpu, result = timed_call( lambda: atomic_engine.compile( ( ActionInvocation( skill_id="move_held_object", goal=HeldObjectPoseGoal(object_target_pose=target_pose), - binding=ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ), + binding=binding, motion_policy=MotionPolicy( sample_count=MOVE_HELD_OBJECT_SAMPLE_INTERVAL ), diff --git a/scripts/benchmark/atomic_action/move_joints_benchmark.py b/scripts/benchmark/atomic_action/move_joints_benchmark.py index 82d43c164..abdfcc6bd 100644 --- a/scripts/benchmark/atomic_action/move_joints_benchmark.py +++ b/scripts/benchmark/atomic_action/move_joints_benchmark.py @@ -106,17 +106,19 @@ def _qpos(values, device): return torch.tensor(values, dtype=torch.float32, device=device) -def _targets_for_sequence(sequence_case: JointSequenceCase, device): +def _targets_for_sequence(atomic_engine, sequence_case: JointSequenceCase, device): """Build typed MoveJoints targets for a sequence case.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, JointPositionGoal, MotionPolicy, ) targets = [] - binding = ActionBinding(manipulators={"primary": "arm"}) + binding = atomic_engine.bind_control_parts( + "move_joints", + {"primary": {"motion": "arm"}}, + ) policy = MotionPolicy(sample_count=MOVE_JOINTS_SAMPLE_INTERVAL) for index, name in enumerate(sequence_case.sequence): if index == 0 and name == "ready": @@ -147,7 +149,7 @@ def _run_case( """Run one MoveJoints case.""" torch = ensure_torch() reset_robot(robot, initial_qpos) - steps = _targets_for_sequence(case, sim.device) + steps = _targets_for_sequence(atomic_engine, case, sim.device) elapsed, mem_delta, peak_gpu, result = timed_call( lambda: atomic_engine.compile(steps) ) diff --git a/scripts/benchmark/atomic_action/pickup_benchmark.py b/scripts/benchmark/atomic_action/pickup_benchmark.py index 4559d3e6e..245efaaad 100644 --- a/scripts/benchmark/atomic_action/pickup_benchmark.py +++ b/scripts/benchmark/atomic_action/pickup_benchmark.py @@ -123,7 +123,6 @@ def _run_case( ): """Run one PickUp benchmark case.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -175,16 +174,17 @@ def _run_case( build_gripper_collision_cfg=build_gripper_collision_cfg, build_grasp_generator_cfg=build_grasp_generator_cfg, ) + binding = atomic_engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "arm", "grasp": "hand"}}, + ) elapsed, mem_delta, peak_gpu, result = timed_call( lambda: atomic_engine.compile( ( ActionInvocation( skill_id="pick_up", goal=GraspGoal(semantics=semantics), - binding=ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ), + binding=binding, motion_policy=MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( approach_direction=approach_direction, @@ -193,7 +193,8 @@ def _run_case( hand_interp_steps=HAND_INTERP_STEPS, ), ), - ) + ), + atomic_engine.initial_context(control_dt=sim.sim_config.physics_dt), ) ) is_success = bool(result.plan_success.all().item()) diff --git a/scripts/benchmark/atomic_action/place_benchmark.py b/scripts/benchmark/atomic_action/place_benchmark.py index 4c8242719..aa73713a9 100644 --- a/scripts/benchmark/atomic_action/place_benchmark.py +++ b/scripts/benchmark/atomic_action/place_benchmark.py @@ -174,7 +174,6 @@ def _prepare_held_state( ): """Run PickUp precondition outside the timed Place block.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -212,9 +211,9 @@ def _prepare_held_state( ActionInvocation( skill_id="pick_up", goal=GraspGoal(semantics=semantics), - binding=ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, + binding=atomic_engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "arm", "grasp": "hand"}}, ), motion_policy=MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( @@ -226,7 +225,8 @@ def _prepare_held_state( hand_interp_steps=HAND_INTERP_STEPS, ), ), - ) + ), + atomic_engine.initial_context(control_dt=sim.sim_config.physics_dt), ) is_success = bool(result.plan_success.all().item()) traj = result.trajectory.positions @@ -255,7 +255,6 @@ def _run_case( ): """Run one Place benchmark case.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -308,16 +307,17 @@ def _run_case( }, ) place_pose = _make_place_pose(sim.device, case.xyz) + binding = atomic_engine.bind_control_parts( + "place", + {"primary": {"motion": "arm", "grasp": "hand"}}, + ) elapsed, mem_delta, peak_gpu, result = timed_call( lambda: atomic_engine.compile( ( ActionInvocation( skill_id="place", goal=PlaceGoal(xpos=place_pose), - binding=ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ), + binding=binding, motion_policy=MotionPolicy(sample_count=PLACE_SAMPLE_INTERVAL), skill_options=PlaceOptions( lift_height=PLACE_LIFT_HEIGHT, diff --git a/scripts/benchmark/atomic_action/press_benchmark.py b/scripts/benchmark/atomic_action/press_benchmark.py deleted file mode 100644 index a5687af7f..000000000 --- a/scripts/benchmark/atomic_action/press_benchmark.py +++ /dev/null @@ -1,994 +0,0 @@ -# ---------------------------------------------------------------------------- -# 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. -# ---------------------------------------------------------------------------- - -"""Benchmark Press atomic action across object presets and start positions. - -The benchmark sweeps object presets such as bottle and mug against multiple -initial XY positions that cover all four workspace quadrants. It reports -planning latency, memory usage, planning success, and whether the generated -trajectory reaches the object's top center. -Run: embodichain benchmark atomic-action --action press -""" - -from __future__ import annotations - -import argparse -import math -import os -import resource -import sys -import time -from dataclasses import dataclass -from datetime import datetime -from pathlib import Path - -from scripts.benchmark.atomic_action.common import ( - add_profile_benchmark_args, - add_video_benchmark_args, - build_video_output_path, - COVERAGE_POSITION_CASE_NAMES, - park_rigid_object, - replay_trajectory_with_recording, - reset_rigid_object, - reset_rigid_object_xy, - resolve_profile, - should_record_case, - SMOKE_POSITION_CASE_NAMES, -) - -try: - import psutil -except ModuleNotFoundError: - psutil = None - -CPU_MEMORY_BACKEND = "psutil" if psutil is not None else "resource" -_RUNTIME_IMPORTS_READY = False - -# Keep these constants aligned with scripts/tutorials/atomic_action/press.py. -DEFAULT_PRESS_TOLERANCE = 0.01 -MOVE_SAMPLE_INTERVAL = 60 -PRESS_SAMPLE_INTERVAL = 90 -HAND_INTERP_STEPS = 12 -TABLE_TOP_Z = -0.045 -PRESS_CLEARANCE = 0.13 -PRESS_SURFACE_OFFSET = 0.003 - - -def _ensure_runtime_imports() -> None: - """Import simulation dependencies only when the benchmark is executed.""" - global _RUNTIME_IMPORTS_READY - if _RUNTIME_IMPORTS_READY: - return - - repo_root = Path(__file__).resolve().parents[3] - if str(repo_root) not in sys.path: - sys.path.insert(0, str(repo_root)) - - try: - import torch as torch_module - from embodichain.lab.sim import SimulationManager as simulation_manager_cls - from embodichain.lab.sim.atomic_actions import ( - ActionBinding as action_binding_cls, - ActionInvocation as action_invocation_cls, - AtomicActionEngine as atomic_action_engine_cls, - ControlPartCommandProfile as control_part_command_profile_cls, - EndEffectorPoseGoal as end_effector_pose_target_cls, - MotionPolicy as motion_policy_cls, - PressGoal as press_target_cls, - PressOptions as press_options_cls, - ) - from embodichain.lab.sim.cfg import ( - RigidBodyAttributesCfg as rigid_body_attributes_cfg_cls, - RigidObjectCfg as rigid_object_cfg_cls, - ) - from embodichain.lab.sim.material import ( - VisualMaterialCfg as visual_material_cfg_cls, - ) - from embodichain.lab.sim.objects import RigidObject as rigid_object_cls - from embodichain.lab.sim.objects import Robot as robot_cls - from embodichain.lab.sim.planners import ( - MotionGenerator as motion_generator_cls, - MotionGenCfg as motion_gen_cfg_cls, - ToppraPlannerCfg as toppra_planner_cfg_cls, - ) - from embodichain.lab.sim.shapes import CubeCfg as cube_cfg_cls - from scripts.tutorials.atomic_action.press import ( - create_robot as create_robot_fn, - create_table as create_table_fn, - get_hand_close_qpos as get_hand_close_qpos_fn, - initialize_simulation as initialize_simulation_fn, - make_top_down_eef_pose as make_top_down_eef_pose_fn, - settle_object as settle_object_fn, - ) - except ModuleNotFoundError as exc: - raise RuntimeError( - "Atomic action benchmark requires the EmbodiChain simulation runtime " - f"and PyTorch. Missing module: {exc.name}." - ) from exc - - globals().update( - { - "torch": torch_module, - "SimulationManager": simulation_manager_cls, - "AtomicActionEngine": atomic_action_engine_cls, - "ControlPartCommandProfile": control_part_command_profile_cls, - "ActionBinding": action_binding_cls, - "ActionInvocation": action_invocation_cls, - "EndEffectorPoseGoal": end_effector_pose_target_cls, - "MotionPolicy": motion_policy_cls, - "PressGoal": press_target_cls, - "PressOptions": press_options_cls, - "RigidBodyAttributesCfg": rigid_body_attributes_cfg_cls, - "RigidObjectCfg": rigid_object_cfg_cls, - "VisualMaterialCfg": visual_material_cfg_cls, - "RigidObject": rigid_object_cls, - "Robot": robot_cls, - "MotionGenerator": motion_generator_cls, - "MotionGenCfg": motion_gen_cfg_cls, - "ToppraPlannerCfg": toppra_planner_cfg_cls, - "CubeCfg": cube_cfg_cls, - "create_robot": create_robot_fn, - "create_table": create_table_fn, - "get_hand_close_qpos": get_hand_close_qpos_fn, - "initialize_simulation": initialize_simulation_fn, - "make_top_down_eef_pose": make_top_down_eef_pose_fn, - "settle_object": settle_object_fn, - } - ) - _RUNTIME_IMPORTS_READY = True - - -@dataclass(frozen=True) -class ObjectPreset: - """Primitive object preset used by the atomic-action benchmark.""" - - object_type: str - material_name: str - size: tuple[float, float, float] - base_color: tuple[float, float, float, float] - roughness: float - dynamic_friction: float = 0.8 - static_friction: float = 0.9 - - -@dataclass(frozen=True) -class PositionCase: - """Initial object position case with a quadrant label.""" - - name: str - quadrant: str - xy: tuple[float, float] - - -@dataclass(frozen=True) -class PressCaseResult: - """Result for one Press benchmark case.""" - - case_id: str - object_type: str - material_name: str - quadrant: str - position_case: str - init_xy: tuple[float, float] - repeat_index: int - planning_success: bool - center_hit: bool - cost_time_ms: float - cpu_delta_mb: float - gpu_delta_mb: float - peak_gpu_mb: float - xy_error_m: float | None - hit_step: int | None - trajectory_waypoints: int - failure_reason: str - video_path: str = "" - - -OBJECT_PRESETS: dict[str, ObjectPreset] = { - "bottle": ObjectPreset( - object_type="bottle", - material_name="green_plastic", - size=(0.06, 0.06, 0.16), - base_color=(0.10, 0.45, 0.32, 1.0), - roughness=0.55, - ), - "mug": ObjectPreset( - object_type="mug", - material_name="ceramic", - size=(0.10, 0.08, 0.10), - base_color=(0.88, 0.85, 0.78, 1.0), - roughness=0.35, - ), - "wooden_block": ObjectPreset( - object_type="wooden_block", - material_name="wood", - size=(0.12, 0.12, 0.06), - base_color=(0.58, 0.32, 0.14, 1.0), - roughness=0.85, - ), -} - -POSITION_CASES: dict[str, PositionCase] = { - "q1_near": PositionCase(name="q1_near", quadrant="q1", xy=(0.02, 0.18)), - "q1_far": PositionCase(name="q1_far", quadrant="q1", xy=(0.12, 0.36)), - "q2_near": PositionCase(name="q2_near", quadrant="q2", xy=(-0.42, 0.18)), - "q2_far": PositionCase(name="q2_far", quadrant="q2", xy=(-0.62, 0.36)), - "q3_near": PositionCase(name="q3_near", quadrant="q3", xy=(-0.42, -0.18)), - "q3_far": PositionCase(name="q3_far", quadrant="q3", xy=(-0.62, -0.36)), - "q4_near": PositionCase(name="q4_near", quadrant="q4", xy=(0.02, -0.18)), - "q4_far": PositionCase(name="q4_far", quadrant="q4", xy=(0.12, -0.36)), -} - -DEFAULT_OBJECT_TYPES = ("bottle", "mug") -FULL_OBJECT_TYPES = tuple(OBJECT_PRESETS.keys()) -SMOKE_OBJECT_TYPES = ("bottle",) - - -def add_benchmark_args(parser: argparse.ArgumentParser) -> None: - """Add atomic-action benchmark arguments to an argument parser.""" - add_profile_benchmark_args(parser) - parser.add_argument( - "--object_types", - nargs="+", - choices=(*OBJECT_PRESETS.keys(), "all"), - default=None, - help=( - "Object presets to benchmark. Defaults are selected by --profile; " - "use 'all' to include every preset." - ), - ) - parser.add_argument( - "--position_cases", - nargs="+", - choices=(*POSITION_CASES.keys(), "all"), - default=None, - help=( - "Initial position cases to benchmark. Defaults are selected by " - "--profile; use 'all' for all near/far cases." - ), - ) - parser.add_argument( - "--repeat", - type=int, - default=1, - help="Number of repeats for every object-position case.", - ) - parser.add_argument( - "--smoke", - action="store_true", - help="Alias for --profile smoke.", - ) - parser.add_argument( - "--device", - type=str, - default="cpu", - help="Simulation device, e.g. 'cpu' or 'cuda'.", - ) - parser.add_argument( - "--renderer", - type=str, - choices=("auto", "hybrid", "fast-rt", "rt"), - default="auto", - help="Renderer backend used by SimulationManager.", - ) - add_video_benchmark_args(parser) - parser.add_argument( - "--press_tolerance", - type=float, - default=DEFAULT_PRESS_TOLERANCE, - help="XY tolerance in meters for the press-center check.", - ) - - -def _parse_args() -> argparse.Namespace: - """Parse command line arguments for the atomic-action benchmark.""" - parser = argparse.ArgumentParser( - description=( - "Benchmark Press atomic action over object presets and initial " - "workspace quadrants." - ) - ) - add_benchmark_args(parser) - return parser.parse_args() - - -def _sync_cuda() -> None: - """Synchronize CUDA stream when available.""" - if torch.cuda.is_available(): - torch.cuda.synchronize() - - -def _reset_peak_gpu_memory() -> None: - """Reset PyTorch peak GPU memory stats when CUDA is available.""" - if torch.cuda.is_available(): - torch.cuda.reset_peak_memory_stats() - - -def _peak_gpu_memory_mb() -> float: - """Return peak GPU memory allocated by PyTorch in MB.""" - if not torch.cuda.is_available(): - return 0.0 - return torch.cuda.max_memory_allocated() / 1024**2 - - -def _memory_snapshot() -> dict[str, float]: - """Return current process memory usage snapshot in MB.""" - if psutil is not None: - process = psutil.Process(os.getpid()) - cpu_mb = process.memory_info().rss / 1024**2 - else: - cpu_mb = resource.getrusage(resource.RUSAGE_SELF).ru_maxrss / 1024.0 - gpu_mb = ( - torch.cuda.memory_allocated() / 1024**2 if torch.cuda.is_available() else 0.0 - ) - return {"cpu_mb": cpu_mb, "gpu_mb": gpu_mb} - - -def _format_float(value: float | None, precision: int = 6) -> str: - """Format finite floats for tables and use N/A for missing values.""" - if value is None or not math.isfinite(value): - return "N/A" - return f"{value:.{precision}f}" - - -def _format_markdown_table(rows: list[dict[str, object]]) -> list[str]: - """Format rows into a markdown table.""" - if not rows: - return ["No data."] - - headers = list(rows[0].keys()) - lines = [ - "| " + " | ".join(headers) + " |", - "| " + " | ".join(["---"] * len(headers)) + " |", - ] - for row in rows: - lines.append("| " + " | ".join(str(row[h]) for h in headers) + " |") - return lines - - -def _write_markdown_report( - benchmark_name: str, - perf_rows: list[dict[str, object]], - metric_rows: list[dict[str, object]], - leaderboard_rows: list[dict[str, object]], - notes: list[str] | None = None, -) -> Path: - """Write benchmark results into one markdown report file.""" - output_dir = Path("outputs/benchmarks") - output_dir.mkdir(parents=True, exist_ok=True) - - timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") - report_path = output_dir / f"{benchmark_name}_{timestamp}.md" - - lines: list[str] = [ - f"# {benchmark_name} Benchmark Report", - "", - f"Generated at: {datetime.now().isoformat(timespec='seconds')}", - "", - "## Time & Memory", - "", - ] - lines.extend(_format_markdown_table(perf_rows)) - lines.extend(["", "## Success & Other Metrics", ""]) - lines.extend(_format_markdown_table(metric_rows)) - lines.extend(["", "## Leaderboard", ""]) - lines.extend(_format_markdown_table(leaderboard_rows)) - - if notes: - lines.extend(["", "## Notes", ""]) - lines.extend([f"- {note}" for note in notes]) - - report_path.write_text("\n".join(lines) + "\n", encoding="utf-8") - return report_path - - -def _default_object_types_for_profile(profile: str) -> tuple[str, ...]: - """Return default Press primitive object names for a profile.""" - if profile in ("smoke", "coverage", "full"): - return SMOKE_OBJECT_TYPES - raise ValueError(f"Unsupported benchmark profile: {profile}") - - -def _select_object_presets( - object_types: list[str] | None, - profile: str, -) -> list[ObjectPreset]: - """Resolve selected object preset names.""" - if not object_types: - object_types = list(_default_object_types_for_profile(profile)) - if "all" in object_types: - return list(OBJECT_PRESETS.values()) - return [OBJECT_PRESETS[name] for name in object_types] - - -def _default_position_cases_for_profile(profile: str) -> tuple[str, ...]: - """Return default Press position case names for a profile.""" - if profile == "smoke": - return SMOKE_POSITION_CASE_NAMES - if profile in ("coverage", "full"): - return COVERAGE_POSITION_CASE_NAMES - raise ValueError(f"Unsupported benchmark profile: {profile}") - - -def _select_position_cases( - position_cases: list[str] | None, - profile: str, -) -> list[PositionCase]: - """Resolve selected position case names.""" - if not position_cases: - position_cases = list(_default_position_cases_for_profile(profile)) - if "all" in position_cases: - return list(POSITION_CASES.values()) - return [POSITION_CASES[name] for name in position_cases] - - -def _create_benchmark_object( - sim: SimulationManager, - preset: ObjectPreset, - position_case: PositionCase, - repeat_index: int, -) -> RigidObject: - """Create one static benchmark object at the requested initial position.""" - init_pos = ( - position_case.xy[0], - position_case.xy[1], - TABLE_TOP_Z + 0.5 * preset.size[2], - ) - uid = f"atomic_benchmark_{preset.object_type}_{position_case.name}_{repeat_index}" - cfg = RigidObjectCfg( - uid=uid, - shape=CubeCfg( - size=list(preset.size), - visual_material=VisualMaterialCfg( - uid=f"{preset.object_type}_{preset.material_name}_mat", - base_color=list(preset.base_color), - roughness=preset.roughness, - ), - ), - body_type="static", - attrs=RigidBodyAttributesCfg( - dynamic_friction=preset.dynamic_friction, - static_friction=preset.static_friction, - ), - init_pos=init_pos, - ) - return sim.add_rigid_object(cfg=cfg) - - -def _reset_robot(robot: Robot, initial_qpos: torch.Tensor) -> None: - """Reset current and target robot qpos to the benchmark initial posture.""" - for target in (False, True): - robot.set_qpos(initial_qpos, target=target) - robot.clear_dynamics() - - -def _build_atomic_engine( - motion_gen: MotionGenerator, - robot: Robot, - device: torch.device, -) -> AtomicActionEngine: - """Build a Press benchmark engine with MoveEndEffector pre-positioning.""" - hand_close = get_hand_close_qpos(robot, device) - atomic_engine = AtomicActionEngine( - motion_generator=motion_gen, - control_profiles={ - "hand": ControlPartCommandProfile.joint_positions( - grasp=hand_close, - ) - }, - ) - return atomic_engine - - -def _make_press_targets( - obj: RigidObject, - preset: ObjectPreset, -) -> tuple[torch.Tensor, torch.Tensor]: - """Create pre-press and press poses for the object's top center.""" - obj_pose = obj.get_local_pose(to_matrix=True) - object_center = obj_pose[0, :3, 3].clone() - object_top_z = object_center[2] + 0.5 * preset.size[2] - - press_position = object_center.clone() - press_position[2] = object_top_z + PRESS_SURFACE_OFFSET - move_position = press_position.clone() - move_position[2] = object_top_z + PRESS_CLEARANCE - - return make_top_down_eef_pose(move_position), make_top_down_eef_pose(press_position) - - -def _compute_press_center_check( - robot: Robot, - traj: torch.Tensor, - obj: RigidObject, - object_height: float, - tolerance: float, -) -> tuple[bool, float, int]: - """Check whether the planned Press trajectory reaches the object top center.""" - if traj.numel() == 0: - return False, float("inf"), -1 - - arm_joint_ids = robot.get_joint_ids(name="arm") - n_down = (PRESS_SAMPLE_INTERVAL - HAND_INTERP_STEPS) // 2 - press_segment_start = MOVE_SAMPLE_INTERVAL + HAND_INTERP_STEPS - press_segment_end = min(press_segment_start + n_down, traj.shape[1]) - arm_traj = traj[:, press_segment_start:press_segment_end, arm_joint_ids] - if arm_traj.shape[1] == 0: - return False, float("inf"), -1 - - fk_pose = torch.stack( - [ - robot.compute_fk( - qpos=waypoint.unsqueeze(0), - name="arm", - to_matrix=True, - )[0] - for waypoint in arm_traj[0] - ], - dim=0, - ) - - obj_pose = obj.get_local_pose(to_matrix=True) - object_center = obj_pose[0, :3, 3] - object_top_z = object_center[2] + 0.5 * object_height - target_xy = object_center[:2] - target_z = object_top_z + PRESS_SURFACE_OFFSET - - xy_error = torch.linalg.norm(fk_pose[:, :2, 3] - target_xy, dim=1) - z_error = torch.abs(fk_pose[:, 2, 3] - target_z) - combined_error = xy_error + z_error - best_idx = int(torch.argmin(combined_error).item()) - best_pos = fk_pose[best_idx, :3, 3] - center_error = float(torch.linalg.norm(best_pos[:2] - target_xy).item()) - return center_error <= tolerance, center_error, press_segment_start + best_idx - - -def _timed_atomic_run( - atomic_engine: AtomicActionEngine, - move_target: torch.Tensor, - press_target: torch.Tensor, -) -> tuple[float, dict[str, float], float, bool, torch.Tensor]: - """Run a timed atomic-action sequence and return timing/memory/results.""" - _reset_peak_gpu_memory() - mem_before = _memory_snapshot() - _sync_cuda() - - start = time.perf_counter() - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ) - result = atomic_engine.compile( - ( - ActionInvocation( - "move_end_effector", - EndEffectorPoseGoal(xpos=move_target), - binding, - MotionPolicy(sample_count=MOVE_SAMPLE_INTERVAL), - ), - ActionInvocation( - "press", - PressGoal(xpos=press_target), - binding, - MotionPolicy(sample_count=PRESS_SAMPLE_INTERVAL), - skill_options=PressOptions( - hand_interp_steps=HAND_INTERP_STEPS, - ), - ), - ) - ) - is_success = bool(result.plan_success.all().item()) - traj = result.trajectory.positions - _sync_cuda() - elapsed = time.perf_counter() - start - - mem_after = _memory_snapshot() - deltas = { - "cpu_mb": mem_after["cpu_mb"] - mem_before["cpu_mb"], - "gpu_mb": mem_after["gpu_mb"] - mem_before["gpu_mb"], - } - return elapsed, deltas, _peak_gpu_memory_mb(), is_success, traj - - -def _run_press_case( - sim: SimulationManager, - robot: Robot, - atomic_engine: AtomicActionEngine, - initial_qpos: torch.Tensor, - obj: RigidObject, - base_obj_pose: torch.Tensor, - preset: ObjectPreset, - position_case: PositionCase, - repeat_index: int, - press_tolerance: float, - args: argparse.Namespace, - recorded_count: int, -) -> PressCaseResult: - """Run one object-position Press benchmark case.""" - case_id = f"{preset.object_type}:{position_case.name}:r{repeat_index}" - try: - _reset_robot(robot, initial_qpos) - initial_obj_pose = reset_rigid_object_xy( - obj=obj, - base_pose=base_obj_pose, - xy=position_case.xy, - sim=sim, - settle_steps=2, - ) - move_target, press_target = _make_press_targets(obj, preset) - - elapsed, mem_delta, peak_gpu, planning_success, traj = _timed_atomic_run( - atomic_engine=atomic_engine, - move_target=move_target, - press_target=press_target, - ) - video_path = None - if should_record_case(args, recorded_count, bool(planning_success)): - _reset_robot(robot, initial_qpos) - reset_rigid_object(obj, initial_obj_pose) - video_path = replay_trajectory_with_recording( - sim=sim, - robot=robot, - traj=traj, - args=args, - video_path=build_video_output_path( - args, - "atomic_action_press", - (f"{preset.object_type}_{position_case.name}" f"_r{repeat_index}"), - ), - ) - _reset_robot(robot, initial_qpos) - reset_rigid_object(obj, initial_obj_pose) - - center_hit = False - xy_error_m: float | None = None - hit_step: int | None = None - failure_reason = "" - if planning_success: - center_hit, xy_error_m, raw_hit_step = _compute_press_center_check( - robot=robot, - traj=traj, - obj=obj, - object_height=preset.size[2], - tolerance=press_tolerance, - ) - hit_step = raw_hit_step if raw_hit_step >= 0 else None - if not center_hit: - failure_reason = "center_miss" - else: - failure_reason = "planning_failed" - - return PressCaseResult( - case_id=case_id, - object_type=preset.object_type, - material_name=preset.material_name, - quadrant=position_case.quadrant, - position_case=position_case.name, - init_xy=position_case.xy, - repeat_index=repeat_index, - planning_success=planning_success, - center_hit=center_hit, - cost_time_ms=elapsed * 1000.0, - cpu_delta_mb=mem_delta["cpu_mb"], - gpu_delta_mb=mem_delta["gpu_mb"], - peak_gpu_mb=peak_gpu, - xy_error_m=xy_error_m, - hit_step=hit_step, - trajectory_waypoints=int(traj.shape[1]) if traj.ndim >= 2 else 0, - failure_reason=failure_reason, - video_path=str(video_path) if video_path is not None else "", - ) - except Exception as exc: - return PressCaseResult( - case_id=case_id, - object_type=preset.object_type, - material_name=preset.material_name, - quadrant=position_case.quadrant, - position_case=position_case.name, - init_xy=position_case.xy, - repeat_index=repeat_index, - planning_success=False, - center_hit=False, - cost_time_ms=0.0, - cpu_delta_mb=0.0, - gpu_delta_mb=0.0, - peak_gpu_mb=0.0, - xy_error_m=None, - hit_step=None, - trajectory_waypoints=0, - failure_reason=f"exception:{type(exc).__name__}:{exc}", - ) - - -def _build_perf_rows(results: list[PressCaseResult]) -> list[dict[str, object]]: - """Build Time & Memory table rows.""" - rows: list[dict[str, object]] = [] - for result in results: - rows.append( - { - "sample_size": 1, - "impl": "press", - "case_id": result.case_id, - "object_type": result.object_type, - "material": result.material_name, - "quadrant": result.quadrant, - "position_case": result.position_case, - "init_xy": f"({result.init_xy[0]:.3f},{result.init_xy[1]:.3f})", - "repeat": result.repeat_index, - "cost_time_ms": _format_float(result.cost_time_ms), - "cpu_delta_mb": _format_float(result.cpu_delta_mb), - "gpu_delta_mb": _format_float(result.gpu_delta_mb), - "peak_gpu_mb": _format_float(result.peak_gpu_mb), - } - ) - return rows - - -def _build_metric_rows(results: list[PressCaseResult]) -> list[dict[str, object]]: - """Build Success & Other Metrics table rows.""" - rows: list[dict[str, object]] = [] - for result in results: - overall_success = result.planning_success and result.center_hit - rows.append( - { - "sample_size": 1, - "impl": "press", - "case_id": result.case_id, - "object_type": result.object_type, - "material": result.material_name, - "quadrant": result.quadrant, - "position_case": result.position_case, - "success_rate": f"{float(overall_success):.6f}", - "planning_success_rate": f"{float(result.planning_success):.6f}", - "center_hit_rate": f"{float(result.center_hit):.6f}", - "xy_error_m": _format_float(result.xy_error_m), - "hit_step": result.hit_step if result.hit_step is not None else "N/A", - "trajectory_waypoints": result.trajectory_waypoints, - "failure_reason": result.failure_reason or "N/A", - } - ) - return rows - - -def _build_leaderboard_rows(results: list[PressCaseResult]) -> list[dict[str, object]]: - """Aggregate and rank object-conditioned Press variants by success rate.""" - aggregate: dict[str, dict[str, float | set[str]]] = {} - for result in results: - algorithm = f"press:{result.object_type}" - if algorithm not in aggregate: - aggregate[algorithm] = { - "overall_success_sum": 0.0, - "planning_success_sum": 0.0, - "xy_error_sum": 0.0, - "xy_error_count": 0.0, - "cost_time_sum": 0.0, - "case_count": 0.0, - "quadrants": set(), - } - - stats = aggregate[algorithm] - stats["overall_success_sum"] = float(stats["overall_success_sum"]) + float( - result.planning_success and result.center_hit - ) - stats["planning_success_sum"] = float(stats["planning_success_sum"]) + float( - result.planning_success - ) - if result.xy_error_m is not None and math.isfinite(result.xy_error_m): - stats["xy_error_sum"] = float(stats["xy_error_sum"]) + result.xy_error_m - stats["xy_error_count"] = float(stats["xy_error_count"]) + 1.0 - stats["cost_time_sum"] = float(stats["cost_time_sum"]) + result.cost_time_ms - stats["case_count"] = float(stats["case_count"]) + 1.0 - quadrants = stats["quadrants"] - if isinstance(quadrants, set): - quadrants.add(result.quadrant) - - ranked = sorted( - aggregate.items(), - key=lambda item: ( - float(item[1]["overall_success_sum"]) - / max(float(item[1]["case_count"]), 1.0), - -float(item[1]["cost_time_sum"]) / max(float(item[1]["case_count"]), 1.0), - ), - reverse=True, - ) - - rows: list[dict[str, object]] = [] - for rank, (algorithm, stats) in enumerate(ranked, start=1): - case_count = max(float(stats["case_count"]), 1.0) - xy_error_count = float(stats["xy_error_count"]) - avg_xy_error = ( - float(stats["xy_error_sum"]) / xy_error_count - if xy_error_count > 0.0 - else None - ) - quadrants = stats["quadrants"] - quadrant_coverage = ( - ",".join(sorted(quadrants)) if isinstance(quadrants, set) else "" - ) - rows.append( - { - "rank": rank, - "algorithm": algorithm, - "overall_success_rate": ( - f"{float(stats['overall_success_sum']) / case_count:.2%}" - ), - "planning_success_rate": ( - f"{float(stats['planning_success_sum']) / case_count:.2%}" - ), - "avg_xy_error_m": _format_float(avg_xy_error), - "avg_cost_time_ms": _format_float( - float(stats["cost_time_sum"]) / case_count - ), - "evaluated_cases": int(case_count), - "quadrant_coverage": quadrant_coverage, - } - ) - return rows - - -def _print_case_result(result: PressCaseResult) -> None: - """Print one aligned case result line.""" - overall_success = result.planning_success and result.center_hit - print( - f" {result.case_id:<28} " - f"time={result.cost_time_ms:>10.2f} ms | " - f"CPU delta={result.cpu_delta_mb:+.1f} MB " - f"GPU delta={result.gpu_delta_mb:+.1f} MB " - f"peak GPU={result.peak_gpu_mb:.1f} MB | " - f"success={overall_success} " - f"xy_error={_format_float(result.xy_error_m, precision=4)}" - ) - if result.failure_reason: - print(f" reason={result.failure_reason}") - - -def _build_notes( - object_presets: list[ObjectPreset], - position_cases: list[PositionCase], - repeat: int, - video_paths: list[str], - profile: str, -) -> list[str]: - """Build report notes with benchmark coverage metadata.""" - quadrant_counts: dict[str, int] = {} - for position_case in position_cases: - quadrant_counts[position_case.quadrant] = ( - quadrant_counts.get(position_case.quadrant, 0) + 1 - ) - return [ - f"Profile: {profile}", - "Object presets: " - + ", ".join( - f"{preset.object_type}/{preset.material_name}/size={preset.size}" - for preset in object_presets - ), - "Position cases per quadrant: " - + ", ".join( - f"{quadrant}={count}" for quadrant, count in sorted(quadrant_counts.items()) - ), - f"CPU memory backend: {CPU_MEMORY_BACKEND}", - f"Repeat per object-position case: {repeat}", - "Replay videos: " + (", ".join(video_paths) if video_paths else "disabled"), - "success_rate is 1 only when planning succeeds and the Press trajectory " - "reaches the object top center.", - ] - - -def run_all_benchmarks(args: argparse.Namespace | None = None) -> Path: - """Run all atomic-action benchmarks and write the markdown report.""" - args = _parse_args() if args is None else args - if args.repeat < 1: - raise ValueError("--repeat must be at least 1.") - profile = resolve_profile(args) - _ensure_runtime_imports() - - object_presets = _select_object_presets(args.object_types, profile) - position_cases = _select_position_cases(args.position_cases, profile) - repeat = 1 if profile == "smoke" else args.repeat - - print("=" * 60) - print("Atomic Action Press Benchmark") - print("=" * 60) - print( - "Coverage: " - f"profile={profile}, {len(object_presets)} object presets x " - f"{len(position_cases)} position cases x {repeat} repeat(s)" - ) - - sim = initialize_simulation(args) - robot = create_robot(sim) - create_table(sim) - initial_qpos = robot.get_qpos().clone() - motion_gen = MotionGenerator( - cfg=MotionGenCfg(planner_cfg=ToppraPlannerCfg(robot_uid=robot.uid)) - ) - atomic_engine = _build_atomic_engine(motion_gen, robot, sim.device) - object_pool = {} - for object_index, preset in enumerate(object_presets): - obj = _create_benchmark_object(sim, preset, position_cases[0], object_index) - settle_object(sim, obj, step=2) - base_pose = obj.get_local_pose(to_matrix=True).clone() - park_rigid_object(obj, base_pose, index=object_index, sim=sim) - object_pool[preset.object_type] = (obj, base_pose) - - results: list[PressCaseResult] = [] - video_paths: list[str] = [] - print("\n=== Press Object/Position Sweep ===") - for preset in object_presets: - obj, base_pose = object_pool[preset.object_type] - for parked_index, parked_preset in enumerate(object_presets): - if parked_preset.object_type == preset.object_type: - continue - parked_obj, parked_base_pose = object_pool[parked_preset.object_type] - park_rigid_object(parked_obj, parked_base_pose, index=parked_index, sim=sim) - for position_case in position_cases: - for repeat_index in range(repeat): - result = _run_press_case( - sim=sim, - robot=robot, - atomic_engine=atomic_engine, - initial_qpos=initial_qpos, - obj=obj, - base_obj_pose=base_pose, - preset=preset, - position_case=position_case, - repeat_index=repeat_index, - press_tolerance=args.press_tolerance, - args=args, - recorded_count=len(video_paths), - ) - results.append(result) - if result.video_path: - video_paths.append(result.video_path) - _print_case_result(result) - - perf_rows = _build_perf_rows(results) - metric_rows = _build_metric_rows(results) - leaderboard_rows = _build_leaderboard_rows(results) - report_path = _write_markdown_report( - benchmark_name="atomic_action_press", - perf_rows=perf_rows, - metric_rows=metric_rows, - leaderboard_rows=leaderboard_rows, - notes=_build_notes( - object_presets, - position_cases, - repeat, - video_paths, - profile, - ), - ) - - print("\n" + "=" * 60) - print("Benchmarks complete.") - print(f"Markdown report saved: {report_path}") - print("=" * 60) - return report_path - - -def main() -> None: - """Run the CLI entry point.""" - try: - run_all_benchmarks() - except RuntimeError as exc: - raise SystemExit(str(exc)) from exc - - -if __name__ == "__main__": - main() - - -__all__ = ["add_benchmark_args", "run_all_benchmarks"] diff --git a/scripts/benchmark/atomic_action/run_benchmark.py b/scripts/benchmark/atomic_action/run_benchmark.py index ab20494e6..d50107756 100644 --- a/scripts/benchmark/atomic_action/run_benchmark.py +++ b/scripts/benchmark/atomic_action/run_benchmark.py @@ -17,7 +17,7 @@ """Dispatch benchmarks for all atomic actions. Run a single action benchmark or all action benchmarks in sequence. -Run: embodichain benchmark atomic-action --action press +Run: embodichain benchmark atomic-action --action move_end_effector """ from __future__ import annotations @@ -43,11 +43,9 @@ "pick_up": "scripts.benchmark.atomic_action.pickup_benchmark", "move_held_object": "scripts.benchmark.atomic_action.move_held_object_benchmark", "place": "scripts.benchmark.atomic_action.place_benchmark", - "press": "scripts.benchmark.atomic_action.press_benchmark", } DEFAULT_ACTIONS = tuple(ACTION_MODULES.keys()) MESH_OBJECT_ACTIONS = {"pick_up", "move_held_object", "place"} -PRESS_OBJECT_TYPES = {"bottle", "mug", "wooden_block", "all"} MESH_OBJECT_TYPES = {*MESH_OBJECT_PRESETS.keys(), "all"} @@ -57,7 +55,7 @@ def add_benchmark_args(parser: argparse.ArgumentParser) -> None: "--action", nargs="+", choices=(*ACTION_MODULES.keys(), "all"), - default=["press"], + default=["move_end_effector"], help="Atomic action benchmark(s) to run. Use 'all' for every action.", ) parser.add_argument( @@ -148,8 +146,6 @@ def _validate_object_types_for_actions( for action_name in selected_actions: if action_name in MESH_OBJECT_ACTIONS: validators[action_name] = MESH_OBJECT_TYPES - elif action_name == "press": - validators[action_name] = PRESS_OBJECT_TYPES invalid_parts = [] for action_name, valid_types in validators.items(): @@ -177,7 +173,6 @@ def _make_child_args(args: argparse.Namespace) -> argparse.Namespace: renderer=args.renderer, object_types=args.object_types, position_cases=args.position_cases, - press_tolerance=0.01, pose_cases=["all"], sequence_cases=["all"], approach_cases=args.approach_cases, @@ -224,7 +219,7 @@ def _make_child_cli_args(args: argparse.Namespace, action_name: str) -> list[str "--video_hold_steps", str(args.video_hold_steps), ] - if action_name in {"pick_up", "move_held_object", "place", "press"}: + if action_name in {"pick_up", "move_held_object", "place"}: if args.object_types: child_args.append("--object_types") child_args.extend(args.object_types) diff --git a/scripts/benchmark/curobo_extraction/run_benchmark.py b/scripts/benchmark/curobo_extraction/run_benchmark.py index f8d07603b..1cd557ea5 100644 --- a/scripts/benchmark/curobo_extraction/run_benchmark.py +++ b/scripts/benchmark/curobo_extraction/run_benchmark.py @@ -175,8 +175,7 @@ def old_assemble_result( else: positions[b, :1] = start[b] positions[b, 1:] = start[b] - duration = dt.sum(dim=1) - return PlanResult(success=alive, positions=positions, dt=dt, duration=duration) + return PlanResult(success=alive, positions=positions, dt=dt) # ============================================================================= diff --git a/scripts/benchmark/expert_program/__init__.py b/scripts/benchmark/expert_program/__init__.py new file mode 100644 index 000000000..57445d243 --- /dev/null +++ b/scripts/benchmark/expert_program/__init__.py @@ -0,0 +1,21 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Expert Program benchmark helpers.""" + +from __future__ import annotations + +__all__: list[str] = [] diff --git a/scripts/benchmark/expert_program/demo_success.py b/scripts/benchmark/expert_program/demo_success.py new file mode 100644 index 000000000..7a9b531fa --- /dev/null +++ b/scripts/benchmark/expert_program/demo_success.py @@ -0,0 +1,1393 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Measure Expert Program demo success without retries. + +The command line can either aggregate an existing raw artifact or construct one +real Gym environment from explicit Gym and Expert Program configurations. Live +runs execute every fixed seed once, discard every episode buffer, then reuse the +same raw JSON and three-table report pipeline as injected programmatic runs. + +Run offline: +``python -m scripts.benchmark.expert_program.demo_success --raw-json RAW`` + +Run live: +``python -m scripts.benchmark.expert_program.demo_success --run-simulation +--gym_config GYM --expert-program PROGRAM --case-id CASE --seeds 0 1 +--raw-json RAW`` +""" + +from __future__ import annotations + +import argparse +from collections import Counter, defaultdict +from collections.abc import Callable, Mapping, Sequence +from copy import deepcopy +from dataclasses import asdict, dataclass +from datetime import datetime, timezone +import json +import math +import os +from pathlib import Path +from statistics import mean +import sys +import time +from typing import Any + +import psutil +import torch +import gymnasium + +from embodichain.lab.gym.envs.demo import ( + DEMO_SCHEMA_VERSION, + DemoEpisodeResult, + execute_demo_episode, +) +from embodichain.lab.gym.envs.expert_program import load_expert_program +from embodichain.lab.gym.utils.gym_utils import ( + add_env_launcher_args_to_parser, + build_env_cfg_from_args, +) +from embodichain.lab.gym.utils.registration import ( + discover_task_packages, + execute_init_hooks, +) + +__all__ = [ + "DEMO_SUCCESS_SCHEMA_VERSION", + "DemoSuccessAggregates", + "DemoSuccessArtifacts", + "DemoSuccessCase", + "DemoSuccessRow", + "DemoSuccessTrial", + "MemorySnapshot", + "aggregate_demo_success_trials", + "capture_memory", + "collect_demo_success_trials", + "load_raw_trials", + "main", + "run_all_benchmarks", + "run_demo_success_benchmark", + "run_gym_demo_success_benchmark", + "write_markdown_report", + "write_raw_trials", +] + +DEMO_SUCCESS_SCHEMA_VERSION = 1 +_BENCHMARK_ID = "expert_program_demo_success" + +_TIME_COLUMNS = ( + "case", + "episodes", + "attempted_rows", + "cost_time_ms", + "mean_episode_ms", + "cpu_delta_mb", + "gpu_delta_mb", + "peak_gpu_mb", +) +_METRIC_COLUMNS = ( + "case", + "attempted", + "successes", + "success_rate", + "terminal_reasons", + "segment_failures", + "segment_failure_breakdown", + "call_failures", + "call_failure_breakdown", + "length_mean", + "length_min", + "length_max", +) +_LEADERBOARD_COLUMNS = ( + "rank", + "case", + "attempted", + "successes", + "overall_success_rate", + "length_mean", + "mean_episode_ms", +) + +EpisodeExecutor = Callable[..., DemoEpisodeResult] +EnvironmentProvider = Callable[["DemoSuccessCase"], Any] +MemorySampler = Callable[..., "MemorySnapshot"] +GymEnvironmentFactory = Callable[[argparse.Namespace, str | Path], Any] +EnvironmentCloser = Callable[[Any], None] + + +def _validate_nonempty_string(value: object, *, field_name: str) -> str: + """Return one exact non-empty string without outer whitespace.""" + if type(value) is not str: + raise TypeError(f"{field_name} must be a string.") + if not value or value != value.strip(): + raise ValueError(f"{field_name} must be non-empty without outer whitespace.") + return value + + +def _add_exception_note(error: BaseException, note: str) -> None: + """Attach an exception note on every supported Python version.""" + add_note = getattr(error, "add_note", None) + if callable(add_note): + add_note(note) + return + notes = list(getattr(error, "__notes__", ())) + notes.append(note) + error.__notes__ = notes + + +def _snapshot_string_tuple( + values: object, + *, + field_name: str, +) -> tuple[str, ...]: + """Validate and snapshot one list-or-tuple of stable string labels.""" + if type(values) not in (list, tuple): + raise TypeError(f"{field_name} must be a list or tuple.") + snapshot = tuple(values) + for index, value in enumerate(snapshot): + _validate_nonempty_string( + value, + field_name=f"{field_name}[{index}]", + ) + return snapshot + + +@dataclass(frozen=True, slots=True) +class DemoSuccessCase: + """One named demo benchmark case and its fixed evaluation seeds. + + Args: + case_id: Stable identity shown in raw artifacts and reports. + seeds: Unique seeds, each executed exactly once in the given order. + """ + + case_id: str + seeds: tuple[int, ...] + + def __post_init__(self) -> None: + _validate_nonempty_string(self.case_id, field_name="case_id") + if type(self.seeds) not in (list, tuple): + raise TypeError("seeds must be a list or tuple.") + owned_seeds = tuple(self.seeds) + if not owned_seeds: + raise ValueError("seeds must contain at least one fixed evaluation seed.") + if any(type(seed) is not int for seed in owned_seeds): + raise TypeError("Every evaluation seed must be an integer.") + if len(set(owned_seeds)) != len(owned_seeds): + raise ValueError("Evaluation seeds must be unique within a case.") + object.__setattr__(self, "seeds", owned_seeds) + + +@dataclass(frozen=True, slots=True) +class MemorySnapshot: + """Current process and PyTorch GPU memory in megabytes. + + Args: + cpu_rss_mb: Current process resident memory. + gpu_allocated_mb: Current PyTorch-allocated GPU memory. + gpu_peak_allocated_mb: Peak PyTorch GPU allocation since the last reset. + """ + + cpu_rss_mb: float + gpu_allocated_mb: float + gpu_peak_allocated_mb: float + + +@dataclass(frozen=True, slots=True) +class DemoSuccessRow: + """Normalized result for one vector-environment row. + + Args: + env_index: Zero-based row index in the vector environment. + success: Whether this row completed the episode successfully. + terminal_reason: Stable terminal-reason label. + length: Recorded row length in environment steps. + segment_failure_reasons: Segment-name-qualified failure keys. + call_failure_keys: Segment/call/status-qualified runtime failure keys. + """ + + env_index: int + success: bool + terminal_reason: str + length: int + segment_failure_reasons: tuple[str, ...] = () + call_failure_keys: tuple[str, ...] = () + + def __post_init__(self) -> None: + if type(self.env_index) is not int: + raise TypeError("env_index must be an integer.") + if self.env_index < 0: + raise ValueError("env_index must be non-negative.") + if type(self.success) is not bool: + raise TypeError("success must be a boolean.") + _validate_nonempty_string( + self.terminal_reason, + field_name="terminal_reason", + ) + if type(self.length) is not int: + raise TypeError("length must be an integer.") + if self.length < 0: + raise ValueError("length must be non-negative.") + object.__setattr__( + self, + "segment_failure_reasons", + _snapshot_string_tuple( + self.segment_failure_reasons, + field_name="segment_failure_reasons", + ), + ) + object.__setattr__( + self, + "call_failure_keys", + _snapshot_string_tuple( + self.call_failure_keys, + field_name="call_failure_keys", + ), + ) + + +@dataclass(frozen=True, slots=True) +class DemoSuccessTrial: + """Raw result for one no-retry seed execution. + + Args: + case_id: Stable benchmark case identity. + seed: Fixed seed executed exactly once. + cost_time_ms: Executor wall-clock duration in milliseconds. + cpu_delta_mb: Process RSS delta across execution. + gpu_delta_mb: PyTorch GPU allocation delta across execution. + peak_gpu_mb: Peak PyTorch GPU allocation during execution. + rows: Normalized per-environment outcomes. + episode_result: Owned JSON-compatible executor metadata. + """ + + case_id: str + seed: int + cost_time_ms: float + cpu_delta_mb: float + gpu_delta_mb: float + peak_gpu_mb: float + rows: tuple[DemoSuccessRow, ...] + episode_result: dict[str, object] + + def __post_init__(self) -> None: + _validate_nonempty_string(self.case_id, field_name="case_id") + if type(self.seed) is not int: + raise TypeError("seed must be an integer.") + numeric_fields = { + "cost_time_ms": self.cost_time_ms, + "cpu_delta_mb": self.cpu_delta_mb, + "gpu_delta_mb": self.gpu_delta_mb, + "peak_gpu_mb": self.peak_gpu_mb, + } + normalized_numeric: dict[str, float] = {} + for field_name, value in numeric_fields.items(): + if type(value) not in (int, float): + raise TypeError(f"{field_name} must be a real number.") + normalized = float(value) + if not math.isfinite(normalized): + raise ValueError(f"{field_name} must be finite.") + normalized_numeric[field_name] = normalized + if ( + normalized_numeric["cost_time_ms"] < 0.0 + or normalized_numeric["peak_gpu_mb"] < 0.0 + ): + raise ValueError("Elapsed time and peak GPU memory cannot be negative.") + if type(self.rows) not in (list, tuple): + raise TypeError("rows must be a list or tuple.") + owned_rows = tuple(self.rows) + if not owned_rows: + raise ValueError("A demo success trial must contain at least one row.") + if not all(type(row) is DemoSuccessRow for row in owned_rows): + raise TypeError("rows must contain exactly DemoSuccessRow values.") + env_indices = tuple(row.env_index for row in owned_rows) + if env_indices != tuple(range(len(owned_rows))): + raise ValueError( + "rows must have unique contiguous env_index values starting at zero." + ) + if type(self.episode_result) is not dict: + raise TypeError("episode_result must be a dictionary.") + owned_result = deepcopy(self.episode_result) + json.dumps(owned_result, allow_nan=False) + for field_name, value in normalized_numeric.items(): + object.__setattr__(self, field_name, value) + object.__setattr__(self, "rows", owned_rows) + object.__setattr__(self, "episode_result", owned_result) + + def to_dict(self) -> dict[str, object]: + """Return a JSON-compatible raw trial mapping. + + Returns: + An independently owned raw trial mapping. + """ + return { + "case_id": self.case_id, + "seed": self.seed, + "cost_time_ms": self.cost_time_ms, + "cpu_delta_mb": self.cpu_delta_mb, + "gpu_delta_mb": self.gpu_delta_mb, + "peak_gpu_mb": self.peak_gpu_mb, + "rows": [asdict(row) for row in self.rows], + "episode_result": deepcopy(self.episode_result), + } + + +@dataclass(frozen=True, slots=True) +class DemoSuccessAggregates: + """The three stable row sets rendered into the Markdown report. + + Args: + time_and_memory: Per-case timing and memory summaries. + success_and_metrics: Per-case success and diagnostic summaries. + leaderboard: All cases ranked by success rate. + """ + + time_and_memory: tuple[dict[str, object], ...] + success_and_metrics: tuple[dict[str, object], ...] + leaderboard: tuple[dict[str, object], ...] + + +@dataclass(frozen=True, slots=True) +class DemoSuccessArtifacts: + """Paths and in-memory results produced by one benchmark run. + + Args: + raw_json_path: Written lossless raw artifact. + report_path: Written three-table Markdown report. + trials: In-memory no-retry trials. + aggregates: In-memory report rows. + """ + + raw_json_path: Path + report_path: Path + trials: tuple[DemoSuccessTrial, ...] + aggregates: DemoSuccessAggregates + + +def capture_memory(*, reset_gpu_peak: bool = False) -> MemorySnapshot: + """Capture CPU RSS and PyTorch GPU allocation. + + Args: + reset_gpu_peak: Reset the PyTorch peak-memory counter before sampling. + + Returns: + Current CPU, GPU, and peak GPU memory in megabytes. + """ + cuda_available = torch.cuda.is_available() + if cuda_available and reset_gpu_peak: + torch.cuda.reset_peak_memory_stats() + cpu_rss_mb = psutil.Process(os.getpid()).memory_info().rss / 1024**2 + gpu_allocated_mb = ( + torch.cuda.memory_allocated() / 1024**2 if cuda_available else 0.0 + ) + gpu_peak_allocated_mb = ( + torch.cuda.max_memory_allocated() / 1024**2 if cuda_available else 0.0 + ) + return MemorySnapshot( + cpu_rss_mb=cpu_rss_mb, + gpu_allocated_mb=gpu_allocated_mb, + gpu_peak_allocated_mb=gpu_peak_allocated_mb, + ) + + +def _vector_or_default( + values: tuple[Any, ...], + *, + row_count: int, + default: Any, + field_name: str, +) -> tuple[Any, ...]: + """Return a validated per-row tuple or broadcast its scalar fallback.""" + if not values: + return tuple(default for _ in range(row_count)) + if len(values) != row_count: + raise ValueError( + f"DemoEpisodeResult.{field_name} has {len(values)} rows; " + f"expected {row_count}." + ) + return values + + +def _normalize_episode_rows(result: DemoEpisodeResult) -> tuple[DemoSuccessRow, ...]: + """Project a batched episode result into independent benchmark rows.""" + row_count = len(result.success) + if row_count == 0: + raise ValueError("DemoEpisodeResult.success must contain at least one row.") + lengths = _vector_or_default( + result.lengths, + row_count=row_count, + default=result.length, + field_name="lengths", + ) + terminal_reasons = _vector_or_default( + result.terminal_reasons, + row_count=row_count, + default=result.terminal_reason, + field_name="terminal_reasons", + ) + failures: list[list[str]] = [[] for _ in range(row_count)] + call_failures: list[list[str]] = [[] for _ in range(row_count)] + for segment in result.segments: + active = _vector_or_default( + segment.active, + row_count=row_count, + default=True, + field_name="segments.active", + ) + successes = _vector_or_default( + segment.successes, + row_count=row_count, + default=segment.success, + field_name="segments.successes", + ) + reasons = _vector_or_default( + segment.failure_reasons, + row_count=row_count, + default=segment.failure_reason, + field_name="segments.failure_reasons", + ) + for env_index in range(row_count): + if not active[env_index]: + continue + reason = reasons[env_index] + if reason is not None: + failures[env_index].append(f"{segment.name}:{reason}") + elif not successes[env_index]: + failures[env_index].append(f"{segment.name}:segment_failed") + runtime = segment.metadata.get("runtime") + if isinstance(runtime, Mapping): + _append_runtime_call_failures( + runtime, + segment_name=segment.name, + row_failures=call_failures, + ) + + return tuple( + DemoSuccessRow( + env_index=env_index, + success=bool(result.success[env_index]), + terminal_reason=str(terminal_reasons[env_index]), + length=int(lengths[env_index]), + segment_failure_reasons=tuple(failures[env_index]), + call_failure_keys=tuple(call_failures[env_index]), + ) + for env_index in range(row_count) + ) + + +def _append_runtime_call_failures( + runtime: Mapping[str, object], + *, + segment_name: str, + row_failures: list[list[str]], + branch_id: str | None = None, +) -> None: + """Attribute canonical runtime call failures to their environment rows.""" + env_ids = runtime.get("env_ids") + calls = runtime.get("calls") + if isinstance(env_ids, list) and isinstance(calls, list): + for call in calls: + if not isinstance(call, Mapping): + continue + semantic_id = call.get("semantic_id") + status = call.get("status") + masks = call.get("masks") + failed = masks.get("failed") if isinstance(masks, Mapping) else None + if ( + not isinstance(semantic_id, str) + or not isinstance(status, str) + or not isinstance(failed, list) + or len(failed) != len(env_ids) + ): + continue + identity = ( + f"{segment_name}:{semantic_id}:{status}" + if branch_id is None + else f"{segment_name}:{branch_id}:{semantic_id}:{status}" + ) + for env_id, is_failed in zip(env_ids, failed): + if ( + type(env_id) is int + and type(is_failed) is bool + and is_failed + and 0 <= env_id < len(row_failures) + ): + row_failures[env_id].append(identity) + + branches = runtime.get("branches") + if isinstance(branches, Mapping): + branch_ids = sorted(key for key in branches if isinstance(key, str)) + for child_branch_id in branch_ids: + branch_runtime = branches[child_branch_id] + if isinstance(branch_runtime, Mapping): + _append_runtime_call_failures( + branch_runtime, + segment_name=segment_name, + row_failures=row_failures, + branch_id=child_branch_id, + ) + + +def _executor_error_trial_rows(env: Any, reason: str) -> tuple[DemoSuccessRow, ...]: + """Return zero-length failed rows for one executor exception.""" + configured_rows = getattr(env, "num_envs", 1) + row_count = ( + configured_rows if type(configured_rows) is int and configured_rows > 0 else 1 + ) + return tuple( + DemoSuccessRow( + env_index=env_index, + success=False, + terminal_reason=reason, + length=0, + ) + for env_index in range(row_count) + ) + + +def _executor_error_metadata( + *, + episode_index: int, + reason: str, + error: Exception, + row_count: int, +) -> dict[str, object]: + """Return raw episode-shaped metadata that preserves one executor error.""" + return { + "schema_version": DEMO_SCHEMA_VERSION, + "episode_index": episode_index, + "length": 0, + "completed": False, + "success": [False] * row_count, + "terminated": [False] * row_count, + "truncated": [False] * row_count, + "terminal_reason": reason, + "segments": [], + "lengths": [0] * row_count, + "completed_by_env": [False] * row_count, + "terminal_reasons": [reason] * row_count, + "executor_error": { + "type": type(error).__name__, + "message": str(error), + }, + } + + +def collect_demo_success_trials( + cases: Sequence[DemoSuccessCase], + env_provider: EnvironmentProvider, + *, + episode_executor: EpisodeExecutor = execute_demo_episode, + clock: Callable[[], float] = time.perf_counter, + memory_sampler: MemorySampler = capture_memory, +) -> tuple[DemoSuccessTrial, ...]: + """Execute every fixed seed once and discard every resulting episode buffer. + + The caller owns environment construction and teardown. The harness performs + one non-committing seeded reset, one executor call, and one mandatory + non-committing discard reset for each seed. Executor exceptions become + failed trials only after that discard succeeds. + + Args: + cases: Named cases with fixed, unique seed sequences. + env_provider: Required environment injection. It is called once per case. + episode_executor: Demo executor, injectable for pure unit tests. + clock: High-resolution monotonic timer. + memory_sampler: CPU/GPU memory sampler. + + Returns: + Raw per-seed trials in case and seed order. + + Raises: + ValueError: If cases are empty, case IDs are duplicated, or an episode + result is malformed. + TypeError: If ``cases`` contains non-``DemoSuccessCase`` values. + """ + try: + case_values = tuple(cases) + except TypeError as error: + raise TypeError( + "cases must be an iterable of DemoSuccessCase values." + ) from error + if not case_values: + raise ValueError("cases must contain at least one benchmark case.") + if not all(type(case) is DemoSuccessCase for case in case_values): + raise TypeError("cases must contain exactly DemoSuccessCase values.") + case_ids = [case.case_id for case in case_values] + if len(set(case_ids)) != len(case_ids): + raise ValueError("Demo success benchmark case IDs must be unique.") + + trials: list[DemoSuccessTrial] = [] + episode_index = 0 + for case in case_values: + env = env_provider(case) + for seed in case.seeds: + env.reset(seed=seed, options={"save_data": False}) + executor_error: Exception | None = None + body_error: BaseException | None = None + try: + before = memory_sampler(reset_gpu_peak=True) + start = clock() + result: DemoEpisodeResult | None = None + try: + result = episode_executor(env, episode_index=episode_index) + except Exception as error: + executor_error = error + elapsed_ms = (clock() - start) * 1000.0 + after = memory_sampler(reset_gpu_peak=False) + except BaseException as error: + body_error = error + if executor_error is not None: + _add_exception_note( + body_error, + "Episode executor also failed before benchmark measurement " + f"completed: {type(executor_error).__name__}: " + f"{executor_error}", + ) + raise + finally: + try: + env.reset(options={"save_data": False}) + except BaseException as discard_error: + discard_note = ( + "Episode discard also failed: " + f"{type(discard_error).__name__}: {discard_error}" + ) + if body_error is not None: + _add_exception_note(body_error, discard_note) + elif executor_error is not None: + _add_exception_note(executor_error, discard_note) + raise executor_error + else: + raise + + if executor_error is None: + if result is None: + raise RuntimeError("The demo episode executor returned no result.") + rows = _normalize_episode_rows(result) + episode_result = result.to_metadata() + else: + reason = f"executor_error:{type(executor_error).__name__}" + rows = _executor_error_trial_rows(env, reason) + episode_result = _executor_error_metadata( + episode_index=episode_index, + reason=reason, + error=executor_error, + row_count=len(rows), + ) + trials.append( + DemoSuccessTrial( + case_id=case.case_id, + seed=seed, + cost_time_ms=elapsed_ms, + cpu_delta_mb=after.cpu_rss_mb - before.cpu_rss_mb, + gpu_delta_mb=after.gpu_allocated_mb - before.gpu_allocated_mb, + peak_gpu_mb=after.gpu_peak_allocated_mb, + rows=rows, + episode_result=episode_result, + ) + ) + episode_index += 1 + return tuple(trials) + + +def _counter_json(counter: Counter[str]) -> str: + """Render a deterministic compact JSON counter for one Markdown cell.""" + ordered = dict(sorted(counter.items(), key=lambda item: (-item[1], item[0]))) + return json.dumps(ordered, ensure_ascii=False, separators=(",", ":")) + + +def _validate_unique_trials( + trials: Sequence[DemoSuccessTrial], +) -> tuple[DemoSuccessTrial, ...]: + """Snapshot non-empty exact trials and reject duplicate identities.""" + try: + trial_values = tuple(trials) + except TypeError as error: + raise TypeError( + "trials must be an iterable of DemoSuccessTrial values." + ) from error + if not trial_values: + raise ValueError("trials must contain at least one demo success trial.") + if not all(type(trial) is DemoSuccessTrial for trial in trial_values): + raise TypeError("trials must contain exactly DemoSuccessTrial values.") + seen: set[tuple[str, int]] = set() + for trial in trial_values: + identity = (trial.case_id, trial.seed) + if identity in seen: + raise ValueError( + "Duplicate demo success trial for " + f"case_id={trial.case_id!r}, seed={trial.seed}." + ) + seen.add(identity) + return trial_values + + +def aggregate_demo_success_trials( + trials: Sequence[DemoSuccessTrial], +) -> DemoSuccessAggregates: + """Aggregate raw trials by case and rank every represented case. + + Args: + trials: Unique case-and-seed trials. + + Returns: + Stable rows for the three report tables. + + Raises: + ValueError: If trials are empty or a case-and-seed identity occurs more + than once. + TypeError: If ``trials`` contains non-``DemoSuccessTrial`` values. + """ + trial_values = _validate_unique_trials(trials) + grouped: dict[str, list[DemoSuccessTrial]] = defaultdict(list) + for trial in trial_values: + grouped[trial.case_id].append(trial) + + time_rows: list[dict[str, object]] = [] + metric_rows: list[dict[str, object]] = [] + for case_id in sorted(grouped): + case_trials = grouped[case_id] + rows = [row for trial in case_trials for row in trial.rows] + attempted = len(rows) + successes = sum(row.success for row in rows) + lengths = [row.length for row in rows] + terminal_reasons = Counter(row.terminal_reason for row in rows) + segment_reasons = Counter( + reason for row in rows for reason in row.segment_failure_reasons + ) + call_failure_keys = Counter( + key for row in rows for key in row.call_failure_keys + ) + time_rows.append( + { + "case": case_id, + "episodes": len(case_trials), + "attempted_rows": attempted, + "cost_time_ms": sum(trial.cost_time_ms for trial in case_trials), + "mean_episode_ms": mean(trial.cost_time_ms for trial in case_trials), + "cpu_delta_mb": mean(trial.cpu_delta_mb for trial in case_trials), + "gpu_delta_mb": mean(trial.gpu_delta_mb for trial in case_trials), + "peak_gpu_mb": max(trial.peak_gpu_mb for trial in case_trials), + } + ) + metric_rows.append( + { + "case": case_id, + "attempted": attempted, + "successes": successes, + "success_rate": successes / attempted, + "terminal_reasons": _counter_json(terminal_reasons), + "segment_failures": sum(segment_reasons.values()), + "segment_failure_breakdown": _counter_json(segment_reasons), + "call_failures": sum(call_failure_keys.values()), + "call_failure_breakdown": _counter_json(call_failure_keys), + "length_mean": mean(lengths), + "length_min": min(lengths), + "length_max": max(lengths), + } + ) + + time_by_case = {str(row["case"]): row for row in time_rows} + ranked_metrics = sorted( + metric_rows, + key=lambda row: (-float(row["success_rate"]), str(row["case"])), + ) + leaderboard = tuple( + { + "rank": rank, + "case": row["case"], + "attempted": row["attempted"], + "successes": row["successes"], + "overall_success_rate": row["success_rate"], + "length_mean": row["length_mean"], + "mean_episode_ms": time_by_case[str(row["case"])]["mean_episode_ms"], + } + for rank, row in enumerate(ranked_metrics, start=1) + ) + return DemoSuccessAggregates( + time_and_memory=tuple(time_rows), + success_and_metrics=tuple(metric_rows), + leaderboard=leaderboard, + ) + + +def write_raw_trials(path: str | Path, trials: Sequence[DemoSuccessTrial]) -> Path: + """Write lossless per-seed and per-row results to one raw JSON artifact. + + Args: + path: Destination JSON path. + trials: Unique case-and-seed trials. + + Returns: + Written artifact path. + + Raises: + ValueError: If trials are empty or a case-and-seed identity occurs more + than once. + TypeError: If ``trials`` contains non-``DemoSuccessTrial`` values. + """ + trial_values = _validate_unique_trials(trials) + output = Path(path) + output.parent.mkdir(parents=True, exist_ok=True) + payload = { + "schema_version": DEMO_SUCCESS_SCHEMA_VERSION, + "benchmark": _BENCHMARK_ID, + "trials": [trial.to_dict() for trial in trial_values], + } + output.write_text( + json.dumps(payload, indent=2, ensure_ascii=False, allow_nan=False) + "\n", + encoding="utf-8", + ) + return output + + +def _require_mapping(value: object, field_name: str) -> Mapping[str, object]: + """Validate one raw JSON mapping boundary.""" + if not isinstance(value, Mapping): + raise ValueError(f"{field_name} must be a JSON object.") + return value + + +def _load_row(value: object, field_name: str) -> DemoSuccessRow: + """Decode one normalized row from a raw JSON trial.""" + data = _require_mapping(value, field_name) + failures = data.get("segment_failure_reasons") + if not isinstance(failures, list) or not all( + isinstance(reason, str) for reason in failures + ): + raise ValueError(f"{field_name}.segment_failure_reasons must be a string list.") + call_failures = data.get("call_failure_keys") + if not isinstance(call_failures, list) or not all( + isinstance(key, str) for key in call_failures + ): + raise ValueError(f"{field_name}.call_failure_keys must be a string list.") + env_index = data.get("env_index") + success = data.get("success") + terminal_reason = data.get("terminal_reason") + length = data.get("length") + if type(env_index) is not int or env_index < 0: + raise ValueError(f"{field_name}.env_index must be a non-negative integer.") + if type(success) is not bool: + raise ValueError(f"{field_name}.success must be a boolean.") + if not isinstance(terminal_reason, str): + raise ValueError(f"{field_name}.terminal_reason must be a string.") + if type(length) is not int or length < 0: + raise ValueError(f"{field_name}.length must be a non-negative integer.") + return DemoSuccessRow( + env_index=env_index, + success=success, + terminal_reason=terminal_reason, + length=length, + segment_failure_reasons=tuple(failures), + call_failure_keys=tuple(call_failures), + ) + + +def _required_number(data: Mapping[str, object], key: str, field_name: str) -> float: + """Read one finite raw numeric field without accepting booleans.""" + value = data.get(key) + if type(value) not in {int, float} or not math.isfinite(float(value)): + raise ValueError(f"{field_name}.{key} must be a finite number.") + return float(value) + + +def _load_trial(value: object, index: int) -> DemoSuccessTrial: + """Decode one validated trial from a raw JSON artifact.""" + field_name = f"trials[{index}]" + data = _require_mapping(value, field_name) + case_id = data.get("case_id") + seed = data.get("seed") + rows = data.get("rows") + episode_result = data.get("episode_result") + if not isinstance(case_id, str) or not case_id: + raise ValueError(f"{field_name}.case_id must be a non-empty string.") + if type(seed) is not int: + raise ValueError(f"{field_name}.seed must be an integer.") + if not isinstance(rows, list): + raise ValueError(f"{field_name}.rows must be a list.") + episode_mapping = _require_mapping(episode_result, f"{field_name}.episode_result") + return DemoSuccessTrial( + case_id=case_id, + seed=seed, + cost_time_ms=_required_number(data, "cost_time_ms", field_name), + cpu_delta_mb=_required_number(data, "cpu_delta_mb", field_name), + gpu_delta_mb=_required_number(data, "gpu_delta_mb", field_name), + peak_gpu_mb=_required_number(data, "peak_gpu_mb", field_name), + rows=tuple( + _load_row(row, f"{field_name}.rows[{i}]") for i, row in enumerate(rows) + ), + episode_result=dict(episode_mapping), + ) + + +def load_raw_trials(path: str | Path) -> tuple[DemoSuccessTrial, ...]: + """Load a raw artifact for deterministic offline re-aggregation. + + Args: + path: Existing raw JSON artifact. + + Returns: + Validated trials in artifact order. + + Raises: + ValueError: If the artifact schema is invalid or contains no valid trial. + """ + payload = json.loads(Path(path).read_text(encoding="utf-8")) + data = _require_mapping(payload, "raw benchmark") + if data.get("schema_version") != DEMO_SUCCESS_SCHEMA_VERSION: + raise ValueError( + "Unsupported demo success raw schema version: " + f"{data.get('schema_version')!r}." + ) + if data.get("benchmark") != _BENCHMARK_ID: + raise ValueError("Raw JSON is not an Expert Program demo success artifact.") + raw_trials = data.get("trials") + if not isinstance(raw_trials, list): + raise ValueError("raw benchmark.trials must be a list.") + trials = tuple(_load_trial(trial, index) for index, trial in enumerate(raw_trials)) + return _validate_unique_trials(trials) + + +def _format_value(column: str, value: object) -> str: + """Format one Markdown value deterministically.""" + if isinstance(value, float): + if column.endswith("rate"): + return f"{value:.2%}" + return f"{value:.6f}" + return str(value).replace("|", "\\|").replace("\n", " ") + + +def _format_table( + rows: Sequence[Mapping[str, object]], columns: tuple[str, ...] +) -> list[str]: + """Render one Markdown table with a stable schema.""" + lines = [ + "| " + " | ".join(columns) + " |", + "| " + " | ".join("---" for _ in columns) + " |", + ] + lines.extend( + "| " + + " | ".join(_format_value(column, row.get(column)) for column in columns) + + " |" + for row in rows + ) + return lines + + +def write_markdown_report(path: str | Path, aggregates: DemoSuccessAggregates) -> Path: + """Write exactly one report containing exactly the required three tables. + + Args: + path: Destination Markdown path. + aggregates: Rows for timing, success metrics, and leaderboard tables. + + Returns: + Written report path. + """ + output = Path(path) + output.parent.mkdir(parents=True, exist_ok=True) + lines = [ + "# Expert Program Demo Success Benchmark", + "", + f"Generated at: {datetime.now(timezone.utc).isoformat(timespec='seconds')}", + "", + "Each fixed seed is executed once, no failed episode is retried, and all " + "episode buffers are discarded without being committed.", + "", + "## Time & Memory", + "", + ] + lines.extend(_format_table(aggregates.time_and_memory, _TIME_COLUMNS)) + lines.extend(["", "## Success & Other Metrics", ""]) + lines.extend(_format_table(aggregates.success_and_metrics, _METRIC_COLUMNS)) + lines.extend(["", "## Leaderboard", ""]) + lines.extend(_format_table(aggregates.leaderboard, _LEADERBOARD_COLUMNS)) + output.write_text("\n".join(lines) + "\n", encoding="utf-8") + return output + + +def run_demo_success_benchmark( + cases: Sequence[DemoSuccessCase], + env_provider: EnvironmentProvider, + *, + raw_json_path: str | Path, + report_path: str | Path, + episode_executor: EpisodeExecutor = execute_demo_episode, + clock: Callable[[], float] = time.perf_counter, + memory_sampler: MemorySampler = capture_memory, +) -> DemoSuccessArtifacts: + """Collect no-retry trials and write one raw JSON plus one Markdown report. + + Args: + cases: Named cases with fixed, unique seed sequences. + env_provider: Environment injection called once per case. + raw_json_path: Destination for lossless trials. + report_path: Destination for the three-table report. + episode_executor: Demo executor, injectable for tests. + clock: High-resolution monotonic timer. + memory_sampler: CPU/GPU memory sampler. + + Returns: + Written paths, raw trials, and aggregate rows. + + Raises: + ValueError: If output paths collide or trial identities are invalid. + """ + if Path(raw_json_path).resolve() == Path(report_path).resolve(): + raise ValueError("raw_json_path and report_path must be different files.") + trials = collect_demo_success_trials( + cases, + env_provider, + episode_executor=episode_executor, + clock=clock, + memory_sampler=memory_sampler, + ) + aggregates = aggregate_demo_success_trials(trials) + raw_path = write_raw_trials(raw_json_path, trials) + markdown_path = write_markdown_report(report_path, aggregates) + return DemoSuccessArtifacts( + raw_json_path=raw_path, + report_path=markdown_path, + trials=trials, + aggregates=aggregates, + ) + + +def _create_gym_demo_success_environment( + launcher_args: argparse.Namespace, + expert_program_path: str | Path, +) -> Any: + """Create one configured Gym environment through the standard launcher APIs.""" + gym_config_path = getattr(launcher_args, "gym_config", "") + if not gym_config_path: + raise ValueError("launcher_args.gym_config must select a Gym config file.") + if getattr(launcher_args, "action_config", None) is not None: + raise ValueError( + "--action_config is not supported by the Expert Program benchmark." + ) + + discover_task_packages() + execute_init_hooks() + env_cfg, gym_config, action_config = build_env_cfg_from_args(launcher_args) + if action_config: + raise RuntimeError( + "The Expert Program benchmark environment builder produced an " + "unexpected action configuration." + ) + env_cfg.expert_program = load_expert_program(expert_program_path) + return gymnasium.make(id=gym_config["id"], cfg=env_cfg) + + +def _flush_simulation_cleanup_queue() -> None: + """Flush deferred simulation cleanup after live benchmark work.""" + from embodichain.lab.sim.sim_manager import SimulationManager + + SimulationManager.flush_cleanup_queue() + + +def _close_gym_demo_success_environment(env: Any) -> None: + """Close one benchmark environment without terminating the host process.""" + target = getattr(env, "unwrapped", env) + close = getattr(target, "close", None) + if not callable(close): + raise TypeError("Benchmark environment must expose close().") + + close_error: BaseException | None = None + try: + close(exit_process=False) + except BaseException as error: + close_error = error + + try: + _flush_simulation_cleanup_queue() + except BaseException as error: + if close_error is None: + raise + _add_exception_note( + close_error, + "Simulation cleanup also failed: " f"{type(error).__name__}: {error}", + ) + if close_error is not None: + raise close_error + + +def run_gym_demo_success_benchmark( + case: DemoSuccessCase, + *, + launcher_args: argparse.Namespace, + expert_program_path: str | Path, + raw_json_path: str | Path, + report_path: str | Path, + episode_executor: EpisodeExecutor = execute_demo_episode, + clock: Callable[[], float] = time.perf_counter, + memory_sampler: MemorySampler = capture_memory, + environment_factory: GymEnvironmentFactory | None = None, + environment_closer: EnvironmentCloser | None = None, +) -> DemoSuccessArtifacts: + """Run one configured real-environment benchmark case and close it safely. + + One environment is constructed for the case and reused across its fixed + seeds. The shared harness performs exactly one execution per seed between + non-committing seeded and discard resets. Closing the environment is an + additional abort barrier and never commits an episode. + + Args: + case: Named case and unique fixed evaluation seeds. + launcher_args: Standard environment-launcher arguments containing the + Gym configuration path and simulation overrides. + expert_program_path: Explicit Expert Program JSON/YAML configuration. + raw_json_path: Destination for lossless per-seed results. + report_path: Destination for the three-table Markdown report. + episode_executor: Demo executor, injectable for pure tests. + clock: High-resolution monotonic timer. + memory_sampler: CPU/GPU memory sampler. + environment_factory: Optional environment construction override. + environment_closer: Optional deterministic close override. + + Returns: + Written artifacts and the in-memory no-retry results. + + Raises: + ValueError: If launcher inputs, output paths, or trials are invalid. + RuntimeError: If environment construction, execution, or cleanup fails. + """ + factory = environment_factory or _create_gym_demo_success_environment + closer = environment_closer or _close_gym_demo_success_environment + try: + env = factory(launcher_args, expert_program_path) + except BaseException as factory_error: + try: + _flush_simulation_cleanup_queue() + except BaseException as cleanup_error: + _add_exception_note( + factory_error, + "Benchmark environment construction cleanup also failed: " + f"{type(cleanup_error).__name__}: {cleanup_error}", + ) + raise + body_error: BaseException | None = None + try: + return run_all_benchmarks( + (case,), + lambda requested_case: env, + raw_json_path=raw_json_path, + report_path=report_path, + episode_executor=episode_executor, + clock=clock, + memory_sampler=memory_sampler, + ) + except BaseException as error: + body_error = error + raise + finally: + try: + closer(env) + except BaseException as cleanup_error: + if body_error is None: + raise + _add_exception_note( + body_error, + "Benchmark environment cleanup also failed: " + f"{type(cleanup_error).__name__}: {cleanup_error}", + ) + + +def run_all_benchmarks( + cases: Sequence[DemoSuccessCase], + env_provider: EnvironmentProvider, + *, + raw_json_path: str | Path, + report_path: str | Path, + episode_executor: EpisodeExecutor = execute_demo_episode, + clock: Callable[[], float] = time.perf_counter, + memory_sampler: MemorySampler = capture_memory, +) -> DemoSuccessArtifacts: + """Run the injected demo benchmark and print its two artifact paths. + + Args: + cases: Named cases with fixed, unique seed sequences. + env_provider: Environment injection called once per case. + raw_json_path: Destination for lossless trials. + report_path: Destination for the three-table report. + episode_executor: Demo executor, injectable for tests. + clock: High-resolution monotonic timer. + memory_sampler: CPU/GPU memory sampler. + + Returns: + Written paths, raw trials, and aggregate rows. + """ + print("=" * 60) + print("Expert Program Demo Success Benchmark") + print("=" * 60) + artifacts = run_demo_success_benchmark( + cases, + env_provider, + raw_json_path=raw_json_path, + report_path=report_path, + episode_executor=episode_executor, + clock=clock, + memory_sampler=memory_sampler, + ) + print(f"Raw JSON saved: {artifacts.raw_json_path}") + print(f"Markdown report saved: {artifacts.report_path}") + print("=" * 60) + print("Benchmarks complete.") + print("=" * 60) + return artifacts + + +def _build_parser() -> argparse.ArgumentParser: + """Build the offline-aggregation and live-simulation command parser.""" + parser = argparse.ArgumentParser( + description=( + "Run one fixed-seed Expert Program benchmark or aggregate an " + "existing raw JSON artifact." + ) + ) + add_env_launcher_args_to_parser(parser, require_gym_config=False) + parser.set_defaults( + num_envs=None, + renderer=None, + viser_image_fps=None, + ) + parser.add_argument( + "--run-simulation", + action="store_true", + help="Create a Gym environment and collect raw fixed-seed trials.", + ) + parser.add_argument( + "--expert-program", + type=Path, + default=None, + help="Expert Program JSON/YAML file used by --run-simulation.", + ) + parser.add_argument( + "--case-id", + type=str, + default=None, + help="Stable benchmark case identity used by --run-simulation.", + ) + parser.add_argument( + "--seeds", + type=int, + nargs="+", + default=None, + help="Unique fixed seeds, each executed exactly once in the given order.", + ) + parser.add_argument( + "--raw-json", + type=Path, + required=True, + help=( + "Raw JSON destination for --run-simulation, or an existing raw " + "artifact in offline aggregation mode." + ), + ) + parser.add_argument( + "--report", + type=Path, + default=None, + help="Output Markdown path (default: RAW with a .md suffix).", + ) + return parser + + +def _provided_option_strings(argv: Sequence[str]) -> frozenset[str]: + """Return normalized long option names explicitly present in ``argv``.""" + return frozenset(token.split("=", 1)[0] for token in argv if token.startswith("--")) + + +def _validate_cli_mode( + parser: argparse.ArgumentParser, + args: argparse.Namespace, + *, + provided_options: frozenset[str], +) -> None: + """Reject incomplete or mixed live/offline command-line inputs.""" + live_values = { + "--gym_config": args.gym_config, + "--expert-program": args.expert_program, + "--case-id": args.case_id, + "--seeds": args.seeds, + } + if args.run_simulation: + missing = [name for name, value in live_values.items() if not value] + if missing: + parser.error("--run-simulation requires " + ", ".join(missing) + ".") + if args.preview: + parser.error("--preview is not supported by --run-simulation.") + if args.action_config is not None: + parser.error("--action_config is not supported by --run-simulation.") + return + + offline_options = frozenset({"--raw-json", "--report"}) + mixed_options = sorted(provided_options - offline_options) + if mixed_options: + parser.error( + "Offline aggregation accepts only --raw-json and --report; " + "live environment options require --run-simulation: " + + ", ".join(mixed_options) + + "." + ) + + +def main(argv: Sequence[str] | None = None) -> int: + """Run live fixed-seed trials or aggregate existing raw benchmark data. + + Args: + argv: Optional command-line arguments for embedding and tests. + + Returns: + Zero after the report is written. + """ + raw_argv = tuple(sys.argv[1:] if argv is None else argv) + parser = _build_parser() + args = parser.parse_args(raw_argv) + _validate_cli_mode( + parser, + args, + provided_options=_provided_option_strings(raw_argv), + ) + report_path = args.report or args.raw_json.with_suffix(".md") + if args.raw_json.resolve() == report_path.resolve(): + raise ValueError( + "The Markdown report must not overwrite the raw JSON artifact." + ) + if args.run_simulation: + case = DemoSuccessCase( + case_id=args.case_id, + seeds=tuple(args.seeds), + ) + run_gym_demo_success_benchmark( + case, + launcher_args=args, + expert_program_path=args.expert_program, + raw_json_path=args.raw_json, + report_path=report_path, + ) + return 0 + + trials = load_raw_trials(args.raw_json) + write_markdown_report(report_path, aggregate_demo_success_trials(trials)) + print(f"Markdown report saved: {report_path}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md b/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md index 66463bb2c..ae1a1bddc 100644 --- a/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md +++ b/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md @@ -431,7 +431,7 @@ For Atomic Action tracks, separate `action_planning_ms`, Distinguish three evaluation views: 1. **path-only**: resample by arc length and compare geometry; -2. **native-timing**: use each planner's own `dt/duration`; +2. **native-timing**: use each planner's own `dt` and derived duration; 3. **common-execution**: use the same controller, control dt, and simulator. Do not directly compare NMG's fixed nominal `dt=0.01` against IK interpolation @@ -561,7 +561,6 @@ Suggested coverage: | PickUp | Approach/lift plan succeeds, `held_object` is created, minimum object lift is reached, no drop | | MoveHeldObject | Object reaches target pose, grasp remains stable, object drift/tilt stays within threshold | | Place | Place pose reached, release succeeds, final object pose is correct and stable | -| Press | Press depth and valid contact/force reached, retract succeeds, no abnormal object motion | | Pick-Move-Place | Every stage succeeds in sequence; final object pose and release state are correct | Record: @@ -675,7 +674,6 @@ tracks: - pick_up - move_held_object - place - - press - pick_move_place scenario_overrides: @@ -960,7 +958,7 @@ Minimum tests: - Parameterize planner construction in Atomic Action benchmarks. - Explicitly separate `ik_interp` and `motion_gen`. - Reuse current object/position/approach profiles and physical-success rules. -- Add MoveEndEffector, PickUp, MoveHeldObject, Place, Press, and +- Add MoveEndEffector, PickUp, MoveHeldObject, Place, and Pick-Move-Place. - Add controller tracking, collision/contact, and stable-hold metrics. diff --git a/scripts/benchmark/motion_generation/planners/ik_interpolate.py b/scripts/benchmark/motion_generation/planners/ik_interpolate.py index fa241b386..f9ef082b1 100644 --- a/scripts/benchmark/motion_generation/planners/ik_interpolate.py +++ b/scripts/benchmark/motion_generation/planners/ik_interpolate.py @@ -18,6 +18,8 @@ from __future__ import annotations +import math + import torch from embodichain.lab.sim.planners import PlanResult @@ -41,6 +43,16 @@ def build(self) -> None: def plan(self, case: BenchmarkCase) -> PlanResult: """Solve each waypoint sequentially while retaining per-env failures.""" robot = self.context.robot + interpolation_dt = self.spec.config.get("interpolation_dt") + if isinstance(interpolation_dt, bool) or not isinstance( + interpolation_dt, (int, float) + ): + raise ValueError( + "ik_interpolate requires an explicit numeric interpolation_dt." + ) + interpolation_dt = float(interpolation_dt) + if not math.isfinite(interpolation_dt) or interpolation_dt <= 0.0: + raise ValueError("interpolation_dt must be finite and greater than zero.") seed = case.start_qpos alive = torch.ones(case.batch_size, dtype=torch.bool, device=robot.device) targets = [seed] @@ -66,10 +78,13 @@ def plan(self, case: BenchmarkCase) -> PlanResult: interp_num=self.context.sample_interval, device=robot.device, ) + dt = torch.zeros(positions.shape[:2], dtype=torch.float32, device=robot.device) + if positions.shape[1] > 1: + dt[:, 1:] = interpolation_dt return PlanResult( success=alive, positions=positions, - duration=torch.zeros(case.batch_size, device=robot.device), + dt=dt, ) diff --git a/scripts/benchmark/motion_generation/suites/coverage.yaml b/scripts/benchmark/motion_generation/suites/coverage.yaml index ebcd8faa9..6cc331254 100644 --- a/scripts/benchmark/motion_generation/suites/coverage.yaml +++ b/scripts/benchmark/motion_generation/suites/coverage.yaml @@ -28,7 +28,8 @@ planners: adapter: ik_interpolate role: diagnostic_baseline enabled: false - config: {} + config: + interpolation_dt: 0.025 - id: toppra adapter: toppra role: diagnostic_baseline diff --git a/scripts/benchmark/motion_generation/suites/smoke.yaml b/scripts/benchmark/motion_generation/suites/smoke.yaml index eafbbc5a6..459fe3f49 100644 --- a/scripts/benchmark/motion_generation/suites/smoke.yaml +++ b/scripts/benchmark/motion_generation/suites/smoke.yaml @@ -28,7 +28,8 @@ planners: adapter: ik_interpolate role: diagnostic_baseline enabled: false - config: {} + config: + interpolation_dt: 0.025 - id: toppra adapter: toppra role: diagnostic_baseline diff --git a/scripts/tools/expert_program_rollout_report.py b/scripts/tools/expert_program_rollout_report.py new file mode 100644 index 000000000..ee0bf2b42 --- /dev/null +++ b/scripts/tools/expert_program_rollout_report.py @@ -0,0 +1,512 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Render the deterministic declarative Expert Program rollout report.""" + +from __future__ import annotations + +import argparse +from collections.abc import Sequence +from dataclasses import dataclass +from pathlib import Path + +__all__ = [ + "DEFAULT_REPORT_PATH", + "REPOSITORY_ROOT", + "SourceSnapshot", + "TaskSizeMetric", + "build_task_size_metrics", + "main", + "render_report", +] + + +REPOSITORY_ROOT = Path(__file__).resolve().parents[2] +DEFAULT_REPORT_PATH = REPOSITORY_ROOT / "docs/design/expert_program_rollout_report.md" + + +@dataclass(frozen=True) +class SourceSnapshot: + """One source file included in a task migration size snapshot. + + Args: + path: Repository-relative source path. + lines: Raw LF-byte count. + bytes: Raw on-disk byte count. + """ + + path: str + lines: int + bytes: int + + +@dataclass(frozen=True) +class TaskSizeMetric: + """Baseline and current source size for one migrated task. + + Args: + task: Stable task label. + baseline_lines: Recorded pre-migration LF-byte count. + baseline_bytes: Recorded pre-migration byte count. + sources: Explicit current source snapshots. + """ + + task: str + baseline_lines: int + baseline_bytes: int + sources: tuple[SourceSnapshot, ...] + + @property + def current_lines(self) -> int: + """Return the current LF-delimited line count across all source files.""" + return sum(source.lines for source in self.sources) + + @property + def current_bytes(self) -> int: + """Return the current raw byte count across all source files.""" + return sum(source.bytes for source in self.sources) + + +@dataclass(frozen=True) +class _TaskSizeSpec: + """Stable baseline snapshot and explicit current source paths.""" + + task: str + baseline_lines: int + baseline_bytes: int + baseline_blob: str + source_paths: tuple[str, ...] + + +_TASK_SIZE_SPECS = ( + _TaskSizeSpec( + task="Cube", + baseline_lines=598, + baseline_bytes=23_912, + baseline_blob="1965563b060d1fc889f03ad13d47655c2edcd99b", + source_paths=( + "embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py", + "embodichain_tasks/configs/expert_program/multi_segments/" + "repeated_cube_pick_place.yaml", + ), + ), + _TaskSizeSpec( + task="Drawer", + baseline_lines=245, + baseline_bytes=8_833, + baseline_blob="3b4cbdc09537098b4f109d46efb8785b88f31ce1", + source_paths=( + "embodichain_tasks/embodichain_tasks/tableware/open_drawer.py", + "embodichain_tasks/configs/expert_program/tableware/open_drawer.json", + ), + ), +) + + +_FRAMEWORK_CAPABILITIES = ( + ( + "Pick + Place(at)", + "framework-tested", + "per-embodiment integration", + "Typed goals, compilation, execution, and terminal effects are covered.", + ), + ( + "Attach/release effect", + "framework-tested", + "per-embodiment integration", + "Effects use accepted commands plus live object-to-endpoint pose evidence.", + ), + ( + "OperateArticulation", + "framework-tested", + "per-embodiment integration", + "Typed articulation goals and execution contracts are covered.", + ), + ( + "Articulation effect", + "framework-tested", + "per-embodiment integration", + "Joint-state terminal effect validation is covered.", + ), + ( + "V1 sequential", + "framework-tested", + "per-task integration", + "Ordered call execution and failure propagation are covered.", + ), + ( + "HandOver", + "framework-tested", + "per-embodiment integration", + "Coordinated effects and bounded recovery are covered.", + ), + ( + "Place relation (on/inside)", + "framework-tested", + "per-scene integration", + "Standard support/container target-frame bindings install exact grounders.", + ), + ( + "Registered call", + "framework-tested", + "integration-required", + "Production registration must declare and validate its concrete contract.", + ), + ( + "V2 parallel", + "framework-tested", + "integration-required", + "Joint/cuRobo validation is available; physical parallel acceptance remains.", + ), +) + + +_LANDED_INTEGRATIONS = ( + ( + "UR5", + "Cube Pick + Place", + "Pick + Place(at)", + "attach/release", + "V1 sequential", + "checked in", + "fixed-seed three-cycle and physical-loss recovery slow gates", + ), + ( + "CobotMagic", + "Open Drawer", + "OperateArticulation", + "articulation effect", + "V1 sequential", + "checked in", + "fixed-seed supported-simulation slow gate; not release-required", + ), + ( + "Dual UR5 + PGI", + "HandOver", + "Pick + HandOver", + "attach/transfer", + "V1 sequential", + "checked in", + "three consecutive supported-simulation contact-dynamics runs", + ), +) + + +def _count_source(repository_root: Path, relative_path: str) -> SourceSnapshot: + """Count raw LF bytes and total bytes for one explicit repository file.""" + data = (repository_root / relative_path).read_bytes() + return SourceSnapshot( + path=relative_path, + lines=data.count(b"\n"), + bytes=len(data), + ) + + +def build_task_size_metrics( + repository_root: str | Path = REPOSITORY_ROOT, +) -> tuple[TaskSizeMetric, ...]: + """Build deterministic migration metrics from the four declared source files. + + Args: + repository_root: EmbodiChain checkout root containing the declared files. + + Returns: + Metrics in the stable order defined by the report specification. + """ + root = Path(repository_root) + return tuple( + TaskSizeMetric( + task=spec.task, + baseline_lines=spec.baseline_lines, + baseline_bytes=spec.baseline_bytes, + sources=tuple( + _count_source(root, relative_path) + for relative_path in spec.source_paths + ), + ) + for spec in _TASK_SIZE_SPECS + ) + + +def _render_table(headers: tuple[str, ...], rows: Sequence[Sequence[str]]) -> list[str]: + """Render a Markdown table with stable column and row ordering.""" + return [ + "| " + " | ".join(headers) + " |", + "| " + " | ".join("---" for _ in headers) + " |", + *("| " + " | ".join(row) + " |" for row in rows), + ] + + +def _format_delta(current: int, baseline: int) -> str: + """Format an absolute and baseline-relative size delta.""" + delta = current - baseline + percentage = delta / baseline * 100.0 + return f"{delta:+d} ({percentage:+.1f}%)" + + +def render_report(metrics: Sequence[TaskSizeMetric]) -> str: + """Render the static rollout snapshot as deterministic Markdown. + + Args: + metrics: Task size metrics, normally from :func:`build_task_size_metrics`. + + Returns: + Complete Markdown document ending with exactly one newline. + """ + if not metrics: + raise ValueError("metrics must contain at least one task snapshot.") + + metric_rows = [] + for metric in metrics: + source_paths = "
".join(f"`{source.path}`" for source in metric.sources) + metric_rows.append( + ( + metric.task, + str(metric.baseline_lines), + str(metric.current_lines), + _format_delta(metric.current_lines, metric.baseline_lines), + str(metric.baseline_bytes), + str(metric.current_bytes), + _format_delta(metric.current_bytes, metric.baseline_bytes), + source_paths, + ) + ) + + total_baseline_lines = sum(metric.baseline_lines for metric in metrics) + total_current_lines = sum(metric.current_lines for metric in metrics) + total_baseline_bytes = sum(metric.baseline_bytes for metric in metrics) + total_current_bytes = sum(metric.current_bytes for metric in metrics) + metric_rows.append( + ( + "Total", + str(total_baseline_lines), + str(total_current_lines), + _format_delta(total_current_lines, total_baseline_lines), + str(total_baseline_bytes), + str(total_current_bytes), + _format_delta(total_current_bytes, total_baseline_bytes), + "the four files above", + ) + ) + + lines = [ + "# Declarative Expert Program Rollout Report", + "", + ( + "This is a deterministic, static Phase 8 snapshot of checked-in " + "framework and integration code. It does not run simulation, report " + "physical acceptance, or certify production readiness for an embodiment." + ), + "", + "## Framework Contract Matrix", + "", + ( + "`framework-tested` describes the reusable framework contract only. A " + "task appears in the matrix below only when its integration/production " + "code is checked in; that code status does not imply physical acceptance." + ), + "", + ] + lines.extend( + _render_table( + ("Capability", "Framework status", "Integration gate", "Scope"), + _FRAMEWORK_CAPABILITIES, + ) + ) + lines.extend( + [ + "", + ( + "Parallel execution remains fail-closed by default. Resource " + "declarations alone do not authorize production concurrency; the " + "selected embodiment must provide an authoritative validator." + ), + "", + "## Checked-in Integration Matrix", + "", + ( + "Only the checked-in vertical slices below are classified as " + "integration/production code. Physical acceptance is tracked " + "separately." + ), + "", + ] + ) + lines.extend( + _render_table( + ( + "Embodiment", + "Task", + "Skill contract", + "Terminal effect", + "Program schema", + "Code status", + "Physical acceptance", + ), + _LANDED_INTEGRATIONS, + ) + ) + lines.extend( + [ + "", + ( + "Place-relation bindings are reusable scene integration rather than " + "a task vertical slice. Registered calls and V2 parallel remain " + "integration-required; physical parallel acceptance is still open." + ), + "", + ( + "The checked-in environment classes have zero task-local motion or " + "demo-generation overrides; " + "`test_task_classes_do_not_override_motion_or_demo_generation` " + "keeps that structural metric at zero." + ), + "", + "## Migration Size Snapshot", + "", + ( + "The baseline is a fixed, manually recorded pre-migration snapshot: " + "Cube is 598 lines / 23912 bytes and Drawer is 245 lines / 8833 bytes. " + "The tool does not inspect Git history. Current values are recomputed " + "only from the four explicit files in the table." + ), + "", + ( + "Baseline identity: Cube uses Git blob " + f"`{_TASK_SIZE_SPECS[0].baseline_blob}` and Drawer uses Git blob " + f"`{_TASK_SIZE_SPECS[1].baseline_blob}` at each task's Python path " + "listed in the current-source column. Blob IDs remain stable across " + "stack rebases." + ), + "", + ( + "Counting rule: `lines` is the number of raw LF (`0x0A`) bytes; " + "`bytes` is the raw on-disk byte length. Counts are summed per task " + "without normalizing encoding or line endings." + ), + "", + ] + ) + lines.extend( + _render_table( + ( + "Task", + "Baseline lines", + "Current lines", + "Line delta", + "Baseline bytes", + "Current bytes", + "Byte delta", + "Current source files", + ), + metric_rows, + ) + ) + lines.extend( + [ + "", + "## Demo Success Measurement", + "", + ( + "`scripts/benchmark/expert_program/demo_success.py` executes each " + "fixed seed exactly once, always discards the episode buffer, and " + "counts executor exceptions as failed rows. It writes raw JSON plus " + "a three-table Markdown report. Its CLI supports offline raw-JSON " + "re-aggregation and an explicit `--run-simulation` mode that " + "constructs one standard Gym environment from Gym and Expert " + "Program configurations." + ), + "", + ( + "No multi-seed success-rate or release gate is checked in yet. Open " + "Drawer has a real-simulation smoke pass; repeated Cube has a " + "fixed-seed three-cycle pass plus physical-loss/re-acquisition gate; " + "and HandOver has three consecutive contact-dynamics runs." + ), + "", + "## Drift Check", + "", + ( + "Regenerate the checked-in report after an intentional source or " + "capability snapshot change:" + ), + "", + "```bash", + "python scripts/tools/expert_program_rollout_report.py", + "```", + "", + "CI and local validation can reject stale output without rewriting it:", + "", + "```bash", + "python scripts/tools/expert_program_rollout_report.py --check", + "```", + ] + ) + return "\n".join(lines) + "\n" + + +def _build_parser() -> argparse.ArgumentParser: + """Create the command-line parser.""" + parser = argparse.ArgumentParser( + description="Generate or check the declarative Expert Program rollout report." + ) + parser.add_argument( + "--check", + action="store_true", + help="Fail when the output file differs from the deterministic render.", + ) + parser.add_argument( + "--output", + type=Path, + default=DEFAULT_REPORT_PATH, + help="Markdown output path (defaults to the checked-in design report).", + ) + return parser + + +def main(argv: Sequence[str] | None = None) -> int: + """Generate the rollout report or check its checked-in representation. + + Args: + argv: Optional command-line argument sequence for tests and embedding. + + Returns: + Zero on success, or one when ``--check`` detects missing or stale output. + """ + args = _build_parser().parse_args(argv) + rendered = render_report(build_task_size_metrics()) + output = args.output + + if args.check: + try: + existing = output.read_text(encoding="utf-8") + except FileNotFoundError: + print(f"rollout report is missing: {output}") + return 1 + if existing != rendered: + print(f"rollout report is stale: {output}") + return 1 + print(f"rollout report is up to date: {output}") + return 0 + + output.parent.mkdir(parents=True, exist_ok=True) + output.write_text(rendered, encoding="utf-8") + print(f"wrote rollout report: {output}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scripts/tutorials/atomic_action/assemble.py b/scripts/tutorials/atomic_action/assemble.py index ac0594ef3..c7429e62a 100644 --- a/scripts/tutorials/atomic_action/assemble.py +++ b/scripts/tutorials/atomic_action/assemble.py @@ -14,7 +14,7 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Demonstrate object assembly with a dual-arm UR5. +"""Demonstrate object assembly with a selectable dual-arm robot. The left arm picks up a soda can (object A) and places it directly above a cube (object B). The relative pose of the can with respect to the cube is declared on @@ -38,8 +38,6 @@ from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, AssembleAffordance, AssembleGoal, AtomicActionEngine, @@ -55,16 +53,16 @@ from embodichain.lab.sim.shapes import CubeCfg, MeshCfg from embodichain.utils import logger from scripts.tutorials.atomic_action.scenario_utils import ( - add_dual_ur5_robot, + add_dual_tutorial_robot, add_support_surface, - make_dual_ur5_solver_cfg, settle_object, ) from scripts.tutorials.atomic_action.tutorial_utils import ( + TutorialRobot, broadcast_pose_batch, clone_local_pose_from_first_env, create_antipodal_semantics, - create_toppra_motion_generator, + create_curobo_motion_generator, create_tutorial_argument_parser, create_tutorial_simulation, draw_axis_marker, @@ -72,6 +70,7 @@ prepare_tutorial_scene, publish_tutorial_scene, replay_trajectory, + run_tutorial, serve_tutorial_scene, ) @@ -87,7 +86,7 @@ # --- Adjustable scene placeholders ----------------------------------------- # Object A (soda can) is staged for the left arm to pick up; object B (cube) is -# the assembly base the can is placed onto. Tweak these to match the dual-UR5 +# the assembly base the can is placed onto. Tweak these to match the dual-arm # reach and the soda-can mesh geometry. OBJECT_A_XY = (0.0, 0.02) OBJECT_B_XY = (0.0, 0.20) @@ -132,22 +131,23 @@ def parse_arguments() -> argparse.Namespace: "headless_play", "visualize_axes", ), - default_device="cpu", default_renderer="hybrid", ) return parser.parse_args() -def create_dual_ur5_robot(sim: SimulationManager) -> Robot: - """Create a dual-UR5 robot with one PGI gripper on each arm.""" - return add_dual_ur5_robot( +def create_dual_robot( + sim: SimulationManager, + robot_type: TutorialRobot, +) -> Robot: + """Create the selected dual-arm robot with one PGI gripper per arm.""" + return add_dual_tutorial_robot( sim, - uid="DualUR5Assemble", - urdf_name="dual_ur5_assemble", - solver_cfg=make_dual_ur5_solver_cfg( - GRIPPER_TCP_Z, - ur_ik_nearest_weight=(1.0, 4.0, 1.0, 1.0, 1.0, 1.0), - ), + robot_type=robot_type, + uid=f"Dual{robot_type.title()}Assemble", + urdf_name=f"dual_{robot_type}_assemble", + tcp_z=GRIPPER_TCP_Z, + ur_ik_nearest_weight=(1.0, 4.0, 1.0, 1.0, 1.0, 1.0), hand_stiffness=1e2, hand_damping=1e1, hand_max_effort=1e3, @@ -257,7 +257,7 @@ def run_assemble_demo( n_sample=args.n_sample, force_reannotate=args.force_reannotate, ) - motion_gen = create_toppra_motion_generator(robot) + motion_gen = create_curobo_motion_generator(robot) left_open, left_close = get_hand_open_close_qpos( robot, hand_control_part="left_hand", close_qpos=HAND_CLOSE_QPOS ) @@ -269,12 +269,12 @@ def run_assemble_demo( cube_pose = cube.get_local_pose(to_matrix=True) assemble_object_target_pose = cube_pose[0] @ assemble_to_base - n_envs = robot.get_qpos().shape[0] + num_envs = robot.get_qpos().shape[0] if not args.no_vis_eef_axis: draw_axis_marker( sim, "assemble_target_axis", - broadcast_pose_batch(assemble_object_target_pose, n_envs), + broadcast_pose_batch(assemble_object_target_pose, num_envs), ) # Step 1 - the left arm picks the soda can up by its top part. @@ -316,27 +316,31 @@ def run_assemble_demo( assemble_object_entity=can, assemble_to_base_pose=assemble_to_base, ) - binding = ActionBinding( - manipulators={"primary": "left_arm"}, - end_effectors={"primary": "left_hand"}, - ) + endpoint_mapping = {"primary": {"motion": "left_arm", "grasp": "left_hand"}} compiled = engine.compile( ( - ActionInvocation( + engine.make_invocation( "pick_up", GraspGoal(can_semantics), - binding, - MotionPolicy(sample_count=PICKUP_SAMPLE_INTERVAL), + control_parts=endpoint_mapping, + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=PICKUP_SAMPLE_INTERVAL, + ), skill_options=pick_up_options, ), - ActionInvocation( + engine.make_invocation( "place", AssembleGoal(affordance=assemble_affordance), - binding, - MotionPolicy(sample_count=PLACE_SAMPLE_INTERVAL), + control_parts=endpoint_mapping, + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=PLACE_SAMPLE_INTERVAL, + ), skill_options=place_options, ), - ) + ), + engine.initial_context(control_dt=sim.sim_config.physics_dt), ) success = compiled.plan_success traj = compiled.trajectory.positions @@ -374,13 +378,10 @@ def main() -> None: arena_space=3.0, light_pos=(0.0, -0.4, 3.0), ) - robot = create_dual_ur5_robot(sim) - try: - run_assemble_demo(args, sim, robot) - serve_tutorial_scene(sim, args) - finally: - sim.destroy() + robot = create_dual_robot(sim, args.robot) + run_assemble_demo(args, sim, robot) + serve_tutorial_scene(sim, args) if __name__ == "__main__": - main() + run_tutorial(main) diff --git a/scripts/tutorials/atomic_action/control_dt.py b/scripts/tutorials/atomic_action/control_dt.py new file mode 100644 index 000000000..617567cdc --- /dev/null +++ b/scripts/tutorials/atomic_action/control_dt.py @@ -0,0 +1,166 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Compare one interpolated action at two explicit control periods.""" + +from __future__ import annotations + +import argparse +import sys +from pathlib import Path + +_REPO_ROOT = Path(__file__).resolve().parents[3] +if str(_REPO_ROOT) not in sys.path: + sys.path.insert(0, str(_REPO_ROOT)) + +import torch + +from embodichain.lab.sim.atomic_actions import ( + AtomicActionEngine, + JointPositionGoal, + MotionPolicy, +) +from embodichain.utils import logger +from scripts.tutorials.atomic_action.tutorial_utils import ( + add_tutorial_robot, + create_toppra_motion_generator, + create_tutorial_argument_parser, + create_tutorial_simulation, + prepare_tutorial_scene, + replay_trajectory, + run_tutorial, +) + +SAMPLE_COUNT = 40 +FAST_CONTROL_STEPS = 2 +SLOW_CONTROL_STEPS = 8 +RESET_STEPS = 20 +POST_TRAJECTORY_STEPS = 40 + + +def parse_arguments() -> argparse.Namespace: + """Parse command-line arguments for the control-period tutorial.""" + parser = create_tutorial_argument_parser( + "Compare identical joint interpolation at two explicit control periods." + ) + return parser.parse_args() + + +def main() -> None: + """Replay the same geometric path with fast and slow waypoint timing.""" + args = parse_arguments() + sim = create_tutorial_simulation(args) + robot = add_tutorial_robot(sim, args.robot) + engine = AtomicActionEngine(motion_generator=create_toppra_motion_generator(robot)) + + initial_qpos = robot.get_qpos().clone() + start_arm_qpos = robot.get_qpos(name="arm")[0] + arm_limits = robot.get_qpos_limits(name="arm")[0] + offsets = torch.zeros_like(start_arm_qpos) + offsets[: min(4, offsets.numel())] = torch.tensor( + [0.30, 0.25, -0.20, -0.10][: min(4, offsets.numel())], + dtype=offsets.dtype, + device=offsets.device, + ) + target_arm_qpos = torch.minimum( + torch.maximum(start_arm_qpos + offsets, arm_limits[:, 0]), + arm_limits[:, 1], + ) + + invocation = engine.make_invocation( + "move_joints", + JointPositionGoal(target_arm_qpos), + control_parts={"primary": {"motion": "arm"}}, + motion_policy=MotionPolicy( + strategy="ik_interp", + sample_count=SAMPLE_COUNT, + ), + ) + physics_dt = float(sim.sim_config.physics_dt) + fast_control_dt = FAST_CONTROL_STEPS * physics_dt + slow_control_dt = SLOW_CONTROL_STEPS * physics_dt + fast = engine.compile( + (invocation,), + engine.initial_context(control_dt=fast_control_dt), + ) + slow = engine.compile( + (invocation,), + engine.initial_context(control_dt=slow_control_dt), + ) + if not fast.plan_success.all() or not slow.plan_success.all(): + logger.log_warning("Failed to compile one of the control-period plans.") + return + if not torch.allclose(fast.trajectory.positions, slow.trajectory.positions): + raise RuntimeError("control_dt unexpectedly changed the geometric path.") + + expected_ratio = slow_control_dt / fast_control_dt + actual_ratio = slow.trajectory.duration / fast.trajectory.duration + if not torch.allclose( + actual_ratio, + torch.full_like(actual_ratio, expected_ratio), + ): + raise RuntimeError("Trajectory duration does not scale with control_dt.") + + logger.log_info( + f"Both plans contain the same {fast.trajectory.waypoint_count} waypoints." + ) + logger.log_info( + f"Fast: control_dt={fast_control_dt:.3f}s, " + f"duration={fast.trajectory.duration.max().item():.3f}s." + ) + logger.log_info( + f"Slow: control_dt={slow_control_dt:.3f}s, " + f"duration={slow.trajectory.duration.max().item():.3f}s " + f"({expected_ratio:.1f}x slower)." + ) + + wait_for_user = prepare_tutorial_scene( + sim, + args, + "The two plans have identical positions. Press Enter to replay the fast one...", + ) + replay_trajectory( + sim, + robot, + fast.trajectory, + args, + video_prefix="control_dt_fast_auto_play", + hold_steps=POST_TRAJECTORY_STEPS, + ) + + robot.set_qpos(initial_qpos, target=False) + robot.set_qpos(initial_qpos, target=True) + zero_qvel = torch.zeros_like(robot.get_qvel()) + robot.set_qvel(zero_qvel, target=False) + robot.set_qvel(zero_qvel, target=True) + sim.update(step=RESET_STEPS) + + if wait_for_user: + input("Robot reset. Press Enter to replay the slow trajectory...") + replay_trajectory( + sim, + robot, + slow.trajectory, + args, + video_prefix="control_dt_slow_auto_play", + hold_steps=POST_TRAJECTORY_STEPS, + ) + if wait_for_user: + input("Press Enter to exit the simulation...") + + +if __name__ == "__main__": + run_tutorial(main) diff --git a/scripts/tutorials/atomic_action/coordinated_pickment.py b/scripts/tutorials/atomic_action/coordinated_pickment.py index b3eeb15b7..2b52be831 100644 --- a/scripts/tutorials/atomic_action/coordinated_pickment.py +++ b/scripts/tutorials/atomic_action/coordinated_pickment.py @@ -16,8 +16,8 @@ """Demonstrate dual-arm coordinated pickment with selectable object meshes. -The two UR5 arms pinch opposite sides of one object, lift it together, and move -the object to an object-centric target pose while both grippers stay closed. +The two selected arms pinch opposite sides of one object, lift it together, and +move the object to an object-centric target pose while both grippers stay closed. """ from __future__ import annotations @@ -35,9 +35,8 @@ import torch from embodichain.lab.sim import SimulationManager +from embodichain.data import get_data_path from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, CoordinatedPickGoal, @@ -53,17 +52,17 @@ from embodichain.utils import logger from embodichain.utils.math import matrix_from_euler from scripts.tutorials.atomic_action.scenario_utils import ( - add_dual_ur5_robot, + add_dual_tutorial_robot, add_support_surface, compute_world_bounds, get_local_vertices, log_action_plan, - make_dual_ur5_solver_cfg, resolve_cached_data_path, rotate_pose_about_world_z, settle_object, ) from scripts.tutorials.atomic_action.tutorial_utils import ( + TutorialRobot, broadcast_pose_batch, clone_local_pose_from_first_env, create_antipodal_semantics, @@ -80,8 +79,8 @@ PICKMENT_ASSET_ROOT = "CoordinatedPlacementAndPickment" GRIPPER_TCP_Z = 0.121 -SUPPORT_SURFACE_Z = 0.65 -SUPPORT_SURFACE_SIZE = (0.60, 0.60, 0.02) +SUPPORT_SURFACE_Z = 0.55 +SUPPORT_SURFACE_SIZE = (0.7, 1.20, 0.02) SUPPORT_SURFACE_CENTER = ( 0.0, 0.0, @@ -133,6 +132,28 @@ class PickmentObjectPreset: target_world_yaw_deg=0.0, hand_close_qpos=0.026, ), + "water_basin": PickmentObjectPreset( + label="water_basin", + mesh_path=get_data_path("WaterBasin/water_basin.glb"), + init_xy=(0.0, 0.02), + init_rot=(0.0, 0.0, 0.0), + surface_clearance=0.008, + body_scale=(1.0, 1.0, 1.0), + target_translation=(-0.12, -0.03, 0.12), + target_world_yaw_deg=0.0, + hand_close_qpos=0.026, + ), + "plastic_tray": PickmentObjectPreset( + label="plastic_tray", + mesh_path=get_data_path("PlasticTray/plastic_tray.glb"), + init_xy=(-0.02, 0.02), + init_rot=(0.0, 0.0, 90.0), + surface_clearance=0.008, + body_scale=(1.0, 1.0, 1.0), + target_translation=(-0.12, -0.03, 0.12), + target_world_yaw_deg=0.0, + hand_close_qpos=0.026, + ), } PICKMENT_SAMPLE_INTERVAL = 96 PICKMENT_OBJECT_MOTION_KEYFRAMES = 6 @@ -160,21 +181,24 @@ def parse_arguments() -> argparse.Namespace: parser.add_argument( "--object", choices=sorted(OBJECT_PRESETS), - default="pencil", + default="plastic_tray", help="Object mesh to grasp in the coordinated pickment demo.", ) return parser.parse_args() -def create_dual_ur5_robot(sim: SimulationManager) -> Robot: - """Create a dual-UR5 robot with one PGI gripper on each arm.""" - return add_dual_ur5_robot( +def create_dual_robot( + sim: SimulationManager, + robot_type: TutorialRobot, +) -> Robot: + """Create the selected dual-arm robot with one PGI gripper per arm.""" + return add_dual_tutorial_robot( sim, - uid="DualUR5CoordinatedPickment", - urdf_name="dual_ur5_coordinated_pickment", - arm_urdf_path=resolve_cached_data_path("UniversalRobots/UR5/UR5.urdf"), - gripper_urdf_path=resolve_cached_data_path("DH_PGI_140_80/DH_PGI_140_80.urdf"), - solver_cfg=make_dual_ur5_solver_cfg(GRIPPER_TCP_Z, solver="pytorch"), + robot_type=robot_type, + uid=f"Dual{robot_type.title()}CoordinatedPickment", + urdf_name=f"dual_{robot_type}_coordinated_pickment", + tcp_z=GRIPPER_TCP_Z, + solver="pytorch", ) @@ -255,12 +279,14 @@ def compute_left_to_right_arm_direction( Returns: A normalized ``(3,)`` direction vector. """ - left_base = robot.get_link_pose( - link_name="left_base_link", env_ids=[0], to_matrix=True - )[0, :3, 3] - right_base = robot.get_link_pose( - link_name="right_base_link", env_ids=[0], to_matrix=True - )[0, :3, 3] + left_root = robot.cfg.solver_cfg["left_arm"].root_link_name + right_root = robot.cfg.solver_cfg["right_arm"].root_link_name + left_base = robot.get_link_pose(link_name=left_root, env_ids=[0], to_matrix=True)[ + 0, :3, 3 + ] + right_base = robot.get_link_pose(link_name=right_root, env_ids=[0], to_matrix=True)[ + 0, :3, 3 + ] direction = (right_base - left_base).to(device=device, dtype=torch.float32) return direction / direction.norm().clamp_min(1e-6) @@ -349,12 +375,13 @@ def run_coordinated_pickment_demo( object_pose_batch = clone_local_pose_from_first_env(obj) obj.clear_dynamics() object_pose = object_pose_batch[0].to(device=sim.device, dtype=torch.float32) - n_envs = object_pose_batch.shape[0] + num_envs = object_pose_batch.shape[0] object_vertices = get_local_vertices(obj) object_semantics = create_antipodal_semantics( obj, label=preset.label, n_sample=args.n_sample, + # n_sample = 1000, force_reannotate=args.force_reannotate, ) left_to_right_arm_direction = compute_left_to_right_arm_direction(robot, sim.device) @@ -377,6 +404,7 @@ def run_coordinated_pickment_demo( hold_steps=PICKMENT_HOLD_STEPS, object_motion_keyframes=PICKMENT_OBJECT_MOTION_KEYFRAMES, left_to_right_arm_direction=left_to_right_arm_direction, + middle_empty_ratio=0.7, ) engine = AtomicActionEngine( motion_generator=motion_gen, @@ -399,12 +427,12 @@ def run_coordinated_pickment_demo( ) log_scene_targets(preset.label, object_pose, target_pose) if not args.no_vis_eef_axis: - draw_pickment_target_axes(sim, target_pose, num_envs=n_envs) + draw_pickment_target_axes(sim, target_pose, num_envs=num_envs) pickment_target = CoordinatedPickGoal( semantics=object_semantics, - object_target_pose=broadcast_pose_batch(target_pose, num_envs=n_envs), - object_initial_pose=broadcast_pose_batch(object_pose, num_envs=n_envs), + object_target_pose=broadcast_pose_batch(target_pose, num_envs=num_envs), + object_initial_pose=broadcast_pose_batch(object_pose, num_envs=num_envs), ) wait_for_user = prepare_tutorial_scene( @@ -414,17 +442,21 @@ def run_coordinated_pickment_demo( start_time = time.time() compiled = engine.compile( ( - ActionInvocation( + engine.make_invocation( "coordinated_pickment", pickment_target, - ActionBinding( - manipulators={"left": "left_arm", "right": "right_arm"}, - end_effectors={"left": "left_hand", "right": "right_hand"}, + control_parts={ + "left": {"motion": "left_arm", "grasp": "left_hand"}, + "right": {"motion": "right_arm", "grasp": "right_hand"}, + }, + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=PICKMENT_SAMPLE_INTERVAL, ), - MotionPolicy(sample_count=PICKMENT_SAMPLE_INTERVAL), skill_options=pickment_options, ), - ) + ), + engine.initial_context(control_dt=sim.sim_config.physics_dt), ) success = compiled.plan_success traj = compiled.trajectory.positions @@ -481,7 +513,7 @@ def main() -> None: arena_space=3.0, light_pos=(0.0, -0.4, 3.0), ) - robot = create_dual_ur5_robot(sim) + robot = create_dual_robot(sim, args.robot) run_coordinated_pickment_demo(args, sim, robot) diff --git a/scripts/tutorials/atomic_action/coordinated_placement.py b/scripts/tutorials/atomic_action/coordinated_placement.py index b715cefda..f24f8aba9 100644 --- a/scripts/tutorials/atomic_action/coordinated_placement.py +++ b/scripts/tutorials/atomic_action/coordinated_placement.py @@ -16,8 +16,8 @@ """Demonstrate dual-arm coordinated placement with bread and pan meshes. -The left UR5 picks up bread. The right UR5 picks up a pan and moves it to the -lower alignment pose. The left UR5 places the bread above the pan and releases +The left arm picks up bread. The right arm picks up a pan and moves it to the +lower alignment pose. The left arm places the bread above the pan and releases it while the right hand keeps holding the pan. """ @@ -38,8 +38,6 @@ from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, CoordinatedPlacementOptions, @@ -59,14 +57,13 @@ from embodichain.lab.sim.shapes import MeshCfg from embodichain.utils import logger from scripts.tutorials.atomic_action.scenario_utils import ( - add_dual_ur5_robot, + add_dual_tutorial_robot, compute_local_bounds, compute_world_bounds, create_manual_object_semantics, get_local_vertices, invert_pose, log_action_plan, - make_dual_ur5_solver_cfg, normalize_vector, resolve_cached_data_path, rotate_pose_about_world_z, @@ -74,6 +71,7 @@ transform_points, ) from scripts.tutorials.atomic_action.tutorial_utils import ( + TutorialRobot, broadcast_pose_batch, clone_local_pose_from_first_env, create_toppra_motion_generator, @@ -212,21 +210,18 @@ def parse_arguments() -> argparse.Namespace: return parser.parse_args() -def create_dual_ur5_robot(sim: SimulationManager) -> Robot: - """Create a dual-UR5 robot with one PGI gripper on each arm.""" - return add_dual_ur5_robot( +def create_dual_robot( + sim: SimulationManager, + robot_type: TutorialRobot, +) -> Robot: + """Create the selected dual-arm robot with one PGI gripper per arm.""" + return add_dual_tutorial_robot( sim, - uid="DualUR5CoordinatedPlacement", - urdf_name="dual_ur5_coordinated_placement", - arm_urdf_path=resolve_cached_data_path("UniversalRobots/UR5/UR5.urdf"), - gripper_urdf_path=resolve_cached_data_path("DH_PGI_140_80/DH_PGI_140_80.urdf"), - solver_cfg=make_dual_ur5_solver_cfg( - GRIPPER_TCP_Z, - clear_urdf_path=True, - ), + robot_type=robot_type, + uid=f"Dual{robot_type.title()}CoordinatedPlacement", + urdf_name=f"dual_{robot_type}_coordinated_placement", + tcp_z=GRIPPER_TCP_Z, init_pos=ROBOT_INIT_POS, - joint_name_case="lower", - set_urdf_name_case=False, ) @@ -548,7 +543,7 @@ def run_coordinated_placement_demo( pan.clear_dynamics() bread_pose = bread_pose_batch[0].to(device=sim.device, dtype=torch.float32) pan_pose = pan_pose_batch[0].to(device=sim.device, dtype=torch.float32) - n_envs = bread_pose_batch.shape[0] + num_envs = bread_pose_batch.shape[0] bread_vertices = get_local_vertices(bread) pan_vertices = get_local_vertices(pan) bread_local_min, bread_local_max = compute_local_bounds(bread_vertices) @@ -600,7 +595,7 @@ def run_coordinated_placement_demo( }, ) full_joint_ids = list(range(robot.dof)) - state = engine.initial_context() + state = engine.initial_context(control_dt=sim.sim_config.physics_dt) wait_for_user = prepare_tutorial_scene( sim, args, "Inspect the scene, then press Enter to compile both pick-ups..." @@ -621,30 +616,30 @@ def run_coordinated_placement_demo( z_clearance=PAN_GRASP_Z_CLEARANCE, ) pick_invocations = ( - ActionInvocation( - skill_id="pick_up", - goal=GraspGoal( + engine.make_invocation( + "pick_up", + GraspGoal( semantics=bread_semantics, - grasp_xpos=broadcast_pose_batch(bread_grasp_pose, num_envs=n_envs), + grasp_xpos=broadcast_pose_batch(bread_grasp_pose, num_envs=num_envs), ), - binding=ActionBinding( - manipulators={"primary": "left_arm"}, - end_effectors={"primary": "left_hand"}, + control_parts={"primary": {"motion": "left_arm", "grasp": "left_hand"}}, + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=PICK_SAMPLE_INTERVAL, ), - motion_policy=MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=left_pick_options, ), - ActionInvocation( - skill_id="pick_up", - goal=GraspGoal( + engine.make_invocation( + "pick_up", + GraspGoal( semantics=pan_semantics, - grasp_xpos=broadcast_pose_batch(pan_grasp_pose, num_envs=n_envs), + grasp_xpos=broadcast_pose_batch(pan_grasp_pose, num_envs=num_envs), ), - binding=ActionBinding( - manipulators={"primary": "right_arm"}, - end_effectors={"primary": "right_hand"}, + control_parts={"primary": {"motion": "right_arm", "grasp": "right_hand"}}, + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=PAN_PICK_SAMPLE_INTERVAL, ), - motion_policy=MotionPolicy(sample_count=PAN_PICK_SAMPLE_INTERVAL), skill_options=right_pick_options, ), ) @@ -663,8 +658,12 @@ def run_coordinated_placement_demo( if not pick_compiled.plan_success.all(): logger.log_warning("Failed to plan right pan pick-up trajectory.") return - left_pick_traj = left_pick_result.trajectory.positions - right_pick_traj = right_pick_result.trajectory.positions + left_pick_trajectory = left_pick_result.joint_trajectory + right_pick_trajectory = right_pick_result.joint_trajectory + if left_pick_trajectory is None or right_pick_trajectory is None: + raise RuntimeError("PickUp did not produce joint trajectories.") + left_pick_traj = left_pick_trajectory.positions + right_pick_traj = right_pick_trajectory.positions state = pick_compiled.projected_context bread_held_state = state.get_held_object("left_arm") if bread_held_state is None: @@ -690,7 +689,7 @@ def log_trajectory_execution(step_idx: int, total_steps: int) -> None: replay_trajectory( sim, robot, - left_pick_result.trajectory, + left_pick_trajectory, args, video_prefix="", hold_steps=0, @@ -703,7 +702,7 @@ def log_trajectory_execution(step_idx: int, total_steps: int) -> None: replay_trajectory( sim, robot, - right_pick_result.trajectory, + right_pick_trajectory, args, video_prefix="", hold_steps=0, @@ -748,7 +747,6 @@ def log_trajectory_execution(step_idx: int, total_steps: int) -> None: batch_size=state.batch_size, device=state.robot.qpos.device, held_objects=held_objects, - coordinated_held_objects=state.task.coordinated_held_objects, ), ) @@ -771,14 +769,14 @@ def log_trajectory_execution(step_idx: int, total_steps: int) -> None: sim, support_target_pose, placing_target_pose, - num_envs=n_envs, + num_envs=num_envs, ) coordinated_target = CoordinatedPlacementGoal( placing_object_target_pose=broadcast_pose_batch( - placing_target_pose, num_envs=n_envs + placing_target_pose, num_envs=num_envs ), support_object_target_pose=broadcast_pose_batch( - support_target_pose, num_envs=n_envs + support_target_pose, num_envs=num_envs ), placing_height_offset=BREAD_TARGET_HEIGHT_OFFSET, support_height_offset=SUPPORT_TARGET_HEIGHT_OFFSET, @@ -787,20 +785,17 @@ def log_trajectory_execution(step_idx: int, total_steps: int) -> None: start_time = time.time() placement_compiled = engine.compile( ( - ActionInvocation( - skill_id="coordinated_placement", - goal=coordinated_target, - binding=ActionBinding( - manipulators={ - "placing": "left_arm", - "support": "right_arm", - }, - end_effectors={ - "placing": "left_hand", - "support": "right_hand", - }, + engine.make_invocation( + "coordinated_placement", + coordinated_target, + control_parts={ + "placing": {"motion": "left_arm", "grasp": "left_hand"}, + "support": {"motion": "right_arm", "grasp": "right_hand"}, + }, + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=COORDINATED_SAMPLE_INTERVAL, ), - motion_policy=MotionPolicy(sample_count=COORDINATED_SAMPLE_INTERVAL), skill_options=coordinated_options, ), ), @@ -835,7 +830,7 @@ def log_trajectory_execution(step_idx: int, total_steps: int) -> None: sim, support_target_pose, placing_target_pose, - num_envs=n_envs, + num_envs=num_envs, ) if wait_for_user: input("Press Enter to execute coordinated placement...") @@ -868,7 +863,7 @@ def main() -> None: arena_space=3.0, light_pos=(0.0, -0.4, 3.0), ) - robot = create_dual_ur5_robot(sim) + robot = create_dual_robot(sim, args.robot) run_coordinated_placement_demo(args, sim, robot) diff --git a/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py b/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py index fc679530d..dd8beedcb 100644 --- a/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py +++ b/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py @@ -30,16 +30,15 @@ import torch -from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser from embodichain.lab.sim import SimulationManager, VisualMaterialCfg from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, ExecutionEventKind, ExecutionRunner, ExecutionRunnerCfg, + JointPositionPayload, + JointPositionTarget, MotionPolicy, RecoveryPolicy, RigidObjectSceneProvider, @@ -47,6 +46,7 @@ RunnerStep, SimulationExecutionAdapter, TaskState, + TimedCommandSequence, ) from embodichain.lab.sim.cfg import RigidBodyAttributesCfg from embodichain.lab.sim.objects import RigidObject, RigidObjectCfg, Robot @@ -56,11 +56,12 @@ CuroboPlannerCfg, CuroboWorldCfg, ) -from embodichain.lab.sim.robots import FrankaPandaCfg from embodichain.lab.sim.shapes import CubeCfg from embodichain.lab.visualization import SceneOverlays, TrajectoryOverlay from embodichain.utils import logger from scripts.tutorials.atomic_action.tutorial_utils import ( + add_tutorial_robot, + create_tutorial_argument_parser, create_tutorial_simulation, draw_axis_marker, prepare_tutorial_scene, @@ -71,7 +72,6 @@ stop_auto_play_recording, ) -ROBOT_UID = "dynamic_scene_franka" OBSTACLE_UID = "dynamic_obstacle" CONTROL_PART = "arm" SAMPLE_COUNT = 80 @@ -294,21 +294,35 @@ def _minimum_cuboid_clearance( return (outside_distance + inside_distance).amin(dim=1) -def _trajectory_eef_positions( +def _command_eef_positions( robot: Robot, - trajectory_positions: torch.Tensor, + commands: TimedCommandSequence, *, control_part: str, ) -> torch.Tensor: - """Convert a full-robot joint trajectory to batched EEF positions.""" - if trajectory_positions.dim() != 3: - raise ValueError("trajectory_positions must have shape (B, N, robot_dof).") - joint_ids = robot.get_joint_ids(name=control_part) - arm_trajectory = trajectory_positions[:, :, joint_ids] + """Convert one endpoint command sequence to batched EEF positions.""" + if not commands.frames: + raise ValueError("commands must contain at least one frame.") positions = [] - for waypoint_index in range(arm_trajectory.shape[1]): + for frame in commands.frames: + matching_commands = tuple( + command + for command in frame.commands + if isinstance(command.target, JointPositionTarget) + and command.target.control_part == control_part + ) + if len(matching_commands) != 1: + raise ValueError( + f"Expected one joint command for control part {control_part!r}, " + f"got {len(matching_commands)}." + ) + payload = matching_commands[0].payload + if not isinstance(payload, JointPositionPayload): + raise TypeError( + f"Control part {control_part!r} did not receive joint positions." + ) pose = robot.compute_fk( - qpos=arm_trajectory[:, waypoint_index], + qpos=payload.positions, name=control_part, to_matrix=True, ) @@ -373,11 +387,9 @@ def _publish_path_overlays( def parse_arguments() -> argparse.Namespace: """Parse command-line arguments for the dynamic-obstacle tutorial.""" - parser = argparse.ArgumentParser( - description="Demonstrate collision-world revision recovery with cuRobo." + parser = create_tutorial_argument_parser( + "Demonstrate collision-world revision recovery with cuRobo." ) - add_env_launcher_args_to_parser(parser) - parser.add_argument("--auto_play", action="store_true") parser.add_argument( "--no_obstacle_motion", action="store_true", @@ -390,9 +402,7 @@ def main() -> None: """Move an obstacle during execution and replan from the latest snapshot.""" args = parse_arguments() sim = create_tutorial_simulation(args) - robot = sim.add_robot( - cfg=FrankaPandaCfg.from_dict({"uid": ROBOT_UID, "robot_type": "panda"}) - ) + robot = add_tutorial_robot(sim, args.robot) obstacle = sim.add_rigid_object( cfg=RigidObjectCfg( uid=OBSTACLE_UID, @@ -417,7 +427,7 @@ def main() -> None: motion_gen = MotionGenerator( MotionGenCfg( planner_cfg=CuroboPlannerCfg( - robot_uid=ROBOT_UID, + robot_uid=robot.uid, # The coarse default voxel fit under-covers the hand and # fingertips. A denser fit plus modest padding matches the # physical gripper without making the arm path infeasible. @@ -442,6 +452,7 @@ def main() -> None: adapter = SimulationExecutionAdapter( sim, robot, + control_dt=COMMAND_CYCLE_TIME, scene_provider=scene_provider, ) @@ -457,14 +468,13 @@ def main() -> None: device=target_pose.device, ) engine = AtomicActionEngine(motion_generator=motion_gen) - invocation = ActionInvocation( - skill_id="move_end_effector", - goal=EndEffectorPoseGoal(target_pose), - binding=ActionBinding(manipulators={"primary": CONTROL_PART}), + invocation = engine.make_invocation( + "move_end_effector", + EndEffectorPoseGoal(target_pose), + control_parts={"primary": {"motion": CONTROL_PART}}, motion_policy=MotionPolicy( strategy="motion_gen", sample_count=SAMPLE_COUNT, - control_dt=COMMAND_CYCLE_TIME, ), recovery_policy=RecoveryPolicy( max_replans=2, @@ -475,9 +485,9 @@ def main() -> None: ) task_state = TaskState.empty(robot.get_qpos().shape[0], robot.device) session = engine.start((invocation,), adapter.observe(task_state)) - initial_eef_path = _trajectory_eef_positions( + initial_eef_path = _command_eef_positions( robot, - session.active_trajectory.positions, + session.active_commands, control_part=CONTROL_PART, ) blocking_obstacle_pose, blocking_waypoint_index = _blocking_obstacle_pose( @@ -496,7 +506,7 @@ def main() -> None: adapter, clock=adapter, # cuRobo can supply a trajectory duration, which takes precedence over - # MotionPolicy.control_dt. Keep a runner-side floor so the simulated + # engine fallback timing. Keep a runner-side floor so the simulated # controller receives enough feedback cycles to follow every waypoint. cfg=ExecutionRunnerCfg(minimum_cycle_time=COMMAND_CYCLE_TIME), ) @@ -586,9 +596,9 @@ def on_step(step: RunnerStep) -> None: and replanned_eef_path is None and ExecutionEventKind.COLLISION_WORLD_CHANGED in observed_events ): - replanned_eef_path = _trajectory_eef_positions( + replanned_eef_path = _command_eef_positions( robot, - session.active_trajectory.positions, + session.active_commands, control_part=CONTROL_PART, ) replan_detour = _maximum_path_deviation( diff --git a/scripts/tutorials/atomic_action/hand_over.py b/scripts/tutorials/atomic_action/hand_over.py index bec85110e..446596c86 100644 --- a/scripts/tutorials/atomic_action/hand_over.py +++ b/scripts/tutorials/atomic_action/hand_over.py @@ -35,8 +35,6 @@ from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, GraspGoal, AtomicActionEngine, ControlPartCommandProfile, @@ -50,12 +48,12 @@ from embodichain.lab.sim.shapes import MeshCfg from embodichain.utils import logger from scripts.tutorials.atomic_action.scenario_utils import ( - add_dual_ur5_robot, + add_dual_tutorial_robot, add_support_surface, - make_dual_ur5_solver_cfg, settle_object, ) from scripts.tutorials.atomic_action.tutorial_utils import ( + TutorialRobot, create_antipodal_semantics, create_toppra_motion_generator, create_tutorial_argument_parser, @@ -65,6 +63,7 @@ prepare_tutorial_scene, publish_tutorial_scene, replay_trajectory, + run_tutorial, serve_tutorial_scene, ) @@ -81,7 +80,7 @@ # --- Adjustable scene placeholders ----------------------------------------- # The object starts on the left side, is handed over at a lifted middle pose, # and is delivered to the right side. Tweak these to match the mesh geometry -# and the dual-UR5 reach. +# and the selected dual-arm robot's reach. OBJECT_INIT_XY = (0.0, 0.02) MIDDLE_OBJECT_XYZ = (0.0, 0.02, 0.82) MIDDLE_OBJECT_YAW_DEG = 0.0 @@ -119,16 +118,18 @@ def parse_arguments() -> argparse.Namespace: return parser.parse_args() -def create_dual_ur5_robot(sim: SimulationManager) -> Robot: - """Create a dual-UR5 robot with one PGI gripper on each arm.""" - return add_dual_ur5_robot( +def create_dual_robot( + sim: SimulationManager, + robot_type: TutorialRobot, +) -> Robot: + """Create the selected dual-arm robot with one PGI gripper per arm.""" + return add_dual_tutorial_robot( sim, - uid="DualUR5HandOver", - urdf_name="dual_ur5_hand_over", - solver_cfg=make_dual_ur5_solver_cfg( - GRIPPER_TCP_Z, - ur_ik_nearest_weight=(1.0, 4.0, 1.0, 1.0, 1.0, 1.0), - ), + robot_type=robot_type, + uid=f"Dual{robot_type.title()}HandOver", + urdf_name=f"dual_{robot_type}_hand_over", + tcp_z=GRIPPER_TCP_Z, + ur_ik_nearest_weight=(1.0, 4.0, 1.0, 1.0, 1.0, 1.0), hand_stiffness=1e2, hand_damping=1e1, hand_max_effort=1e3, @@ -221,9 +222,6 @@ def run_handover_demo( pre_grasp_distance=PICKUP_PRE_GRASP_DISTANCE, lift_height=PICKUP_LIFT_HEIGHT, hand_interp_steps=PICKUP_HAND_INTERP_STEPS, - approach_direction=torch.as_tensor( - [0.0, -707106781, -707106781], dtype=torch.float32 - ), ) # Step 2 - hand the object from the left arm to the right arm. handover_options = HandOverOptions( @@ -260,33 +258,34 @@ def run_handover_demo( sim.update(step=10) compiled = engine.compile( ( - ActionInvocation( + engine.make_invocation( "pick_up", GraspGoal(object_semantics), - ActionBinding( - manipulators={"primary": "left_arm"}, - end_effectors={"primary": "left_hand"}, + control_parts={"primary": {"motion": "left_arm", "grasp": "left_hand"}}, + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=PICKUP_SAMPLE_INTERVAL, ), - MotionPolicy(sample_count=PICKUP_SAMPLE_INTERVAL), skill_options=pick_up_options, ), - ActionInvocation( + engine.make_invocation( "hand_over", GraspGoal(object_semantics), - ActionBinding( - manipulators={ - "source": "left_arm", - "destination": "right_arm", - }, - end_effectors={ - "source": "left_hand", - "destination": "right_hand", + control_parts={ + "source": {"motion": "left_arm", "grasp": "left_hand"}, + "destination": { + "motion": "right_arm", + "grasp": "right_hand", }, + }, + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=HANDOVER_SAMPLE_INTERVAL, ), - MotionPolicy(sample_count=HANDOVER_SAMPLE_INTERVAL), skill_options=handover_options, ), - ) + ), + engine.initial_context(control_dt=sim.sim_config.physics_dt), ) success = compiled.plan_success traj = compiled.trajectory.positions @@ -324,13 +323,10 @@ def main() -> None: arena_space=3.0, light_pos=(0.0, -0.4, 3.0), ) - robot = create_dual_ur5_robot(sim) - try: - run_handover_demo(args, sim, robot) - serve_tutorial_scene(sim, args) - finally: - sim.destroy() + robot = create_dual_robot(sim, args.robot) + run_handover_demo(args, sim, robot) + serve_tutorial_scene(sim, args) if __name__ == "__main__": - main() + run_tutorial(main) diff --git a/scripts/tutorials/atomic_action/move_end_effector.py b/scripts/tutorials/atomic_action/move_end_effector.py index f46dbe250..9993916e1 100644 --- a/scripts/tutorials/atomic_action/move_end_effector.py +++ b/scripts/tutorials/atomic_action/move_end_effector.py @@ -29,22 +29,19 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, MotionPolicy, ) from embodichain.utils import logger from scripts.tutorials.atomic_action.tutorial_utils import ( - add_ur5_gripper_robot, + add_tutorial_robot, broadcast_pose_batch, broadcast_waypoint_pose_batch, - create_toppra_motion_generator, + create_curobo_motion_generator, create_tutorial_argument_parser, create_tutorial_simulation, draw_axis_marker, - make_top_down_eef_pose, prepare_tutorial_scene, replay_trajectory, run_tutorial, @@ -67,26 +64,27 @@ def main() -> None: """Move the robot end effector through two pose waypoints.""" args = parse_arguments() sim = create_tutorial_simulation(args) - robot = add_ur5_gripper_robot(sim) - motion_gen = create_toppra_motion_generator(robot) + robot = add_tutorial_robot(sim, args.robot) + motion_gen = create_curobo_motion_generator(robot) engine = AtomicActionEngine(motion_generator=motion_gen) - poses = torch.stack( - [ - make_top_down_eef_pose( - torch.tensor([0.30, -0.20, 0.36], device=sim.device) - ), - make_top_down_eef_pose(torch.tensor([0.45, 0.10, 0.30], device=sim.device)), - ] + start_pose = robot.compute_fk( + robot.get_qpos(name="arm"), name="arm", to_matrix=True + )[0] + poses = start_pose.unsqueeze(0).repeat(2, 1, 1) + poses[:, :3, 3] += torch.tensor( + [[-0.08, -0.08, 0.08], [0.04, 0.12, 0.04]], + dtype=poses.dtype, + device=poses.device, ) - n_envs = robot.get_qpos().shape[0] + num_envs = robot.get_qpos().shape[0] if not args.no_vis_eef_axis: for name, pose in zip(("target", "side"), poses, strict=True): draw_axis_marker( sim, f"move_end_effector_{name}_axis", - broadcast_pose_batch(pose, num_envs=n_envs), + broadcast_pose_batch(pose, num_envs=num_envs), ) wait_for_user = prepare_tutorial_scene( sim, args, "Inspect the robot, then press Enter to plan MoveEndEffector..." @@ -94,13 +92,17 @@ def main() -> None: compiled = engine.compile( ( - ActionInvocation( - skill_id="move_end_effector", - goal=EndEffectorPoseGoal(broadcast_waypoint_pose_batch(poses, n_envs)), - binding=ActionBinding(manipulators={"primary": "arm"}), - motion_policy=MotionPolicy(sample_count=MOVE_SAMPLE_INTERVAL), + engine.make_invocation( + "move_end_effector", + EndEffectorPoseGoal(broadcast_waypoint_pose_batch(poses, num_envs)), + control_parts={"primary": {"motion": "arm"}}, + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=MOVE_SAMPLE_INTERVAL, + ), ), - ) + ), + engine.initial_context(control_dt=sim.sim_config.physics_dt), ) if not compiled.plan_success.all(): logger.log_warning("Failed to plan MoveEndEffector demo trajectory.") diff --git a/scripts/tutorials/atomic_action/move_held_object.py b/scripts/tutorials/atomic_action/move_held_object.py index f320b6118..797e157c4 100644 --- a/scripts/tutorials/atomic_action/move_held_object.py +++ b/scripts/tutorials/atomic_action/move_held_object.py @@ -30,8 +30,6 @@ from embodichain.data import get_data_path from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, EndEffectorPoseGoal, @@ -45,11 +43,11 @@ from embodichain.lab.sim.shapes import MeshCfg from embodichain.utils import logger from scripts.tutorials.atomic_action.tutorial_utils import ( - add_ur5_gripper_robot, + add_tutorial_robot, broadcast_pose_batch, clone_local_pose_from_first_env, create_antipodal_semantics, - create_toppra_motion_generator, + create_curobo_motion_generator, create_tutorial_argument_parser, create_tutorial_simulation, draw_axis_marker, @@ -117,9 +115,9 @@ def main() -> None: """Plan MoveEndEffector -> PickUp -> MoveHeldObject.""" args = parse_arguments() sim = create_tutorial_simulation(args) - robot = add_ur5_gripper_robot(sim) + robot = add_tutorial_robot(sim, args.robot) obj = create_pick_object(sim) - motion_gen = create_toppra_motion_generator(robot) + motion_gen = create_curobo_motion_generator(robot) hand_open, hand_close = get_hand_open_close_qpos(robot) engine = AtomicActionEngine( @@ -139,45 +137,53 @@ def main() -> None: ) move_position = obj.get_local_pose(to_matrix=True)[0, :3, 3].clone() move_position[2] = 0.36 - n_envs = robot.get_qpos().shape[0] - move_target = broadcast_pose_batch(make_eef_pose_at(robot, move_position), n_envs) - object_target = broadcast_pose_batch(make_object_target_pose(sim.device), n_envs) + num_envs = robot.get_qpos().shape[0] + move_target = broadcast_pose_batch(make_eef_pose_at(robot, move_position), num_envs) + object_target = broadcast_pose_batch(make_object_target_pose(sim.device), num_envs) if not args.no_vis_eef_axis: draw_axis_marker(sim, "move_held_object_target_axis", object_target) wait_for_user = prepare_tutorial_scene( sim, args, "Inspect the paper cup, then press Enter to plan..." ) - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ) + motion_mapping = {"primary": {"motion": "arm"}} + manipulation_mapping = {"primary": {"motion": "arm", "grasp": "hand"}} compiled = engine.compile( ( - ActionInvocation( + engine.make_invocation( "move_end_effector", EndEffectorPoseGoal(move_target), - binding, - MotionPolicy(sample_count=MOVE_SAMPLE_INTERVAL), + control_parts=motion_mapping, + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=MOVE_SAMPLE_INTERVAL, + ), ), - ActionInvocation( + engine.make_invocation( "pick_up", GraspGoal(semantics), - binding, - MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), + control_parts=manipulation_mapping, + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=PICK_SAMPLE_INTERVAL, + ), skill_options=PickUpOptions( pre_grasp_distance=0.15, lift_height=0.16, hand_interp_steps=HAND_INTERP_STEPS, ), ), - ActionInvocation( + engine.make_invocation( "move_held_object", HeldObjectPoseGoal(object_target), - binding, - MotionPolicy(sample_count=MOVE_HELD_OBJECT_SAMPLE_INTERVAL), + control_parts=manipulation_mapping, + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=MOVE_HELD_OBJECT_SAMPLE_INTERVAL, + ), ), - ) + ), + engine.initial_context(control_dt=sim.sim_config.physics_dt), ) if not compiled.plan_success.all(): logger.log_warning("Failed to plan MoveHeldObject demo trajectory.") diff --git a/scripts/tutorials/atomic_action/move_joints.py b/scripts/tutorials/atomic_action/move_joints.py index 7fbee6948..0a35a5b9f 100644 --- a/scripts/tutorials/atomic_action/move_joints.py +++ b/scripts/tutorials/atomic_action/move_joints.py @@ -29,8 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, JointPositionGoal, @@ -38,8 +36,8 @@ ) from embodichain.utils import logger from scripts.tutorials.atomic_action.tutorial_utils import ( - add_ur5_gripper_robot, - create_toppra_motion_generator, + add_tutorial_robot, + create_curobo_motion_generator, create_tutorial_argument_parser, create_tutorial_simulation, draw_axis_marker, @@ -65,17 +63,22 @@ def main() -> None: """Move the robot arm through a named target and two explicit waypoints.""" args = parse_arguments() sim = create_tutorial_simulation(args) - robot = add_ur5_gripper_robot(sim) - motion_gen = create_toppra_motion_generator(robot) - - ready, mid, home = ( - torch.tensor(qpos, dtype=torch.float32, device=sim.device) - for qpos in ( - [0.35, -1.20, 1.30, -1.65, -1.57, 0.20], - [0.15, -1.40, 1.45, -1.60, -1.57, 0.10], - [0.0, -1.57, 1.57, -1.57, -1.57, 0.0], + robot = add_tutorial_robot(sim, args.robot) + motion_gen = create_curobo_motion_generator(robot) + + home = robot.get_qpos(name="arm")[0].clone() + limits = robot.get_qpos_limits(name="arm")[0] + + def offset_from_home(offsets: tuple[float, ...]) -> torch.Tensor: + target = home.clone() + count = min(target.numel(), len(offsets)) + target[:count] += torch.tensor( + offsets[:count], dtype=target.dtype, device=target.device ) - ) + return torch.minimum(torch.maximum(target, limits[:, 0]), limits[:, 1]) + + ready = offset_from_home((0.35, 0.37, -0.27, -0.08, 0.0, 0.20)) + mid = offset_from_home((0.15, 0.17, -0.12, -0.03, 0.0, 0.10)) engine = AtomicActionEngine( motion_generator=motion_gen, control_profiles={ @@ -95,17 +98,27 @@ def main() -> None: waypoints = ( torch.stack([mid, home]).unsqueeze(0).repeat(robot.get_qpos().shape[0], 1, 1) ) - binding = ActionBinding(manipulators={"primary": "arm"}) - policy = MotionPolicy(sample_count=MOVE_JOINTS_SAMPLE_INTERVAL) + control_parts = {"primary": {"motion": "arm"}} + policy = MotionPolicy( + strategy="motion_gen", + sample_count=MOVE_JOINTS_SAMPLE_INTERVAL, + ) compiled = engine.compile( ( - ActionInvocation( - "move_joints", JointPositionGoal("ready"), binding, policy + engine.make_invocation( + "move_joints", + JointPositionGoal("ready"), + control_parts=control_parts, + motion_policy=policy, ), - ActionInvocation( - "move_joints", JointPositionGoal(waypoints), binding, policy + engine.make_invocation( + "move_joints", + JointPositionGoal(waypoints), + control_parts=control_parts, + motion_policy=policy, ), - ) + ), + engine.initial_context(control_dt=sim.sim_config.physics_dt), ) if not compiled.plan_success.all(): logger.log_warning("Failed to plan MoveJoints demo trajectory.") diff --git a/scripts/tutorials/atomic_action/moving_target_recovery.py b/scripts/tutorials/atomic_action/moving_target_recovery.py index a9053453d..a8f23a774 100644 --- a/scripts/tutorials/atomic_action/moving_target_recovery.py +++ b/scripts/tutorials/atomic_action/moving_target_recovery.py @@ -31,16 +31,15 @@ from embodichain.lab.sim import SimulationManager, VisualMaterialCfg from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, Affordance, AtomicActionEngine, ControlPartCommandProfile, EntityState, + EffectVerificationRequest, + EffectVerificationResult, ExecutionEventKind, ExecutionRunner, ExecutionRunnerCfg, - ExecutionTick, GraspGoal, MotionPolicy, ObjectSemantics, @@ -59,8 +58,8 @@ from embodichain.lab.sim.shapes import CubeCfg from embodichain.utils import logger from scripts.tutorials.atomic_action.tutorial_utils import ( - add_ur5_gripper_robot, - create_toppra_motion_generator, + add_tutorial_robot, + create_curobo_motion_generator, create_tutorial_argument_parser, create_tutorial_simulation, draw_axis_marker, @@ -243,16 +242,17 @@ def main() -> None: """Replan a late-bound PickUp request and lift the relocated cube.""" args = parse_arguments() sim = create_tutorial_simulation(args) - robot = add_ur5_gripper_robot(sim) + robot = add_tutorial_robot(sim, args.robot) target = _create_moving_target(sim) sim.update(step=10) target_scene = _MovingTargetScene(target, MOVED_TARGET_POSITION) sim_runtime = SimulationExecutionAdapter( sim, robot, + control_dt=2.0 * sim.sim_config.physics_dt, scene_supplier=target_scene.snapshot, ) - motion_gen = create_toppra_motion_generator(robot) + motion_gen = create_curobo_motion_generator(robot) hand_open, hand_close = get_hand_open_close_qpos(robot) initialize_pre_pick_robot_pose(robot, target, hand_open) if args.no_target_motion: @@ -275,10 +275,7 @@ def main() -> None: geometry={}, label="cube", entity=target, - ) - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, + entity_id=TARGET_ENTITY_ID, ) engine = AtomicActionEngine( motion_generator=motion_gen, @@ -289,19 +286,19 @@ def main() -> None: ) }, ) - pick_invocation = ActionInvocation( - skill_id="pick_up", - goal=GraspGoal( + pick_invocation = engine.make_invocation( + "pick_up", + GraspGoal( semantics, grasp_xpos=SceneEntityPose( TARGET_ENTITY_ID, relative_pose=target_to_grasp, ), ), - binding=binding, + control_parts={"primary": {"motion": "arm", "grasp": "hand"}}, motion_policy=MotionPolicy( + strategy="motion_gen", sample_count=PICK_SAMPLE_COUNT, - control_dt=2.0 * sim_runtime.physics_dt, ), recovery_policy=RecoveryPolicy( max_replans=2, @@ -406,8 +403,8 @@ def on_step(step: RunnerStep) -> None: def verify_pickup_effect( _context: PlanningContext, - _: ExecutionTick, - ) -> torch.Tensor: + request: EffectVerificationRequest, + ) -> EffectVerificationResult: """Verify that the cube rose with, and remains near, the end effector.""" cube_position = target.get_local_pose(to_matrix=True)[:, :3, 3] eef_position = robot.compute_fk( @@ -426,7 +423,14 @@ def verify_pickup_effect( f"cube-to-EEF={held_distance.detach().cpu().tolist()} m, " f"success={success.detach().cpu().tolist()}." ) - return success + verified_success = request.env_mask & success + return EffectVerificationResult( + verification_id=request.verification_id, + success_mask=verified_success, + failure_mask=request.env_mask & ~success, + invalidation_mask=request.env_mask & ~success, + retry_mask=request.env_mask & ~success, + ) recording_started = start_auto_play_recording( sim, diff --git a/scripts/tutorials/atomic_action/pickup.py b/scripts/tutorials/atomic_action/pickup.py index a9c361671..b5450f6bc 100644 --- a/scripts/tutorials/atomic_action/pickup.py +++ b/scripts/tutorials/atomic_action/pickup.py @@ -29,8 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, GraspGoal, @@ -42,10 +40,10 @@ from embodichain.lab.sim.shapes import CubeCfg from embodichain.utils import logger from scripts.tutorials.atomic_action.tutorial_utils import ( - add_ur5_gripper_robot, + add_tutorial_robot, clone_local_pose_from_first_env, create_antipodal_semantics, - create_toppra_motion_generator, + create_curobo_motion_generator, create_tutorial_argument_parser, create_tutorial_simulation, draw_axis_marker, @@ -126,11 +124,11 @@ def main() -> None: """Plan and replay a sampled antipodal PickUp trajectory.""" args = parse_arguments() sim = create_tutorial_simulation(args) - robot = add_ur5_gripper_robot(sim) + robot = add_tutorial_robot(sim, args.robot) obj = create_pick_object(sim) hand_open, hand_close = get_hand_open_close_qpos(robot) initialize_pre_pick_robot_pose(robot, obj, hand_open) - motion_gen = create_toppra_motion_generator(robot) + motion_gen = create_curobo_motion_generator(robot) engine = AtomicActionEngine( motion_generator=motion_gen, @@ -155,14 +153,14 @@ def main() -> None: compiled = engine.compile( ( - ActionInvocation( - skill_id="pick_up", - goal=GraspGoal(semantics), - binding=ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, + engine.make_invocation( + "pick_up", + GraspGoal(semantics), + control_parts={"primary": {"motion": "arm", "grasp": "hand"}}, + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=PICK_SAMPLE_INTERVAL, ), - motion_policy=MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( approach_direction=resolve_approach_direction(args, sim.device), pre_grasp_distance=0.15, @@ -170,7 +168,8 @@ def main() -> None: hand_interp_steps=HAND_INTERP_STEPS, ), ), - ) + ), + engine.initial_context(control_dt=sim.sim_config.physics_dt), ) if not compiled.plan_success.all(): logger.log_warning("Failed to plan PickUp demo trajectory.") diff --git a/scripts/tutorials/atomic_action/place.py b/scripts/tutorials/atomic_action/place.py index ae30526ad..17a1bad8d 100644 --- a/scripts/tutorials/atomic_action/place.py +++ b/scripts/tutorials/atomic_action/place.py @@ -29,8 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, GraspGoal, @@ -44,12 +42,12 @@ from embodichain.lab.sim.shapes import CubeCfg from embodichain.utils import logger from scripts.tutorials.atomic_action.tutorial_utils import ( - add_ur5_gripper_robot, + add_tutorial_robot, broadcast_pose_batch, broadcast_waypoint_pose_batch, clone_local_pose_from_first_env, create_antipodal_semantics, - create_toppra_motion_generator, + create_curobo_motion_generator, create_tutorial_argument_parser, create_tutorial_simulation, draw_axis_marker, @@ -124,9 +122,9 @@ def main() -> None: """Plan and replay PickUp followed by a multi-waypoint Place.""" args = parse_arguments() sim = create_tutorial_simulation(args) - robot = add_ur5_gripper_robot(sim) + robot = add_tutorial_robot(sim, args.robot) obj = create_pick_object(sim) - motion_gen = create_toppra_motion_generator(robot) + motion_gen = create_curobo_motion_generator(robot) hand_open, hand_close = get_hand_open_close_qpos(robot) initialize_pre_pick_robot_pose(robot, obj, hand_open) @@ -156,38 +154,42 @@ def main() -> None: sim, args, "Inspect the cube, then press Enter to plan PickUp -> Place..." ) - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ) + endpoint_mapping = {"primary": {"motion": "arm", "grasp": "hand"}} compiled = engine.compile( ( - ActionInvocation( + engine.make_invocation( "pick_up", GraspGoal(semantics), - binding, - MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), + control_parts=endpoint_mapping, + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=PICK_SAMPLE_INTERVAL, + ), skill_options=PickUpOptions( pre_grasp_distance=0.15, lift_height=0.16, hand_interp_steps=HAND_INTERP_STEPS, ), ), - ActionInvocation( + engine.make_invocation( "place", PlaceGoal( broadcast_waypoint_pose_batch( place_poses, robot.get_qpos().shape[0] ) ), - binding, - MotionPolicy(sample_count=PLACE_SAMPLE_INTERVAL), + control_parts=endpoint_mapping, + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=PLACE_SAMPLE_INTERVAL, + ), skill_options=PlaceOptions( lift_height=PLACE_LIFT_HEIGHT, hand_interp_steps=HAND_INTERP_STEPS, ), ), - ) + ), + engine.initial_context(control_dt=sim.sim_config.physics_dt), ) if not compiled.plan_success.all(): logger.log_warning("Failed to plan Place demo trajectory.") diff --git a/scripts/tutorials/atomic_action/press.py b/scripts/tutorials/atomic_action/press.py index 59a4e5b6e..384b836ad 100644 --- a/scripts/tutorials/atomic_action/press.py +++ b/scripts/tutorials/atomic_action/press.py @@ -14,7 +14,7 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Demonstrate Press on the center of a regular wooden block.""" +"""Demonstrate Press on an articulation link or rigid object.""" from __future__ import annotations @@ -28,138 +28,169 @@ import torch +from embodichain.data import get_data_path from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, - EndEffectorPoseGoal, - PressOptions, - PressGoal, + EntityState, MotionPolicy, + ObjectSemantics, + PressAffordance, + PressGoal, + PressOptions, + SceneEntityPose, + SceneSnapshot, ) from embodichain.lab.sim.cfg import ( - RigidBodyAttributesCfg, + ArticulationCfg, + JointDrivePropertiesCfg, RigidObjectCfg, ) -from embodichain.lab.sim.material import VisualMaterialCfg -from embodichain.lab.sim.objects import RigidObject +from embodichain.lab.sim.objects import Articulation, RigidObject from embodichain.lab.sim.shapes import CubeCfg from embodichain.utils import logger from scripts.tutorials.atomic_action.tutorial_utils import ( add_ur5_gripper_robot, - broadcast_pose_batch, create_toppra_motion_generator, create_tutorial_argument_parser, create_tutorial_simulation, - draw_axis_marker, - format_tensor, get_hand_open_close_qpos, - make_top_down_eef_pose, prepare_tutorial_scene, replay_trajectory, run_tutorial, ) -MOVE_SAMPLE_INTERVAL = 60 -PRESS_SAMPLE_INTERVAL = 90 +MICROWAVE_ASSET = "MicrowaveOven/microwave_oven_with_inertials.urdf" +BUTTON_LINK_NAME = "button_cap" +MICROWAVE_POSITION = (-1.0, -0.30, 0.4) +MICROWAVE_ORIENTATION = (0.0, 0.0, 90) # degrees +PRESS_SAMPLE_INTERVAL = 140 HAND_INTERP_STEPS = 12 -POST_TRAJECTORY_STEPS = 180 -BLOCK_SIZE = (0.12, 0.12, 0.06) -PRESS_CLEARANCE = 0.13 -PRESS_SURFACE_OFFSET = 0.003 -DEFAULT_PRESS_TOLERANCE = 0.01 +POST_TRAJECTORY_STEPS = 240 +RIGID_BUTTON_POSITION = (-0.7, -0.00, 0.70) +RIGID_BUTTON_SIZE = (0.04, 0.02, 0.04) +BUTTON_SCENE_ENTITY_ID = "press-target" def parse_arguments() -> argparse.Namespace: """Parse command-line arguments for the Press tutorial.""" parser = create_tutorial_argument_parser( - "Demonstrate Press on a wooden block.", - features=("debug_state", "visualize_axes"), + "Demonstrate Press on an articulation-link or rigid button.", + features=("visualize_axes",), + ) + parser.add_argument("--press_distance", type=float, default=0.03) + parser.add_argument( + "--press_position", + type=float, + nargs=3, + default=None, + metavar=("X", "Y", "Z"), + help="Optional target-local press position overriding the affordance.", ) parser.add_argument( - "--press_tolerance", type=float, default=DEFAULT_PRESS_TOLERANCE + "--rigid_object", + action="store_true", + help="Use a standalone rigid button instead of the microwave link.", ) - parser.add_argument("--block_pos", type=float, nargs=2, default=(-0.30, -0.12)) return parser.parse_args() -def create_wooden_block(sim, center: list[float]) -> RigidObject: - """Create the static block used as a press target.""" - return sim.add_rigid_object( - cfg=RigidObjectCfg( - uid="wooden_block", - shape=CubeCfg( - size=list(BLOCK_SIZE), - visual_material=VisualMaterialCfg( - uid="wooden_block_mat", - base_color=[0.58, 0.32, 0.14, 1.0], - roughness=0.85, - ), +def create_microwave(sim) -> Articulation: + """Create the fixed-base microwave articulation used by the demo.""" + microwave = sim.add_articulation( + cfg=ArticulationCfg( + uid="microwave", + fpath=get_data_path(MICROWAVE_ASSET), + init_pos=MICROWAVE_POSITION, + init_qpos=(0, 0, 0, 0), + init_rot=MICROWAVE_ORIENTATION, + drive_pros=JointDrivePropertiesCfg( + stiffness=1e-3, damping=1e2, max_effort=1e-2 ), - body_type="static", - attrs=RigidBodyAttributesCfg(dynamic_friction=0.8, static_friction=0.9), - init_pos=center, + fix_base=True, ) ) + sim.update(step=10) + return microwave -def compute_press_center_check( - robot, - trajectory: torch.Tensor, - block: RigidObject, - tolerance: float, -) -> tuple[bool, float, int, torch.Tensor, torch.Tensor]: - """Return whether the press trajectory reaches the block center tolerance.""" - arm_joint_ids = robot.get_joint_ids(name="arm") - start = MOVE_SAMPLE_INTERVAL + HAND_INTERP_STEPS - arm_traj = trajectory[ - :, start : MOVE_SAMPLE_INTERVAL + PRESS_SAMPLE_INTERVAL, arm_joint_ids - ] - fk_pose = torch.stack( - [ - robot.compute_fk(qpos=qpos, name="arm", to_matrix=True) - for qpos in arm_traj.unbind(dim=1) - ], - dim=1, - ) - block_center = block.get_local_pose(to_matrix=True)[:, :3, 3] - target_z = block_center[:, 2] + 0.5 * BLOCK_SIZE[2] + PRESS_SURFACE_OFFSET - xy_error = torch.linalg.norm( - fk_pose[:, :, :2, 3] - block_center[:, None, :2], dim=2 - ) - z_error = torch.abs(fk_pose[:, :, 2, 3] - target_z[:, None]) - best_idx = (xy_error + z_error).argmin(dim=1) - env_idx = torch.arange(trajectory.shape[0], device=trajectory.device) - best_pos = fk_pose[env_idx, best_idx, :3, 3] - center_error = torch.linalg.norm(best_pos[:, :2] - block_center[:, :2], dim=1) - worst_env = int(center_error.argmax().item()) - expected = torch.stack( - [block_center[worst_env, 0], block_center[worst_env, 1], target_z[worst_env]] +def create_rigid_button(sim) -> RigidObject: + """Create the standalone static rigid button used by the optional demo.""" + button = sim.add_rigid_object( + cfg=RigidObjectCfg( + uid="rigid_button", + shape=CubeCfg(size=list(RIGID_BUTTON_SIZE)), + body_type="static", + init_pos=RIGID_BUTTON_POSITION, + ) ) + sim.update(step=10) + return button + + +def create_button_semantics( + target: Articulation | RigidObject, +) -> tuple[ObjectSemantics, torch.Tensor]: + """Create press semantics for an articulation-link or rigid button.""" + if isinstance(target, Articulation): + vertices, _ = target.get_link_vert_face(BUTTON_LINK_NAME) + target_pose = target.get_link_pose(BUTTON_LINK_NAME, to_matrix=True) + press_axis = torch.tensor([0.0, 0.0, -1.0], device=target.device) + affordance = PressAffordance( + # button_cap's local -z direction matches the prismatic joint's + # inward press direction in this asset. + press_axis=press_axis, + press_position=_surface_center(vertices, press_axis), + ) + label = "microwave_start_button" + else: + vertices = target.get_vertices(env_ids=[0], scale=True)[0] + target_pose = target.get_local_pose(to_matrix=True) + press_axis = torch.tensor([-1.0, 0.0, 0.0], device=target.device) + affordance = PressAffordance( + press_axis=press_axis, + press_position=_surface_center(vertices, press_axis), + ) + label = "rigid_button" return ( - bool(torch.all(center_error <= tolerance)), - float(center_error[worst_env].item()), - start + int(best_idx[worst_env].item()), - best_pos[worst_env], - expected, + ObjectSemantics( + label=label, + geometry={}, + entity_id=BUTTON_SCENE_ENTITY_ID, + affordance=affordance, + ), + target_pose, ) +def _surface_center( + vertices: torch.Tensor, + inward_axis: torch.Tensor, +) -> tuple[float, float, float]: + """Return the center of the outermost mesh face opposite inward travel.""" + vertices = torch.as_tensor(vertices, dtype=torch.float32, device=inward_axis.device) + axis = inward_axis.to(dtype=torch.float32) + axis = axis / torch.linalg.vector_norm(axis) + projection = torch.matmul(vertices, axis) + surface = vertices[torch.isclose(projection, projection.min(), atol=1.0e-5)] + point = surface.mean(dim=0) + return tuple(float(value) for value in point) + + def main() -> None: - """Plan, verify, and replay MoveEndEffector followed by Press.""" + """Plan and replay Press for the selected target object type.""" args = parse_arguments() sim = create_tutorial_simulation(args) - robot = add_ur5_gripper_robot(sim) - block = create_wooden_block(sim, [*args.block_pos, 0.5 * BLOCK_SIZE[2]]) - if sim.device.type == "cuda": - sim.init_gpu_physics() - block.reset() - sim.update(step=5) - block.clear_dynamics() - + robot = add_ur5_gripper_robot( + sim, init_qpos=[0.0, -1.57, 1.57, -3.14, -1.57, 0.0, 0.0, 0.0] + ) + target = create_rigid_button(sim) if args.rigid_object else create_microwave(sim) + hand_open, hand_close = get_hand_open_close_qpos(robot, close_qpos=0.040) motion_gen = create_toppra_motion_generator(robot) - hand_open, hand_close = get_hand_open_close_qpos(robot) + semantics, target_pose = create_button_semantics(target) + affordance = semantics.affordance + assert isinstance(affordance, PressAffordance) + engine = AtomicActionEngine( motion_generator=motion_gen, control_profiles={ @@ -169,81 +200,74 @@ def main() -> None: ) }, ) - block_center = block.get_local_pose(to_matrix=True)[0, :3, 3] - press_position = block_center.clone() - press_position[2] += 0.5 * BLOCK_SIZE[2] + PRESS_SURFACE_OFFSET - move_position = press_position.clone() - move_position[2] += PRESS_CLEARANCE - PRESS_SURFACE_OFFSET - n_envs = robot.get_qpos().shape[0] - move_target = broadcast_pose_batch(make_top_down_eef_pose(move_position), n_envs) - press_target = broadcast_pose_batch(make_top_down_eef_pose(press_position), n_envs) - if not args.no_vis_eef_axis: - draw_axis_marker(sim, "press_target_axis", press_target) wait_for_user = prepare_tutorial_scene( - sim, args, "Inspect the wooden block, then press Enter to plan..." + sim, + args, + "Inspect the button target, then press Enter to plan Press...", ) - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ) compiled = engine.compile( ( - ActionInvocation( - "move_end_effector", - EndEffectorPoseGoal(move_target), - binding, - MotionPolicy(sample_count=MOVE_SAMPLE_INTERVAL), - ), - ActionInvocation( + engine.make_invocation( "press", - PressGoal(press_target), - binding, - MotionPolicy(sample_count=PRESS_SAMPLE_INTERVAL), + PressGoal( + semantics, + SceneEntityPose(BUTTON_SCENE_ENTITY_ID), + ), + control_parts={"primary": {"motion": "arm", "grasp": "hand"}}, + motion_policy=MotionPolicy(sample_count=PRESS_SAMPLE_INTERVAL), skill_options=PressOptions( hand_interp_steps=HAND_INTERP_STEPS, + approach_distance=0.12, + press_distance=args.press_distance, + press_position=( + None + if args.press_position is None + else tuple(args.press_position) + ), ), ), - ) + ), + context=engine.initial_context( + scene=SceneSnapshot( + timestamp=0.0, + version=0, + entities={BUTTON_SCENE_ENTITY_ID: EntityState(target_pose)}, + ), + control_dt=sim.sim_config.physics_dt, + ), ) if not compiled.plan_success.all(): - logger.log_warning("Failed to plan Press demo trajectory.") - return - trajectory = compiled.trajectory.positions - is_center_hit, center_error, hit_step, hit_pos, expected_pos = ( - compute_press_center_check(robot, trajectory, block, args.press_tolerance) - ) - logger.log_info( - "Press center check: " - f"success={is_center_hit}, xy_error={center_error:.4f} m, hit_step={hit_step}, " - f"hit_pos={format_tensor(hit_pos)}, expected={format_tensor(expected_pos)}" - ) - if not is_center_hit: - logger.log_warning( - "Press trajectory did not reach the block center within tolerance." - ) + logger.log_warning("Failed to plan the Press demo trajectory.") return + if isinstance(target, RigidObject): + focus_pose = target.get_local_pose(to_matrix=True) + elif isinstance(target, Articulation): + focus_pose = target.get_link_pose(BUTTON_LINK_NAME, to_matrix=True) + else: + raise ValueError("Unsupported target type for Press demo.") + focus_position = [focus_pose[0, 0, 3], focus_pose[0, 1, 3], focus_pose[0, 2, 3]] + camera_position = [ + focus_position[0] + 0.0, + focus_position[1] + 0.3, + focus_position[2] + 0.2, + ] + look_at = [camera_position, focus_position, [0, 0, 1]] if wait_for_user: input("Press Enter to replay the Press demo...") - - def log_state(step_idx: int, total_steps: int) -> None: - if args.debug_state and ( - step_idx % max(1, total_steps // 10) == 0 or step_idx == total_steps - 1 - ): - logger.log_info( - f"replay step {step_idx}/{total_steps - 1}: " - f"pos={format_tensor(block.get_local_pose(to_matrix=True)[0, :3, 3])}" - ) - replay_trajectory( sim, robot, compiled.trajectory, args, - video_prefix="press_auto_play", + video_prefix=( + "press_rigid_button_auto_play" + if args.rigid_object + else "press_microwave_button_auto_play" + ), hold_steps=POST_TRAJECTORY_STEPS, - on_trajectory_step=log_state, + look_at=look_at, ) if wait_for_user: input("Press Enter to exit the simulation...") diff --git a/scripts/tutorials/atomic_action/scenario_utils.py b/scripts/tutorials/atomic_action/scenario_utils.py index b51aefb91..4fb0f8fda 100644 --- a/scripts/tutorials/atomic_action/scenario_utils.py +++ b/scripts/tutorials/atomic_action/scenario_utils.py @@ -39,8 +39,14 @@ ) from embodichain.lab.sim.objects import RigidObject, Robot from embodichain.lab.sim.shapes import CubeCfg +from embodichain.lab.sim.robots import build_dual_arm_cfg from embodichain.lab.sim.solvers import PytorchSolverCfg, SolverCfg, URSolverCfg from embodichain.utils import logger +from scripts.tutorials.atomic_action.tutorial_utils import ( + GRIPPER_HAND_JOINT_PATTERN, + TutorialRobot, + create_tutorial_robot_cfg, +) ARM_URDF_PATH = "UniversalRobots/UR5/UR5.urdf" GRIPPER_URDF_PATH = "DH_PGI_140_80/DH_PGI_140_80.urdf" @@ -210,6 +216,190 @@ def add_dual_ur5_robot( return sim.add_robot(cfg=cfg) +def create_dual_tutorial_robot_cfg( + *, + robot_type: TutorialRobot, + uid: str, + urdf_name: str, + tcp_z: float, + solver: Literal["ur", "pytorch"] = "ur", + ur_ik_nearest_weight: Sequence[float] | None = None, + pytorch_num_samples: int = 30, + init_pos: Sequence[float] = DUAL_UR5_INIT_POS, + init_rot: Sequence[float] = DUAL_UR5_INIT_ROT, + left_arm_home: Sequence[float] | None = None, + right_arm_home: Sequence[float] | None = None, + hand_stiffness: float = 1e3, + hand_damping: float = 1e2, + hand_max_effort: float = 1e4, +) -> RobotCfg: + """Build a dual tutorial robot from the selected arm and shared PGI hand. + + Franka always uses its PyTorch kinematics solver; ``solver="ur"`` selects + the analytical solver only when ``robot_type="ur5"``. The mounting layout, + control-part names, gripper component, and downstream action bindings stay + identical across both robot choices. + + Args: + robot_type: Arm family to mount on both sides. + uid: Simulation robot identifier. + urdf_name: Cache name for the assembled dual-arm URDF. + tcp_z: PGI tool-center-point offset along local Z. + solver: Preferred UR5 solver implementation. + ur_ik_nearest_weight: Optional nearest-solution weights for UR5 IK. + pytorch_num_samples: Number of PyTorch IK seed samples. + init_pos: Root position of the assembled robot. + init_rot: Root xyz Euler rotation in degrees. + left_arm_home: Optional left-arm initial configuration. + right_arm_home: Optional right-arm initial configuration. + hand_stiffness: PGI joint drive stiffness. + hand_damping: PGI joint drive damping. + hand_max_effort: PGI joint maximum effort. + + Returns: + A dual-arm robot configuration with two PGI grippers. + """ + base_cfg = create_tutorial_robot_cfg(robot_type) + tcp = [ + [1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 1.0, tcp_z], + [0.0, 0.0, 0.0, 1.0], + ] + base_solver = base_cfg.solver_cfg["arm"] + if robot_type == "ur5" and solver == "ur": + base_solver.tcp = tcp + base_solver.ik_nearest_weight = ur_ik_nearest_weight + else: + base_solver = PytorchSolverCfg( + end_link_name=base_solver.end_link_name, + root_link_name=base_solver.root_link_name, + tcp=tcp, + num_samples=pytorch_num_samples, + ) + base_cfg.solver_cfg["arm"] = base_solver + + for property_name, value in ( + ("stiffness", hand_stiffness), + ("damping", hand_damping), + ("max_effort", hand_max_effort), + ): + getattr(base_cfg.drive_pros, property_name)[GRIPPER_HAND_JOINT_PATTERN] = value + + arm_facing_rotation = make_yaw_transform( + (0.0, 0.0, 0.0), + math.radians(float(base_cfg.init_rot[2])), + ) + mounts = { + "left": make_yaw_transform((-0.3, -1.45, 0.4), np.pi / 2) @ arm_facing_rotation, + "right": make_yaw_transform((0.3, -1.45, 0.4), np.pi / 2) @ arm_facing_rotation, + } + cfg = build_dual_arm_cfg(base_cfg, mounts) + + # ``build_dual_arm_cfg`` duplicates the arm component and all control + # parts. The tutorial robots intentionally keep the PGI as a separate URDF + # component, so mount one copy on each assembled arm as well. + cfg.urdf_cfg.fname = urdf_name + hand_component = base_cfg.urdf_cfg.components["hand"] + for side in ("left", "right"): + cfg.urdf_cfg.add_component( + f"{side}_hand", + hand_component["urdf_path"], + hand_component["transform"], + **hand_component.get("params", {}), + ) + + arm_dof = len(base_cfg.control_parts["arm"]) + base_arm_home = list(base_cfg.init_qpos[:arm_dof]) + base_hand_home = list(base_cfg.init_qpos[arm_dof:]) + if left_arm_home is None: + left_arm_home = base_arm_home + if right_arm_home is None: + right_arm_home = base_arm_home + if len(left_arm_home) != arm_dof or len(right_arm_home) != arm_dof: + raise ValueError( + f"Dual {robot_type} arm homes must each contain {arm_dof} joints." + ) + + cfg.uid = uid + cfg.init_pos = list(init_pos) + cfg.init_rot = list(init_rot) + # DexSim traverses the two arm branches breadth-first, so their active + # joints appear left/right interleaved even though the URDF components are + # emitted one after the other. Match that runtime order before appending + # the two gripper components. + cfg.init_qpos = ( + [ + qpos + for joint_pair in zip(left_arm_home, right_arm_home, strict=True) + for qpos in joint_pair + ] + + base_hand_home + + base_hand_home + ) + return cfg + + +def add_dual_tutorial_robot( + sim: SimulationManager, + *, + robot_type: TutorialRobot, + uid: str, + urdf_name: str, + tcp_z: float, + solver: Literal["ur", "pytorch"] = "ur", + ur_ik_nearest_weight: Sequence[float] | None = None, + pytorch_num_samples: int = 30, + init_pos: Sequence[float] = DUAL_UR5_INIT_POS, + init_rot: Sequence[float] = DUAL_UR5_INIT_ROT, + left_arm_home: Sequence[float] | None = None, + right_arm_home: Sequence[float] | None = None, + hand_stiffness: float = 1e3, + hand_damping: float = 1e2, + hand_max_effort: float = 1e4, +) -> Robot: + """Add a dual UR5 or Franka tutorial robot to a simulation. + + Args: + sim: Simulation manager that owns the robot. + robot_type: Arm family to mount on both sides. + uid: Simulation robot identifier. + urdf_name: Cache name for the assembled dual-arm URDF. + tcp_z: PGI tool-center-point offset along local Z. + solver: Preferred UR5 solver implementation. + ur_ik_nearest_weight: Optional nearest-solution weights for UR5 IK. + pytorch_num_samples: Number of PyTorch IK seed samples. + init_pos: Root position of the assembled robot. + init_rot: Root xyz Euler rotation in degrees. + left_arm_home: Optional left-arm initial configuration. + right_arm_home: Optional right-arm initial configuration. + hand_stiffness: PGI joint drive stiffness. + hand_damping: PGI joint drive damping. + hand_max_effort: PGI joint maximum effort. + + Returns: + The added dual-arm robot instance. + """ + return sim.add_robot( + cfg=create_dual_tutorial_robot_cfg( + robot_type=robot_type, + uid=uid, + urdf_name=urdf_name, + tcp_z=tcp_z, + solver=solver, + ur_ik_nearest_weight=ur_ik_nearest_weight, + pytorch_num_samples=pytorch_num_samples, + init_pos=init_pos, + init_rot=init_rot, + left_arm_home=left_arm_home, + right_arm_home=right_arm_home, + hand_stiffness=hand_stiffness, + hand_damping=hand_damping, + hand_max_effort=hand_max_effort, + ) + ) + + def add_support_surface( sim: SimulationManager, *, @@ -333,11 +523,13 @@ def log_action_plan( "GRIPPER_URDF_PATH", "LEFT_ARM_HOME", "RIGHT_ARM_HOME", + "add_dual_tutorial_robot", "add_dual_ur5_robot", "add_support_surface", "compute_local_bounds", "compute_world_bounds", "create_manual_object_semantics", + "create_dual_tutorial_robot_cfg", "get_local_vertices", "invert_pose", "log_action_plan", diff --git a/scripts/tutorials/atomic_action/slide.py b/scripts/tutorials/atomic_action/slide.py new file mode 100644 index 000000000..aa5a5a8d1 --- /dev/null +++ b/scripts/tutorials/atomic_action/slide.py @@ -0,0 +1,310 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Demonstrate Slide on a translating drawer.""" + +from __future__ import annotations + +import argparse +import sys +from pathlib import Path +from typing import Literal + +_REPO_ROOT = Path(__file__).resolve().parents[3] +if str(_REPO_ROOT) not in sys.path: + sys.path.insert(0, str(_REPO_ROOT)) + +import torch + +from embodichain.data import get_data_path +from embodichain.lab.sim import SimulationManager +from embodichain.lab.sim.atomic_actions import ( + ActionInvocation, + AtomicActionEngine, + ControlPartCommandProfile, + EntityState, + MotionPolicy, + ObjectSemantics, + SlideAffordance, + SlideGoal, + SlideOptions, + SceneEntityPose, + SceneSnapshot, +) +from embodichain.lab.sim.cfg import ( + ArticulationCfg, + JointDrivePropertiesCfg, + RigidBodyAttributesCfg, +) +from embodichain.lab.sim.objects import Articulation +from embodichain.toolkits.graspkit.pg_grasp.antipodal_generator import ( + AntipodalSamplerCfg, + GraspGeneratorCfg, +) +from embodichain.toolkits.graspkit.pg_grasp.gripper_collision_checker import ( + GripperCollisionCfg, +) +from embodichain.utils import logger +from scripts.tutorials.atomic_action.tutorial_utils import ( + add_ur5_gripper_robot, + create_toppra_motion_generator, + create_tutorial_argument_parser, + create_tutorial_simulation, + draw_axis_marker, + get_hand_open_close_qpos, + prepare_tutorial_scene, + replay_trajectory, + run_tutorial, +) + +DRAWER_ASSET = "Drawer/model_split_links_with_inertials.urdf" +HANDLE_LINK_NAME = "large_handle_bar" +DRAWER_POSITION = (-1.1, 0.0, 0.0) +DRAWER_ORIENTATION = (0.0, 0.0, 90.0) # degrees +TRANSLATION_AXIS = (0.0, 1.0, 0.0) # handle-link frame, approach/push direction +TRAJECTORY_SAMPLE_COUNT = 140 +HAND_INTERP_STEPS = 12 +POST_TRAJECTORY_STEPS = 240 +HANDLE_SCENE_ENTITY_ID = "drawer-large-handle" + + +def parse_arguments() -> argparse.Namespace: + """Parse command-line arguments for the drawer pull/push tutorial.""" + parser = create_tutorial_argument_parser( + "Pull a drawer open, then push it closed with Slide.", + features=("grasp_sampling", "visualize_axes"), + ) + parser.add_argument("--translation_distance", type=float, default=0.18) + parser.add_argument("--approach_distance", type=float, default=0.10) + return parser.parse_args() + + +def create_drawer( + sim: SimulationManager, +) -> Articulation: + """Create the fixed-base drawer in its closed initial state.""" + drawer = sim.add_articulation( + cfg=ArticulationCfg( + uid="drawer", + fpath=get_data_path(DRAWER_ASSET), + init_pos=DRAWER_POSITION, + init_rot=DRAWER_ORIENTATION, + init_qpos=(0.0,), + drive_pros=JointDrivePropertiesCfg(drive_type="none"), + attrs=RigidBodyAttributesCfg( + static_friction=1.0, + dynamic_friction=1.0, + ), + fix_base=True, + ) + ) + sim.update(step=10) + return drawer + + +def create_drawer_semantics( + drawer: Articulation, + *, + n_sample: int, + force_reannotate: bool, +) -> ObjectSemantics: + """Create sampled-grasp translation semantics for the drawer handle. + + Args: + drawer: Drawer articulation that owns the target handle link. + n_sample: Number of antipodal surface samples. + force_reannotate: Whether to ignore a cached grasp annotation. + + Returns: + Pure target-local semantics for the handle's pull/push affordance. + """ + vertices, triangles = drawer.get_link_vert_face(HANDLE_LINK_NAME) + return ObjectSemantics( + label="drawer_large_handle", + geometry={}, + entity_id=HANDLE_SCENE_ENTITY_ID, + affordance=SlideAffordance( + mesh_vertices=torch.as_tensor(vertices), + mesh_triangles=torch.as_tensor(triangles), + translation_axis=torch.tensor( + TRANSLATION_AXIS, + dtype=torch.float32, + device=drawer.device, + ), + generator_cfg=GraspGeneratorCfg( + antipodal_sampler_cfg=AntipodalSamplerCfg( + n_sample=n_sample, + max_length=0.1, + min_length=0.003, + ), + is_partial_annotate=False, + is_filter_ground_collision=False, + ), + gripper_collision_cfg=GripperCollisionCfg( + max_open_length=0.1, + finger_length=0.1, + y_thickness=0.04, + root_z_width=0.096, + open_check_margin=0.03, + point_sample_dense=0.012, + ), + force_reannotate=force_reannotate, + ), + ) + + +def create_invocation( + engine: AtomicActionEngine, + semantics: ObjectSemantics, + *, + direction: Literal["pull", "push"], + approach_distance: float, + translation_distance: float, +) -> ActionInvocation: + """Create one pull or push invocation for the shared drawer target. + + Args: + engine: Engine used to resolve the slide control-part binding. + semantics: Drawer-handle semantics shared by both operations. + direction: Whether this invocation pulls open or pushes closed. + approach_distance: Pre-grasp offset opposite the approach axis. + translation_distance: Drawer travel distance for this operation. + + Returns: + A grounded pull/push invocation for the tutorial UR5. + """ + return engine.make_invocation( + "slide", + SlideGoal( + semantics, + SceneEntityPose(HANDLE_SCENE_ENTITY_ID), + ), + control_parts={"primary": {"motion": "arm", "grasp": "hand"}}, + motion_policy=MotionPolicy(sample_count=TRAJECTORY_SAMPLE_COUNT), + skill_options=SlideOptions( + direction=direction, + hand_interp_steps=HAND_INTERP_STEPS, + approach_distance=approach_distance, + translation_distance=translation_distance, + ), + ) + + +def main() -> None: + """Plan and replay a drawer pull followed by a push.""" + args = parse_arguments() + if args.translation_distance <= 0.0: + raise ValueError("--translation_distance must be positive.") + if args.translation_distance > 0.285: + raise ValueError( + "--translation_distance must not exceed the drawer limit 0.285." + ) + + sim = create_tutorial_simulation(args) + robot = add_ur5_gripper_robot( + sim, init_qpos=[0.0, -1.57, 1.57, -3.14, -1.57, 0.0, 0.0, 0.0], tcp_z=0.15 + ) + drawer = create_drawer(sim) + hand_open, hand_close = get_hand_open_close_qpos(robot) + motion_gen = create_toppra_motion_generator(robot) + semantics = create_drawer_semantics( + drawer, + n_sample=args.n_sample, + force_reannotate=args.force_reannotate, + ) + affordance = semantics.affordance + assert isinstance(affordance, SlideAffordance) + if not args.no_vis_eef_axis: + draw_axis_marker( + sim, + "drawer_handle_link_pose", + drawer.get_link_pose(HANDLE_LINK_NAME, to_matrix=True), + ) + + engine = AtomicActionEngine( + motion_generator=motion_gen, + control_profiles={ + "hand": ControlPartCommandProfile.joint_positions( + open=hand_open, + grasp=hand_close, + ) + }, + ) + wait_for_user = prepare_tutorial_scene( + sim, + args, + "Inspect the closed drawer, then press Enter to plan the pull...", + ) + + for scene_version, direction in enumerate(("pull", "push")): + if direction == "push" and wait_for_user: + input( + "Pull replay finished. Press Enter to read the moved handle " + "pose and plan the push..." + ) + + handle_pose = drawer.get_link_pose(HANDLE_LINK_NAME, to_matrix=True) + compiled = engine.compile( + ( + create_invocation( + engine, + semantics, + direction=direction, + approach_distance=args.approach_distance, + translation_distance=args.translation_distance, + ), + ), + context=engine.initial_context( + scene=SceneSnapshot( + timestamp=float(scene_version), + version=scene_version, + entities={ + HANDLE_SCENE_ENTITY_ID: EntityState(handle_pose), + }, + ), + control_dt=sim.sim_config.physics_dt, + ), + ) + if not compiled.plan_success.all(): + logger.log_warning(f"Failed to plan the Slide {direction} trajectory.") + return + + if wait_for_user: + input(f"Press Enter to replay the drawer {direction}...") + focus_pose = drawer.get_link_pose(HANDLE_LINK_NAME, to_matrix=True) + focus_position = [focus_pose[0, 0, 3], focus_pose[0, 1, 3], focus_pose[0, 2, 3]] + camera_position = [ + focus_position[0] + 0.5, + focus_position[1] + 0.5, + focus_position[2] + 0.5, + ] + look_at = [camera_position, focus_position, [0, 0, 1]] + replay_trajectory( + sim, + robot, + compiled.trajectory, + args, + video_prefix=f"{direction}_drawer_auto_play", + hold_steps=POST_TRAJECTORY_STEPS, + look_at=look_at, + ) + + if wait_for_user: + input("Press Enter to exit the simulation...") + + +if __name__ == "__main__": + run_tutorial(main) diff --git a/scripts/tutorials/atomic_action/tutorial_utils.py b/scripts/tutorials/atomic_action/tutorial_utils.py index bfca00fe1..8165f8ff3 100644 --- a/scripts/tutorials/atomic_action/tutorial_utils.py +++ b/scripts/tutorials/atomic_action/tutorial_utils.py @@ -36,8 +36,13 @@ ) from embodichain.lab.sim.cfg import LightCfg, MarkerCfg, RenderCfg, RobotCfg from embodichain.lab.sim.objects import RigidObject, Robot -from embodichain.lab.sim.planners import MotionGenCfg, MotionGenerator, ToppraPlannerCfg -from embodichain.lab.sim.robots import URRobotCfg +from embodichain.lab.sim.planners import ( + CuroboPlannerCfg, + MotionGenCfg, + MotionGenerator, + ToppraPlannerCfg, +) +from embodichain.lab.sim.robots import FrankaPandaCfg, URRobotCfg from embodichain.lab.sim.solvers import URSolverCfg from embodichain.toolkits.graspkit.pg_grasp.antipodal_generator import ( AntipodalSamplerCfg, @@ -64,13 +69,21 @@ GRIPPER_URDF_PATH = "DH_PGI_140_80/DH_PGI_140_80.urdf" GRIPPER_HAND_JOINT_PATTERN = "gripper_finger1_joint_1" -GRIPPER_TCP_Z = 0.15 GRIPPER_MAX_OPEN_WIDTH = 0.100 -GRIPPER_FINGER_LENGTH = 0.12 +GRIPPER_MIN_OPEN_WIDTH = 0.003 +GRIPPER_FINGER_LENGTH = 0.10 GRIPPER_ROOT_Z_WIDTH = 0.096 GRIPPER_Y_THICKNESS = 0.040 DEFAULT_GRIPPER_CLOSE_QPOS = 0.024 DEFAULT_TUTORIAL_LIGHT_POS = (1.0, 0.0, 3.0) +_FRANKA_TUTORIAL_BASE_ROTATION = (0.0, 0.0, 180.0) +_DEFAULT_GRIPPER_TCP_Z = 0.17 +_GRIPPER_TCP = ( + (1.0, 0.0, 0.0, 0.0), + (0.0, 1.0, 0.0, 0.0), + (0.0, 0.0, 1.0, _DEFAULT_GRIPPER_TCP_Z), + (0.0, 0.0, 0.0, 1.0), +) TOP_DOWN_EEF_ROTATION = ( (-0.0539, -0.9985, -0.0022), (-0.9977, 0.0540, -0.0401), @@ -84,6 +97,8 @@ "headless_play", "visualize_axes", ] +TutorialRobot = Literal["ur5", "franka"] +TUTORIAL_ROBOTS: tuple[TutorialRobot, ...] = ("ur5", "franka") def create_tutorial_argument_parser( @@ -109,6 +124,12 @@ def create_tutorial_argument_parser( action="store_true", help="Run the demo without waiting for keyboard input.", ) + parser.add_argument( + "--robot", + choices=TUTORIAL_ROBOTS, + default="ur5", + help="Robot construction to use (default: ur5).", + ) if "debug_state" in features: parser.add_argument( "--debug_state", @@ -209,21 +230,37 @@ def run_tutorial(main: Callable[[], None]) -> None: Args: main: Zero-argument tutorial entry point. """ + interrupted = False try: - main() + try: + main() + except KeyboardInterrupt: + # Handle Ctrl+C before native cleanup. An active traceback keeps + # main() locals (including borrowed C++ material wrappers) alive; + # destroying World first would make their later destructors unsafe. + interrupted = True + logger.log_info("Tutorial interrupted; shutting down cleanly.") finally: if SimulationManager.is_instantiated(): sim = SimulationManager.get_instance() - if sim.is_window_recording(): - sim.stop_window_record() - sim.wait_window_record_saves() - sim.destroy(exit_process=False) - SimulationManager.flush_cleanup_queue() + if not getattr(sim, "_is_constructed", False): + SimulationManager.reset(getattr(sim, "instance_id", 0)) + else: + if sim.is_window_recording(): + sim.stop_window_record() + sim.wait_window_record_saves() + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + + if interrupted: + raise SystemExit(130) def add_ur5_gripper_robot( sim: SimulationManager, init_pos: Sequence[float] = (0.0, 0.0, 0.0), + init_qpos: Sequence[float] | None = None, + tcp_z: float = _DEFAULT_GRIPPER_TCP_Z, ) -> Robot: """Add the standard UR5 plus PGI gripper tutorial robot. @@ -234,7 +271,65 @@ def add_ur5_gripper_robot( Returns: The added robot instance. """ - return sim.add_robot(cfg=create_ur5_gripper_robot_cfg(init_pos=init_pos)) + return sim.add_robot( + cfg=create_ur5_gripper_robot_cfg( + init_pos=init_pos, + init_qpos=init_qpos, + tcp_z=tcp_z, + ) + ) + + +def add_franka_panda_robot( + sim: SimulationManager, + init_pos: Sequence[float] = (0.0, 0.0, 0.0), + init_qpos: Sequence[float] | None = None, +) -> Robot: + """Add a Franka arm with the standard PGI tutorial gripper. + + Args: + sim: Simulation manager that owns the robot. + init_pos: Root position of the robot in its arena. + init_qpos: Optional full robot joint configuration. + + Returns: + The added robot instance. + """ + return sim.add_robot( + cfg=create_franka_panda_robot_cfg( + init_pos=init_pos, + init_qpos=init_qpos, + ) + ) + + +def add_tutorial_robot( + sim: SimulationManager, + robot_type: TutorialRobot, + init_pos: Sequence[float] = (0.0, 0.0, 0.0), + init_qpos: Sequence[float] | None = None, +) -> Robot: + """Add a selected tutorial robot with the shared PGI gripper. + + Args: + sim: Simulation manager that owns the robot. + robot_type: Tutorial robot family to construct. + init_pos: Root position of the robot in its arena. + init_qpos: Optional full robot joint configuration. + + Returns: + The added robot instance. + + Raises: + ValueError: If ``robot_type`` is not supported. + """ + return sim.add_robot( + cfg=create_tutorial_robot_cfg( + robot_type, + init_pos=init_pos, + init_qpos=init_qpos, + ) + ) def create_toppra_motion_generator(robot: Robot) -> MotionGenerator: @@ -251,13 +346,27 @@ def create_toppra_motion_generator(robot: Robot) -> MotionGenerator: ) +def create_curobo_motion_generator(robot: Robot) -> MotionGenerator: + """Create a cuRobo-backed motion generator for a tutorial robot. + + Args: + robot: Robot whose trajectories will be planned. + + Returns: + The configured motion generator with an empty external collision world. + """ + return MotionGenerator( + cfg=MotionGenCfg(planner_cfg=CuroboPlannerCfg(robot_uid=robot.uid)) + ) + + def get_hand_open_close_qpos( robot: Robot, *, hand_control_part: str = "hand", close_qpos: float = DEFAULT_GRIPPER_CLOSE_QPOS, ) -> tuple[torch.Tensor, torch.Tensor]: - """Return the open limit and a safe closed position for a gripper. + """Return the open limit and a safe closed position for a PGI gripper. Args: robot: Robot containing the gripper control part. @@ -308,7 +417,7 @@ def create_antipodal_semantics( finger_length=GRIPPER_FINGER_LENGTH, y_thickness=GRIPPER_Y_THICKNESS, root_z_width=GRIPPER_ROOT_Z_WIDTH, - open_check_margin=0.002, + open_check_margin=0.03, point_sample_dense=0.012, ), generator_cfg=GraspGeneratorCfg( @@ -316,7 +425,7 @@ def create_antipodal_semantics( antipodal_sampler_cfg=AntipodalSamplerCfg( n_sample=n_sample, max_length=GRIPPER_MAX_OPEN_WIDTH, - min_length=0.005, + min_length=GRIPPER_MIN_OPEN_WIDTH, ), is_partial_annotate=False, is_filter_ground_collision=False, @@ -364,7 +473,7 @@ def make_eef_pose_at(robot: Robot, position: torch.Tensor) -> torch.Tensor: Args: robot: Robot whose current arm pose supplies the orientation. - position: Position tensor with shape ``(3,)`` or ``(n_envs, 3)``. + position: Position tensor with shape ``(3,)`` or ``(num_envs, 3)``. Returns: A single or batched homogeneous pose matching ``position``. @@ -385,7 +494,7 @@ def initialize_pre_pick_robot_pose( *, height: float = 0.36, ) -> None: - """Set a UR5 at a deterministic open-gripper pose above an object. + """Set a tutorial robot at a deterministic open-gripper pose above an object. Args: robot: Robot to initialize. @@ -505,7 +614,7 @@ def replay_trajectory( sim: Simulation manager to step and record. robot: Robot receiving full-DOF trajectory positions. trajectory: Timed full-robot trajectory, or a legacy position tensor - with shape ``(n_envs, n_steps, dof)``. + with shape ``(num_envs, n_steps, dof)``. args: Parsed tutorial arguments controlling auto-play recording. video_prefix: Output video filename prefix. hold_steps: Number of final-pose simulation updates after the trajectory. @@ -678,14 +787,14 @@ def draw_axis_marker( ) -> None: """Draw a named coordinate-frame marker for a semantic tutorial target.""" arena_offsets = sim.arena_offsets - n_envs = arena_offsets.shape[0] + num_envs = arena_offsets.shape[0] # Normalize a single (4, 4) pose to (1, 4, 4) so it is not mistaken for a # per-environment batch when num_envs happens to equal 4. if xpos.dim() == 2: xpos = xpos.unsqueeze(0) - if n_envs == xpos.shape[0]: + if num_envs == xpos.shape[0]: # add arena offsets to xpos draw_xpos = xpos.clone() draw_xpos[:, :3, 3] += arena_offsets @@ -749,6 +858,8 @@ def clone_local_pose_from_first_env(entity) -> torch.Tensor: def create_ur5_gripper_robot_cfg( init_pos: Sequence[float] = (0.0, 0.0, 0.0), + init_qpos: Sequence[float] | None = None, + tcp_z: float = _DEFAULT_GRIPPER_TCP_Z, ) -> RobotCfg: """Build a UR5 arm + DH_PGI_140_80 gripper robot configuration. @@ -773,6 +884,11 @@ def create_ur5_gripper_robot_cfg( Returns: A fully populated :class:`~embodichain.lab.sim.cfg.RobotCfg`. """ + qpos = ( + [0.0, -1.57, 1.57, -1.57, -1.57, 0.0, 0.0, 0.0] + if init_qpos is None + else list(init_qpos) + ) return URRobotCfg.from_dict( { "robot_type": "ur5", @@ -804,17 +920,112 @@ def create_ur5_gripper_robot_cfg( "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, 1.0, tcp_z], [0.0, 0.0, 0.0, 1.0], ] } }, - "init_qpos": [0.0, -1.57, 1.57, -1.57, -1.57, 0.0, 0.0, 0.0], + "init_qpos": qpos, "init_pos": init_pos, } ) +def create_franka_panda_robot_cfg( + init_pos: Sequence[float] = (0.0, 0.0, 0.0), + init_qpos: Sequence[float] | None = None, +) -> RobotCfg: + """Build a Franka arm + PGI gripper configuration for the tutorials. + + The shared scenes place manipulation targets on the robot's negative-X + side, so the base is rotated 180 degrees. The arm-only Franka URDF is + assembled with the same DH_PGI_140_80 component, hand control part, drive + properties, open state, and TCP offset used by the UR5 tutorial robot. + + Args: + init_pos: Initial root position of the robot in the arena. + init_qpos: Optional full robot joint configuration. + + Returns: + A fully populated :class:`~embodichain.lab.sim.cfg.RobotCfg`. + """ + overrides = { + "robot_type": "panda", + "uid": "FrankaPanda", + "init_pos": init_pos, + "init_rot": _FRANKA_TUTORIAL_BASE_ROTATION, + "urdf_cfg": { + "components": [ + { + "component_type": "arm", + "urdf_path": "Franka/Panda/Panda.urdf", + }, + { + "component_type": "hand", + "urdf_path": GRIPPER_URDF_PATH, + }, + ], + }, + "control_parts": {"hand": [GRIPPER_HAND_JOINT_PATTERN]}, + "drive_pros": { + "stiffness": {GRIPPER_HAND_JOINT_PATTERN: 1e3}, + "damping": {GRIPPER_HAND_JOINT_PATTERN: 1e2}, + "max_effort": {GRIPPER_HAND_JOINT_PATTERN: 1e4}, + }, + "solver_cfg": { + "arm": { + "end_link_name": "fr3_link8", + "tcp": _GRIPPER_TCP, + } + }, + } + if init_qpos is not None: + overrides["init_qpos"] = list(init_qpos) + cfg = FrankaPandaCfg.from_dict(overrides) + if init_qpos is None: + cfg.init_qpos[-2:] = [0.0, 0.0] + for drive_values in ( + cfg.drive_pros.stiffness, + cfg.drive_pros.damping, + cfg.drive_pros.max_effort, + ): + drive_values.pop("fr3_finger_joint[1-2]", None) + return cfg + + +def create_tutorial_robot_cfg( + robot_type: TutorialRobot, + init_pos: Sequence[float] = (0.0, 0.0, 0.0), + init_qpos: Sequence[float] | None = None, +) -> RobotCfg: + """Build a selected tutorial arm with the common PGI gripper contract. + + Args: + robot_type: Tutorial robot family to construct. + init_pos: Initial root position of the robot in its arena. + init_qpos: Optional full robot joint configuration. + + Returns: + A UR5 or Franka robot configuration exposing ``arm`` and ``hand``. + + Raises: + ValueError: If ``robot_type`` is not supported. + """ + if robot_type == "ur5": + return create_ur5_gripper_robot_cfg( + init_pos=init_pos, + init_qpos=init_qpos, + ) + if robot_type == "franka": + return create_franka_panda_robot_cfg( + init_pos=init_pos, + init_qpos=init_qpos, + ) + raise ValueError( + f"Unsupported tutorial robot {robot_type!r}; expected one of {TUTORIAL_ROBOTS}." + ) + + __all__ = [ "DEFAULT_AUTO_PLAY_LOOK_AT", "DEFAULT_AXIS_LEN", @@ -822,17 +1033,23 @@ def create_ur5_gripper_robot_cfg( "DEFAULT_GRIPPER_CLOSE_QPOS", "DEFAULT_TUTORIAL_LIGHT_POS", "GRIPPER_HAND_JOINT_PATTERN", - "GRIPPER_TCP_Z", "GRIPPER_URDF_PATH", "TOP_DOWN_EEF_ROTATION", "TutorialCliFeature", + "TutorialRobot", + "TUTORIAL_ROBOTS", + "add_franka_panda_robot", + "add_tutorial_robot", "add_ur5_gripper_robot", "broadcast_pose_batch", "broadcast_waypoint_pose_batch", "clone_local_pose_from_first_env", "create_antipodal_semantics", + "create_curobo_motion_generator", + "create_franka_panda_robot_cfg", "create_toppra_motion_generator", "create_tutorial_argument_parser", + "create_tutorial_robot_cfg", "create_tutorial_simulation", "create_ur5_gripper_robot_cfg", "format_tensor", diff --git a/scripts/tutorials/atomic_action/twist.py b/scripts/tutorials/atomic_action/twist.py new file mode 100644 index 000000000..44f6ebec6 --- /dev/null +++ b/scripts/tutorials/atomic_action/twist.py @@ -0,0 +1,253 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Demonstrate Twist on an articulation link or rigid object.""" + +from __future__ import annotations + +import argparse +import sys +from pathlib import Path + +_REPO_ROOT = Path(__file__).resolve().parents[3] +if str(_REPO_ROOT) not in sys.path: + sys.path.insert(0, str(_REPO_ROOT)) + +import torch + +from embodichain.data import get_data_path +from embodichain.lab.sim.atomic_actions import ( + AtomicActionEngine, + ControlPartCommandProfile, + EntityState, + MotionPolicy, + ObjectSemantics, + TwistAffordance, + TwistGoal, + TwistOptions, + SceneEntityPose, + SceneSnapshot, +) +from embodichain.lab.sim.cfg import ( + ArticulationCfg, + JointDrivePropertiesCfg, + RigidObjectCfg, +) +from embodichain.lab.sim.objects import Articulation, RigidObject +from embodichain.lab.sim.shapes import CubeCfg +from embodichain.utils import logger +from scripts.tutorials.atomic_action.tutorial_utils import ( + add_ur5_gripper_robot, + create_toppra_motion_generator, + create_tutorial_argument_parser, + create_tutorial_simulation, + get_hand_open_close_qpos, + prepare_tutorial_scene, + replay_trajectory, + run_tutorial, +) + +MICROWAVE_ASSET = "MicrowaveOven/microwave_oven_with_inertials.urdf" +KNOB_LINK_NAME = "cap_1" +MICROWAVE_POSITION = (-1.0, -0.30, 0.4) +MICROWAVE_ORIENTATION = (0.0, 0.0, 90) # degrees +TWIST_SAMPLE_INTERVAL = 140 +HAND_INTERP_STEPS = 12 +POST_TRAJECTORY_STEPS = 240 +RIGID_KNOB_POSITION = (-0.7, -0.00, 0.70) +RIGID_KNOB_SIZE = (0.05, 0.05, 0.05) +KNOB_SCENE_ENTITY_ID = "twist-target" +KNOB_AXIS_ORIGIN = (0.0, 0.0, 0.0) + + +def parse_arguments() -> argparse.Namespace: + """Parse command-line arguments for the Twist tutorial.""" + parser = create_tutorial_argument_parser( + "Demonstrate Twist on an articulation-link or rigid knob.", + features=("visualize_axes",), + ) + parser.add_argument("--twist_angle", type=float, default=-0.7853981634) + parser.add_argument( + "--rigid_object", + action="store_true", + help="Use a standalone rigid knob instead of the microwave link.", + ) + return parser.parse_args() + + +def create_microwave(sim) -> Articulation: + """Create the fixed-base microwave articulation used by the demo.""" + microwave = sim.add_articulation( + cfg=ArticulationCfg( + uid="microwave", + fpath=get_data_path(MICROWAVE_ASSET), + init_pos=MICROWAVE_POSITION, + init_rot=MICROWAVE_ORIENTATION, + drive_pros=JointDrivePropertiesCfg( + stiffness=1e-3, damping=1e2, max_effort=1e-2 + ), + fix_base=True, + ) + ) + sim.update(step=10) + return microwave + + +def create_rigid_knob(sim) -> RigidObject: + """Create the standalone static rigid knob used by the optional demo.""" + knob = sim.add_rigid_object( + cfg=RigidObjectCfg( + uid="rigid_knob", + shape=CubeCfg(size=list(RIGID_KNOB_SIZE)), + body_type="static", + init_pos=RIGID_KNOB_POSITION, + ) + ) + sim.update(step=10) + return knob + + +def create_knob_semantics( + target: Articulation | RigidObject, +) -> tuple[ObjectSemantics, torch.Tensor]: + """Create twist semantics for an articulation-link or rigid knob.""" + if isinstance(target, Articulation): + vertices, _ = target.get_link_vert_face(KNOB_LINK_NAME) + target_pose = target.get_link_pose(KNOB_LINK_NAME, to_matrix=True) + affordance = TwistAffordance( + grasp_position=_mesh_center(vertices), + # The cap_1 revolute axis passes through its link-frame origin. + axis_origin=KNOB_AXIS_ORIGIN, + twist_axis=torch.tensor([0.0, 0.0, -1.0], device=target.device), + ) + label = "microwave_power_knob" + else: + vertices = target.get_vertices(env_ids=[0], scale=True)[0] + target_pose = target.get_local_pose(to_matrix=True) + affordance = TwistAffordance( + grasp_position=_mesh_center(vertices), + axis_origin=KNOB_AXIS_ORIGIN, + twist_axis=torch.tensor([-1.0, 0.0, 0.0], device=target.device), + ) + label = "rigid_knob" + return ( + ObjectSemantics( + label=label, + geometry={}, + entity_id=KNOB_SCENE_ENTITY_ID, + affordance=affordance, + ), + target_pose, + ) + + +def _mesh_center(vertices: torch.Tensor) -> tuple[float, float, float]: + """Return an explicit local gripper-center point for a knob mesh.""" + center = torch.as_tensor(vertices, dtype=torch.float32).mean(dim=0) + return tuple(float(value) for value in center) + + +def main() -> None: + """Plan and replay Twist for the selected target object type.""" + args = parse_arguments() + sim = create_tutorial_simulation(args) + robot = add_ur5_gripper_robot( + sim, init_qpos=[0.0, -1.57, 1.57, -3.14, -1.57, 0.0, 0.0, 0.0] + ) + target = create_rigid_knob(sim) if args.rigid_object else create_microwave(sim) + hand_open, hand_close = get_hand_open_close_qpos(robot) + motion_gen = create_toppra_motion_generator(robot) + semantics, target_pose = create_knob_semantics(target) + + engine = AtomicActionEngine( + motion_generator=motion_gen, + control_profiles={ + "hand": ControlPartCommandProfile.joint_positions( + open=hand_open, + grasp=hand_close, + ) + }, + ) + wait_for_user = prepare_tutorial_scene( + sim, + args, + "Inspect the knob target, then press Enter to plan Twist...", + ) + + compiled = engine.compile( + ( + engine.make_invocation( + "twist", + TwistGoal( + semantics, + SceneEntityPose(KNOB_SCENE_ENTITY_ID), + ), + control_parts={"primary": {"motion": "arm", "grasp": "hand"}}, + motion_policy=MotionPolicy(sample_count=TWIST_SAMPLE_INTERVAL), + skill_options=TwistOptions( + hand_interp_steps=HAND_INTERP_STEPS, + pre_grasp_distance=0.12, + twist_angle=args.twist_angle, + ), + ), + ), + context=engine.initial_context( + scene=SceneSnapshot( + timestamp=0.0, + version=0, + entities={KNOB_SCENE_ENTITY_ID: EntityState(target_pose)}, + ), + control_dt=sim.sim_config.physics_dt, + ), + ) + if not compiled.plan_success.all(): + logger.log_warning("Failed to plan the Twist demo trajectory.") + return + + if isinstance(target, RigidObject): + focus_pose = target.get_local_pose(to_matrix=True) + elif isinstance(target, Articulation): + focus_pose = target.get_link_pose(KNOB_LINK_NAME, to_matrix=True) + else: + raise ValueError("Unsupported target type for Press demo.") + focus_position = [focus_pose[0, 0, 3], focus_pose[0, 1, 3], focus_pose[0, 2, 3]] + camera_position = [ + focus_position[0] + 0.3, + focus_position[1] + 0.3, + focus_position[2] + 0.3, + ] + look_at = [camera_position, focus_position, [0, 0, 1]] + if wait_for_user: + input("Press Enter to replay the Twist demo...") + replay_trajectory( + sim, + robot, + compiled.trajectory, + args, + video_prefix=( + "twist_rigid_knob_auto_play" + if args.rigid_object + else "twist_microwave_knob_auto_play" + ), + hold_steps=POST_TRAJECTORY_STEPS, + look_at=look_at, + ) + if wait_for_user: + input("Press Enter to exit the simulation...") + + +if __name__ == "__main__": + run_tutorial(main) diff --git a/scripts/tutorials/grasp/grasp_generator.py b/scripts/tutorials/grasp/grasp_generator.py index b7282601d..ed0fdaae6 100644 --- a/scripts/tutorials/grasp/grasp_generator.py +++ b/scripts/tutorials/grasp/grasp_generator.py @@ -171,7 +171,7 @@ def create_obj(sim: SimulationManager): def get_grasp_traj(sim: SimulationManager, robot: Robot, grasp_xpos: torch.Tensor): - n_envs = sim.num_envs + num_envs = sim.num_envs rest_arm_qpos = robot.get_qpos("arm") approach_xpos = grasp_xpos.clone() @@ -201,12 +201,12 @@ def get_grasp_traj(sim: SimulationManager, robot: Robot, grasp_xpos: torch.Tenso ) hand_trajectory = torch.cat( [ - hand_open_qpos[None, None, :].repeat(n_envs, 1, 1), - hand_open_qpos[None, None, :].repeat(n_envs, 1, 1), - hand_open_qpos[None, None, :].repeat(n_envs, 1, 1), - hand_close_qpos[None, None, :].repeat(n_envs, 1, 1), - hand_close_qpos[None, None, :].repeat(n_envs, 1, 1), - hand_close_qpos[None, None, :].repeat(n_envs, 1, 1), + hand_open_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_open_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_open_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_close_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_close_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_close_qpos[None, None, :].repeat(num_envs, 1, 1), ], dim=1, ) diff --git a/scripts/tutorials/sim/open_drawer.py b/scripts/tutorials/sim/open_drawer.py new file mode 100644 index 000000000..9e9e9ec3b --- /dev/null +++ b/scripts/tutorials/sim/open_drawer.py @@ -0,0 +1,532 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Use a Franka Panda and MotionGenerator to open a passive drawer.""" + +from __future__ import annotations + +import argparse +from collections.abc import Sequence + +import torch + +from embodichain.data import get_data_path +from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.cfg import ( + ArticulationCfg, + JointDrivePropertiesCfg, + RenderCfg, + RigidBodyAttributesCfg, +) +from embodichain.lab.sim.objects import Articulation, Robot +from embodichain.lab.sim.planners import ( + MotionGenCfg, + MotionGenerator, + MotionGenOptions, + PlanState, + ToppraPlannerCfg, + ToppraPlanOptions, + TrajectorySampleMethod, +) +from embodichain.lab.sim.robots import FrankaPandaCfg +from embodichain.lab.visualization import visualization_cfg_from_args + +__all__ = [ + "create_scene", + "generate_arm_trajectory", + "get_handle_grasp_pose", + "main", + "move_gripper", + "open_drawer", + "play_arm_trajectory", + "solve_ik_waypoints", +] + +ARM_NAME = "arm" +HAND_NAME = "hand" +HANDLE_LINK_NAME = "handle_xpos" +DRAWER_ASSET = "SlidingBoxDrawer/SlidingBoxDrawer.urdf" + +APPROACH_DISTANCE = 0.10 +PULL_DISTANCE = 0.16 +DRAWER_SUCCESS_THRESHOLD = 0.10 +HALF_OPEN_FRACTION = 0.5 +HALF_OPEN_TOLERANCE = 0.02 +RECORD_WIDTH = 1280 +RECORD_HEIGHT = 720 +RECORD_LOOK_AT = ( + (-0.72, -1.05, 1.0), + (0.45, 0.0, 0.52), + (0.0, 0.0, 1.0), +) + + +def create_scene(sim: SimulationManager) -> tuple[Robot, Articulation]: + """Add a Franka Panda and a passive sliding drawer to the scene. + + Args: + sim: Simulation manager that owns the scene. + + Returns: + The Franka robot and drawer articulation. + + Raises: + RuntimeError: If the robot could not be added. + """ + # Add the existing Franka configuration. Higher contact friction helps the + # fingertips retain the narrow drawer handle during the pull phase. + robot_cfg = FrankaPandaCfg.from_dict( + { + "uid": "tutorial_franka", + "robot_type": "panda", + "attrs": { + "static_friction": 1.0, + "dynamic_friction": 1.0, + }, + } + ) + robot = sim.add_robot(cfg=robot_cfg) + if robot is None: + raise RuntimeError("Failed to add the Franka Panda robot.") + + # Keep the drawer base fixed while leaving its prismatic joint passive. The + # 180-degree yaw makes the drawer's opening direction point toward Franka. + drawer = sim.add_articulation( + cfg=ArticulationCfg( + uid="drawer", + fpath=get_data_path(DRAWER_ASSET), + init_pos=(0.72, 0.0, 0.42), + init_rot=(0.0, 0.0, 180.0), + fix_base=True, + drive_pros=JointDrivePropertiesCfg(drive_type="none"), + attrs=RigidBodyAttributesCfg( + static_friction=1.0, + dynamic_friction=1.0, + ), + ) + ) + return robot, drawer + + +def solve_ik_waypoints( + robot: Robot, + target_poses: Sequence[torch.Tensor], + start_qpos: torch.Tensor, +) -> list[torch.Tensor]: + """Solve sparse Cartesian waypoints with the previous solution as the seed. + + Args: + robot: Robot whose arm solver is used. + target_poses: Batched target TCP poses, each shaped ``(B, 4, 4)``. + start_qpos: Batched initial arm positions shaped ``(B, arm_dof)``. + + Returns: + Batched arm-joint waypoints in the same order as ``target_poses``. + + Raises: + RuntimeError: If IK fails for any environment. + """ + qpos_seed = start_qpos + qpos_waypoints: list[torch.Tensor] = [] + for waypoint_index, target_pose in enumerate(target_poses): + success, qpos = robot.compute_ik( + pose=target_pose, + joint_seed=qpos_seed, + name=ARM_NAME, + ) + failed_env_ids = ( + torch.nonzero(~success.bool(), as_tuple=False).flatten().cpu().tolist() + ) + if failed_env_ids: + raise RuntimeError( + f"IK failed at waypoint {waypoint_index} for environments " + f"{failed_env_ids}." + ) + qpos_waypoints.append(qpos) + qpos_seed = qpos + return qpos_waypoints + + +def generate_arm_trajectory( + motion_generator: MotionGenerator, + qpos_waypoints: Sequence[torch.Tensor], + start_qpos: torch.Tensor, + sample_count: int, +) -> torch.Tensor: + """Time-parameterize arm waypoints with MotionGenerator and TOPPRA. + + Args: + motion_generator: Motion generator bound to the Franka robot. + qpos_waypoints: Batched arm-joint targets. + start_qpos: Batched starting arm positions. + sample_count: Number of trajectory samples returned by TOPPRA. + + Returns: + Joint positions shaped ``(B, sample_count, arm_dof)``. + + Raises: + ValueError: If no target waypoint is supplied. + RuntimeError: If trajectory generation fails. + """ + if not qpos_waypoints: + raise ValueError("qpos_waypoints must contain at least one target.") + + result = motion_generator.generate( + target_states=[PlanState.from_qpos(qpos) for qpos in qpos_waypoints], + options=MotionGenOptions( + control_part=ARM_NAME, + start_qpos=start_qpos, + is_interpolate=True, + is_linear=False, + interpolate_nums=8, + plan_opts=ToppraPlanOptions( + constraints={ + "velocity": 0.35, + "acceleration": 0.75, + }, + sample_method=TrajectorySampleMethod.QUANTITY, + sample_interval=sample_count, + ), + ), + ) + if result.positions is None or not result.is_all_success(): + raise RuntimeError("MotionGenerator failed to produce an arm trajectory.") + return result.positions + + +def play_arm_trajectory( + sim: SimulationManager, + robot: Robot, + trajectory: torch.Tensor, + *, + physics_steps_per_waypoint: int = 4, +) -> None: + """Send a planned arm trajectory to the robot's position drives. + + Args: + sim: Simulation manager to advance. + robot: Franka robot to control. + trajectory: Batched joint positions shaped ``(B, N, arm_dof)``. + physics_steps_per_waypoint: Physics updates between consecutive targets. + """ + for qpos in trajectory.unbind(dim=1): + robot.set_qpos(qpos=qpos, name=ARM_NAME) + sim.update(step=physics_steps_per_waypoint) + + +def move_gripper( + sim: SimulationManager, + robot: Robot, + target_qpos: torch.Tensor, + *, + num_steps: int = 40, +) -> None: + """Interpolate the gripper from its current position to a target. + + Args: + sim: Simulation manager to advance. + robot: Franka robot to control. + target_qpos: Batched gripper target shaped ``(B, hand_dof)``. + num_steps: Number of interpolation samples. + """ + start_qpos = robot.get_qpos(name=HAND_NAME) + interpolation = torch.linspace( + 0.0, + 1.0, + steps=num_steps, + dtype=start_qpos.dtype, + device=start_qpos.device, + ) + for alpha in interpolation: + robot.set_qpos( + qpos=torch.lerp(start_qpos, target_qpos, alpha), + name=HAND_NAME, + ) + sim.update(step=4) + + +def get_handle_grasp_pose(drawer: Articulation) -> torch.Tensor: + """Return the handle frame with the gripper rolled 90 degrees. + + The rotation is applied around the TCP's local Z axis, preserving the + approach and pull direction while rotating the finger-closing direction. + + Args: + drawer: Drawer articulation that owns the handle link. + + Returns: + Batched grasp poses shaped ``(B, 4, 4)``. + """ + grasp_pose = drawer.get_link_pose(HANDLE_LINK_NAME, to_matrix=True) + quarter_turn_about_tcp_z = grasp_pose.new_tensor( + [ + [0.0, -1.0, 0.0], + [1.0, 0.0, 0.0], + [0.0, 0.0, 1.0], + ] + ) + grasp_pose[:, :3, :3] = grasp_pose[:, :3, :3] @ quarter_turn_about_tcp_z + return grasp_pose + + +def open_drawer( + sim: SimulationManager, + robot: Robot, + drawer: Articulation, + motion_generator: MotionGenerator, + *, + wait_for_input: bool = True, +) -> torch.Tensor: + """Pull the drawer open, then push it halfway closed. + + Args: + sim: Simulation manager to advance. + robot: Franka robot used for manipulation. + drawer: Passive drawer articulation. + motion_generator: Motion generator bound to ``robot``. + wait_for_input: Whether to wait for Enter before executing trajectories. + + Returns: + Final drawer joint positions shaped ``(B, drawer_dof)``. + + Raises: + RuntimeError: If the drawer does not open or return halfway as expected. + """ + hand_limits = robot.get_qpos_limits(name=HAND_NAME) + hand_open_qpos = hand_limits[..., 1] + hand_closed_qpos = hand_limits[..., 0] + move_gripper(sim, robot, hand_open_qpos, num_steps=20) + + # Finish tool initialization before establishing the task's initial state. + # This also clears any startup contact impulse from opening the fingers. + drawer.reset() + sim.update(step=5) + + # Roll the asset's handle frame 90 degrees around TCP Z. Its approach axis + # stays unchanged while the fingers rotate to close vertically on the handle. + grasp_pose = get_handle_grasp_pose(drawer) + approach_pose = grasp_pose.clone() + approach_pose[:, :3, 3] -= grasp_pose[:, :3, 2] * APPROACH_DISTANCE + + start_qpos = robot.get_qpos(name=ARM_NAME) + approach_waypoints = solve_ik_waypoints( + robot, + target_poses=[approach_pose, grasp_pose], + start_qpos=start_qpos, + ) + approach_trajectory = generate_arm_trajectory( + motion_generator, + qpos_waypoints=approach_waypoints, + start_qpos=start_qpos, + sample_count=60, + ) + if wait_for_input: + input("[READY]: Trajectory planned. Press Enter to start execution...") + play_arm_trajectory(sim, robot, approach_trajectory) + + # Close around the handle, then allow contacts to settle before pulling. + move_gripper(sim, robot, hand_closed_qpos) + sim.update(step=10) + + # Re-read the live handle frame after grasping. Pulling along its -Z axis + # follows the drawer's prismatic joint toward Franka. + grasped_handle_pose = get_handle_grasp_pose(drawer) + pull_pose = grasped_handle_pose.clone() + pull_pose[:, :3, 3] -= grasped_handle_pose[:, :3, 2] * PULL_DISTANCE + + pull_start_qpos = robot.get_qpos(name=ARM_NAME) + pull_waypoints = solve_ik_waypoints( + robot, + target_poses=[pull_pose], + start_qpos=pull_start_qpos, + ) + pull_trajectory = generate_arm_trajectory( + motion_generator, + qpos_waypoints=pull_waypoints, + start_qpos=pull_start_qpos, + sample_count=80, + ) + play_arm_trajectory( + sim, + robot, + pull_trajectory, + physics_steps_per_waypoint=5, + ) + sim.update(step=50) + + pulled_opening = drawer.get_qpos()[:, 0].clone() + print( + "[INFO]: Drawer opening after pull (m): " + f"{pulled_opening.detach().cpu().tolist()}", + flush=True, + ) + if not torch.all(pulled_opening >= DRAWER_SUCCESS_THRESHOLD).item(): + raise RuntimeError( + "The drawer did not open far enough through gripper contact. " + f"Expected at least {DRAWER_SUCCESS_THRESHOLD:.2f} m." + ) + + # Push the drawer back by half of its measured opening. Moving along the + # handle frame's +Z axis reverses the pull while the gripper stays closed. + half_open_target = pulled_opening * HALF_OPEN_FRACTION + push_distance = pulled_opening - half_open_target + pushed_handle_pose = get_handle_grasp_pose(drawer) + push_pose = pushed_handle_pose.clone() + push_pose[:, :3, 3] += pushed_handle_pose[:, :3, 2] * push_distance.unsqueeze(-1) + + push_start_qpos = robot.get_qpos(name=ARM_NAME) + push_waypoints = solve_ik_waypoints( + robot, + target_poses=[push_pose], + start_qpos=push_start_qpos, + ) + push_trajectory = generate_arm_trajectory( + motion_generator, + qpos_waypoints=push_waypoints, + start_qpos=push_start_qpos, + sample_count=50, + ) + play_arm_trajectory( + sim, + robot, + push_trajectory, + physics_steps_per_waypoint=5, + ) + sim.update(step=50) + + drawer_qpos = drawer.get_qpos() + final_opening = drawer_qpos[:, 0] + print( + "[INFO]: Drawer opening after half push (m): " + f"{final_opening.detach().cpu().tolist()}", + flush=True, + ) + if not torch.all( + torch.abs(final_opening - half_open_target) <= HALF_OPEN_TOLERANCE + ).item(): + raise RuntimeError( + "The drawer did not return to half of its pulled opening. " + f"Expected an error no greater than {HALF_OPEN_TOLERANCE:.2f} m." + ) + return drawer_qpos + + +def main() -> None: + """Run the Franka drawer-manipulation tutorial.""" + parser = argparse.ArgumentParser( + description="Use a Franka Panda and MotionGenerator to open a drawer." + ) + add_env_launcher_args_to_parser(parser) + parser.add_argument( + "--hold-steps", + type=int, + default=100, + help="Physics steps to hold the final open-drawer pose before exiting.", + ) + parser.add_argument( + "--auto-start", + action="store_true", + help="Execute trajectories without waiting for Enter.", + ) + parser.add_argument( + "--record-save-path", + type=str, + default=None, + help="Optional MP4 path for recording from a fixed headless camera.", + ) + parser.add_argument( + "--record-fps", + type=int, + default=30, + help="Frames per second for headless recording.", + ) + args = parser.parse_args() + if args.num_envs < 1: + parser.error("--num_envs must be at least 1") + if args.hold_steps < 0: + parser.error("--hold-steps must be non-negative") + if args.record_fps < 1: + parser.error("--record-fps must be at least 1") + if args.record_save_path is not None and not args.headless: + parser.error("--record-save-path requires --headless") + + sim = SimulationManager( + SimulationManagerCfg( + width=RECORD_WIDTH, + height=RECORD_HEIGHT, + headless=args.headless, + sim_device=args.device, + num_envs=args.num_envs, + arena_space=args.arena_space, + physics_dt=1.0 / 100.0, + render_cfg=RenderCfg(renderer=args.renderer), + visualization=visualization_cfg_from_args(args), + ) + ) + + try: + robot, drawer = create_scene(sim) + + if sim.is_use_gpu_physics: + sim.init_gpu_physics() + if not args.headless and not args.viser: + sim.open_window() + + sim.update(step=5) + motion_generator = MotionGenerator( + cfg=MotionGenCfg( + planner_cfg=ToppraPlannerCfg( + robot_uid=robot.uid, + # Keep this small tutorial deterministic across platforms. + max_workers=1, + ), + ) + ) + + if args.record_save_path is not None: + if not sim.start_window_record( + save_path=args.record_save_path, + fps=args.record_fps, + max_memory=2048, + video_prefix="open_drawer_headless", + look_at=RECORD_LOOK_AT, + use_sim_time=True, + ): + raise RuntimeError("Failed to start headless recording.") + + print( + f"[INFO]: Opening drawers in {sim.num_envs} environment(s).", + flush=True, + ) + open_drawer( + sim, + robot, + drawer, + motion_generator, + wait_for_input=not args.auto_start, + ) + if args.hold_steps: + sim.update(step=args.hold_steps) + finally: + if sim.is_window_recording(): + sim.stop_window_record() + sim.wait_window_record_saves() + sim.destroy() + + +if __name__ == "__main__": + main() diff --git a/tests/agents/mllm/test_expert_program.py b/tests/agents/mllm/test_expert_program.py new file mode 100644 index 000000000..1a45b4f65 --- /dev/null +++ b/tests/agents/mllm/test_expert_program.py @@ -0,0 +1,432 @@ +# ---------------------------------------------------------------------------- +# 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 strict MLLM Expert Program frontend.""" + +from __future__ import annotations + +from collections.abc import Iterable +import json + +import pytest + +from embodichain.agents.mllm import ( + compile_mllm_expert_program, + decode_mllm_expert_program, +) +from embodichain.lab.gym.envs.expert_program import ( + CompiledProgram, + EnvironmentStepClock, + ExpertProgramCompileError, + ExpertProgramDecodeError, + ExpertProgramEnvironmentAdapter, + ExpertProgramIntegrationCfg, + PlanningObservationPort, + decode_expert_program, +) +from embodichain.lab.sim.atomic_actions import AtomicActionEngine, EntityState +from embodichain.lab.sim.skills import ( + EffectEvidenceProvider, + Pick, + RobotSkillProfile, + SceneEntityRegistration, + SceneObjectRef, + SceneRegistry, +) + + +def _integration() -> ExpertProgramIntegrationCfg: + """Return the exact trusted integration selected by the host.""" + return ExpertProgramIntegrationCfg( + robot_profile="test_robot", + scene_registry="test_scene", + runtime_preset="safe", + ) + + +def _invoke(call: dict[str, object]) -> dict[str, object]: + """Wrap one semantic call in an Expert Program invoke node.""" + return {"kind": "invoke", "call": call} + + +def _model_data( + call: dict[str, object] | None = None, + *, + schema_version: int = 1, + program: dict[str, object] | None = None, +) -> dict[str, object]: + """Build the integration-free JSON envelope exposed to the model.""" + if call is None: + call = {"kind": "pick", "object": "cube"} + return { + "schema_version": schema_version, + "program_id": "model_program", + "targets": {}, + "program": _invoke(call) if program is None else program, + } + + +def _model_json( + call: dict[str, object] | None = None, + *, + schema_version: int = 1, + program: dict[str, object] | None = None, +) -> str: + """Serialize one integration-free model response.""" + return json.dumps( + _model_data( + call, + schema_version=schema_version, + program=program, + ) + ) + + +class _UnusedStateProvider: + """Satisfy the static scene contract without allowing live observation.""" + + def observe( + self, + *, + timestamp: float, + env_ids: object, + ) -> EntityState: + """Fail if provider-free compilation accidentally observes the scene.""" + del timestamp, env_ids + raise AssertionError("Provider-free compilation must not observe the scene.") + + +class _CompileOnlyFactory: + """Expose only the scene snapshot needed by adapter compilation.""" + + scene_registry_id = "test_scene" + robot_profile_id = "test_robot" + + def __init__(self) -> None: + self.scene_registry_calls = 0 + + def create_scene_registry(self) -> SceneRegistry: + """Return one canonical object registration and count compilation.""" + self.scene_registry_calls += 1 + return SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_UnusedStateProvider(), + semantic_type="cube", + ), + ) + ) + + def create_robot_skill_profile(self) -> RobotSkillProfile: + """Reject runtime assembly in this compile-only test factory.""" + raise AssertionError("MLLM frontend compilation must not assemble a runtime.") + + def create_atomic_action_engine( + self, + profile: RobotSkillProfile, + ) -> AtomicActionEngine: + """Reject engine creation in this compile-only test factory.""" + del profile + raise AssertionError("MLLM frontend compilation must not create an engine.") + + def create_planning_observation_provider( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + clock: EnvironmentStepClock, + ) -> PlanningObservationPort: + """Reject observation-port creation during provider-free compilation.""" + del scene_registry, engine, clock + raise AssertionError("MLLM frontend compilation must not create live ports.") + + def create_effect_evidence_providers( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + observation_provider: PlanningObservationPort, + ) -> Iterable[EffectEvidenceProvider]: + """Reject evidence-provider creation during provider-free compilation.""" + del scene_registry, engine, observation_provider + raise AssertionError("MLLM frontend compilation must not create live ports.") + + +def _adapter(factory: _CompileOnlyFactory) -> ExpertProgramEnvironmentAdapter: + """Create the existing production adapter around the compile-only factory.""" + return ExpertProgramEnvironmentAdapter(factory, step_dt=0.02) + + +def test_decoder_injects_exact_host_integration() -> None: + config = decode_mllm_expert_program( + _model_json(), + integration=_integration(), + ) + + assert config.integration.robot_profile == "test_robot" + assert config.integration.scene_registry == "test_scene" + assert config.integration.runtime_preset == "safe" + + +def test_decoder_rejects_model_controlled_integration() -> None: + response = _model_data() + response["integration"] = { + "robot_profile": "attacker_robot", + "scene_registry": "attacker_scene", + "runtime_preset": "unsafe", + } + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_mllm_expert_program( + json.dumps(response), + integration=_integration(), + ) + + assert error.value.code == "model_controlled_integration" + assert error.value.path == ("integration",) + + +def test_decoder_rejects_version_two_parallel_program() -> None: + parallel = { + "kind": "parallel", + "branches": [ + _invoke({"kind": "pick", "object": "cube"}), + _invoke({"kind": "pick", "object": "cube"}), + ], + "barrier": {"kind": "barrier", "name": "join"}, + } + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_mllm_expert_program( + _model_json(schema_version=2, program=parallel), + integration=_integration(), + ) + + assert error.value.code == "mllm_schema_version_not_allowed" + assert error.value.path == ("schema_version",) + + +def test_decoder_rejects_registered_semantic_calls() -> None: + registered = { + "kind": "registered", + "call_id": "vendor.inspect", + "schema_version": 1, + "arguments": {}, + } + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_mllm_expert_program( + _model_json(registered), + integration=_integration(), + ) + + assert error.value.code == "mllm_call_not_allowed" + assert error.value.path == ("program", "call", "kind") + + +@pytest.mark.parametrize( + "call", + [ + {"kind": "pick", "object": "cube", "resources": {"primary": "left"}}, + { + "kind": "place", + "object": "cube", + "on": "tray", + "resources": {"primary": "left"}, + }, + { + "kind": "hand_over", + "object": "cube", + "resources": {"destination": "right"}, + }, + { + "kind": "operate_articulation", + "articulation": "drawer", + "target": "open", + "resources": {"primary": "left"}, + }, + ], +) +def test_decoder_rejects_explicit_nonempty_resource_overrides( + call: dict[str, object], +) -> None: + with pytest.raises(ExpertProgramDecodeError) as error: + decode_mllm_expert_program( + _model_json(call), + integration=_integration(), + ) + + assert error.value.code == "mllm_resource_override_not_allowed" + assert error.value.path == ("program", "call", "resources") + + +def test_decoder_allows_explicit_empty_resources() -> None: + config = decode_mllm_expert_program( + _model_json({"kind": "pick", "object": "cube", "resources": {}}), + integration=_integration(), + ) + + assert config.program.call.resources == {} # type: ignore[union-attr] + + +def test_decoder_rejects_removed_handover_receiver_alias() -> None: + with pytest.raises(ExpertProgramDecodeError) as error: + decode_mllm_expert_program( + _model_json( + { + "kind": "hand_over", + "object": "cube", + "receiver": "right", + } + ), + integration=_integration(), + ) + + assert error.value.code == "unknown_field" + assert error.value.path == ("program", "call", "receiver") + + +def test_decoder_rejects_explicit_articulation_motion_target() -> None: + with pytest.raises(ExpertProgramDecodeError) as error: + decode_mllm_expert_program( + _model_json( + { + "kind": "operate_articulation", + "articulation": "drawer", + "target_position": 1_000_000.0, + "target_displacement": 1_000_000.0, + } + ), + integration=_integration(), + ) + + assert error.value.code == "mllm_articulation_target_not_allowed" + assert error.value.path == ("program", "call", "target_position") + + +def test_decoder_allows_named_articulation_target() -> None: + config = decode_mllm_expert_program( + _model_json( + { + "kind": "operate_articulation", + "articulation": "drawer", + "target": "open", + } + ), + integration=_integration(), + ) + + assert config.program.call.target == "open" # type: ignore[union-attr] + + +@pytest.mark.parametrize( + ("call", "code"), + [ + ( + {"kind": "pick", "object": "env.robot.control_parts"}, + "environment_traversal", + ), + ({"kind": "pick", "object": "eval(1 + 1)"}, "executable_expression"), + ], +) +def test_decoder_reuses_executable_free_value_validation( + call: dict[str, object], + code: str, +) -> None: + with pytest.raises(ExpertProgramDecodeError) as error: + decode_mllm_expert_program( + _model_json(call), + integration=_integration(), + ) + + assert error.value.code == code + + +@pytest.mark.parametrize( + ("response", "code"), + [ + ("```json\n{}\n```", "invalid_json"), + ('{"schema_version": 1, "schema_version": 1}', "duplicate_json_key"), + ('{"schema_version": NaN}', "non_finite_number"), + ('{"schema_version": 1e400}', "non_finite_number"), + ], +) +def test_decoder_propagates_strict_json_failures(response: str, code: str) -> None: + with pytest.raises(ExpertProgramDecodeError) as error: + decode_mllm_expert_program(response, integration=_integration()) + + assert error.value.code == code + + +def test_compile_frontend_reuses_existing_adapter_and_compiler() -> None: + factory = _CompileOnlyFactory() + adapter = _adapter(factory) + response = _model_json() + + model_compiled = compile_mllm_expert_program( + response, + adapter=adapter, + integration=_integration(), + ) + direct_data = _model_data() + direct_data["integration"] = { + "robot_profile": "test_robot", + "scene_registry": "test_scene", + "runtime_preset": "safe", + } + direct_compiled = adapter.compile(decode_expert_program(direct_data)) + + model_call = list(model_compiled)[0].calls[0].call + direct_call = list(direct_compiled)[0].calls[0].call + assert type(model_compiled) is CompiledProgram + assert type(model_call) is Pick + assert type(direct_call) is Pick + assert model_call.object.entity_id == direct_call.object.entity_id == "cube" + assert factory.scene_registry_calls == 2 + + +def test_policy_failure_does_not_touch_adapter_or_runtime() -> None: + factory = _CompileOnlyFactory() + adapter = _adapter(factory) + registered = { + "kind": "registered", + "call_id": "vendor.inspect", + "schema_version": 1, + } + + with pytest.raises(ExpertProgramDecodeError): + compile_mllm_expert_program( + _model_json(registered), + adapter=adapter, + integration=_integration(), + ) + + assert factory.scene_registry_calls == 0 + + +def test_compile_frontend_rejects_unknown_scene_reference() -> None: + factory = _CompileOnlyFactory() + + with pytest.raises(ExpertProgramCompileError) as error: + compile_mllm_expert_program( + _model_json({"kind": "pick", "object": "missing"}), + adapter=_adapter(factory), + integration=_integration(), + ) + + assert error.value.code == "unknown_scene_reference" + assert error.value.path == ("program", "call", "object") diff --git a/tests/benchmark/expert_program/__init__.py b/tests/benchmark/expert_program/__init__.py new file mode 100644 index 000000000..b1ad75924 --- /dev/null +++ b/tests/benchmark/expert_program/__init__.py @@ -0,0 +1,21 @@ +# ---------------------------------------------------------------------------- +# 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 Expert Program benchmarks.""" + +from __future__ import annotations + +__all__: list[str] = [] diff --git a/tests/benchmark/expert_program/test_cube_physical_recovery_sim.py b/tests/benchmark/expert_program/test_cube_physical_recovery_sim.py new file mode 100644 index 000000000..f1b64384a --- /dev/null +++ b/tests/benchmark/expert_program/test_cube_physical_recovery_sim.py @@ -0,0 +1,241 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Live physical held-object loss and workflow-recovery regression coverage.""" + +from __future__ import annotations + +import argparse +from pathlib import Path +import subprocess +import sys +from typing import Any + +import pytest +import torch + +from embodichain.lab.gym.envs.demo import ( + DemoEpisodeResult, + ProcessedEnvAction, + execute_demo_episode, +) +from embodichain_tasks.configs import get_config_path +from scripts.benchmark.expert_program.demo_success import ( + DemoSuccessCase, + _build_parser, + load_raw_trials, + run_gym_demo_success_benchmark, +) + +_REPOSITORY_ROOT = Path(__file__).resolve().parents[3] +_CUBE_GYM_CONFIG = get_config_path("gym/multi_segments/cube_pick_place.json") +_CUBE_EXPERT_PROGRAM = get_config_path( + "expert_program/multi_segments/repeated_cube_pick_place.yaml" +) +_FAULT_SEGMENT_INDEX = 1 +_FAULT_CALL_INDEX = 1 +_FAULT_OPEN_STEPS = 20 +_SUBPROCESS_TIMEOUT_SECONDS = 240 + + +class _GripperOpenFaultEnvironment: + """Replace a bounded command window with a real gripper-open command. + + The wrapper changes only the controller-ready action before the ordinary + ``env.step()`` call. It never writes an object pose, velocity, attachment, + constraint, or symbolic task state. + """ + + def __init__(self, environment: Any) -> None: + self._environment = environment + target = getattr(environment, "unwrapped", environment) + self._hand_joint_ids = tuple(target.robot.get_joint_ids(name="hand")) + if not self._hand_joint_ids: + raise ValueError("The cube recovery gate requires hand joint IDs.") + self.injected_open_steps = 0 + + @property + def unwrapped(self) -> Any: + """Return the original task environment for demo lifecycle hooks.""" + return getattr(self._environment, "unwrapped", self._environment) + + def __getattr__(self, name: str) -> Any: + return getattr(self._environment, name) + + def step(self, action: object) -> object: + """Open the physical gripper during the selected Place approach.""" + if isinstance(action, ProcessedEnvAction): + metadata = action.metadata + should_inject = ( + metadata.get("program_segment_index") == _FAULT_SEGMENT_INDEX + and metadata.get("runtime_call_index") == _FAULT_CALL_INDEX + and metadata.get("bridge_action_kind") == "runtime_command" + and self.injected_open_steps < _FAULT_OPEN_STEPS + ) + if should_inject: + if not isinstance(action.value, torch.Tensor): + raise TypeError("The cube task must emit a tensor action.") + value = action.value.clone() + value[:, self._hand_joint_ids] = 0.0 + action = ProcessedEnvAction( + value=value, + metadata={ + **metadata, + "test_fault": "open_gripper_before_place", + }, + ) + self.injected_open_steps += 1 + return self._environment.step(action) + + +def _execute_fault_episode( + environment: Any, + *, + episode_index: int, +) -> DemoEpisodeResult: + """Execute one episode through the controller-command fault wrapper.""" + fault_environment = _GripperOpenFaultEnvironment(environment) + result = execute_demo_episode( + fault_environment, + episode_index=episode_index, + ) + print(f"injected_open_steps={fault_environment.injected_open_steps}") + return result + + +def _run_fault_subprocess(raw_path: Path, report_path: Path) -> int: + """Create and close the native simulator inside the child process.""" + launcher_args = _build_parser().parse_args( + [ + "--run-simulation", + "--gym_config", + str(_CUBE_GYM_CONFIG), + "--expert-program", + str(_CUBE_EXPERT_PROGRAM), + "--case-id", + "cube_physical_loss_recovery", + "--seeds", + "0", + "--raw-json", + str(raw_path), + "--report", + str(report_path), + "--headless", + "--device", + "cuda", + "--num_envs", + "1", + "--filter_dataset_saving", + ] + ) + run_gym_demo_success_benchmark( + DemoSuccessCase("cube_physical_loss_recovery", (0,)), + launcher_args=launcher_args, + expert_program_path=_CUBE_EXPERT_PROGRAM, + raw_json_path=raw_path, + report_path=report_path, + episode_executor=_execute_fault_episode, + ) + return 0 + + +@pytest.mark.requires_sim +@pytest.mark.slow +@pytest.mark.gpu +def test_physical_cube_loss_triggers_real_reacquisition(tmp_path: Path) -> None: + """Observe physical loss, invalidate state, reacquire, and finish the task.""" + raw_path = tmp_path / "fault_raw.json" + report_path = tmp_path / "fault_report.md" + completed = subprocess.run( + [ + sys.executable, + str(Path(__file__).resolve()), + "--run-fault", + "--raw-json", + str(raw_path), + "--report", + str(report_path), + ], + cwd=_REPOSITORY_ROOT, + capture_output=True, + text=True, + timeout=_SUBPROCESS_TIMEOUT_SECONDS, + check=False, + ) + + assert completed.returncode == 0, completed.stdout + completed.stderr + assert f"injected_open_steps={_FAULT_OPEN_STEPS}" in completed.stdout + trial = load_raw_trials(raw_path)[0] + assert trial.rows[0].success + assert trial.rows[0].terminal_reason == "success" + + segments = trial.episode_result["segments"] + assert isinstance(segments, list) + assert len(segments) == 3 + recovery_segment = segments[_FAULT_SEGMENT_INDEX] + runtime = recovery_segment["metadata"]["runtime"] + assert runtime["status"] == "completed" + assert runtime["masks"]["success"] == [True] + assert runtime["task_state"]["held_objects"] == [] + + failed_place = runtime["calls"][_FAULT_CALL_INDEX] + assert failed_place["semantic_id"] == "place" + assert failed_place["status"] == "failed" + assert failed_place["masks"]["failed"] == [True] + assert {event["kind"] for event in failed_place["events"]} >= { + "held_object_lost", + "recovery_required", + } + physical_failures = [ + effect + for effect in failed_place["effects"] + if effect["boundary"]["kind"] == "in_flight_guard" + and effect["decision"]["failure_mask"] == [True] + ] + assert len(physical_failures) == 1 + physical_failure = physical_failures[0] + assert physical_failure["decision"]["expectations"][0]["contradicted_mask"] == [ + True + ] + assert physical_failure["evidence"]["source.constraint"]["values"] == [True] + assert physical_failure["evidence"]["source.pose"]["valid_mask"] == [True] + + recoveries = runtime["workflow_recoveries"] + assert [recovery["role"] for recovery in recoveries] == [ + "reacquire", + "retry_reacquired", + ] + assert [recovery["attempt_index"] for recovery in recoveries] == [1, 1] + assert [recovery["call"]["semantic_id"] for recovery in recoveries] == [ + "pick", + "place", + ] + assert all(recovery["call"]["status"] == "completed" for recovery in recoveries) + assert recovery_segment["metadata"]["validation"]["accepted_mask"] == [True] + + +def _main(argv: list[str] | None = None) -> int: + """Run only the isolated native-simulation helper mode.""" + parser = argparse.ArgumentParser() + parser.add_argument("--run-fault", action="store_true", required=True) + parser.add_argument("--raw-json", type=Path, required=True) + parser.add_argument("--report", type=Path, required=True) + args = parser.parse_args(argv) + return _run_fault_subprocess(args.raw_json, args.report) + + +if __name__ == "__main__": + raise SystemExit(_main()) diff --git a/tests/benchmark/expert_program/test_demo_success.py b/tests/benchmark/expert_program/test_demo_success.py new file mode 100644 index 000000000..535941a15 --- /dev/null +++ b/tests/benchmark/expert_program/test_demo_success.py @@ -0,0 +1,971 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Pure-Python tests for the no-retry demo-success benchmark.""" + +from __future__ import annotations + +import argparse +from collections import deque +import json +from pathlib import Path + +import pytest + +from embodichain.lab.gym.envs.demo import DemoEpisodeResult, DemoSegmentResult +from scripts.benchmark.expert_program import demo_success as demo_success_module +from scripts.benchmark.expert_program.demo_success import ( + DemoSuccessCase, + DemoSuccessRow, + DemoSuccessTrial, + MemorySnapshot, + aggregate_demo_success_trials, + collect_demo_success_trials, + load_raw_trials, + main, + run_all_benchmarks, + run_gym_demo_success_benchmark, + write_markdown_report, + write_raw_trials, +) + + +class _FakeEnv: + """Record benchmark reset calls without creating a simulation.""" + + def __init__(self, num_envs: int = 1) -> None: + self.num_envs = num_envs + self.reset_calls: list[dict[str, object]] = [] + self.seed: int | None = None + + def reset(self, **kwargs: object) -> None: + self.reset_calls.append(dict(kwargs)) + if "seed" in kwargs: + self.seed = int(kwargs["seed"]) + + +class _PostEpisodeDiscardFailureEnv(_FakeEnv): + """Fail the discard reset after allowing the non-committing seed reset.""" + + def __init__(self) -> None: + super().__init__() + self.non_committing_resets = 0 + + def reset(self, **kwargs: object) -> None: + super().reset(**kwargs) + if kwargs.get("options") == {"save_data": False}: + self.non_committing_resets += 1 + if self.non_committing_resets == 2: + raise RuntimeError("synthetic discard failure") + + +class _EpisodeExecutor: + """Return queued demo results and record one call per seed.""" + + def __init__(self, results: list[DemoEpisodeResult]) -> None: + self.results = deque(results) + self.calls: list[tuple[int | None, int]] = [] + + def __call__(self, env: _FakeEnv, *, episode_index: int) -> DemoEpisodeResult: + self.calls.append((env.seed, episode_index)) + return self.results.popleft() + + +def _result( + successes: tuple[bool, ...], + *, + lengths: tuple[int, ...] | None = None, + reasons: tuple[str, ...] | None = None, + segments: tuple[DemoSegmentResult, ...] = (), +) -> DemoEpisodeResult: + """Build a compact batched result with consistent vector metadata.""" + row_count = len(successes) + row_lengths = lengths or tuple(1 for _ in successes) + row_reasons = reasons or tuple( + "success" if success else "task_incomplete" for success in successes + ) + return DemoEpisodeResult( + episode_index=0, + length=max(row_lengths), + completed=all(successes), + success=successes, + terminated=tuple(successes), + truncated=tuple(False for _ in successes), + terminal_reason="success" if all(successes) else "task_incomplete", + segments=segments, + lengths=row_lengths, + completed_by_env=successes, + terminal_reasons=row_reasons, + ) + + +def _clock(values: list[float]): + """Return a deterministic clock backed by the supplied readings.""" + readings = iter(values) + return lambda: next(readings) + + +def _memory_sampler(values: list[MemorySnapshot]): + """Return a deterministic memory sampler backed by supplied snapshots.""" + snapshots = iter(values) + + def sample(*, reset_gpu_peak: bool = False) -> MemorySnapshot: # noqa: ARG001 + return next(snapshots) + + return sample + + +def test_public_case_and_row_types_validate_and_snapshot_inputs() -> None: + seeds = [3, 5] + segment_failures = ["place:timeout"] + call_failures = ["place:place:failed"] + + case = DemoSuccessCase("cube", seeds) # type: ignore[arg-type] + row = DemoSuccessRow( + env_index=0, + success=False, + terminal_reason="timeout", + length=4, + segment_failure_reasons=segment_failures, # type: ignore[arg-type] + call_failure_keys=call_failures, # type: ignore[arg-type] + ) + seeds.append(7) + segment_failures.append("mutated") + call_failures.append("mutated") + + assert case.seeds == (3, 5) + assert row.segment_failure_reasons == ("place:timeout",) + assert row.call_failure_keys == ("place:place:failed",) + with pytest.raises(TypeError, match="case_id must be a string"): + DemoSuccessCase(7, (1,)) # type: ignore[arg-type] + with pytest.raises(TypeError, match="evaluation seed"): + DemoSuccessCase("cube", (True,)) + with pytest.raises(ValueError, match="env_index must be non-negative"): + DemoSuccessRow(-1, False, "timeout", 0) + with pytest.raises(TypeError, match="success must be a boolean"): + DemoSuccessRow(0, 1, "timeout", 0) # type: ignore[arg-type] + + +def test_public_trial_validates_rows_and_owns_nested_inputs() -> None: + row = DemoSuccessRow(0, True, "success", 2) + rows = [row] + episode_result: dict[str, object] = {"success": [True]} + + trial = DemoSuccessTrial( + case_id="cube", + seed=3, + cost_time_ms=1, + cpu_delta_mb=0, + gpu_delta_mb=0, + peak_gpu_mb=0, + rows=rows, # type: ignore[arg-type] + episode_result=episode_result, + ) + rows.clear() + episode_result["success"] = [False] + + assert trial.rows == (row,) + assert trial.cost_time_ms == 1.0 + assert trial.episode_result == {"success": [True]} + with pytest.raises(ValueError, match="unique contiguous env_index"): + DemoSuccessTrial( + "cube", + 3, + 1.0, + 0.0, + 0.0, + 0.0, + (DemoSuccessRow(1, True, "success", 1),), + {}, + ) + with pytest.raises(TypeError, match="exactly DemoSuccessRow"): + DemoSuccessTrial( + "cube", + 3, + 1.0, + 0.0, + 0.0, + 0.0, + (object(),), # type: ignore[arg-type] + {}, + ) + + +def test_each_seed_executes_once_without_retry_and_discards_data() -> None: + env = _FakeEnv() + executor = _EpisodeExecutor( + [_result((False,)), _result((True,)), _result((False,))] + ) + case = DemoSuccessCase(case_id="drawer", seeds=(11, 22, 33)) + memory_values = [MemorySnapshot(100.0, 10.0, 10.0)] * 6 + + trials = collect_demo_success_trials( + [case], + lambda requested: env, + episode_executor=executor, + clock=_clock([0.0, 0.1, 1.0, 1.2, 2.0, 2.3]), + memory_sampler=_memory_sampler(memory_values), + ) + + assert [call[0] for call in executor.calls] == [11, 22, 33] + assert len(trials) == len(case.seeds) + assert env.reset_calls == [ + {"seed": 11, "options": {"save_data": False}}, + {"options": {"save_data": False}}, + {"seed": 22, "options": {"save_data": False}}, + {"options": {"save_data": False}}, + {"seed": 33, "options": {"save_data": False}}, + {"options": {"save_data": False}}, + ] + + +def test_executor_error_is_counted_and_next_seed_still_executes() -> None: + env = _FakeEnv(num_envs=2) + calls: list[int | None] = [] + + def execute( + env: _FakeEnv, *, episode_index: int + ) -> DemoEpisodeResult: # noqa: ARG001 + calls.append(env.seed) + if env.seed == 7: + raise RuntimeError("synthetic execution failure") + return _result((True, True)) + + trials = collect_demo_success_trials( + [DemoSuccessCase("drawer", (7, 8))], + lambda requested: env, + episode_executor=execute, + clock=_clock([0.0, 0.1, 1.0, 1.1]), + memory_sampler=_memory_sampler([MemorySnapshot(100.0, 0.0, 0.0)] * 4), + ) + + assert calls == [7, 8] + assert [row.terminal_reason for row in trials[0].rows] == [ + "executor_error:RuntimeError", + "executor_error:RuntimeError", + ] + assert [row.length for row in trials[0].rows] == [0, 0] + assert trials[0].episode_result["executor_error"] == { + "type": "RuntimeError", + "message": "synthetic execution failure", + } + assert all(row.success for row in trials[1].rows) + metric = aggregate_demo_success_trials(trials).success_and_metrics[0] + assert metric["attempted"] == 4 + assert metric["successes"] == 2 + assert metric["success_rate"] == pytest.approx(0.5) + assert env.reset_calls[-1] == {"options": {"save_data": False}} + + +def test_executor_error_remains_primary_when_discard_also_fails() -> None: + env = _PostEpisodeDiscardFailureEnv() + + def execute(env: _FakeEnv, *, episode_index: int) -> DemoEpisodeResult: + del env, episode_index + raise ValueError("synthetic executor failure") + + with pytest.raises(ValueError, match="synthetic executor failure") as error: + collect_demo_success_trials( + [DemoSuccessCase("drawer", (7,))], + lambda requested: env, + episode_executor=execute, + clock=_clock([0.0, 0.1]), + memory_sampler=_memory_sampler([MemorySnapshot(100.0, 0.0, 0.0)] * 2), + ) + + assert error.value.__notes__ == [ + "Episode discard also failed: RuntimeError: synthetic discard failure" + ] + assert env.reset_calls == [ + {"seed": 7, "options": {"save_data": False}}, + {"options": {"save_data": False}}, + ] + + +def test_measurement_error_remains_primary_when_discard_also_fails() -> None: + env = _PostEpisodeDiscardFailureEnv() + clock_calls = 0 + + def failing_clock() -> float: + nonlocal clock_calls + clock_calls += 1 + if clock_calls == 2: + raise LookupError("synthetic clock failure") + return 0.0 + + with pytest.raises(LookupError, match="synthetic clock failure") as error: + collect_demo_success_trials( + [DemoSuccessCase("drawer", (7,))], + lambda requested: env, + episode_executor=_EpisodeExecutor([_result((True,))]), + clock=failing_clock, + memory_sampler=_memory_sampler([MemorySnapshot(100.0, 0.0, 0.0)]), + ) + + assert error.value.__notes__ == [ + "Episode discard also failed: RuntimeError: synthetic discard failure" + ] + + +def test_batched_rows_aggregate_success_reasons_failures_and_lengths() -> None: + env = _FakeEnv() + segment = DemoSegmentResult( + segment_id=0, + name="place", + start_step=0, + end_step=5, + success=False, + failure_reason="segment_validation_failed", + active=(True, True), + start_steps=(0, 0), + end_steps=(3, 5), + successes=(True, False), + failure_reasons=(None, "segment_validation_failed"), + ) + executor = _EpisodeExecutor( + [ + _result( + (True, False), + lengths=(3, 5), + reasons=("success", "segment_validation_failed"), + segments=(segment,), + ) + ] + ) + trials = collect_demo_success_trials( + [DemoSuccessCase("batched", (5,))], + lambda requested: env, + episode_executor=executor, + clock=_clock([1.0, 1.25]), + memory_sampler=_memory_sampler( + [ + MemorySnapshot(100.0, 20.0, 20.0), + MemorySnapshot(104.0, 22.0, 25.0), + ] + ), + ) + + metric = aggregate_demo_success_trials(trials).success_and_metrics[0] + + assert metric["attempted"] == 2 + assert metric["successes"] == 1 + assert metric["success_rate"] == pytest.approx(0.5) + assert json.loads(str(metric["terminal_reasons"])) == { + "segment_validation_failed": 1, + "success": 1, + } + assert metric["segment_failures"] == 1 + assert json.loads(str(metric["segment_failure_breakdown"])) == { + "place:segment_validation_failed": 1 + } + assert metric["length_mean"] == pytest.approx(4.0) + + +def test_runtime_call_failures_are_attributed_by_env_and_segment() -> None: + env = _FakeEnv(num_envs=3) + sequential = DemoSegmentResult( + segment_id=0, + name="prepare", + start_step=0, + end_step=1, + success=False, + metadata={ + "runtime": { + "kind": "skill_result", + "env_ids": [0, 1, 2], + "calls": [ + { + "semantic_id": "open", + "status": "failed", + "masks": {"failed": [True, False, False]}, + } + ], + } + }, + active=(True, True, True), + start_steps=(0, 0, 0), + end_steps=(1, 1, 1), + successes=(False, True, True), + failure_reasons=("timeout", None, None), + ) + parallel = DemoSegmentResult( + segment_id=1, + name="transfer", + start_step=1, + end_step=2, + success=False, + metadata={ + "runtime": { + "kind": "parallel_skill_result", + "branches": { + "left": { + "kind": "skill_result", + "env_ids": [0, 2], + "calls": [ + { + "semantic_id": "pick", + "status": "completed", + "masks": {"failed": [False, True]}, + } + ], + }, + "right": { + "kind": "skill_result", + "env_ids": [1], + "calls": [ + { + "semantic_id": "place", + "status": "failed", + "masks": {"failed": [True]}, + } + ], + }, + }, + } + }, + active=(True, True, True), + start_steps=(1, 1, 1), + end_steps=(2, 2, 2), + successes=(True, False, False), + failure_reasons=(None, "collision", "batch_aborted"), + ) + trials = collect_demo_success_trials( + [DemoSuccessCase("runtime", (3,))], + lambda requested: env, + episode_executor=_EpisodeExecutor( + [_result((False, False, False), segments=(sequential, parallel))] + ), + clock=_clock([0.0, 0.1]), + memory_sampler=_memory_sampler([MemorySnapshot(100.0, 0.0, 0.0)] * 2), + ) + + metric = aggregate_demo_success_trials(trials).success_and_metrics[0] + + assert metric["call_failures"] == 3 + assert json.loads(str(metric["call_failure_breakdown"])) == { + "prepare:open:failed": 1, + "transfer:left:pick:completed": 1, + "transfer:right:place:failed": 1, + } + assert json.loads(str(metric["segment_failure_breakdown"])) == { + "prepare:timeout": 1, + "transfer:batch_aborted": 1, + "transfer:collision": 1, + } + + +def _single_trial(case_id: str, successes: tuple[bool, ...]): + """Collect one deterministic trial for ranking/report tests.""" + env = _FakeEnv() + return collect_demo_success_trials( + [DemoSuccessCase(case_id, (1,))], + lambda requested: env, + episode_executor=_EpisodeExecutor([_result(successes)]), + clock=_clock([0.0, 0.01]), + memory_sampler=_memory_sampler( + [ + MemorySnapshot(100.0, 0.0, 0.0), + MemorySnapshot(100.0, 0.0, 0.0), + ] + ), + )[0] + + +def test_leaderboard_contains_every_case_with_deterministic_tie_break() -> None: + trials = ( + _single_trial("zeta", (True, False)), + _single_trial("alpha", (True, False)), + _single_trial("winner", (True, True)), + ) + + leaderboard = aggregate_demo_success_trials(trials).leaderboard + + assert [row["case"] for row in leaderboard] == ["winner", "alpha", "zeta"] + assert [row["rank"] for row in leaderboard] == [1, 2, 3] + + +def test_report_contains_exactly_three_tables(tmp_path: Path) -> None: + trials = (_single_trial("case-a", (True,)),) + report = write_markdown_report( + tmp_path / "report.md", aggregate_demo_success_trials(trials) + ) + + text = report.read_text(encoding="utf-8") + + assert text.count("\n## ") == 3 + assert text.count("\n| ---") == 3 + assert "## Time & Memory" in text + assert "## Success & Other Metrics" in text + assert "## Leaderboard" in text + + +def test_raw_json_round_trip_preserves_trials(tmp_path: Path) -> None: + trials = (_single_trial("case-a", (True, False)),) + raw_path = write_raw_trials(tmp_path / "raw.json", trials) + + loaded = load_raw_trials(raw_path) + + assert [trial.to_dict() for trial in loaded] == [ + trial.to_dict() for trial in trials + ] + + +def test_duplicate_case_seed_is_rejected_by_aggregate_write_and_load( + tmp_path: Path, +) -> None: + trial = _single_trial("case-a", (True,)) + duplicates = (trial, trial) + + with pytest.raises(ValueError, match="Duplicate demo success trial"): + aggregate_demo_success_trials(duplicates) + with pytest.raises(ValueError, match="Duplicate demo success trial"): + write_raw_trials(tmp_path / "duplicates.json", duplicates) + + raw_path = write_raw_trials(tmp_path / "raw.json", (trial,)) + payload = json.loads(raw_path.read_text(encoding="utf-8")) + payload["trials"].append(payload["trials"][0]) + raw_path.write_text(json.dumps(payload), encoding="utf-8") + with pytest.raises(ValueError, match="Duplicate demo success trial"): + load_raw_trials(raw_path) + + +def test_zero_case_and_zero_trial_benchmarks_are_rejected(tmp_path: Path) -> None: + with pytest.raises(ValueError, match="at least one benchmark case"): + collect_demo_success_trials((), lambda requested: _FakeEnv()) + with pytest.raises(ValueError, match="at least one demo success trial"): + aggregate_demo_success_trials(()) + with pytest.raises(ValueError, match="at least one demo success trial"): + write_raw_trials(tmp_path / "empty.json", ()) + + empty_raw = tmp_path / "empty-input.json" + empty_raw.write_text( + json.dumps( + { + "schema_version": 1, + "benchmark": "expert_program_demo_success", + "trials": [], + } + ), + encoding="utf-8", + ) + with pytest.raises(ValueError, match="at least one demo success trial"): + load_raw_trials(empty_raw) + + +def test_cli_offline_mode_aggregates_existing_raw_json(tmp_path: Path) -> None: + raw_path = write_raw_trials( + tmp_path / "raw.json", (_single_trial("case-a", (True,)),) + ) + report_path = tmp_path / "offline-report.md" + + exit_code = main(["--raw-json", str(raw_path), "--report", str(report_path)]) + + assert exit_code == 0 + assert report_path.is_file() + assert len(list(tmp_path.glob("*.md"))) == 1 + + +@pytest.mark.parametrize( + "live_args", + ( + ("--preview",), + ("--action_config", "actions.json"), + ("--headless",), + ("--device", "cpu"), + ("--num_envs", "1"), + ("--renderer", "auto"), + ), +) +def test_cli_offline_mode_rejects_explicit_live_options( + tmp_path: Path, + live_args: tuple[str, ...], +) -> None: + with pytest.raises(SystemExit) as error: + main([*live_args, "--raw-json", str(tmp_path / "raw.json")]) + + assert error.value.code == 2 + + +def test_gym_runner_reuses_one_environment_and_shared_no_retry_harness( + tmp_path: Path, +) -> None: + env = _FakeEnv() + launcher_args = argparse.Namespace(gym_config="gym.json", action_config=None) + factory_calls: list[tuple[object, Path]] = [] + closed: list[object] = [] + + def environment_factory(args: object, program_path: str | Path) -> _FakeEnv: + factory_calls.append((args, Path(program_path))) + return env + + artifacts = run_gym_demo_success_benchmark( + DemoSuccessCase("cube", (3, 5)), + launcher_args=launcher_args, + expert_program_path=tmp_path / "program.yaml", + raw_json_path=tmp_path / "raw.json", + report_path=tmp_path / "report.md", + episode_executor=_EpisodeExecutor([_result((False,)), _result((True,))]), + clock=_clock([0.0, 0.1, 1.0, 1.2]), + memory_sampler=_memory_sampler([MemorySnapshot(100.0, 0.0, 0.0)] * 4), + environment_factory=environment_factory, + environment_closer=closed.append, + ) + + assert factory_calls == [(launcher_args, tmp_path / "program.yaml")] + assert closed == [env] + assert env.reset_calls == [ + {"seed": 3, "options": {"save_data": False}}, + {"options": {"save_data": False}}, + {"seed": 5, "options": {"save_data": False}}, + {"options": {"save_data": False}}, + ] + assert [trial.seed for trial in artifacts.trials] == [3, 5] + assert artifacts.raw_json_path.is_file() + assert artifacts.report_path.is_file() + + +def test_gym_runner_closes_environment_when_seed_reset_fails(tmp_path: Path) -> None: + class _ResetFailureEnv(_FakeEnv): + def reset(self, **kwargs: object) -> None: + super().reset(**kwargs) + if "seed" in kwargs: + raise RuntimeError("synthetic reset failure") + + env = _ResetFailureEnv() + closed: list[object] = [] + + with pytest.raises(RuntimeError, match="synthetic reset failure"): + run_gym_demo_success_benchmark( + DemoSuccessCase("cube", (3,)), + launcher_args=argparse.Namespace( + gym_config="gym.json", + action_config=None, + ), + expert_program_path=tmp_path / "program.yaml", + raw_json_path=tmp_path / "raw.json", + report_path=tmp_path / "report.md", + environment_factory=lambda args, path: env, + environment_closer=closed.append, + ) + + assert closed == [env] + + +def test_gym_runner_flushes_cleanup_without_closing_when_factory_fails( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + factory_error = LookupError("synthetic factory failure") + cleanup_calls: list[str] = [] + close_calls: list[object] = [] + + def fail_factory( + launcher_args: argparse.Namespace, + expert_program_path: str | Path, + ) -> _FakeEnv: + raise factory_error + + monkeypatch.setattr( + "embodichain.lab.sim.sim_manager.SimulationManager.flush_cleanup_queue", + lambda: cleanup_calls.append("flush_cleanup_queue"), + ) + + with pytest.raises(LookupError, match="synthetic factory failure") as error: + run_gym_demo_success_benchmark( + DemoSuccessCase("cube", (3,)), + launcher_args=argparse.Namespace( + gym_config="gym.json", + action_config=None, + ), + expert_program_path=tmp_path / "program.yaml", + raw_json_path=tmp_path / "raw.json", + report_path=tmp_path / "report.md", + environment_factory=fail_factory, + environment_closer=close_calls.append, + ) + + assert error.value is factory_error + assert cleanup_calls == ["flush_cleanup_queue"] + assert close_calls == [] + + +def test_gym_runner_preserves_factory_error_when_cleanup_flush_also_fails( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + factory_error = LookupError("synthetic factory failure") + cleanup_calls: list[str] = [] + close_calls: list[object] = [] + + def fail_factory( + launcher_args: argparse.Namespace, + expert_program_path: str | Path, + ) -> _FakeEnv: + raise factory_error + + def fail_cleanup() -> None: + cleanup_calls.append("flush_cleanup_queue") + raise RuntimeError("synthetic cleanup failure") + + monkeypatch.setattr( + "embodichain.lab.sim.sim_manager.SimulationManager.flush_cleanup_queue", + fail_cleanup, + ) + + with pytest.raises(LookupError, match="synthetic factory failure") as error: + run_gym_demo_success_benchmark( + DemoSuccessCase("cube", (3,)), + launcher_args=argparse.Namespace( + gym_config="gym.json", + action_config=None, + ), + expert_program_path=tmp_path / "program.yaml", + raw_json_path=tmp_path / "raw.json", + report_path=tmp_path / "report.md", + environment_factory=fail_factory, + environment_closer=close_calls.append, + ) + + assert error.value is factory_error + assert error.value.__notes__ == [ + "Benchmark environment construction cleanup also failed: " + "RuntimeError: synthetic cleanup failure" + ] + assert cleanup_calls == ["flush_cleanup_queue"] + assert close_calls == [] + + +def test_default_gym_environment_closer_uses_unwrapped_target_and_flushes_cleanup( + monkeypatch: pytest.MonkeyPatch, +) -> None: + calls: list[object] = [] + + class _UnwrappedEnv: + def close(self, *, exit_process: bool) -> None: + calls.append(("close", exit_process)) + + env = argparse.Namespace(unwrapped=_UnwrappedEnv()) + monkeypatch.setattr( + "embodichain.lab.sim.sim_manager.SimulationManager.flush_cleanup_queue", + lambda: calls.append("flush_cleanup_queue"), + ) + + demo_success_module._close_gym_demo_success_environment(env) + + assert calls == [("close", False), "flush_cleanup_queue"] + + +def test_gym_runner_preserves_body_error_when_default_close_also_fails( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + close_calls: list[bool] = [] + cleanup_calls: list[str] = [] + + class _CloseFailureTarget: + def close(self, *, exit_process: bool) -> None: + close_calls.append(exit_process) + raise RuntimeError("synthetic close failure") + + class _BodyFailureEnv(_FakeEnv): + def __init__(self) -> None: + super().__init__() + self.unwrapped = _CloseFailureTarget() + + def reset(self, **kwargs: object) -> None: + super().reset(**kwargs) + if "seed" in kwargs: + raise LookupError("synthetic benchmark body failure") + + env = _BodyFailureEnv() + monkeypatch.setattr( + "embodichain.lab.sim.sim_manager.SimulationManager.flush_cleanup_queue", + lambda: cleanup_calls.append("flush_cleanup_queue"), + ) + + with pytest.raises(LookupError, match="synthetic benchmark body failure") as error: + run_gym_demo_success_benchmark( + DemoSuccessCase("cube", (3,)), + launcher_args=argparse.Namespace( + gym_config="gym.json", + action_config=None, + ), + expert_program_path=tmp_path / "program.yaml", + raw_json_path=tmp_path / "raw.json", + report_path=tmp_path / "report.md", + environment_factory=lambda args, path: env, + ) + + assert error.value.__notes__ == [ + "Benchmark environment cleanup also failed: " + "RuntimeError: synthetic close failure" + ] + assert close_calls == [False] + assert cleanup_calls == ["flush_cleanup_queue"] + + +def test_gym_environment_builder_uses_standard_public_config_pipeline( + monkeypatch: pytest.MonkeyPatch, +) -> None: + calls: list[object] = [] + launcher_args = argparse.Namespace( + gym_config="gym.json", + action_config=None, + ) + env_cfg = argparse.Namespace(expert_program=None) + program = object() + env = object() + + monkeypatch.setattr( + demo_success_module, + "discover_task_packages", + lambda: calls.append("discover"), + ) + monkeypatch.setattr( + demo_success_module, + "execute_init_hooks", + lambda: calls.append("hooks"), + ) + + def build(args: argparse.Namespace): + calls.append(("build", args)) + return env_cfg, {"id": "ExpertTask-v1"}, {} + + monkeypatch.setattr(demo_success_module, "build_env_cfg_from_args", build) + monkeypatch.setattr( + demo_success_module, + "load_expert_program", + lambda path: calls.append(("load", path)) or program, + ) + monkeypatch.setattr( + demo_success_module.gymnasium, + "make", + lambda **kwargs: calls.append(("make", kwargs)) or env, + ) + + created = demo_success_module._create_gym_demo_success_environment( + launcher_args, + "program.yaml", + ) + + assert created is env + assert env_cfg.expert_program is program + assert calls == [ + "discover", + "hooks", + ("build", launcher_args), + ("load", "program.yaml"), + ("make", {"id": "ExpertTask-v1", "cfg": env_cfg}), + ] + + +@pytest.mark.parametrize( + "unsupported", + ( + ("--preview",), + ("--action_config", "actions.json"), + ), +) +def test_cli_live_mode_rejects_unsupported_launcher_options( + tmp_path: Path, + unsupported: tuple[str, ...], +) -> None: + with pytest.raises(SystemExit) as error: + main( + [ + "--run-simulation", + "--gym_config", + "gym.json", + "--expert-program", + "program.yaml", + "--case-id", + "cube", + "--seeds", + "7", + "--raw-json", + str(tmp_path / "raw.json"), + *unsupported, + ] + ) + + assert error.value.code == 2 + + +def test_cli_live_mode_dispatches_fixed_seed_case( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + captured: dict[str, object] = {} + + def run(case: DemoSuccessCase, **kwargs: object) -> object: + captured["case"] = case + captured.update(kwargs) + return object() + + monkeypatch.setattr( + demo_success_module, + "run_gym_demo_success_benchmark", + run, + ) + raw_path = tmp_path / "raw.json" + + exit_code = main( + [ + "--run-simulation", + "--gym_config", + "gym.json", + "--expert-program", + "program.yaml", + "--case-id", + "cube", + "--seeds", + "7", + "11", + "--raw-json", + str(raw_path), + ] + ) + + assert exit_code == 0 + assert captured["case"] == DemoSuccessCase("cube", (7, 11)) + assert captured["expert_program_path"] == Path("program.yaml") + assert captured["raw_json_path"] == raw_path + assert captured["report_path"] == raw_path.with_suffix(".md") + launcher_args = captured["launcher_args"] + assert isinstance(launcher_args, argparse.Namespace) + assert launcher_args.gym_config == "gym.json" + assert launcher_args.num_envs is None + assert launcher_args.renderer is None + + +def test_run_all_benchmarks_prints_report_path( + tmp_path: Path, capsys: pytest.CaptureFixture[str] +) -> None: + env = _FakeEnv() + report_path = tmp_path / "report.md" + + artifacts = run_all_benchmarks( + [DemoSuccessCase("case-a", (1,))], + lambda requested: env, + raw_json_path=tmp_path / "raw.json", + report_path=report_path, + episode_executor=_EpisodeExecutor([_result((True,))]), + clock=_clock([0.0, 0.1]), + memory_sampler=_memory_sampler([MemorySnapshot(100.0, 0.0, 0.0)] * 2), + ) + + assert artifacts.report_path == report_path + assert f"Markdown report saved: {report_path}" in capsys.readouterr().out diff --git a/tests/benchmark/expert_program/test_demo_success_cube_sim.py b/tests/benchmark/expert_program/test_demo_success_cube_sim.py new file mode 100644 index 000000000..d77d757e7 --- /dev/null +++ b/tests/benchmark/expert_program/test_demo_success_cube_sim.py @@ -0,0 +1,150 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Live repeated-cube regression coverage for the Expert Program benchmark.""" + +from __future__ import annotations + +from pathlib import Path +import subprocess +import sys + +import pytest + +from embodichain_tasks.configs import get_config_path +from scripts.benchmark.expert_program.demo_success import ( + aggregate_demo_success_trials, + load_raw_trials, +) + +_REPOSITORY_ROOT = Path(__file__).resolve().parents[3] +_CUBE_GYM_CONFIG = get_config_path("gym/multi_segments/cube_pick_place.json") +_CUBE_EXPERT_PROGRAM = get_config_path( + "expert_program/multi_segments/repeated_cube_pick_place.yaml" +) +_CASE_ID = "repeated_cube_three_cycle_live" +_SEED = 0 +_NUM_ENVS = 1 +_SUBPROCESS_TIMEOUT_SECONDS = 180 +_RUN_PUBLIC_MAIN = ( + "from scripts.benchmark.expert_program.demo_success import main; " + "raise SystemExit(main())" +) + + +def _successful_effect_decisions(call: dict[str, object]) -> list[dict[str, object]]: + """Return successful physical-effect observations from one call trace.""" + effects = call["effects"] + assert isinstance(effects, list) + return [ + effect + for effect in effects + if isinstance(effect, dict) + and isinstance(effect.get("decision"), dict) + and effect["decision"].get("success_mask") == [True] + ] + + +@pytest.mark.requires_sim +@pytest.mark.slow +@pytest.mark.gpu +def test_live_repeated_cube_completes_three_physical_cycles( + tmp_path: Path, +) -> None: + """Run all three no-retry segments through the public live entry point.""" + raw_path = tmp_path / "cube_raw.json" + report_path = tmp_path / "cube_report.md" + completed = subprocess.run( + [ + sys.executable, + "-c", + _RUN_PUBLIC_MAIN, + "--run-simulation", + "--gym_config", + str(_CUBE_GYM_CONFIG), + "--expert-program", + str(_CUBE_EXPERT_PROGRAM), + "--case-id", + _CASE_ID, + "--seeds", + str(_SEED), + "--raw-json", + str(raw_path), + "--report", + str(report_path), + "--headless", + "--device", + "cuda", + "--num_envs", + str(_NUM_ENVS), + "--filter_dataset_saving", + ], + cwd=_REPOSITORY_ROOT, + capture_output=True, + text=True, + timeout=_SUBPROCESS_TIMEOUT_SECONDS, + check=False, + ) + + assert completed.returncode == 0, completed.stdout + completed.stderr + decoded_trials = load_raw_trials(raw_path) + assert len(decoded_trials) == 1 + trial = decoded_trials[0] + assert trial.case_id == _CASE_ID + assert trial.seed == _SEED + assert trial.rows[0].success + assert trial.rows[0].terminal_reason == "success" + + episode = trial.episode_result + assert episode["completed"] is True + assert episode["success"] == [True] + assert episode["terminal_reason"] == "success" + segments = episode["segments"] + assert isinstance(segments, list) + assert len(segments) == 3 + + for segment_index, (segment, target_index) in enumerate( + zip(segments, (0, 1, 0), strict=True) + ): + assert isinstance(segment, dict) + assert segment["segment_id"] == segment_index + assert segment["name"] == "move_cube" + metadata = segment["metadata"] + assert isinstance(metadata, dict) + runtime = metadata["runtime"] + assert runtime["status"] == "completed" + assert runtime["masks"]["success"] == [True] + calls = runtime["calls"] + assert [call["semantic_id"] for call in calls] == ["pick", "place"] + assert all(call["status"] == "completed" for call in calls) + assert all(_successful_effect_decisions(call) for call in calls) + + post_policies = metadata["post_policies"] + assert len(post_policies) == 1 + assert post_policies[0]["kind"] == "wait_stable" + assert post_policies[0]["result"]["status"] == "settled" + validation = metadata["validation"] + assert validation["accepted_mask"] == [True] + validators = validation["validators"] + assert len(validators) == 1 + assert validators[0]["result"]["target_value_index"] == target_index + assert validators[0]["result"]["accepted_mask"] == [True] + + aggregates = aggregate_demo_success_trials(decoded_trials) + metrics = aggregates.success_and_metrics[0] + assert metrics["attempted"] == 1 + assert metrics["successes"] == 1 + assert metrics["success_rate"] == 1.0 diff --git a/tests/benchmark/expert_program/test_demo_success_open_drawer_sim.py b/tests/benchmark/expert_program/test_demo_success_open_drawer_sim.py new file mode 100644 index 000000000..c3af1a94c --- /dev/null +++ b/tests/benchmark/expert_program/test_demo_success_open_drawer_sim.py @@ -0,0 +1,166 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Live OpenDrawer regression coverage for the Expert Program benchmark.""" + +from __future__ import annotations + +import json +from pathlib import Path +import subprocess +import sys + +import pytest + +from embodichain_tasks.configs import get_config_path +from scripts.benchmark.expert_program.demo_success import ( + aggregate_demo_success_trials, + load_raw_trials, +) + +_REPOSITORY_ROOT = Path(__file__).resolve().parents[3] +_OPEN_DRAWER_GYM_CONFIG = get_config_path("gym/open_drawer/cobot_magic_3cam.json") +_OPEN_DRAWER_EXPERT_PROGRAM = get_config_path( + "expert_program/tableware/open_drawer.json" +) +_CASE_ID = "open_drawer_live" +_SEED = 0 +_NUM_ENVS = 1 +_SUBPROCESS_TIMEOUT_SECONDS = 180 +_RUN_PUBLIC_MAIN = ( + "from scripts.benchmark.expert_program.demo_success import main; " + "raise SystemExit(main())" +) + + +def _write_headless_cpu_gym_config(tmp_path: Path) -> Path: + """Write a camera-free copy of the packaged live-physics configuration.""" + payload = json.loads(_OPEN_DRAWER_GYM_CONFIG.read_text(encoding="utf-8")) + if type(payload) is not dict: + raise TypeError("The packaged OpenDrawer Gym config must be a JSON object.") + env_config = payload.get("env") + if type(env_config) is not dict: + raise TypeError("The packaged OpenDrawer env config must be a JSON object.") + + # Cameras and their recording event are orthogonal to drawer physics and make + # this CPU regression unnecessarily renderer-sensitive. + payload["sensor"] = [] + env_config["events"] = {} + env_config["observations"] = {} + env_config["dataset"] = {} + payload["expert_program_path"] = str(_OPEN_DRAWER_EXPERT_PROGRAM) + + output = tmp_path / "open_drawer_headless_cpu.json" + output.write_text( + json.dumps(payload, indent=2, ensure_ascii=False) + "\n", + encoding="utf-8", + ) + return output + + +@pytest.mark.requires_sim +@pytest.mark.slow +def test_live_open_drawer_benchmark_writes_successful_decodable_artifacts( + tmp_path: Path, +) -> None: + """Run one no-retry seed through the public live benchmark entry point.""" + gym_config_path = _write_headless_cpu_gym_config(tmp_path) + raw_path = tmp_path / "open_drawer_raw.json" + report_path = tmp_path / "open_drawer_report.md" + completed = subprocess.run( + [ + sys.executable, + "-c", + _RUN_PUBLIC_MAIN, + "--run-simulation", + "--gym_config", + str(gym_config_path), + "--expert-program", + str(_OPEN_DRAWER_EXPERT_PROGRAM), + "--case-id", + _CASE_ID, + "--seeds", + str(_SEED), + "--raw-json", + str(raw_path), + "--report", + str(report_path), + "--headless", + "--device", + "cpu", + "--num_envs", + str(_NUM_ENVS), + "--filter_dataset_saving", + ], + cwd=_REPOSITORY_ROOT, + capture_output=True, + text=True, + timeout=_SUBPROCESS_TIMEOUT_SECONDS, + check=False, + ) + + # main() returns zero only after the live runner's default closer completes; + # the process boundary also isolates native simulator teardown from pytest. + assert completed.returncode == 0, completed.stdout + completed.stderr + assert f"Raw JSON saved: {raw_path}" in completed.stdout + assert f"Markdown report saved: {report_path}" in completed.stdout + + decoded_trials = load_raw_trials(raw_path) + assert len(decoded_trials) == 1 + trial = decoded_trials[0] + assert trial.case_id == _CASE_ID + assert trial.seed == _SEED + assert len(trial.rows) == _NUM_ENVS + row = trial.rows[0] + assert row.success + assert row.terminal_reason == "success" + assert row.length > 0 + + segments = trial.episode_result["segments"] + assert isinstance(segments, list) + assert len(segments) == 1 + segment = segments[0] + assert isinstance(segment, dict) + assert segment["name"] == "open_drawer" + runtime = segment["metadata"]["runtime"] + assert runtime["kind"] == "skill_result" + assert runtime["status"] == "completed" + calls = runtime["calls"] + assert isinstance(calls, list) + assert len(calls) == 1 + call = calls[0] + assert call["semantic_id"] == "operate_articulation" + assert call["status"] == "completed" + effects = call["effects"] + assert isinstance(effects, list) + assert effects + for effect in effects: + evidence = effect["evidence"]["joint.position"] + assert evidence["valid_mask"] == [True] + assert evidence["acquisition_errors"] == [None] + + aggregates = aggregate_demo_success_trials(decoded_trials) + assert len(aggregates.success_and_metrics) == 1 + metrics = aggregates.success_and_metrics[0] + assert metrics["attempted"] == 1 + assert metrics["successes"] == 1 + assert metrics["success_rate"] == 1.0 + + report = report_path.read_text(encoding="utf-8") + assert report.count("\n## ") == 3 + assert "## Success & Other Metrics" in report + assert "## Leaderboard" in report + assert _CASE_ID in report diff --git a/tests/benchmark/motion_generation/test_motion_generation_benchmark.py b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py index d98beaa66..d29a50af1 100644 --- a/tests/benchmark/motion_generation/test_motion_generation_benchmark.py +++ b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py @@ -172,10 +172,26 @@ def _valid_motion_case_and_positions() -> tuple[BenchmarkCase, torch.Tensor]: return case, positions +def _timed_plan_result( + positions: torch.Tensor, + *, + success: bool | torch.Tensor, +) -> PlanResult: + """Build a synthetic plan with explicit benchmark timing.""" + dt = torch.zeros(positions.shape[:2], device=positions.device) + if positions.shape[1] > 1: + dt[:, 1:] = 0.025 + return PlanResult( + success=success, + positions=positions, + dt=dt, + ) + + def test_motion_valid_ignores_planner_reported_failure_in_outcomes_and_aggregates(): case, positions = _valid_motion_case_and_positions() outcomes = compute_case_outcomes( - PlanResult(success=False, positions=positions), + _timed_plan_result(positions, success=False), case, _MetricRobot(), "arm", @@ -245,7 +261,7 @@ def test_missing_positions_and_joint_limit_violation_fail_motion_valid(): positions = torch.zeros(1, 2, 7) positions[0, :, 0] = 2.0 violated = compute_case_outcomes( - PlanResult(success=True, positions=positions), + _timed_plan_result(positions, success=True), case, _MetricRobot(), "arm", @@ -265,7 +281,7 @@ def test_non_finite_trajectory_skips_joint_limit_metrics(): positions = torch.zeros(1, 2, 7) positions[0, 1, 0] = float("inf") outcomes = compute_case_outcomes( - PlanResult(success=True, positions=positions), + _timed_plan_result(positions, success=True), case, _MetricRobot(), "arm", @@ -1098,7 +1114,7 @@ def build(self) -> None: def plan(self, case: BenchmarkCase) -> PlanResult: steps = max(case.num_waypoints + 1, 2) positions = case.start_qpos.unsqueeze(1).expand(-1, steps, -1).clone() - return PlanResult(success=True, positions=positions) + return _timed_plan_result(positions, success=True) class _IncapableFake(PlannerAdapter): capabilities = frozenset({"eef_waypoint"}) diff --git a/tests/docs/test_check_api_docs.py b/tests/docs/test_check_api_docs.py new file mode 100644 index 000000000..d3eb209de --- /dev/null +++ b/tests/docs/test_check_api_docs.py @@ -0,0 +1,235 @@ +# ---------------------------------------------------------------------------- +# 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 read-only public API documentation checker.""" + +from __future__ import annotations + +import importlib.util +import json +import sys +from pathlib import Path + +import pytest + +_REPO_ROOT = Path(__file__).resolve().parents[2] +_SCRIPT = _REPO_ROOT / "docs" / "scripts" / "check_api_docs.py" + + +def _load_checker_module(): + spec = importlib.util.spec_from_file_location("check_api_docs", _SCRIPT) + if spec is None or spec.loader is None: + raise ImportError(f"Cannot load {_SCRIPT}") + module = importlib.util.module_from_spec(spec) + sys.modules["check_api_docs"] = module + spec.loader.exec_module(module) + return module + + +_checker = _load_checker_module() +ApiDocsError = _checker.ApiDocsError +MissingExport = _checker.MissingExport +PackageRoot = _checker.PackageRoot +PublicModule = _checker.PublicModule +check_api_docs = _checker.check_api_docs +collect_documented_exports = _checker.collect_documented_exports +discover_public_modules = _checker.discover_public_modules +find_missing_exports = _checker.find_missing_exports +format_json_report = _checker.format_json_report +format_text_report = _checker.format_text_report + + +def _write(path: Path, content: str) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text(content, encoding="utf-8") + + +def _file_snapshot(root: Path) -> dict[Path, bytes]: + return { + path.relative_to(root): path.read_bytes() + for path in root.rglob("*") + if path.is_file() + } + + +def test_discover_public_modules_uses_static_all(tmp_path: Path) -> None: + package_path = tmp_path / "sample" + _write(package_path / "__init__.py", '__all__ = ["Alpha", "helper"]\n') + _write(package_path / "feature" / "__init__.py", '__all__ = ["Feature"]\n') + _write(package_path / "module.py", '__all__ = ["NotPackageLevel"]\n') + _write(package_path / "_private" / "__init__.py", '__all__ = ["Hidden"]\n') + + modules = discover_public_modules((PackageRoot("sample", package_path),)) + + assert [(module.name, module.exports) for module in modules] == [ + ("sample", ("Alpha", "helper")), + ("sample.feature", ("Feature",)), + ("sample.module", ("NotPackageLevel",)), + ] + + +def test_discover_public_modules_collects_branch_scoped_static_all( + tmp_path: Path, +) -> None: + package_path = tmp_path / "sample" + _write( + package_path / "__init__.py", + """try: + __all__ = ["Primary", "Shared"] +except ImportError: + __all__: list[str] = ["Fallback", "Shared"] + +def local_scope() -> None: + __all__ = ["NotAModuleExport"] +""", + ) + + modules = discover_public_modules((PackageRoot("sample", package_path),)) + + assert [(module.name, module.exports) for module in modules] == [ + ("sample", ("Primary", "Shared", "Fallback")), + ] + + +def test_discover_public_modules_rejects_dynamic_all(tmp_path: Path) -> None: + package_path = tmp_path / "sample" + _write(package_path / "__init__.py", "__all__ = build_exports()\n") + + with pytest.raises(ApiDocsError, match="static list of strings"): + discover_public_modules((PackageRoot("sample", package_path),)) + + +def test_collect_documented_exports_parses_all_api_pages(tmp_path: Path) -> None: + api_root = tmp_path / "api_reference" + public_modules = ( + PublicModule( + "sample", + ("Alpha", "Beta", "Gamma", "Orphan"), + tmp_path / "sample", + ), + PublicModule("other", ("Delta", "Epsilon"), tmp_path / "other"), + ) + _write( + api_root / "a_other.rst", + """.. currentmodule:: other + +.. autoclass:: Epsilon +""", + ) + _write( + api_root / "z_sample.rst", + """.. automodule:: sample + + .. autosummary:: + + Alpha + +.. currentmodule:: sample + +.. autoclass:: Beta + +.. automodule:: other + :members: + :exclude-members: Epsilon +""", + ) + _write( + api_root / "public_api.rst", + """.. currentmodule:: sample + +.. autosummary:: + + Gamma +""", + ) + _write( + api_root / "_autosummary" / "orphan.rst", + """.. currentmodule:: sample + +.. autodata:: Orphan +""", + ) + + documented = collect_documented_exports(api_root, public_modules) + + assert documented == { + "sample.Alpha", + "sample.Beta", + "sample.Gamma", + "other.Delta", + "other.Epsilon", + } + + +def test_find_missing_exports_reports_public_import_path(tmp_path: Path) -> None: + modules = (PublicModule("sample", ("Alpha", "Beta"), tmp_path / "sample.py"),) + + missing = find_missing_exports(modules, {"sample.Alpha"}) + + assert missing == (MissingExport("sample", "Beta", tmp_path / "sample.py"),) + assert missing[0].qualified_name == "sample.Beta" + + +def test_check_api_docs_does_not_modify_files(tmp_path: Path) -> None: + package_path = tmp_path / "sample" + api_root = tmp_path / "api_reference" + _write(package_path / "__init__.py", '__all__ = ["Alpha", "Beta"]\n') + _write( + api_root / "sample.rst", + """.. currentmodule:: sample + +.. autoclass:: Alpha +""", + ) + before = _file_snapshot(tmp_path) + + result = check_api_docs( + package_roots=(PackageRoot("sample", package_path),), + api_reference_root=api_root, + ) + + assert result.total_exports == 2 + assert [item.qualified_name for item in result.missing] == ["sample.Beta"] + assert _file_snapshot(tmp_path) == before + + +def test_reports_support_humans_and_agent_skill(tmp_path: Path) -> None: + missing = MissingExport("sample", "Beta", tmp_path / "sample.py") + result = _checker.CheckResult(total_exports=2, missing=(missing,)) + + payload = json.loads(format_json_report(result)) + + assert payload["documented_exports"] == 1 + assert payload["missing_count"] == 1 + assert payload["missing"][0]["qualified_name"] == "sample.Beta" + assert "$update-api-docs" in format_text_report(result) + + +def test_main_writes_missing_json_to_stdout( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, + capsys: pytest.CaptureFixture[str], +) -> None: + missing = MissingExport("sample", "Beta", tmp_path / "sample.py") + result = _checker.CheckResult(total_exports=2, missing=(missing,)) + monkeypatch.setattr(_checker, "check_api_docs", lambda: result) + + exit_code = _checker.main(["--format", "json"]) + captured = capsys.readouterr() + + assert exit_code == 1 + assert json.loads(captured.out)["missing_count"] == 1 + assert captured.err == "" diff --git a/tests/gym/envs/expert_program/test_articulation_program.py b/tests/gym/envs/expert_program/test_articulation_program.py new file mode 100644 index 000000000..6629724cc --- /dev/null +++ b/tests/gym/envs/expert_program/test_articulation_program.py @@ -0,0 +1,185 @@ +# ---------------------------------------------------------------------------- +# 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 declarative articulation calls in Expert Programs.""" + +from __future__ import annotations + +import pytest +import torch + +from embodichain.lab.gym.envs.expert_program import ( + ExpertProgramCfg, + ExpertProgramCompiler, + ExpertProgramDecodeError, + ExpertProgramIntegrationCfg, + InvokeCfg, + OperateArticulationCfg, + decode_expert_program, +) +from embodichain.lab.sim.atomic_actions import Affordance, EntityState +from embodichain.lab.sim.skills.calls import OperateArticulation +from embodichain.lab.sim.skills.scene import ( + SceneAffordanceRef, + SceneArticulationRef, + SceneEntityRegistration, + SceneRegistry, +) + + +class _NeverObserveProvider: + """Reject state observation during static program compilation.""" + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp, env_ids + raise AssertionError("Compilation must not observe providers.") + + +def _payload(call: dict[str, object]) -> dict[str, object]: + return { + "schema_version": 1, + "program_id": "open_drawer", + "integration": { + "robot_profile": "manipulator", + "scene_registry": "scene", + "runtime_preset": "safe", + }, + "targets": {}, + "program": {"kind": "invoke", "call": call}, + } + + +def _compiler() -> ExpertProgramCompiler: + provider = _NeverObserveProvider() + drawer = SceneArticulationRef("drawer") + registry = SceneRegistry( + ( + SceneEntityRegistration(ref=drawer, state_provider=provider), + SceneEntityRegistration( + ref=SceneAffordanceRef("drawer_handle"), + parent=drawer, + native_name="handle", + relative_pose=torch.eye(4), + affordance=Affordance(), + ), + ) + ) + return ExpertProgramCompiler.from_scene_registry(registry) + + +def _integration() -> ExpertProgramIntegrationCfg: + return ExpertProgramIntegrationCfg( + robot_profile="manipulator", + scene_registry="scene", + runtime_preset="safe", + ) + + +def test_decoder_accepts_named_and_explicit_articulation_targets() -> None: + named = decode_expert_program( + _payload( + { + "kind": "operate_articulation", + "articulation": "drawer", + "handle": "drawer_handle", + "target": "open", + "resources": {"primary": "right_arm"}, + } + ) + ) + explicit = decode_expert_program( + _payload( + { + "kind": "operate_articulation", + "articulation": "drawer", + "target_position": 0.42, + "target_displacement": 0.40, + } + ) + ) + + assert type(named.program) is InvokeCfg + assert named.program.call == OperateArticulationCfg( + articulation="drawer", + handle="drawer_handle", + target="open", + resources={"primary": "right_arm"}, + ) + assert type(explicit.program) is InvokeCfg + assert explicit.program.call == OperateArticulationCfg( + articulation="drawer", + target_position=0.42, + target_displacement=0.40, + ) + + +@pytest.mark.parametrize( + ("fields", "code"), + ( + ({"target": "open", "target_position": 0.4}, "conflicting_articulation_target"), + ({"target_position": 0.4}, "incomplete_articulation_target"), + ({"target_displacement": 0.2}, "incomplete_articulation_target"), + ({"target_position": True, "target_displacement": 0.2}, "invalid_number"), + ), +) +def test_decoder_rejects_ambiguous_or_incomplete_articulation_targets( + fields: dict[str, object], + code: str, +) -> None: + call = { + "kind": "operate_articulation", + "articulation": "drawer", + **fields, + } + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(_payload(call)) + + assert error.value.code == code + + +def test_compiler_preserves_typed_articulation_call_without_observation() -> None: + config = ExpertProgramCfg( + schema_version=1, + program_id="open_drawer", + integration=_integration(), + targets={}, + program=InvokeCfg( + call=OperateArticulationCfg( + articulation="drawer", + handle="drawer_handle", + target="open", + ) + ), + ) + + segment = tuple(_compiler().compile(config))[0] + call = segment.calls[0].call + + assert type(call) is OperateArticulation + assert call.articulation == SceneArticulationRef("drawer") + assert call.handle == SceneAffordanceRef("drawer_handle") + assert call.target == "open" + assert call.target_position is None + assert call.target_displacement is None + + +__all__: list[str] = [] diff --git a/tests/gym/envs/expert_program/test_bridge.py b/tests/gym/envs/expert_program/test_bridge.py new file mode 100644 index 000000000..7859d0063 --- /dev/null +++ b/tests/gym/envs/expert_program/test_bridge.py @@ -0,0 +1,2193 @@ +# ---------------------------------------------------------------------------- +# 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 + +from dataclasses import dataclass +import json +from typing import Any + +import pytest +import torch + +from embodichain.lab.gym.envs.demo import ProcessedEnvAction, execute_demo_episode +from embodichain.lab.gym.envs.expert_program.bridge import ( + AtomicDemoBridge, + BufferedGymCommandSink, + DemoBridgeError, + EnvironmentStepClock, + EnvironmentStepTimingError, + GymPlanningObservationProvider, + RuntimeCommandFrameEncoder, + UnsupportedRuntimeTransportError, +) +import embodichain.lab.gym.envs.expert_program.bridge as bridge_module +from embodichain.lab.sim.atomic_actions.bindings import ( + JointPositionTarget, + RuntimeEndpointTarget, +) +from embodichain.lab.sim.atomic_actions.execution import ( + ExecutionEvent, + ExecutionEventKind, +) +from embodichain.lab.sim.atomic_actions.runner import ExecutionRunnerCfg +from embodichain.lab.sim.atomic_actions.runtime_commands import ( + EndpointCommand, + JointPositionPayload, + RuntimeCommandFrame, + RuntimeCommandPayload, +) +from embodichain.lab.sim.atomic_actions.state import ( + PlanningContext, + RobotObservation, + SceneSnapshot, + TaskState, +) +from embodichain.lab.sim.skills.calls import RegisteredSemanticCall +from embodichain.lab.sim.skills.parallel import ParallelTimingPolicy +from embodichain.lab.sim.skills.runtime import SkillResult, SkillStatus +from embodichain.lab.sim.skills.parallel_runtime import ( + ParallelLaneCommandSink, + ParallelRuntimeBranch, + ParallelSkillResult, + ParallelSkillRuntime, +) +from embodichain.lab.sim.skills.profiles import ResourceClaim + +STEP_DT = 0.02 +BATCH_SIZE = 2 +ROBOT_DOF = 5 + + +class _QposProvider: + """Return an owned fixed full-qpos snapshot.""" + + def __init__(self, qpos: torch.Tensor) -> None: + self.qpos = qpos.clone() + + def current_qpos(self, env_ids: torch.Tensor) -> torch.Tensor: + assert env_ids.numel() == self.qpos.shape[0] + return self.qpos.clone() + + +def _context( + *, + qpos: torch.Tensor | None = None, + env_ids: torch.Tensor | None = None, +) -> PlanningContext: + qpos = torch.zeros(BATCH_SIZE, ROBOT_DOF) if qpos is None else qpos + env_ids = torch.tensor([7, 3], dtype=torch.long) if env_ids is None else env_ids + return PlanningContext( + robot=RobotObservation( + timestamp=0.0, + qpos=qpos, + qvel=torch.zeros_like(qpos), + ), + task=TaskState.empty(qpos.shape[0], qpos.device), + scene=SceneSnapshot.empty(), + env_ids=env_ids, + ) + + +def _joint_frame( + *, + duration: float, + active_mask: torch.Tensor | None = None, + positions: torch.Tensor | None = None, +) -> RuntimeCommandFrame: + active_mask = torch.tensor([True, True]) if active_mask is None else active_mask + positions = ( + torch.tensor([[10.0, 30.0], [11.0, 31.0]]) if positions is None else positions + ) + return RuntimeCommandFrame( + commands=( + EndpointCommand( + target=JointPositionTarget( + control_part="arm", + joint_ids=(1, 3), + ), + payload=JointPositionPayload(positions=positions), + ), + ), + active_mask=active_mask, + env_ids=torch.tensor([7, 3], dtype=torch.long), + hold_duration=torch.full((BATCH_SIZE,), duration), + ) + + +@dataclass(frozen=True, slots=True) +class _DummyTarget(RuntimeEndpointTarget): + """Test-only non-joint runtime target.""" + + name: str + + @property + def transport_id(self) -> str: + return "test.transport" + + @property + def target_id(self) -> str: + return self.name + + def snapshot(self) -> _DummyTarget: + return _DummyTarget(self.name) + + +@dataclass(frozen=True, slots=True, eq=False) +class _DummyPayload(RuntimeCommandPayload): + """Test-only scalar payload.""" + + values: torch.Tensor + + def __post_init__(self) -> None: + object.__setattr__(self, "values", self.values.clone()) + + @property + def batch_size(self) -> int: + return int(self.values.shape[0]) + + @property + def device(self) -> torch.device: + return self.values.device + + @property + def transport_id(self) -> str: + return "test.transport" + + def snapshot(self) -> _DummyPayload: + return _DummyPayload(self.values) + + +class _DummyTransportEncoder: + """Test registration proving the frame encoder is transport-extensible.""" + + transport_id = "test.transport" + target_types = (_DummyTarget,) + payload_types = (_DummyPayload,) + + def encode( + self, + command: EndpointCommand, + *, + base_action: Any, + active_mask: torch.Tensor, + ) -> Any: + assert isinstance(command.payload, _DummyPayload) + action = base_action.clone() + action[active_mask, 0] = command.payload.values[active_mask] + return action + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + base_action: Any, + context: PlanningContext, + ) -> Any: + del targets, context + return base_action.clone() + + +class _RecordingAcceptedCommandObserver: + """Record transactional sink notifications and optional callback failures.""" + + def __init__( + self, + *, + fail_accept: bool = False, + fail_cancel: bool = False, + ) -> None: + self.fail_accept = fail_accept + self.fail_cancel = fail_cancel + self.sink: BufferedGymCommandSink | None = None + self.accepted_frames: list[RuntimeCommandFrame] = [] + self.accepted_pending_counts: list[int] = [] + self.cancelled_targets: list[tuple[RuntimeEndpointTarget, ...]] = [] + self.cancelled_pending_counts: list[int] = [] + self.discard_count = 0 + + def accepted(self, command: RuntimeCommandFrame) -> None: + """Record acceptance after observing the sink's committed buffer.""" + if self.sink is None: + raise AssertionError("Observer sink must be assigned before use.") + self.accepted_pending_counts.append(self.sink.pending_count) + self.accepted_frames.append(command) + if self.fail_accept: + raise RuntimeError("observer rejected accepted command") + + def cancelled(self, targets: tuple[RuntimeEndpointTarget, ...]) -> None: + """Record owned cancellation targets.""" + if self.sink is None: + raise AssertionError("Observer sink must be assigned before use.") + self.cancelled_pending_counts.append(self.sink.pending_count) + self.cancelled_targets.append(targets) + if self.fail_cancel: + raise RuntimeError("observer rejected cancellation") + + def discarded(self) -> None: + """Record one fail-closed observer reset.""" + self.discard_count += 1 + + +def _dummy_frame() -> RuntimeCommandFrame: + return RuntimeCommandFrame( + commands=( + EndpointCommand( + target=_DummyTarget("base"), + payload=_DummyPayload(torch.tensor([4.0, 5.0])), + ), + ), + active_mask=torch.tensor([True, False]), + env_ids=torch.tensor([7, 3], dtype=torch.long), + hold_duration=torch.full((BATCH_SIZE,), STEP_DT), + ) + + +@dataclass(frozen=True, slots=True) +class _FakeCompiledCall: + call_index: int + call: object + + +@dataclass(frozen=True, slots=True) +class _FakeSegment: + segment_index: int = 0 + segment_id: str = "segment-0" + name: str = "pick-and-place" + calls: tuple[_FakeCompiledCall, ...] = ( + _FakeCompiledCall(0, "pick"), + _FakeCompiledCall(1, "place"), + ) + source_path: tuple[object, ...] = ("program", "steps", 0) + post_policies: tuple[object, ...] = () + validators: tuple[object, ...] = () + parallel_block: object | None = None + implicit: bool = False + + +@dataclass(frozen=True, slots=True) +class _FakeParallelBranch: + branch_index: int + calls: tuple[_FakeCompiledCall, ...] + + +@dataclass(frozen=True, slots=True) +class _FakeBarrier: + timeout_steps: int = 17 + failure_policy: str = "fail_fast" + + +@dataclass(frozen=True, slots=True) +class _FakeParallelBlock: + branches: tuple[_FakeParallelBranch, ...] + barrier: _FakeBarrier = _FakeBarrier() + + +@dataclass(frozen=True, slots=True) +class _FakeProgramAnalysis: + calls: tuple[object, ...] + execution_prefix_length: int + + +class _FakeProgram: + schema_version = 1 + program_id = "demo-program" + + def __init__(self, *segments: _FakeSegment) -> None: + self.segments = segments + + def iter_segments(self): + yield from self.segments + + def sequential_execution_analysis( + self, + segment_index: int, + ) -> _FakeProgramAnalysis: + current = self.segments[segment_index] + if current.parallel_block is not None: + raise ValueError("Parallel segments have no sequential analysis.") + calls: list[object] = [] + for segment in self.segments[segment_index:]: + if segment.parallel_block is not None: + break + calls.extend(compiled.call for compiled in segment.calls) + return _FakeProgramAnalysis(tuple(calls), len(current.calls)) + + +def _skill_result( + status: SkillStatus, + *, + wait_duration: float = 0.0, + workflow_id: str = "demo-program/segment-0", +) -> SkillResult: + env_ids = torch.tensor([7, 3], dtype=torch.long) + eligible = torch.ones(BATCH_SIZE, dtype=torch.bool) + success = ( + torch.ones(BATCH_SIZE, dtype=torch.bool) + if status is SkillStatus.COMPLETED + else torch.zeros(BATCH_SIZE, dtype=torch.bool) + ) + failure = ( + torch.ones(BATCH_SIZE, dtype=torch.bool) + if status is SkillStatus.FAILED + else torch.zeros(BATCH_SIZE, dtype=torch.bool) + ) + if status is SkillStatus.FAILED: + eligible = torch.zeros_like(eligible) + return SkillResult( + status=status, + workflow_id=workflow_id, + current_call_index=0 if status is SkillStatus.RUNNING else None, + env_ids=env_ids, + success_mask=success, + failure_mask=failure, + cancelled_mask=torch.zeros(BATCH_SIZE, dtype=torch.bool), + eligible_mask=eligible, + task_state=TaskState.empty(BATCH_SIZE, "cpu"), + wait_duration=wait_duration, + ) + + +class _FakeRuntime: + """Clock-aware nonblocking runtime used to test the Gym boundary only.""" + + def __init__( + self, + sink: BufferedGymCommandSink, + clock: EnvironmentStepClock, + frame: RuntimeCommandFrame, + ) -> None: + self.sink = sink + self.clock = clock + self.frame = frame + self._status = SkillStatus.IDLE + self._result = _skill_result(SkillStatus.IDLE) + self._due_at = 0.0 + self._sent = False + self.start_count = 0 + self.step_count = 0 + self.cancel_count = 0 + self.calls: tuple[object, ...] = () + self.execution_prefix_lengths: list[int | None] = [] + self.eligible_masks: list[torch.Tensor | None] = [] + self.adopted_states: list[TaskState] = [] + + @property + def result(self) -> SkillResult: + return self._result + + @property + def status(self) -> SkillStatus: + return self._status + + def start( + self, + *calls: object, + workflow_id: str = "semantic_workflow", + eligible_mask: torch.Tensor | None = None, + execution_prefix_length: int | None = None, + ) -> SkillResult: + self.start_count += 1 + self.calls = tuple(calls[0]) if len(calls) == 1 else calls + self.execution_prefix_lengths.append(execution_prefix_length) + self.eligible_masks.append( + None if eligible_mask is None else eligible_mask.clone() + ) + self._sent = False + self._due_at = 0.0 + self._status = SkillStatus.RUNNING + self._result = _skill_result( + SkillStatus.RUNNING, + workflow_id=workflow_id, + ) + return self._result + + def adopt_verified_task_state(self, task_state: TaskState) -> SkillResult: + self.adopted_states.append(task_state) + return self._result + + def step(self) -> SkillResult: + self.step_count += 1 + if not self._sent: + self.sink.send(self.frame, timeout=1.0) + self._sent = True + self._due_at = self.clock.now() + float( + self.frame.hold_duration.max().item() + ) + remaining = max(self._due_at - self.clock.now(), 0.0) + if remaining > 1.0e-9: + self._result = _skill_result( + SkillStatus.RUNNING, + wait_duration=remaining, + workflow_id=self._result.workflow_id or "semantic_workflow", + ) + return self._result + self._status = SkillStatus.COMPLETED + self._result = _skill_result( + SkillStatus.COMPLETED, + workflow_id=self._result.workflow_id or "semantic_workflow", + ) + return self._result + + def cancel(self, reason: str) -> SkillResult: + del reason + self.cancel_count += 1 + self.sink.cancel(self.frame.targets, timeout=1.0) + self.sink.hold(self.frame.targets, _context(), timeout=1.0) + self._status = SkillStatus.CANCELLED + self._result = SkillResult( + status=SkillStatus.CANCELLED, + workflow_id=self._result.workflow_id, + current_call_index=None, + env_ids=torch.tensor([7, 3], dtype=torch.long), + success_mask=torch.zeros(BATCH_SIZE, dtype=torch.bool), + failure_mask=torch.zeros(BATCH_SIZE, dtype=torch.bool), + cancelled_mask=torch.ones(BATCH_SIZE, dtype=torch.bool), + eligible_mask=torch.zeros(BATCH_SIZE, dtype=torch.bool), + task_state=TaskState.empty(BATCH_SIZE, "cpu"), + ) + return self._result + + +class _StartFailingRuntime(_FakeRuntime): + """Fail semantic preflight before accepting any controller command.""" + + def start( + self, + *calls: object, + workflow_id: str = "semantic_workflow", + eligible_mask: torch.Tensor | None = None, + execution_prefix_length: int | None = None, + ) -> SkillResult: + del calls, workflow_id, eligible_mask, execution_prefix_length + self.start_count += 1 + raise RuntimeError("semantic runtime preflight failed") + + +class _TerminalHoldRuntime(_FakeRuntime): + """Emit a terminal safe hold after one consumed command.""" + + def step(self) -> SkillResult: + self.step_count += 1 + if not self._sent: + self.sink.send(self.frame, timeout=1.0) + self._sent = True + self._status = SkillStatus.RUNNING + self._result = _skill_result( + SkillStatus.RUNNING, + workflow_id=self._result.workflow_id or "semantic_workflow", + ) + return self._result + self.sink.hold(self.frame.targets, _context(), timeout=1.0) + self._status = SkillStatus.COMPLETED + self._result = _skill_result( + SkillStatus.COMPLETED, + workflow_id=self._result.workflow_id or "semantic_workflow", + ) + return self._result + + +class _TerminalFailedRuntime(_FakeRuntime): + """Fail terminally during planning without accepting a command.""" + + def start( + self, + *calls: object, + workflow_id: str = "semantic_workflow", + eligible_mask: torch.Tensor | None = None, + execution_prefix_length: int | None = None, + ) -> SkillResult: + running = super().start( + *calls, + workflow_id=workflow_id, + eligible_mask=eligible_mask, + execution_prefix_length=execution_prefix_length, + ) + failed_mask = torch.ones(BATCH_SIZE, dtype=torch.bool) + self._status = SkillStatus.FAILED + self._result = SkillResult( + status=SkillStatus.FAILED, + workflow_id=running.workflow_id, + current_call_index=None, + env_ids=running.env_ids, + success_mask=torch.zeros_like(failed_mask), + failure_mask=failed_mask, + cancelled_mask=torch.zeros_like(failed_mask), + eligible_mask=torch.zeros_like(failed_mask), + task_state=running.task_state, + events=( + ExecutionEvent( + kind=ExecutionEventKind.ACTION_PLANNING_FAILED, + timestamp=self.clock.now(), + skill_id="operate_articulation", + invocation_id="open-drawer-call", + invocation_revision=0, + invocation_index=0, + env_mask=failed_mask, + message="Articulation motion phase 'operate' failed.", + ), + ), + message="Motion planning failed before the first command.", + ) + return self._result + + +class _PartialSuccessRuntime(_FakeRuntime): + """Complete the workflow while retaining one failed environment row.""" + + def step(self) -> SkillResult: + result = super().step() + if result.status is SkillStatus.COMPLETED: + active_mask = torch.tensor([True, False]) + self._result = SkillResult( + status=SkillStatus.COMPLETED, + workflow_id=result.workflow_id, + current_call_index=None, + env_ids=result.env_ids, + success_mask=active_mask, + failure_mask=~active_mask, + cancelled_mask=torch.zeros_like(active_mask), + eligible_mask=active_mask, + task_state=result.task_state, + ) + return self._result + + +def _parallel_result( + status: SkillStatus, + *, + wait_duration: float = 0.0, +) -> ParallelSkillResult: + env_ids = torch.tensor([7, 3], dtype=torch.long) + terminal = status in { + SkillStatus.COMPLETED, + SkillStatus.FAILED, + SkillStatus.CANCELLED, + } + return ParallelSkillResult( + status=status, + env_ids=env_ids, + success_mask=( + torch.ones(BATCH_SIZE, dtype=torch.bool) + if status is SkillStatus.COMPLETED + else torch.zeros(BATCH_SIZE, dtype=torch.bool) + ), + failure_mask=( + torch.ones(BATCH_SIZE, dtype=torch.bool) + if status is SkillStatus.FAILED + else torch.zeros(BATCH_SIZE, dtype=torch.bool) + ), + cancelled_mask=( + torch.ones(BATCH_SIZE, dtype=torch.bool) + if status is SkillStatus.CANCELLED + else torch.zeros(BATCH_SIZE, dtype=torch.bool) + ), + pending_mask=( + torch.zeros(BATCH_SIZE, dtype=torch.bool) + if terminal + else torch.ones(BATCH_SIZE, dtype=torch.bool) + ), + task_state=TaskState.empty(BATCH_SIZE, "cpu"), + branch_results={}, + elapsed_steps=0, + command_count=0, + wait_duration=wait_duration, + ) + + +class _FakeParallelRuntime: + """One-grid-frame parallel coordinator used at the bridge boundary.""" + + def __init__( + self, + sink: BufferedGymCommandSink, + clock: EnvironmentStepClock, + ) -> None: + self.sink = sink + self.clock = clock + self._result = _parallel_result(SkillStatus.IDLE) + self._sent = False + self._due_at = 0.0 + self.eligible_mask: torch.Tensor | None = None + + @property + def result(self) -> ParallelSkillResult: + return self._result + + def start( + self, + *, + workflow_id: str = "parallel_workflow", + eligible_mask: torch.Tensor | None = None, + ) -> ParallelSkillResult: + del workflow_id + self.eligible_mask = None if eligible_mask is None else eligible_mask.clone() + self._result = _parallel_result(SkillStatus.RUNNING) + return self._result + + def step(self) -> ParallelSkillResult: + if not self._sent: + self.sink.send(_joint_frame(duration=STEP_DT), timeout=1.0) + self._sent = True + self._due_at = self.clock.now() + STEP_DT + remaining = max(self._due_at - self.clock.now(), 0.0) + self._result = ( + _parallel_result(SkillStatus.RUNNING, wait_duration=remaining) + if remaining > 1.0e-9 + else _parallel_result(SkillStatus.COMPLETED) + ) + return self._result + + def cancel(self, reason: str) -> ParallelSkillResult: + del reason + self._result = _parallel_result(SkillStatus.CANCELLED) + return self._result + + +class _GridLaneRuntime: + """Small branch runtime used with the real parallel coordinator and sink.""" + + def __init__( + self, + sink: ParallelLaneCommandSink, + script: tuple[tuple[SkillStatus, RuntimeCommandFrame | None], ...], + ) -> None: + self.sink = sink + self.script = script + self.step_count = 0 + self._result = _skill_result(SkillStatus.IDLE) + + @property + def result(self) -> SkillResult: + return self._result + + def start( + self, + *calls: object, + workflow_id: str, + eligible_mask: torch.Tensor | None = None, + ) -> SkillResult: + del calls, eligible_mask + self._result = _skill_result(SkillStatus.RUNNING, workflow_id=workflow_id) + return self._result + + def step(self) -> SkillResult: + status, frame = self.script[min(self.step_count, len(self.script) - 1)] + self.step_count += 1 + if frame is not None: + self.sink.send(frame, timeout=1.0) + if status is not SkillStatus.RUNNING: + last_frame = frame or self.sink.last_frame + assert last_frame is not None + self.sink.hold(last_frame.targets, _context(), timeout=1.0) + self._result = _skill_result( + status, workflow_id=self._result.workflow_id or "lane" + ) + return self._result + + def deactivate_rows( + self, + env_mask: torch.Tensor, + *, + reason: str, + ) -> SkillResult: + del env_mask, reason + return self._result + + def cancel(self, reason: str) -> SkillResult: + del reason + last_frame = self.sink.last_frame + if last_frame is not None: + self.sink.cancel(last_frame.targets, timeout=1.0) + self.sink.hold(last_frame.targets, _context(), timeout=1.0) + self._result = SkillResult( + status=SkillStatus.CANCELLED, + workflow_id=self._result.workflow_id, + current_call_index=None, + env_ids=torch.tensor([7, 3], dtype=torch.long), + success_mask=torch.zeros(BATCH_SIZE, dtype=torch.bool), + failure_mask=torch.zeros(BATCH_SIZE, dtype=torch.bool), + cancelled_mask=torch.ones(BATCH_SIZE, dtype=torch.bool), + eligible_mask=torch.zeros(BATCH_SIZE, dtype=torch.bool), + task_state=TaskState.empty(BATCH_SIZE, "cpu"), + ) + return self._result + + +def _grid_frame( + control_part: str, + joint_id: int, + value: float, +) -> RuntimeCommandFrame: + return RuntimeCommandFrame( + commands=( + EndpointCommand( + JointPositionTarget(control_part, (joint_id,)), + JointPositionPayload( + torch.full((BATCH_SIZE, 1), value, dtype=torch.float32) + ), + ), + ), + active_mask=torch.ones(BATCH_SIZE, dtype=torch.bool), + env_ids=torch.tensor([7, 3], dtype=torch.long), + hold_duration=torch.full((BATCH_SIZE,), STEP_DT), + ) + + +class _PostPolicyPort: + def __init__(self, action: torch.Tensor) -> None: + self.action = action + self.seen: list[object] = [] + self.active_masks: list[torch.Tensor] = [] + + def validate_policy(self, policy: object, *, segment: object) -> None: + del policy, segment + + def actions( + self, + policy: object, + *, + segment: object, + active_mask: torch.Tensor, + ): + del segment + self.seen.append(policy) + self.active_masks.append(active_mask.clone()) + yield self.action + + +class _ValidatorPort: + def __init__(self, result: torch.Tensor) -> None: + self.result = result + self.seen: list[object] = [] + + def validate_validator(self, validator: object, *, segment: object) -> None: + del validator, segment + + def validate(self, validator: object, *, segment: object) -> torch.Tensor: + del segment + self.seen.append(validator) + return self.result + + +class _MetadataPostPolicyPort(_PostPolicyPort): + """Post-policy test port exposing a deterministic result trace.""" + + def post_policy_metadata( + self, + policy: object, + *, + segment: object, + ) -> dict[str, object]: + del policy, segment + return { + "status": "timed_out", + "state": { + "elapsed_steps": 1, + "settled_mask": [True, False], + "timeout_mask": [False, True], + }, + } + + def post_policy_result( + self, + policy: object, + *, + segment: object, + ) -> torch.Tensor: + del policy, segment + return torch.tensor([True, False]) + + +class _MetadataValidatorPort(_ValidatorPort): + """Validator test port exposing observed error metadata.""" + + def validator_metadata( + self, + validator: object, + *, + segment: object, + ) -> dict[str, object]: + del validator, segment + return {"position_error": [0.01, 0.10]} + + +class _FailingPostPolicyPort: + """Raise from lazy policy iteration after the runtime reached a safe hold.""" + + def validate_policy(self, policy: object, *, segment: object) -> None: + del policy, segment + + def actions( + self, + policy: object, + *, + segment: object, + active_mask: torch.Tensor, + ): + del policy, segment, active_mask + if False: + yield torch.empty(0) + raise RuntimeError("post-policy observation failed") + + +class _AcceptParallelSafety: + """Test-only authoritative gate that accepts the supplied merged frame.""" + + def validate( + self, + *, + branch_frames: dict[str, RuntimeCommandFrame], + merged_frame: RuntimeCommandFrame, + ) -> None: + assert branch_frames + assert isinstance(merged_frame, RuntimeCommandFrame) + + +def _bridge( + *, + duration: float, + segment: _FakeSegment | None = None, + post_policy_port: object | None = None, + validator_port: object | None = None, + parallel_safety_validator: object | None = None, + runner_cfg: ExecutionRunnerCfg | None = None, +) -> tuple[AtomicDemoBridge, _FakeRuntime, EnvironmentStepClock]: + clock = EnvironmentStepClock(STEP_DT) + encoder = RuntimeCommandFrameEncoder( + _QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF)) + ) + sink = BufferedGymCommandSink(encoder, clock) + runtime = _FakeRuntime(sink, clock, _joint_frame(duration=duration)) + bridge = AtomicDemoBridge( + _FakeProgram(_FakeSegment() if segment is None else segment), + runtime, + sink, + clock, + post_policy_port=post_policy_port, + validator_port=validator_port, + runner_cfg=runner_cfg, + parallel_safety_validator=parallel_safety_validator, + ) + return bridge, runtime, clock + + +def test_bridge_snapshots_runner_cfg_before_lazy_parallel_creation() -> None: + """Later advanced-path config mutation cannot change lazy bridge policy.""" + runner_cfg = ExecutionRunnerCfg(command_timeout=0.25) + bridge, _, _ = _bridge(duration=STEP_DT, runner_cfg=runner_cfg) + + runner_cfg.command_timeout = 9.0 + + assert bridge._runner_cfg is not runner_cfg + assert bridge._runner_cfg.command_timeout == pytest.approx(0.25) + + +def test_environment_step_clock_advances_only_explicitly() -> None: + clock = EnvironmentStepClock(STEP_DT) + + assert clock.now() == 0.0 + assert clock.steps_for_duration(3 * STEP_DT) == 3 + with pytest.raises(EnvironmentStepTimingError, match="not an integer multiple"): + clock.steps_for_duration(0.03) + with pytest.raises(RuntimeError, match="cannot sleep"): + clock.sleep(STEP_DT) + assert clock.now() == 0.0 + + clock.advance_after_env_step() + assert clock.step_index == 1 + assert clock.now() == pytest.approx(STEP_DT) + + +def test_observation_provider_reorders_qpos_by_stable_env_id() -> None: + context = _context( + qpos=torch.tensor([[7.0, 7.1, 7.2, 7.3, 7.4], [3.0, 3.1, 3.2, 3.3, 3.4]]) + ) + provider = GymPlanningObservationProvider(lambda task_state: context) + + observed = provider.observe(context.task) + reordered = provider.current_qpos(torch.tensor([3, 7], dtype=torch.long)) + + assert observed is context + assert torch.equal(reordered[0], context.robot.qpos[1]) + assert torch.equal(reordered[1], context.robot.qpos[0]) + + +def test_joint_encoder_emits_full_qpos_and_holds_inactive_rows() -> None: + qpos = torch.arange(BATCH_SIZE * ROBOT_DOF, dtype=torch.float32).reshape( + BATCH_SIZE, ROBOT_DOF + ) + encoder = RuntimeCommandFrameEncoder(_QposProvider(qpos)) + frame = _joint_frame( + duration=STEP_DT, + active_mask=torch.tensor([True, False]), + ) + + action = encoder.encode(frame) + + assert isinstance(action, torch.Tensor) + assert action.shape == qpos.shape + assert torch.equal(action[0, torch.tensor([1, 3])], torch.tensor([10.0, 30.0])) + assert torch.equal(action[0, torch.tensor([0, 2, 4])], qpos[0, [0, 2, 4]]) + assert torch.equal(action[1], qpos[1]) + + +def test_frame_encoder_supports_registered_future_transport() -> None: + encoder = RuntimeCommandFrameEncoder( + _QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF)) + ) + frame = _dummy_frame() + with pytest.raises(UnsupportedRuntimeTransportError, match="test.transport"): + encoder.encode(frame) + + encoder.register_transport(_DummyTransportEncoder()) + action = encoder.encode(frame) + + assert isinstance(action, torch.Tensor) + assert action[0, 0].item() == 4.0 + assert action[1, 0].item() == 0.0 + + +def test_frame_encoder_composes_in_registration_not_frame_order( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """Transport registration is the stable controller composition order.""" + calls: list[str] = [] + original_joint_encode = bridge_module.JointPositionGymTransportEncoder.encode + original_dummy_encode = _DummyTransportEncoder.encode + + def record_joint(self: object, *args: object, **kwargs: object) -> object: + calls.append("joint") + return original_joint_encode(self, *args, **kwargs) + + def record_dummy(self: object, *args: object, **kwargs: object) -> object: + calls.append("dummy") + return original_dummy_encode(self, *args, **kwargs) + + monkeypatch.setattr( + bridge_module.JointPositionGymTransportEncoder, + "encode", + record_joint, + ) + monkeypatch.setattr(_DummyTransportEncoder, "encode", record_dummy) + joint = _joint_frame(duration=STEP_DT).commands[0] + dummy = _dummy_frame().commands[0] + frame = RuntimeCommandFrame( + commands=(dummy, joint), + active_mask=torch.tensor([True, False]), + env_ids=torch.tensor([7, 3], dtype=torch.long), + hold_duration=torch.full((BATCH_SIZE,), STEP_DT), + ) + encoder = RuntimeCommandFrameEncoder( + _QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF)), + transports=(_DummyTransportEncoder(),), + ) + + encoder.encode(frame) + + assert calls == ["joint", "dummy"] + + +def test_hold_encoder_composes_in_registration_not_target_order( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """Safe-hold transport composition uses the same registered ordering.""" + calls: list[str] = [] + original_joint_hold = bridge_module.JointPositionGymTransportEncoder.hold + original_dummy_hold = _DummyTransportEncoder.hold + + def record_joint(self: object, *args: object, **kwargs: object) -> object: + calls.append("joint") + return original_joint_hold(self, *args, **kwargs) + + def record_dummy(self: object, *args: object, **kwargs: object) -> object: + calls.append("dummy") + return original_dummy_hold(self, *args, **kwargs) + + monkeypatch.setattr( + bridge_module.JointPositionGymTransportEncoder, + "hold", + record_joint, + ) + monkeypatch.setattr(_DummyTransportEncoder, "hold", record_dummy) + encoder = RuntimeCommandFrameEncoder( + _QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF)), + transports=(_DummyTransportEncoder(),), + ) + dummy_target = _dummy_frame().targets[0] + joint_target = _joint_frame(duration=STEP_DT).targets[0] + + encoder.encode_hold((dummy_target, joint_target), _context()) + + assert calls == ["joint", "dummy"] + + +def test_frame_encoder_rejects_transport_without_static_type_declarations() -> None: + """Every runtime transport declares its exact pre-sim routing surface.""" + + class MissingDeclarations: + transport_id = "test.missing" + + def encode(self, *args: object, **kwargs: object) -> object: + raise AssertionError + + def hold(self, *args: object, **kwargs: object) -> object: + raise AssertionError + + encoder = RuntimeCommandFrameEncoder( + _QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF)) + ) + + with pytest.raises(TypeError, match="RuntimeTransportActionEncoder"): + encoder.register_transport(MissingDeclarations()) # type: ignore[arg-type] + + +def test_frame_encoder_requires_exact_declared_target_coverage() -> None: + """Transport routing never widens a declaration through subclass checks.""" + + class WrongTargetCoverage(_DummyTransportEncoder): + target_types = (JointPositionTarget,) + + encoder = RuntimeCommandFrameEncoder( + _QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF)), + transports=(WrongTargetCoverage(),), + ) + + with pytest.raises(TypeError, match="does not declare exact target type"): + encoder.encode(_dummy_frame()) + + with pytest.raises(TypeError, match="does not declare exact hold target type"): + encoder.encode_hold(_dummy_frame().targets, _context()) + + +def test_frame_encoder_requires_exact_declared_payload_coverage() -> None: + """Payload declarations are enforced independently of target coverage.""" + + class WrongPayloadCoverage(_DummyTransportEncoder): + payload_types = (JointPositionPayload,) + + encoder = RuntimeCommandFrameEncoder( + _QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF)), + transports=(WrongPayloadCoverage(),), + ) + + with pytest.raises(TypeError, match="does not declare exact payload type"): + encoder.encode(_dummy_frame()) + + +def test_buffered_sink_rejects_off_grid_frame_before_buffering() -> None: + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + + with pytest.raises(EnvironmentStepTimingError, match="hold_duration"): + sink.send(_joint_frame(duration=0.03), timeout=1.0) + + assert sink.pending_count == 0 + assert clock.step_index == 0 + + +def test_buffered_sink_buffers_command_hold_and_cancel_without_stepping() -> None: + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + frame = _joint_frame(duration=STEP_DT) + + acknowledgement = sink.send(frame, timeout=1.0) + assert acknowledgement.accepted + assert sink.pending_count == 1 + action = sink.pop() + assert isinstance(action, ProcessedEnvAction) + assert action.metadata["bridge_action_kind"] == "runtime_command" + assert clock.step_index == 0 + + sink.hold(frame.targets, _context(), timeout=1.0) + assert sink.pending_count == 1 + sink.cancel(frame.targets, timeout=1.0) + assert sink.pending_count == 0 + assert clock.step_index == 0 + + +def test_buffered_sink_notifies_observer_only_after_successful_buffering() -> None: + """Observer acceptance follows encoding and owns a command snapshot.""" + clock = EnvironmentStepClock(STEP_DT) + observer = _RecordingAcceptedCommandObserver() + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + accepted_command_observer=observer, + ) + observer.sink = sink + frame = _joint_frame(duration=STEP_DT) + + sink.send(frame, timeout=1.0) + + assert observer.accepted_pending_counts == [1] + assert len(observer.accepted_frames) == 1 + observed = observer.accepted_frames[0] + assert observed is not frame + assert torch.equal(observed.env_ids, frame.env_ids) + assert observed.env_ids.data_ptr() != frame.env_ids.data_ptr() + + +def test_buffered_sink_does_not_notify_observer_when_encoding_fails() -> None: + """A frame that never reaches the buffer cannot establish evidence.""" + clock = EnvironmentStepClock(STEP_DT) + observer = _RecordingAcceptedCommandObserver() + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + accepted_command_observer=observer, + ) + observer.sink = sink + + with pytest.raises(UnsupportedRuntimeTransportError, match="test.transport"): + sink.send(_dummy_frame(), timeout=1.0) + + assert sink.pending_count == 0 + assert observer.accepted_frames == [] + assert observer.discard_count == 0 + + +def test_buffered_sink_rolls_back_buffer_when_observer_rejects_acceptance() -> None: + """Observer failure atomically clears the pending action and evidence state.""" + clock = EnvironmentStepClock(STEP_DT) + observer = _RecordingAcceptedCommandObserver(fail_accept=True) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + accepted_command_observer=observer, + ) + observer.sink = sink + + with pytest.raises(RuntimeError, match="observer rejected accepted command"): + sink.send(_joint_frame(duration=STEP_DT), timeout=1.0) + + assert observer.accepted_pending_counts == [1] + assert sink.pending_count == 0 + assert observer.discard_count == 1 + + +def test_buffered_sink_notifies_observer_on_cancel_and_explicit_discard() -> None: + """Cancel is target-scoped while a local discard resets all evidence.""" + clock = EnvironmentStepClock(STEP_DT) + observer = _RecordingAcceptedCommandObserver() + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + accepted_command_observer=observer, + ) + observer.sink = sink + frame = _joint_frame(duration=STEP_DT) + sink.send(frame, timeout=1.0) + + sink.cancel(frame.targets, timeout=1.0) + + assert sink.pending_count == 0 + assert observer.cancelled_pending_counts == [0] + assert len(observer.cancelled_targets) == 1 + assert observer.cancelled_targets[0][0].address_fingerprint == ( + frame.targets[0].address_fingerprint + ) + assert observer.discard_count == 0 + + sink.send(frame, timeout=1.0) + sink.discard_pending() + assert sink.pending_count == 0 + assert observer.discard_count == 1 + + +def test_atomic_demo_bridge_is_lazy_and_waits_with_hold_actions() -> None: + bridge, runtime, clock = _bridge(duration=2 * STEP_DT) + + demo_segment = next(bridge.iter_segments()) + assert runtime.start_count == 0 + with pytest.raises(RuntimeError, match="before its action iterable"): + demo_segment.validator() + + actions = iter(demo_segment.actions) + command = next(actions) + assert runtime.start_count == 1 + assert runtime.calls == ("pick", "place") + assert clock.step_index == 0 + assert command.metadata["bridge_action_kind"] == "runtime_command" + assert command.metadata["environment_step"] == 0 + + wait_hold = next(actions) + assert clock.step_index == 1 + assert wait_hold.metadata["bridge_action_kind"] == "runtime_wait_hold" + assert torch.equal(wait_hold.value, command.value) + + with pytest.raises(StopIteration): + next(actions) + assert clock.step_index == 2 + assert runtime.status is SkillStatus.COMPLETED + assert runtime.cancel_count == 0 + assert demo_segment.validator().tolist() == [True, True] + + +def test_closing_without_abort_handshake_fails_loudly_and_does_not_ack() -> None: + bridge, runtime, clock = _bridge(duration=2 * STEP_DT) + actions = iter(next(bridge.iter_segments()).actions) + + next(actions) + with pytest.raises(DemoBridgeError, match="abort_actions"): + actions.close() + + assert clock.step_index == 0 + assert runtime.cancel_count == 1 + + +def test_abort_handshake_discards_unconsumed_command_and_yields_safe_hold() -> None: + bridge, runtime, clock = _bridge(duration=2 * STEP_DT) + segment = next(bridge.iter_segments()) + actions = iter(segment.actions) + + command = next(actions) + assert command.metadata["bridge_action_kind"] == "runtime_command" + assert segment.abort_actions is not None + emergency = iter(segment.abort_actions("operator stop", last_action_consumed=False)) + hold = next(emergency) + + assert hold.metadata["bridge_action_kind"] == "runtime_abort_safe_hold" + assert clock.step_index == 0 + with pytest.raises(StopIteration): + next(emergency) + assert clock.step_index == 1 + assert runtime.cancel_count == 1 + assert runtime.sink.pending_count == 0 + assert segment.metadata["runtime"]["status"] == "cancelled" + assert segment.metadata["runtime"]["masks"]["cancelled"] == [True, True] + with pytest.raises(RuntimeError, match="already started"): + next( + iter( + segment.abort_actions( + "duplicate stop", + last_action_consumed=False, + ) + ) + ) + actions.close() + + +def test_abort_handshake_acknowledges_consumed_command_exactly_once() -> None: + bridge, runtime, clock = _bridge(duration=2 * STEP_DT) + segment = next(bridge.iter_segments()) + actions = iter(segment.actions) + + next(actions) + assert segment.abort_actions is not None + emergency = iter( + segment.abort_actions("environment failure", last_action_consumed=True) + ) + hold = next(emergency) + + assert clock.step_index == 1 + assert hold.metadata["bridge_action_kind"] == "runtime_abort_safe_hold" + with pytest.raises(StopIteration): + next(emergency) + assert clock.step_index == 2 + assert runtime.cancel_count == 1 + actions.close() + + +def test_abort_replays_unconsumed_terminal_safe_hold_without_recancelling() -> None: + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + runtime = _TerminalHoldRuntime(sink, clock, _joint_frame(duration=STEP_DT)) + bridge = AtomicDemoBridge(_FakeProgram(_FakeSegment()), runtime, sink, clock) + segment = next(bridge.iter_segments()) + actions = iter(segment.actions) + + command = next(actions) + assert command.metadata["bridge_action_kind"] == "runtime_command" + terminal_hold = next(actions) + assert terminal_hold.metadata["bridge_action_kind"] == "runtime_safe_hold" + assert runtime.status is SkillStatus.COMPLETED + assert clock.step_index == 1 + + assert segment.abort_actions is not None + emergency = iter( + segment.abort_actions("stop before hold", last_action_consumed=False) + ) + replay = next(emergency) + assert replay.metadata["bridge_action_kind"] == "runtime_abort_safe_hold" + assert torch.equal(replay.value, terminal_hold.value) + with pytest.raises(StopIteration): + next(emergency) + + assert clock.step_index == 2 + assert runtime.cancel_count == 0 + assert sink.pending_count == 0 + actions.close() + + +def test_post_policy_interruption_replays_last_runtime_safe_hold() -> None: + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + runtime = _TerminalHoldRuntime(sink, clock, _joint_frame(duration=STEP_DT)) + post_policy = object() + segment_spec = _FakeSegment(post_policies=(post_policy,)) + bridge = AtomicDemoBridge( + _FakeProgram(segment_spec), + runtime, + sink, + clock, + post_policy_port=_PostPolicyPort(torch.ones(BATCH_SIZE, ROBOT_DOF)), + ) + segment = next(bridge.iter_segments()) + actions = iter(segment.actions) + + assert next(actions).metadata["bridge_action_kind"] == "runtime_command" + assert next(actions).metadata["bridge_action_kind"] == "runtime_safe_hold" + post_action = next(actions) + assert post_action.metadata["bridge_action_kind"] == "program_post_policy" + assert clock.step_index == 2 + + assert segment.abort_actions is not None + emergency = iter( + segment.abort_actions("post policy stop", last_action_consumed=False) + ) + replay = next(emergency) + assert replay.metadata["bridge_action_kind"] == "runtime_abort_safe_hold" + assert torch.equal(replay.value, torch.zeros(BATCH_SIZE, ROBOT_DOF)) + with pytest.raises(StopIteration): + next(emergency) + + assert clock.step_index == 3 + assert runtime.cancel_count == 0 + actions.close() + + +class _BridgeExecutorEnv: + """Minimal demo executor proving abort actions cross the Gym boundary.""" + + def __init__( + self, + bridge: AtomicDemoBridge, + *, + fail_first_mask: torch.Tensor | None = None, + raise_first_step: bool = False, + ) -> None: + self.bridge = bridge + self.fail_first_mask = ( + torch.zeros(BATCH_SIZE, dtype=torch.bool) + if fail_first_mask is None + else fail_first_mask.clone() + ) + self.raise_first_step = raise_first_step + self.num_envs = BATCH_SIZE + self.steps: list[ProcessedEnvAction] = [] + self._demo_no_auto_reset = False + + @property + def unwrapped(self) -> _BridgeExecutorEnv: + return self + + def create_demo_segments(self): + return self.bridge.iter_segments() + + def step(self, action: ProcessedEnvAction): + assert isinstance(action, ProcessedEnvAction) + self.steps.append(action.snapshot()) + if self.raise_first_step and len(self.steps) == 1: + raise RuntimeError("simulated environment failure") + failed = ( + self.fail_first_mask + if len(self.steps) == 1 + else torch.zeros(BATCH_SIZE, dtype=torch.bool) + ) + return ( + None, + torch.zeros(BATCH_SIZE), + torch.zeros(BATCH_SIZE, dtype=torch.bool), + torch.zeros(BATCH_SIZE, dtype=torch.bool), + {"fail": failed}, + ) + + def _mask_demo_action( + self, + action: ProcessedEnvAction, + active_mask: tuple[bool, ...], + ) -> ProcessedEnvAction: + del active_mask + return action.snapshot() + + +def test_zero_command_terminal_runtime_failure_preserves_trace_and_validates_once() -> ( + None +): + validator = object() + first = _FakeSegment( + calls=(_FakeCompiledCall(0, "operate_articulation"),), + validators=(validator,), + ) + second = _FakeSegment( + segment_index=1, + segment_id="segment-1", + name="must-not-start", + calls=(_FakeCompiledCall(1, "place"),), + source_path=("program", "steps", 1), + ) + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + runtime = _TerminalFailedRuntime( + sink, + clock, + _joint_frame(duration=STEP_DT), + ) + validator_port = _ValidatorPort(torch.ones(BATCH_SIZE, dtype=torch.bool)) + bridge = AtomicDemoBridge( + _FakeProgram(first, second), + runtime, + sink, + clock, + validator_port=validator_port, + ) + env = _BridgeExecutorEnv(bridge) + + result = execute_demo_episode(env) + + assert env.steps == [] + assert clock.step_index == 0 + assert runtime.start_count == 1 + assert runtime.step_count == 0 + assert validator_port.seen == [validator] + assert not result.completed + assert result.terminal_reason == "segment_validation_failed" + assert len(result.segments) == 1 + segment_result = result.segments[0] + assert segment_result.failure_reason == "segment_validation_failed" + runtime_trace = segment_result.metadata["runtime"] + assert runtime_trace["status"] == "failed" + assert ( + runtime_trace["message"] == "Motion planning failed before the first command." + ) + assert runtime_trace["events"] == [ + { + "kind": "action_planning_failed", + "timestamp": 0.0, + "skill_id": "operate_articulation", + "invocation_id": "open-drawer-call", + "invocation_revision": 0, + "invocation_index": 0, + "env_mask": [True, True], + "message": "Articulation motion phase 'operate' failed.", + } + ] + assert segment_result.metadata["validation"] == { + "env_ids": [7, 3], + "runtime_success_mask": [False, False], + "eligible_mask_before_validation": [False, False], + "post_policy_success_mask": None, + "validators": [ + { + "validator_index": 0, + "kind": "object", + "source_path": [], + "result_mask": [True, True], + "result": None, + } + ], + "accepted_mask": [False, False], + } + + +def test_sequential_start_failure_before_first_command_preserves_cause() -> None: + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + runtime = _StartFailingRuntime(sink, clock, _joint_frame(duration=STEP_DT)) + bridge = AtomicDemoBridge(_FakeProgram(_FakeSegment()), runtime, sink, clock) + env = _BridgeExecutorEnv(bridge) + + with pytest.raises(RuntimeError, match="action generation") as error: + execute_demo_episode(env) + + assert isinstance(error.value.__cause__, RuntimeError) + assert str(error.value.__cause__) == "semantic runtime preflight failed" + assert env.steps == [] + assert clock.step_index == 0 + assert runtime.start_count == 1 + assert runtime.cancel_count == 0 + assert sink.pending_count == 0 + + +def test_parallel_construction_failure_before_first_command_preserves_cause( + monkeypatch: pytest.MonkeyPatch, +) -> None: + block = _FakeParallelBlock( + branches=( + _FakeParallelBranch(0, (_FakeCompiledCall(0, "left"),)), + _FakeParallelBranch(1, (_FakeCompiledCall(1, "right"),)), + ) + ) + segment = _FakeSegment(parallel_block=block) + bridge, runtime, clock = _bridge( + duration=STEP_DT, + segment=segment, + parallel_safety_validator=_AcceptParallelSafety(), + ) + env = _BridgeExecutorEnv(bridge) + + def fail_construction(*args: object, **kwargs: object) -> _FakeParallelRuntime: + del args, kwargs + raise RuntimeError("parallel runtime construction failed") + + monkeypatch.setattr(bridge_module, "SkillRuntime", _FakeRuntime) + monkeypatch.setattr( + ParallelSkillRuntime, + "from_template", + classmethod(fail_construction), + ) + + with pytest.raises(RuntimeError, match="action generation") as error: + execute_demo_episode(env) + + assert isinstance(error.value.__cause__, RuntimeError) + assert str(error.value.__cause__) == "parallel runtime construction failed" + assert env.steps == [] + assert clock.step_index == 0 + assert runtime.start_count == 0 + assert runtime.cancel_count == 0 + assert runtime.sink.pending_count == 0 + + +def test_post_policy_timeout_is_row_local_and_preserved_in_segment_result() -> None: + segment = _FakeSegment(post_policies=(object(),)) + bridge, runtime, clock = _bridge( + duration=STEP_DT, + segment=segment, + post_policy_port=_MetadataPostPolicyPort(torch.ones(BATCH_SIZE, ROBOT_DOF)), + ) + env = _BridgeExecutorEnv(bridge) + + result = execute_demo_episode(env) + + assert result.segments[0].successes == (True, False) + assert result.segments[0].failure_reasons == ( + None, + "segment_validation_failed", + ) + assert result.segments[0].metadata["post_policies"][0]["result_mask"] == [ + True, + False, + ] + assert result.segments[0].metadata["validation"]["accepted_mask"] == [ + True, + False, + ] + assert [action.metadata["bridge_action_kind"] for action in env.steps] == [ + "runtime_command", + "program_post_policy", + ] + assert runtime.cancel_count == 0 + assert clock.step_index == 2 + + +def test_post_policy_generator_error_replays_safe_hold_before_propagating() -> None: + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + runtime = _TerminalHoldRuntime(sink, clock, _joint_frame(duration=STEP_DT)) + segment_spec = _FakeSegment(post_policies=(object(),)) + bridge = AtomicDemoBridge( + _FakeProgram(segment_spec), + runtime, + sink, + clock, + post_policy_port=_FailingPostPolicyPort(), + ) + env = _BridgeExecutorEnv(bridge) + + with pytest.raises(RuntimeError, match="action generation") as error: + execute_demo_episode(env) + + assert isinstance(error.value.__cause__, RuntimeError) + assert [action.metadata["bridge_action_kind"] for action in env.steps] == [ + "runtime_command", + "runtime_safe_hold", + "runtime_abort_safe_hold", + ] + assert runtime.cancel_count == 0 + assert sink.pending_count == 0 + assert clock.step_index == 3 + + +def test_demo_executor_pre_step_stop_consumes_only_abort_hold() -> None: + bridge, runtime, clock = _bridge(duration=2 * STEP_DT) + env = _BridgeExecutorEnv(bridge) + checks = iter((False, True)) + + result = execute_demo_episode(env, should_stop=lambda: next(checks, True)) + + assert result.terminal_reason == "interrupted" + assert result.length == 1 + assert [action.metadata["bridge_action_kind"] for action in env.steps] == [ + "runtime_abort_safe_hold" + ] + assert clock.step_index == 1 + assert runtime.cancel_count == 1 + assert runtime.sink.pending_count == 0 + + +def test_demo_executor_post_step_failure_acknowledges_then_safe_stops() -> None: + bridge, runtime, clock = _bridge(duration=2 * STEP_DT) + env = _BridgeExecutorEnv( + bridge, + fail_first_mask=torch.ones(BATCH_SIZE, dtype=torch.bool), + ) + + result = execute_demo_episode(env) + + assert result.terminal_reason == "failure" + assert result.length == 2 + assert [action.metadata["bridge_action_kind"] for action in env.steps] == [ + "runtime_command", + "runtime_abort_safe_hold", + ] + assert clock.step_index == 2 + assert runtime.cancel_count == 1 + assert runtime.sink.pending_count == 0 + + +def test_demo_executor_safe_stops_when_regular_env_step_raises() -> None: + bridge, runtime, clock = _bridge(duration=2 * STEP_DT) + env = _BridgeExecutorEnv(bridge, raise_first_step=True) + + with pytest.raises(RuntimeError, match="emergency safe-stop"): + execute_demo_episode(env) + + assert [action.metadata["bridge_action_kind"] for action in env.steps] == [ + "runtime_command", + "runtime_abort_safe_hold", + ] + assert clock.step_index == 1 + assert runtime.cancel_count == 1 + assert runtime.sink.pending_count == 0 + + +def test_row_independent_partial_failure_does_not_abort_healthy_peer() -> None: + bridge, runtime, clock = _bridge(duration=2 * STEP_DT) + env = _BridgeExecutorEnv( + bridge, + fail_first_mask=torch.tensor([True, False]), + ) + + result = execute_demo_episode(env) + + assert result.lengths == (1, 2) + assert [action.metadata["bridge_action_kind"] for action in env.steps] == [ + "runtime_command", + "runtime_wait_hold", + ] + assert clock.step_index == 2 + assert runtime.cancel_count == 0 + assert runtime.sink.pending_count == 0 + + +def test_post_policy_and_validator_ports_stay_at_demo_boundary() -> None: + post_policy = object() + validator = object() + segment = _FakeSegment( + post_policies=(post_policy,), + validators=(validator,), + ) + post_port = _PostPolicyPort(torch.ones(BATCH_SIZE, ROBOT_DOF)) + validator_port = _ValidatorPort(torch.tensor([True, False])) + bridge, _, clock = _bridge( + duration=STEP_DT, + segment=segment, + post_policy_port=post_port, + validator_port=validator_port, + ) + demo_segment = next(bridge.iter_segments()) + actions = iter(demo_segment.actions) + + runtime_action = next(actions) + assert runtime_action.metadata["bridge_action_kind"] == "runtime_command" + post_action = next(actions) + assert clock.step_index == 1 + assert post_action.metadata["bridge_action_kind"] == "program_post_policy" + with pytest.raises(StopIteration): + next(actions) + + assert clock.step_index == 2 + assert post_port.seen == [post_policy] + assert len(post_port.active_masks) == 1 + assert post_port.active_masks[0].tolist() == [True, True] + assert demo_segment.validator().tolist() == [True, False] + assert validator_port.seen == [validator] + + +def test_post_policy_receives_only_rows_surviving_partial_runtime_failure() -> None: + """Post-policy completion cannot be blocked by a runtime-failed row.""" + post_policy = object() + segment = _FakeSegment(post_policies=(post_policy,)) + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + runtime = _PartialSuccessRuntime( + sink, + clock, + _joint_frame(duration=STEP_DT), + ) + post_port = _PostPolicyPort(torch.ones(BATCH_SIZE, ROBOT_DOF)) + bridge = AtomicDemoBridge( + _FakeProgram(segment), + runtime, + sink, + clock, + post_policy_port=post_port, + ) + demo_segment = next(bridge.iter_segments()) + + tuple(demo_segment.actions) + + assert len(post_port.active_masks) == 1 + assert post_port.active_masks[0].tolist() == [True, False] + assert demo_segment.metadata["post_policies"][0]["result_mask"] == [True, False] + assert demo_segment.validator().tolist() == [True, False] + + +def test_later_post_policy_receives_only_rows_passing_prior_policy() -> None: + """Sequential post-policies monotonically narrow their active cohort.""" + segment = _FakeSegment(post_policies=(object(), object())) + post_port = _MetadataPostPolicyPort( + torch.ones(BATCH_SIZE, ROBOT_DOF), + ) + bridge, _, _ = _bridge( + duration=STEP_DT, + segment=segment, + post_policy_port=post_port, + ) + + tuple(next(bridge.iter_segments()).actions) + + assert [mask.tolist() for mask in post_port.active_masks] == [ + [True, True], + [True, False], + ] + + +def test_segment_lifecycle_metadata_records_runtime_post_and_validation() -> None: + post_policy = object() + validator = object() + segment = _FakeSegment( + post_policies=(post_policy,), + validators=(validator,), + ) + bridge, _, _ = _bridge( + duration=STEP_DT, + segment=segment, + post_policy_port=_MetadataPostPolicyPort(torch.ones(BATCH_SIZE, ROBOT_DOF)), + validator_port=_MetadataValidatorPort(torch.tensor([True, False])), + ) + demo_segment = next(bridge.iter_segments()) + + tuple(demo_segment.actions) + assert demo_segment.metadata["runtime"]["status"] == "completed" + assert demo_segment.metadata["post_policies"] == [ + { + "policy_index": 0, + "kind": "object", + "source_path": [], + "result_mask": [True, False], + "result": { + "status": "timed_out", + "state": { + "elapsed_steps": 1, + "settled_mask": [True, False], + "timeout_mask": [False, True], + }, + }, + } + ] + + assert demo_segment.validator().tolist() == [True, False] + assert demo_segment.metadata["validation"] == { + "env_ids": [7, 3], + "runtime_success_mask": [True, True], + "eligible_mask_before_validation": [True, True], + "post_policy_success_mask": [True, False], + "validators": [ + { + "validator_index": 0, + "kind": "object", + "source_path": [], + "result_mask": [True, False], + "result": {"position_error": [0.01, 0.1]}, + } + ], + "accepted_mask": [True, False], + } + json.dumps(demo_segment.metadata, allow_nan=False, sort_keys=True) + + +def test_declared_post_policy_requires_explicit_port() -> None: + segment = _FakeSegment(post_policies=(object(),)) + bridge, _, _ = _bridge(duration=STEP_DT, segment=segment) + + with pytest.raises(DemoBridgeError, match="no SegmentPostPolicyPort"): + tuple(next(bridge.iter_segments()).actions) + + +def test_bridge_marks_segments_row_independent_and_retains_failed_rows() -> None: + first_validator = object() + first = _FakeSegment(validators=(first_validator,)) + second = _FakeSegment( + segment_index=1, + segment_id="segment-1", + name="place-next", + calls=(_FakeCompiledCall(2, "place-next"),), + source_path=("program", "steps", 1), + ) + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + runtime = _FakeRuntime(sink, clock, _joint_frame(duration=STEP_DT)) + bridge = AtomicDemoBridge( + _FakeProgram(first, second), + runtime, + sink, + clock, + validator_port=_ValidatorPort(torch.tensor([True, False])), + ) + segments = iter(bridge.iter_segments()) + + first_demo = next(segments) + assert first_demo.failure_policy == "row_independent" + tuple(first_demo.actions) + assert first_demo.validator().tolist() == [True, False] + + second_demo = next(segments) + tuple(second_demo.actions) + assert runtime.eligible_masks[0] is None + assert runtime.eligible_masks[1].tolist() == [True, False] + assert second_demo.validator().tolist() == [True, False] + + +def test_bridge_refuses_next_segment_when_validator_was_skipped() -> None: + first = _FakeSegment(calls=(_FakeCompiledCall(0, "pick"),)) + second = _FakeSegment( + segment_index=1, + segment_id="segment-1", + name="place", + calls=(_FakeCompiledCall(1, "place"),), + source_path=("program", "steps", 1), + ) + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + runtime = _FakeRuntime(sink, clock, _joint_frame(duration=STEP_DT)) + segments = iter(AtomicDemoBridge(_FakeProgram(first, second), runtime, sink, clock)) + + first_demo = next(segments) + tuple(first_demo.actions) + + with pytest.raises(DemoBridgeError, match="validator must be called"): + next(segments) + assert runtime.start_count == 1 + + +def test_demo_executor_consumes_validation_before_requesting_next_segment() -> None: + first_validator = object() + first = _FakeSegment( + calls=(_FakeCompiledCall(0, "pick"),), + validators=(first_validator,), + ) + second = _FakeSegment( + segment_index=1, + segment_id="segment-1", + name="place", + calls=(_FakeCompiledCall(1, "place"),), + source_path=("program", "steps", 1), + ) + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + runtime = _FakeRuntime(sink, clock, _joint_frame(duration=STEP_DT)) + validator_port = _ValidatorPort(torch.ones(BATCH_SIZE, dtype=torch.bool)) + bridge = AtomicDemoBridge( + _FakeProgram(first, second), + runtime, + sink, + clock, + validator_port=validator_port, + ) + + result = execute_demo_episode(_BridgeExecutorEnv(bridge)) + + assert result.completed + assert len(result.segments) == 2 + assert [segment.success for segment in result.segments] == [True, True] + assert runtime.start_count == 2 + assert validator_port.seen == [first_validator] + + +def test_sequential_segment_analyzes_downstream_calls_but_executes_own_prefix() -> None: + first = _FakeSegment( + calls=(_FakeCompiledCall(0, "pick"),), + ) + second = _FakeSegment( + segment_index=1, + segment_id="segment-1", + name="place", + calls=(_FakeCompiledCall(1, "place"),), + source_path=("program", "steps", 1), + ) + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + runtime = _FakeRuntime(sink, clock, _joint_frame(duration=STEP_DT)) + bridge = AtomicDemoBridge(_FakeProgram(first, second), runtime, sink, clock) + segments = iter(bridge.iter_segments()) + + first_demo = next(segments) + tuple(first_demo.actions) + assert first_demo.validator().tolist() == [True, True] + + assert runtime.calls == ("pick", "place") + assert runtime.execution_prefix_lengths == [1] + + tuple(next(segments).actions) + assert runtime.calls == ("place",) + assert runtime.execution_prefix_lengths == [1, 1] + + +def test_parallel_segment_preserves_branches_barrier_and_adopts_state( + monkeypatch: pytest.MonkeyPatch, +) -> None: + left_calls = (_FakeCompiledCall(0, "left-pick"),) + right_calls = ( + _FakeCompiledCall(1, "right-pick"), + _FakeCompiledCall(2, "right-place"), + ) + block = _FakeParallelBlock( + branches=( + _FakeParallelBranch(0, left_calls), + _FakeParallelBranch(1, right_calls), + ) + ) + segment = _FakeSegment( + calls=left_calls + right_calls, + parallel_block=block, + ) + safety_validator = _AcceptParallelSafety() + bridge, runtime, clock = _bridge( + duration=STEP_DT, + segment=segment, + parallel_safety_validator=safety_validator, + ) + captured: dict[str, Any] = {} + fake_parallel = _FakeParallelRuntime(runtime.sink, clock) + + def from_template( + cls: type[ParallelSkillRuntime], + template_runtime: object, + branch_calls: dict[str, tuple[object, ...]], + command_sink: object, + timing_policy: object, + supplied_safety_validator: object, + *, + timeout_steps: int, + failure_policy: str, + runner_cfg: object, + workflow_id: str, + branch_paths: dict[str, tuple[object, ...]], + ) -> _FakeParallelRuntime: + del cls + captured.update( + { + "template_runtime": template_runtime, + "branch_calls": branch_calls, + "command_sink": command_sink, + "timing_policy": timing_policy, + "safety_validator": supplied_safety_validator, + "timeout_steps": timeout_steps, + "failure_policy": failure_policy, + "runner_cfg": runner_cfg, + "workflow_id": workflow_id, + "branch_paths": branch_paths, + } + ) + return fake_parallel + + monkeypatch.setattr(bridge_module, "SkillRuntime", _FakeRuntime) + monkeypatch.setattr( + ParallelSkillRuntime, + "from_template", + classmethod(from_template), + ) + + demo_segment = next(bridge.iter_segments()) + actions = tuple(demo_segment.actions) + + assert len(actions) == 1 + assert captured["template_runtime"] is runtime + assert captured["command_sink"] is runtime.sink + assert captured["branch_calls"] == { + "branch_0": ("left-pick",), + "branch_1": ("right-pick", "right-place"), + } + assert captured["timing_policy"].step_dt == STEP_DT + assert captured["safety_validator"] is safety_validator + assert captured["timeout_steps"] == 17 + assert captured["failure_policy"] == "fail_fast" + assert captured["runner_cfg"] is not None + assert captured["workflow_id"].endswith(":parallel_analysis") + assert captured["branch_paths"] == { + "branch_0": segment.source_path, + "branch_1": segment.source_path, + } + assert len(runtime.adopted_states) == 1 + assert demo_segment.failure_policy == "row_independent" + assert demo_segment.metadata["runtime"]["kind"] == "parallel_skill_result" + assert demo_segment.metadata["runtime"]["status"] == "completed" + assert demo_segment.metadata["runtime"]["masks"]["success"] == [True, True] + assert demo_segment.validator().tolist() == [True, True] + + +def test_real_parallel_coordinator_buffers_one_ordered_gym_action_per_step() -> None: + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_QposProvider(torch.zeros(BATCH_SIZE, ROBOT_DOF))), + clock, + ) + left_sink = ParallelLaneCommandSink() + right_sink = ParallelLaneCommandSink() + left_runtime = _GridLaneRuntime( + left_sink, + ( + (SkillStatus.RUNNING, _grid_frame("left_arm", 0, 1.0)), + (SkillStatus.COMPLETED, None), + ), + ) + right_runtime = _GridLaneRuntime( + right_sink, + ( + (SkillStatus.RUNNING, _grid_frame("right_arm", 1, 2.0)), + (SkillStatus.RUNNING, _grid_frame("right_arm", 1, 3.0)), + (SkillStatus.COMPLETED, None), + ), + ) + runtime = ParallelSkillRuntime( + ( + ParallelRuntimeBranch( + "left", + (RegisteredSemanticCall("test.left"),), + ResourceClaim(frozenset({"left_arm"}), (0,)), + left_runtime, + left_sink, + ), + ParallelRuntimeBranch( + "right", + (RegisteredSemanticCall("test.right"),), + ResourceClaim(frozenset({"right_arm"}), (1,)), + right_runtime, + right_sink, + ), + ), + sink, + clock, + ParallelTimingPolicy(STEP_DT), + _AcceptParallelSafety(), + timeout_steps=8, + ) + + runtime.start() + first = runtime.step() + assert first.status is SkillStatus.RUNNING + assert sink.pending_count == 1 + first_action = sink.pop() + assert first_action.metadata["bridge_action_kind"] == "runtime_command" + assert torch.equal(first_action.value[:, 0], torch.ones(BATCH_SIZE)) + assert torch.equal(first_action.value[:, 1], torch.full((BATCH_SIZE,), 2.0)) + clock.advance_after_env_step() + + padded = runtime.step() + assert padded.status is SkillStatus.RUNNING + assert sink.pending_count == 1 + padding_action = sink.pop() + assert padding_action.metadata["bridge_action_kind"] == "runtime_safe_hold" + assert torch.equal(padding_action.value, torch.zeros(BATCH_SIZE, ROBOT_DOF)) + lane_steps = (left_runtime.step_count, right_runtime.step_count) + clock.advance_after_env_step() + + deferred = runtime.step() + assert deferred.status is SkillStatus.RUNNING + assert sink.pending_count == 1 + deferred_action = sink.pop() + assert deferred_action.metadata["bridge_action_kind"] == "runtime_command" + assert torch.equal(deferred_action.value[:, 0], torch.zeros(BATCH_SIZE)) + assert torch.equal( + deferred_action.value[:, 1], + torch.full((BATCH_SIZE,), 3.0), + ) + assert (left_runtime.step_count, right_runtime.step_count) == lane_steps + clock.advance_after_env_step() + + completed = runtime.step() + assert completed.status is SkillStatus.COMPLETED + assert sink.pending_count == 1 + terminal_hold = sink.pop() + assert terminal_hold.metadata["bridge_action_kind"] == "runtime_safe_hold" + assert completed.command_count == 2 + clock.advance_after_env_step() + assert clock.step_index == 4 + + +def test_parallel_segment_fails_closed_without_safety_validator() -> None: + block = _FakeParallelBlock( + branches=( + _FakeParallelBranch(0, (_FakeCompiledCall(0, "left"),)), + _FakeParallelBranch(1, (_FakeCompiledCall(1, "right"),)), + ) + ) + segment = _FakeSegment(parallel_block=block) + bridge, runtime, _ = _bridge(duration=STEP_DT, segment=segment) + + with pytest.raises(DemoBridgeError, match="requires an explicit"): + tuple(next(bridge.iter_segments()).actions) + + assert runtime.start_count == 0 diff --git a/tests/gym/envs/expert_program/test_catalog.py b/tests/gym/envs/expert_program/test_catalog.py new file mode 100644 index 000000000..c1e06825e --- /dev/null +++ b/tests/gym/envs/expert_program/test_catalog.py @@ -0,0 +1,1075 @@ +# ---------------------------------------------------------------------------- +# 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 task-registration-owned Expert Program integration catalogs.""" + +from __future__ import annotations + +from concurrent.futures import ThreadPoolExecutor +from dataclasses import dataclass, replace +import json +from threading import Event, Lock +from types import SimpleNamespace +from typing import ClassVar + +import pytest + +from embodichain.lab.gym.envs.expert_program import ( + ExpertProgramIntegrationCatalog, + ExpertProgramValidationError, + IntegrationFingerprintMismatch, + SimulationArticulationLinkBinding, + SimulationExpertProgramRegistration, + SimulationRigidObjectBinding, + SimulationSceneBinding, + SupportSurfaceAffordanceBinding, + decode_expert_program, +) +from embodichain.lab.gym.utils.registration import EnvSpec +from embodichain.lab.sim.atomic_actions import ( + Affordance, + AtomicActionEngine, + PlanningContext, +) +from embodichain.lab.sim.skills import ( + PLACEMENT_TARGET_AFFORDANCE_REVISION, + PLACE_ON_AFFORDANCE_CAPABILITY, + BoundSemanticCall, + ControlPartEndpoint, + HandOver, + HandOverPoseProvider, + HandOverPoseTargets, + OperateArticulation, + RelationTargetGrounder, + SemanticCallCatalog, + SceneAffordanceRef, + SceneDynamics, + SceneEntityManifest, + SceneManifest, + SceneObjectRef, + SceneRegistry, + SemanticRelationTarget, + RegisteredSemanticCall, + SemanticCallDescriptor, + SkillPolicyPreset, + SupportSurfaceAffordance, + SupportSurfaceRelationTargetGrounder, + WorkflowRecoveryPolicy, + builtin_semantic_call_catalog, +) +from embodichain.lab.sim.atomic_actions.tracking import ( + InFlightTrackingPolicy, + TimedTerminalAcceptance, + TrackingMetricCfg, + TrackingPolicy, +) +from embodichain.lab.sim.skills.effects import EffectMonitorRef +from embodichain.lab.sim.skills.parallel_runtime import ( + ParallelCommandSafetyValidator, +) +from embodichain_tasks.multi_segments.cube_pick_place import ( + CUBE_ROBOT_PROFILE_ID, + CUBE_SCENE_REGISTRY_ID, + create_cube_robot_profile_binding, + create_cube_scene_binding, +) +from embodichain_tasks.tableware.open_drawer import ( + DRAWER_HANDLE_AFFORDANCE_ID, + DRAWER_ROBOT_PROFILE_ID, + DRAWER_SCENE_REGISTRY_ID, + DRAWER_UID, + OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION, +) + + +class _CatalogRelationGrounder(RelationTargetGrounder): + """Typed relation-grounder sentinel for registration validation.""" + + capability: ClassVar[str] = "test.catalog_relation" + affordance_type: ClassVar[type[Affordance]] = Affordance + affordance_revision: ClassVar[str] = "test-v1" + + def ground( + self, + relation: SemanticRelationTarget, + *, + affordance: Affordance, + context: PlanningContext, + ) -> object: + """Remain unreachable in provider-free catalog tests.""" + del relation, affordance, context + raise AssertionError("Catalog tests must not execute live providers.") + + +class _CatalogPlaceAffordance(Affordance): + """Typed provider-free payload marker for relation-linking tests.""" + + +@dataclass(frozen=True, slots=True) +class _CatalogHandOverPoseProvider(HandOverPoseProvider): + """Frozen declaration used to prove malicious drift detection.""" + + provider_id: ClassVar[str] = "test.catalog_handover" + transfer_height: float + + def resolve( + self, + call: HandOver, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> HandOverPoseTargets: + """Remain unreachable in provider-free catalog tests.""" + del call, context, bound + raise AssertionError("Catalog tests must not execute live providers.") + + +class _SecondCatalogRelationGrounder(_CatalogRelationGrounder): + """Second stateless grounder used for ordering regressions.""" + + capability: ClassVar[str] = "test.catalog_relation.second" + affordance_revision: ClassVar[str] = "test-v2" + + +class _SecondCatalogHandOverPoseProvider(HandOverPoseProvider): + """Second stateless hand-over provider used for ordering regressions.""" + + provider_id: ClassVar[str] = "test.catalog_handover.second" + + def resolve( + self, + call: HandOver, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> HandOverPoseTargets: + """Remain unreachable in provider-free catalog tests.""" + del call, context, bound + raise AssertionError("Catalog tests must not execute live providers.") + + +class _StatefulCatalogRelationGrounder(_CatalogRelationGrounder): + """Invalid non-dataclass provider with public instance state.""" + + capability: ClassVar[str] = "test.catalog_relation.stateful" + + def __init__(self) -> None: + self.height = 0.5 + + +@dataclass(frozen=True, slots=True) +class _NestedMutableCatalogRelationGrounder(_CatalogRelationGrounder): + """Invalid frozen provider retaining one mutable nested configuration.""" + + capability: ClassVar[str] = "test.catalog_relation.mutable_nested" + offsets: list[float] + + +class _PrivateSlotHandOverPoseProvider(HandOverPoseProvider): + """Invalid provider whose state is hidden behind a mangled slot name.""" + + __slots__ = ("__height",) + + provider_id: ClassVar[str] = "test.catalog_handover.private_slot" + + def __init__(self) -> None: + self.__height = 0.5 + + def resolve( + self, + call: HandOver, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> HandOverPoseTargets: + """Remain unreachable because registration rejects this provider.""" + del call, context, bound + raise AssertionError("Rejected providers must never execute.") + + +class _InheritedCachedHandOverPoseProvider(_CatalogHandOverPoseProvider): + """Invalid non-dataclass subclass adding state to a frozen declaration.""" + + __slots__ = ("cache",) + + provider_id: ClassVar[str] = "test.catalog_handover.inherited_cache" + + def __init__(self) -> None: + super().__init__(transfer_height=0.5) + object.__setattr__(self, "cache", {}) + + +@dataclass(frozen=True, slots=True) +class _OpaqueHandOverPoseProvider(HandOverPoseProvider): + """Provider declaration containing an unsupported opaque nested value.""" + + provider_id: ClassVar[str] = "test.catalog_handover.opaque" + opaque: object + + def resolve( + self, + call: HandOver, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> HandOverPoseTargets: + """Remain unreachable because fingerprinting rejects this provider.""" + del call, context, bound + raise AssertionError("Opaque providers must never reach runtime.") + + +class _AcceptParallelSafety: + """Stateless safety sentinel returned by the registration-owned factory.""" + + def validate(self, *, branch_frames: object, merged_frame: object) -> None: + """Accept the provider-free test command without observing simulation.""" + del branch_frames, merged_frame + + +@dataclass(frozen=True, slots=True) +class _CatalogParallelSafetyFactory: + """Frozen declaration covering the built-in transport exactly.""" + + validator_id: ClassVar[str] = "test.catalog_parallel_safety" + revision: ClassVar[str] = "1" + supported_transport_ids: ClassVar[frozenset[str]] = frozenset( + {"robot.joint_position"} + ) + margin: float = 0.02 + + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + ) -> ParallelCommandSafetyValidator: + """Return one independent protocol-compatible safety gate.""" + del simulation, robot, scene_registry, engine + return _AcceptParallelSafety() + + +class _SerializedParallelSafetyFactory: + """Instrument concurrent create calls without carrying instance state.""" + + validator_id: ClassVar[str] = "test.serialized_parallel_safety" + revision: ClassVar[str] = "1" + supported_transport_ids: ClassVar[frozenset[str]] = frozenset( + {"robot.joint_position"} + ) + _state_lock: ClassVar[Lock] = Lock() + _first_entered: ClassVar[Event] = Event() + _second_entered: ClassVar[Event] = Event() + _release_first: ClassVar[Event] = Event() + _calls: ClassVar[int] = 0 + _active: ClassVar[int] = 0 + _max_active: ClassVar[int] = 0 + + @classmethod + def reset(cls) -> None: + """Reset class-owned concurrency instrumentation for one test.""" + cls._first_entered = Event() + cls._second_entered = Event() + cls._release_first = Event() + cls._calls = 0 + cls._active = 0 + cls._max_active = 0 + + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + ) -> ParallelCommandSafetyValidator: + """Block the first call so a second call can attempt registration entry.""" + del simulation, robot, scene_registry, engine + with self._state_lock: + call_index = self._calls + type(self)._calls += 1 + type(self)._active += 1 + type(self)._max_active = max(self._max_active, self._active) + if call_index == 0: + self._first_entered.set() + if not self._release_first.wait(timeout=2.0): + raise TimeoutError("Timed out waiting to release first safety create.") + else: + self._second_entered.set() + with self._state_lock: + type(self)._active -= 1 + return _AcceptParallelSafety() + + +@dataclass(frozen=True, slots=True) +class _CatalogCustomTrackingMetric(TrackingMetricCfg): + """Metric with no built-in exact evaluator registration.""" + + metric_id: ClassVar[str] = "test.catalog_metric" + revision: ClassVar[str] = "1" + channel_id: ClassVar[str] = "joint.position" + + +def _program_payload( + *, + scene_registry: str = CUBE_SCENE_REGISTRY_ID, + runtime_preset: str = "safe", + object_id: str = "cube", +) -> dict[str, object]: + """Return one minimal catalog-linked program payload.""" + return { + "schema_version": 1, + "program_id": "catalog_pick", + "integration": { + "robot_profile": CUBE_ROBOT_PROFILE_ID, + "scene_registry": scene_registry, + "runtime_preset": runtime_preset, + }, + "targets": {}, + "program": { + "kind": "invoke", + "call": {"kind": "pick", "object": object_id}, + }, + } + + +def _registration() -> SimulationExpertProgramRegistration: + """Build one isolated provider-free task registration.""" + return SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + ) + + +def _parallel_live_inputs() -> tuple[object, SceneRegistry, AtomicActionEngine]: + """Build minimal identity-consistent inputs for factory lifecycle tests.""" + robot = object() + engine = AtomicActionEngine.__new__(AtomicActionEngine) + engine._planning_services = SimpleNamespace(robot=robot) # type: ignore[attr-defined] + return robot, SceneRegistry(), engine + + +def _operate_articulation_payload( + *, + target: str, + handle: str | None = None, +) -> dict[str, object]: + """Return one named drawer-operation program with an optional handle.""" + call: dict[str, object] = { + "kind": "operate_articulation", + "articulation": DRAWER_UID, + "target": target, + } + if handle is not None: + call["handle"] = handle + return { + "schema_version": 1, + "program_id": "catalog_open_drawer", + "integration": { + "robot_profile": DRAWER_ROBOT_PROFILE_ID, + "scene_registry": DRAWER_SCENE_REGISTRY_ID, + "runtime_preset": "safe", + }, + "targets": {}, + "program": {"kind": "invoke", "call": call}, + } + + +def _place_relation_catalog( + *, + install_grounder_key: bool, +) -> ExpertProgramIntegrationCatalog: + """Build one provider-free placement catalog with an optional grounder key.""" + base = _registration().catalog + support_ref = SceneObjectRef("support") + affordance_ref = SceneAffordanceRef("support_top") + scene = SceneManifest( + ( + SceneEntityManifest(ref=SceneObjectRef("cube")), + SceneEntityManifest( + ref=support_ref, + default_affordances={ + PLACE_ON_AFFORDANCE_CAPABILITY: affordance_ref, + }, + ), + SceneEntityManifest( + ref=affordance_ref, + parent=support_ref, + native_name="support_top_surface", + affordance_capabilities=frozenset({PLACE_ON_AFFORDANCE_CAPABILITY}), + affordance_payload_type=_CatalogPlaceAffordance, + affordance_revision="test-v1", + ), + ) + ) + grounder_keys = ( + frozenset( + { + ( + PLACE_ON_AFFORDANCE_CAPABILITY, + _CatalogPlaceAffordance, + "test-v1", + ) + } + ) + if install_grounder_key + else frozenset() + ) + return ExpertProgramIntegrationCatalog( + scene_registry_id="relation_scene", + robot_profile_id=base.robot_profile_id, + scene=scene, + robot_profile=base.robot_profile, + call_catalog=base.call_catalog, + relation_grounder_keys=grounder_keys, + articulation_operation_targets={}, + settle_preset_ids=base.settle_preset_ids, + endpoint_adapter_declarations=base.endpoint_adapter_declarations, + runtime_transport_declarations=base.runtime_transport_declarations, + parallel_safety_declaration=base.parallel_safety_declaration, + fingerprint="0" * 64, + _required_skills={}, + ) + + +def _place_relation_payload() -> dict[str, object]: + """Return one Place(on=object) program requiring relation grounding.""" + return { + "schema_version": 1, + "program_id": "catalog_place_relation", + "integration": { + "robot_profile": CUBE_ROBOT_PROFILE_ID, + "scene_registry": "relation_scene", + "runtime_preset": "safe", + }, + "targets": {}, + "program": { + "kind": "invoke", + "call": { + "kind": "place", + "object": "cube", + "on": "support", + }, + }, + } + + +def _parallel_pick_payload() -> dict[str, object]: + """Return one schema-v2 parallel program rooted at an exact config path.""" + return { + "schema_version": 2, + "program_id": "catalog_parallel_pick", + "integration": { + "robot_profile": CUBE_ROBOT_PROFILE_ID, + "scene_registry": CUBE_SCENE_REGISTRY_ID, + "runtime_preset": "safe", + }, + "targets": {}, + "program": { + "kind": "parallel", + "branches": [ + { + "kind": "invoke", + "call": {"kind": "pick", "object": "cube"}, + }, + { + "kind": "invoke", + "call": {"kind": "pick", "object": "cube"}, + }, + ], + "barrier": { + "kind": "barrier", + "name": "catalog_join", + "timeout_steps": 40, + "failure_policy": "fail_fast", + }, + }, + } + + +def _registration_with_preset( + preset: SkillPolicyPreset, +) -> SimulationExpertProgramRegistration: + """Replace the Cube task's sole preset for registration validation tests.""" + binding = create_cube_robot_profile_binding() + return SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=replace(binding, presets=(preset,)), + ) + + +def test_catalog_decodes_compiles_and_links_without_simulation() -> None: + """All external references are linked before a simulation is available.""" + registration = _registration() + + program = decode_expert_program( + _program_payload(), + validation_context=registration.catalog, + ) + compiled = registration.catalog.preflight(program) + + assert tuple(compiled.iter_segments())[0].calls[0].call.semantic_id == "pick" + + +def test_catalog_declares_builtin_endpoint_and_ordered_transport_contracts() -> None: + """The standard provider-free catalog contains its exact built-in wiring.""" + catalog = _registration().catalog + + adapter = catalog.endpoint_adapter_declarations[ControlPartEndpoint] + + assert adapter.adapter_id == "control_part" + assert adapter.runtime_transport_ids == frozenset({"robot.joint_position"}) + assert tuple( + value.transport_id for value in catalog.runtime_transport_declarations + ) == ("robot.joint_position",) + + +def test_parallel_preflight_requires_registered_safety_factory_at_exact_path() -> None: + """Parallel programs cannot defer physical-safety wiring to live startup.""" + registration = _registration() + program = decode_expert_program( + _parallel_pick_payload(), + validation_context=registration.catalog, + ) + + with pytest.raises(ExpertProgramValidationError) as error: + registration.catalog.preflight(program) + + assert error.value.code == "parallel_safety_factory_not_registered" + assert error.value.path == ("program",) + + +def test_parallel_preflight_accepts_exact_registration_owned_safety_factory() -> None: + """A factory declaration covers preflight and creates a fresh live gate.""" + registration = SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + parallel_safety_factory=_CatalogParallelSafetyFactory(), + ) + program = decode_expert_program( + _parallel_pick_payload(), + validation_context=registration.catalog, + ) + + compiled = registration.catalog.preflight(program) + robot, scene_registry, engine = _parallel_live_inputs() + validator = registration.create_parallel_safety_validator( + simulation=object(), + robot=robot, + scene_registry=scene_registry, + engine=engine, + ) + + assert tuple(compiled.iter_segments())[0].parallel_block is not None + assert isinstance(validator, ParallelCommandSafetyValidator) + + +def test_parallel_safety_factory_must_return_a_validator() -> None: + """A malformed registration-owned factory fails before runtime dispatch.""" + + class InvalidParallelSafetyFactory: + validator_id: ClassVar[str] = "test.invalid_parallel_safety" + revision: ClassVar[str] = "1" + supported_transport_ids: ClassVar[frozenset[str]] = frozenset( + {"robot.joint_position"} + ) + + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + ) -> object: + del simulation, robot, scene_registry, engine + return object() + + registration = SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + parallel_safety_factory=InvalidParallelSafetyFactory(), + ) + + robot, scene_registry, engine = _parallel_live_inputs() + with pytest.raises(TypeError, match="must return a ParallelCommandSafetyValidator"): + registration.create_parallel_safety_validator( + simulation=object(), + robot=robot, + scene_registry=scene_registry, + engine=engine, + ) + + +def test_parallel_safety_creation_and_history_are_one_registration_lock_scope() -> None: + """Concurrent assemblies cannot enter one registration factory together.""" + factory_type = _SerializedParallelSafetyFactory + factory_type.reset() + registration = SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + parallel_safety_factory=factory_type(), + ) + robot, scene_registry, engine = _parallel_live_inputs() + + def create_validator() -> ParallelCommandSafetyValidator | None: + return registration.create_parallel_safety_validator( + simulation=object(), + robot=robot, + scene_registry=scene_registry, + engine=engine, + ) + + with ThreadPoolExecutor(max_workers=2) as executor: + first = executor.submit(create_validator) + assert factory_type._first_entered.wait(timeout=1.0) + second = executor.submit(create_validator) + assert not factory_type._second_entered.wait(timeout=0.05) + factory_type._release_first.set() + assert isinstance(first.result(timeout=1.0), ParallelCommandSafetyValidator) + assert isinstance(second.result(timeout=1.0), ParallelCommandSafetyValidator) + + assert factory_type._calls == 2 + assert factory_type._max_active == 1 + + +def test_standard_registration_rejects_registered_semantic_descriptors() -> None: + """Executable lowerer extensions are outside the standard factory contract.""" + catalog = builtin_semantic_call_catalog() + target = catalog.descriptors["pick"].target_descriptor + assert target is not None and target.binding_contract is not None + custom = SemanticCallDescriptor( + call_id="test.catalog_call", + spec_type=RegisteredSemanticCall, + target_descriptor=target, + ) + + with pytest.raises(ValueError, match="Registered semantic call"): + SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + call_catalog=catalog.with_descriptor(custom), + ) + + +def test_standard_registration_rejects_nonbuiltin_effect_monitor() -> None: + """Custom effect-monitor factories cannot be injected after registration.""" + base = create_cube_robot_profile_binding().presets[0] + preset = SkillPolicyPreset( + "safe", + action_option_templates=base.action_option_templates, + motion_policy=base.motion_policy, + tracking_policy=base.tracking_policy, + recovery_policy=base.recovery_policy, + runner_cfg=base.runner_cfg, + effect_monitors={"pick": EffectMonitorRef("test.monitor", "1")}, + ) + + with pytest.raises(ValueError, match="non-built-in effect monitor"): + _registration_with_preset(preset) + + +def test_standard_registration_rejects_tracking_metric_without_builtin_evaluator() -> ( + None +): + """Metric evaluator availability is proven before simulation startup.""" + base = create_cube_robot_profile_binding().presets[0] + preset = SkillPolicyPreset( + "safe", + action_option_templates=base.action_option_templates, + motion_policy=base.motion_policy, + tracking_policy=TrackingPolicy( + in_flight=InFlightTrackingPolicy( + metrics=(_CatalogCustomTrackingMetric(),), + ), + terminal=TimedTerminalAcceptance(), + ), + recovery_policy=base.recovery_policy, + runner_cfg=base.runner_cfg, + effect_monitors=base.effect_monitors, + ) + + with pytest.raises(ValueError, match="no exact built-in evaluator"): + _registration_with_preset(preset) + + +@pytest.mark.parametrize("validation_stage", ("decode", "preflight")) +def test_catalog_rejects_unknown_named_articulation_target_at_exact_path( + validation_stage: str, +) -> None: + """Unknown provider-owned target IDs fail before simulation startup.""" + catalog = OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION.catalog + payload = _operate_articulation_payload(target="does_not_exist") + + with pytest.raises(ExpertProgramValidationError) as error: + if validation_stage == "decode": + decode_expert_program(payload, validation_context=catalog) + else: + catalog.preflight(decode_expert_program(payload)) + + assert error.value.code == "unknown_articulation_operation_target" + assert error.value.path == ("program", "call", "target") + + +@pytest.mark.parametrize("handle", (None, DRAWER_HANDLE_AFFORDANCE_ID)) +def test_catalog_accepts_named_target_through_default_or_explicit_affordance( + handle: str | None, +) -> None: + """Both handle-selection forms resolve the same registered target table.""" + catalog = OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION.catalog + program = decode_expert_program( + _operate_articulation_payload(target="open", handle=handle), + validation_context=catalog, + ) + + compiled = catalog.preflight(program) + + call = tuple(compiled.iter_segments())[0].calls[0].call + assert type(call) is OperateArticulation + assert call.target == "open" + + +def test_catalog_owns_immutable_articulation_operation_target_metadata() -> None: + """Named target IDs are a read-only task-registration catalog surface.""" + targets = ( + OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION.catalog.articulation_operation_targets + ) + + assert targets == {DRAWER_HANDLE_AFFORDANCE_ID: frozenset({"open"})} + with pytest.raises(TypeError): + targets[DRAWER_HANDLE_AFFORDANCE_ID] = frozenset() # type: ignore[index] + + +def test_catalog_rejects_linked_place_relation_without_exact_grounder() -> None: + """A linked affordance cannot defer a missing typed grounder to runtime.""" + catalog = _place_relation_catalog(install_grounder_key=False) + program = decode_expert_program( + _place_relation_payload(), + validation_context=catalog, + ) + + with pytest.raises(ExpertProgramValidationError) as error: + catalog.preflight(program) + + assert error.value.code == "relation_grounder_not_registered" + assert error.value.path == ("program", "call", "on") + + +def test_catalog_accepts_linked_place_relation_with_exact_grounder_key() -> None: + """The capability, payload type, and revision must all match exactly.""" + catalog = _place_relation_catalog(install_grounder_key=True) + program = decode_expert_program( + _place_relation_payload(), + validation_context=catalog, + ) + + compiled = catalog.preflight(program) + + assert tuple(compiled.iter_segments())[0].calls[0].call.semantic_id == "place" + + +def test_standard_support_binding_installs_grounder_without_task_code() -> None: + """A task relation declaration supplies its production grounder implicitly.""" + base = create_cube_scene_binding(grasp_samples=32) + scene = replace( + base, + registry_id="relation_scene", + rigid_objects=( + *base.rigid_objects, + SimulationRigidObjectBinding( + entity_id="support", + simulation_uid="support", + dynamics=SceneDynamics.STATIC, + semantic_type="support_surface", + ), + ), + support_surfaces=( + SupportSurfaceAffordanceBinding( + entity_id="support_top", + parent_id="support", + native_name="top_object_target", + is_default=True, + ), + ), + ) + registration = SimulationExpertProgramRegistration( + scene_binding=scene, + robot_profile_binding=create_cube_robot_profile_binding(), + ) + + assert len(registration.relation_grounders) == 1 + assert type(registration.relation_grounders[0]) is ( + SupportSurfaceRelationTargetGrounder + ) + assert registration.catalog.relation_grounder_keys == frozenset( + { + ( + PLACE_ON_AFFORDANCE_CAPABILITY, + SupportSurfaceAffordance, + PLACEMENT_TARGET_AFFORDANCE_REVISION, + ) + } + ) + + program = decode_expert_program( + _place_relation_payload(), + validation_context=registration.catalog, + ) + compiled = registration.catalog.preflight(program) + + assert tuple(compiled.iter_segments())[0].calls[0].call.semantic_id == "place" + + +@pytest.mark.parametrize( + ("overrides", "path"), + ( + ({"scene_registry": "other_scene"}, ("integration",)), + ({"runtime_preset": "unknown"}, ("integration",)), + ({"object_id": "unknown_object"}, ("program", "call", "object")), + ), +) +def test_catalog_rejects_unknown_references_at_decode_time( + overrides: dict[str, str], + path: tuple[str, ...], +) -> None: + """Invalid task integration references retain exact config paths.""" + registration = _registration() + + with pytest.raises(ExpertProgramValidationError) as error: + decode_expert_program( + _program_payload(**overrides), + validation_context=registration.catalog, + ) + + assert error.value.path == path + + +def test_scene_declare_rejects_orphan_link_without_simulation() -> None: + """Canonical topology failures do not reach native entity lookup.""" + binding = SimulationSceneBinding( + registry_id="orphan_scene", + links=( + SimulationArticulationLinkBinding( + entity_id="handle", + articulation_id="missing_drawer", + native_link_name="handle_link", + ), + ), + ) + + with pytest.raises(KeyError, match="missing_drawer"): + binding.declare() + + +def test_fingerprint_is_stable_for_equivalent_declarations() -> None: + """Fresh equivalent registrations produce the same canonical digest.""" + left = _registration() + right = _registration() + + assert left.fingerprint == right.fingerprint + assert len(left.fingerprint) == 64 + + +def test_fingerprint_covers_workflow_recovery_policy() -> None: + """A recovery budget is immutable registration-owned runtime behavior.""" + base = create_cube_robot_profile_binding().presets[0] + changed = SkillPolicyPreset( + base.preset_id, + schema_version=base.schema_version, + action_option_templates=base.action_option_templates, + motion_policy=base.motion_policy, + tracking_policy=base.tracking_policy, + recovery_policy=base.recovery_policy, + workflow_recovery_policy=WorkflowRecoveryPolicy( + max_recovery_attempts=1, + ), + runner_cfg=base.runner_cfg, + effect_monitors=base.effect_monitors, + ) + + assert _registration().fingerprint != _registration_with_preset(changed).fingerprint + + +def test_fingerprint_is_independent_of_catalog_and_provider_insertion_order() -> None: + """Semantically equivalent unordered registration inputs hash identically.""" + descriptors = tuple(builtin_semantic_call_catalog().descriptors.values()) + first_relation = _CatalogRelationGrounder() + second_relation = _SecondCatalogRelationGrounder() + first_handover = _CatalogHandOverPoseProvider(transfer_height=0.6) + second_handover = _SecondCatalogHandOverPoseProvider() + common = { + "scene_binding": create_cube_scene_binding(grasp_samples=32), + "robot_profile_binding": create_cube_robot_profile_binding(), + } + forward = SimulationExpertProgramRegistration( + **common, + call_catalog=SemanticCallCatalog(descriptors), + relation_grounders=(first_relation, second_relation), + handover_pose_providers=(first_handover, second_handover), + ) + reversed_registration = SimulationExpertProgramRegistration( + **common, + call_catalog=SemanticCallCatalog(tuple(reversed(descriptors))), + relation_grounders=(second_relation, first_relation), + handover_pose_providers=(second_handover, first_handover), + ) + + assert forward.fingerprint == reversed_registration.fingerprint + + +def test_fingerprint_owns_provider_ids_and_declarative_fields() -> None: + """Provider identity and dataclass configuration are registration data.""" + provider = _CatalogHandOverPoseProvider(transfer_height=0.6) + registration = SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + relation_grounders=(_CatalogRelationGrounder(),), + handover_pose_providers=(provider,), + ) + changed_value = SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + relation_grounders=(_CatalogRelationGrounder(),), + handover_pose_providers=(_CatalogHandOverPoseProvider(transfer_height=0.7),), + ) + + assert registration.handover_pose_providers == (provider,) + assert registration.fingerprint != changed_value.fingerprint + object.__setattr__(provider, "transfer_height", 0.8) + with pytest.raises(IntegrationFingerprintMismatch, match="changed"): + registration.assert_unchanged() + + +def test_fingerprint_rejects_opaque_nested_declaration_values() -> None: + """Unknown nested values cannot silently collapse to their Python type.""" + with pytest.raises(TypeError, match="unsupported value type"): + SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + handover_pose_providers=(_OpaqueHandOverPoseProvider(opaque=object()),), + ) + + +def test_registration_rejects_duplicate_provider_keys_and_ids() -> None: + """Provider lookup tables remain unambiguous before simulation startup.""" + common = { + "scene_binding": create_cube_scene_binding(grasp_samples=32), + "robot_profile_binding": create_cube_robot_profile_binding(), + } + + with pytest.raises(ValueError, match="Duplicate relation grounder key"): + SimulationExpertProgramRegistration( + **common, + relation_grounders=( + _CatalogRelationGrounder(), + _CatalogRelationGrounder(), + ), + ) + with pytest.raises(ValueError, match="Duplicate handover pose provider"): + SimulationExpertProgramRegistration( + **common, + handover_pose_providers=( + _CatalogHandOverPoseProvider(transfer_height=0.6), + _CatalogHandOverPoseProvider(transfer_height=0.7), + ), + ) + + +def test_registration_requires_immutable_provider_tuples() -> None: + """Mutable provider containers cannot enter task registration metadata.""" + with pytest.raises(TypeError, match="relation_grounders must be an exact tuple"): + SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + relation_grounders=[_CatalogRelationGrounder()], # type: ignore[arg-type] + ) + with pytest.raises( + TypeError, + match="handover_pose_providers must be an exact tuple", + ): + SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + handover_pose_providers=[ # type: ignore[arg-type] + _CatalogHandOverPoseProvider(transfer_height=0.6) + ], + ) + + +@pytest.mark.parametrize( + ("field_name", "provider"), + ( + ("relation_grounders", _StatefulCatalogRelationGrounder()), + ("handover_pose_providers", _PrivateSlotHandOverPoseProvider()), + ( + "handover_pose_providers", + _InheritedCachedHandOverPoseProvider(), + ), + ), +) +def test_registration_rejects_stateful_non_dataclass_providers( + field_name: str, + provider: object, +) -> None: + """Public and name-mangled provider state cannot evade fingerprinting.""" + kwargs = {field_name: (provider,)} + + with pytest.raises(TypeError, match="Use a frozen dataclass"): + SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + **kwargs, + ) + + +def test_registration_rejects_nested_mutable_relation_grounder_state() -> None: + """Catalog providers reuse the standard recursive immutability boundary.""" + with pytest.raises(TypeError, match="deeply immutable"): + SimulationExpertProgramRegistration( + scene_binding=create_cube_scene_binding(grasp_samples=32), + robot_profile_binding=create_cube_robot_profile_binding(), + relation_grounders=(_NestedMutableCatalogRelationGrounder(offsets=[0.1]),), + ) + + +def test_nested_declaration_drift_is_detected_before_live_build() -> None: + """Mutable nested config cannot silently change a registered binding.""" + registration = _registration() + generator_cfg = registration.scene_binding.antipodal_grasps[0].generator_cfg + assert generator_cfg is not None + generator_cfg.antipodal_sampler_cfg.n_sample = 64 + + with pytest.raises(IntegrationFingerprintMismatch, match="changed"): + registration.assert_unchanged() + + +def test_env_spec_keeps_typed_registration_out_of_gym_kwargs() -> None: + """The integration catalog is metadata, not a duplicated Gym config source.""" + + class _Environment: + pass + + registration = _registration() + spec = EnvSpec( + "CatalogTest-v1", + _Environment, + default_kwargs={"physical_option": 3}, + expert_program_registration=registration, + ) + + assert spec.expert_program_registration is registration + assert spec.gym_spec.kwargs == {"physical_option": 3} diff --git a/tests/gym/envs/expert_program/test_cfg.py b/tests/gym/envs/expert_program/test_cfg.py new file mode 100644 index 000000000..a504f371c --- /dev/null +++ b/tests/gym/envs/expert_program/test_cfg.py @@ -0,0 +1,206 @@ +# ---------------------------------------------------------------------------- +# 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 typed Expert Program configuration values.""" + +from __future__ import annotations + +import math + +import pytest + +from embodichain.lab.gym.envs.expert_program import ( + MAX_REPEAT_COUNT, + CyclicPoseTargetCfg, + ExpertProgramCfg, + ExpertProgramIntegrationCfg, + HandOverCfg, + InvokeCfg, + ObjectNearTargetValidatorCfg, + PickCfg, + PlaceCfg, + PoseCfg, + RegisteredSemanticCallCfg, + RepeatCfg, + SegmentCfg, + SequenceCfg, + TargetRefCfg, + WaitStablePostCfg, +) +from embodichain.utils.configclass import is_configclass + + +def _integration() -> ExpertProgramIntegrationCfg: + """Return one valid provider-free integration selection.""" + return ExpertProgramIntegrationCfg( + robot_profile="auto", + scene_registry="env", + runtime_preset="safe", + ) + + +def _pick_invoke() -> InvokeCfg: + """Return one minimal semantic invocation.""" + return InvokeCfg(call=PickCfg(object="cube")) + + +def test_every_public_schema_value_uses_configclass() -> None: + classes = ( + ExpertProgramCfg, + ExpertProgramIntegrationCfg, + PoseCfg, + TargetRefCfg, + CyclicPoseTargetCfg, + PickCfg, + PlaceCfg, + HandOverCfg, + RegisteredSemanticCallCfg, + WaitStablePostCfg, + ObjectNearTargetValidatorCfg, + InvokeCfg, + SequenceCfg, + RepeatCfg, + SegmentCfg, + ) + + assert all(is_configclass(cls) for cls in classes) + + +def test_call_configs_own_resources_and_registered_payloads() -> None: + resources = {"primary": "left_actor"} + arguments = {"waypoints": [1, {"enabled": True}]} + pick = PickCfg(object="cube", resources=resources) + registered = RegisteredSemanticCallCfg( + call_id="example.inspect", + arguments=arguments, + ) + + resources["primary"] = "right_actor" + arguments["waypoints"][1]["enabled"] = False + + assert pick.resources == {"primary": "left_actor"} + assert registered.arguments == { + "waypoints": (1, {"enabled": True}), + } + + +@pytest.mark.parametrize("count", [False, 0, -1, MAX_REPEAT_COUNT + 1]) +def test_repeat_rejects_non_positive_non_integer_or_excessive_count( + count: object, +) -> None: + with pytest.raises(ValueError, match="count must be an integer"): + RepeatCfg(count=count, body=_pick_invoke()) + + +def test_program_rejects_nested_repeat_expansion_above_static_budget() -> None: + nested = RepeatCfg( + count=MAX_REPEAT_COUNT, + body=RepeatCfg(count=MAX_REPEAT_COUNT, body=_pick_invoke()), + ) + + with pytest.raises(ValueError, match="expands to more than"): + ExpertProgramCfg( + schema_version=1, + program_id="too_large", + integration=_integration(), + targets={}, + program=nested, + ) + + +@pytest.mark.parametrize( + ("kwargs", "message"), + [ + ({}, "exactly one"), + ( + { + "at": TargetRefCfg(target="drop"), + "on": "tray", + }, + "exactly one", + ), + ], +) +def test_place_requires_exactly_one_typed_destination( + kwargs: dict[str, object], + message: str, +) -> None: + with pytest.raises(ValueError, match=message): + PlaceCfg(object="cube", **kwargs) + + +def test_programmatic_config_rejects_unknown_target_reference() -> None: + program = InvokeCfg( + call=PlaceCfg( + object="cube", + at=TargetRefCfg(target="missing"), + ) + ) + + with pytest.raises(ValueError, match="Unknown target reference 'missing'"): + ExpertProgramCfg( + schema_version=1, + program_id="missing_target", + integration=_integration(), + targets={}, + program=program, + ) + + +@pytest.mark.parametrize( + "arguments", + [ + {"callback": lambda: None}, + {"eval": "1 + 1"}, + {"source": "env.robot.control_parts"}, + {"bad": math.inf}, + ], +) +def test_registered_call_rejects_executable_or_non_declarative_payload( + arguments: dict[str, object], +) -> None: + with pytest.raises((TypeError, ValueError)): + RegisteredSemanticCallCfg( + call_id="example.inspect", + arguments=arguments, + ) + + +def test_pose_rejects_zero_quaternion() -> None: + with pytest.raises(ValueError, match="non-zero magnitude"): + PoseCfg( + position=(0.0, 0.0, 0.0), + quaternion_wxyz=(0.0, 0.0, 0.0, 0.0), + ) + + +def test_segment_owns_post_policy_and_validator_sequences() -> None: + post = [WaitStablePostCfg(entity="cube")] + validators = [ObjectNearTargetValidatorCfg(object="cube", target="drop_pose")] + segment = SegmentCfg( + name="move_cube", + steps=SequenceCfg(items=(_pick_invoke(),)), + post=post, + validators=validators, + ) + + post.clear() + validators.clear() + + assert segment.post == (WaitStablePostCfg(entity="cube"),) + assert segment.validators == ( + ObjectNearTargetValidatorCfg(object="cube", target="drop_pose"), + ) diff --git a/tests/gym/envs/expert_program/test_compiler.py b/tests/gym/envs/expert_program/test_compiler.py new file mode 100644 index 000000000..afed14046 --- /dev/null +++ b/tests/gym/envs/expert_program/test_compiler.py @@ -0,0 +1,548 @@ +# ---------------------------------------------------------------------------- +# 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 provider-free Expert Program compilation and lazy expansion.""" + +from __future__ import annotations + +from itertools import islice + +import pytest +import torch + +from embodichain.lab.gym.envs.expert_program import ( + CyclicPoseTargetCfg, + ExpertProgramCfg, + ExpertProgramCompileError, + ExpertProgramCompiler, + ExpertProgramIntegrationCfg, + HandOverCfg, + InvokeCfg, + MaterializedCompiledProgram, + ObjectNearTargetValidatorCfg, + PickCfg, + PlaceCfg, + PoseCfg, + RegisteredSemanticCallCfg, + RepeatCfg, + SegmentCfg, + SequenceCfg, + TargetRefCfg, + WaitStablePostCfg, +) +from embodichain.lab.sim.atomic_actions import Affordance, EntityState +from embodichain.lab.sim.skills.calls import ( + HandOver, + Pick, + Place, + RegisteredSemanticCall, + SemanticCallSpec, + SemanticPose, +) +from embodichain.lab.sim.skills.scene import ( + SceneAffordanceRef, + SceneArticulationRef, + SceneEntityRegistration, + SceneLinkRef, + SceneObjectRef, + SceneRegistry, +) + + +class _NeverObserveProvider: + """Record and reject every attempted dynamic scene observation.""" + + def __init__(self) -> None: + self.calls = 0 + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp, env_ids + self.calls += 1 + raise AssertionError("Expert Program compilation must not observe state.") + + +def _scene_registry() -> tuple[SceneRegistry, _NeverObserveProvider]: + """Return static identities backed by a provider that must stay unused.""" + provider = _NeverObserveProvider() + cube = SceneObjectRef("cube") + tray = SceneObjectRef("tray") + arm = SceneArticulationRef("arm") + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=cube, + state_provider=provider, + aliases=("sim_cube",), + ), + SceneEntityRegistration(ref=tray, state_provider=provider), + SceneEntityRegistration(ref=arm, state_provider=provider), + SceneEntityRegistration( + ref=SceneLinkRef("arm_tcp"), + state_provider=provider, + parent=arm, + native_name="tcp", + ), + SceneEntityRegistration( + ref=SceneAffordanceRef("cube_grasp"), + aliases=("legacy_grasp",), + parent=cube, + native_name="grasp", + affordance=Affordance(), + relative_pose=torch.eye(4), + ), + SceneEntityRegistration( + ref=SceneAffordanceRef("tray_top"), + parent=tray, + native_name="top", + affordance=Affordance(), + relative_pose=torch.eye(4), + ), + ) + ) + return registry, provider + + +def _integration() -> ExpertProgramIntegrationCfg: + """Return one static integration selection.""" + return ExpertProgramIntegrationCfg( + robot_profile="auto", + scene_registry="env", + runtime_preset="safe", + ) + + +def _pose(x: float, y: float = 0.0, z: float = 0.2) -> PoseCfg: + """Build one target pose with an identity WXYZ quaternion.""" + return PoseCfg( + position=(x, y, z), + quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), + ) + + +def _program( + node: InvokeCfg | SequenceCfg | RepeatCfg | SegmentCfg, + *, + targets: dict[str, CyclicPoseTargetCfg] | None = None, + program_id: str = "test_program", +) -> ExpertProgramCfg: + """Build one valid Version 1 program around a supplied node.""" + return ExpertProgramCfg( + schema_version=1, + program_id=program_id, + integration=_integration(), + program=node, + targets={} if targets is None else targets, + ) + + +def _assert_pose_equal(actual: SemanticPose, expected: SemanticPose) -> None: + """Compare owned pose tensor values.""" + assert torch.allclose(actual.position, expected.position) + assert torch.allclose(actual.quaternion_wxyz, expected.quaternion_wxyz) + + +def _assert_semantic_call_equal( + actual: SemanticCallSpec, + expected: SemanticCallSpec, +) -> None: + """Compare exact semantic call values whose public classes use eq=False.""" + assert type(actual) is type(expected) + assert dict(actual.resources) == dict(expected.resources) + if type(actual) is Pick and type(expected) is Pick: + assert actual.object == expected.object + assert actual.grasp == expected.grasp + elif type(actual) is Place and type(expected) is Place: + assert actual.object == expected.object + assert actual.on == expected.on + assert actual.inside == expected.inside + assert (actual.at is None) == (expected.at is None) + if actual.at is not None and expected.at is not None: + _assert_pose_equal(actual.at, expected.at) + elif type(actual) is HandOver and type(expected) is HandOver: + assert actual.object == expected.object + assert (actual.final_target is None) == (expected.final_target is None) + if actual.final_target is not None and expected.final_target is not None: + _assert_pose_equal(actual.final_target, expected.final_target) + elif ( + type(actual) is RegisteredSemanticCall + and type(expected) is RegisteredSemanticCall + ): + assert actual.call_id == expected.call_id + assert actual.arguments == expected.arguments + else: # pragma: no cover - exact supported union is exhausted above + raise AssertionError(f"Unsupported call type {type(actual).__name__}.") + + +def test_compiler_matches_direct_python_semantic_calls_and_sequence_order() -> None: + registry, provider = _scene_registry() + target = _pose(0.5, 0.1) + config = _program( + SequenceCfg( + items=( + InvokeCfg( + call=PickCfg( + object="sim_cube", + grasp="legacy_grasp", + resources={"primary": "left_actor"}, + ) + ), + InvokeCfg(call=PlaceCfg(object="sim_cube", on="tray_top")), + InvokeCfg( + call=HandOverCfg( + object="sim_cube", + resources={"destination": "right_actor"}, + final_target=TargetRefCfg(target="handover_pose"), + ) + ), + InvokeCfg( + call=RegisteredSemanticCallCfg( + call_id="example.inspect", + arguments={ + "labels": ["front", "back"], + "options": {"confidence": 0.9}, + }, + ) + ), + ) + ), + targets={"handover_pose": CyclicPoseTargetCfg(values=(target,))}, + ) + + compiled = ExpertProgramCompiler.from_scene_registry(registry).compile(config) + segments = list(compiled) + + expected = ( + Pick( + object=SceneObjectRef("cube"), + grasp=SceneAffordanceRef("cube_grasp"), + resources={"primary": "left_actor"}, + ), + Place( + object=SceneObjectRef("cube"), + on=SceneAffordanceRef("tray_top"), + ), + HandOver( + object=SceneObjectRef("cube"), + resources={"destination": "right_actor"}, + final_target=SemanticPose(target.position, target.quaternion_wxyz), + ), + RegisteredSemanticCall( + call_id="example.inspect", + arguments={ + "labels": ("front", "back"), + "options": {"confidence": 0.9}, + }, + ), + ) + assert len(segments) == len(expected) + assert all(segment.implicit for segment in segments) + assert [segment.segment_index for segment in segments] == list(range(4)) + assert [segment.calls[0].call_index for segment in segments] == list(range(4)) + assert len({segment.segment_id for segment in segments}) == 4 + for segment, expected_call in zip(segments, expected, strict=True): + _assert_semantic_call_equal(segment.calls[0].call, expected_call) + assert provider.calls == 0 + + +def test_repeat_expands_independent_segments_with_cyclic_targets() -> None: + registry, provider = _scene_registry() + poses = (_pose(0.45, -0.2), _pose(0.45, 0.0), _pose(0.45, 0.2)) + body = SegmentCfg( + name="move_cube", + steps=SequenceCfg( + items=( + InvokeCfg(call=PickCfg(object="cube")), + InvokeCfg( + call=PlaceCfg( + object="cube", + at=TargetRefCfg(target="drop_pose"), + ) + ), + ) + ), + post=(WaitStablePostCfg(entity="cube"),), + validators=(ObjectNearTargetValidatorCfg(object="cube", target="drop_pose"),), + ) + config = _program( + RepeatCfg(count=3, body=body), + targets={"drop_pose": CyclicPoseTargetCfg(values=poses)}, + program_id="repeated_cube", + ) + + compiled = ExpertProgramCompiler.from_scene_registry(registry).compile(config) + segments = list(compiled) + second_pass = list(compiled) + + assert [segment.segment_id for segment in segments] == [ + segment.segment_id for segment in second_pass + ] + assert len(segments) == 3 + assert len({segment.segment_id for segment in segments}) == 3 + assert [segment.segment_index for segment in segments] == [0, 1, 2] + assert [call.call_index for segment in segments for call in segment.calls] == list( + range(6) + ) + assert all(not segment.implicit for segment in segments) + assert all(segment is not other for segment, other in zip(segments, second_pass)) + for index, (segment, pose) in enumerate(zip(segments, poses, strict=True)): + assert len(segment.repeat_frames) == 1 + assert segment.repeat_frames[0].path == ("program",) + assert segment.repeat_frames[0].iteration_index == index + assert segment.repeat_frames[0].count == 3 + place = segment.calls[1] + assert type(place.call) is Place + assert place.call.at is not None + _assert_pose_equal( + place.call.at, + SemanticPose(pose.position, pose.quaternion_wxyz), + ) + assert place.target_selections[0].value_index == index + validator = segment.validators[0] + _assert_pose_equal(validator.target_pose, place.call.at) + assert validator.target_selection == place.target_selections[0] + assert segment.post_policies[0].entity == SceneObjectRef("cube") + assert provider.calls == 0 + + +def test_repeat_expansion_is_lazy_and_never_observes_scene_providers() -> None: + registry, provider = _scene_registry() + config = _program( + RepeatCfg( + count=1_000, + body=InvokeCfg(call=PickCfg(object="sim_cube")), + ) + ) + + compiled = ExpertProgramCompiler.from_scene_registry(registry).compile(config) + iterator = iter(compiled) + + assert provider.calls == 0 + first_two = list(islice(iterator, 2)) + assert [segment.segment_index for segment in first_two] == [0, 1] + assert [segment.repeat_frames[0].iteration_index for segment in first_two] == [ + 0, + 1, + ] + assert provider.calls == 0 + + +def test_materialized_program_builds_cross_segment_analysis_windows_provider_free() -> ( + None +): + registry, provider = _scene_registry() + config = _program( + SequenceCfg( + items=( + SegmentCfg( + name="pick", + steps=InvokeCfg(call=PickCfg(object="cube")), + ), + SegmentCfg( + name="place", + steps=InvokeCfg( + call=PlaceCfg( + object="cube", + at=TargetRefCfg(target="drop_pose"), + ) + ), + ), + ) + ), + targets={"drop_pose": CyclicPoseTargetCfg(values=(_pose(0.5),))}, + ) + + materialized = ( + ExpertProgramCompiler.from_scene_registry(registry) + .compile(config) + .materialize() + ) + preflight = materialized.preflight_analyses() + execution = materialized.sequential_execution_analysis(0) + + assert type(materialized) is MaterializedCompiledProgram + assert materialized.segment_count == 2 + assert len(tuple(materialized.iter_segments())) == 2 + assert len(preflight) == 1 + assert preflight[0].kind == "sequential_stretch" + assert [type(call) for call in preflight[0].calls] == [Pick, Place] + assert execution.kind == "sequential_suffix" + assert execution.execution_prefix_length == 1 + assert [type(call) for call in execution.calls] == [Pick, Place] + assert provider.calls == 0 + + +def test_materialization_rechecks_expanded_call_bound_after_config_mutation() -> None: + registry, provider = _scene_registry() + inner = RepeatCfg(count=1, body=InvokeCfg(call=PickCfg(object="cube"))) + config = _program(RepeatCfg(count=1, body=inner)) + assert type(config.program) is RepeatCfg + assert type(config.program.body) is RepeatCfg + config.program.count = 1_000 + config.program.body.count = 1_000 + compiled = ExpertProgramCompiler.from_scene_registry(registry).compile(config) + + with pytest.raises(ExpertProgramCompileError) as error: + compiled.materialize() + + assert error.value.code == "expanded_call_limit" + assert provider.calls == 0 + + +def test_wait_stable_accepts_every_canonical_scene_entity_subtype() -> None: + registry, provider = _scene_registry() + config = _program( + SegmentCfg( + name="link_settle", + steps=InvokeCfg(call=PickCfg(object="cube")), + post=(WaitStablePostCfg(entity="arm_tcp"),), + ) + ) + + segment = next( + iter(ExpertProgramCompiler.from_scene_registry(registry).compile(config)) + ) + + assert segment.post_policies[0].entity == SceneLinkRef("arm_tcp") + assert provider.calls == 0 + + +def test_compiled_program_owns_source_and_each_emitted_mutable_config() -> None: + registry, _ = _scene_registry() + target = CyclicPoseTargetCfg(values=(_pose(0.4), _pose(0.5))) + registered = RegisteredSemanticCallCfg( + call_id="example.inspect", + arguments={"settings": {"enabled": True}}, + ) + repeat = RepeatCfg( + count=2, + body=SegmentCfg( + name="inspect_and_place", + steps=SequenceCfg( + items=( + InvokeCfg(call=registered), + InvokeCfg( + call=PlaceCfg( + object="cube", + at=TargetRefCfg(target="drop_pose"), + ) + ), + ) + ), + post=(WaitStablePostCfg(entity="cube", preset="rigid_object"),), + validators=( + ObjectNearTargetValidatorCfg( + object="cube", + target="drop_pose", + position_tolerance=0.03, + ), + ), + ), + ) + config = _program(repeat, targets={"drop_pose": target}) + compiled = ExpertProgramCompiler.from_scene_registry(registry).compile(config) + + compiled_source = config.program + assert type(compiled_source) is RepeatCfg + source_segment = compiled_source.body + assert type(source_segment) is SegmentCfg + source_steps = source_segment.steps + assert type(source_steps) is SequenceCfg + source_registered = source_steps.items[0].call + assert type(source_registered) is RegisteredSemanticCallCfg + compiled_source.count = 1 + config.targets["drop_pose"].values = (_pose(9.0),) + source_registered.arguments["settings"]["enabled"] = False + source_segment.post[0].preset = "changed" + source_segment.validators[0].position_tolerance = 9.0 + + first_pass = list(compiled) + assert len(first_pass) == 2 + first_registered = first_pass[0].calls[0].call + assert type(first_registered) is RegisteredSemanticCall + assert first_registered.arguments["settings"]["enabled"] is True + first_place = first_pass[0].calls[1].call + assert type(first_place) is Place and first_place.at is not None + assert first_place.at.position[0].item() == pytest.approx(0.4) + assert first_pass[0].post_policies[0].cfg.preset == "rigid_object" + assert first_pass[0].validators[0].cfg.position_tolerance == pytest.approx(0.03) + + first_pass[0].post_policies[0].cfg.preset = "mutated_output" + first_pass[0].validators[0].cfg.position_tolerance = 8.0 + exposed_position = compiled.targets["drop_pose"][0].position + exposed_position[0] = -10.0 + + second_pass = list(compiled) + assert second_pass[0].post_policies[0].cfg.preset == "rigid_object" + assert second_pass[0].validators[0].cfg.position_tolerance == pytest.approx(0.03) + assert compiled.targets["drop_pose"][0].position[0].item() == pytest.approx(0.4) + + +def test_compiler_rejects_nested_segment_at_exact_path() -> None: + registry, _ = _scene_registry() + config = _program( + SegmentCfg( + name="outer", + steps=SegmentCfg( + name="inner", + steps=InvokeCfg(call=PickCfg(object="cube")), + ), + ) + ) + + with pytest.raises(ExpertProgramCompileError) as error: + ExpertProgramCompiler.from_scene_registry(registry).compile(config) + + assert error.value.code == "nested_segment" + assert error.value.path == ("program", "steps") + + +def test_compiler_reports_typed_scene_mismatch_at_reference_site() -> None: + registry, _ = _scene_registry() + config = _program( + InvokeCfg(call=PickCfg(object="tray_top")), + ) + + with pytest.raises(ExpertProgramCompileError) as error: + ExpertProgramCompiler.from_scene_registry(registry).compile(config) + + assert error.value.code == "scene_reference_type_mismatch" + assert error.value.path == ("program", "call", "object") + + +def test_compiler_rechecks_mutated_repeat_and_target_bounds() -> None: + registry, _ = _scene_registry() + repeat = RepeatCfg(count=1, body=InvokeCfg(call=PickCfg(object="cube"))) + target = CyclicPoseTargetCfg(values=(_pose(0.4),)) + config = _program(repeat, targets={"drop_pose": target}) + assert type(config.program) is RepeatCfg + config.program.count = 0 + + with pytest.raises(ExpertProgramCompileError) as repeat_error: + ExpertProgramCompiler.from_scene_registry(registry).compile(config) + assert repeat_error.value.code == "invalid_repeat_count" + assert repeat_error.value.path == ("program", "count") + + config.program.count = 1 + config.targets["drop_pose"].values = () + with pytest.raises(ExpertProgramCompileError) as target_error: + ExpertProgramCompiler.from_scene_registry(registry).compile(config) + assert target_error.value.code == "empty_target_values" + assert target_error.value.path == ("targets", "drop_pose", "values") diff --git a/tests/gym/envs/expert_program/test_completion_metadata.py b/tests/gym/envs/expert_program/test_completion_metadata.py new file mode 100644 index 000000000..3bfc46fac --- /dev/null +++ b/tests/gym/envs/expert_program/test_completion_metadata.py @@ -0,0 +1,505 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Completion-trace audit across semantic execution and the Gym bridge.""" + +from __future__ import annotations + +from dataclasses import dataclass +import json +from types import SimpleNamespace +from typing import ClassVar +from unittest.mock import Mock + +import torch + +from embodichain.lab.gym.envs.expert_program.bridge import ( + AtomicDemoBridge, + BufferedGymCommandSink, + EnvironmentStepClock, + RuntimeCommandFrameEncoder, +) +from embodichain.lab.sim.atomic_actions import ( + ActionInvocation, + ActionOptions, + ActionPlan, + AtomicAction, + AtomicActionEngine, + EndpointCommand, + EntityState, + JointPositionPayload, + JointPositionTarget, + MotionPolicy, + PlannerDiagnostics, + PlanningContext, + RecoveryPolicy, + ResolvedActionRequest, + RobotObservation, + RuntimeCommandFrame, + SceneSnapshot, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, + TaskState, + TimedCommandSequence, +) +from embodichain.lab.sim.skills.calls import RegisteredSemanticCall +from embodichain.lab.sim.skills.compiler import SemanticSkillCompiler +from embodichain.lab.sim.skills.runtime import SkillRuntime, SkillStatus +from embodichain.lab.sim.skills.scene import SceneRegistry + +STEP_DT = 0.02 +BATCH_SIZE = 2 +ROBOT_DOF = 5 +ENV_IDS = torch.tensor([7, 3], dtype=torch.long) +INITIAL_SCENE_VERSION = 41 +REPLANNED_SCENE_VERSION = 42 +INITIAL_COLLISION_REVISIONS = (5, 7) +REPLANNED_COLLISION_REVISIONS = (6, 8) + + +@dataclass(frozen=True, slots=True) +class _TraceGoal: + """Test goal for a deterministic two-phase runtime command sequence.""" + + goal_kind: ClassVar[str] = "completion_trace" + + +class _TraceAction(AtomicAction[_TraceGoal, ActionOptions]): + """Emit named segments and preserve distinct diagnostics on every replan.""" + + skill_id: ClassVar[str] = "completion_trace" + GoalType: ClassVar[type] = _TraceGoal + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=(SkillEndpointRequirement(endpoint_id="motion"),), + ), + ) + ) + + def __init__(self) -> None: + super().__init__() + self.plan_count = 0 + + def _scene_dependencies( + self, + request: ResolvedActionRequest[_TraceGoal, ActionOptions], + ) -> tuple[str, ...]: + del request + return ("trace_target",) + + def _plan( + self, + request: ResolvedActionRequest[_TraceGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + self.require_goal(request) + generation = self.plan_count + self.plan_count += 1 + target = request.binding.endpoint( + "primary", + "motion", + ).require_target(JointPositionTarget) + frames = tuple( + RuntimeCommandFrame( + commands=( + EndpointCommand( + target=target, + payload=JointPositionPayload( + torch.full( + (context.batch_size, len(target.joint_ids)), + float(generation + phase_index + 1), + device=context.robot.qpos.device, + dtype=context.robot.qpos.dtype, + ) + ), + ), + ), + active_mask=torch.ones( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ), + env_ids=context.env_ids, + hold_duration=torch.full( + (context.batch_size,), + STEP_DT, + device=context.robot.qpos.device, + dtype=context.robot.qpos.dtype, + ), + ) + for phase_index in range(2) + ) + return self.build_command_plan( + request, + context, + success=True, + commands=TimedCommandSequence(frames, context.env_ids), + replannable=True, + diagnostics=PlannerDiagnostics( + backend="completion_trace_planner", + messages=(f"installed generation {generation}",), + metadata={ + "generation": generation, + "quality": {"accepted": True, "score": generation + 0.25}, + }, + ), + segment_lengths={"approach": 1, "commit": 1}, + scene_dependency_monitor_until={"trace_target": 2}, + ) + + +class _TraceObservationProvider: + """Move the scene once and report accepted commands as observed state.""" + + def __init__(self, clock: EnvironmentStepClock) -> None: + self.clock = clock + self.calls = 0 + + def observe(self, task_state: TaskState) -> PlanningContext: + self.calls += 1 + replanned_scene = self.calls >= 2 + pose = torch.eye(4).repeat(BATCH_SIZE, 1, 1) + if replanned_scene: + pose[:, 0, 3] = 0.25 + qpos = torch.full( + (BATCH_SIZE, ROBOT_DOF), + float(min(max(self.calls - 1, 0), 3)), + ) + timestamp = self.clock.now() + return PlanningContext( + robot=RobotObservation( + timestamp=timestamp, + qpos=qpos, + qvel=torch.zeros_like(qpos), + ), + task=task_state, + scene=SceneSnapshot( + timestamp=timestamp, + version=( + REPLANNED_SCENE_VERSION + if replanned_scene + else INITIAL_SCENE_VERSION + ), + entities={"trace_target": EntityState(pose)}, + collision_world_revision=( + REPLANNED_COLLISION_REVISIONS + if replanned_scene + else INITIAL_COLLISION_REVISIONS + ), + ), + env_ids=ENV_IDS, + ) + + +class _StaticQposProvider: + """Supply full robot state to the bridge's transport encoder.""" + + def current_qpos(self, env_ids: torch.Tensor) -> torch.Tensor: + assert torch.equal(env_ids, ENV_IDS) + return torch.zeros(BATCH_SIZE, ROBOT_DOF) + + +class _UnusedEvidenceCollector: + """Satisfy the runtime port; this action declares no physical effect.""" + + def collect( + self, + spec: object, + *, + timestamp: float, + observation_revision: int, + env_ids: torch.Tensor | None = None, + ) -> dict[str, object]: + del spec, timestamp, observation_revision, env_ids + raise AssertionError("The completion trace must not request effect evidence.") + + +@dataclass(frozen=True, slots=True) +class _TraceWorkflow: + """Minimal analyzed workflow retained by the production runtime.""" + + workflow_id: str + calls: tuple[RegisteredSemanticCall, ...] + + +@dataclass(frozen=True, slots=True) +class _TraceIntegration: + """Production engine and registry exposed through the compiler boundary.""" + + engine: AtomicActionEngine + scene_registry: SceneRegistry + + +@dataclass(frozen=True, slots=True) +class _TraceGroundedCall: + """One grounded invocation with no external effect-verification boundary.""" + + analyzed: object + invocation: ActionInvocation + eligible_mask: torch.Tensor + effect_spec: None = None + effect_monitor: None = None + + +class _TraceCompiler(SemanticSkillCompiler): + """Keep semantic boundaries real while making lowering deterministic.""" + + def __init__(self, engine: AtomicActionEngine) -> None: + self._trace_integration = _TraceIntegration(engine, SceneRegistry()) + + @property + def integration(self) -> _TraceIntegration: + return self._trace_integration + + def analyze( + self, + calls: tuple[RegisteredSemanticCall, ...], + *, + workflow_id: str = "semantic_workflow", + path: tuple[object, ...] = ("workflow",), + ) -> _TraceWorkflow: + del path + return _TraceWorkflow(workflow_id, tuple(calls)) + + def ground( + self, + workflow: _TraceWorkflow, + call_index: int, + context: PlanningContext, + *, + eligible_mask: torch.Tensor | None = None, + revision: int = 0, + path: tuple[object, ...] = ("workflow",), + ) -> _TraceGroundedCall: + del context, path + assert eligible_mask is not None + binding = self.integration.engine.bind_control_parts( + _TraceAction.skill_id, + {"primary": {"motion": "arm"}}, + ) + invocation = ActionInvocation( + skill_id=_TraceAction.skill_id, + goal=_TraceGoal(), + binding=binding, + motion_policy=MotionPolicy( + strategy="ik_interp", + sample_count=9, + ), + recovery_policy=RecoveryPolicy( + max_replans=2, + max_action_retries=1, + action_timeout=1.0, + ), + invocation_id=f"{workflow.workflow_id}:{call_index}", + revision=revision, + ) + analyzed = SimpleNamespace( + bound=SimpleNamespace( + robot_profile=SimpleNamespace(profile_id="completion_trace_robot"), + preset=SimpleNamespace( + preset_id="completion_trace_preset", + schema_version=1, + motion_policy=invocation.motion_policy, + recovery_policy=invocation.recovery_policy, + ), + ) + ) + return _TraceGroundedCall( + analyzed=analyzed, + invocation=invocation, + eligible_mask=eligible_mask.clone(), + ) + + +@dataclass(frozen=True, slots=True) +class _CompiledCall: + """Program-owned semantic call and stable call index.""" + + call_index: int + call: RegisteredSemanticCall + + +@dataclass(frozen=True, slots=True) +class _CompiledSegment: + """One logical program segment consumed by the production bridge.""" + + calls: tuple[_CompiledCall, ...] + segment_index: int = 0 + segment_id: str = "completion-segment" + name: str = "completion-audit" + source_path: tuple[object, ...] = ("program", "steps", 0) + post_policies: tuple[object, ...] = () + validators: tuple[object, ...] = () + parallel_block: None = None + implicit: bool = False + + +@dataclass(frozen=True, slots=True) +class _ProgramAnalysis: + """Sequential look-ahead window selected for one bridge segment.""" + + calls: tuple[RegisteredSemanticCall, ...] + execution_prefix_length: int + + +class _CompiledProgram: + """Single-segment compiled-program port for the completion audit.""" + + schema_version = 2 + program_id = "completion-audit-program" + + def __init__(self, segment: _CompiledSegment) -> None: + self.segment = segment + + def iter_segments(self): + yield self.segment + + def sequential_execution_analysis(self, segment_index: int) -> _ProgramAnalysis: + assert segment_index == self.segment.segment_index + return _ProgramAnalysis( + tuple(compiled.call for compiled in self.segment.calls), + len(self.segment.calls), + ) + + +def _runtime_and_bridge() -> tuple[AtomicDemoBridge, _TraceAction]: + """Assemble real execution/runtime/bridge layers around deterministic ports.""" + robot = Mock() + robot.device = torch.device("cpu") + robot.dof = ROBOT_DOF + robot.control_parts = {"arm": object()} + robot.get_joint_ids.return_value = (1, 3) + robot.get_qpos.return_value = torch.zeros(BATCH_SIZE, ROBOT_DOF) + robot.get_qvel.return_value = torch.zeros(BATCH_SIZE, ROBOT_DOF) + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "completion_trace_planner" + engine = AtomicActionEngine(generator, load_builtins=False) + action = _TraceAction() + engine.register(action) + + clock = EnvironmentStepClock(STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_StaticQposProvider()), + clock, + ) + runtime = SkillRuntime.from_components( + _TraceCompiler(engine), + _TraceObservationProvider(clock), + sink, + _UnusedEvidenceCollector(), + task_state=TaskState.empty(BATCH_SIZE, "cpu"), + clock=clock, + ) + call = RegisteredSemanticCall(call_id="audit.completion_metadata") + segment = _CompiledSegment((_CompiledCall(0, call),)) + bridge = AtomicDemoBridge(_CompiledProgram(segment), runtime, sink, clock) + return bridge, action + + +def test_completion_trace_preserves_every_plan_generation_as_json_metadata() -> None: + """A real scene replan remains complete after SkillResult and bridge snapshots.""" + bridge, action = _runtime_and_bridge() + demo_segment = next(bridge.iter_segments()) + + emitted_actions = tuple(demo_segment.actions) + accepted = demo_segment.validator() + metadata = demo_segment.metadata + + serialized = json.dumps(metadata, allow_nan=False, sort_keys=True) + assert json.loads(serialized) == metadata + assert emitted_actions + assert accepted.tolist() == [True, True] + assert action.plan_count == 2 + + assert metadata["expert_program_schema_version"] == 2 + assert metadata["expert_program_id"] == "completion-audit-program" + assert metadata["program_segment_id"] == "completion-segment" + assert metadata["program_segment_index"] == 0 + assert metadata["program_segment_source_path"] == ["program", "steps", 0] + assert metadata["program_segment_implicit"] is False + assert metadata["semantic_call_indices"] == [0] + assert metadata["post_policy_count"] == 0 + assert metadata["validator_count"] == 0 + assert metadata["parallel"] is False + assert metadata["validation"]["accepted_mask"] == [True, True] + + runtime_trace = metadata["runtime"] + assert runtime_trace["kind"] == "skill_result" + assert runtime_trace["status"] == SkillStatus.COMPLETED.value + call_trace = runtime_trace["calls"][0] + assert call_trace["active_plan_attempt_generation"] == 1 + attempts = call_trace["plan_attempts"] + assert [attempt["attempt_generation"] for attempt in attempts] == [0, 1] + assert [attempt["trigger"] for attempt in attempts] == [ + "action_planned", + "replanned", + ] + assert [attempt["planned_scene_version"] for attempt in attempts] == [ + INITIAL_SCENE_VERSION, + REPLANNED_SCENE_VERSION, + ] + assert [attempt["planned_collision_world_revision"] for attempt in attempts] == [ + list(INITIAL_COLLISION_REVISIONS), + list(REPLANNED_COLLISION_REVISIONS), + ] + assert all( + attempt["scene_dependency_monitor_until"] == {"trace_target": 2} + for attempt in attempts + ) + assert all( + attempt["trajectory_segments"] + == [ + {"name": "approach", "start": 0, "stop": 1, "waypoint_count": 1}, + {"name": "commit", "start": 1, "stop": 2, "waypoint_count": 1}, + ] + for attempt in attempts + ) + assert [attempt["recovery_counters"] for attempt in attempts] == [ + {"action_retries": [0, 0], "replans": [0, 0]}, + {"action_retries": [0, 0], "replans": [1, 1]}, + ] + assert [attempt["planner_diagnostics"] for attempt in attempts] == [ + { + "backend": "completion_trace_planner", + "messages": ["installed generation 0"], + "metadata": { + "generation": 0, + "quality": {"accepted": True, "score": 0.25}, + }, + }, + { + "backend": "completion_trace_planner", + "messages": ["installed generation 1"], + "metadata": { + "generation": 1, + "quality": {"accepted": True, "score": 1.25}, + }, + }, + ] + event_kinds = [event["kind"] for event in call_trace["events"]] + assert runtime_trace["events"] == call_trace["events"] + assert "dynamic_goal_changed" in event_kinds + assert "replanned" in event_kinds + assert event_kinds[-3:] == [ + "trajectory_completed", + "action_completed", + "session_completed", + ] diff --git a/tests/gym/envs/expert_program/test_decoder.py b/tests/gym/envs/expert_program/test_decoder.py new file mode 100644 index 000000000..a2997f0cc --- /dev/null +++ b/tests/gym/envs/expert_program/test_decoder.py @@ -0,0 +1,596 @@ +# ---------------------------------------------------------------------------- +# 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 strict Expert Program Version 1 decoding.""" + +from __future__ import annotations + +from copy import deepcopy + +import pytest + +from embodichain.lab.gym.envs.expert_program import ( + MAX_REPEAT_COUNT, + ConfigPath, + ExpertProgramDecodeError, + ExpertProgramIntegrationCfg, + ExpertProgramValidationError, + HandOverCfg, + PickCfg, + PlaceCfg, + PostPolicyCfg, + RegisteredSemanticCallCfg, + RepeatCfg, + SceneReferenceRole, + SegmentCfg, + SequenceCfg, + TargetRefCfg, + ValidatorCfg, + decode_expert_program, + decode_semantic_call, + encode_semantic_call, + render_config_path, +) + + +def _program_data() -> dict[str, object]: + """Return the repeated-cube Version 1 example as plain JSON values.""" + return { + "schema_version": 1, + "program_id": "repeated_cube_pick_place", + "integration": { + "robot_profile": "auto", + "scene_registry": "env", + "runtime_preset": "safe", + }, + "targets": { + "drop_pose": { + "kind": "cyclic_pose", + "values": [ + { + "position": [0.45, -0.20, 0.20], + "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + }, + { + "position": [0.45, 0.00, 0.20], + "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + }, + { + "position": [0.45, 0.20, 0.20], + "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + }, + ], + } + }, + "program": { + "kind": "repeat", + "count": 3, + "body": { + "kind": "segment", + "name": "move_cube", + "steps": { + "kind": "sequence", + "items": [ + { + "kind": "invoke", + "call": {"kind": "pick", "object": "cube"}, + }, + { + "kind": "invoke", + "call": { + "kind": "place", + "object": "cube", + "at": { + "kind": "target_ref", + "target": "drop_pose", + }, + }, + }, + ], + }, + "post": [ + { + "kind": "wait_stable", + "entity": "cube", + "preset": "rigid_object", + } + ], + "validators": [ + { + "kind": "object_near_target", + "object": "cube", + "target": "drop_pose", + "position_tolerance": 0.03, + } + ], + }, + }, + } + + +def _invoke(call: dict[str, object]) -> dict[str, object]: + """Wrap one call mapping in an invoke node.""" + return {"kind": "invoke", "call": call} + + +@pytest.mark.parametrize( + "payload", + ( + { + "kind": "pick", + "object": "cube", + "grasp": "cube_grasp", + "resources": {"primary": "left"}, + }, + { + "kind": "place", + "object": "cube", + "at": {"kind": "target_ref", "target": "drop_pose"}, + }, + { + "kind": "hand_over", + "object": "cube", + "resources": {"destination": "right"}, + "final_target": {"kind": "target_ref", "target": "drop_pose"}, + }, + { + "kind": "operate_articulation", + "articulation": "drawer", + "handle": "drawer_handle", + "target": "open", + }, + { + "kind": "registered", + "call_id": "example.inspect", + "schema_version": 1, + "arguments": {"labels": ["front", "back"]}, + }, + ), +) +def test_public_semantic_call_codec_round_trips_exact_schema( + payload: dict[str, object], +) -> None: + call = decode_semantic_call( + payload, + target_ids=frozenset({"drop_pose"}), + ) + encoded = encode_semantic_call(call) + round_trip = decode_semantic_call( + encoded, + target_ids=frozenset({"drop_pose"}), + ) + + assert type(round_trip) is type(call) + assert encode_semantic_call(round_trip) == encoded + + +def test_public_semantic_call_decoder_owns_input_and_validates_context() -> None: + payload = {"kind": "pick", "object": "cube", "grasp": "cube_grasp"} + context = _StaticValidationContext() + + call = decode_semantic_call(payload, validation_context=context) + payload["object"] = "changed" + + assert type(call) is PickCfg + assert call.object == "cube" + assert ("call",) in context.validated_paths + assert ("call", "object") in context.validated_paths + + +def test_decoder_builds_owned_repeated_cube_ast() -> None: + data = _program_data() + + config = decode_expert_program(data) + data["program"]["count"] = 99 + data["targets"]["drop_pose"]["values"][0]["position"][0] = -1.0 + + assert type(config.program) is RepeatCfg + assert config.program.count == 3 + assert type(config.program.body) is SegmentCfg + assert type(config.program.body.steps) is SequenceCfg + place = config.program.body.steps.items[1].call + assert type(place) is PlaceCfg + assert place.at == TargetRefCfg(target="drop_pose") + assert config.targets["drop_pose"].values[0].position[0] == pytest.approx(0.45) + + +def test_decoder_supports_every_version_one_semantic_call() -> None: + data = _program_data() + data["program"] = { + "kind": "sequence", + "items": [ + _invoke( + { + "kind": "pick", + "object": "cube", + "grasp": "cube_grasp", + "resources": {"primary": "left_actor"}, + } + ), + _invoke({"kind": "place", "object": "cube", "on": "tray_top"}), + _invoke( + { + "kind": "hand_over", + "object": "cube", + "resources": {"destination": "right_actor"}, + "final_target": { + "kind": "target_ref", + "target": "drop_pose", + }, + } + ), + _invoke( + { + "kind": "registered", + "call_id": "example.inspect", + "schema_version": 1, + "arguments": { + "labels": ["front", "back"], + "options": {"confidence": 0.9}, + }, + } + ), + ], + } + + config = decode_expert_program(data) + assert [type(node.call) for node in config.program.items] == [ + PickCfg, + PlaceCfg, + HandOverCfg, + RegisteredSemanticCallCfg, + ] + handover = config.program.items[2].call + assert handover.resources == {"destination": "right_actor"} + registered = config.program.items[3].call + assert registered.arguments == { + "labels": ("front", "back"), + "options": {"confidence": 0.9}, + } + + +@pytest.mark.parametrize( + ("mutate", "expected_path"), + [ + ( + lambda data: data.update({"unexpected": True}), + "$.unexpected", + ), + ( + lambda data: data["program"]["body"].update({"unexpected": True}), + "$.program.body.unexpected", + ), + ( + lambda data: data["program"]["body"]["steps"]["items"][0]["call"].update( + {"unexpected": True} + ), + "$.program.body.steps.items[0].call.unexpected", + ), + ], +) +def test_decoder_rejects_unknown_fields_with_complete_path( + mutate: object, + expected_path: str, +) -> None: + data = _program_data() + mutate(data) + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(data) + + assert error.value.code == "unknown_field" + assert render_config_path(error.value.path) == expected_path + + +def test_decoder_reports_missing_required_field_at_exact_path() -> None: + data = _program_data() + del data["integration"]["runtime_preset"] + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(data) + + assert error.value.code == "missing_field" + assert render_config_path(error.value.path) == "$.integration.runtime_preset" + + +@pytest.mark.parametrize( + ("value", "code"), + [ + (None, "missing_discriminator"), + ("parallel", "unknown_discriminator"), + ], +) +def test_decoder_rejects_missing_or_reserved_program_discriminator( + value: str | None, + code: str, +) -> None: + data = _program_data() + if value is None: + del data["program"]["kind"] + else: + data["program"]["kind"] = value + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(data) + + assert error.value.code == code + assert error.value.path == ("program", "kind") + + +@pytest.mark.parametrize( + ("mutate", "expected_path"), + [ + ( + lambda data: data["targets"]["drop_pose"].update({"kind": "pose"}), + "$.targets.drop_pose.kind", + ), + ( + lambda data: data["program"]["body"]["steps"]["items"][0]["call"].update( + {"kind": "move"} + ), + "$.program.body.steps.items[0].call.kind", + ), + ( + lambda data: data["program"]["body"]["steps"]["items"][1]["call"][ + "at" + ].update({"kind": "env_ref"}), + "$.program.body.steps.items[1].call.at.kind", + ), + ( + lambda data: data["program"]["body"]["post"][0].update({"kind": "sleep"}), + "$.program.body.post[0].kind", + ), + ( + lambda data: data["program"]["body"]["validators"][0].update( + {"kind": "python"} + ), + "$.program.body.validators[0].kind", + ), + ], +) +def test_every_union_rejects_unknown_discriminator_at_exact_path( + mutate: object, + expected_path: str, +) -> None: + data = _program_data() + mutate(data) + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(data) + + assert error.value.code == "unknown_discriminator" + assert render_config_path(error.value.path) == expected_path + + +@pytest.mark.parametrize("schema_version", [False, 0, 3, "1"]) +def test_decoder_rejects_unsupported_top_level_schema_version( + schema_version: object, +) -> None: + data = _program_data() + data["schema_version"] = schema_version + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(data) + + assert error.value.code == "unsupported_schema_version" + assert error.value.path == ("schema_version",) + + +def test_decoder_reports_unknown_target_at_reference_site() -> None: + data = _program_data() + data["program"]["body"]["steps"]["items"][1]["call"]["at"]["target"] = "missing" + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(data) + + assert error.value.code == "unknown_target" + assert render_config_path(error.value.path) == ( + "$.program.body.steps.items[1].call.at.target" + ) + + +@pytest.mark.parametrize("count", [False, 0, MAX_REPEAT_COUNT + 1]) +def test_decoder_rejects_unbounded_or_invalid_repeat_count(count: object) -> None: + data = _program_data() + data["program"]["count"] = count + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(data) + + assert error.value.code == "invalid_repeat_count" + assert render_config_path(error.value.path) == "$.program.count" + + +def test_registered_call_schema_version_error_reports_version_field() -> None: + data = _program_data() + data["program"] = _invoke( + { + "kind": "registered", + "call_id": "example.inspect", + "schema_version": 2, + } + ) + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(data) + + assert error.value.code == "invalid_schema_version" + assert render_config_path(error.value.path) == "$.program.call.schema_version" + + +@pytest.mark.parametrize( + ("arguments", "code", "suffix"), + [ + ({"eval": "1 + 1"}, "forbidden_construct", ".arguments.eval"), + ( + {"source": "env.robot.control_parts"}, + "environment_traversal", + ".arguments.source", + ), + ( + {"source": "eval(1 + 1)"}, + "executable_expression", + ".arguments.source", + ), + ({"callback": lambda: None}, "non_declarative_value", ".arguments.callback"), + ({"live": object()}, "non_declarative_value", ".arguments.live"), + ], +) +def test_decoder_rejects_executable_traversal_or_live_registered_payload( + arguments: dict[str, object], + code: str, + suffix: str, +) -> None: + data = _program_data() + data["program"] = _invoke( + { + "kind": "registered", + "call_id": "example.inspect", + "schema_version": 1, + "arguments": arguments, + } + ) + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(data) + + assert error.value.code == code + assert render_config_path(error.value.path).endswith(suffix) + + +def test_decoder_rejects_cyclic_input_before_ast_recursion() -> None: + data = _program_data() + cyclic: dict[str, object] = {} + cyclic["self"] = cyclic + data["program"] = _invoke( + { + "kind": "registered", + "call_id": "example.inspect", + "schema_version": 1, + "arguments": cyclic, + } + ) + + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(data) + + assert error.value.code == "cyclic_input" + + +class _StaticValidationContext: + """Small provider-free reference catalog used by decoder tests.""" + + def __init__( + self, + *, + calls: set[str] | None = None, + scene: set[str] | None = None, + ) -> None: + self.calls = {"pick", "place", "hand_over"} if calls is None else calls + self.scene = {"cube", "cube_grasp", "tray_top"} if scene is None else scene + self.validated_paths: list[ConfigPath] = [] + + def validate_integration( + self, + integration: ExpertProgramIntegrationCfg, + *, + path: ConfigPath, + ) -> None: + if integration.robot_profile != "auto": + raise KeyError(integration.robot_profile) + self.validated_paths.append(path) + + def validate_semantic_call( + self, + call: object, + *, + path: ConfigPath, + ) -> None: + semantic_id = ( + call.call_id if type(call) is RegisteredSemanticCallCfg else call.kind + ) + if semantic_id not in self.calls: + raise KeyError(semantic_id) + self.validated_paths.append(path) + + def validate_scene_reference( + self, + reference: str, + *, + role: SceneReferenceRole, + path: ConfigPath, + ) -> None: + del role + if reference not in self.scene: + raise KeyError(reference) + self.validated_paths.append(path) + + def validate_post_policy( + self, + policy: PostPolicyCfg, + *, + path: ConfigPath, + ) -> None: + if policy.kind != "wait_stable" or policy.preset != "rigid_object": + raise KeyError(policy.preset) + self.validated_paths.append(path) + + def validate_validator( + self, + validator: ValidatorCfg, + *, + path: ConfigPath, + ) -> None: + if validator.kind != "object_near_target": + raise KeyError(validator.kind) + self.validated_paths.append(path) + + +def test_decoder_runs_explicit_provider_free_validation_context() -> None: + data = _program_data() + context = _StaticValidationContext() + + config = decode_expert_program(data, validation_context=context) + + assert config.program_id == "repeated_cube_pick_place" + assert ("integration",) in context.validated_paths + assert ("program", "body", "steps", "items", 0, "call") in (context.validated_paths) + assert ("program", "body", "post", 0, "entity") in (context.validated_paths) + + +def test_validation_context_failure_is_wrapped_at_exact_reference_path() -> None: + data = _program_data() + context = _StaticValidationContext(scene={"cube"}) + data["program"]["body"]["steps"]["items"][0]["call"]["grasp"] = "missing_grasp" + + with pytest.raises(ExpertProgramValidationError) as error: + decode_expert_program(data, validation_context=context) + + assert error.value.code == "reference_validation_failed" + assert render_config_path(error.value.path) == ( + "$.program.body.steps.items[0].call.grasp" + ) + + +def test_decoder_does_not_mutate_caller_input_on_failure() -> None: + data = _program_data() + data["program"]["unexpected"] = True + before = deepcopy(data) + + with pytest.raises(ExpertProgramDecodeError): + decode_expert_program(data) + + assert data == before diff --git a/tests/gym/envs/expert_program/test_environment.py b/tests/gym/envs/expert_program/test_environment.py new file mode 100644 index 000000000..a7bd5edf9 --- /dev/null +++ b/tests/gym/envs/expert_program/test_environment.py @@ -0,0 +1,963 @@ +# ---------------------------------------------------------------------------- +# 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 reusable environment-backed Expert Program assembly.""" + +from __future__ import annotations + +from collections import Counter +from unittest.mock import Mock + +import pytest +import torch + +from embodichain.lab.gym.envs.expert_program.bridge import ( + AtomicDemoBridge, + DemoBridgeError, + EnvironmentStepClock, + GymPlanningObservationProvider, +) +from embodichain.lab.gym.envs.expert_program.cfg import ( + EXPERT_PROGRAM_SCHEMA_VERSION, + EXPERT_PROGRAM_SCHEMA_VERSION_V2, + BarrierCfg, + CyclicPoseTargetCfg, + ExpertProgramCfg, + ExpertProgramIntegrationCfg, + InvokeCfg, + ObjectNearTargetValidatorCfg, + OperateArticulationCfg, + ParallelCfg, + PickCfg, + PlaceCfg, + PoseCfg, + ProgramNodeCfg, + SegmentCfg, + SequenceCfg, + TargetRefCfg, + WaitStablePostCfg, +) +from embodichain.lab.gym.envs.expert_program.environment import ( + ExpertProgramEnvironmentAdapter, + ExpertProgramEnvironmentMixin, + PlanningObservationPort, + SkillRuntimeAssemblyPort, +) +from embodichain.lab.sim.atomic_actions import ( + AntipodalAffordance, + ArticulationOperationAffordance, + ArticulationOperationTarget, + AtomicActionEngine, + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + ControlPartCommandProfile, + EntityState, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + JOINT_POSITION_CAPABILITY, + MotionPolicy, + OperateArticulationOptions, + PickUpOptions, + PlanningContext, + PlaceOptions, + RobotObservation, + TaskState, +) +from embodichain.lab.sim.skills import ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + GRASP_AFFORDANCE_CAPABILITY, + ControlPartEndpoint, + RobotResource, + RobotSkillProfile, + SceneAffordanceRef, + SceneArticulationRef, + SceneCollisionRole, + SceneCollisionWorldMode, + SceneEntityRegistration, + SceneObjectRef, + SceneRegistry, + SkillPolicyPreset, +) +from embodichain.lab.sim.skills.compiler import SemanticSkillCompiler +from embodichain.lab.sim.skills.evidence import EffectEvidenceProvider +from embodichain.lab.sim.skills.integration import SemanticValidationError + +_BATCH_SIZE = 2 +_ROBOT_DOF = 2 +_STEP_DT = 0.02 + + +class _PoseProvider: + """Return a stable owned pose for the fake environment scene.""" + + def __init__(self) -> None: + self._pose = torch.eye(4).repeat(_BATCH_SIZE, 1, 1) + self.calls = 0 + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + """Return rows aligned to the requested environment IDs.""" + del timestamp + self.calls += 1 + return EntityState(self._pose.index_select(0, env_ids)) + + +class _GeometryProvider: + """Return one opaque planner-facing geometry descriptor.""" + + def get_geometry(self) -> object: + return object() + + +def _scene_registry( + *, + dynamic_collision: bool = False, + pose_provider: _PoseProvider | None = None, +) -> SceneRegistry: + """Build an explicitly named object and default grasp affordance.""" + cube = SceneObjectRef("cube") + grasp = SceneAffordanceRef("cube_grasp") + selected_pose_provider = _PoseProvider() if pose_provider is None else pose_provider + return SceneRegistry( + ( + SceneEntityRegistration( + ref=cube, + state_provider=selected_pose_provider, + semantic_type="cube", + default_affordances={GRASP_AFFORDANCE_CAPABILITY: grasp}, + geometry_provider=(_GeometryProvider() if dynamic_collision else None), + collision_role=( + SceneCollisionRole.DYNAMIC + if dynamic_collision + else SceneCollisionRole.NONE + ), + ), + SceneEntityRegistration( + ref=grasp, + parent=cube, + native_name="grasp", + affordance=AntipodalAffordance(), + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + affordance_revision="grasp-v1", + relative_pose=torch.eye(4), + ), + ), + collision_world_mode=( + SceneCollisionWorldMode.PER_ENV if dynamic_collision else None + ), + ) + + +def _robot_profile( + profile_id: str = "fake_robot", + *, + safe_motion_policy: MotionPolicy | None = None, +) -> RobotSkillProfile: + """Build the declarative resource graph used by the fake backend.""" + return RobotSkillProfile( + profile_id=profile_id, + resources={ + "manipulator": RobotResource( + resource_id="manipulator", + endpoints={ + "motion": ControlPartEndpoint( + control_part="arm", + capabilities=frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + JOINT_POSITION_CAPABILITY, + } + ), + ), + "grasp": ControlPartEndpoint( + control_part="hand", + capabilities=frozenset({GRASP_CAPABILITY}), + ), + }, + ) + }, + command_profiles={ + "hand": ControlPartCommandProfile.joint_positions( + open=torch.tensor((0.0,)), + grasp=torch.tensor((1.0,)), + ) + }, + presets={ + "safe": SkillPolicyPreset( + "safe", + action_option_templates={ + "pick": PickUpOptions(), + "place": PlaceOptions(), + }, + motion_policy=safe_motion_policy, + ) + }, + default_preset="safe", + ) + + +def _parallel_articulation_scene_registry() -> SceneRegistry: + """Build one drawer whose exact joint key is statically discoverable.""" + drawer = SceneArticulationRef("drawer") + handle = SceneAffordanceRef("drawer_handle") + return SceneRegistry( + ( + SceneEntityRegistration( + ref=drawer, + state_provider=_PoseProvider(), + semantic_type="drawer", + default_affordances={ + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY: handle + }, + ), + SceneEntityRegistration( + ref=handle, + state_provider=_PoseProvider(), + parent=drawer, + native_name="handle", + affordance=ArticulationOperationAffordance( + joint_id="drawer_slide", + operation_axis=torch.tensor((1.0, 0.0, 0.0)), + semantic_targets={ + "open": ArticulationOperationTarget( + target_position=0.4, + displacement=0.35, + ) + }, + ), + affordance_capabilities=frozenset( + {ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY} + ), + affordance_revision="drawer-operation-v1", + ), + ) + ) + + +def _parallel_articulation_profile() -> RobotSkillProfile: + """Build two physically disjoint resources that can address one drawer.""" + + def resource(resource_id: str) -> RobotResource: + return RobotResource( + resource_id=resource_id, + endpoints={ + "motion": ControlPartEndpoint( + control_part=f"{resource_id}_arm", + capabilities=frozenset( + {CARTESIAN_POSE_CAPABILITY, JOINT_POSITION_CAPABILITY} + ), + ), + "interaction": ControlPartEndpoint( + control_part=f"{resource_id}_hand", + capabilities=frozenset({GRASP_CAPABILITY}), + ), + }, + ) + + return RobotSkillProfile( + profile_id="parallel_articulation_robot", + resources={ + "left": resource("left"), + "right": resource("right"), + }, + command_profiles={ + hand: ControlPartCommandProfile.joint_positions( + open=torch.tensor((0.0,)), + grasp=torch.tensor((1.0,)), + ) + for hand in ("left_hand", "right_hand") + }, + presets={ + "safe": SkillPolicyPreset( + "safe", + action_option_templates={ + "operate_articulation": OperateArticulationOptions(), + }, + ) + }, + default_preset="safe", + ) + + +def _parallel_articulation_engine( + profile: RobotSkillProfile, +) -> AtomicActionEngine: + """Build the disjoint four-control-part engine used only for preflight.""" + robot = Mock() + robot.device = torch.device("cpu") + robot.dof = 4 + robot.control_parts = { + "left_arm": object(), + "left_hand": object(), + "right_arm": object(), + "right_hand": object(), + } + robot.get_qpos.return_value = torch.zeros(_BATCH_SIZE, robot.dof) + robot.get_qvel.return_value = torch.zeros(_BATCH_SIZE, robot.dof) + joint_ids = { + "left_arm": [0], + "left_hand": [1], + "right_arm": [2], + "right_hand": [3], + } + robot.get_joint_ids.side_effect = lambda name: joint_ids[name] + robot.get_solver.return_value = object() + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "fake_planner" + return AtomicActionEngine(generator, skill_profile=profile) + + +def _engine(profile: RobotSkillProfile) -> AtomicActionEngine: + """Build a CPU-only engine around a minimal typed robot surface.""" + robot = Mock() + robot.device = torch.device("cpu") + robot.dof = _ROBOT_DOF + robot.control_parts = {"arm": object(), "hand": object()} + robot.get_qpos.return_value = torch.zeros(_BATCH_SIZE, _ROBOT_DOF) + robot.get_qvel.return_value = torch.zeros(_BATCH_SIZE, _ROBOT_DOF) + robot.get_joint_ids.side_effect = lambda name: {"arm": [0], "hand": [1]}[name] + robot.get_solver.return_value = object() + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "fake_planner" + return AtomicActionEngine(generator, skill_profile=profile) + + +class _FakeEnvironmentFactory: + """Count every explicit factory boundary used by the production adapter.""" + + scene_registry_id = "fake_scene" + robot_profile_id = "fake_robot" + + def __init__(self, *, returned_profile_id: str = "fake_robot") -> None: + self.returned_profile_id = returned_profile_id + self.calls: Counter[str] = Counter() + self.observation_samples = 0 + + def create_scene_registry(self) -> SceneRegistry: + """Create a fresh live registry.""" + self.calls["scene"] += 1 + return _scene_registry() + + def create_robot_skill_profile(self) -> RobotSkillProfile: + """Create the configured robot profile.""" + self.calls["profile"] += 1 + return _robot_profile(self.returned_profile_id) + + def create_atomic_action_engine( + self, + profile: RobotSkillProfile, + ) -> AtomicActionEngine: + """Create an engine for exactly the supplied profile.""" + self.calls["engine"] += 1 + return _engine(profile) + + def create_planning_observation_provider( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + clock: EnvironmentStepClock, + ) -> GymPlanningObservationProvider: + """Create a callback-backed Gym observation port.""" + self.calls["observation"] += 1 + scene_provider = scene_registry.make_scene_provider(batch_size=_BATCH_SIZE) + + def capture(task_state: TaskState) -> PlanningContext: + self.observation_samples += 1 + env_ids = torch.arange(_BATCH_SIZE, dtype=torch.long) + timestamp = clock.now() + return PlanningContext( + robot=RobotObservation( + timestamp=timestamp, + qpos=engine.robot.get_qpos(), + qvel=engine.robot.get_qvel(), + ), + task=task_state, + scene=scene_provider.snapshot( + timestamp=timestamp, + env_ids=env_ids, + ), + env_ids=env_ids, + ) + + return GymPlanningObservationProvider(capture) + + def create_effect_evidence_providers( + self, + *, + scene_registry: SceneRegistry, + engine: AtomicActionEngine, + observation_provider: PlanningObservationPort, + ) -> tuple[EffectEvidenceProvider, ...]: + """Return the fake environment's explicit evidence-provider set.""" + del scene_registry, engine, observation_provider + self.calls["evidence"] += 1 + return () + + +class _ParallelArticulationFactory(_FakeEnvironmentFactory): + """Expose two robot resources and one shared articulation write target.""" + + scene_registry_id = "parallel_articulation_scene" + robot_profile_id = "parallel_articulation_robot" + + def create_scene_registry(self) -> SceneRegistry: + self.calls["scene"] += 1 + return _parallel_articulation_scene_registry() + + def create_robot_skill_profile(self) -> RobotSkillProfile: + self.calls["profile"] += 1 + return _parallel_articulation_profile() + + def create_atomic_action_engine( + self, + profile: RobotSkillProfile, + ) -> AtomicActionEngine: + self.calls["engine"] += 1 + return _parallel_articulation_engine(profile) + + +class _DynamicCollisionFactory(_FakeEnvironmentFactory): + """Expose a safe dynamic scene backed by an unsupported planner.""" + + def __init__(self) -> None: + super().__init__() + self.pose_provider = _PoseProvider() + self.last_engine: AtomicActionEngine | None = None + + def create_scene_registry(self) -> SceneRegistry: + self.calls["scene"] += 1 + return _scene_registry( + dynamic_collision=True, + pose_provider=self.pose_provider, + ) + + def create_robot_skill_profile(self) -> RobotSkillProfile: + self.calls["profile"] += 1 + return _robot_profile( + safe_motion_policy=MotionPolicy(strategy="motion_gen"), + ) + + def create_atomic_action_engine( + self, + profile: RobotSkillProfile, + ) -> AtomicActionEngine: + self.calls["engine"] += 1 + engine = _engine(profile) + engine.motion_generator.supports_dynamic_collision_world = False + self.last_engine = engine + return engine + + +class _FakeDeclarativeEnvironment(ExpertProgramEnvironmentMixin): + """Environment surface requiring no task-level motion implementation.""" + + def __init__(self, adapter: ExpertProgramEnvironmentAdapter) -> None: + self._adapter = adapter + + @property + def expert_program_adapter(self) -> ExpertProgramEnvironmentAdapter: + """Return the reusable environment adapter.""" + return self._adapter + + +class _AcceptParallelSafety: + """Accept test-only merged commands after static preflight succeeds.""" + + def validate( + self, + *, + branch_frames: object, + merged_frame: object, + ) -> None: + del branch_frames, merged_frame + + +class _PresetCheckingPostPolicyPort: + """Pure test port that rejects policies outside its preset table.""" + + def __init__(self, preset_ids: tuple[str, ...]) -> None: + self._preset_ids = frozenset(preset_ids) + self.validated_presets: list[str] = [] + + def validate_policy(self, policy: object, *, segment: object) -> None: + del segment + cfg = getattr(policy, "cfg") + preset = getattr(cfg, "preset") + self.validated_presets.append(preset) + if preset not in self._preset_ids: + raise KeyError(f"Unknown settle preset {preset!r}.") + + def actions( + self, + policy: object, + *, + segment: object, + active_mask: torch.Tensor, + ) -> tuple[torch.Tensor, ...]: + del policy, segment, active_mask + raise AssertionError("Preflight must not request post-policy actions.") + + +def _program( + *, + robot_profile: str = "fake_robot", + scene_registry: str = "fake_scene", + runtime_preset: str = "safe", + schema_version: int = EXPERT_PROGRAM_SCHEMA_VERSION, + node: ProgramNodeCfg | None = None, + targets: dict[str, CyclicPoseTargetCfg] | None = None, +) -> ExpertProgramCfg: + """Build one minimal declarative pick program.""" + return ExpertProgramCfg( + schema_version=schema_version, + program_id="fake_pick", + integration=ExpertProgramIntegrationCfg( + robot_profile=robot_profile, + scene_registry=scene_registry, + runtime_preset=runtime_preset, + ), + program=(InvokeCfg(call=PickCfg(object="cube")) if node is None else node), + targets={} if targets is None else targets, + ) + + +def _program_with_later_parallel_conflict() -> ExpertProgramCfg: + """Build an early sequential call followed by conflicting branch claims.""" + return _program( + schema_version=EXPERT_PROGRAM_SCHEMA_VERSION_V2, + node=SequenceCfg( + items=( + InvokeCfg(call=PickCfg(object="cube")), + ParallelCfg( + branches=( + InvokeCfg(call=PickCfg(object="cube")), + InvokeCfg(call=PickCfg(object="cube")), + ), + barrier=BarrierCfg(name="conflicting_join"), + ), + ) + ), + ) + + +def _program_with_later_segment_hooks( + *, + post: tuple[WaitStablePostCfg, ...] = (), + validators: tuple[ObjectNearTargetValidatorCfg, ...] = (), +) -> ExpertProgramCfg: + """Build a valid pick/place flow whose hooks live on the later segment.""" + return _program( + node=SequenceCfg( + items=( + SegmentCfg( + name="pick", + steps=InvokeCfg(call=PickCfg(object="cube")), + ), + SegmentCfg( + name="place", + steps=InvokeCfg( + call=PlaceCfg( + object="cube", + at=TargetRefCfg(target="drop"), + ) + ), + post=post, + validators=validators, + ), + ) + ), + targets={ + "drop": CyclicPoseTargetCfg( + values=( + PoseCfg( + position=(0.4, 0.1, 0.2), + quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), + ), + ) + ) + }, + ) + + +def _parallel_articulation_program() -> ExpertProgramCfg: + """Operate one joint from disjoint resources in two parallel branches.""" + return ExpertProgramCfg( + schema_version=EXPERT_PROGRAM_SCHEMA_VERSION_V2, + program_id="conflicting_drawer_operations", + integration=ExpertProgramIntegrationCfg( + robot_profile="parallel_articulation_robot", + scene_registry="parallel_articulation_scene", + runtime_preset="safe", + ), + targets={}, + program=ParallelCfg( + branches=tuple( + InvokeCfg( + call=OperateArticulationCfg( + articulation="drawer", + target="open", + resources={"primary": resource_id}, + ) + ) + for resource_id in ("left", "right") + ), + barrier=BarrierCfg(name="drawer_join"), + ), + ) + + +def test_mixin_compiles_and_assembles_bridge_without_task_motion_code() -> None: + """One adapter property implements both EmbodiedEnv integration hooks.""" + factory = _FakeEnvironmentFactory() + adapter = ExpertProgramEnvironmentAdapter(factory, step_dt=_STEP_DT) + env = _FakeDeclarativeEnvironment(adapter) + + assert isinstance(adapter, SkillRuntimeAssemblyPort) + + compiled = env.compile_expert_program(_program()) + + assert factory.calls == Counter(scene=1) + bridge = env.create_expert_program_bridge(compiled) + assert isinstance(bridge, AtomicDemoBridge) + assert factory.calls == Counter( + scene=2, + profile=1, + engine=1, + observation=1, + evidence=1, + ) + segment_iterator = bridge.iter_segments() + segment = next(segment_iterator) + assert segment.name == "invoke:pick" + assert segment.failure_policy == "row_independent" + segment_iterator.close() + + +def test_later_sequential_resource_error_fails_before_observation_or_action() -> None: + factory = _FakeEnvironmentFactory() + adapter = ExpertProgramEnvironmentAdapter(factory, step_dt=_STEP_DT) + compiled = adapter.compile( + _program( + node=SequenceCfg( + items=( + InvokeCfg(call=PickCfg(object="cube")), + InvokeCfg( + call=PickCfg( + object="cube", + resources={"primary": "missing"}, + ) + ), + ) + ) + ) + ) + + with pytest.raises(SemanticValidationError) as error: + adapter.create_bridge(compiled) + + assert error.value.diagnostic.code == "unknown_resource" + assert factory.calls == Counter(scene=2, profile=1, engine=1) + assert factory.observation_samples == 0 + + +def test_later_post_policy_requires_port_before_runtime_assembly() -> None: + """A later hook cannot defer its missing-port error until segment execution.""" + factory = _FakeEnvironmentFactory() + adapter = ExpertProgramEnvironmentAdapter(factory, step_dt=_STEP_DT) + compiled = adapter.compile( + _program_with_later_segment_hooks( + post=(WaitStablePostCfg(entity="cube", preset="fast"),), + ) + ) + + with pytest.raises(DemoBridgeError, match="SegmentPostPolicyPort"): + adapter.create_bridge(compiled) + + assert factory.calls == Counter(scene=1) + assert factory.observation_samples == 0 + + +def test_later_validator_requires_port_before_runtime_assembly() -> None: + """A later validator must have an installed pure-validation boundary.""" + factory = _FakeEnvironmentFactory() + adapter = ExpertProgramEnvironmentAdapter(factory, step_dt=_STEP_DT) + compiled = adapter.compile( + _program_with_later_segment_hooks( + validators=( + ObjectNearTargetValidatorCfg( + object="cube", + target="drop", + ), + ), + ) + ) + + with pytest.raises(DemoBridgeError, match="SegmentValidatorPort"): + adapter.create_bridge(compiled) + + assert factory.calls == Counter(scene=1) + assert factory.observation_samples == 0 + + +def test_later_unknown_settle_preset_fails_during_pure_preflight() -> None: + """Every declared preset is checked before semantic or live runtime assembly.""" + factory = _FakeEnvironmentFactory() + post_policy_port = _PresetCheckingPostPolicyPort(("fast",)) + adapter = ExpertProgramEnvironmentAdapter( + factory, + step_dt=_STEP_DT, + post_policy_port=post_policy_port, + ) + compiled = adapter.compile( + _program_with_later_segment_hooks( + post=(WaitStablePostCfg(entity="cube", preset="missing"),), + ) + ) + + with pytest.raises(KeyError, match="Unknown settle preset 'missing'"): + adapter.create_bridge(compiled) + + assert post_policy_port.validated_presets == ["missing"] + assert factory.calls == Counter(scene=1) + assert factory.observation_samples == 0 + + +def test_preflight_preserves_pick_target_lookahead_across_explicit_segments( + monkeypatch: pytest.MonkeyPatch, +) -> None: + factory = _FakeEnvironmentFactory() + adapter = ExpertProgramEnvironmentAdapter(factory, step_dt=_STEP_DT) + config = _program( + node=SequenceCfg( + items=( + SegmentCfg( + name="pick", + steps=InvokeCfg(call=PickCfg(object="cube")), + ), + SegmentCfg( + name="place", + steps=InvokeCfg( + call=PlaceCfg( + object="cube", + at=TargetRefCfg(target="drop"), + ) + ), + ), + ) + ), + targets={ + "drop": CyclicPoseTargetCfg( + values=( + PoseCfg( + position=(0.4, 0.1, 0.2), + quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), + ), + ) + ) + }, + ) + workflows: list[object] = [] + original_analyze = SemanticSkillCompiler.analyze + + def record_analyze( + compiler: SemanticSkillCompiler, + calls: object, + **kwargs: object, + ) -> object: + workflow = original_analyze(compiler, calls, **kwargs) + workflows.append(workflow) + return workflow + + monkeypatch.setattr(SemanticSkillCompiler, "analyze", record_analyze) + + adapter.create_bridge(adapter.compile(config)) + + assert len(workflows) == 1 + workflow = workflows[0] + assert len(workflow.calls) == 2 # type: ignore[attr-defined] + downstream = workflow.calls[0].downstream_object_targets # type: ignore[attr-defined] + assert len(downstream) == 1 + assert downstream[0].pose is not None + torch.testing.assert_close( + downstream[0].pose.position, + torch.tensor((0.4, 0.1, 0.2)), + ) + assert factory.observation_samples == 0 + + +def test_parallel_program_requires_safety_validator_before_first_action() -> None: + factory = _FakeEnvironmentFactory() + adapter = ExpertProgramEnvironmentAdapter(factory, step_dt=_STEP_DT) + compiled = adapter.compile(_program_with_later_parallel_conflict()) + + with pytest.raises(ValueError, match="ParallelCommandSafetyValidator"): + adapter.create_bridge(compiled) + + assert factory.observation_samples == 0 + + +def test_later_parallel_claim_conflict_fails_during_whole_program_preflight() -> None: + factory = _FakeEnvironmentFactory() + adapter = ExpertProgramEnvironmentAdapter( + factory, + step_dt=_STEP_DT, + parallel_safety_validator=_AcceptParallelSafety(), + ) + compiled = adapter.compile(_program_with_later_parallel_conflict()) + + with pytest.raises(ValueError, match="overlapping resource claims"): + adapter.create_bridge(compiled) + + assert factory.observation_samples == 0 + + +def test_parallel_symbolic_write_conflict_fails_before_observation_or_action() -> None: + factory = _ParallelArticulationFactory() + adapter = ExpertProgramEnvironmentAdapter( + factory, + step_dt=_STEP_DT, + parallel_safety_validator=_AcceptParallelSafety(), + ) + compiled = adapter.compile(_parallel_articulation_program()) + materialized = compiled.materialize() + parallel_block = tuple(materialized.iter_segments())[0].parallel_block + assert parallel_block is not None + expected_path = parallel_block.branches[1].source_path + + with pytest.raises(SemanticValidationError) as error: + adapter.create_bridge(compiled) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "parallel_symbolic_write_conflict" + assert diagnostic.path == expected_path + assert "articulation_joint['drawer', 'drawer_slide']" in diagnostic.message + assert factory.observation_samples == 0 + + +def test_runtime_assembly_shares_exact_bound_components() -> None: + """Compiler, runtime, clock, sink, scene, and profile form one ownership graph.""" + factory = _FakeEnvironmentFactory() + adapter = ExpertProgramEnvironmentAdapter(factory, step_dt=_STEP_DT) + + assembly = adapter.assemble_runtime(_program().integration) + + assert assembly.compiler.integration.scene_registry is assembly.scene_registry + assert assembly.compiler.integration.manifest is assembly.manifest + assert assembly.compiler.integration.engine is assembly.engine + assert assembly.compiler.integration.manifest.runtime_preset == "safe" + assert assembly.compiler.integration.robot_profile.engine is assembly.engine + assert assembly.runtime.compiler is assembly.compiler + assert assembly.runtime.clock is assembly.clock + assert assembly.command_sink.clock is assembly.clock + assert assembly.clock.step_dt == pytest.approx(_STEP_DT) + assert assembly.evidence_collector.registry.providers == {} + + +def test_safe_dynamic_collision_fails_before_observation_planning_or_command() -> None: + factory = _DynamicCollisionFactory() + adapter = ExpertProgramEnvironmentAdapter(factory, step_dt=_STEP_DT) + compiled = adapter.compile(_program(runtime_preset="safe")) + + with pytest.raises(SemanticValidationError) as error: + adapter.create_bridge(compiled) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "safe_dynamic_collision_unsupported" + assert diagnostic.path == ( + "integration", + "robot_profile", + "presets", + "safe", + "motion_policy", + "dynamic_collision_mode", + ) + assert factory.calls == Counter(scene=2, profile=1, engine=1) + assert factory.pose_provider.calls == 0 + assert factory.observation_samples == 0 + assert factory.last_engine is not None + factory.last_engine.motion_generator.generate.assert_not_called() + + +@pytest.mark.parametrize( + ("field", "value", "match"), + ( + ("robot_profile", "other_robot", "selects robot_profile"), + ("scene_registry", "other_scene", "selects scene_registry"), + ), +) +def test_integration_id_mismatch_fails_before_live_factory_access( + field: str, + value: str, + match: str, +) -> None: + """Static selection drift never reaches simulation or motion factories.""" + factory = _FakeEnvironmentFactory() + adapter = ExpertProgramEnvironmentAdapter(factory, step_dt=_STEP_DT) + options = {field: value} + + with pytest.raises(ValueError, match=match): + adapter.compile(_program(**options)) + + assert factory.calls == Counter() + + +def test_robot_profile_factory_drift_fails_before_engine_creation() -> None: + """The declared profile ID must match the concrete factory output.""" + factory = _FakeEnvironmentFactory(returned_profile_id="different_robot") + adapter = ExpertProgramEnvironmentAdapter(factory, step_dt=_STEP_DT) + + with pytest.raises(ValueError, match="profile declaration drifted"): + adapter.assemble_runtime(_program().integration) + + assert factory.calls == Counter(scene=1, profile=1) + + +def test_factory_selection_declaration_drift_fails_before_live_access() -> None: + """A mutable factory cannot silently change IDs after adapter creation.""" + factory = _FakeEnvironmentFactory() + adapter = ExpertProgramEnvironmentAdapter(factory, step_dt=_STEP_DT) + factory.scene_registry_id = "changed_scene" + + with pytest.raises(ValueError, match="scene registry declaration drifted"): + adapter.compile(_program()) + + assert factory.calls == Counter() + + +def test_unknown_runtime_preset_fails_during_manifest_assembly() -> None: + """Runtime preset names are validated against the selected robot profile.""" + factory = _FakeEnvironmentFactory() + adapter = ExpertProgramEnvironmentAdapter(factory, step_dt=_STEP_DT) + + with pytest.raises(ValueError, match="Unknown runtime preset"): + adapter.assemble_runtime(_program(runtime_preset="unregistered").integration) + + +def test_factory_protocol_is_required() -> None: + """Loose objects cannot enter the production assembly boundary.""" + with pytest.raises(TypeError, match="ExpertProgramEnvironmentFactory"): + ExpertProgramEnvironmentAdapter(object(), step_dt=_STEP_DT) diff --git a/tests/gym/envs/expert_program/test_extensions.py b/tests/gym/envs/expert_program/test_extensions.py new file mode 100644 index 000000000..602013fc5 --- /dev/null +++ b/tests/gym/envs/expert_program/test_extensions.py @@ -0,0 +1,549 @@ +# ---------------------------------------------------------------------------- +# 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 exact standard-runtime Expert Program extension declarations.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import ClassVar + +import pytest +import torch + +from embodichain.lab.gym.envs.expert_program import ( + RobotResourceBinding, + SimulationExpertProgramRegistration, + SimulationRobotSkillProfileBinding, + SimulationSceneBinding, +) +from embodichain.lab.gym.envs.expert_program.bridge import ( + JointPositionGymTransportEncoder, +) +from embodichain.lab.gym.envs.expert_program.extensions import ( + RuntimeTransportDeclaration, + build_standard_extension_declarations, + validate_immutable_extension_declaration, +) +from embodichain.lab.sim.atomic_actions import PlanningContext +from embodichain.lab.sim.atomic_actions.bindings import RuntimeEndpointTarget +from embodichain.lab.sim.atomic_actions.runtime_commands import ( + EndpointCommand, + RuntimeCommandPayload, +) +from embodichain.lab.sim.skills import ( + ControlPartEndpoint, + ControlPartEndpointAdapter, + EndpointResolution, + ResourceEndpoint, + ResourceEndpointAdapter, + RobotResource, + RobotSkillProfile, +) +from embodichain.lab.sim.types import EnvAction + + +@dataclass(frozen=True, slots=True, kw_only=True) +class _MobileEndpoint(ResourceEndpoint): + """Custom endpoint declaration used by the catalog-only tests.""" + + controller: str = "base" + + +@dataclass(frozen=True, slots=True, kw_only=True) +class _ToolEndpoint(ResourceEndpoint): + """Second exact endpoint type used to prove transport ordering.""" + + controller: str = "tool" + + +@dataclass(frozen=True, slots=True) +class _MobileTarget(RuntimeEndpointTarget): + """Immutable custom runtime destination.""" + + TRANSPORT_ID: ClassVar[str] = "test.mobile" + controller: str + + @property + def transport_id(self) -> str: + return self.TRANSPORT_ID + + @property + def target_id(self) -> str: + return self.controller + + +@dataclass(frozen=True, slots=True) +class _ToolTarget(RuntimeEndpointTarget): + """Immutable destination owned by the second transport.""" + + TRANSPORT_ID: ClassVar[str] = "test.tool" + controller: str + + @property + def transport_id(self) -> str: + return self.TRANSPORT_ID + + @property + def target_id(self) -> str: + return self.controller + + +@dataclass(frozen=True, slots=True, eq=False) +class _MobilePayload(RuntimeCommandPayload): + """Minimal typed payload declaration for the mobile transport.""" + + TRANSPORT_ID: ClassVar[str] = _MobileTarget.TRANSPORT_ID + values: torch.Tensor + + @property + def batch_size(self) -> int: + return int(self.values.shape[0]) + + @property + def device(self) -> torch.device: + return self.values.device + + @property + def transport_id(self) -> str: + return self.TRANSPORT_ID + + def snapshot(self) -> _MobilePayload: + return _MobilePayload(self.values.clone()) + + +@dataclass(frozen=True, slots=True, eq=False) +class _ToolPayload(RuntimeCommandPayload): + """Minimal typed payload declaration for the tool transport.""" + + TRANSPORT_ID: ClassVar[str] = _ToolTarget.TRANSPORT_ID + values: torch.Tensor + + @property + def batch_size(self) -> int: + return int(self.values.shape[0]) + + @property + def device(self) -> torch.device: + return self.values.device + + @property + def transport_id(self) -> str: + return self.TRANSPORT_ID + + def snapshot(self) -> _ToolPayload: + return _ToolPayload(self.values.clone()) + + +class _MobileAdapter(ResourceEndpointAdapter): + """Stateless custom adapter with only standard-factory provider routes.""" + + adapter_id: ClassVar[str] = "test.mobile" + endpoint_type: ClassVar[type[ResourceEndpoint]] = _MobileEndpoint + runtime_transport_ids: ClassVar[frozenset[str]] = frozenset( + {_MobileTarget.TRANSPORT_ID} + ) + runtime_target_types: ClassVar[tuple[type[RuntimeEndpointTarget], ...]] = ( + _MobileTarget, + ) + tracking_feedback_source_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset() + tracking_projector_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset() + effect_evidence_source_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset() + + def resolve( + self, endpoint: ResourceEndpoint, *, engine: object + ) -> EndpointResolution: + del endpoint, engine + return EndpointResolution( + runtime_target=_MobileTarget("base"), + claim_tokens=frozenset({"test.mobile:base"}), + ) + + +class _ToolAdapter(ResourceEndpointAdapter): + """Second stateless adapter used by ordering tests.""" + + adapter_id: ClassVar[str] = "test.tool" + endpoint_type: ClassVar[type[ResourceEndpoint]] = _ToolEndpoint + runtime_transport_ids: ClassVar[frozenset[str]] = frozenset( + {_ToolTarget.TRANSPORT_ID} + ) + runtime_target_types: ClassVar[tuple[type[RuntimeEndpointTarget], ...]] = ( + _ToolTarget, + ) + tracking_feedback_source_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset() + tracking_projector_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset() + effect_evidence_source_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset() + + def resolve( + self, endpoint: ResourceEndpoint, *, engine: object + ) -> EndpointResolution: + del endpoint, engine + return EndpointResolution( + runtime_target=_ToolTarget("tool"), + claim_tokens=frozenset({"test.tool:tool"}), + ) + + +class _MobileTransport: + """Stateless action composition transport for the mobile target.""" + + transport_id: ClassVar[str] = _MobileTarget.TRANSPORT_ID + target_types: ClassVar[tuple[type[RuntimeEndpointTarget], ...]] = (_MobileTarget,) + payload_types: ClassVar[tuple[type[RuntimeCommandPayload], ...]] = (_MobilePayload,) + + def encode( + self, + command: EndpointCommand, + *, + base_action: EnvAction, + active_mask: torch.Tensor, + ) -> EnvAction: + del command, active_mask + return base_action + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + base_action: EnvAction, + context: PlanningContext, + ) -> EnvAction: + del targets, context + return base_action + + +class _ToolTransport(_MobileTransport): + """Second stateless action composition transport.""" + + transport_id: ClassVar[str] = _ToolTarget.TRANSPORT_ID + target_types: ClassVar[tuple[type[RuntimeEndpointTarget], ...]] = (_ToolTarget,) + payload_types: ClassVar[tuple[type[RuntimeCommandPayload], ...]] = (_ToolPayload,) + + +class _SafetyValidator: + """Protocol-compatible no-op validator used only for factory typing.""" + + def validate(self, *, branch_frames: object, merged_frame: object) -> None: + del branch_frames, merged_frame + + +class _MobileSafetyFactory: + """Stateless exact safety-factory declaration.""" + + validator_id: ClassVar[str] = "test.mobile_safety" + revision: ClassVar[str] = "1" + supported_transport_ids: ClassVar[frozenset[str]] = frozenset( + {_MobileTarget.TRANSPORT_ID} + ) + + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: object, + engine: object, + ) -> _SafetyValidator: + del simulation, robot, scene_registry, engine + return _SafetyValidator() + + +def _custom_profile(*, include_tool: bool = False) -> RobotSkillProfile: + """Return a pure provider-free profile with exact custom endpoint types.""" + endpoints: dict[str, ResourceEndpoint] = { + "motion": _MobileEndpoint(capabilities=frozenset()) + } + if include_tool: + endpoints["tool"] = _ToolEndpoint(capabilities=frozenset()) + resource = RobotResource(resource_id="custom", endpoints=endpoints) + return RobotSkillProfile(profile_id="custom", resources={"custom": resource}) + + +def test_custom_endpoint_transport_and_safety_declarations_are_exact() -> None: + """A complete custom extension set produces an immutable provider-free catalog.""" + declarations = build_standard_extension_declarations( + profile=_custom_profile(), + endpoint_adapters=(_MobileAdapter(),), + runtime_transports=(_MobileTransport(),), + parallel_safety_factory=_MobileSafetyFactory(), + ) + + assert declarations.endpoint_adapters[_MobileEndpoint].adapter_id == "test.mobile" + assert tuple(value.transport_id for value in declarations.runtime_transports) == ( + "test.mobile", + ) + assert declarations.parallel_safety is not None + assert declarations.parallel_safety.supported_transport_ids == frozenset( + {"test.mobile"} + ) + + +def test_parallel_safety_transport_coverage_must_match_registration() -> None: + """A safety factory must cover the exact installed transport set.""" + + class MismatchedSafetyFactory(_MobileSafetyFactory): + supported_transport_ids: ClassVar[frozenset[str]] = frozenset({"test.other"}) + + with pytest.raises(ValueError, match="must support exactly"): + build_standard_extension_declarations( + profile=_custom_profile(), + endpoint_adapters=(_MobileAdapter(),), + runtime_transports=(_MobileTransport(),), + parallel_safety_factory=MismatchedSafetyFactory(), + ) + + +@pytest.mark.parametrize("declaration_kind", ("target", "payload")) +def test_runtime_transport_types_require_direct_transport_id( + declaration_kind: str, +) -> None: + """Every registered runtime value type owns its transport ID directly.""" + + class MissingTarget(RuntimeEndpointTarget): + @property + def transport_id(self) -> str: + return _MobileTarget.TRANSPORT_ID + + @property + def target_id(self) -> str: + return "missing" + + class MissingPayload(RuntimeCommandPayload): + @property + def batch_size(self) -> int: + return 1 + + @property + def device(self) -> torch.device: + return torch.device("cpu") + + @property + def transport_id(self) -> str: + return _MobileTarget.TRANSPORT_ID + + def snapshot(self) -> MissingPayload: + return MissingPayload() + + target_types = ( + (MissingTarget,) if declaration_kind == "target" else (_MobileTarget,) + ) + payload_types = ( + (MissingPayload,) if declaration_kind == "payload" else (_MobilePayload,) + ) + + with pytest.raises(TypeError, match="must declare an exact ClassVar TRANSPORT_ID"): + RuntimeTransportDeclaration( + transport_type=_MobileTransport, + transport_id=_MobileTarget.TRANSPORT_ID, + target_types=target_types, + payload_types=payload_types, + ) + + +@pytest.mark.parametrize("declaration_kind", ("target", "payload")) +def test_runtime_transport_types_cannot_inherit_transport_id( + declaration_kind: str, +) -> None: + """A subtype cannot silently inherit another runtime type's transport owner.""" + + class InheritedTarget(_MobileTarget): + pass + + class InheritedPayload(_MobilePayload): + pass + + target_types = ( + (InheritedTarget,) if declaration_kind == "target" else (_MobileTarget,) + ) + payload_types = ( + (InheritedPayload,) if declaration_kind == "payload" else (_MobilePayload,) + ) + + with pytest.raises(TypeError, match="inherited or instance-only"): + RuntimeTransportDeclaration( + transport_type=_MobileTransport, + transport_id=_MobileTarget.TRANSPORT_ID, + target_types=target_types, + payload_types=payload_types, + ) + + +@pytest.mark.parametrize("declaration_kind", ("target", "payload")) +def test_runtime_transport_type_transport_id_must_match_encoder( + declaration_kind: str, +) -> None: + """Static runtime value ownership must match the encoder transport exactly.""" + + class MismatchedTarget(_MobileTarget): + TRANSPORT_ID: ClassVar[str] = "test.mismatched" + + class MismatchedPayload(_MobilePayload): + TRANSPORT_ID: ClassVar[str] = "test.mismatched" + + target_types = ( + (MismatchedTarget,) if declaration_kind == "target" else (_MobileTarget,) + ) + payload_types = ( + (MismatchedPayload,) if declaration_kind == "payload" else (_MobilePayload,) + ) + + with pytest.raises(ValueError, match="not 'test.mobile'"): + RuntimeTransportDeclaration( + transport_type=_MobileTransport, + transport_id=_MobileTarget.TRANSPORT_ID, + target_types=target_types, + payload_types=payload_types, + ) + + +@pytest.mark.parametrize( + ("adapters", "transports", "message"), + ( + ((), (_MobileTransport(),), "missing"), + ((_MobileAdapter(),), (), "missing"), + ( + (_MobileAdapter(), _ToolAdapter()), + (_MobileTransport(), _ToolTransport()), + "unused", + ), + ), +) +def test_extension_coverage_rejects_missing_and_unused_declarations( + adapters: tuple[ResourceEndpointAdapter, ...], + transports: tuple[object, ...], + message: str, +) -> None: + """Every custom adapter and transport must be necessary and complete.""" + with pytest.raises(ValueError, match=message): + build_standard_extension_declarations( + profile=_custom_profile(), + endpoint_adapters=adapters, + runtime_transports=transports, # type: ignore[arg-type] + parallel_safety_factory=None, + ) + + +def test_builtin_adapter_and_transport_cannot_be_overridden() -> None: + """Standard built-ins retain exact ownership of their endpoint and transport.""" + profile = RobotSkillProfile( + profile_id="joint", + resources={ + "arm": RobotResource( + resource_id="arm", + endpoints={ + "motion": ControlPartEndpoint( + control_part="arm", + capabilities=frozenset(), + ) + }, + ) + }, + ) + + with pytest.raises(ValueError, match="override the built-in ControlPartEndpoint"): + build_standard_extension_declarations( + profile=profile, + endpoint_adapters=(ControlPartEndpointAdapter(),), + runtime_transports=(), + parallel_safety_factory=None, + ) + with pytest.raises(ValueError, match="override the built-in joint-position"): + build_standard_extension_declarations( + profile=profile, + endpoint_adapters=(), + runtime_transports=(JointPositionGymTransportEncoder(),), + parallel_safety_factory=None, + ) + + +def test_nonbuiltin_provider_route_is_rejected_by_standard_registration() -> None: + """Provider declarations cannot name a live registry absent from the factory.""" + + class UnsupportedProviderAdapter(_MobileAdapter): + adapter_id: ClassVar[str] = "test.unsupported_provider" + tracking_feedback_source_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset( + {("test.feedback", "1")} + ) + + with pytest.raises(ValueError, match="does not install"): + build_standard_extension_declarations( + profile=_custom_profile(), + endpoint_adapters=(UnsupportedProviderAdapter(),), + runtime_transports=(_MobileTransport(),), + parallel_safety_factory=None, + ) + + +@pytest.mark.parametrize( + "mutable_leaf", + ( + [0.1], + {"gain": 0.1}, + {0.1}, + bytearray(b"gain"), + torch.tensor((0.1,)), + ), + ids=("list", "dict", "set", "bytearray", "tensor"), +) +def test_extension_declarations_reject_nested_mutable_state( + mutable_leaf: object, +) -> None: + """Frozen wrappers cannot retain mutable state used by a live extension.""" + + @dataclass(frozen=True, slots=True) + class NestedDeclaration: + config: tuple[object, ...] + + with pytest.raises(TypeError, match="deeply immutable"): + validate_immutable_extension_declaration( + NestedDeclaration((mutable_leaf,)), + field_name="runtime_transports", + ) + + +def test_runtime_transport_tuple_order_changes_registration_fingerprint() -> None: + """Transport composition order is semantic registration data.""" + profile_binding = SimulationRobotSkillProfileBinding( + profile_id="custom", + resources=( + RobotResourceBinding( + resource_id="custom", + endpoints={ + "motion": _MobileEndpoint(capabilities=frozenset()), + "tool": _ToolEndpoint(capabilities=frozenset()), + }, + ), + ), + ) + common = { + "scene_binding": SimulationSceneBinding(registry_id="custom_scene"), + "robot_profile_binding": profile_binding, + "endpoint_adapters": (_MobileAdapter(), _ToolAdapter()), + } + forward = SimulationExpertProgramRegistration( + **common, + runtime_transports=(_MobileTransport(), _ToolTransport()), + ) + reversed_registration = SimulationExpertProgramRegistration( + **common, + runtime_transports=(_ToolTransport(), _MobileTransport()), + ) + + assert forward.fingerprint != reversed_registration.fingerprint + + +__all__: list[str] = [] diff --git a/tests/gym/envs/expert_program/test_loader.py b/tests/gym/envs/expert_program/test_loader.py new file mode 100644 index 000000000..4ef5c4bb6 --- /dev/null +++ b/tests/gym/envs/expert_program/test_loader.py @@ -0,0 +1,252 @@ +# ---------------------------------------------------------------------------- +# 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 strict serialized Expert Program loading.""" + +from __future__ import annotations + +import json +from pathlib import Path + +import pytest +import yaml + +from embodichain.lab.gym.envs.expert_program import ( + ConfigPath, + ExpertProgramDecodeError, + ExpertProgramValidationError, + InvokeCfg, + PickCfg, + load_expert_program, + loads_expert_program_json, + parse_expert_program_json, +) + + +def _program_data(*, schema_version: int = 1) -> dict[str, object]: + """Return one minimal complete Expert Program JSON value.""" + program: dict[str, object] = { + "kind": "invoke", + "call": {"kind": "pick", "object": "cube"}, + } + if schema_version == 2: + program = { + "kind": "parallel", + "branches": [ + program, + { + "kind": "invoke", + "call": {"kind": "pick", "object": "other_cube"}, + }, + ], + "barrier": { + "kind": "barrier", + "name": "both_picked", + "timeout_steps": 20, + "failure_policy": "fail_fast", + }, + } + return { + "schema_version": schema_version, + "program_id": "loader_pick", + "integration": { + "robot_profile": "test_robot", + "scene_registry": "test_scene", + "runtime_preset": "safe", + }, + "targets": {}, + "program": program, + } + + +def _program_json() -> str: + """Serialize the minimal program using standards-compliant JSON.""" + return json.dumps(_program_data()) + + +class _RejectingValidationContext: + """Reject integration references after recording their exact path.""" + + def __init__(self) -> None: + self.integration_paths: list[ConfigPath] = [] + + def validate_integration( + self, + integration: object, + *, + path: ConfigPath, + ) -> None: + del integration + self.integration_paths.append(path) + raise KeyError("unavailable integration") + + def validate_semantic_call(self, call: object, *, path: ConfigPath) -> None: + del call, path + + def validate_scene_reference( + self, + reference: str, + *, + role: str, + path: ConfigPath, + ) -> None: + del reference, role, path + + def validate_post_policy(self, policy: object, *, path: ConfigPath) -> None: + del policy, path + + def validate_validator(self, validator: object, *, path: ConfigPath) -> None: + del validator, path + + +def test_parse_expert_program_json_preserves_predecode_mapping() -> None: + value = parse_expert_program_json('{"host_integration_pending": [true, null, 3.5]}') + + assert value == {"host_integration_pending": [True, None, 3.5]} + + +@pytest.mark.parametrize("response", ["[]", "null", '"program"']) +def test_parse_expert_program_json_requires_top_level_mapping(response: str) -> None: + with pytest.raises(ExpertProgramDecodeError) as error: + parse_expert_program_json(response) + + assert error.value.code == "expected_mapping" + assert error.value.path == () + + +def test_loads_expert_program_json_decodes_one_plain_document() -> None: + config = loads_expert_program_json(f"\n{_program_json()}\t") + + assert type(config.program) is InvokeCfg + assert type(config.program.call) is PickCfg + assert config.program.call.object == "cube" + + +@pytest.mark.parametrize("suffix", [".json", ".yaml"]) +@pytest.mark.parametrize("schema_version", [1, 2]) +def test_load_expert_program_forwards_validation_context_for_each_format( + tmp_path: Path, + suffix: str, + schema_version: int, +) -> None: + data = _program_data(schema_version=schema_version) + serialized = json.dumps(data) if suffix == ".json" else yaml.safe_dump(data) + path = tmp_path / f"program{suffix}" + path.write_text(serialized, encoding="utf-8") + context = _RejectingValidationContext() + + with pytest.raises(ExpertProgramValidationError) as error: + load_expert_program(path, validation_context=context) + + assert error.value.code == "reference_validation_failed" + assert error.value.path == ("integration",) + assert context.integration_paths == [("integration",)] + + +def test_loads_expert_program_json_rejects_nested_duplicate_keys() -> None: + duplicate = _program_json().replace( + '"object": "cube"', + '"object": "cube", "object": "other"', + ) + + with pytest.raises(ExpertProgramDecodeError) as error: + loads_expert_program_json(duplicate) + + assert error.value.code == "duplicate_json_key" + + +@pytest.mark.parametrize( + "invalid_response", + [ + "```json\n{}\n```", + f"{_program_json()} trailing text", + f"{_program_json()} {_program_json()}", + ], +) +def test_loads_expert_program_json_requires_one_unfenced_document( + invalid_response: str, +) -> None: + with pytest.raises(ExpertProgramDecodeError) as error: + loads_expert_program_json(invalid_response) + + assert error.value.code == "invalid_json" + + +@pytest.mark.parametrize("number", ["NaN", "Infinity", "-Infinity", "1e400"]) +def test_loads_expert_program_json_rejects_non_finite_numbers(number: str) -> None: + response = f'{{"value": {number}}}' + + with pytest.raises(ExpertProgramDecodeError) as error: + loads_expert_program_json(response) + + assert error.value.code == "non_finite_number" + + +def test_loads_expert_program_json_enforces_utf8_byte_limit() -> None: + response = _program_json() + too_small = len(response.encode("utf-8")) - 1 + + with pytest.raises(ExpertProgramDecodeError) as error: + loads_expert_program_json(response, max_bytes=too_small) + + assert error.value.code == "input_too_large" + + +def test_loads_expert_program_json_normalizes_invalid_utf8_text() -> None: + response = "\ud800" + + with pytest.raises(ExpertProgramDecodeError) as error: + loads_expert_program_json(response) + + assert error.value.code == "invalid_utf8" + + +def test_loads_expert_program_json_rejects_escaped_unpaired_surrogate() -> None: + response = _program_json().replace("loader_pick", r"\ud800") + + with pytest.raises(ExpertProgramDecodeError) as error: + loads_expert_program_json(response) + + assert error.value.code == "invalid_utf8" + + +def test_loads_expert_program_json_accepts_escaped_surrogate_pair() -> None: + response = _program_json().replace("loader_pick", r"\ud83d\ude00") + + config = loads_expert_program_json(response) + + assert config.program_id == "😀" + + +def test_loads_expert_program_json_normalizes_oversized_integer() -> None: + data = _program_data() + data["targets"] = { + "goal": { + "kind": "cyclic_pose", + "values": [ + { + "position": [10**400, 0, 0], + "quaternion_wxyz": [1, 0, 0, 0], + } + ], + } + } + + with pytest.raises(ExpertProgramDecodeError) as error: + loads_expert_program_json(json.dumps(data)) + + assert error.value.code == "invalid_value" + assert error.value.path == ("targets", "goal", "values", 0) diff --git a/tests/gym/envs/expert_program/test_parallel_compiler.py b/tests/gym/envs/expert_program/test_parallel_compiler.py new file mode 100644 index 000000000..84c678c3e --- /dev/null +++ b/tests/gym/envs/expert_program/test_parallel_compiler.py @@ -0,0 +1,221 @@ +# ---------------------------------------------------------------------------- +# 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 provider-free schema-v2 parallel program compilation.""" + +from __future__ import annotations + +import pytest +import torch + +from embodichain.lab.gym.envs.expert_program import ( + BarrierCfg, + ExpertProgramCfg, + ExpertProgramCompileError, + ExpertProgramCompiler, + ExpertProgramIntegrationCfg, + InvokeCfg, + ParallelCfg, + PickCfg, + RepeatCfg, + SegmentCfg, + SequenceCfg, +) +from embodichain.lab.sim.atomic_actions import EntityState +from embodichain.lab.sim.skills.calls import Pick +from embodichain.lab.sim.skills.scene import ( + SceneEntityRegistration, + SceneObjectRef, + SceneRegistry, +) + + +class _NeverObserveProvider: + """Reject dynamic observation during provider-free compilation.""" + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp, env_ids + raise AssertionError("Compilation must not observe the scene.") + + +def _compiler() -> ExpertProgramCompiler: + provider = _NeverObserveProvider() + registry = SceneRegistry( + tuple( + SceneEntityRegistration( + ref=SceneObjectRef(entity_id), + state_provider=provider, + ) + for entity_id in ("left_cube", "right_cube") + ) + ) + return ExpertProgramCompiler.from_scene_registry(registry) + + +def _integration() -> ExpertProgramIntegrationCfg: + return ExpertProgramIntegrationCfg( + robot_profile="dual_arm", + scene_registry="scene", + runtime_preset="safe", + ) + + +def _parallel() -> ParallelCfg: + return ParallelCfg( + branches=( + SequenceCfg( + items=( + InvokeCfg(call=PickCfg(object="left_cube")), + InvokeCfg(call=PickCfg(object="left_cube")), + ) + ), + RepeatCfg( + count=2, + body=InvokeCfg(call=PickCfg(object="right_cube")), + ), + ), + barrier=BarrierCfg( + name="both_arms_done", + timeout_steps=240, + failure_policy="fail_fast", + ), + ) + + +def _config(program: ParallelCfg | SegmentCfg | SequenceCfg) -> ExpertProgramCfg: + return ExpertProgramCfg( + schema_version=2, + program_id="parallel_pick", + integration=_integration(), + program=program, + targets={}, + ) + + +def test_parallel_compiles_independent_ordered_lanes_and_explicit_join() -> None: + segment = tuple(_compiler().compile(_config(_parallel())))[0] + + assert segment.implicit + assert segment.name == "parallel:both_arms_done" + assert segment.parallel_block is not None + block = segment.parallel_block + assert block.barrier.name == "both_arms_done" + assert block.barrier.timeout_steps == 240 + assert block.barrier.failure_policy == "fail_fast" + assert tuple(branch.branch_index for branch in block.branches) == (0, 1) + assert tuple(len(branch.calls) for branch in block.branches) == (2, 2) + assert tuple(call.call_index for call in segment.calls) == (0, 1, 2, 3) + assert tuple(call.segment_call_index for call in segment.calls) == (0, 1, 2, 3) + assert segment.calls == tuple( + call for branch in block.branches for call in branch.calls + ) + assert all(type(call.call) is Pick for call in segment.calls) + assert tuple(call.call.object.entity_id for call in block.branches[0].calls) == ( + "left_cube", + "left_cube", + ) + assert tuple(call.call.object.entity_id for call in block.branches[1].calls) == ( + "right_cube", + "right_cube", + ) + assert tuple( + frame.iteration_index + for call in block.branches[1].calls + for frame in call.repeat_frames + ) == (0, 1) + + +def test_segment_may_wrap_one_parallel_block() -> None: + segment = tuple( + _compiler().compile(_config(SegmentCfg(name="dual_pick", steps=_parallel()))) + )[0] + + assert not segment.implicit + assert segment.name == "dual_pick" + assert segment.parallel_block is not None + assert len(segment.calls) == 4 + + +def test_materialized_analysis_stops_sequential_lookahead_at_parallel_barriers() -> ( + None +): + config = _config( + SequenceCfg( + items=( + InvokeCfg(call=PickCfg(object="left_cube")), + _parallel(), + InvokeCfg(call=PickCfg(object="left_cube")), + ) + ) + ) + + program = _compiler().compile(config).materialize() + analyses = program.preflight_analyses() + + assert [analysis.kind for analysis in analyses] == [ + "sequential_stretch", + "parallel_branch", + "parallel_branch", + "sequential_stretch", + ] + assert [analysis.segment_indices for analysis in analyses] == [ + (0,), + (1,), + (1,), + (2,), + ] + assert program.sequential_execution_analysis(0).segment_indices == (0,) + assert program.sequential_execution_analysis(2).segment_indices == (2,) + with pytest.raises(ValueError, match="Parallel segments"): + program.sequential_execution_analysis(1) + + +def test_parallel_branch_rejects_segment_owned_lifecycle() -> None: + invoke = InvokeCfg(call=PickCfg(object="left_cube")) + parallel = ParallelCfg( + branches=( + SegmentCfg(name="branch", steps=invoke), + invoke, + ), + barrier=BarrierCfg(name="join"), + ) + + with pytest.raises(ValueError, match="wrap the Parallel node in one Segment"): + _config(parallel) + + +def test_segment_rejects_mixed_sequential_and_parallel_tree() -> None: + invoke = InvokeCfg(call=PickCfg(object="left_cube")) + config = _config( + SegmentCfg( + name="ambiguous_boundary", + steps=SequenceCfg(items=(invoke, _parallel())), + ) + ) + + with pytest.raises( + ExpertProgramCompileError, + match="either a call-only program or one direct Parallel", + ): + _compiler().compile(config) + + +__all__: list[str] = [] diff --git a/tests/gym/envs/expert_program/test_parallel_schema.py b/tests/gym/envs/expert_program/test_parallel_schema.py new file mode 100644 index 000000000..b34ffcb69 --- /dev/null +++ b/tests/gym/envs/expert_program/test_parallel_schema.py @@ -0,0 +1,137 @@ +# ---------------------------------------------------------------------------- +# 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 Expert Program schema Version 2 parallel nodes.""" + +from __future__ import annotations + +import pytest + +from embodichain.lab.gym.envs.expert_program.cfg import ( + BarrierCfg, + ExpertProgramCfg, + ExpertProgramIntegrationCfg, + InvokeCfg, + ParallelCfg, + PickCfg, +) +from embodichain.lab.gym.envs.expert_program.decoder import ( + ExpertProgramDecodeError, + decode_expert_program, +) + + +def _payload(*, schema_version: int = 2) -> dict[str, object]: + return { + "schema_version": schema_version, + "program_id": "parallel_pick", + "integration": { + "robot_profile": "dual_arm", + "scene_registry": "scene", + "runtime_preset": "safe", + }, + "targets": {}, + "program": { + "kind": "parallel", + "branches": [ + { + "kind": "invoke", + "call": {"kind": "pick", "object": "left_cube"}, + }, + { + "kind": "invoke", + "call": {"kind": "pick", "object": "right_cube"}, + }, + ], + "barrier": { + "kind": "barrier", + "name": "both_picked", + "timeout_steps": 200, + "failure_policy": "fail_fast", + }, + }, + } + + +def test_decode_schema_v2_parallel_with_explicit_barrier() -> None: + config = decode_expert_program(_payload()) + + assert config.schema_version == 2 + assert type(config.program) is ParallelCfg + assert len(config.program.branches) == 2 + assert config.program.barrier == BarrierCfg( + name="both_picked", + timeout_steps=200, + failure_policy="fail_fast", + ) + + +def test_schema_v1_rejects_parallel_discriminator() -> None: + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(_payload(schema_version=1)) + + assert error.value.code == "unknown_discriminator" + assert error.value.path == ("program", "kind") + + +def test_parallel_requires_two_branches_and_explicit_barrier() -> None: + payload = _payload() + program = payload["program"] + assert type(program) is dict + program["branches"] = program["branches"][:1] # type: ignore[index] + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(payload) + assert error.value.code == "parallel_branch_count" + + payload = _payload() + program = payload["program"] + assert type(program) is dict + del program["barrier"] + with pytest.raises(ExpertProgramDecodeError) as error: + decode_expert_program(payload) + assert error.value.code == "missing_field" + assert error.value.path == ("program", "barrier") + + +def test_barrier_is_not_valid_as_a_standalone_program() -> None: + with pytest.raises(ValueError, match="only be owned by Parallel"): + ExpertProgramCfg( + schema_version=2, + program_id="invalid_barrier", + integration=ExpertProgramIntegrationCfg( + robot_profile="profile", + scene_registry="scene", + runtime_preset="safe", + ), + targets={}, + program=BarrierCfg(name="orphan"), + ) + + +def test_parallel_cfg_rejects_nested_parallel() -> None: + invoke = InvokeCfg(call=PickCfg(object="cube")) + nested = ParallelCfg( + branches=(invoke, invoke), + barrier=BarrierCfg(name="inner"), + ) + with pytest.raises(ValueError, match="Nested Parallel"): + ParallelCfg( + branches=(nested, invoke), + barrier=BarrierCfg(name="outer"), + ) + + +__all__: list[str] = [] diff --git a/tests/gym/envs/expert_program/test_simulation.py b/tests/gym/envs/expert_program/test_simulation.py new file mode 100644 index 000000000..2cb1d27c4 --- /dev/null +++ b/tests/gym/envs/expert_program/test_simulation.py @@ -0,0 +1,524 @@ +# ---------------------------------------------------------------------------- +# 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 explicit Expert Program simulation bindings.""" + +from __future__ import annotations + +from dataclasses import dataclass, replace + +import pytest +import torch + +from embodichain.lab.gym.envs.expert_program.simulation import ( + AntipodalGraspAffordanceBinding, + ArticulationOperationAffordanceBinding, + ArticulationOperationTargetBinding, + ContainerAffordanceBinding, + ControlPartCommandPreset, + ControlPartEndpointBinding, + ControlPartResourceBinding, + RobotResourceBinding, + SimulationArticulationBinding, + SimulationArticulationLinkBinding, + SimulationRigidObjectBinding, + SimulationResourceEndpointBinding, + SimulationRobotResourceBinding, + SimulationRobotSkillProfileBinding, + SimulationSceneBinding, + SupportSurfaceAffordanceBinding, +) +from embodichain.lab.sim.atomic_actions import ( + AntipodalAffordance, + ArticulationOperationAffordance, + CARTESIAN_POSE_CAPABILITY, + GRASP_CAPABILITY, + PickUpOptions, +) +from embodichain.lab.sim.skills import ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + ContainerAffordance, + GRASP_AFFORDANCE_CAPABILITY, + PLACE_IN_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, + SceneAffordanceRef, + SceneArticulationRef, + SceneDynamics, + SceneLinkRef, + SceneObjectRef, + SkillPolicyPreset, + SupportSurfaceAffordance, +) +from embodichain.lab.sim.skills.profiles import ResourceEndpoint + +_BATCH_SIZE = 2 +_OPEN_TARGET = 0.42 +_OPEN_DISPLACEMENT = 0.4 + + +class _RigidObject: + """Minimal selected rigid object with a batched triangle mesh.""" + + def __init__(self) -> None: + self.pose = torch.eye(4).repeat(_BATCH_SIZE, 1, 1) + self.vertices = torch.tensor( + ( + (0.0, 0.0, 0.0), + (1.0, 0.0, 0.0), + (0.0, 1.0, 0.0), + ), + dtype=torch.float32, + ).repeat(_BATCH_SIZE, 1, 1) + self.triangles = torch.tensor( + (((0, 1, 2),),), + dtype=torch.int32, + ).repeat(_BATCH_SIZE, 1, 1) + + def get_local_pose(self, *, to_matrix: bool) -> torch.Tensor: + assert to_matrix is True + return self.pose + + def get_vertices( + self, + env_ids: list[int], + *, + scale: bool, + ) -> torch.Tensor: + assert scale is True + return self.vertices[env_ids] + + def get_triangles(self, env_ids: list[int]) -> torch.Tensor: + return self.triangles[env_ids] + + +class _Articulation: + """Minimal articulation exposing exact joint and link lookup surfaces.""" + + joint_names = ("drawer_slide",) + link_names = ("drawer_handle",) + + def __init__(self) -> None: + self.pose = torch.eye(4).repeat(_BATCH_SIZE, 1, 1) + self.qpos = torch.tensor(((0.1,), (0.2,)), dtype=torch.float32) + self.link_pose = torch.eye(4).repeat(_BATCH_SIZE, 1, 1) + self.link_pose[:, 0, 3] = torch.tensor((0.3, 0.4)) + + def get_local_pose(self, *, to_matrix: bool) -> torch.Tensor: + assert to_matrix is True + return self.pose + + def get_qpos(self, *, target: bool) -> torch.Tensor: + assert target is False + return self.qpos + + def get_link_pose( + self, + link_name: str, + *, + env_ids: list[int], + to_matrix: bool, + ) -> torch.Tensor: + assert link_name == "drawer_handle" + assert to_matrix is True + return self.link_pose[env_ids] + + +class _Simulation: + """Explicit native-UID lookup fixture.""" + + def __init__(self) -> None: + self.rigid_object = _RigidObject() + self.articulation = _Articulation() + + def get_rigid_object(self, uid: str) -> _RigidObject | None: + return self.rigid_object if uid == "native_cube" else None + + def get_articulation(self, uid: str) -> _Articulation | None: + return self.articulation if uid == "native_drawer" else None + + +class _Robot: + """Minimal robot control-part lookup fixture.""" + + control_parts = {"arm": object(), "hand": object()} + + def get_joint_ids(self, *, name: str) -> list[int]: + return {"arm": [0, 1], "hand": [2]}[name] + + +@dataclass(frozen=True, slots=True) +class _MobileEndpoint(ResourceEndpoint): + """Test-only non-joint endpoint declaration.""" + + controller_id: str + + +def _scene_binding() -> SimulationSceneBinding: + """Build one cube-and-drawer binding using only typed declarations.""" + return SimulationSceneBinding( + registry_id="tabletop", + rigid_objects=( + SimulationRigidObjectBinding( + entity_id="cube", + simulation_uid="native_cube", + aliases=("perceived_cube",), + dynamics=SceneDynamics.DYNAMIC, + semantic_type="cube", + default_grasp_affordance="cube_grasp", + ), + ), + articulations=( + SimulationArticulationBinding( + entity_id="drawer", + simulation_uid="native_drawer", + semantic_type="drawer", + default_operation_affordance="drawer_handle_operation", + ), + ), + links=( + SimulationArticulationLinkBinding( + entity_id="drawer_handle_link", + articulation_id="drawer", + native_link_name="drawer_handle", + ), + ), + antipodal_grasps=( + AntipodalGraspAffordanceBinding( + entity_id="cube_grasp", + object_id="cube", + native_name="mesh_antipodal", + revision="cube-grasp-v1", + ), + ), + articulation_operations=( + ArticulationOperationAffordanceBinding( + entity_id="drawer_handle_operation", + articulation_id="drawer", + link_id="drawer_handle_link", + joint_id="drawer_slide", + revision="drawer-operation-v1", + semantic_targets={ + "open": ArticulationOperationTargetBinding( + target_position=_OPEN_TARGET, + displacement=_OPEN_DISPLACEMENT, + ) + }, + ), + ), + support_surfaces=( + SupportSurfaceAffordanceBinding( + entity_id="cube_support_target", + parent_id="cube", + native_name="support_target", + object_target_pose=( + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.25, + 0.0, + 0.0, + 0.0, + 1.0, + ), + minimum_confidence=0.7, + is_default=True, + ), + ), + containers=( + ContainerAffordanceBinding( + entity_id="drawer_inside_target", + parent_id="drawer_handle_link", + native_name="inside_target", + object_target_pose=( + 1.0, + 0.0, + 0.0, + 0.1, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + ), + minimum_confidence=0.8, + is_default=True, + ), + ), + ) + + +def _profile_binding() -> SimulationRobotSkillProfileBinding: + """Build one manipulation profile declaration.""" + return SimulationRobotSkillProfileBinding( + profile_id="test_robot", + resources=( + ControlPartResourceBinding( + resource_id="manipulator", + endpoints=( + ControlPartEndpointBinding( + endpoint_id="motion", + control_part="arm", + capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + ), + ControlPartEndpointBinding( + endpoint_id="grasp", + control_part="hand", + capabilities=frozenset({GRASP_CAPABILITY}), + command_preset="parallel_gripper", + ), + ), + ), + ), + command_presets=( + ControlPartCommandPreset( + preset_id="parallel_gripper", + control_part="hand", + commands={"open": (0.0,), "grasp": (1.0,)}, + ), + ), + defaults={"pick_up": {"primary": "manipulator"}}, + presets=( + SkillPolicyPreset( + "safe", + action_option_templates={"pick": PickUpOptions()}, + ), + ), + default_preset="safe", + ) + + +def test_scene_binding_builds_existing_registry_contracts() -> None: + simulation = _Simulation() + + registry = _scene_binding().build(simulation) # type: ignore[arg-type] + snapshot = registry.make_scene_provider().snapshot( + timestamp=0.0, + env_ids=torch.tensor((0, 1), dtype=torch.long), + ) + + assert registry.resolve("perceived_cube") == SceneObjectRef("cube") + assert registry.resolve("drawer") == SceneArticulationRef("drawer") + assert registry.resolve("drawer_handle_link") == SceneLinkRef("drawer_handle_link") + grasp_ref = registry.resolve_affordance( + "cube", + capability=GRASP_AFFORDANCE_CAPABILITY, + ) + assert grasp_ref == SceneAffordanceRef("cube_grasp") + grasp = registry.lookup(grasp_ref).affordance + assert isinstance(grasp, AntipodalAffordance) + assert grasp.mesh_vertices is not None and grasp.mesh_vertices.shape == (3, 3) + operation_ref = registry.resolve_affordance( + "drawer", + capability=ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + ) + operation = registry.lookup(operation_ref).affordance + assert isinstance(operation, ArticulationOperationAffordance) + assert operation.joint_id == "drawer_slide" + assert operation.semantic_targets["open"].target_position == pytest.approx( + _OPEN_TARGET + ) + assert torch.equal( + snapshot.articulation_joints[("drawer", "drawer_slide")].position, + simulation.articulation.qpos, + ) + assert torch.equal( + snapshot.entities["drawer_handle_operation"].pose, + simulation.articulation.link_pose, + ) + support_ref = registry.resolve_affordance( + "cube", + capability=PLACE_ON_AFFORDANCE_CAPABILITY, + ) + support = registry.lookup(support_ref).affordance + assert type(support) is SupportSurfaceAffordance + assert support.minimum_confidence == pytest.approx(0.7) + assert torch.equal( + snapshot.entities[support_ref.entity_id].pose[:, 2, 3], + torch.full((_BATCH_SIZE,), 0.25), + ) + container_ref = registry.resolve_affordance( + "drawer_handle_link", + capability=PLACE_IN_AFFORDANCE_CAPABILITY, + ) + container = registry.lookup(container_ref).affordance + assert type(container) is ContainerAffordance + assert container.minimum_confidence == pytest.approx(0.8) + assert torch.allclose( + snapshot.entities[container_ref.entity_id].pose[:, 0, 3], + torch.tensor((0.4, 0.5)), + ) + + +def test_scene_binding_fails_closed_on_missing_native_entity() -> None: + binding = _scene_binding() + missing = replace( + binding.rigid_objects[0], + simulation_uid="missing_cube", + ) + + with pytest.raises(KeyError, match="missing_cube"): + replace(binding, rigid_objects=(missing,)).build( # type: ignore[arg-type] + _Simulation() + ) + + +def test_scene_binding_fails_closed_on_missing_native_link() -> None: + binding = _scene_binding() + missing = replace(binding.links[0], native_link_name="missing_handle") + + with pytest.raises(KeyError, match="missing_handle"): + replace(binding, links=(missing,)).build( # type: ignore[arg-type] + _Simulation() + ) + + +def test_scene_binding_fails_closed_on_missing_native_joint() -> None: + binding = _scene_binding() + missing = replace( + binding.articulation_operations[0], + joint_id="missing_joint", + ) + + with pytest.raises(KeyError, match="missing_joint"): + replace(binding, articulation_operations=(missing,)).build( # type: ignore[arg-type] + _Simulation() + ) + + +def test_robot_profile_binding_builds_existing_profile_contracts() -> None: + profile = _profile_binding().build(_Robot()) # type: ignore[arg-type] + + resource = profile.resources["manipulator"] + motion = resource.endpoints["motion"] + grasp = resource.endpoints["grasp"] + assert motion.control_part == "arm" + assert motion.capabilities == frozenset({CARTESIAN_POSE_CAPABILITY}) + assert grasp.control_part == "hand" + assert grasp.command_profile == "parallel_gripper" + command = profile.command_profiles["parallel_gripper"].commands["grasp"] + assert torch.equal(command.positions, torch.tensor((1.0,))) + assert profile.defaults["pick_up"].resources == {"primary": "manipulator"} + + +def test_generic_resource_binding_owns_arbitrary_typed_endpoint() -> None: + endpoint = _MobileEndpoint( + controller_id="base_controller", + capabilities=frozenset({"motion.base.velocity"}), + ) + binding = RobotResourceBinding( + resource_id="mobile_base", + endpoints={"motion": endpoint}, + ) + + assert isinstance(binding, SimulationRobotResourceBinding) + resource = binding.build(object()) # type: ignore[arg-type] + built_endpoint = resource.endpoints["motion"] + + assert isinstance(built_endpoint, _MobileEndpoint) + assert built_endpoint is not endpoint + assert built_endpoint.controller_id == "base_controller" + assert resource.members == () + + +def test_control_part_endpoint_binding_implements_public_build_protocol() -> None: + binding = ControlPartEndpointBinding( + endpoint_id="motion", + control_part="arm", + capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + ) + + assert isinstance(binding, SimulationResourceEndpointBinding) + + +def test_whole_body_control_part_remains_supported_and_strict() -> None: + class WholeBodyRobot: + control_parts = {"whole_body": object()} + + def get_joint_ids(self, *, name: str) -> list[int]: + assert name == "whole_body" + return [0, 1, 2, 3] + + binding = SimulationRobotSkillProfileBinding( + profile_id="whole_body_robot", + resources=( + ControlPartResourceBinding( + resource_id="body", + endpoints=( + ControlPartEndpointBinding( + endpoint_id="motion", + control_part="whole_body", + capabilities=frozenset({"motion.whole_body"}), + ), + ), + ), + ), + ) + + profile = binding.build(WholeBodyRobot()) # type: ignore[arg-type] + endpoint = profile.resources["body"].endpoints["motion"] + + assert endpoint.control_part == "whole_body" + assert endpoint.capabilities == frozenset({"motion.whole_body"}) + + +def test_robot_profile_binding_fails_closed_on_missing_control_part() -> None: + binding = _profile_binding() + resource = binding.resources[0] + missing_endpoint = replace( + resource.endpoints[0], + control_part="missing_arm", + ) + + with pytest.raises(KeyError, match="missing_arm"): + replace( + binding, + resources=( + replace( + resource, + endpoints=(missing_endpoint, resource.endpoints[1]), + ), + ), + ).build( + _Robot() + ) # type: ignore[arg-type] + + +def test_robot_profile_binding_rejects_wrong_command_width() -> None: + binding = _profile_binding() + invalid = replace( + binding.command_presets[0], + commands={"open": (0.0, 0.0), "grasp": (1.0, 1.0)}, + ) + + with pytest.raises(ValueError, match="has 2 positions.*has 1 joints"): + replace(binding, command_presets=(invalid,)).build( # type: ignore[arg-type] + _Robot() + ) diff --git a/tests/gym/envs/expert_program/test_simulation_environment.py b/tests/gym/envs/expert_program/test_simulation_environment.py new file mode 100644 index 000000000..11e07b7c0 --- /dev/null +++ b/tests/gym/envs/expert_program/test_simulation_environment.py @@ -0,0 +1,2885 @@ +# ---------------------------------------------------------------------------- +# 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 reusable simulation-backed Expert Program assembly.""" + +from __future__ import annotations + +import ast +from collections.abc import Mapping, Sequence +from dataclasses import dataclass, fields, is_dataclass, replace +import inspect +import json +import textwrap +from types import MappingProxyType, MethodType, SimpleNamespace +from typing import Any, ClassVar +from unittest.mock import MagicMock + +import pytest +import torch + +from embodichain.agents.mllm import compile_mllm_expert_program +from embodichain.lab.gym.envs.expert_program import ( + AntipodalGraspAffordanceBinding, + CompiledProgram, + ControlCommandStateEvidenceTracker, + ControlPartCommandPreset, + ControlPartEndpointBinding, + ControlPartResourceBinding, + ExpertProgramCompiler, + ExpertProgramCfg, + ExpertProgramIntegrationCfg, + ExpertProgramEnvironmentAdapter, + ExpertProgramRuntimeAssembly, + HandOverCfg, + InvokeCfg, + IntegrationFingerprintMismatch, + RobotResourceBinding, + SharedTickSceneProvider, + SimulationExpertProgramRegistration, + SimulationExpertProgramFactory, + SimulationPlanningObservationProvider, + SimulationRigidObjectBinding, + SimulationRobotSkillProfileBinding, + SimulationSceneBinding, + create_simulation_expert_program_adapter, + decode_expert_program, +) +from embodichain.lab.gym.envs.expert_program.bridge import EnvironmentStepClock +from embodichain.lab.sim.atomic_actions import ( + ActionInvocation, + Affordance, + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + CommandAcknowledgement, + EntityState, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + HeldObjectState, + HandOverOptions, + MotionPolicy, + ObservedArticulationJointState, + PlanningContext, + PickUpOptions, + PlaceOptions, + StateDelta, + TaskState, + TimedTrajectory, + TrackingPolicy, +) +from embodichain.lab.sim.atomic_actions.runner import ExecutionRunnerCfg +from embodichain.lab.sim.atomic_actions.tracking import ( + JOINT_POSITION_CHANNEL, + EndpointTrackingChannelBinding, + EndpointTrackingFeedbackAddress, + TrackingFeedbackSourceRef, + TrackingProjectorRef, +) +from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget +from embodichain.lab.sim.atomic_actions.bindings import RuntimeEndpointTarget +from embodichain.lab.sim.atomic_actions.control import ControlPartCommandProfile +from embodichain.lab.sim.atomic_actions.runtime_commands import ( + EndpointCommand, + JointPositionPayload, + RuntimeCommandFrame, + RuntimeCommandPayload, +) +from embodichain.lab.sim.planners import MotionGenerator +from embodichain.lab.sim.skills import ( + AtomicSkills, + BoundSemanticCall, + EndpointResolution, + HandOver, + HandOverPoseProvider, + HandOverPoseTargets, + Pick, + Place, + RelationTargetGrounder, + ResourceEndpoint, + ResourceEndpointAdapter, + SceneArticulationRef, + SceneEntityRegistration, + SceneRegistry, + SemanticCallSpec, + SemanticObjectTarget, + SemanticPose, + SemanticRelationTarget, + SkillPolicyPreset, + WorkflowRecoveryPolicy, +) +from embodichain.lab.sim.skills.effects import ( + CONSTRAINT_EFFECT_CHANNEL, + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + BinaryEffectClause, + BinaryEvidenceKind, + ControlPartEvidenceAddress, + EffectEvidenceSourceRef, + HeldObjectRelation, + HeldObjectStateExpectation, + JOINT_STATE_EFFECT_CHANNEL, +) +from embodichain.lab.sim.skills.evidence import ( + BinaryEffectEvidenceQuery, + BinaryEffectEvidenceBatch, + BinaryEffectObservation, + EffectEvidenceCollectionContext, + PoseRelationEvidenceBatch, +) +from embodichain.lab.sim.skills.runtime import ( + SkillEffectTrace, + SkillResult, + SkillRuntime, + SkillStatus, +) +from embodichain.lab.sim.skills.scene import SceneObjectRef + +_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 +_HAND_INTERMEDIATE_POSITION = 0.4 +_DUAL_ROBOT_DOF = 4 +_RELEASE_SEPARATION = 0.2 +_DIRECT_PLACE_TARGET = SemanticPose( + position=(0.0, 0.0, 0.0), + quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), +) +_QUICKSTART_MAX_LINES = 15 + + +def _command_state_tracker() -> ControlCommandStateEvidenceTracker: + """Build a three-row tracker for one semantic gripper profile.""" + profile = ControlPartCommandProfile.joint_positions( + open=torch.tensor((_HAND_OPEN_POSITION,)), + grasp=torch.tensor((_HAND_GRASP_POSITION,)), + ) + return ControlCommandStateEvidenceTracker( + {"hand": profile}, + _TRACKER_ENV_IDS, + ) + + +def _hand_command_frame( + *, + env_ids: tuple[int, ...], + positions: tuple[float, ...], + active: tuple[bool, ...] | None = None, +) -> RuntimeCommandFrame: + """Build one row-addressed semantic hand command frame.""" + batch_size = len(env_ids) + if len(positions) != batch_size: + raise ValueError("positions must have one value per environment ID.") + if active is None: + active = (True,) * batch_size + return RuntimeCommandFrame( + commands=( + EndpointCommand( + target=JointPositionTarget("hand", (1,)), + payload=JointPositionPayload( + torch.tensor(positions, dtype=torch.float32).unsqueeze(1) + ), + ), + ), + active_mask=torch.tensor(active, dtype=torch.bool), + env_ids=torch.tensor(env_ids, dtype=torch.long), + hold_duration=torch.full((batch_size,), _STEP_DT), + ) + + +def _hand_state_observation( + tracker: ControlCommandStateEvidenceTracker, + *env_ids: int, +) -> BinaryEffectObservation: + """Observe command-state evidence in an explicit stable-ID order.""" + expectation = HeldObjectStateExpectation( + expectation_id="held-cube", + relation=HeldObjectRelation.ATTACHED, + object_id="cube", + slot_id="primary", + resource_id="manipulator", + task_state_key="held-cube", + ) + query = BinaryEffectEvidenceQuery( + BinaryEffectClause( + clause_id="hand-constraint", + expectation_id=expectation.expectation_id, + source=EffectEvidenceSourceRef( + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + ControlPartEvidenceAddress("hand", CONSTRAINT_EFFECT_CHANNEL), + ), + evidence_kind=BinaryEvidenceKind.CONSTRAINT, + expected=True, + ), + expectation, + ) + context = EffectEvidenceCollectionContext( + timestamp=0.0, + observation_revision=0, + env_ids=torch.tensor(env_ids, dtype=torch.long), + ) + return tracker.observe(query, context) + + +class _CountingEntityProvider: + """Return row-addressed poses and record every native acquisition.""" + + def __init__(self) -> None: + self.calls: list[tuple[float, torch.Tensor]] = [] + + def observe(self, *, timestamp: float, env_ids: torch.Tensor) -> EntityState: + """Return one distinct x translation for each environment ID.""" + self.calls.append((timestamp, env_ids.clone())) + pose = torch.eye(4).repeat(env_ids.numel(), 1, 1) + pose[:, 0, 3] = env_ids.to(dtype=pose.dtype) + return EntityState(pose) + + +class _CountingJointProvider: + """Return row-addressed articulation state and record acquisitions.""" + + def __init__(self) -> None: + self.calls: list[tuple[float, torch.Tensor]] = [] + + def observe_joints( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> dict[str, ObservedArticulationJointState]: + """Return one scalar joint position per environment ID.""" + self.calls.append((timestamp, env_ids.clone())) + position = env_ids.to(dtype=torch.float32).unsqueeze(1) + return { + "slide": ObservedArticulationJointState( + position, + torch.ones(env_ids.numel(), dtype=torch.bool), + ) + } + + +def _shared_scene_provider() -> tuple[ + SharedTickSceneProvider, + _CountingEntityProvider, + _CountingJointProvider, +]: + """Build one full-batch registry provider with observable acquisitions.""" + entity_provider = _CountingEntityProvider() + joint_provider = _CountingJointProvider() + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneArticulationRef("drawer"), + state_provider=entity_provider, + joint_state_provider=joint_provider, + ), + ) + ) + delegate = registry.make_scene_provider(batch_size=_BATCH_SIZE) + full_env_ids = torch.arange(_BATCH_SIZE, dtype=torch.long) + return ( + SharedTickSceneProvider(delegate, full_env_ids), + entity_provider, + joint_provider, + ) + + +def test_shared_tick_scene_provider_projects_partial_rows_without_resampling() -> None: + """Planning full batch and evidence subsets share one native acquisition.""" + provider, entity_provider, joint_provider = _shared_scene_provider() + full_env_ids = torch.arange(_BATCH_SIZE, dtype=torch.long) + + full = provider.snapshot(timestamp=0.0, env_ids=full_env_ids) + subset = provider.snapshot( + timestamp=0.0, + env_ids=torch.tensor((2, 0), dtype=torch.long), + ) + + assert len(entity_provider.calls) == 1 + assert len(joint_provider.calls) == 1 + assert torch.equal(entity_provider.calls[0][1], full_env_ids) + assert full.entities["drawer"].pose[:, 0, 3].tolist() == [0.0, 1.0, 2.0] + assert subset.entities["drawer"].pose[:, 0, 3].tolist() == [2.0, 0.0] + joint = subset.articulation_joints[("drawer", "slide")] + assert joint.position[:, 0].tolist() == [2.0, 0.0] + assert joint.valid_mask is not None and joint.valid_mask.tolist() == [True, True] + assert subset.collision_world_revision == (0, 0) + + +def test_shared_tick_scene_provider_captures_full_batch_when_subset_arrives_first() -> ( + None +): + """A partial first consumer cannot poison the delegate's stable batch.""" + provider, entity_provider, joint_provider = _shared_scene_provider() + requested = torch.tensor((1,), dtype=torch.long) + + first = provider.snapshot(timestamp=0.0, env_ids=requested) + second = provider.snapshot( + timestamp=0.0, + env_ids=torch.tensor((2, 1), dtype=torch.long), + ) + + expected_full = torch.arange(_BATCH_SIZE, dtype=torch.long) + assert torch.equal(entity_provider.calls[0][1], expected_full) + assert torch.equal(joint_provider.calls[0][1], expected_full) + assert len(entity_provider.calls) == 1 + assert first.entities["drawer"].pose[:, 0, 3].tolist() == [1.0] + assert second.entities["drawer"].pose[:, 0, 3].tolist() == [2.0, 1.0] + + +def test_shared_tick_scene_provider_rejects_unknown_or_regressing_rows() -> None: + """Unknown correlations and time regressions fail before native sampling.""" + provider, entity_provider, _ = _shared_scene_provider() + provider.snapshot( + timestamp=0.5, + env_ids=torch.tensor((0, 2), dtype=torch.long), + ) + + with pytest.raises(ValueError, match="absent from full_env_ids"): + provider.snapshot( + timestamp=0.5, + env_ids=torch.tensor((3,), dtype=torch.long), + ) + with pytest.raises(ValueError, match="monotonic"): + provider.snapshot( + timestamp=0.4, + env_ids=torch.tensor((0,), dtype=torch.long), + ) + + assert len(entity_provider.calls) == 1 + + +def test_command_state_tracker_correlates_open_and_grasp_across_subsets() -> None: + """Stable IDs, not subset row positions, own accepted gripper state.""" + tracker = _command_state_tracker() + + tracker.accepted( + _hand_command_frame( + env_ids=(3, 7), + positions=(_HAND_GRASP_POSITION, _HAND_OPEN_POSITION), + ) + ) + observation = _hand_state_observation(tracker, 7, 11, 3) + + assert tracker.tracked_control_parts == ("hand",) + assert observation.values.tolist() == [False, False, True] + assert observation.valid is not None + assert observation.valid.tolist() == [True, False, True] + assert observation.acquisition_errors[0] is None + assert observation.acquisition_errors[1] is not None + assert observation.acquisition_errors[2] is None + + +def test_command_state_tracker_preserves_intermediate_and_inactive_rows() -> None: + """Unrecognized targets and inactive rows cannot overwrite prior evidence.""" + tracker = _command_state_tracker() + tracker.accepted( + _hand_command_frame( + env_ids=(7, 3), + positions=(_HAND_OPEN_POSITION, _HAND_GRASP_POSITION), + ) + ) + + tracker.accepted( + _hand_command_frame( + env_ids=(3, 7), + positions=( + _HAND_INTERMEDIATE_POSITION, + _HAND_INTERMEDIATE_POSITION, + ), + ) + ) + after_intermediate = _hand_state_observation(tracker, 3, 7) + assert after_intermediate.values.tolist() == [True, False] + assert after_intermediate.valid is not None + assert after_intermediate.valid.tolist() == [True, True] + + tracker.accepted( + _hand_command_frame( + env_ids=(3, 7), + positions=(_HAND_OPEN_POSITION, _HAND_GRASP_POSITION), + active=(False, True), + ) + ) + after_inactive_row = _hand_state_observation(tracker, 3, 7) + assert after_inactive_row.values.tolist() == [True, True] + assert after_inactive_row.valid is not None + assert after_inactive_row.valid.tolist() == [True, True] + + +def test_command_state_tracker_cancel_invalidates_target_state() -> None: + """Cancelling a hand destination invalidates every correlated hand row.""" + tracker = _command_state_tracker() + frame = _hand_command_frame( + env_ids=(7, 3), + positions=(_HAND_OPEN_POSITION, _HAND_GRASP_POSITION), + ) + tracker.accepted(frame) + + tracker.cancelled(frame.targets) + observation = _hand_state_observation(tracker, 3, 7) + + assert observation.values.tolist() == [False, False] + assert observation.valid is not None + assert observation.valid.tolist() == [False, False] + assert all(error is not None for error in observation.acquisition_errors) + + +def test_command_state_tracker_discard_invalidates_all_state() -> None: + """A fail-closed sink discard removes every accepted row state.""" + tracker = _command_state_tracker() + tracker.accepted( + _hand_command_frame( + env_ids=(11, 3), + positions=(_HAND_GRASP_POSITION, _HAND_OPEN_POSITION), + ) + ) + + tracker.discarded() + observation = _hand_state_observation(tracker, 11, 3) + + assert observation.values.tolist() == [False, False] + assert observation.valid is not None + assert observation.valid.tolist() == [False, False] + + +def test_command_state_tracker_rejects_unknown_environment_ids() -> None: + """Unknown correlation IDs fail before tracker state can be mutated or read.""" + tracker = _command_state_tracker() + + with pytest.raises(ValueError, match="absent from tracker env_ids"): + tracker.accepted( + _hand_command_frame( + env_ids=(99,), + positions=(_HAND_GRASP_POSITION,), + ) + ) + with pytest.raises(ValueError, match="absent from tracker env_ids"): + _hand_state_observation(tracker, 99) + + observation = _hand_state_observation(tracker, 7, 3, 11) + assert observation.valid is not None + assert observation.valid.tolist() == [False, False, False] + + +class _Robot: + """Minimal typed robot surface used by the production factory.""" + + uid = "robot" + device = torch.device("cpu") + dof = _ROBOT_DOF + control_parts = {"arm": ("joint_0",)} + + def __init__(self) -> None: + self.qpos = torch.zeros(_BATCH_SIZE, _ROBOT_DOF) + + def get_qpos( + self, + name: str | None = None, + target: bool = False, + ) -> torch.Tensor: + """Return full or control-part positions.""" + del target + return self.qpos if name is None else self.qpos[:, :1] + + def get_qvel( + self, + name: str | None = None, + target: bool = False, + ) -> torch.Tensor: + """Return zero measured velocities.""" + return torch.zeros_like(self.get_qpos(name=name, target=target)) + + def get_qf(self, name: str | None = None) -> torch.Tensor: + """Return zero measured effort.""" + return torch.zeros_like(self.get_qpos(name=name)) + + def get_joint_ids(self, name: str) -> list[int]: + """Resolve the only declared control part.""" + if name != "arm": + raise KeyError(name) + return [0] + + def get_solver(self, name: str) -> object: + """Return a configured solver marker for Cartesian capability.""" + if name != "arm": + raise KeyError(name) + return object() + + def compute_fk( + self, + qpos: torch.Tensor, + name: str | None = None, + env_ids: list[int] | None = None, + to_matrix: bool = False, + ) -> torch.Tensor: + """Return identity endpoint poses for evidence adapter validation.""" + del name, env_ids + if not to_matrix: + raise ValueError("Tests require matrix FK output.") + return torch.eye(4).repeat(qpos.shape[0], 1, 1) + + +class _EvidenceRobot(_Robot): + """Joint-backed arm and hand with a mutable measured endpoint pose.""" + + control_parts = { + "arm": ("joint_0",), + "hand": ("joint_1",), + } + + def __init__(self) -> None: + super().__init__() + self.endpoint_pose = torch.eye(4).repeat(_BATCH_SIZE, 1, 1) + + def get_qpos( + self, + name: str | None = None, + target: bool = False, + ) -> torch.Tensor: + """Return the full state or the selected control-part state.""" + del target + if name is None: + return self.qpos + joint_id = self.get_joint_ids(name)[0] + return self.qpos[:, joint_id : joint_id + 1] + + def get_joint_ids(self, name: str) -> list[int]: + """Resolve the disjoint arm and hand joints.""" + if name == "arm": + return [0] + if name == "hand": + return [1] + raise KeyError(name) + + def get_solver(self, name: str) -> object: + """Return the configured arm solver marker.""" + if name != "arm": + raise KeyError(name) + return object() + + def compute_fk( + self, + qpos: torch.Tensor, + name: str | None = None, + env_ids: list[int] | None = None, + to_matrix: bool = False, + ) -> torch.Tensor: + """Return the live arm endpoint pose for requested simulator rows.""" + del qpos + if name != "arm" or not to_matrix: + raise ValueError("Evidence FK requires the arm matrix pose.") + rows = list(range(_BATCH_SIZE)) if env_ids is None else env_ids + return self.endpoint_pose[rows].clone() + + +class _DualRobot(_Robot): + """Four-part dual-arm robot used for provider-aware helper preflight.""" + + uid = "dual_robot" + dof = _DUAL_ROBOT_DOF + control_parts = { + "left_arm": ("left_arm_joint",), + "left_hand": ("left_hand_joint",), + "right_arm": ("right_arm_joint",), + "right_hand": ("right_hand_joint",), + } + _joint_ids = { + "left_arm": (0,), + "left_hand": (1,), + "right_arm": (2,), + "right_hand": (3,), + } + + def __init__(self) -> None: + self.qpos = torch.zeros(_BATCH_SIZE, self.dof) + + def get_qpos( + self, + name: str | None = None, + target: bool = False, + ) -> torch.Tensor: + """Return full state or the selected one-joint control part.""" + del target + if name is None: + return self.qpos + return self.qpos[:, list(self._joint_ids[name])] + + def get_joint_ids(self, name: str) -> list[int]: + """Resolve one disjoint arm or hand joint.""" + return list(self._joint_ids[name]) + + def get_solver(self, name: str) -> object: + """Return configured solver markers for both motion endpoints.""" + if name not in {"left_arm", "right_arm"}: + raise KeyError(name) + return object() + + def compute_fk( + self, + qpos: torch.Tensor, + name: str | None = None, + env_ids: list[int] | None = None, + to_matrix: bool = False, + ) -> torch.Tensor: + """Return identity arm endpoint poses for runtime assembly checks.""" + del env_ids + if name not in {"left_arm", "right_arm"} or not to_matrix: + raise ValueError("Dual-arm evidence requires an arm matrix pose.") + return torch.eye(4).repeat(qpos.shape[0], 1, 1) + + +class _RigidObject: + """Mutable batched rigid object with the mesh surface required by binding.""" + + def __init__(self) -> None: + self.pose = torch.eye(4).repeat(_BATCH_SIZE, 1, 1) + + def get_local_pose(self, *, to_matrix: bool = False) -> torch.Tensor: + """Return the current measured object pose.""" + if not to_matrix: + raise ValueError("Tests require matrix object poses.") + return self.pose.clone() + + def get_vertices( + self, + *, + env_ids: list[int], + scale: bool = True, + ) -> torch.Tensor: + """Return one minimal triangular mesh per requested row.""" + del scale + vertices = torch.tensor(((0.0, 0.0, 0.0), (0.04, 0.0, 0.0), (0.0, 0.04, 0.0))) + return vertices.unsqueeze(0).repeat(len(env_ids), 1, 1) + + def get_triangles(self, *, env_ids: list[int]) -> torch.Tensor: + """Return one valid triangle per requested row.""" + return ( + torch.tensor(((0, 1, 2),), dtype=torch.long) + .unsqueeze(0) + .repeat(len(env_ids), 1, 1) + ) + + +class _ForwardedRelationGrounder(RelationTargetGrounder): + """Sentinel relation grounder installed only to prove helper forwarding.""" + + capability: ClassVar[str] = "test.place_relation" + affordance_type: ClassVar[type[Affordance]] = Affordance + affordance_revision: ClassVar[str] = "test-v1" + + def ground( + self, + relation: SemanticRelationTarget, + *, + affordance: Affordance, + context: PlanningContext, + ) -> torch.Tensor: + """Return a direct identity target when explicitly exercised.""" + del relation, affordance, context + return torch.eye(4) + + +class _ForwardedHandOverPoseProvider(HandOverPoseProvider): + """Sentinel embodiment provider installed only through the standard helper.""" + + provider_id: ClassVar[str] = "test.handover_pose" + + def resolve( + self, + call: HandOver, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> HandOverPoseTargets: + """Return owned direct targets without embedding task-side motion code.""" + del call, context, bound + pose = SemanticPose( + position=(0.0, 0.0, 0.5), + quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), + ) + return HandOverPoseTargets( + middle=SemanticObjectTarget(pose), + final=SemanticObjectTarget(pose), + ) + + +@dataclass(frozen=True, slots=True) +class _MobileEndpoint(ResourceEndpoint): + """Non-joint endpoint used by the standard simulation factory test.""" + + controller_id: str + + +@dataclass(frozen=True, slots=True) +class _MobileTarget(RuntimeEndpointTarget): + """Runtime destination for the test mobile controller.""" + + TRANSPORT_ID: ClassVar[str] = "test.mobile_velocity" + controller_id: str + + @property + def transport_id(self) -> str: + """Return the matching test Gym transport ID.""" + return self.TRANSPORT_ID + + @property + def target_id(self) -> str: + """Return the selected controller ID.""" + return self.controller_id + + +@dataclass(frozen=True, slots=True) +class _UndeclaredMobileTarget(RuntimeEndpointTarget): + """Live target intentionally absent from the adapter declaration.""" + + TRANSPORT_ID: ClassVar[str] = _MobileTarget.TRANSPORT_ID + controller_id: str + + @property + def transport_id(self) -> str: + return self.TRANSPORT_ID + + @property + def target_id(self) -> str: + return self.controller_id + + +@dataclass(frozen=True, slots=True) +class _LyingTransportMobileTarget(RuntimeEndpointTarget): + """Declare one transport statically but expose another on the live value.""" + + TRANSPORT_ID: ClassVar[str] = _MobileTarget.TRANSPORT_ID + controller_id: str + + @property + def transport_id(self) -> str: + return "test.unregistered_live_transport" + + @property + def target_id(self) -> str: + return self.controller_id + + +class _MobileEndpointAdapter(ResourceEndpointAdapter): + """Resolve a mobile endpoint without consulting robot control parts.""" + + adapter_id: ClassVar[str] = "test.mobile_velocity" + endpoint_type: ClassVar[type[ResourceEndpoint]] = _MobileEndpoint + runtime_transport_ids: ClassVar[frozenset[str]] = frozenset( + {_MobileTarget.TRANSPORT_ID} + ) + runtime_target_types: ClassVar[tuple[type[RuntimeEndpointTarget], ...]] = ( + _MobileTarget, + ) + tracking_feedback_source_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset() + tracking_projector_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset() + effect_evidence_source_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset() + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: Any, + ) -> EndpointResolution: + """Resolve one exclusive controller claim.""" + del engine + if not isinstance(endpoint, _MobileEndpoint): + raise TypeError("_MobileEndpointAdapter requires _MobileEndpoint.") + return EndpointResolution( + runtime_target=_MobileTarget(endpoint.controller_id), + claim_tokens=frozenset({f"controller:{endpoint.controller_id}"}), + ) + + +class _LyingMobileEndpointAdapter(_MobileEndpointAdapter): + """Declare one target type but resolve a different live target type.""" + + adapter_id: ClassVar[str] = "test.lying_mobile_velocity" + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: Any, + ) -> EndpointResolution: + del engine + if not isinstance(endpoint, _MobileEndpoint): + raise TypeError("_LyingMobileEndpointAdapter requires _MobileEndpoint.") + return EndpointResolution( + runtime_target=_UndeclaredMobileTarget(endpoint.controller_id), + claim_tokens=frozenset({f"controller:{endpoint.controller_id}"}), + ) + + +class _LyingTransportMobileEndpointAdapter(_MobileEndpointAdapter): + """Resolve a target whose live transport contradicts its static owner.""" + + adapter_id: ClassVar[str] = "test.lying_mobile_transport" + runtime_target_types: ClassVar[tuple[type[RuntimeEndpointTarget], ...]] = ( + _LyingTransportMobileTarget, + ) + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: Any, + ) -> EndpointResolution: + del engine + if not isinstance(endpoint, _MobileEndpoint): + raise TypeError("_LyingTransportMobileEndpointAdapter requires mobile.") + return EndpointResolution( + runtime_target=_LyingTransportMobileTarget(endpoint.controller_id), + claim_tokens=frozenset({f"controller:{endpoint.controller_id}"}), + ) + + +class _LyingAdapterIdMobileEndpointAdapter(_MobileEndpointAdapter): + """Expose a different live adapter ID than the class declaration.""" + + adapter_id: ClassVar[str] = "test.declared_mobile_adapter" + + def __getattribute__(self, name: str) -> Any: + if name == "adapter_id": + return "test.live_mobile_adapter" + return super().__getattribute__(name) + + +class _LyingFeedbackMobileEndpointAdapter(_MobileEndpointAdapter): + """Emit fingerprinted tracking routes absent from the declaration.""" + + adapter_id: ClassVar[str] = "test.lying_mobile_feedback" + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: Any, + ) -> EndpointResolution: + del engine + if not isinstance(endpoint, _MobileEndpoint): + raise TypeError("_LyingFeedbackMobileEndpointAdapter requires mobile.") + target = _MobileTarget(endpoint.controller_id) + tracking = EndpointTrackingChannelBinding( + JOINT_POSITION_CHANNEL, + TrackingFeedbackSourceRef( + "planning_context.robot", + "1", + EndpointTrackingFeedbackAddress(target, JOINT_POSITION_CHANNEL), + ), + TrackingProjectorRef("joint_position_payload", "1"), + ) + return EndpointResolution( + runtime_target=target, + tracking_channels={JOINT_POSITION_CHANNEL: tracking}, + claim_tokens=frozenset({f"controller:{endpoint.controller_id}"}), + ) + + +class _LyingProjectorMobileEndpointAdapter(_LyingFeedbackMobileEndpointAdapter): + """Declare only the live feedback route while hiding its projector route.""" + + adapter_id: ClassVar[str] = "test.lying_mobile_projector" + tracking_feedback_source_keys: ClassVar[frozenset[tuple[str, str]]] = frozenset( + {("planning_context.robot", "1")} + ) + + +class _LyingEvidenceMobileEndpointAdapter(_MobileEndpointAdapter): + """Emit effect evidence absent from the adapter declaration.""" + + adapter_id: ClassVar[str] = "test.lying_mobile_evidence" + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: Any, + ) -> EndpointResolution: + del engine + if not isinstance(endpoint, _MobileEndpoint): + raise TypeError("_LyingEvidenceMobileEndpointAdapter requires mobile.") + return EndpointResolution( + runtime_target=_MobileTarget(endpoint.controller_id), + effect_sources={ + JOINT_STATE_EFFECT_CHANNEL: EffectEvidenceSourceRef( + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + ControlPartEvidenceAddress( + endpoint.controller_id, + JOINT_STATE_EFFECT_CHANNEL, + ), + ) + }, + claim_tokens=frozenset({f"controller:{endpoint.controller_id}"}), + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class _MobilePayload(RuntimeCommandPayload): + """Minimal payload declaration for the custom transport contract.""" + + TRANSPORT_ID: ClassVar[str] = _MobileTarget.TRANSPORT_ID + values: torch.Tensor + + def __post_init__(self) -> None: + object.__setattr__(self, "values", self.values.clone()) + + @property + def batch_size(self) -> int: + return int(self.values.shape[0]) + + @property + def device(self) -> torch.device: + return self.values.device + + @property + def transport_id(self) -> str: + return self.TRANSPORT_ID + + def snapshot(self) -> _MobilePayload: + return _MobilePayload(self.values) + + +class _MobileTransportEncoder: + """Minimal Gym encoder registered for the custom mobile target.""" + + transport_id: ClassVar[str] = _MobileTarget.TRANSPORT_ID + target_types: ClassVar[tuple[type[RuntimeEndpointTarget], ...]] = (_MobileTarget,) + payload_types: ClassVar[tuple[type[RuntimeCommandPayload], ...]] = (_MobilePayload,) + + def encode( + self, + command: EndpointCommand, + *, + base_action: Any, + active_mask: torch.Tensor, + ) -> Any: + """Write the custom payload into one test controller channel.""" + if type(command.payload) is not _MobilePayload: + raise TypeError("_MobileTransportEncoder requires _MobilePayload.") + action = base_action.clone() + action[active_mask, 0] = command.payload.values[active_mask] + return action + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + base_action: Any, + context: Any, + ) -> Any: + """Preserve the base action for a mobile safe hold.""" + del targets, context + return base_action.clone() + + +class _LyingTargetMobileTransportEncoder(_MobileTransportEncoder): + """Declare the statically owned type whose live transport property lies.""" + + target_types: ClassVar[tuple[type[RuntimeEndpointTarget], ...]] = ( + _LyingTransportMobileTarget, + ) + + +class _MobileRobot: + """Full-state robot fixture with no control-parts or joint-ID surface.""" + + uid = "mobile_robot" + device = torch.device("cpu") + dof = _ROBOT_DOF + + def __init__(self) -> None: + self.qpos = torch.zeros(_BATCH_SIZE, _ROBOT_DOF) + + 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: + """Return the full measured velocity state.""" + return torch.zeros_like(self.qpos) + + def get_qf(self) -> torch.Tensor: + """Return the full measured effort state.""" + return torch.zeros_like(self.qpos) + + +class _Simulation: + """Minimal simulation registry for one exact robot.""" + + def __init__( + self, + robot: _Robot, + rigid_objects: dict[str, _RigidObject] | None = None, + ) -> None: + self.robot = robot + self.rigid_objects = {} if rigid_objects is None else dict(rigid_objects) + + def get_robot(self, uid: str) -> _Robot | None: + """Resolve the selected robot UID.""" + return self.robot if uid == self.robot.uid else None + + def get_rigid_object(self, uid: str) -> _RigidObject | None: + """Resolve one explicitly registered rigid-object UID.""" + return self.rigid_objects.get(uid) + + +class _RegisteredParallelSafety: + """Fresh test gate produced only by its registration-owned factory.""" + + def validate( + self, + *, + branch_frames: Mapping[str, RuntimeCommandFrame], + merged_frame: RuntimeCommandFrame, + ) -> None: + del branch_frames, merged_frame + + +class _RegisteredParallelSafetyFactory: + """Stateless declarative factory for a live joint-transport gate.""" + + validator_id: ClassVar[str] = "test.registered_parallel_safety" + revision: ClassVar[str] = "1" + supported_transport_ids: ClassVar[frozenset[str]] = frozenset( + {JointPositionTarget.TRANSPORT_ID} + ) + + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: object, + engine: object, + ) -> _RegisteredParallelSafety: + assert getattr(simulation, "get_robot")(getattr(robot, "uid")) is robot + assert scene_registry is not None + assert getattr(engine, "robot") is robot + return _RegisteredParallelSafety() + + +class _ReusedParallelSafetyFactory(_RegisteredParallelSafetyFactory): + """Invalid stateless factory that reuses one live validator singleton.""" + + validator_id: ClassVar[str] = "test.reused_parallel_safety" + _validator: ClassVar[_RegisteredParallelSafety] = _RegisteredParallelSafety() + + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: object, + engine: object, + ) -> _RegisteredParallelSafety: + del simulation, robot, scene_registry, engine + return self._validator + + +class _AlternatingReusedParallelSafetyFactory(_RegisteredParallelSafetyFactory): + """Invalid factory that hides A/B/A reuse behind alternating instances.""" + + validator_id: ClassVar[str] = "test.alternating_parallel_safety" + _validators: ClassVar[tuple[_RegisteredParallelSafety, ...]] = ( + _RegisteredParallelSafety(), + _RegisteredParallelSafety(), + ) + _next_index: ClassVar[int] = 0 + + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: object, + engine: object, + ) -> _RegisteredParallelSafety: + del simulation, robot, scene_registry, engine + validator = self._validators[self._next_index % len(self._validators)] + type(self)._next_index += 1 + return validator + + +def _profile_binding() -> SimulationRobotSkillProfileBinding: + """Build one motion-only profile with typed tracking policy.""" + return SimulationRobotSkillProfileBinding( + profile_id="robot_profile", + resources=( + ControlPartResourceBinding( + resource_id="manipulator", + endpoints=( + ControlPartEndpointBinding( + endpoint_id="motion", + control_part="arm", + capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + ), + ), + ), + ), + presets=( + SkillPolicyPreset( + "safe", + action_option_templates={ + "pick": PickUpOptions(), + "place": PlaceOptions(), + }, + motion_policy=MotionPolicy(sample_count=17), + tracking_policy=TrackingPolicy.joint_position( + in_flight_max_abs_error=0.037, + terminal_max_abs_error=0.019, + ), + runner_cfg=ExecutionRunnerCfg( + command_timeout=0.37, + safe_stop_timeout=0.61, + minimum_cycle_time=_UNALIGNED_PROFILE_DT, + hold_on_completion=False, + ), + ), + ), + default_preset="safe", + ) + + +def _mobile_profile_binding() -> SimulationRobotSkillProfileBinding: + """Build one pure custom-endpoint profile without a joint transport.""" + return SimulationRobotSkillProfileBinding( + profile_id="mobile_profile", + resources=( + RobotResourceBinding( + resource_id="mobile_base", + endpoints={ + "motion": _MobileEndpoint( + controller_id="base_velocity", + capabilities=frozenset({"motion.base.velocity"}), + ) + }, + ), + ), + presets=(SkillPolicyPreset("runtime", action_option_templates={}),), + default_preset="runtime", + ) + + +def _handover_profile_binding() -> SimulationRobotSkillProfileBinding: + """Declare two disjoint manipulators and one selected pose provider ID.""" + motion_capabilities = frozenset( + { + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } + ) + 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}_hand_commands", + ), + ), + ) + for side in ("left", "right") + ) + command_presets = tuple( + ControlPartCommandPreset( + preset_id=f"{side}_hand_commands", + control_part=f"{side}_hand", + commands={ + "open": (_HAND_OPEN_POSITION,), + "grasp": (_HAND_GRASP_POSITION,), + }, + ) + for side in ("left", "right") + ) + return SimulationRobotSkillProfileBinding( + profile_id="handover_profile", + resources=resources, + command_presets=command_presets, + defaults={ + "hand_over": {"source": "left", "destination": "right"}, + }, + presets=( + SkillPolicyPreset( + "safe", + action_option_templates={"hand_over": HandOverOptions()}, + ), + ), + default_preset="safe", + grounding_providers={ + "hand_over": _ForwardedHandOverPoseProvider.provider_id, + }, + ) + + +def _handover_helper_inputs() -> tuple[ + SimpleNamespace, + SimulationSceneBinding, + SimulationRobotSkillProfileBinding, +]: + """Build standard-helper inputs for one provider-aware HandOver program.""" + robot = _DualRobot() + cube = _RigidObject() + simulation = _Simulation(robot, {"cube_native": cube}) + environment = SimpleNamespace( + sim=simulation, + robot=robot, + step_dt=_STEP_DT, + ) + scene_binding = SimulationSceneBinding( + registry_id="handover_scene", + rigid_objects=( + SimulationRigidObjectBinding( + entity_id="cube", + simulation_uid="cube_native", + default_grasp_affordance="cube_grasp", + ), + ), + antipodal_grasps=( + AntipodalGraspAffordanceBinding( + entity_id="cube_grasp", + object_id="cube", + native_name="body", + revision="1", + ), + ), + ) + return environment, scene_binding, _handover_profile_binding() + + +def _handover_program() -> ExpertProgramCfg: + """Build one external-held-state HandOver call for static preflight.""" + return ExpertProgramCfg( + schema_version=1, + program_id="handover_preflight", + integration=ExpertProgramIntegrationCfg( + robot_profile="handover_profile", + scene_registry="handover_scene", + runtime_preset="safe", + ), + program=InvokeCfg(call=HandOverCfg(object="cube")), + ) + + +def _motion_generator(robot: _Robot) -> MotionGenerator: + """Build a type-checkable motion-generator test double.""" + generator = MagicMock(spec=MotionGenerator) + generator.robot = robot + generator.device = robot.device + generator.planner = SimpleNamespace(cfg=SimpleNamespace(planner_type="test")) + generator.collision_world_info = None + return generator + + +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=selected_profile_binding, + ), + step_dt=_STEP_DT, + motion_generator_factory=lambda: _motion_generator(robot), + ), + robot, + ) + + +def _mobile_factory() -> tuple[ + SimulationExpertProgramFactory, + SimulationExpertProgramRegistration, +]: + """Create one pure-custom standard factory and its exact registration.""" + robot = _MobileRobot() + simulation = _Simulation(robot) # type: ignore[arg-type] + registration = SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="mobile_scene"), + robot_profile_binding=_mobile_profile_binding(), + endpoint_adapters=(_MobileEndpointAdapter(),), + runtime_transports=(_MobileTransportEncoder(),), + ) + return ( + SimulationExpertProgramFactory( + simulation, # type: ignore[arg-type] + robot, # type: ignore[arg-type] + registration, + step_dt=_STEP_DT, + motion_generator_factory=lambda: _motion_generator(robot), # type: ignore[arg-type] + ), + registration, + ) + + +def _evidence_profile_binding() -> SimulationRobotSkillProfileBinding: + """Declare one manipulation resource with exact open/grasp semantics.""" + motion_capabilities = frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } + ) + return SimulationRobotSkillProfileBinding( + profile_id="evidence_profile", + resources=( + ControlPartResourceBinding( + resource_id="manipulator", + endpoints=( + ControlPartEndpointBinding( + endpoint_id="motion", + control_part="arm", + capabilities=motion_capabilities, + ), + ControlPartEndpointBinding( + endpoint_id="grasp", + control_part="hand", + capabilities=frozenset({GRASP_CAPABILITY}), + command_preset="hand_commands", + ), + ), + ), + ), + command_presets=( + ControlPartCommandPreset( + preset_id="hand_commands", + control_part="hand", + commands={ + "open": (_HAND_OPEN_POSITION,), + "grasp": (_HAND_GRASP_POSITION,), + }, + ), + ), + defaults={ + "pick_up": {"primary": "manipulator"}, + "place": {"primary": "manipulator"}, + }, + presets=( + SkillPolicyPreset( + "evidence", + action_option_templates={ + "pick": PickUpOptions(), + "place": PlaceOptions(), + }, + runner_cfg=ExecutionRunnerCfg( + minimum_cycle_time=0.0, + hold_on_completion=False, + ), + ), + ), + default_preset="evidence", + ) + + +def _pick_evidence_plan(action: Any, request: Any, context: Any) -> Any: + """Build one grasp frame and an identity object-to-endpoint expectation.""" + goal = action.require_goal(request) + trajectory = context.robot.qpos.unsqueeze(1).repeat(1, 2, 1) + trajectory[:, :, 1] = _HAND_GRASP_POSITION + relation = torch.eye(4).repeat(context.batch_size, 1, 1) + held = HeldObjectState( + semantics=goal.semantics, + object_to_eef=relation, + grasp_xpos=relation, + ) + return action.build_plan( + request, + context, + success=torch.ones(context.batch_size, dtype=torch.bool), + trajectory=TimedTrajectory.from_uniform_step( + trajectory, + env_ids=context.env_ids, + step_dt=context.require_control_dt(), + ), + expected_effects=StateDelta( + held_object_updates={"manipulator": held}, + ), + replannable=False, + segment_lengths={"close": 1, "lift": 1}, + scene_dependency_monitor_until={"cube": 0}, + ) + + +def _place_evidence_plan(action: Any, request: Any, context: Any) -> Any: + """Build one open frame and the matching held-object removal delta.""" + trajectory = context.robot.qpos.unsqueeze(1).repeat(1, 2, 1) + trajectory[:, :, 1] = _HAND_OPEN_POSITION + return action.build_plan( + request, + context, + success=torch.ones(context.batch_size, dtype=torch.bool), + trajectory=TimedTrajectory.from_uniform_step( + trajectory, + env_ids=context.env_ids, + step_dt=context.require_control_dt(), + ), + expected_effects=StateDelta( + held_object_updates={"manipulator": None}, + ), + replannable=False, + segment_lengths={"release": 1, "retract": 1}, + ) + + +def _evidence_integration() -> ExpertProgramIntegrationCfg: + """Return the host-owned integration shared by all frontend paths.""" + return ExpertProgramIntegrationCfg( + robot_profile="evidence_profile", + scene_registry="evidence_scene", + runtime_preset="evidence", + ) + + +def _evidence_adapter_runtime() -> tuple[ + ExpertProgramEnvironmentAdapter, + ExpertProgramRuntimeAssembly, + _EvidenceRobot, + _RigidObject, +]: + """Assemble the production adapter and Pick/Place evidence chain.""" + robot = _EvidenceRobot() + cube = _RigidObject() + simulation = _Simulation(robot, {"cube_native": cube}) + scene_binding = SimulationSceneBinding( + registry_id="evidence_scene", + rigid_objects=( + SimulationRigidObjectBinding( + entity_id="cube", + simulation_uid="cube_native", + default_grasp_affordance="cube_grasp", + ), + ), + antipodal_grasps=( + AntipodalGraspAffordanceBinding( + entity_id="cube_grasp", + object_id="cube", + native_name="body", + revision="1", + ), + ), + ) + factory = SimulationExpertProgramFactory( + simulation, # type: ignore[arg-type] + robot, # type: ignore[arg-type] + SimulationExpertProgramRegistration( + scene_binding=scene_binding, + robot_profile_binding=_evidence_profile_binding(), + ), + step_dt=_STEP_DT, + motion_generator_factory=lambda: _motion_generator(robot), + ) + adapter = factory.create_adapter() + assembly = adapter.assemble_runtime(_evidence_integration()) + pick_action = assembly.engine.actions["pick_up"] + place_action = assembly.engine.actions["place"] + pick_action._plan = MethodType(_pick_evidence_plan, pick_action) + place_action._plan = MethodType(_place_evidence_plan, place_action) + return adapter, assembly, robot, cube + + +def _evidence_runtime() -> tuple[ + ExpertProgramRuntimeAssembly, + _EvidenceRobot, + _RigidObject, +]: + """Assemble the production Pick/Place evidence chain on CPU fixtures.""" + _, assembly, robot, cube = _evidence_adapter_runtime() + return assembly, robot, cube + + +def _consume_buffered_action( + assembly: ExpertProgramRuntimeAssembly, + robot: _EvidenceRobot, +) -> None: + """Apply one accepted Gym action and advance the authoritative clock.""" + processed = assembly.command_sink.pop() + if not isinstance(processed.value, torch.Tensor): + raise TypeError("Joint-backed evidence actions must be tensors.") + robot.qpos = processed.value.clone() + assembly.clock.advance_after_env_step() + + +def _accept_hand_command( + assembly: ExpertProgramRuntimeAssembly, + robot: _EvidenceRobot, + position: float, +) -> None: + """Accept and consume one semantic hand command through the Gym sink.""" + assert assembly.command_sink.pending_count == 0 + frame = _hand_command_frame( + env_ids=tuple(range(_BATCH_SIZE)), + positions=(position,) * _BATCH_SIZE, + ) + acknowledgement = assembly.command_sink.send(frame, timeout=1.0) + assert acknowledgement.accepted + _consume_buffered_action(assembly, robot) + + +def _sample_effect( + assembly: ExpertProgramRuntimeAssembly, + robot: _EvidenceRobot, + *, + expected_trace_count: int, + advance_clock: bool = True, +) -> tuple[Any, SkillEffectTrace]: + """Advance one fresh environment tick and return its production trace.""" + if advance_clock: + assembly.clock.advance_after_env_step() + for _ in range(4): + result = assembly.runtime.step() + while assembly.command_sink.pending_count: + _consume_buffered_action(assembly, robot) + if len(result.effects) == expected_trace_count: + return result, result.effects[-1] + assert len(result.effects) < expected_trace_count + assembly.clock.advance_after_env_step() + raise AssertionError( + f"Expected {expected_trace_count} effect traces, got {len(result.effects)}." + ) + + +class _SynchronousEvidenceClock: + """Advance the fixture's simulation clock during standalone facade waits.""" + + def __init__(self, clock: EnvironmentStepClock) -> None: + self._clock = clock + + def now(self) -> float: + """Return the simulation fixture's authoritative time.""" + return self._clock.now() + + def sleep(self, duration: float) -> None: + """Advance the exact number of fixture ticks requested by the runner.""" + steps = self._clock.steps_for_duration(duration) + if steps: + self._clock.advance_after_env_step(steps) + + +class _ImmediateEvidenceCommandSink: + """Apply accepted endpoint frames immediately for standalone CPU execution.""" + + def __init__( + self, + assembly: ExpertProgramRuntimeAssembly, + robot: _EvidenceRobot, + cube: _RigidObject, + ) -> None: + self._encoder = assembly.command_encoder + self._observer = assembly.accepted_command_observer + self._clock = assembly.clock + self._robot = robot + self._cube = cube + + def send( + self, + command: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Apply one command and publish its accepted semantic hand state.""" + assert timeout > 0.0 + action = self._encoder.encode(command) + if not isinstance(action, torch.Tensor): + raise TypeError("The CPU quickstart fixture requires tensor actions.") + self._robot.qpos = action.clone() + if self._observer is None: + raise RuntimeError("The evidence fixture requires an accepted observer.") + self._observer.accepted(command.snapshot()) + if torch.allclose( + action[:, 1], + torch.full_like(action[:, 1], _HAND_OPEN_POSITION), + ): + self._cube.pose[:, 0, 3] = _RELEASE_SEPARATION + self._clock.advance_after_env_step() + return CommandAcknowledgement.accepted_ack() + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Apply the encoder's observed-position hold immediately.""" + assert timeout > 0.0 + action = self._encoder.encode_hold(targets, context) + if not isinstance(action, torch.Tensor): + raise TypeError("The CPU quickstart fixture requires tensor actions.") + self._robot.qpos = action.clone() + return CommandAcknowledgement.accepted_ack() + + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + """Clear accepted command evidence for cancelled destinations.""" + assert timeout > 0.0 + if self._observer is not None: + self._observer.cancelled(targets) + return CommandAcknowledgement.accepted_ack() + + +class _QuickstartRuntimeProvider: + """Explicit provider used by the public ``AtomicSkills.from_env`` path.""" + + def __init__(self, runtime: SkillRuntime) -> None: + self._runtime = runtime + self.presets: list[str] = [] + + def create_skill_runtime(self, *, preset: str) -> SkillRuntime: + """Return the configured canonical runtime and record preset selection.""" + self.presets.append(preset) + return self._runtime + + +def _quickstart_runtime_provider() -> _QuickstartRuntimeProvider: + """Build a synchronous provider from the shared production CPU fixture.""" + assembly, robot, cube = _evidence_runtime() + runtime = SkillRuntime.from_components( + assembly.compiler, + assembly.observation_provider, + _ImmediateEvidenceCommandSink(assembly, robot, cube), + assembly.evidence_collector, + clock=_SynchronousEvidenceClock(assembly.clock), + runner_cfg=ExecutionRunnerCfg( + minimum_cycle_time=0.0, + hold_on_completion=False, + ), + ) + return _QuickstartRuntimeProvider(runtime) + + +def _documented_pick_place_quickstart( + runtime_provider: _QuickstartRuntimeProvider, +) -> SkillResult: + """Run the application-facing quickstart, excluding scene construction.""" + skills = AtomicSkills.from_env(runtime_provider, preset="evidence") + cube = skills.scene.object("cube") + return skills.run( + Pick(object=cube), + Place(object=cube, at=_DIRECT_PLACE_TARGET), + ) + + +def _python_pick_place_calls() -> tuple[SemanticCallSpec, ...]: + """Return the application-facing calls used by both acceptance paths.""" + cube = SceneObjectRef("cube") + return ( + Pick(object=cube), + Place(object=cube, at=_DIRECT_PLACE_TARGET), + ) + + +def _pick_place_program_data() -> dict[str, object]: + """Return the integration-free program shared with the MLLM frontend.""" + return { + "schema_version": 1, + "program_id": "pick_place_equivalence", + "targets": { + "place_target": { + "kind": "cyclic_pose", + "values": [ + { + "position": _DIRECT_PLACE_TARGET.position.tolist(), + "quaternion_wxyz": ( + _DIRECT_PLACE_TARGET.quaternion_wxyz.tolist() + ), + } + ], + } + }, + "program": { + "kind": "sequence", + "items": [ + { + "kind": "invoke", + "call": {"kind": "pick", "object": "cube"}, + }, + { + "kind": "invoke", + "call": { + "kind": "place", + "object": "cube", + "at": { + "kind": "target_ref", + "target": "place_target", + }, + }, + }, + ], + }, + } + + +def _compiled_program_calls(program: CompiledProgram) -> tuple[SemanticCallSpec, ...]: + """Flatten one provider-free compiled program into semantic calls.""" + return tuple( + compiled_call.call for segment in program for compiled_call in segment.calls + ) + + +def _decoded_pick_place_calls( + adapter: ExpertProgramEnvironmentAdapter, +) -> tuple[SemanticCallSpec, ...]: + """Decode and compile the config equivalent of the Python calls.""" + data = _pick_place_program_data() + data["integration"] = { + "robot_profile": "evidence_profile", + "scene_registry": "evidence_scene", + "runtime_preset": "evidence", + } + return _compiled_program_calls(adapter.compile(decode_expert_program(data))) + + +def _mllm_pick_place_calls( + adapter: ExpertProgramEnvironmentAdapter, +) -> tuple[SemanticCallSpec, ...]: + """Compile the same program through the strict MLLM frontend.""" + program = compile_mllm_expert_program( + json.dumps(_pick_place_program_data()), + adapter=adapter, + integration=_evidence_integration(), + ) + return _compiled_program_calls(program) + + +def _capture_grounded_invocations( + monkeypatch: pytest.MonkeyPatch, + assembly: ExpertProgramRuntimeAssembly, +) -> list[ActionInvocation[Any, Any]]: + """Record the production compiler's final lowering without replacing it.""" + invocations: list[ActionInvocation[Any, Any]] = [] + ground = assembly.compiler.ground + + def recording_ground(*args: Any, **kwargs: Any) -> Any: + grounded = ground(*args, **kwargs) + invocations.append(grounded.invocation) + return grounded + + monkeypatch.setattr(assembly.compiler, "ground", recording_ground) + return invocations + + +def _run_evidence_pick_place( + assembly: ExpertProgramRuntimeAssembly, + robot: _EvidenceRobot, + cube: _RigidObject, + calls: tuple[SemanticCallSpec, ...], + *, + skills: AtomicSkills | None = None, +) -> tuple[SkillResult, HeldObjectState]: + """Drive one happy-path workflow through accepted commands and live evidence.""" + entry = assembly.runtime if skills is None else skills + result = entry.start(calls, workflow_id="pick_place_equivalence") + verified_pick: HeldObjectState | None = None + for _ in range(32): + while assembly.command_sink.pending_count: + _consume_buffered_action(assembly, robot) + if result.terminal: + break + if result.current_call_index == 1: + if verified_pick is None: + verified_pick = result.task_state.get_held_object("manipulator") + cube.pose[:, 0, 3] = _RELEASE_SEPARATION + assembly.clock.advance_after_env_step() + result = entry.step() + + assert result.status is SkillStatus.COMPLETED + assert verified_pick is not None + assert result.task_state.get_held_object("manipulator") is None + assert { + (effect.call_index, effect.gate_id, effect.segment_name) + for effect in result.effects + if effect.boundary_kind == "phase_effect_gate" + } == { + (0, "destination_acquired", "lift"), + (1, "source_released", "retract"), + } + return result, verified_pick + + +def _assert_typed_equivalent( + actual: object, + expected: object, + *, + path: str = "value", +) -> None: + """Compare nested typed compiler output, including owned tensor values.""" + assert type(actual) is type(expected), path + if isinstance(actual, torch.Tensor): + assert isinstance(expected, torch.Tensor) + torch.testing.assert_close(actual, expected) + return + if isinstance(actual, Mapping): + assert isinstance(expected, Mapping) + assert tuple(actual) == tuple(expected) + for key in actual: + _assert_typed_equivalent( + actual[key], + expected[key], + path=f"{path}[{key!r}]", + ) + return + if isinstance(actual, Sequence) and not isinstance(actual, (str, bytes)): + assert isinstance(expected, Sequence) + assert len(actual) == len(expected) + for index, (actual_item, expected_item) in enumerate( + zip(actual, expected, strict=True) + ): + _assert_typed_equivalent( + actual_item, + expected_item, + path=f"{path}[{index}]", + ) + return + if is_dataclass(actual) and not isinstance(actual, type): + assert is_dataclass(expected) and not isinstance(expected, type) + for data_field in fields(actual): + _assert_typed_equivalent( + getattr(actual, data_field.name), + getattr(expected, data_field.name), + path=f"{path}.{data_field.name}", + ) + return + assert actual == expected, path + + +def _assert_invocation_equivalent( + actual: ActionInvocation[Any, Any], + expected: ActionInvocation[Any, Any], +) -> None: + """Compare semantic lowering while ignoring engine-instance owner UUIDs.""" + assert actual.skill_id == expected.skill_id + assert actual.invocation_id == expected.invocation_id + assert actual.revision == expected.revision + _assert_typed_equivalent(actual.goal, expected.goal, path="invocation.goal") + _assert_typed_equivalent( + actual.binding.endpoints, + expected.binding.endpoints, + path="invocation.binding.endpoints", + ) + _assert_typed_equivalent( + actual.motion_policy, + expected.motion_policy, + path="invocation.motion_policy", + ) + _assert_typed_equivalent( + actual.recovery_policy, + expected.recovery_policy, + path="invocation.recovery_policy", + ) + _assert_typed_equivalent( + actual.skill_options, + expected.skill_options, + path="invocation.skill_options", + ) + _assert_typed_equivalent( + actual.control_overrides, + expected.control_overrides, + path="invocation.control_overrides", + ) + + +def test_simulation_factory_aligns_runner_policy_to_gym_step() -> None: + """Runner cadence lowering preserves source declarations and policy.""" + binding = _profile_binding() + base_preset = binding.presets[0] + source_preset = SkillPolicyPreset( + base_preset.preset_id, + schema_version=base_preset.schema_version, + action_option_templates=base_preset.action_option_templates, + motion_policy=base_preset.motion_policy, + tracking_policy=base_preset.tracking_policy, + recovery_policy=base_preset.recovery_policy, + workflow_recovery_policy=WorkflowRecoveryPolicy( + max_recovery_attempts=2, + ), + runner_cfg=base_preset.runner_cfg, + effect_monitors=base_preset.effect_monitors, + ) + binding = replace(binding, presets=(source_preset,)) + source_runner_cfg = source_preset.runner_cfg + factory, _ = _factory(binding) + + profile = factory.create_robot_skill_profile() + + aligned_preset = profile.presets["safe"] + aligned_runner_cfg = aligned_preset.runner_cfg + assert aligned_preset.motion_policy.sample_count == 17 + 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 aligned_preset.workflow_recovery_policy.max_recovery_attempts == 2 + assert binding.presets[0].motion_policy.sample_count == 17 + 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( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """All public frontends reach equivalent invocations and verified state.""" + ( + _, + python_assembly, + python_robot, + python_cube, + ) = _evidence_adapter_runtime() + ( + config_adapter, + config_assembly, + config_robot, + config_cube, + ) = _evidence_adapter_runtime() + ( + mllm_adapter, + mllm_assembly, + mllm_robot, + mllm_cube, + ) = _evidence_adapter_runtime() + python_invocations = _capture_grounded_invocations(monkeypatch, python_assembly) + config_invocations = _capture_grounded_invocations(monkeypatch, config_assembly) + mllm_invocations = _capture_grounded_invocations(monkeypatch, mllm_assembly) + runtime_provider = _QuickstartRuntimeProvider(python_assembly.runtime) + python_skills = AtomicSkills.from_env(runtime_provider, preset="evidence") + + python_result, python_held = _run_evidence_pick_place( + python_assembly, + python_robot, + python_cube, + _python_pick_place_calls(), + skills=python_skills, + ) + config_result, config_held = _run_evidence_pick_place( + config_assembly, + config_robot, + config_cube, + _decoded_pick_place_calls(config_adapter), + ) + mllm_result, mllm_held = _run_evidence_pick_place( + mllm_assembly, + mllm_robot, + mllm_cube, + _mllm_pick_place_calls(mllm_adapter), + ) + + assert runtime_provider.presets == ["evidence"] + assert ( + len(python_invocations) == len(config_invocations) == len(mllm_invocations) == 2 + ) + for python_invocation, config_invocation, mllm_invocation in zip( + python_invocations, + config_invocations, + mllm_invocations, + strict=True, + ): + _assert_invocation_equivalent(python_invocation, config_invocation) + _assert_invocation_equivalent(python_invocation, mllm_invocation) + _assert_typed_equivalent(python_held, config_held) + _assert_typed_equivalent(python_held, mllm_held) + _assert_typed_equivalent(python_result, config_result) + _assert_typed_equivalent(python_result, mllm_result) + + +def test_atomic_skills_from_env_runs_documented_pick_place_quickstart() -> None: + """The small public facade executes without exposing core motion plumbing.""" + provider = _quickstart_runtime_provider() + + result = _documented_pick_place_quickstart(provider) + + source = textwrap.dedent(inspect.getsource(_documented_pick_place_quickstart)) + function = ast.parse(source).body[0] + assert isinstance(function, ast.FunctionDef) + executable = function.body[1:] # Exclude the helper's docstring. + assert executable[-1].end_lineno is not None + assert executable[-1].end_lineno - executable[0].lineno + 1 <= ( + _QUICKSTART_MAX_LINES + ) + identifiers = { + identifier + for node in ast.walk(function) + for identifier in ( + node.id if isinstance(node, ast.Name) else None, + node.attr if isinstance(node, ast.Attribute) else None, + ) + if identifier is not None + } + assert identifiers.isdisjoint( + { + "qpos", + "matrix", + "planner", + "session", + "MotionGenerator", + "PlanningContext", + "ExecutionSession", + } + ) + assert provider.presets == ["evidence"] + assert result.status is SkillStatus.COMPLETED + assert result.success_mask.tolist() == [True] * _BATCH_SIZE + assert [call.semantic_id for call in result.calls] == ["pick", "place"] + assert result.task_state.get_held_object("manipulator") is None + + +def test_simulation_factory_builds_shared_observation_and_evidence_ports() -> None: + """Observation and both built-in evidence providers share one scene source.""" + factory, robot = _factory() + registry = factory.create_scene_registry() + profile = factory.create_robot_skill_profile() + engine = factory.create_atomic_action_engine(profile) + clock = EnvironmentStepClock(_STEP_DT) + + observation = factory.create_planning_observation_provider( + scene_registry=registry, + engine=engine, + clock=clock, + ) + assert type(observation) is SimulationPlanningObservationProvider + context = observation.observe(TaskState.empty(_BATCH_SIZE, robot.device)) + providers = tuple( + factory.create_effect_evidence_providers( + scene_registry=registry, + engine=engine, + observation_provider=observation, + ) + ) + accepted_command_observer = factory.create_accepted_runtime_command_observer( + scene_registry=registry, + engine=engine, + observation_provider=observation, + ) + + assert context.robot.timestamp == pytest.approx(0.0) + assert torch.equal(observation.current_qpos(context.env_ids), robot.qpos) + assert accepted_command_observer is observation.command_state_tracker + assert len(providers) == 2 + assert all( + getattr(provider, "_scene_provider") is observation.scene_provider + for provider in providers + ) + + +def test_simulation_factory_returns_exact_environment_adapter() -> None: + """The convenience path remains compatible with the exact-type mixin check.""" + factory, _ = _factory() + + adapter = factory.create_adapter() + + assert type(adapter) is ExpertProgramEnvironmentAdapter + assert adapter.step_dt == pytest.approx(_STEP_DT) + assert factory.segment_policy_port is not None + + +@pytest.mark.parametrize( + "override", + ( + {"call_catalog": object()}, + {"endpoint_adapters": {}}, + {"registered_lowerers": (object(),)}, + {"relation_grounders": (object(),)}, + {"handover_pose_providers": (object(),)}, + {"effect_monitor_registry": object()}, + {"runtime_transports": (object(),)}, + {"runner_cfg": ExecutionRunnerCfg()}, + {"post_policy_port": object()}, + {"validator_port": object()}, + {"parallel_safety_validator": object()}, + ), +) +def test_standard_registration_rejects_runtime_side_channel_overrides( + override: dict[str, object], +) -> None: + """The exact registration is the standard path's only extension owner.""" + factory, _ = _factory() + + with pytest.raises(ValueError, match="external overrides are forbidden"): + ExpertProgramEnvironmentAdapter( + factory, + step_dt=_STEP_DT, + registration=factory.expert_program_registration, + **override, + ) + + +def test_registration_owning_factory_rejects_catalog_only_adapter() -> None: + """A standard factory cannot be rewrapped through the advanced catalog seam.""" + factory, _ = _factory() + + with pytest.raises(ValueError, match="catalog-only"): + ExpertProgramEnvironmentAdapter( + factory, + step_dt=_STEP_DT, + integration_catalog=factory.expert_program_registration.catalog, + ) + + +def test_standard_registration_rejects_integration_catalog_override() -> None: + """Even the owner's catalog cannot be resupplied beside exact registration.""" + factory, _ = _factory() + registration = factory.expert_program_registration + + with pytest.raises(ValueError, match="cannot override"): + ExpertProgramEnvironmentAdapter( + factory, + step_dt=_STEP_DT, + registration=registration, + integration_catalog=registration.catalog, + ) + + +def test_registration_owning_factory_rejects_equivalent_registration_object() -> None: + """Equal IDs and fingerprint cannot substitute for the factory-owned object.""" + factory, _ = _factory() + owned = factory.expert_program_registration + equivalent = SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="scene"), + robot_profile_binding=_profile_binding(), + ) + assert equivalent is not owned + assert equivalent.fingerprint == owned.fingerprint + + with pytest.raises(ValueError, match="exact object owned by the factory"): + ExpertProgramEnvironmentAdapter( + factory, + step_dt=_STEP_DT, + registration=equivalent, + ) + + +def test_adapter_rejects_factory_registration_ownership_drift() -> None: + """A factory cannot replace its registration after adapter construction.""" + factory, _ = _factory() + adapter = factory.create_adapter() + equivalent = SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="scene"), + robot_profile_binding=_profile_binding(), + ) + factory._registration = equivalent + + with pytest.raises(IntegrationFingerprintMismatch, match="ownership changed"): + adapter.assemble_runtime( + ExpertProgramIntegrationCfg( + robot_profile="robot_profile", + scene_registry="scene", + runtime_preset="safe", + ) + ) + + +def test_adapter_rejects_engine_bound_to_equivalent_profile_object() -> None: + """The engine must bind the exact profile object validated by the adapter.""" + factory, _ = _factory() + original_create_engine = factory.create_atomic_action_engine + + def create_with_different_profile( + owner: SimulationExpertProgramFactory, + profile: Any, + ) -> Any: + replacement = owner.create_robot_skill_profile() + assert replacement is not profile + return original_create_engine(replacement) + + factory.create_atomic_action_engine = MethodType( + create_with_different_profile, + factory, + ) + + with pytest.raises( + IntegrationFingerprintMismatch, + match="different robot profile object", + ): + factory.create_adapter().assemble_runtime( + ExpertProgramIntegrationCfg( + robot_profile="robot_profile", + scene_registry="scene", + runtime_preset="safe", + ) + ) + + +def test_standard_factory_uses_preset_runner_and_fresh_registered_safety() -> None: + """Live assembly consumes preset policy and creates no shared safety gate.""" + robot = _Robot() + simulation = _Simulation(robot) + registration = SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="scene"), + robot_profile_binding=_profile_binding(), + parallel_safety_factory=_RegisteredParallelSafetyFactory(), + ) + factory = SimulationExpertProgramFactory( + simulation, # type: ignore[arg-type] + robot, # type: ignore[arg-type] + registration, + step_dt=_STEP_DT, + motion_generator_factory=lambda: _motion_generator(robot), + ) + adapter = factory.create_adapter() + integration = ExpertProgramIntegrationCfg( + robot_profile="robot_profile", + scene_registry="scene", + runtime_preset="safe", + ) + + first = adapter.assemble_runtime(integration) + second = adapter.assemble_runtime(integration) + + assert first.runner_cfg.command_timeout == pytest.approx(0.37) + assert first.runner_cfg.safe_stop_timeout == pytest.approx(0.61) + assert first.runner_cfg.minimum_cycle_time == pytest.approx(0.04) + assert first.runner_cfg.hold_on_completion is False + assert type(first.parallel_safety_validator) is _RegisteredParallelSafety + assert type(second.parallel_safety_validator) is _RegisteredParallelSafety + assert first.parallel_safety_validator is not second.parallel_safety_validator + + +def test_standard_factory_rejects_reused_live_safety_validator() -> None: + """A declarative factory cannot leak one validator across runtime assemblies.""" + robot = _Robot() + simulation = _Simulation(robot) + registration = SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="scene"), + robot_profile_binding=_profile_binding(), + parallel_safety_factory=_ReusedParallelSafetyFactory(), + ) + factory = SimulationExpertProgramFactory( + simulation, # type: ignore[arg-type] + robot, # type: ignore[arg-type] + registration, + step_dt=_STEP_DT, + motion_generator_factory=lambda: _motion_generator(robot), + ) + adapter = factory.create_adapter() + integration = ExpertProgramIntegrationCfg( + robot_profile="robot_profile", + scene_registry="scene", + runtime_preset="safe", + ) + adapter.assemble_runtime(integration) + + with pytest.raises(ValueError, match="fresh validator"): + adapter.assemble_runtime(integration) + + +def test_registration_rejects_a_b_a_safety_reuse_across_factories() -> None: + """Freshness history belongs to the registration rather than one factory.""" + robot = _Robot() + simulation = _Simulation(robot) + _AlternatingReusedParallelSafetyFactory._next_index = 0 + registration = SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="scene"), + robot_profile_binding=_profile_binding(), + parallel_safety_factory=_AlternatingReusedParallelSafetyFactory(), + ) + factories = tuple( + SimulationExpertProgramFactory( + simulation, # type: ignore[arg-type] + robot, # type: ignore[arg-type] + registration, + step_dt=_STEP_DT, + motion_generator_factory=lambda: _motion_generator(robot), + ) + for _ in range(3) + ) + integration = ExpertProgramIntegrationCfg( + robot_profile="robot_profile", + scene_registry="scene", + runtime_preset="safe", + ) + + factories[0].create_adapter().assemble_runtime(integration) + factories[1].create_adapter().assemble_runtime(integration) + + with pytest.raises(ValueError, match="fresh validator"): + factories[2].create_adapter().assemble_runtime(integration) + + +def test_standard_simulation_helper_has_no_live_extension_override_parameters() -> None: + """Task code can select only its immutable registration on the standard path.""" + parameters = inspect.signature(create_simulation_expert_program_adapter).parameters + + assert { + "endpoint_adapters", + "runtime_transports", + "contact_observer", + "constraint_observer", + "force_observer", + "wrench_observer", + "parallel_safety_validator", + }.isdisjoint(parameters) + + +def test_simulation_helper_consumes_registered_semantic_grounding_extensions() -> None: + """Both registration-owned grounding seams reach the compiler unchanged.""" + robot = _Robot() + environment = SimpleNamespace( + sim=_Simulation(robot), + robot=robot, + step_dt=_STEP_DT, + ) + relation_grounder = _ForwardedRelationGrounder() + handover_provider = _ForwardedHandOverPoseProvider() + adapter = create_simulation_expert_program_adapter( + environment, # type: ignore[arg-type] + registration=SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="scene"), + robot_profile_binding=_profile_binding(), + relation_grounders=(relation_grounder,), + handover_pose_providers=(handover_provider,), + ), + motion_generator_factory=lambda: _motion_generator(robot), + ) + + assembly = adapter.assemble_runtime( + ExpertProgramIntegrationCfg( + robot_profile="robot_profile", + scene_registry="scene", + runtime_preset="safe", + ) + ) + + assert tuple(assembly.compiler.relation_grounders.values()) == (relation_grounder,) + assert tuple(assembly.compiler.handover_pose_providers.values()) == ( + handover_provider, + ) + + +def test_handover_registration_is_fail_closed_without_selected_provider() -> None: + """A profile-selected provider must be installed before simulation startup.""" + _, scene_binding, profile_binding = _handover_helper_inputs() + + with pytest.raises(ValueError, match="selects handover pose provider"): + SimulationExpertProgramRegistration( + scene_binding=scene_binding, + robot_profile_binding=profile_binding, + ) + + +def test_simulation_helper_uses_registered_handover_provider_for_preflight() -> None: + """A registration-owned embodiment provider satisfies standard preflight.""" + environment, scene_binding, profile_binding = _handover_helper_inputs() + robot = environment.robot + provider = _ForwardedHandOverPoseProvider() + adapter = create_simulation_expert_program_adapter( + environment, # type: ignore[arg-type] + registration=SimulationExpertProgramRegistration( + scene_binding=scene_binding, + robot_profile_binding=profile_binding, + handover_pose_providers=(provider,), + ), + motion_generator_factory=lambda: _motion_generator(robot), + ) + + bridge = adapter.create_bridge(adapter.compile(_handover_program())) + + assert bridge is not None + + +def test_simulation_helper_assembles_mobile_endpoint_and_transport_without_joints() -> ( + None +): + """The one-line factory path supports a custom non-joint controller.""" + robot = _MobileRobot() + simulation = _Simulation(robot) # type: ignore[arg-type] + registration = SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="mobile_scene"), + robot_profile_binding=_mobile_profile_binding(), + endpoint_adapters=(_MobileEndpointAdapter(),), + runtime_transports=(_MobileTransportEncoder(),), + ) + environment = SimpleNamespace( + sim=simulation, + robot=robot, + step_dt=_STEP_DT, + ) + + adapter = create_simulation_expert_program_adapter( + environment, # type: ignore[arg-type] + registration=registration, + motion_generator_factory=lambda: _motion_generator(robot), # type: ignore[arg-type] + ) + assembly = adapter.assemble_runtime( + ExpertProgramIntegrationCfg( + robot_profile="mobile_profile", + scene_registry="mobile_scene", + runtime_preset="runtime", + ) + ) + + endpoint = assembly.robot_profile.resources["mobile_base"].endpoints["motion"] + assert isinstance(endpoint, _MobileEndpoint) + assert assembly.command_encoder.transport_ids == (_MobileTarget.TRANSPORT_ID,) + assert assembly.command_encoder.is_frozen + with pytest.raises(RuntimeError, match="registration is frozen"): + assembly.command_encoder.register_transport( + _MobileTransportEncoder(), + replace=True, + ) + assert assembly.engine.skill_profile is not None + resolved = assembly.engine.skill_profile.resources["mobile_base"] + assert isinstance(resolved.endpoints["motion"].runtime_target, _MobileTarget) + assert resolved.claim.claim_tokens == frozenset({"controller:base_velocity"}) + context = assembly.observation_provider.observe( + TaskState.empty(_BATCH_SIZE, robot.device) + ) + values = torch.linspace(0.1, 0.2, _BATCH_SIZE) + action = assembly.command_encoder.encode( + RuntimeCommandFrame( + commands=( + EndpointCommand( + _MobileTarget("base_velocity"), + _MobilePayload(values), + ), + ), + active_mask=torch.ones(_BATCH_SIZE, dtype=torch.bool), + env_ids=context.env_ids, + hold_duration=torch.full((_BATCH_SIZE,), _STEP_DT), + ) + ) + torch.testing.assert_close(action[:, 0], values) + + +@pytest.mark.parametrize( + "drift", + ("missing_resource", "extra_resource", "missing_endpoint", "extra_endpoint"), +) +def test_catalog_rejects_live_resource_and_endpoint_coverage_drift( + drift: str, +) -> None: + """Live bound topology must cover the registered profile exactly.""" + factory, registration = _mobile_factory() + assembly = factory.create_adapter().assemble_runtime( + ExpertProgramIntegrationCfg( + robot_profile="mobile_profile", + scene_registry="mobile_scene", + runtime_preset="runtime", + ) + ) + bound_profile = assembly.engine.skill_profile + assert bound_profile is not None + resources = dict(bound_profile.resources) + resource = resources["mobile_base"] + if drift == "missing_resource": + resources.pop("mobile_base") + elif drift == "extra_resource": + resources["extra"] = replace( + resource, + resource_id="extra", + claim=replace( + resource.claim, + leaf_resource_ids=frozenset({"extra"}), + ), + ) + elif drift == "missing_endpoint": + resources["mobile_base"] = replace( + resource, + endpoints={}, + claim=replace( + resource.claim, + joint_ids=(), + claim_tokens=frozenset(), + ), + ) + else: + endpoints = dict(resource.endpoints) + endpoints["extra"] = endpoints["motion"] + resources["mobile_base"] = replace(resource, endpoints=endpoints) + bound_profile._resources = MappingProxyType(resources) + + with pytest.raises(IntegrationFingerprintMismatch, match="IDs differ"): + registration.catalog.validate_bound_endpoint_extensions(bound_profile) + + +@pytest.mark.parametrize( + ("route", "message"), + (("tracking", "tracking address"), ("evidence", "effect-evidence address")), +) +def test_catalog_rejects_control_part_live_route_address_drift( + route: str, + message: str, +) -> None: + """Built-in route IDs cannot hide a different target or evidence address.""" + factory, _ = _factory() + registration = factory.expert_program_registration + assembly = factory.create_adapter().assemble_runtime( + ExpertProgramIntegrationCfg( + robot_profile="robot_profile", + scene_registry="scene", + runtime_preset="safe", + ) + ) + bound_profile = assembly.engine.skill_profile + assert bound_profile is not None + resources = dict(bound_profile.resources) + resource = resources["manipulator"] + endpoints = dict(resource.endpoints) + endpoint = endpoints["motion"] + if route == "tracking": + tracking = endpoint.tracking_channels[JOINT_POSITION_CHANNEL] + wrong_target = JointPositionTarget( + "different_arm", + endpoint.runtime_target.joint_ids, + ) + wrong_tracking = EndpointTrackingChannelBinding( + JOINT_POSITION_CHANNEL, + TrackingFeedbackSourceRef( + tracking.source.provider_id, + tracking.source.revision, + EndpointTrackingFeedbackAddress( + wrong_target, + JOINT_POSITION_CHANNEL, + ), + ), + tracking.projector, + ) + endpoints["motion"] = replace( + endpoint, + tracking_channels={JOINT_POSITION_CHANNEL: wrong_tracking}, + ) + else: + effect_sources = dict(endpoint.effect_sources) + channel = next(iter(effect_sources)) + source = effect_sources[channel] + effect_sources[channel] = EffectEvidenceSourceRef( + source.provider_id, + source.revision, + ControlPartEvidenceAddress("different_arm", channel), + ) + endpoints["motion"] = replace( + endpoint, + effect_sources=effect_sources, + ) + resources["manipulator"] = replace(resource, endpoints=endpoints) + bound_profile._resources = MappingProxyType(resources) + + with pytest.raises(IntegrationFingerprintMismatch, match=message): + registration.catalog.validate_bound_endpoint_extensions(bound_profile) + + +def test_standard_factory_rejects_adapter_live_target_declaration_drift() -> None: + """A lying adapter cannot emit a target absent from its catalog declaration.""" + robot = _MobileRobot() + simulation = _Simulation(robot) # type: ignore[arg-type] + registration = SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="mobile_scene"), + robot_profile_binding=_mobile_profile_binding(), + endpoint_adapters=(_LyingMobileEndpointAdapter(),), + runtime_transports=(_MobileTransportEncoder(),), + ) + factory = SimulationExpertProgramFactory( + simulation, # type: ignore[arg-type] + robot, # type: ignore[arg-type] + registration, + step_dt=_STEP_DT, + motion_generator_factory=lambda: _motion_generator(robot), # type: ignore[arg-type] + ) + + with pytest.raises( + IntegrationFingerprintMismatch, + match="undeclared exact runtime target type", + ): + factory.create_adapter().assemble_runtime( + ExpertProgramIntegrationCfg( + robot_profile="mobile_profile", + scene_registry="mobile_scene", + runtime_preset="runtime", + ) + ) + + +def test_standard_factory_rejects_target_live_transport_declaration_drift() -> None: + """A target instance cannot contradict its statically registered transport.""" + robot = _MobileRobot() + simulation = _Simulation(robot) # type: ignore[arg-type] + registration = SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="mobile_scene"), + robot_profile_binding=_mobile_profile_binding(), + endpoint_adapters=(_LyingTransportMobileEndpointAdapter(),), + runtime_transports=(_LyingTargetMobileTransportEncoder(),), + ) + factory = SimulationExpertProgramFactory( + simulation, # type: ignore[arg-type] + robot, # type: ignore[arg-type] + registration, + step_dt=_STEP_DT, + motion_generator_factory=lambda: _motion_generator(robot), # type: ignore[arg-type] + ) + + with pytest.raises(IntegrationFingerprintMismatch, match="live transport"): + factory.create_adapter().assemble_runtime( + ExpertProgramIntegrationCfg( + robot_profile="mobile_profile", + scene_registry="mobile_scene", + runtime_preset="runtime", + ) + ) + + +@pytest.mark.parametrize( + ("endpoint_adapter", "message"), + ( + (_LyingAdapterIdMobileEndpointAdapter(), "adapter ID"), + (_LyingFeedbackMobileEndpointAdapter(), "tracking-feedback routes"), + (_LyingEvidenceMobileEndpointAdapter(), "effect-evidence routes"), + ), +) +def test_standard_factory_rejects_adapter_live_route_declaration_drift( + endpoint_adapter: ResourceEndpointAdapter, + message: str, +) -> None: + """Every live adapter identity and provider route must match its fingerprint.""" + robot = _MobileRobot() + simulation = _Simulation(robot) # type: ignore[arg-type] + registration = SimulationExpertProgramRegistration( + scene_binding=SimulationSceneBinding(registry_id="mobile_scene"), + robot_profile_binding=_mobile_profile_binding(), + endpoint_adapters=(endpoint_adapter,), + runtime_transports=(_MobileTransportEncoder(),), + ) + factory = SimulationExpertProgramFactory( + simulation, # type: ignore[arg-type] + robot, # type: ignore[arg-type] + registration, + step_dt=_STEP_DT, + motion_generator_factory=lambda: _motion_generator(robot), # type: ignore[arg-type] + ) + + with pytest.raises(IntegrationFingerprintMismatch, match=message): + factory.create_adapter().assemble_runtime( + ExpertProgramIntegrationCfg( + robot_profile="mobile_profile", + scene_registry="mobile_scene", + runtime_preset="runtime", + ) + ) + + +def test_pick_place_effects_require_accepted_hand_state_and_live_pose() -> None: + """Terminal Pick/Place evidence stays conjunctive through runtime traces.""" + assembly, robot, cube = _evidence_runtime() + + def without_phase_gates( + self: Any, + analyzed: Any, + effect_spec: Any, + *, + path: tuple[Any, ...], + ) -> tuple[()]: + del self, analyzed, effect_spec, path + return () + + def without_in_flight_guards( + self: Any, + analyzed: Any, + effect_spec: Any, + context: Any, + *, + path: tuple[Any, ...], + ) -> tuple[()]: + del self, analyzed, effect_spec, context, path + return () + + assembly.compiler._ground_phase_effect_gates = MethodType( + without_phase_gates, + assembly.compiler, + ) + assembly.compiler._ground_held_object_guards = MethodType( + without_in_flight_guards, + assembly.compiler, + ) + assert type(assembly.accepted_command_observer) is ( + ControlCommandStateEvidenceTracker + ) + cube.pose[:, 0, 3] = 0.2 + result = assembly.runtime.start( + ( + Pick(object=SceneObjectRef("cube")), + Place( + object=SceneObjectRef("cube"), + at=SemanticPose( + position=(0.0, 0.0, 0.0), + quaternion_wxyz=(1.0, 0.0, 0.0, 0.0), + ), + ), + ), + workflow_id="production_evidence_chain", + ) + assert result.status is SkillStatus.RUNNING + + result = assembly.runtime.step() + assert assembly.command_sink.pending_count == 1 + assert len(result.effects) == 0 + _consume_buffered_action(assembly, robot) + result = assembly.runtime.step() + assert len(result.effects) == 0 + while assembly.command_sink.pending_count: + _consume_buffered_action(assembly, robot) + + result, pick_pose_missing = _sample_effect( + assembly, + robot, + expected_trace_count=1, + ) + pick_pose = pick_pose_missing.evidence["destination.pose"] + pick_constraint = pick_pose_missing.evidence["destination.constraint"] + assert type(pick_pose) is PoseRelationEvidenceBatch + assert type(pick_constraint) is BinaryEffectEvidenceBatch + assert pick_pose.object_to_endpoint[:, 0, 3].tolist() == pytest.approx( + [-0.2] * _BATCH_SIZE + ) + assert pick_constraint.values.tolist() == [True] * _BATCH_SIZE + assert pick_constraint.valid.tolist() == [True] * _BATCH_SIZE + assert not pick_pose_missing.success_mask.any() + + cube.pose = torch.eye(4).repeat(_BATCH_SIZE, 1, 1) + assembly.command_sink.discard_pending() + result, pick_command_missing = _sample_effect( + assembly, + robot, + expected_trace_count=2, + ) + pick_pose = pick_command_missing.evidence["destination.pose"] + pick_constraint = pick_command_missing.evidence["destination.constraint"] + torch.testing.assert_close( + pick_pose.object_to_endpoint, + torch.eye(4).repeat(_BATCH_SIZE, 1, 1), + ) + assert pick_constraint.valid.tolist() == [False] * _BATCH_SIZE + assert not pick_command_missing.success_mask.any() + + _accept_hand_command(assembly, robot, _HAND_GRASP_POSITION) + result, pick_first_complete_sample = _sample_effect( + assembly, + robot, + expected_trace_count=3, + advance_clock=False, + ) + assert not pick_first_complete_sample.success_mask.any() + result, pick_success = _sample_effect( + assembly, + robot, + expected_trace_count=4, + ) + assert pick_success.call_index == 0 + assert pick_success.effect_spec.semantic_id == "pick" + assert pick_success.success_mask.tolist() == [True] * _BATCH_SIZE + assert ( + pick_success.evidence["destination.constraint"].values.tolist() + == [True] * _BATCH_SIZE + ) + assert result.task_state.get_held_object("manipulator") is not None + assert result.current_call_index == 1 + + result = assembly.runtime.step() + assert assembly.command_sink.pending_count == 1 + assert len(result.effects) == 4 + _consume_buffered_action(assembly, robot) + result = assembly.runtime.step() + assert len(result.effects) == 4 + while assembly.command_sink.pending_count: + _consume_buffered_action(assembly, robot) + + result, place_pose_missing = _sample_effect( + assembly, + robot, + expected_trace_count=5, + ) + place_pose = place_pose_missing.evidence["source.pose"] + place_constraint = place_pose_missing.evidence["source.constraint"] + assert type(place_pose) is PoseRelationEvidenceBatch + assert type(place_constraint) is BinaryEffectEvidenceBatch + torch.testing.assert_close( + place_pose.object_to_endpoint, + torch.eye(4).repeat(_BATCH_SIZE, 1, 1), + ) + assert place_constraint.values.tolist() == [False] * _BATCH_SIZE + assert place_constraint.valid.tolist() == [True] * _BATCH_SIZE + assert not place_pose_missing.success_mask.any() + + cube.pose[:, 0, 3] = 0.2 + assembly.command_sink.discard_pending() + result, place_command_missing = _sample_effect( + assembly, + robot, + expected_trace_count=6, + ) + place_pose = place_command_missing.evidence["source.pose"] + place_constraint = place_command_missing.evidence["source.constraint"] + assert place_pose.object_to_endpoint[:, 0, 3].tolist() == pytest.approx( + [-0.2] * _BATCH_SIZE + ) + assert place_constraint.valid.tolist() == [False] * _BATCH_SIZE + assert not place_command_missing.success_mask.any() + + _accept_hand_command(assembly, robot, _HAND_OPEN_POSITION) + result, place_first_complete_sample = _sample_effect( + assembly, + robot, + expected_trace_count=7, + advance_clock=False, + ) + assert not place_first_complete_sample.success_mask.any() + result, place_success = _sample_effect( + assembly, + robot, + expected_trace_count=8, + ) + assert result.status is SkillStatus.COMPLETED + assert place_success.call_index == 1 + assert place_success.effect_spec.semantic_id == "place" + assert place_success.success_mask.tolist() == [True] * _BATCH_SIZE + assert ( + place_success.evidence["source.constraint"].values.tolist() + == [False] * _BATCH_SIZE + ) + assert result.task_state.get_held_object("manipulator") is None + assert [len(call.effects) for call in result.calls] == [4, 4] + assert assembly.command_sink.accepted_action_count >= 4 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_parallel_safety.py b/tests/gym/envs/expert_program/test_simulation_parallel_safety.py new file mode 100644 index 000000000..ce037af0c --- /dev/null +++ b/tests/gym/envs/expert_program/test_simulation_parallel_safety.py @@ -0,0 +1,238 @@ +# ---------------------------------------------------------------------------- +# 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 production cuRobo parallel-command safety gate.""" + +from __future__ import annotations + +from types import MethodType, SimpleNamespace + +import pytest +import torch + +from embodichain.lab.gym.envs.expert_program import ( + CuroboParallelCommandSafetyValidator, + CuroboParallelSafetyValidatorFactory, +) +from embodichain.lab.sim.atomic_actions import ( + AtomicActionEngine, + EndpointCommand, + JointPositionPayload, + JointPositionTarget, + RuntimeCommandFrame, +) +from embodichain.lab.sim.planners import CuroboPlanner, MotionGenerator +from embodichain.lab.sim.skills import ParallelSafetyError, SceneRegistry + + +class _Robot: + """Expose current full joint state and one aggregate validation part.""" + + def __init__(self) -> None: + self.qpos = torch.zeros(2, 3) + + def get_qpos(self, *, target: bool = False) -> torch.Tensor: + assert target is False + return self.qpos.clone() + + def get_joint_ids(self, *, name: str) -> list[int]: + assert name == "dual_arm" + return [0, 1] + + +def _motion_generator( + *, + reject_env: int | None = None, +) -> tuple[MotionGenerator, list[torch.Tensor]]: + """Build a no-CUDA shell around the exact CuroboPlanner type.""" + observed: list[torch.Tensor] = [] + planner = CuroboPlanner.__new__(CuroboPlanner) + + def validate_joint_trajectory( + self: CuroboPlanner, + trajectory: torch.Tensor, + *, + control_part: str, + obstacle_poses: object, + ) -> torch.Tensor: + del self, obstacle_poses + assert control_part == "dual_arm" + observed.append(trajectory.clone()) + validity = torch.ones(trajectory.shape[:2], dtype=torch.bool) + if reject_env is not None: + validity[reject_env, trajectory.shape[1] // 2] = False + return validity + + planner.validate_joint_trajectory = MethodType( # type: ignore[method-assign] + validate_joint_trajectory, + planner, + ) + generator = MotionGenerator.__new__(MotionGenerator) + generator.planner = planner + return generator, observed + + +def _frame( + *commands: EndpointCommand, + active: tuple[bool, bool] = (True, True), +) -> RuntimeCommandFrame: + """Build one two-row runtime frame.""" + return RuntimeCommandFrame( + commands=commands, + active_mask=torch.tensor(active, dtype=torch.bool), + env_ids=torch.tensor((0, 1), dtype=torch.long), + hold_duration=torch.full((2,), 0.05), + ) + + +def _command( + target_id: str, + joint_id: int, + positions: tuple[float, float], +) -> EndpointCommand: + """Build one single-joint batched command.""" + return EndpointCommand( + target=JointPositionTarget(target_id, (joint_id,)), + payload=JointPositionPayload( + positions=torch.tensor(positions, dtype=torch.float32).unsqueeze(1) + ), + ) + + +def _validator( + *, + reject_env: int | None = None, + max_joint_step: float = 0.05, + max_interpolation_samples: int = 16, +) -> tuple[CuroboParallelCommandSafetyValidator, list[torch.Tensor]]: + """Create one validator with a deterministic collision backend shell.""" + generator, observed = _motion_generator(reject_env=reject_env) + return ( + CuroboParallelCommandSafetyValidator( + robot=_Robot(), + motion_generator=generator, + scene_registry=SceneRegistry(), + validation_control_part="dual_arm", + max_joint_step=max_joint_step, + max_interpolation_samples=max_interpolation_samples, + ), + observed, + ) + + +def test_curobo_parallel_safety_checks_exact_dense_merged_segment() -> None: + """Disjoint lane targets become one densely sampled aggregate trajectory.""" + validator, observed = _validator() + left = _command("left_arm", 0, (0.1, 0.0)) + right = _command("right_arm", 1, (0.2, -0.1)) + + validator.validate( + branch_frames={"left": _frame(left), "right": _frame(right)}, + merged_frame=_frame(left, right), + ) + + assert len(observed) == 1 + trajectory = observed[0] + # float32 represents 0.2 just above the mathematical value, so the strict + # 0.05 maximum step requires five intervals rather than rounding down. + assert trajectory.shape == (2, 6, 2) + torch.testing.assert_close(trajectory[:, 0], torch.zeros(2, 2)) + torch.testing.assert_close( + trajectory[:, -1], + torch.tensor(((0.1, 0.2), (0.0, -0.1))), + ) + + +def test_curobo_parallel_safety_reports_row_local_collision() -> None: + """One invalid environment rejects dispatch with its stable env ID.""" + validator, _ = _validator(reject_env=1) + left = _command("left_arm", 0, (0.1, 0.1)) + right = _command("right_arm", 1, (0.2, 0.2)) + + with pytest.raises(ParallelSafetyError, match=r"env IDs \(1,\)"): + validator.validate( + branch_frames={"left": _frame(left), "right": _frame(right)}, + merged_frame=_frame(left, right), + ) + + +def test_curobo_parallel_safety_rejects_uncovered_joint() -> None: + """Every outgoing joint must belong to the aggregate collision model.""" + validator, _ = _validator() + left = _command("left_arm", 0, (0.1, 0.1)) + hand = _command("left_hand", 2, (0.2, 0.2)) + + with pytest.raises(ParallelSafetyError, match="outside validation control part"): + validator.validate( + branch_frames={"left": _frame(left), "hand": _frame(hand)}, + merged_frame=_frame(left, hand), + ) + + +def test_curobo_parallel_safety_fails_instead_of_under_sampling() -> None: + """The configured memory bound cannot silently enlarge the joint step.""" + validator, _ = _validator( + max_joint_step=0.01, + max_interpolation_samples=4, + ) + left = _command("left_arm", 0, (0.1, 0.1)) + right = _command("right_arm", 1, (0.0, 0.0)) + + with pytest.raises(ParallelSafetyError, match="exceeding configured limit"): + validator.validate( + branch_frames={"left": _frame(left), "right": _frame(right)}, + merged_frame=_frame(left, right), + ) + + +@pytest.mark.parametrize( + "kwargs", + ( + {"validation_control_part": ""}, + {"validation_control_part": "dual_arm", "max_joint_step": 0.0}, + { + "validation_control_part": "dual_arm", + "max_interpolation_samples": 1, + }, + ), +) +def test_curobo_parallel_safety_factory_validates_configuration( + kwargs: dict[str, object], +) -> None: + """Invalid safety declarations fail during task registration.""" + with pytest.raises((TypeError, ValueError)): + CuroboParallelSafetyValidatorFactory(**kwargs) # type: ignore[arg-type] + + +def test_curobo_parallel_safety_factory_binds_exact_runtime_components() -> None: + """The production factory consumes the assembled engine and registry.""" + robot = _Robot() + motion_generator, _ = _motion_generator() + engine = AtomicActionEngine.__new__(AtomicActionEngine) + engine._planning_services = SimpleNamespace( # type: ignore[attr-defined] + robot=robot, + motion_generator=motion_generator, + ) + factory = CuroboParallelSafetyValidatorFactory(validation_control_part="dual_arm") + + validator = factory.create( + simulation=object(), + robot=robot, + scene_registry=SceneRegistry(), + engine=engine, + ) + + assert type(validator) is CuroboParallelCommandSafetyValidator diff --git a/tests/gym/envs/expert_program/test_simulation_policies.py b/tests/gym/envs/expert_program/test_simulation_policies.py new file mode 100644 index 000000000..5ad6a2e1c --- /dev/null +++ b/tests/gym/envs/expert_program/test_simulation_policies.py @@ -0,0 +1,542 @@ +# ---------------------------------------------------------------------------- +# 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 explicit simulation-backed segment policies.""" + +from __future__ import annotations + +import json +from types import SimpleNamespace + +import pytest +import torch + +from embodichain.lab.gym.envs.expert_program import ( + ExpertProgramCompiler, + SimulationRigidObjectBinding, + SimulationSceneBinding, + decode_expert_program, +) +from embodichain.lab.gym.envs.expert_program.bridge import ( + SegmentPostPolicyMetadataPort, + SegmentPostPolicyPort, + SegmentPostPolicyResultPort, + SegmentValidatorMetadataPort, + SegmentValidatorPort, +) +from embodichain.lab.gym.envs.expert_program.simulation_policies import ( + SimulationSegmentPolicyPort, +) +from embodichain.lab.gym.envs.settling import DynamicSettleMonitorCfg +from embodichain.lab.sim.atomic_actions import EntityState +from embodichain.lab.sim.skills.scene import ( + SceneEntityRegistration, + SceneObjectRef, + SceneRegistry, +) + + +class _StaticStateProvider: + """Provide an inert object state for provider-free compilation.""" + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp + return EntityState( + torch.eye(4, device=env_ids.device).expand(env_ids.numel(), -1, -1) + ) + + +class _RigidObject: + """Small live rigid-object double with mutable velocities and poses.""" + + def __init__(self, positions: torch.Tensor) -> None: + batch_size = positions.shape[0] + self.is_non_dynamic = False + self.pose_reads = 0 + self.body_data = SimpleNamespace( + lin_vel=torch.zeros(batch_size, 3), + ang_vel=torch.zeros(batch_size, 3), + ) + self._pose = torch.eye(4).expand(batch_size, -1, -1).clone() + self._pose[:, :3, 3] = positions + + def get_local_pose(self, *, to_matrix: bool) -> torch.Tensor: + assert to_matrix + self.pose_reads += 1 + return self._pose.clone() + + +class _Robot: + """Distinct current- and target-qpos source for post-policy holds.""" + + 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, target: bool = False) -> torch.Tensor: + self.qpos_reads.append(target) + return (self.target_qpos if target else self.current_qpos).clone() + + +class _Simulation: + """Resolve only one explicitly selected native rigid object.""" + + def __init__(self, entity: _RigidObject) -> None: + self.entity = entity + + def get_rigid_object(self, uid: str) -> _RigidObject | None: + return self.entity if uid == "native_cube" else None + + def get_articulation(self, uid: str) -> None: + del uid + return None + + +def _compiled_segment(*, settle_preset: str = "fast"): + """Compile one segment containing both supported policy types.""" + payload = { + "schema_version": 1, + "program_id": "policy_test", + "integration": { + "robot_profile": "test_robot", + "scene_registry": "test_scene", + "runtime_preset": "safe", + }, + "targets": { + "drop": { + "kind": "cyclic_pose", + "values": [ + { + "position": [0.0, 0.0, 0.0], + "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + } + ], + } + }, + "program": { + "kind": "segment", + "name": "place", + "steps": { + "kind": "invoke", + "call": { + "kind": "place", + "object": "cube", + "at": {"kind": "target_ref", "target": "drop"}, + }, + }, + "post": [ + { + "kind": "wait_stable", + "entity": "cube", + "preset": settle_preset, + } + ], + "validators": [ + { + "kind": "object_near_target", + "object": "cube", + "target": "drop", + "position_tolerance": 0.05, + } + ], + }, + } + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StaticStateProvider(), + ), + ) + ) + compiled = ExpertProgramCompiler.from_scene_registry(registry).compile( + decode_expert_program(payload) + ) + return next(compiled.iter_segments()) + + +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]]), + target_qpos=target_qpos, + ) + port = SimulationSegmentPolicyPort( + _Simulation(entity), + robot, + SimulationSceneBinding( + registry_id="test_scene", + rigid_objects=( + SimulationRigidObjectBinding( + entity_id="cube", + simulation_uid="native_cube", + ), + ), + ), + settle_presets={ + "fast": preset + or DynamicSettleMonitorCfg( + min_steps=0, + max_steps=3, + check_interval_steps=1, + required_stable_checks=2, + ) + }, + ) + return port, entity, robot + + +def test_port_implements_both_bridge_policy_protocols() -> None: + """One shared instance serves post-policy and validator boundaries.""" + port, _, _ = _port(torch.zeros(2, 3)) + + assert isinstance(port, SegmentPostPolicyPort) + assert isinstance(port, SegmentPostPolicyMetadataPort) + assert isinstance(port, SegmentPostPolicyResultPort) + assert isinstance(port, SegmentValidatorPort) + assert isinstance(port, SegmentValidatorMetadataPort) + assert port.settle_preset_ids == ("fast",) + + +def test_pure_preflight_validates_hooks_without_reading_live_state() -> None: + """Static hook validation emits no hold and samples no pose or qpos.""" + segment = _compiled_segment() + port, entity, robot = _port(torch.zeros(2, 3)) + + port.validate_policy(segment.post_policies[0], segment=segment) + port.validate_validator(segment.validators[0], segment=segment) + + assert robot.qpos_reads == [False] + assert entity.pose_reads == 0 + + +def test_pure_preflight_rejects_unknown_settle_preset_without_observation() -> None: + """An unknown preset fails before policy iteration can sample live state.""" + segment = _compiled_segment(settle_preset="missing") + port, entity, robot = _port(torch.zeros(2, 3)) + + with pytest.raises(KeyError, match="Unknown settle preset 'missing'"): + port.validate_policy(segment.post_policies[0], segment=segment) + + assert robot.qpos_reads == [False] + assert entity.pose_reads == 0 + + +def test_wait_stable_yields_fresh_target_qpos_holds_through_gym() -> None: + """Settling preserves loaded drive targets with independently owned holds.""" + 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, + ), + ) + actions = port.actions( + segment.post_policies[0], + segment=segment, + active_mask=torch.ones(2, dtype=torch.bool), + ) + + first = next(actions) + 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.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], + segment=segment, + ) + assert metadata["status"] == "settled" + assert metadata["preset"] == "fast" + assert metadata["thresholds"] == { + "linear_velocity": 0.03, + "angular_velocity": 0.2, + "min_steps": 0, + "max_steps": 4, + "check_interval_steps": 1, + "required_stable_checks": 3, + } + 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] + assert port.post_policy_result( + segment.post_policies[0], + segment=segment, + ).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() + port, entity, _ = _port(torch.zeros(2, 3)) + entity.body_data.lin_vel[1, 0] = 1.0 + actions = port.actions( + segment.post_policies[0], + segment=segment, + active_mask=torch.ones(2, dtype=torch.bool), + ) + + assert sum(1 for _ in (next(actions), next(actions), next(actions))) == 3 + with pytest.raises(StopIteration): + next(actions) + + metadata = port.post_policy_metadata( + segment.post_policies[0], + segment=segment, + ) + assert metadata["status"] == "timed_out" + assert metadata["state"]["elapsed_steps"] == 3 + assert metadata["state"]["settled_mask"] == [True, False] + assert metadata["state"]["timeout_mask"] == [False, True] + assert metadata["state"]["max_linear_speed"] == [0.0, 1.0] + assert port.post_policy_result( + segment.post_policies[0], + segment=segment, + ).tolist() == [True, False] + + +def test_in_progress_settling_metadata_uses_json_null_for_unchecked_speeds() -> None: + segment = _compiled_segment() + port, _, _ = _port( + torch.zeros(2, 3), + preset=DynamicSettleMonitorCfg( + min_steps=2, + max_steps=4, + check_interval_steps=1, + required_stable_checks=1, + ), + ) + actions = port.actions( + segment.post_policies[0], + segment=segment, + active_mask=torch.ones(2, dtype=torch.bool), + ) + + next(actions) + metadata = port.post_policy_metadata( + segment.post_policies[0], + segment=segment, + ) + actions.close() + + assert metadata["status"] == "running" + assert metadata["state"]["max_linear_speed"] == [None, None] + assert metadata["state"]["max_angular_speed"] == [None, None] + json.dumps(metadata, allow_nan=False, sort_keys=True) + + +def test_wait_stable_excludes_inactive_moving_row_from_completion() -> None: + """A failed runtime row cannot block or pass a later settling policy.""" + segment = _compiled_segment() + port, entity, _ = _port(torch.zeros(2, 3)) + entity.body_data.lin_vel[1, 0] = 1.0 + active_mask = torch.tensor([True, False]) + actions = port.actions( + segment.post_policies[0], + segment=segment, + active_mask=active_mask, + ) + + assert sum(1 for _ in actions) == 1 + + metadata = port.post_policy_metadata( + segment.post_policies[0], + segment=segment, + ) + assert metadata["status"] == "settled" + assert metadata["active_mask"] == [True, False] + assert metadata["state"]["active_mask"] == [True, False] + assert metadata["state"]["settled_mask"] == [True, False] + assert metadata["state"]["timeout_mask"] == [False, False] + assert metadata["state"]["max_linear_speed"] == [0.0, None] + assert port.post_policy_result( + segment.post_policies[0], + segment=segment, + ).tolist() == [True, False] + + +def test_wait_stable_skips_when_no_rows_remain_active() -> None: + """An empty active cohort completes without an environment hold.""" + segment = _compiled_segment() + port, _, _ = _port(torch.zeros(2, 3)) + + actions = port.actions( + segment.post_policies[0], + segment=segment, + active_mask=torch.zeros(2, dtype=torch.bool), + ) + + assert tuple(actions) == () + metadata = port.post_policy_metadata( + segment.post_policies[0], + segment=segment, + ) + assert metadata["status"] == "skipped" + assert metadata["state"]["settled_mask"] == [False, False] + assert metadata["state"]["timeout_mask"] == [False, False] + assert port.post_policy_result( + segment.post_policies[0], + segment=segment, + ).tolist() == [False, False] + + +def test_object_near_target_validates_rows_independently() -> None: + """The validator compares explicit native object poses row by row.""" + segment = _compiled_segment() + port, _, _ = _port(torch.tensor([[0.01, 0.0, 0.0], [0.20, 0.0, 0.0]])) + + result = port.validate(segment.validators[0], segment=segment) + + assert result.dtype == torch.bool + assert result.tolist() == [True, False] + metadata = port.validator_metadata(segment.validators[0], segment=segment) + assert metadata["kind"] == "object_near_target" + assert metadata["object_id"] == "cube" + assert metadata["target_id"] == "drop" + assert metadata["position_tolerance"] == 0.05 + assert metadata["position_error"] == pytest.approx([0.01, 0.20]) + assert metadata["accepted_mask"] == [True, False] + + +def test_policy_port_rejects_unbound_native_entities_and_foreign_members() -> None: + """Bindings and compiled segment ownership are exact fail-closed boundaries.""" + binding = SimulationSceneBinding( + registry_id="test_scene", + rigid_objects=( + SimulationRigidObjectBinding( + entity_id="missing", + simulation_uid="unknown", + ), + ), + ) + robot = _Robot(torch.zeros(2, 2)) + with pytest.raises(KeyError, match="unknown"): + SimulationSegmentPolicyPort( + _Simulation(_RigidObject(torch.zeros(2, 3))), + robot, + binding, + ) + + segment = _compiled_segment() + other = _compiled_segment() + port, _, _ = _port(torch.zeros(2, 3)) + with pytest.raises(ValueError, match="does not belong"): + tuple( + port.actions( + other.post_policies[0], + segment=segment, + active_mask=torch.ones(2, dtype=torch.bool), + ) + ) + + +__all__: list[str] = [] diff --git a/tests/gym/envs/expert_program/test_task_vertical_slices.py b/tests/gym/envs/expert_program/test_task_vertical_slices.py new file mode 100644 index 000000000..4766acf7a --- /dev/null +++ b/tests/gym/envs/expert_program/test_task_vertical_slices.py @@ -0,0 +1,629 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Configuration and non-physical bridge vertical slices for Expert Programs.""" + +from __future__ import annotations + +from copy import deepcopy +import json +from pathlib import Path + +import pytest +import torch +import yaml + +from embodichain.lab.gym.envs.expert_program import ( + ExpertProgramCompiler, + decode_expert_program, +) +from embodichain.lab.gym.envs.expert_program.bridge import ( + AtomicDemoBridge, + BufferedGymCommandSink, + EnvironmentStepClock, + RuntimeCommandFrameEncoder, +) +from embodichain.lab.sim.atomic_actions import Affordance, EntityState, TaskState +from embodichain.lab.sim.skills.calls import OperateArticulation, Pick, Place +from embodichain.lab.sim.skills.runtime import SkillResult, SkillStatus +from embodichain.lab.sim.skills.scene import ( + SceneAffordanceRef, + SceneArticulationRef, + SceneCollisionRole, + SceneEntityRegistration, + SceneObjectRef, + SceneRegistry, +) +from embodichain.lab.gym.utils.registration import discover_task_packages +from embodichain_tasks.configs import get_config_path + +discover_task_packages() + +from embodichain_tasks.multi_segments import cube_pick_place as cube_task +from embodichain_tasks.tableware import open_drawer as drawer_task + +_REPEATED_CUBE_PROGRAM = Path( + "expert_program/multi_segments/repeated_cube_pick_place.yaml" +) +_OPEN_DRAWER_PROGRAM = Path("expert_program/tableware/open_drawer.json") +_LIFECYCLE_BATCH_SIZE = 2 +_LIFECYCLE_ROBOT_DOF = 3 +_LIFECYCLE_STEP_DT = 0.02 + + +class _NeverObserveProvider: + """Reject dynamic observations during configuration decoding/compilation.""" + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp, env_ids + raise AssertionError("Task configuration compilation must not observe state.") + + +class _FixedQposProvider: + """Return a finite full-qpos hold for the bridge's unused command sink.""" + + def current_qpos(self, env_ids: torch.Tensor) -> torch.Tensor: + return torch.zeros( + (env_ids.numel(), _LIFECYCLE_ROBOT_DOF), + dtype=torch.float32, + device=env_ids.device, + ) + + +class _FreshObservationPort: + """Issue one distinct observation generation for every segment runtime.""" + + def __init__(self) -> None: + self.generations: list[int] = [] + + def capture(self) -> int: + generation = len(self.generations) + 1 + self.generations.append(generation) + return generation + + +class _CompletedSegmentRuntime: + """Complete each semantic prefix from one freshly captured observation.""" + + def __init__( + self, + observation: _FreshObservationPort, + lifecycle_events: list[tuple[str, int]], + ) -> None: + self._observation = observation + self._lifecycle_events = lifecycle_events + self._status = SkillStatus.IDLE + self._result = self._make_result( + status=SkillStatus.IDLE, + workflow_id=None, + eligible_mask=torch.ones(_LIFECYCLE_BATCH_SIZE, dtype=torch.bool), + generation=0, + ) + self.analysis_window_lengths: list[int] = [] + self.executed_semantic_ids: list[str] = [] + self.eligible_masks: list[torch.Tensor | None] = [] + + @staticmethod + def _make_result( + *, + status: SkillStatus, + workflow_id: str | None, + eligible_mask: torch.Tensor, + generation: int, + ) -> SkillResult: + terminal = status is SkillStatus.COMPLETED + return SkillResult( + status=status, + workflow_id=workflow_id, + current_call_index=None, + env_ids=torch.arange(_LIFECYCLE_BATCH_SIZE, dtype=torch.long), + success_mask=( + eligible_mask.clone() if terminal else torch.zeros_like(eligible_mask) + ), + failure_mask=torch.zeros_like(eligible_mask), + cancelled_mask=torch.zeros_like(eligible_mask), + eligible_mask=eligible_mask, + task_state=TaskState.empty(_LIFECYCLE_BATCH_SIZE, "cpu"), + message=f"observation_generation={generation}", + ) + + @property + def result(self) -> SkillResult: + return self._result + + @property + def status(self) -> SkillStatus: + return self._status + + def start( + self, + *calls: object, + workflow_id: str = "semantic_workflow", + eligible_mask: torch.Tensor | None = None, + execution_prefix_length: int | None = None, + ) -> SkillResult: + call_values = tuple(calls[0]) if len(calls) == 1 else tuple(calls) + if execution_prefix_length is None: + raise AssertionError("A packaged sequential segment requires a prefix.") + selected = ( + torch.ones(_LIFECYCLE_BATCH_SIZE, dtype=torch.bool) + if eligible_mask is None + else eligible_mask.clone() + ) + execution_calls = call_values[:execution_prefix_length] + generation = self._observation.capture() + self._lifecycle_events.append(("observe", generation)) + self.analysis_window_lengths.append(len(call_values)) + self.executed_semantic_ids.extend( + str(getattr(call, "semantic_id")) for call in execution_calls + ) + self.eligible_masks.append( + None if eligible_mask is None else eligible_mask.clone() + ) + self._status = SkillStatus.COMPLETED + self._result = self._make_result( + status=SkillStatus.COMPLETED, + workflow_id=workflow_id, + eligible_mask=selected, + generation=generation, + ) + return self._result + + def step(self) -> SkillResult: + raise AssertionError("A terminal fake runtime must not be stepped.") + + def cancel(self, reason: str) -> SkillResult: + raise AssertionError(f"A completed fake runtime cannot be cancelled: {reason}") + + def adopt_verified_task_state(self, task_state: TaskState) -> SkillResult: + del task_state + return self._result + + +class _LifecyclePostPolicyPort: + """Run every packaged settle policy and expose deterministic metadata.""" + + def __init__( + self, + observation: _FreshObservationPort, + lifecycle_events: list[tuple[str, int]], + ) -> None: + self._observation = observation + self._lifecycle_events = lifecycle_events + self.active_masks: list[torch.Tensor] = [] + self._metadata: dict[int, dict[str, object]] = {} + + def validate_policy(self, policy: object, *, segment: object) -> None: + del policy, segment + + def actions( + self, + policy: object, + *, + segment: object, + active_mask: torch.Tensor, + ): + segment_index = int(getattr(segment, "segment_index")) + generation = self._observation.generations[-1] + self._lifecycle_events.append(("settle", segment_index)) + self.active_masks.append(active_mask.clone()) + self._metadata[id(policy)] = { + "status": "settled", + "segment_index": segment_index, + "observation_generation": generation, + } + yield torch.zeros( + (_LIFECYCLE_BATCH_SIZE, _LIFECYCLE_ROBOT_DOF), + dtype=torch.float32, + ) + + def post_policy_result( + self, + policy: object, + *, + segment: object, + ) -> torch.Tensor: + del policy, segment + return self.active_masks[-1].clone() + + def post_policy_metadata( + self, + policy: object, + *, + segment: object, + ) -> dict[str, object]: + del segment + return dict(self._metadata[id(policy)]) + + +class _LifecycleValidatorPort: + """Validate every segment and filter one row after the first cycle.""" + + def __init__( + self, + observation: _FreshObservationPort, + lifecycle_events: list[tuple[str, int]], + ) -> None: + self._observation = observation + self._lifecycle_events = lifecycle_events + self._metadata: dict[int, dict[str, object]] = {} + + def validate_validator(self, validator: object, *, segment: object) -> None: + del validator, segment + + def validate(self, validator: object, *, segment: object) -> torch.Tensor: + segment_index = int(getattr(segment, "segment_index")) + generation = self._observation.generations[-1] + self._lifecycle_events.append(("validate", segment_index)) + result = ( + torch.tensor([True, False]) + if segment_index == 0 + else torch.ones(_LIFECYCLE_BATCH_SIZE, dtype=torch.bool) + ) + self._metadata[id(validator)] = { + "segment_index": segment_index, + "observation_generation": generation, + "accepted_mask": result.tolist(), + } + return result + + def validator_metadata( + self, + validator: object, + *, + segment: object, + ) -> dict[str, object]: + del segment + return dict(self._metadata[id(validator)]) + + +def _read_payload(relative_path: Path) -> dict[str, object]: + """Load one packaged JSON/YAML example as inert data.""" + path = get_config_path(relative_path) + if path.suffix == ".json": + payload = json.loads(path.read_text(encoding="utf-8")) + else: + payload = yaml.safe_load(path.read_text(encoding="utf-8")) + assert type(payload) is dict + return payload + + +def _cube_compiler() -> ExpertProgramCompiler: + """Build the smallest typed identity registry needed by the cube program.""" + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_NeverObserveProvider(), + ), + ) + ) + return ExpertProgramCompiler.from_scene_registry(registry) + + +def _drawer_compiler() -> ExpertProgramCompiler: + """Build typed drawer and handle identities without any motion code.""" + provider = _NeverObserveProvider() + drawer = SceneArticulationRef("drawer") + registry = SceneRegistry( + ( + SceneEntityRegistration(ref=drawer, state_provider=provider), + SceneEntityRegistration( + ref=SceneAffordanceRef("drawer_handle"), + parent=drawer, + native_name="handle_xpos", + affordance=Affordance(), + relative_pose=torch.eye(4), + ), + ) + ) + return ExpertProgramCompiler.from_scene_registry(registry) + + +def test_repeated_cube_program_is_three_lazy_semantic_segments() -> None: + """The packaged cube task expands to three independently scoped cycles.""" + config = decode_expert_program(_read_payload(_REPEATED_CUBE_PROGRAM)) + + assert config.integration.scene_registry == cube_task.CUBE_SCENE_REGISTRY_ID + assert config.integration.robot_profile == cube_task.CUBE_ROBOT_PROFILE_ID + + segments = tuple(_cube_compiler().compile(config)) + + assert [segment.name for segment in segments] == ["move_cube"] * 3 + assert [segment.segment_index for segment in segments] == [0, 1, 2] + assert [len(segment.calls) for segment in segments] == [2, 2, 2] + assert all(type(segment.calls[0].call) is Pick for segment in segments) + assert all(type(segment.calls[1].call) is Place for segment in segments) + assert [ + segment.calls[1].target_selections[0].value_index for segment in segments + ] == [0, 1, 0] + assert [ + segment.validators[0].target_selection.value_index for segment in segments + ] == [ + 0, + 1, + 0, + ] + assert all( + segment.post_policies[0].cfg.kind == "wait_stable" for segment in segments + ) + assert all( + segment.validators[0].cfg.position_tolerance == 0.12 for segment in segments + ) + + +def test_packaged_repeated_cube_runs_three_lazy_bridge_lifecycles() -> None: + """The real packaged program owns three ordered observable lifecycles.""" + config = decode_expert_program(_read_payload(_REPEATED_CUBE_PROGRAM)) + compiled = _cube_compiler().compile(config).materialize() + lifecycle_events: list[tuple[str, int]] = [] + observation = _FreshObservationPort() + clock = EnvironmentStepClock(_LIFECYCLE_STEP_DT) + sink = BufferedGymCommandSink( + RuntimeCommandFrameEncoder(_FixedQposProvider()), + clock, + ) + runtime = _CompletedSegmentRuntime(observation, lifecycle_events) + post_port = _LifecyclePostPolicyPort(observation, lifecycle_events) + validator_port = _LifecycleValidatorPort(observation, lifecycle_events) + bridge = AtomicDemoBridge( + compiled, + runtime, + sink, + clock, + post_policy_port=post_port, + validator_port=validator_port, + ) + + iterator = iter(bridge.iter_segments()) + segment_names: list[str | None] = [] + segment_metadata: list[dict[str, object]] = [] + action_metadata: list[dict[str, object]] = [] + accepted_masks: list[list[bool]] = [] + for segment_index in range(3): + observation_count = len(observation.generations) + demo_segment = next(iterator) + segment_names.append(demo_segment.name) + + # Merely requesting the next lazy segment must not capture live state. + assert len(observation.generations) == observation_count + actions = tuple(demo_segment.actions) + + assert observation.generations == list(range(1, segment_index + 2)) + assert len(actions) == 1 + assert demo_segment.metadata["validation"] is None + action_metadata.append(dict(actions[0].metadata)) + accepted_masks.append(demo_segment.validator().tolist()) + segment_metadata.append(dict(demo_segment.metadata)) + + with pytest.raises(StopIteration): + next(iterator) + + assert segment_names == ["move_cube"] * 3 + assert runtime.analysis_window_lengths == [6, 4, 2] + assert runtime.executed_semantic_ids == ["pick", "place"] * 3 + assert observation.generations == [1, 2, 3] + assert lifecycle_events == [ + ("observe", 1), + ("settle", 0), + ("validate", 0), + ("observe", 2), + ("settle", 1), + ("validate", 1), + ("observe", 3), + ("settle", 2), + ("validate", 2), + ] + assert runtime.eligible_masks[0] is None + assert [mask.tolist() for mask in runtime.eligible_masks[1:]] == [ + [True, False], + [True, False], + ] + assert [mask.tolist() for mask in post_port.active_masks] == [ + [True, True], + [True, False], + [True, False], + ] + assert accepted_masks == [[True, False]] * 3 + + for segment_index, metadata in enumerate(segment_metadata): + eligible_before = [True, True] if segment_index == 0 else [True, False] + validator_result = [True, False] if segment_index == 0 else [True, True] + assert metadata["expert_program_id"] == compiled.program_id + assert metadata["program_segment_index"] == segment_index + assert metadata["semantic_call_indices"] == [ + 2 * segment_index, + 2 * segment_index + 1, + ] + assert metadata["post_policy_count"] == 1 + assert metadata["validator_count"] == 1 + runtime_metadata = metadata["runtime"] + assert isinstance(runtime_metadata, dict) + assert runtime_metadata["message"] == ( + f"observation_generation={segment_index + 1}" + ) + post_policies = metadata["post_policies"] + assert isinstance(post_policies, list) + assert post_policies[0]["kind"] == "wait_stable" + assert post_policies[0]["result_mask"] == eligible_before + assert post_policies[0]["result"] == { + "status": "settled", + "segment_index": segment_index, + "observation_generation": segment_index + 1, + } + validation = metadata["validation"] + assert isinstance(validation, dict) + assert validation["eligible_mask_before_validation"] == eligible_before + assert validation["accepted_mask"] == [True, False] + validators = validation["validators"] + assert validators[0]["kind"] == "object_near_target" + assert validators[0]["result_mask"] == validator_result + assert validators[0]["result"] == { + "segment_index": segment_index, + "observation_generation": segment_index + 1, + "accepted_mask": validator_result, + } + json.dumps(metadata, allow_nan=False, sort_keys=True) + + assert action_metadata[segment_index]["bridge_action_kind"] == ( + "program_post_policy" + ) + assert action_metadata[segment_index]["program_segment_index"] == ( + segment_index + ) + + +def test_cube_variant_extends_by_data_without_motion_generation_code() -> None: + """A fourth destination and cycle require only serialized-data changes.""" + payload = deepcopy(_read_payload(_REPEATED_CUBE_PROGRAM)) + target = payload["targets"]["drop_pose"] + target["values"].extend( + ( + { + "position": [-0.25, -0.20, 0.10], + "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + }, + { + "position": [-0.25, 0.20, 0.10], + "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + }, + ) + ) + payload["program"]["count"] = 4 + + segments = tuple(_cube_compiler().compile(decode_expert_program(payload))) + + assert len(segments) == 4 + last_place = segments[-1].calls[-1].call + assert type(last_place) is Place + assert last_place.at is not None + assert last_place.at.position.tolist() == pytest.approx([-0.25, 0.20, 0.10]) + + +def test_open_drawer_program_compiles_to_reusable_articulation_skill() -> None: + """The drawer task supplies a goal and identities, never a trajectory.""" + payload = _read_payload(_OPEN_DRAWER_PROGRAM) + config = decode_expert_program(payload) + + assert config.integration.scene_registry == drawer_task.DRAWER_SCENE_REGISTRY_ID + assert config.integration.robot_profile == drawer_task.DRAWER_ROBOT_PROFILE_ID + + segments = tuple(_drawer_compiler().compile(config)) + + assert len(segments) == 1 + assert segments[0].name == "open_drawer" + assert len(segments[0].calls) == 1 + call = segments[0].calls[0].call + assert type(call) is OperateArticulation + assert call.articulation == SceneArticulationRef("drawer") + assert call.handle == SceneAffordanceRef("drawer_handle") + assert call.target == "open" + assert call.target_position is None + assert call.target_displacement is None + assert dict(call.resources) == {} + + +def test_task_classes_do_not_override_motion_or_demo_generation() -> None: + """Both environments delegate planning and execution to the shared runtime.""" + forbidden_overrides = { + "create_demo_action_list", + "create_demo_segments", + "_generate_eef_motion", + "_initialize_atomic_actions", + "_plan_pick_place_cycle", + } + + for env_type in ( + cube_task.MultiSegmentsCubePickPlaceEnv, + drawer_task.OpenDrawerEnv, + ): + assert forbidden_overrides.isdisjoint(env_type.__dict__) + + +def test_cube_task_declares_scene_and_robot_bindings_without_trajectory_code() -> None: + """Cube integration is an auditable identity/resource declaration.""" + scene = cube_task.create_cube_scene_binding(grasp_samples=32) + profile = cube_task.create_cube_robot_profile_binding() + + assert scene.registry_id == cube_task.CUBE_SCENE_REGISTRY_ID + assert scene.rigid_objects[0].simulation_uid == "cube" + assert scene.rigid_objects[0].collision_role is SceneCollisionRole.NONE + assert scene.rigid_objects[0].default_grasp_affordance == ( + cube_task.CUBE_GRASP_AFFORDANCE_ID + ) + assert scene.antipodal_grasps[0].object_id == "cube" + assert profile.profile_id == cube_task.CUBE_ROBOT_PROFILE_ID + assert dict(profile.defaults) == { + "pick_up": {"primary": "manipulator"}, + "place": {"primary": "manipulator"}, + } + assert profile.command_presets[0].commands["grasp"] == (0.024,) + + +def test_drawer_task_declares_native_link_joint_and_named_target() -> None: + """Drawer operation grounds through explicit native simulation identities.""" + scene = drawer_task.create_open_drawer_scene_binding() + profile = drawer_task.create_open_drawer_robot_profile_binding() + + operation = scene.articulation_operations[0] + assert scene.registry_id == drawer_task.DRAWER_SCENE_REGISTRY_ID + assert scene.articulations[0].collision_role is SceneCollisionRole.NONE + assert scene.links[0].native_link_name == "handle_xpos" + assert operation.joint_id == "slide_rails" + assert operation.operation_axis == (0.0, 0.0, -1.0) + assert operation.semantic_targets["open"].target_position == 0.11 + assert profile.profile_id == drawer_task.DRAWER_ROBOT_PROFILE_ID + assert dict(profile.defaults) == { + "operate_articulation": {"primary": "right_manipulator"} + } + assert profile.command_presets[0].commands == { + "open": (0.05, 0.05), + "grasp": (0.0, 0.0), + } + + +def test_vertical_slice_payloads_expose_no_motion_layer_fields() -> None: + """Official examples remain semantic data without controller/planner knobs.""" + forbidden_fields = { + "action", + "control_part", + "eef", + "joint_ids", + "motion_generator", + "planner", + "qpos", + "sample_count", + "tcp", + "trajectory", + } + + def keys(value: object) -> set[str]: + if type(value) is dict: + return set(value).union(*(keys(item) for item in value.values())) + if type(value) is list: + return set().union(*(keys(item) for item in value)) + return set() + + for path in (_REPEATED_CUBE_PROGRAM, _OPEN_DRAWER_PROGRAM): + assert forbidden_fields.isdisjoint(keys(_read_payload(path))) + + +__all__: list[str] = [] 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..96a1e11bb --- /dev/null +++ b/tests/gym/envs/tasks/test_hand_over.py @@ -0,0 +1,542 @@ +# ---------------------------------------------------------------------------- +# 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].workflow_recovery_policy.max_recovery_attempts == 2 + 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/gym/envs/tasks/test_multi_segments_cube_pick_place.py b/tests/gym/envs/tasks/test_multi_segments_cube_pick_place.py index 1c04d6ca7..5649a005a 100644 --- a/tests/gym/envs/tasks/test_multi_segments_cube_pick_place.py +++ b/tests/gym/envs/tasks/test_multi_segments_cube_pick_place.py @@ -14,18 +14,19 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Tests for the lazy multi-segment cube pick-and-place task.""" +"""Tests for the declarative multi-segment cube task.""" from __future__ import annotations +import importlib import json from pathlib import Path -from types import MethodType, SimpleNamespace +from types import SimpleNamespace -import pytest import torch from embodichain.lab.gym.envs import EmbodiedEnv +from embodichain.lab.gym.envs.expert_program import ExpertProgramEnvironmentMixin from embodichain.lab.gym.utils.gym_utils import config_to_cfg from embodichain.lab.gym.utils.registration import ( REGISTERED_ENVS, @@ -37,125 +38,212 @@ discover_task_packages() from embodichain_tasks.multi_segments.cube_pick_place import ( # noqa: E402 + CUBE_ROBOT_PROFILE_ID, + CUBE_SCENE_REGISTRY_ID, + CUBE_EXPERT_PROGRAM_REGISTRATION, MultiSegmentsCubePickPlaceEnv, + _create_default_env_cfg, + create_cube_robot_profile_binding, ) -class TestMultiSegmentsCubePickPlaceEnv: - """Registration, config, and lazy-planning tests.""" - - def test_registered_and_exported(self) -> None: - """The new task category exports a registered environment.""" - from embodichain_tasks.multi_segments import __all__ - - assert "MultiSegmentsCubePickPlaceEnv" in __all__ - spec = REGISTERED_ENVS["MultiSegmentsCubePickPlace-v1"] - assert spec.cls is MultiSegmentsCubePickPlaceEnv - assert spec.max_episode_steps == 1200 - assert issubclass(MultiSegmentsCubePickPlaceEnv, EmbodiedEnv) - - def test_gym_config_targets_the_registered_task(self) -> None: - """The runnable gym config selects the task and three cycles.""" - config_path = ( - Path(__file__).parents[4] - / "embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json" - ) - config = json.loads(config_path.read_text()) - - assert config["id"] == "MultiSegmentsCubePickPlace-v1" - assert config["env"]["extensions"]["num_cycles"] == 3 - assert config["env"]["extensions"]["grasp_hold_steps"] == 45 - assert len(config["env"]["extensions"]["place_positions"]) == 2 - assert config["rigid_object"][0]["uid"] == "cube" - assert config["robot"]["class_type"] == "URRobot" - assert config["robot"]["robot_type"] == "ur5" - recorder = config["env"]["dataset"]["lerobot"] - assert recorder["func"] == "LeRobotRecorder" - assert recorder["params"]["robot_meta"] == { - "robot_type": "UR5", - "control_freq": 25, - } - assert recorder["params"]["save_path"] == "outputs/lerobot/multi_segments" - - cfg = config_to_cfg(config) - - assert isinstance(cfg.robot, URRobotCfg) - assert cfg.robot.robot_type == "ur5" - assert cfg.robot.control_parts["arm"] == [ - "joint1", - "joint2", - "joint3", - "joint4", - "joint5", - "joint6", - ] - assert cfg.robot.solver_cfg["arm"].ur_type == "ur5" - assert cfg.robot.solver_cfg["arm"].d1 == 0.089159 - - def test_segments_are_planned_lazily_from_updated_scene(self) -> None: - """Requesting the next segment observes the post-execution cube pose.""" - env = object.__new__(MultiSegmentsCubePickPlaceEnv) - env.num_cycles = 3 - env.place_positions = ((1.0, 0.0, 0.1), (2.0, 0.0, 0.1)) - env._completed_cycles = 0 - env._last_target_position = None - env.sim = SimpleNamespace(device=torch.device("cpu")) - env._scene_position_for_test = 0.0 - env._planned_positions_for_test = [] - - def fake_plan( - self: MultiSegmentsCubePickPlaceEnv, target_position: torch.Tensor - ): - source_pose = torch.eye(4).unsqueeze(0) - source_pose[:, 0, 3] = self._scene_position_for_test - self._planned_positions_for_test.append(self._scene_position_for_test) - action = torch.tensor([[self._scene_position_for_test]]) - return torch.ones(1, dtype=torch.bool), (action,), source_pose - - env._plan_pick_place_cycle = MethodType(fake_plan, env) - segments = iter(env.create_demo_segments()) - - first = next(segments) - assert env._planned_positions_for_test == [0.0] - assert first.metadata["planned_source_poses"][0][0][3] == 0.0 - - # In the real executor the first segment actions run while the outer - # generator is suspended. Emulate the resulting free-fall displacement. - list(first.actions) - env._scene_position_for_test = 0.17 - second = next(segments) - assert env._planned_positions_for_test == [0.0, 0.17] - assert second.metadata["planned_source_poses"][0][0][3] == pytest.approx(0.17) - - env._scene_position_for_test = -0.04 - third = next(segments) - assert env._planned_positions_for_test == [0.0, 0.17, -0.04] - assert third.metadata["target_position"] == pytest.approx([1.0, 0.0, 0.1]) - - list(third.actions) - try: - next(segments) - except StopIteration: - pass - else: - raise AssertionError("Expected exactly three demo segments.") - assert env._completed_cycles == 3 - - def test_invalid_positions_are_rejected(self) -> None: - """Every configured placement target must be an XYZ position.""" - with pytest.raises(ValueError, match="XYZ"): - MultiSegmentsCubePickPlaceEnv._validate_place_positions([(1.0, 2.0)]) - - def test_grasp_hold_is_inserted_before_lift(self) -> None: - """The closed grasp waypoint is held before the pickup lift starts.""" - env = object.__new__(MultiSegmentsCubePickPlaceEnv) - env.grasp_hold_steps = 2 - trajectory = torch.arange(120, dtype=torch.float32).reshape(1, 120, 1) - - augmented, clear_step = env._insert_grasp_hold(trajectory) - - assert augmented.shape == (1, 122, 1) - assert clear_step == 78 - assert augmented[0, 75, 0] == 75 - assert augmented[0, 76:78, 0].tolist() == [75, 75] - assert augmented[0, 78, 0] == 76 +def _gym_config_path() -> Path: + """Return the installed-source cube Gym config path.""" + return ( + Path(__file__).parents[4] + / "embodichain_tasks/configs/gym/multi_segments/cube_pick_place.json" + ) + + +def _gym_payload() -> dict[str, object]: + """Load the runnable Gym configuration as inert JSON data.""" + path = _gym_config_path() + payload = json.loads(path.read_text(encoding="utf-8")) + assert type(payload) is dict + return payload + + +def test_registered_task_uses_shared_expert_program_mixin() -> None: + """The task is registered and delegates semantic execution to the mixin.""" + from embodichain_tasks.multi_segments import __all__ + + assert "MultiSegmentsCubePickPlaceEnv" in __all__ + spec = REGISTERED_ENVS["MultiSegmentsCubePickPlace-v1"] + assert spec.cls is MultiSegmentsCubePickPlaceEnv + assert spec.max_episode_steps == 1200 + assert spec.expert_program_registration is CUBE_EXPERT_PROGRAM_REGISTRATION + assert "expert_program_registration" not in spec.default_kwargs + assert issubclass(MultiSegmentsCubePickPlaceEnv, ExpertProgramEnvironmentMixin) + assert issubclass(MultiSegmentsCubePickPlaceEnv, EmbodiedEnv) + + +def test_gym_config_selects_packaged_expert_program() -> None: + """Normal Gym startup selects the semantic program by a relative path.""" + payload = _gym_payload() + + assert payload["id"] == "MultiSegmentsCubePickPlace-v1" + assert payload["expert_program_path"] == ( + "../../expert_program/multi_segments/repeated_cube_pick_place.yaml" + ) + assert payload["env"]["extensions"] == {} + settle = payload["env"]["events"]["settle_cube_on_reset"] + assert settle["func"] == "wait_for_dynamic_objects_to_settle" + assert settle["mode"] == "reset" + assert settle["params"]["entity_cfgs"] == [{"uid": "cube"}] + + +def test_gym_config_keeps_scene_and_robot_configuration() -> None: + """The migration changes the expert layer, not the physical environment.""" + payload = _gym_payload() + cfg = config_to_cfg(payload, source_path=_gym_config_path()) + + assert isinstance(cfg.robot, URRobotCfg) + assert cfg.robot.robot_type == "ur5" + assert cfg.robot.control_parts["arm"] == [ + "joint1", + "joint2", + "joint3", + "joint4", + "joint5", + "joint6", + ] + assert cfg.robot.control_parts["hand"] == ["gripper_finger1_joint_1"] + assert cfg.rigid_object[0].uid == "cube" + + +def test_direct_default_cfg_loads_the_same_typed_program() -> None: + """Direct Python construction and Gym startup share one packaged program.""" + cfg = _create_default_env_cfg() + + assert cfg.expert_program is not None + assert cfg.expert_program.integration.scene_registry == CUBE_SCENE_REGISTRY_ID + assert cfg.expert_program.integration.robot_profile == CUBE_ROBOT_PROFILE_ID + assert cfg.expert_program.program_id == "repeated_cube_pick_place" + settle = cfg.events["settle_cube_on_reset"] + assert settle.func is not None + assert settle.params["entity_cfgs"][0].uid == "cube" + + +def test_robot_profile_calibrates_physical_motion_and_tracking() -> None: + """The UR5 preset slows motion without weakening feedback or recovery.""" + binding = create_cube_robot_profile_binding() + + assert binding.presets[0].preset_id == "safe" + assert binding.presets[0].motion_policy.sample_count == 100 + tracking = binding.presets[0].tracking_policy + assert tracking.in_flight is not None + assert tracking.in_flight.metrics[0].tolerance == 0.08 + assert tracking.terminal.metrics[0].tolerance == 0.08 + assert binding.presets[0].workflow_recovery_policy.max_recovery_attempts == 2 + + +def test_task_initialization_delegates_to_shared_simulation_factory( + monkeypatch, +) -> None: + """Task setup contributes bindings but no task-local motion generator.""" + adapter = object() + captured: dict[str, object] = {} + + def fake_base_init(self, cfg, **kwargs) -> None: + del cfg, kwargs + self.grasp_samples = 48 + self.force_reannotate = True + + 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(MultiSegmentsCubePickPlaceEnv.__module__) + monkeypatch.setattr( + task_module, + "create_simulation_expert_program_adapter", + fake_create_adapter, + ) + + env = MultiSegmentsCubePickPlaceEnv(cfg=object()) + + assert env.expert_program_adapter is adapter + assert captured["environment"] is env + registration = captured["registration"] + assert registration is CUBE_EXPERT_PROGRAM_REGISTRATION + assert ( + registration.scene_binding.antipodal_grasps[ + 0 + ].generator_cfg.antipodal_sampler_cfg.n_sample + == 10000 + ) + assert registration.scene_binding.antipodal_grasps[0].force_reannotate is False + assert registration.robot_profile_binding.profile_id == CUBE_ROBOT_PROFILE_ID + + +def test_task_config_compiles_through_real_simulation_factory( + monkeypatch, +) -> None: + """Packaged config reaches the real adapter with explicitly bound mocks.""" + + class FakeRobot: + uid = "UR5" + + @staticmethod + def get_qpos(*, target: bool = False) -> torch.Tensor: + del target + return torch.zeros((1, 8), dtype=torch.float32) + + class FakeCube: + is_non_dynamic = False + + @staticmethod + def get_vertices(*, env_ids, scale) -> torch.Tensor: + assert env_ids == [0] + assert scale is True + return torch.tensor( + [[[-0.5, -0.5, 0.0], [0.5, -0.5, 0.0], [0.0, 0.5, 0.0]]], + dtype=torch.float32, + ) + + @staticmethod + def get_triangles(*, env_ids) -> torch.Tensor: + assert env_ids == [0] + return torch.tensor([[[0, 1, 2]]], dtype=torch.int64) + + @staticmethod + def get_local_pose(*, to_matrix) -> torch.Tensor: + assert to_matrix is True + return torch.eye(4, dtype=torch.float32).unsqueeze(0) + + robot = FakeRobot() + cube = FakeCube() + + class FakeSimulation: + @staticmethod + def get_robot(uid: str): + return robot if uid == "UR5" else None + + @staticmethod + def get_rigid_object(uid: str): + return cube if uid == "cube" else None + + 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 + for name, value in cfg.extensions.items(): + setattr(self, name, value) + + monkeypatch.setattr(EmbodiedEnv, "__init__", fake_base_init) + cfg = _create_default_env_cfg() + + env = MultiSegmentsCubePickPlaceEnv(cfg=cfg) + segments = tuple(env.compile_expert_program(cfg.expert_program)) + + assert len(segments) == 3 + assert [segment.name for segment in segments] == ["move_cube"] * 3 + assert env.expert_program_adapter.scene_registry_id == CUBE_SCENE_REGISTRY_ID + assert env.expert_program_adapter.robot_profile_id == CUBE_ROBOT_PROFILE_ID + + +__all__: list[str] = [] diff --git a/tests/gym/envs/tasks/test_open_drawer.py b/tests/gym/envs/tasks/test_open_drawer.py new file mode 100644 index 000000000..acb4293d1 --- /dev/null +++ b/tests/gym/envs/tasks/test_open_drawer.py @@ -0,0 +1,312 @@ +# ---------------------------------------------------------------------------- +# 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 drawer-opening 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 ExpertProgramEnvironmentMixin +from embodichain.lab.gym.utils.gym_utils import config_to_cfg +from embodichain.lab.gym.utils.registration import ( + REGISTERED_ENVS, + discover_task_packages, +) + +# Trigger official task auto-registration (idempotent). +discover_task_packages() + +from embodichain_tasks.tableware.open_drawer import ( # noqa: E402 + DRAWER_NATIVE_SLIDE_JOINT, + DRAWER_OPEN_POSITION, + DRAWER_ROBOT_PROFILE_ID, + DRAWER_UID, + OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION, + OpenDrawerEnv, + create_open_drawer_scene_binding, +) + + +def _gym_config_path() -> Path: + """Return the installed-source drawer Gym config path.""" + return ( + Path(__file__).parents[4] + / "embodichain_tasks/configs/gym/open_drawer/cobot_magic_3cam.json" + ) + + +def _gym_payload() -> dict[str, object]: + """Load the drawer 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_drawer_task_uses_shared_expert_program_mixin() -> None: + """The environment delegates all demo generation to the shared runtime.""" + spec = REGISTERED_ENVS["OpenDrawer-v1"] + + assert spec.cls is OpenDrawerEnv + assert spec.expert_program_registration is OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION + assert "expert_program_registration" not in spec.default_kwargs + assert issubclass(OpenDrawerEnv, ExpertProgramEnvironmentMixin) + assert issubclass(OpenDrawerEnv, EmbodiedEnv) + assert "create_demo_action_list" not in OpenDrawerEnv.__dict__ + + +def test_drawer_gym_config_selects_packaged_semantic_program() -> None: + """The runnable task config points at the named-target Expert Program.""" + payload = _gym_payload() + + assert payload["id"] == "OpenDrawer-v1" + assert payload["expert_program_path"] == ( + "../../expert_program/tableware/open_drawer.json" + ) + assert payload["env"]["extensions"] == {} + + +def test_drawer_gym_config_preserves_physical_scene() -> None: + """Parsing still creates the CobotMagic robot and native drawer entity.""" + path = _gym_config_path() + cfg = config_to_cfg(_gym_payload(), source_path=path) + + assert cfg.robot.uid == "CobotMagic" + assert cfg.robot.control_parts["right_arm"] == [ + "right_joint1", + "right_joint2", + "right_joint3", + "right_joint4", + "right_joint5", + "right_joint6", + ] + assert cfg.robot.control_parts["right_eef"] == [ + "right_joint7", + "right_joint8", + ] + assert cfg.articulation[0].uid == "drawer" + assert cfg.expert_program is not None + assert cfg.expert_program.program_id == "open_drawer" + + +def test_drawer_affordance_uses_reachable_post_release_retract() -> None: + """The opened drawer retract remains clear of the handle and IK-reachable.""" + operation = create_open_drawer_scene_binding().articulation_operations[0] + contact_z = operation.contact_offset[11] + retract_z = operation.retract_offset[11] + + assert retract_z < contact_z + assert contact_z - retract_z == pytest.approx(0.01) + + +def test_task_initialization_delegates_to_shared_simulation_factory( + monkeypatch, +) -> None: + """Drawer setup contributes declarations but no planner implementation.""" + 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(OpenDrawerEnv.__module__) + monkeypatch.setattr( + task_module, + "create_simulation_expert_program_adapter", + fake_create_adapter, + ) + + env = OpenDrawerEnv(cfg=object()) + + assert env.expert_program_adapter is adapter + assert captured["environment"] is env + registration = captured["registration"] + assert registration is OPEN_DRAWER_EXPERT_PROGRAM_REGISTRATION + assert registration.scene_binding.links[0].native_link_name == "handle_xpos" + assert registration.robot_profile_binding.profile_id == DRAWER_ROBOT_PROFILE_ID + + +def test_task_config_compiles_through_real_simulation_factory( + monkeypatch, +) -> None: + """Packaged drawer config reaches the real adapter with explicit mocks.""" + + class FakeRobot: + uid = "CobotMagic" + + @staticmethod + def get_qpos(*, target: bool = False) -> torch.Tensor: + del target + return torch.zeros((1, 16), dtype=torch.float32) + + class FakeDrawer: + link_names = ("outer_box", "inner_box", "handle_xpos") + joint_names = ("slide_rails",) + + @staticmethod + def get_local_pose(*, to_matrix) -> torch.Tensor: + assert to_matrix is True + return torch.eye(4, dtype=torch.float32).unsqueeze(0) + + @staticmethod + def get_link_pose(name: str, *, env_ids, to_matrix) -> torch.Tensor: + assert name == "handle_xpos" + assert env_ids == [0] + assert to_matrix is True + return torch.eye(4, dtype=torch.float32).unsqueeze(0) + + robot = FakeRobot() + drawer = FakeDrawer() + + class FakeSimulation: + @staticmethod + def get_robot(uid: str): + return robot if uid == "CobotMagic" else None + + @staticmethod + def get_articulation(uid: str): + return drawer if uid == "drawer" else None + + 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) + path = _gym_config_path() + cfg = config_to_cfg(_gym_payload(), source_path=path) + + env = OpenDrawerEnv(cfg=cfg) + segments = tuple(env.compile_expert_program(cfg.expert_program)) + + assert len(segments) == 1 + assert segments[0].name == "open_drawer" + assert env.expert_program_adapter.scene_registry_id == "open_drawer_v1" + assert env.expert_program_adapter.robot_profile_id == DRAWER_ROBOT_PROFILE_ID + + +@pytest.mark.requires_sim +@pytest.mark.slow +def test_real_sim_expert_episode_opens_drawer_with_joint_effect_trace() -> None: + """The packaged program completes against live drawer physics and evidence.""" + import gc + + from embodichain.lab.sim import SimulationManager, SimulationManagerCfg + + path = _gym_config_path() + cfg = config_to_cfg(_gym_payload(), source_path=path) + cfg.num_envs = 1 + cfg.sim_cfg = SimulationManagerCfg( + headless=True, + sim_device="cpu", + num_envs=1, + ) + cfg.sensor = [] + cfg.events = None + cfg.observations = None + cfg.dataset = None + cfg.init_rollout_buffer = False + cfg.record_trajectory = False + cfg.filter_dataset_saving = True + + env: OpenDrawerEnv | None = None + try: + env = OpenDrawerEnv(cfg=cfg) + env.reset(seed=0) + + result = execute_demo_episode(env) + + assert result.completed + assert result.all_success + assert result.terminal_reason == "success" + assert len(result.segments) == 1 + segment = result.segments[0] + assert segment.name == "open_drawer" + assert segment.success + + metadata = segment.metadata + runtime = metadata["runtime"] + assert runtime["kind"] == "skill_result" + assert runtime["status"] == "completed" + assert runtime["masks"]["success"] == [True] + assert len(runtime["calls"]) == 1 + call = runtime["calls"][0] + assert call["semantic_id"] == "operate_articulation" + 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] + + effects = call["effects"] + assert effects + for effect in effects: + assert effect["effect_spec"]["semantic_id"] == "operate_articulation" + evidence = effect["evidence"]["joint.position"] + assert evidence["valid_mask"] == [True] + assert evidence["acquisition_errors"] == [None] + assert evidence["env_ids"] == [0] + final_effect = effects[-1] + assert final_effect["decision"] == { + "success_mask": [True], + "failure_mask": [False], + } + + assert metadata["post_policies"] == [] + assert metadata["validation"] == { + "env_ids": [0], + "runtime_success_mask": [True], + "eligible_mask_before_validation": [True], + "post_policy_success_mask": None, + "validators": [], + "accepted_mask": [True], + } + + drawer = env.sim.get_articulation(DRAWER_UID) + assert drawer is not None + joint_index = drawer.joint_names.index(DRAWER_NATIVE_SLIDE_JOINT) + final_position = float(drawer.get_qpos()[0, joint_index].item()) + joint_tolerance = float( + final_effect["monitor"]["resolved_params"]["joint_success_tolerance"] + ) + assert abs(final_position - DRAWER_OPEN_POSITION) <= joint_tolerance + finally: + if env is not None: + env.close() + SimulationManager.flush_cleanup_queue() + gc.collect() + + +__all__: list[str] = [] diff --git a/tests/gym/envs/test_demo.py b/tests/gym/envs/test_demo.py index 1cbfcb29e..b0ef3a740 100644 --- a/tests/gym/envs/test_demo.py +++ b/tests/gym/envs/test_demo.py @@ -20,15 +20,99 @@ import threading from typing import Any +from unittest.mock import Mock import pytest import torch from tensordict import TensorDict -from embodichain.lab.gym.envs.demo import DemoSegment, execute_demo_episode +from embodichain.lab.gym.envs.demo import ( + DemoSegment, + DemoSegmentResult, + ProcessedEnvAction, + execute_demo_episode, +) from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv +def test_processed_env_action_owns_value_and_metadata() -> None: + value = torch.tensor([[1.0, 2.0]]) + metadata = {"semantic_id": "pick", "segments": ["approach"]} + + action = ProcessedEnvAction(value=value, metadata=metadata) + value.zero_() + metadata["segments"].append("close") + snapshot = action.snapshot() + + assert action.value.tolist() == [[1.0, 2.0]] + assert dict(action.metadata) == { + "semantic_id": "pick", + "segments": ["approach"], + } + assert snapshot is not action + assert snapshot.value is not action.value + + +def test_demo_segment_result_owns_json_safe_lifecycle_metadata() -> None: + metadata = { + "runtime": {"status": "completed"}, + "validation": {"accepted_mask": [True, False]}, + } + result = DemoSegmentResult( + segment_id=0, + name="place", + start_step=0, + end_step=2, + success=False, + metadata=metadata, + ) + + metadata["runtime"]["status"] = "mutated" + exported = result.to_metadata() + exported["metadata"]["validation"]["accepted_mask"][0] = False + + assert result.metadata["runtime"]["status"] == "completed" + assert result.metadata["validation"]["accepted_mask"] == [True, False] + + +def test_demo_segment_result_rejects_non_json_metadata() -> None: + with pytest.raises(TypeError, match="non-JSON value Tensor"): + DemoSegmentResult( + segment_id=0, + name="place", + start_step=0, + end_step=1, + success=True, + metadata={"mask": torch.tensor([True])}, + ) + + +def test_embodied_env_skips_preprocessing_for_processed_action() -> None: + env = object.__new__(EmbodiedEnv) + env._num_envs = 2 + env._traj_buffer = None + env.action_manager = Mock() + env._demo_no_auto_reset = False + action = ProcessedEnvAction(value=torch.ones(2, 3)) + + normalized = env._normalize_demo_action(action) + processed = env._preprocess_action(normalized) + + assert isinstance(normalized, ProcessedEnvAction) + assert normalized is not action + assert torch.equal(processed, action.value) + env.action_manager.process_action.assert_not_called() + + +def test_embodied_env_validates_processed_action_batch_size() -> None: + env = object.__new__(EmbodiedEnv) + env._num_envs = 2 + action = ProcessedEnvAction(value=torch.ones(1, 3)) + + with pytest.raises(ValueError, match="batch size"): + env._normalize_demo_action(action) + + class _SegmentedEnv: """Small environment stub that supports lazy two-segment planning.""" @@ -94,6 +178,142 @@ def test_execute_demo_episode_runs_lazy_segments_as_one_episode() -> None: assert not env._demo_no_auto_reset +class _LifecycleMetadataEnv: + """Populate one shared metadata mapping at lazy lifecycle boundaries.""" + + def __init__(self) -> None: + self.num_envs = 1 + self.lifecycle = {"runtime": None, "validation": None} + + def create_demo_segments(self): + def actions(): + yield 1 + self.lifecycle["runtime"] = {"status": "completed"} + + def validate() -> bool: + self.lifecycle["validation"] = {"accepted_mask": [True]} + return True + + return ( + DemoSegment( + actions=actions(), + name="lifecycle", + metadata=self.lifecycle, + validator=validate, + ), + ) + + def step(self, action: int): + del action + return ( + None, + torch.zeros(1), + torch.zeros(1, dtype=torch.bool), + torch.zeros(1, dtype=torch.bool), + {"success": torch.tensor([True])}, + ) + + def is_task_success(self) -> torch.Tensor: + return torch.tensor([True]) + + +class _EmptySuccessfulSegmentEnv: + """Expose an empty ordinary segment whose callbacks otherwise succeed.""" + + num_envs = 1 + + def __init__(self) -> None: + self.validator_calls = 0 + self.step_calls = 0 + + def create_demo_segments(self): + return ( + DemoSegment( + actions=(), + name="empty", + validator=self._validate, + ), + ) + + def _validate(self) -> bool: + self.validator_calls += 1 + return True + + def step(self, action: object): + del action + self.step_calls += 1 + raise AssertionError("An empty segment must not call env.step().") + + @staticmethod + def is_task_success() -> torch.Tensor: + return torch.tensor([True]) + + +def test_execute_demo_episode_snapshots_finalized_lifecycle_metadata() -> None: + env = _LifecycleMetadataEnv() + + result = execute_demo_episode(env) + env.lifecycle["runtime"]["status"] = "mutated" + + assert result.segments[0].metadata == { + "runtime": {"status": "completed"}, + "validation": {"accepted_mask": [True]}, + } + + +def test_empty_ordinary_segment_keeps_existing_empty_segment_guard() -> None: + env = _EmptySuccessfulSegmentEnv() + + result = execute_demo_episode(env) + + assert env.step_calls == 0 + assert env.validator_calls == 0 + assert not result.completed + assert result.terminal_reason == "empty_segment" + assert result.segments[0].failure_reason == "empty_segment" + + +class _GeneratorFailureEnv: + """Raise between lazy actions and expose an emergency hold callback.""" + + def __init__(self) -> None: + self.num_envs = 1 + self.actions: list[int] = [] + self.abort_calls: list[tuple[str, bool]] = [] + + def create_demo_segments(self): + def actions(): + yield 1 + raise ValueError("planner stream failed") + + def abort(reason: str, *, last_action_consumed: bool): + self.abort_calls.append((reason, last_action_consumed)) + yield 0 + + return (DemoSegment(actions=actions(), abort_actions=abort),) + + def step(self, action: int): + self.actions.append(action) + return ( + None, + torch.zeros(1), + torch.zeros(1, dtype=torch.bool), + torch.zeros(1, dtype=torch.bool), + {}, + ) + + +def test_action_generator_failure_safe_stops_before_propagating() -> None: + env = _GeneratorFailureEnv() + + with pytest.raises(RuntimeError, match="action generation") as error: + execute_demo_episode(env) + + assert isinstance(error.value.__cause__, ValueError) + assert env.actions == [1, 0] + assert env.abort_calls == [("action_generation_failed", True)] + + class _TerminatingEnv(_SegmentedEnv): def create_demo_segments(self): return (DemoSegment(actions=(1, 2, 3), name="pick"),) @@ -213,6 +433,30 @@ def test_vector_failure_aborts_peer_and_preserves_per_env_reason() -> None: assert result.lengths == (2, 2) +class _RowIndependentFailureEnv(_VectorFailureEnv): + def create_demo_segments(self): + return ( + DemoSegment( + actions=(1, 2, 3), + name="shared", + failure_policy="row_independent", + ), + ) + + +def test_row_independent_failure_freezes_only_failed_environment() -> None: + env = _RowIndependentFailureEnv() + + result = execute_demo_episode(env) + + assert env.actions == [1, 2, 3] + assert env.masked_actions == [(3, (False, True))] + assert result.completed_by_env == (False, True) + assert result.terminal_reasons == ("failure", "success") + assert result.success == (False, True) + assert result.lengths == (2, 3) + + class _ValidatedSegmentEnv(_SegmentedEnv): def __init__(self, validation: bool) -> None: super().__init__() @@ -388,6 +632,35 @@ def test_validator_batch_abort_has_consistent_peer_status() -> None: assert result.segments[0].failure_reason == "segment_validation_failed" +class _RowIndependentValidatorEnv(_VectorValidatorEnv): + def create_demo_segments(self): + return ( + DemoSegment( + actions=(1,), + name="validated", + validator=lambda: torch.tensor([True, False]), + failure_policy="row_independent", + ), + ) + + +def test_row_independent_validator_keeps_accepted_peer_active() -> None: + result = execute_demo_episode(_RowIndependentValidatorEnv()) + + assert result.segments[0].successes == (True, False) + assert result.segments[0].failure_reasons == ( + None, + "segment_validation_failed", + ) + assert result.completed_by_env == (True, False) + assert result.terminal_reasons == ("success", "segment_validation_failed") + + +def test_demo_segment_rejects_unknown_failure_policy() -> None: + with pytest.raises(ValueError, match="failure_policy"): + DemoSegment(actions=(1,), failure_policy="continue") + + class _CancellationEnv(_ValidatedSegmentEnv): def __init__(self) -> None: super().__init__(validation=True) diff --git a/tests/gym/envs/test_embodied_env_expert_program.py b/tests/gym/envs/test_embodied_env_expert_program.py new file mode 100644 index 000000000..fc7892b9f --- /dev/null +++ b/tests/gym/envs/test_embodied_env_expert_program.py @@ -0,0 +1,92 @@ +# ---------------------------------------------------------------------------- +# 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 explicit Expert Program environment integration hooks.""" + +from __future__ import annotations + +from types import SimpleNamespace + +import pytest + +from embodichain.lab.gym.envs.demo import DemoSegment +from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv, EmbodiedEnvCfg + + +class _FakeBridge: + """Minimal bridge protocol used by the environment adapter test.""" + + def __init__(self, segment: DemoSegment) -> None: + self._segment = segment + self.iteration_count = 0 + + def iter_segments(self): + """Yield the configured segment lazily.""" + self.iteration_count += 1 + yield self._segment + + +class _DeclarativeEnv(EmbodiedEnv): + """Environment stub with explicit compiler and bridge factories.""" + + def compile_expert_program(self, program): + self.compiled_input = program + return self.compiled_program + + def create_expert_program_bridge(self, program): + self.bridge_input = program + return self.bridge + + +def _uninitialized_env(cls: type[EmbodiedEnv], expert_program: object) -> EmbodiedEnv: + """Create an environment instance without starting simulation.""" + env = object.__new__(cls) + env.cfg = SimpleNamespace(expert_program=expert_program) + return env + + +def test_embodied_env_cfg_disables_expert_program_by_default() -> None: + """Declarative execution remains an explicit opt-in configuration.""" + cfg = EmbodiedEnvCfg() + + assert cfg.expert_program is None + + +def test_create_demo_segments_uses_explicit_compiler_and_bridge_hooks() -> None: + """Configured programs flow through provider and runtime factories lazily.""" + program = object() + compiled_program = object() + expected_segment = DemoSegment(actions=(), name="declarative") + bridge = _FakeBridge(expected_segment) + env = _uninitialized_env(_DeclarativeEnv, program) + env.compiled_program = compiled_program + env.bridge = bridge + + segments = env.create_demo_segments(debug_mode=True) + + assert bridge.iteration_count == 0 + assert tuple(segments) == (expected_segment,) + assert bridge.iteration_count == 1 + assert env.compiled_input is program + assert env.bridge_input is compiled_program + + +def test_configured_program_requires_explicit_scene_provider_hook() -> None: + """The base environment never guesses a live scene provider.""" + env = _uninitialized_env(EmbodiedEnv, object()) + + with pytest.raises(NotImplementedError, match="explicit scene resolver"): + env.create_demo_segments() diff --git a/tests/gym/envs/test_initialization_summary.py b/tests/gym/envs/test_initialization_summary.py new file mode 100644 index 000000000..341c49b75 --- /dev/null +++ b/tests/gym/envs/test_initialization_summary.py @@ -0,0 +1,164 @@ +# ---------------------------------------------------------------------------- +# 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 environment initialization summary.""" + +from __future__ import annotations + +from types import SimpleNamespace + +import pytest +import torch + +import embodichain.lab.gym.envs.base_env as base_env_module +from embodichain.lab.gym.envs import EmbodiedEnv + +pytestmark = pytest.mark.no_sim + + +class _SummaryEnv(EmbodiedEnv): + """Environment stub used to exercise summary formatting only.""" + + +class _SummaryCfg: + """Minimal environment configuration needed by the summary.""" + + seed = 42 + sim_steps_per_control = 4 + max_episode_steps = 300 + + +class _RobotStub: + """Minimal robot carrying the identity shown in the summary.""" + + uid = "test_arm" + + +class _ManagerStub: + """Manager stub exposing the common active-functor contract.""" + + def __init__(self, active_functors: dict[str, list[str]]) -> None: + self.active_functors = active_functors + + +class _ActionManagerStub: + """Action-manager stub exposing terms by processing mode.""" + + active_functors = ["delta_qpos", "smooth_action"] + + def get_terms_by_mode(self, mode: str) -> list[tuple[str, object]]: + terms = { + "pre": [("delta_qpos", object())], + "post": [("smooth_action", object())], + } + return terms[mode] + + +def _make_summary_env() -> _SummaryEnv: + """Create a fully populated environment shell without starting simulation.""" + env = object.__new__(_SummaryEnv) + env.cfg = _SummaryCfg() + env.sim_cfg = SimpleNamespace(physics_dt=0.005, headless=True) + env.sim = SimpleNamespace(device=torch.device("cuda:0")) + env._num_envs = 8 + env.robot = _RobotStub() + env.sensors = {"front_camera": object(), "wrist_camera": object()} + env.metadata = { + "render_fps": 50.0, + "task_type": "manipulation", + "dataset": { + "instruction": "Pick up the red cube", + "robot_meta": {"model": "test_arm"}, + }, + } + env.event_manager = _ManagerStub( + { + "startup": ["load_scene"], + "reset": ["reset_robot", "randomize_objects"], + } + ) + env.observation_manager = _ManagerStub( + {"modify": ["normalize_rgb"], "add": ["task_state"]} + ) + env.reward_manager = None + env.action_manager = _ActionManagerStub() + env.dataset_manager = _ManagerStub({"save": ["record_episode"]}) + return env + + +def test_summary_includes_runtime_metadata_and_every_manager_functor() -> None: + """The summary exposes key runtime facts and every configured functor.""" + lines = _make_summary_env()._initialization_summary_lines() + rendered = "\n".join(lines) + normalized_lines = {" ".join(line.split()) for line in lines} + + assert rendered.startswith("╭─ Environment initialized: _SummaryEnv") + assert "├─ Runtime" in rendered + assert "Config _SummaryCfg" in rendered + assert "Device cuda:0" in rendered + assert "Parallel environments 8" in rendered + assert "Robot _RobotStub (uid=test_arm)" in rendered + assert "Sensors 2 (front_camera, wrist_camera)" in rendered + assert "├─ Timing" in rendered + assert "Physics 0.005 s (200 Hz)" in rendered + assert "Control 0.02 s (50 Hz, 4 physics steps)" in rendered + assert "├─ Metadata" in rendered + assert "dataset 2 keys (instruction, robot_meta)" in rendered + assert "render_fps" not in rendered + assert "Pick up the red cube" not in rendered + assert "├─ Managers (4/5 active, 8 functors)" in rendered + assert "EventManager 3 functors" in rendered + assert "│ startup load_scene" in normalized_lines + assert "│ reset reset_robot, randomize_objects" in normalized_lines + assert "ObservationManager 2 functors" in rendered + assert "│ modify normalize_rgb" in normalized_lines + assert "│ add task_state" in normalized_lines + assert "RewardManager disabled" in rendered + assert "ActionManager 2 functors" in rendered + assert "│ pre delta_qpos" in normalized_lines + assert "│ post smooth_action" in normalized_lines + assert "DatasetManager 1 functor" in rendered + assert "│ save record_episode" in normalized_lines + assert rendered.endswith("╰─ Ready") + + +def test_summary_omits_metadata_section_for_render_fps_only() -> None: + """Gym's internal render cadence does not create a metadata section.""" + env = _make_summary_env() + env.metadata = {"render_fps": 50.0} + + rendered = "\n".join(env._initialization_summary_lines()) + + assert "├─ Metadata" not in rendered + + +def test_summary_logs_tree_as_one_record_without_prefix( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """The complete tree is emitted once without standard log columns.""" + env = _make_summary_env() + calls: list[tuple[object, dict[str, object]]] = [] + + def capture(message: object, **kwargs: object) -> None: + calls.append((message, kwargs)) + + monkeypatch.setattr(base_env_module.logger, "log_info", capture) + + env._log_initialization_summary() + + assert calls == [ + ("\n".join(env._initialization_summary_lines()), {"prefix": False}) + ] diff --git a/tests/gym/envs/test_settling.py b/tests/gym/envs/test_settling.py new file mode 100644 index 000000000..d1476b842 --- /dev/null +++ b/tests/gym/envs/test_settling.py @@ -0,0 +1,145 @@ +# ---------------------------------------------------------------------------- +# 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 pytest +import torch + +from embodichain.lab.gym.envs.settling import ( + DynamicSettleMonitor, + DynamicSettleMonitorCfg, + DynamicSettleSample, +) + + +def _sample( + linear: tuple[float, ...], angular: tuple[float, ...] +) -> DynamicSettleSample: + return DynamicSettleSample( + entity_id="cube", + linear_speed=torch.tensor(linear, dtype=torch.float32).unsqueeze(1), + angular_speed=torch.tensor(angular, dtype=torch.float32).unsqueeze(1), + ) + + +def test_settle_monitor_tracks_rows_independently_and_owns_metadata() -> None: + env_ids = torch.tensor([4, 9], dtype=torch.long) + monitor = DynamicSettleMonitor( + DynamicSettleMonitorCfg( + min_steps=1, + max_steps=5, + check_interval_steps=1, + required_stable_checks=2, + ), + env_ids, + ) + + first = monitor.observe((_sample((0.0, 1.0), (0.0, 1.0)),), elapsed_steps=1) + second = monitor.observe((_sample((0.0, 1.0), (0.0, 1.0)),), elapsed_steps=2) + third = monitor.observe((_sample((0.0, 0.0), (0.0, 0.0)),), elapsed_steps=3) + final = monitor.observe((_sample((0.0, 0.0), (0.0, 0.0)),), elapsed_steps=4) + + assert first.stable_counts.tolist() == [1, 0] + assert second.settled_mask.tolist() == [True, False] + assert third.stable_counts.tolist() == [2, 1] + assert final.settled_mask.tolist() == [True, True] + assert final.timeout_mask.tolist() == [False, False] + assert final.complete is True + metadata = final.to_metadata() + assert metadata["env_ids"] == [4, 9] + assert metadata["settled_mask"] == [True, True] + + env_ids[0] = 100 + assert monitor.env_ids.tolist() == [4, 9] + + +def test_settle_monitor_duplicate_observation_is_idempotent() -> None: + monitor = DynamicSettleMonitor( + DynamicSettleMonitorCfg( + min_steps=0, + max_steps=3, + check_interval_steps=1, + required_stable_checks=2, + ), + torch.tensor([0], dtype=torch.long), + ) + + first = monitor.observe((_sample((0.0,), (0.0,)),), elapsed_steps=0) + duplicate = monitor.observe((_sample((0.0,), (0.0,)),), elapsed_steps=0) + second = monitor.observe((_sample((0.0,), (0.0,)),), elapsed_steps=1) + + assert first.checked is True + assert duplicate.checked is False + assert duplicate.stable_counts.tolist() == [1] + assert second.settled_mask.tolist() == [True] + assert second.observation_count == 2 + + +def test_settle_monitor_marks_only_unresolved_rows_timed_out() -> None: + monitor = DynamicSettleMonitor( + DynamicSettleMonitorCfg( + min_steps=0, + max_steps=1, + check_interval_steps=1, + required_stable_checks=1, + ), + torch.tensor([0, 1], dtype=torch.long), + ) + + state = monitor.observe((_sample((0.0, 1.0), (0.0, 1.0)),), elapsed_steps=1) + + assert state.settled_mask.tolist() == [True, False] + assert state.timeout_mask.tolist() == [False, True] + assert state.complete is True + + +@pytest.mark.parametrize( + ("kwargs", "match"), + ( + ({"min_steps": -1}, "min_steps"), + ({"max_steps": 1, "min_steps": 2}, "max_steps"), + ({"check_interval_steps": 0}, "check_interval_steps"), + ({"linear_velocity_threshold": float("nan")}, "linear_velocity_threshold"), + ( + {"min_steps": 0, "max_steps": 0, "required_stable_checks": 2}, + "cannot be reached", + ), + ), +) +def test_settle_monitor_cfg_rejects_invalid_values( + kwargs: dict[str, object], match: str +) -> None: + with pytest.raises((TypeError, ValueError), match=match): + DynamicSettleMonitorCfg(**kwargs) + + +def test_settle_monitor_rejects_regressing_steps_and_incomplete_samples() -> None: + monitor = DynamicSettleMonitor( + DynamicSettleMonitorCfg( + min_steps=0, + max_steps=2, + required_stable_checks=1, + ), + torch.tensor([0], dtype=torch.long), + ) + sample = _sample((1.0,), (1.0,)) + monitor.observe((sample,), elapsed_steps=1) + + with pytest.raises(ValueError, match="monotonic"): + monitor.observe((sample,), elapsed_steps=0) + with pytest.raises(ValueError, match="contain DynamicSettleSample"): + monitor.observe((), elapsed_steps=2) diff --git a/tests/gym/utils/test_gym_utils.py b/tests/gym/utils/test_gym_utils.py index 12d46874b..c0cd8ee2d 100644 --- a/tests/gym/utils/test_gym_utils.py +++ b/tests/gym/utils/test_gym_utils.py @@ -27,6 +27,7 @@ from tensordict import TensorDict +from embodichain.lab.gym.envs.expert_program import IntegrationFingerprintMismatch from embodichain.lab.gym.utils.gym_utils import ( add_env_launcher_args_to_parser, build_env_cfg_from_args, @@ -39,6 +40,11 @@ ) from embodichain.lab.sim.robots import URRobotCfg from embodichain.utils.utility import load_config, save_config +from embodichain_tasks.multi_segments.cube_pick_place import ( + CUBE_EXPERT_PROGRAM_REGISTRATION, + CUBE_ROBOT_PROFILE_ID, + CUBE_SCENE_REGISTRY_ID, +) class TestInitRolloutBufferFromConfig: @@ -509,6 +515,37 @@ def test_different_max_episode_steps(): class TestConfigToCfgFromFile: + @staticmethod + def _minimal_gym_config() -> dict[str, object]: + """Return a minimal config that reaches the generic parser.""" + return { + "id": "MultiSegmentsCubePickPlace-v1", + "env": {}, + "robot": { + "class_type": "URRobot", + "robot_type": "ur5", + "uid": "TestUR5", + }, + } + + @staticmethod + def _expert_program_payload() -> dict[str, object]: + """Return one minimal strict Expert Program payload.""" + return { + "schema_version": 1, + "program_id": "configured_pick", + "integration": { + "robot_profile": CUBE_ROBOT_PROFILE_ID, + "scene_registry": CUBE_SCENE_REGISTRY_ID, + "runtime_preset": "safe", + }, + "targets": {}, + "program": { + "kind": "invoke", + "call": {"kind": "pick", "object": "cube"}, + }, + } + def test_robot_class_type_preserves_ur_variant(self): config = { "id": "EmbodiedEnv-v1", @@ -532,6 +569,182 @@ def test_robot_class_type_preserves_ur_variant(self): "uid": "TestUR5", } + def test_expert_program_path_is_resolved_from_gym_config_source( + self, + tmp_path, + ) -> None: + """A serialized program path is relative to its Gym config file.""" + gym_dir = tmp_path / "gym" / "task" + program_dir = tmp_path / "expert_program" + gym_dir.mkdir(parents=True) + program_dir.mkdir() + gym_path = gym_dir / "gym_config.json" + program_path = program_dir / "program.yaml" + save_config(program_path, self._expert_program_payload()) + config = self._minimal_gym_config() + config["expert_program_path"] = "../../expert_program/program.yaml" + + cfg = config_to_cfg( + config, + manager_modules=DEFAULT_MANAGER_MODULES, + source_path=gym_path, + ) + + assert cfg.expert_program.program_id == "configured_pick" + assert cfg.expert_program.integration.scene_registry == CUBE_SCENE_REGISTRY_ID + + def test_build_env_cfg_loads_source_relative_expert_program( + self, + tmp_path, + ) -> None: + """The normal file launcher attaches the decoded program before init.""" + gym_dir = tmp_path / "gym" + program_dir = tmp_path / "programs" + gym_dir.mkdir() + program_dir.mkdir() + gym_path = gym_dir / "gym_config.json" + program_path = program_dir / "program.json" + save_config(program_path, self._expert_program_payload()) + config = self._minimal_gym_config() + config["expert_program_path"] = "../programs/program.json" + save_config(gym_path, config) + args = argparse.Namespace( + gym_config=str(gym_path), + num_envs=1, + device="cpu", + headless=True, + renderer=None, + gpu_id=0, + arena_space=2.0, + max_episodes=None, + filter_visual_rand=False, + filter_dataset_saving=False, + preview=False, + action_config=None, + ) + + cfg, _, _ = build_env_cfg_from_args(args) + + assert cfg.expert_program.program_id == "configured_pick" + + def test_cli_program_override_is_selected_and_loaded_once( + self, + tmp_path, + monkeypatch, + ) -> None: + """The CLI override replaces the Gym path at the single loader boundary.""" + from embodichain.lab.gym.envs.expert_program import loader + + gym_path = tmp_path / "gym_config.json" + override_path = tmp_path / "override.yaml" + save_config(override_path, self._expert_program_payload()) + config = self._minimal_gym_config() + config["expert_program_path"] = "must_not_be_loaded.yaml" + save_config(gym_path, config) + args = argparse.Namespace( + gym_config=str(gym_path), + expert_program=str(override_path), + num_envs=1, + device="cpu", + headless=True, + renderer=None, + gpu_id=0, + arena_space=2.0, + max_episodes=None, + filter_visual_rand=False, + filter_dataset_saving=False, + preview=False, + action_config=None, + ) + calls: list[str] = [] + original = loader.load_expert_program + + def load_once(path, **kwargs): + calls.append(str(path)) + return original(path, **kwargs) + + monkeypatch.setattr(loader, "load_expert_program", load_once) + + cfg, _, _ = build_env_cfg_from_args(args) + + assert cfg.expert_program.program_id == "configured_pick" + assert calls == [str(override_path)] + + def test_registration_drift_fails_before_program_loader( + self, + tmp_path, + monkeypatch, + ) -> None: + """The config boundary checks registration integrity before file loading.""" + from embodichain.lab.gym.envs.expert_program import loader + + program_path = tmp_path / "program.yaml" + save_config(program_path, self._expert_program_payload()) + config = self._minimal_gym_config() + config["expert_program_path"] = str(program_path) + generator_cfg = CUBE_EXPERT_PROGRAM_REGISTRATION.scene_binding.antipodal_grasps[ + 0 + ].generator_cfg + assert generator_cfg is not None + sampler_cfg = generator_cfg.antipodal_sampler_cfg + monkeypatch.setattr(sampler_cfg, "n_sample", sampler_cfg.n_sample + 1) + loader_calls: list[str] = [] + + def unexpected_load(path, **kwargs): + del kwargs + loader_calls.append(str(path)) + raise AssertionError("Drift must fail before program loading.") + + monkeypatch.setattr(loader, "load_expert_program", unexpected_load) + + with pytest.raises(IntegrationFingerprintMismatch, match="changed"): + config_to_cfg(config, manager_modules=DEFAULT_MANAGER_MODULES) + + assert loader_calls == [] + + def test_config_to_cfg_uses_cwd_without_source_path( + self, + tmp_path, + monkeypatch, + ) -> None: + """Dictionary-only callers retain explicit current-directory semantics.""" + program_path = tmp_path / "program.yaml" + save_config(program_path, self._expert_program_payload()) + monkeypatch.chdir(tmp_path) + config = self._minimal_gym_config() + config["expert_program_path"] = "program.yaml" + + cfg = config_to_cfg(config, manager_modules=DEFAULT_MANAGER_MODULES) + + assert cfg.expert_program.program_id == "configured_pick" + + @pytest.mark.parametrize("value", [None, True, 1, {}, "", " program.yaml"]) + def test_expert_program_path_rejects_ambiguous_values( + self, + value, + ) -> None: + """The path field never accepts coercion, null, or outer whitespace.""" + config = self._minimal_gym_config() + config["expert_program_path"] = value + + with pytest.raises((TypeError, ValueError), match="expert_program_path"): + config_to_cfg(config, manager_modules=DEFAULT_MANAGER_MODULES) + + def test_expert_program_path_missing_file_fails_before_environment_init( + self, + tmp_path, + ) -> None: + """A configured program must exist when the Gym config is decoded.""" + config = self._minimal_gym_config() + config["expert_program_path"] = "missing.yaml" + + with pytest.raises(FileNotFoundError, match="missing.yaml"): + config_to_cfg( + config, + manager_modules=DEFAULT_MANAGER_MODULES, + source_path=tmp_path / "gym_config.json", + ) + def test_yaml_gym_config_parses_to_cfg(self, tmp_path): config = { "id": "EmbodiedEnv-v1", diff --git a/tests/lab/scripts/test_run_env.py b/tests/lab/scripts/test_run_env.py index 0c495a4f5..1b0b1d1d8 100644 --- a/tests/lab/scripts/test_run_env.py +++ b/tests/lab/scripts/test_run_env.py @@ -16,6 +16,7 @@ from __future__ import annotations +import json from types import SimpleNamespace from unittest.mock import MagicMock @@ -23,6 +24,9 @@ import torch from embodichain.lab.gym.envs.demo import DemoEpisodeResult +from embodichain.lab.gym.envs.expert_program.loader import ( + load_expert_program as _load_expert_program, +) from embodichain.lab.gym.utils.gym_utils import merge_args_with_gym_config from embodichain.lab.scripts import run_env from embodichain.lab.scripts.run_env import ( @@ -40,6 +44,24 @@ VISER_POLL_INTERVAL = 0.05 +def _expert_program_payload() -> dict[str, object]: + """Return one minimal strict Expert Program payload.""" + return { + "schema_version": 1, + "program_id": "cli_pick", + "integration": { + "robot_profile": "default_robot", + "scene_registry": "default_scene", + "runtime_preset": "default_runtime", + }, + "targets": {}, + "program": { + "kind": "invoke", + "call": {"kind": "pick", "object": "cube"}, + }, + } + + class _LegacyProgressEnv: num_envs = 1 @@ -123,6 +145,86 @@ def test_run_env_preserves_configured_viser_image_fps() -> None: assert merged["visualization"]["sensor_image_fps"] == configured_fps +def test_run_env_parser_accepts_expert_program_path() -> None: + """The declarative program is an explicit, opt-in CLI input.""" + program_path = "program.yaml" + + args = _create_parser().parse_args( + ["--gym_config", GYM_CONFIG_PATH, "--expert-program", program_path] + ) + + assert args.expert_program == program_path + + +def test_run_env_parser_accepts_debug_trace_mode() -> None: + """Failed Expert Program attempts can expose their structured trace.""" + args = _create_parser().parse_args( + ["--gym_config", GYM_CONFIG_PATH, "--debug-mode"] + ) + + assert args.debug_mode is True + + +@pytest.mark.parametrize("suffix", [".json", ".yaml", ".yml"]) +def test_load_expert_program_safely_decodes_supported_files( + tmp_path, + suffix: str, +) -> None: + """JSON and safe YAML inputs share the same strict schema decoder.""" + path = tmp_path / f"program{suffix}" + payload = _expert_program_payload() + if suffix == ".json": + serialized = json.dumps(payload) + else: + import yaml + + serialized = yaml.safe_dump(payload) + path.write_text(serialized, encoding="utf-8") + + program = _load_expert_program(path) + + assert program.program_id == "cli_pick" + assert program.integration.scene_registry == "default_scene" + + +@pytest.mark.parametrize( + ("filename", "serialized", "message"), + [ + ( + "program.json", + '{"schema_version": 1, "schema_version": 1}', + "Duplicate JSON key", + ), + ( + "program.yaml", + "schema_version: 1\nschema_version: 1\n", + "found duplicate key", + ), + ], +) +def test_load_expert_program_rejects_duplicate_mapping_keys( + tmp_path, + filename: str, + serialized: str, + message: str, +) -> None: + """Ambiguous duplicate keys are rejected before schema decoding.""" + path = tmp_path / filename + path.write_text(serialized, encoding="utf-8") + + with pytest.raises(ValueError, match=message): + _load_expert_program(path) + + +def test_load_expert_program_rejects_unsupported_file_extension(tmp_path) -> None: + """Only explicit JSON and YAML file formats are accepted.""" + path = tmp_path / "program.toml" + path.write_text("schema_version = 1", encoding="utf-8") + + with pytest.raises(ValueError, match=".json, .yaml, or .yml"): + _load_expert_program(path) + + def test_replay_restores_wrapper_state_without_closing_caller_env(monkeypatch) -> None: """Replay leaves the environment close to its CLI owner.""" env = MagicMock() @@ -230,6 +332,34 @@ def test_generate_function_discards_retry_then_commits_once(monkeypatch) -> None assert env.reset_options == [{"save_data": False}, None] +def test_generate_function_logs_failed_trace_in_debug_mode(monkeypatch) -> None: + """Debug retries expose the owned structured episode trace.""" + env = _ResetTrackingEnv() + result = _episode_result(success=False, reason="segment_validation_failed") + warnings: list[str] = [] + monkeypatch.setattr( + "embodichain.lab.scripts.run_env.execute_demo_episode", + lambda *args, **kwargs: result, + ) + monkeypatch.setattr( + "embodichain.lab.scripts.run_env.log_warning", + warnings.append, + ) + + generated = generate_function( + env, + max_attempts=1, + reset_before=False, + debug_mode=True, + ) + + assert not generated + debug_trace = next( + message for message in warnings if "Failed demo trace" in message + ) + assert '"terminal_reason":"segment_validation_failed"' in debug_trace + + def test_generate_function_commits_failed_episode_when_configured(monkeypatch) -> None: """A recorded task failure is a persisted result when explicitly enabled.""" env = _ResetTrackingEnv(save_failed_episodes=True) @@ -421,6 +551,46 @@ def test_cli_aborts_before_closing_environment_once(monkeypatch) -> None: assert env.events == [abort_event, abort_event, ("close", None)] +def test_cli_uses_program_already_loaded_by_config_builder( + monkeypatch, +) -> None: + """The CLI attaches the strict program config to the environment config.""" + env = _LifecycleTrackingEnv() + env_cfg = SimpleNamespace(expert_program=None) + decoded_program = object() + args = SimpleNamespace( + replay=False, + replay_mode="kinematic", + preview=True, + expert_program="program.yaml", + ) + parser = MagicMock() + parser.parse_args.return_value = args + make = MagicMock(return_value=env) + + monkeypatch.setattr(run_env, "_create_parser", lambda: parser) + monkeypatch.setattr(run_env, "discover_task_packages", lambda: None) + monkeypatch.setattr(run_env, "execute_init_hooks", lambda: None) + + def build(parsed_args): + assert parsed_args is args + env_cfg.expert_program = decoded_program + return env_cfg, {"id": GYM_ID}, {} + + monkeypatch.setattr(run_env, "build_env_cfg_from_args", build) + monkeypatch.setattr(run_env.gymnasium, "make", make) + monkeypatch.setattr(run_env, "main", lambda *args, **kwargs: None) + monkeypatch.setattr( + "embodichain.lab.sim.sim_manager.SimulationManager.flush_cleanup_queue", + lambda: None, + ) + + run_env.cli([]) + + assert env_cfg.expert_program is decoded_program + make.assert_called_once_with(id=GYM_ID, cfg=env_cfg) + + def test_close_durability_failure_is_not_swallowed() -> None: """A failed recorder barrier makes the runner fail after aborting pending data.""" env = _LifecycleTrackingEnv() diff --git a/tests/scripts/tools/test_expert_program_rollout_report.py b/tests/scripts/tools/test_expert_program_rollout_report.py new file mode 100644 index 000000000..0c295cce0 --- /dev/null +++ b/tests/scripts/tools/test_expert_program_rollout_report.py @@ -0,0 +1,94 @@ +# ---------------------------------------------------------------------------- +# 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 pytest + +from scripts.tools.expert_program_rollout_report import ( + DEFAULT_REPORT_PATH, + REPOSITORY_ROOT, + build_task_size_metrics, + main, + render_report, +) + +EXPECTED_CURRENT_COUNTS = { + # Each tuple is (raw LF bytes, raw file bytes) for the explicit task pair. + "Cube": (395, 13_272), + "Drawer": (265, 8_916), +} + +EXPECTED_SOURCE_PATHS = { + "Cube": ( + "embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py", + "embodichain_tasks/configs/expert_program/multi_segments/" + "repeated_cube_pick_place.yaml", + ), + "Drawer": ( + "embodichain_tasks/embodichain_tasks/tableware/open_drawer.py", + "embodichain_tasks/configs/expert_program/tableware/open_drawer.json", + ), +} + + +def test_current_counts_use_only_the_four_declared_sources() -> None: + metrics = build_task_size_metrics(REPOSITORY_ROOT) + + actual = { + metric.task: ( + metric.current_lines, + metric.current_bytes, + tuple(source.path for source in metric.sources), + ) + for metric in metrics + } + expected = { + task: (*EXPECTED_CURRENT_COUNTS[task], EXPECTED_SOURCE_PATHS[task]) + for task in EXPECTED_CURRENT_COUNTS + } + assert actual == expected + + +def test_render_is_deterministic() -> None: + metrics = build_task_size_metrics(REPOSITORY_ROOT) + + first = render_report(metrics) + second = render_report(metrics) + + assert first == second + + +def test_render_rejects_empty_metric_snapshot() -> None: + with pytest.raises(ValueError, match="at least one task snapshot"): + render_report(()) + + +def test_checked_in_report_matches_deterministic_render() -> None: + expected = render_report(build_task_size_metrics(REPOSITORY_ROOT)) + + assert DEFAULT_REPORT_PATH.read_text(encoding="utf-8") == expected + + +def test_check_mode_accepts_current_report() -> None: + assert main(["--check"]) == 0 + + +def test_check_mode_rejects_stale_report(tmp_path) -> None: + stale_report = tmp_path / "expert_program_rollout_report.md" + stale_report.write_text("stale\n", encoding="utf-8") + + assert main(["--check", "--output", str(stale_report)]) == 1 diff --git a/tests/sim/atomic_actions/test_actions.py b/tests/sim/atomic_actions/test_actions.py index 06703fae5..ed40079c1 100644 --- a/tests/sim/atomic_actions/test_actions.py +++ b/tests/sim/atomic_actions/test_actions.py @@ -18,6 +18,7 @@ from __future__ import annotations +from dataclasses import replace from typing import TypeVar from unittest.mock import Mock @@ -27,14 +28,16 @@ from embodichain.lab.sim.atomic_actions import ( ActionBinding, ActionInvocation, + ActionPlan, Affordance, AntipodalAffordance, + AssembleAffordance, AssembleGoal, AtomicAction, AtomicActionEngine, ControlPartCommandProfile, - CoordinatedHeldObjectState, CoordinatedPickGoal, + CoordinatedHeldObjectState, CoordinatedPickment, CoordinatedPickmentOptions, CoordinatedPlacement, @@ -49,6 +52,8 @@ HeldObjectPoseGoal, HeldObjectState, JointPositionGoal, + JointPositionPayload, + JointPositionTarget, MotionPolicy, MoveEndEffector, MoveEndEffectorOptions, @@ -57,6 +62,10 @@ MoveJoints, MoveJointsOptions, ObjectSemantics, + ObservedArticulationJointState, + OperateArticulation, + OperateArticulationGoal, + OperateArticulationOptions, PickUp, PickUpOptions, Place, @@ -64,28 +73,44 @@ PlaceOptions, PlanningContext, Press, + PressAffordance, PressGoal, PressOptions, + SlideAffordance, + Slide, + SlideGoal, + SlideOptions, RobotObservation, + SceneArticulationOperationGeometry, SceneEntityPose, SceneSnapshot, TaskState, + TimedTrajectory, + TwistAffordance, + Twist, + TwistGoal, + TwistOptions, ) +from embodichain.lab.sim.atomic_actions.goals import collect_scene_dependencies +from embodichain.lab.sim.common import BatchEntity from embodichain.lab.sim.planners import ( MotionGenerator, MoveType, PlanOptions, PlanResult, ) +from embodichain.utils.math import pose_inv NUM_ENVS = 2 ARM_DOF = 6 HAND_DOF = 2 ROBOT_DOF = ARM_DOF + HAND_DOF +CONTROL_DT = 1.0 / 60.0 DUAL_ARM_DOF = 2 * ARM_DOF DUAL_ROBOT_DOF = DUAL_ARM_DOF + 2 * HAND_DOF ActionT = TypeVar("ActionT", bound=AtomicAction) +_ACTION_ENGINES: dict[int, AtomicActionEngine] = {} @pytest.fixture(autouse=True) @@ -173,6 +198,7 @@ def _motion_generator() -> MotionGenerator: generator.device = torch.device("cpu") generator.planner = Mock() generator.planner.cfg.planner_type = "stub" + generator.planner.collision_world_info = None generator.planner.preserve_plan_samples = False generator.planner.supports_move_type.return_value = False generator.planner.default_plan_options.return_value = PlanOptions() @@ -203,6 +229,7 @@ def _bind_action( load_builtins=False, ) engine.register(action) + _ACTION_ENGINES[id(action)] = engine return action @@ -231,6 +258,7 @@ def _context( task=task or TaskState.empty(batch_size=NUM_ENVS, device="cpu"), scene=SceneSnapshot.empty() if scene is None else scene, env_ids=torch.arange(NUM_ENVS), + control_dt=CONTROL_DT, ) @@ -248,44 +276,145 @@ def _target_scene( ) -def _binding() -> ActionBinding: - return ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, +def _articulation_geometry() -> SceneArticulationOperationGeometry: + """Build late-bound identity handle geometry for atomic tests.""" + identity = torch.eye(4) + return SceneArticulationOperationGeometry( + handle_pose=SceneEntityPose("drawer_handle"), + approach_offset=identity, + contact_offset=identity, + operation_offset=identity, + retract_offset=identity, + operation_axis=torch.tensor((1.0, 0.0, 0.0)), + ) + + +def _articulation_scene( + position: torch.Tensor, + *, + handle_x: float = 0.0, + timestamp: float = 0.0, + version: int = 0, +) -> SceneSnapshot: + """Build one live handle and articulation-joint snapshot.""" + handle = torch.eye(4).repeat(NUM_ENVS, 1, 1) + handle[:, 0, 3] = handle_x + return SceneSnapshot( + timestamp=timestamp, + version=version, + entities={"drawer_handle": EntityState(handle)}, + articulation_joints={ + ("drawer", "slide"): ObservedArticulationJointState(position) + }, + ) + + +def _binding( + action: AtomicAction, + *, + motion: str = "arm", + grasp: str = "hand", + task_state_key: str | None = None, +) -> ActionBinding: + """Bind one single-participant action through its owning engine.""" + contract = type(action).__dict__.get("binding_contract") + assert contract is not None + endpoint_parts = { + "motion": motion, + "grasp": grasp, + "interaction": grasp, + } + return _ACTION_ENGINES[id(action)].bind_control_parts( + action.skill_id, + { + slot.slot_id: { + endpoint.endpoint_id: endpoint_parts[endpoint.endpoint_id] + for endpoint in slot.endpoints + } + for slot in contract.slots + }, + task_state_keys=( + None + if task_state_key is None + else {slot.slot_id: task_state_key for slot in contract.slots} + ), ) def _invocation( - skill_id: str, + action: AtomicAction, goal, *, sample_count: int = 20, ) -> ActionInvocation: return ActionInvocation( - skill_id=skill_id, + skill_id=action.skill_id, goal=goal, - binding=_binding(), + binding=_binding(action), motion_policy=MotionPolicy(sample_count=sample_count), ) -def _semantics() -> ObjectSemantics: - entity = Mock() +def _joint_trajectory(plan: ActionPlan) -> TimedTrajectory: + """Return the owned planner trajectory for a joint-feedback plan.""" + assert plan.joint_trajectory is not None + return plan.joint_trajectory + + +def _joint_command_positions( + plan: ActionPlan, + control_part: str, +) -> torch.Tensor: + """Stack runtime joint commands sent to one concrete control part.""" + return torch.stack( + [payload.positions for payload in _joint_command_payloads(plan, control_part)], + dim=1, + ) + + +def _joint_command_payloads( + plan: ActionPlan, + control_part: str, +) -> tuple[JointPositionPayload, ...]: + """Return runtime joint payloads sent to one concrete control part.""" + payloads: list[JointPositionPayload] = [] + for frame in plan.commands.frames: + matching = [ + command + for command in frame.commands + if isinstance(command.target, JointPositionTarget) + and command.target.control_part == control_part + ] + assert len(matching) == 1 + payload = matching[0].payload + assert isinstance(payload, JointPositionPayload) + payloads.append(payload) + return tuple(payloads) + + +def _semantics(*, entity_id: str | None = None) -> ObjectSemantics: + entity = Mock(spec=BatchEntity) entity.get_local_pose.return_value = torch.eye(4).repeat(NUM_ENVS, 1, 1) return ObjectSemantics( affordance=Affordance(), geometry={}, label="test_object", entity=entity, + entity_id=entity_id, ) -def _held(semantics: ObjectSemantics | None = None) -> HeldObjectState: +def _held( + semantics: ObjectSemantics | None = None, + *, + env_mask: torch.Tensor | None = None, +) -> HeldObjectState: poses = torch.eye(4).repeat(NUM_ENVS, 1, 1) return HeldObjectState( semantics=semantics or _semantics(), object_to_eef=poses, grasp_xpos=poses, + env_mask=env_mask, ) @@ -353,6 +482,7 @@ def compute_fk( generator.device = torch.device("cpu") generator.planner = Mock() generator.planner.cfg.planner_type = "stub" + generator.planner.collision_world_info = None generator.planner.preserve_plan_samples = False generator.planner.supports_move_type.return_value = False generator.planner.default_plan_options.return_value = PlanOptions() @@ -362,29 +492,41 @@ def compute_fk( return generator -def _dual_context(task: TaskState | None = None) -> PlanningContext: +def _dual_context( + task: TaskState | None = None, + *, + scene: SceneSnapshot | None = None, +) -> PlanningContext: qpos = torch.zeros(NUM_ENVS, DUAL_ROBOT_DOF) return PlanningContext( robot=RobotObservation(0.0, qpos, torch.zeros_like(qpos)), task=task or TaskState.empty(NUM_ENVS, "cpu"), - scene=SceneSnapshot.empty(), + scene=SceneSnapshot.empty() if scene is None else scene, env_ids=torch.arange(NUM_ENVS), + control_dt=CONTROL_DT, ) def _dual_binding( - first_role: str, - second_role: str, + action: AtomicAction, + first_slot: str, + second_slot: str, + *, + task_state_keys: dict[str, str] | None = None, ) -> ActionBinding: - return ActionBinding( - manipulators={ - first_role: "left_arm", - second_role: "right_arm", - }, - end_effectors={ - first_role: "left_hand", - second_role: "right_hand", + return _ACTION_ENGINES[id(action)].bind_control_parts( + action.skill_id, + { + first_slot: { + "motion": "left_arm", + "grasp": "left_hand", + }, + second_slot: { + "motion": "right_arm", + "grasp": "right_hand", + }, }, + task_state_keys=task_state_keys, ) @@ -403,6 +545,93 @@ def _sample(obj_poses: torch.Tensor, **_kwargs: object) -> list[dict]: affordance.get_dual_arm_valid_grasp_poses = Mock(side_effect=_sample) +def _plan_segment_contract_case(case_id: str) -> ActionPlan: + """Plan one built-in used by the Version 1 trajectory-segment contract.""" + generator = _motion_generator() + sample_count = 20 + + if case_id == "move_joints": + action = _bind_action(generator, MoveJoints()) + goal = JointPositionGoal(torch.zeros(ARM_DOF)) + return _plan_action( + action, + _invocation(action, goal, sample_count=sample_count), + _context(), + ) + + if case_id == "move_end_effector": + action = _bind_action(generator, MoveEndEffector()) + goal = EndEffectorPoseGoal(torch.eye(4)) + return _plan_action( + action, + _invocation(action, goal, sample_count=sample_count), + _context(), + ) + + held_task = TaskState( + batch_size=NUM_ENVS, + device="cpu", + held_objects={"arm": _held()}, + ) + if case_id == "move_held_object": + action = _bind_action(generator, MoveHeldObject()) + goal = HeldObjectPoseGoal(torch.eye(4)) + return _plan_action( + action, + _invocation(action, goal, sample_count=sample_count), + _context(held_task), + ) + + if case_id == "place": + action = _bind_action(generator, Place()) + goal = PlaceGoal(torch.eye(4)) + return _plan_action( + action, + _invocation(action, goal, sample_count=sample_count), + _context(held_task), + ) + + if case_id == "assemble": + action = _bind_action(generator, Place()) + goal = AssembleGoal( + affordance=AssembleAffordance( + base_object_entity=Mock(), + assemble_to_base_pose=torch.eye(4), + ), + base_pose=SceneEntityPose("base"), + ) + base_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + scene = SceneSnapshot( + timestamp=0.0, + version=0, + entities={"base": EntityState(base_pose)}, + ) + return _plan_action( + action, + _invocation(action, goal, sample_count=sample_count), + _context(held_task, scene=scene), + ) + + if case_id == "press": + action = _bind_action(generator, Press()) + semantics = ObjectSemantics( + affordance=PressAffordance( + press_axis=torch.tensor([1.0, 0.0, 0.0]), + press_position=(0.0, 0.0, 0.0), + ), + geometry={}, + label="button", + ) + goal = PressGoal(semantics, torch.eye(4)) + return _plan_action( + action, + _invocation(action, goal, sample_count=sample_count), + _context(), + ) + + raise AssertionError(f"Unknown trajectory-segment contract case {case_id!r}.") + + def test_builtin_descriptors_expose_goals_not_legacy_targets() -> None: assert MoveEndEffector.GoalType is EndEffectorPoseGoal assert MoveJoints.GoalType is JointPositionGoal @@ -410,9 +639,47 @@ def test_builtin_descriptors_expose_goals_not_legacy_targets() -> None: assert MoveHeldObject.GoalType is HeldObjectPoseGoal assert Place.GoalType == (PlaceGoal, AssembleGoal) assert Press.GoalType is PressGoal + assert Slide.GoalType is SlideGoal + assert Twist.GoalType is TwistGoal assert CoordinatedPickment.GoalType is CoordinatedPickGoal assert CoordinatedPlacement.GoalType is CoordinatedPlacementGoal assert HandOver.GoalType is GraspGoal + assert OperateArticulation.GoalType is OperateArticulationGoal + + +@pytest.mark.parametrize( + ("case_id", "expected_names"), + ( + ("move_joints", ("move_joints",)), + ("move_end_effector", ("move_end_effector",)), + ("move_held_object", ("transport",)), + ("place", ("approach", "release", "retract")), + ("assemble", ("approach", "release", "retract")), + ("press", ("close", "approach", "contact", "press", "retract")), + ), +) +def test_builtin_trajectory_segment_names_and_ranges_are_stable( + case_id: str, + expected_names: tuple[str, ...], +) -> None: + plan = _plan_segment_contract_case(case_id) + + assert plan.success_all + assert tuple(segment.name for segment in plan.segments) == expected_names + assert plan.segments[0].start == 0 + assert all( + previous.stop == current.start + for previous, current in zip(plan.segments, plan.segments[1:]) + ) + assert plan.segments[-1].stop == plan.commands.frame_count + + +def test_interaction_primitives_use_motion_centric_skill_ids() -> None: + assert (Press.skill_id, Slide.skill_id, Twist.skill_id) == ( + "press", + "slide", + "twist", + ) @pytest.mark.parametrize( @@ -422,9 +689,12 @@ def test_builtin_descriptors_expose_goals_not_legacy_targets() -> None: MoveHeldObjectOptions(), PlaceOptions(), PressOptions(), + SlideOptions(), + TwistOptions(), CoordinatedPickmentOptions(), CoordinatedPlacementOptions(), HandOverOptions(), + OperateArticulationOptions(), ), ) def test_action_options_do_not_contain_embodiment_resources(options: object) -> None: @@ -435,6 +705,27 @@ def test_action_options_do_not_contain_embodiment_resources(options: object) -> assert not any(name.endswith("_qpos") for name in field_names) +def test_pose_options_own_late_bound_relative_transforms() -> None: + relative_pose = torch.eye(4) + target = SceneEntityPose("target", relative_pose=relative_pose) + pick_options = PickUpOptions(downstream_object_target_poses=(target,)) + handover_options = HandOverOptions( + middle_object_pose=target, + final_object_pose=target, + ) + + assert target.relative_pose is not None + target.relative_pose[0, 3] = 9.0 + + pick_target = pick_options.downstream_object_target_poses[0] + assert type(pick_target) is SceneEntityPose + assert pick_target.relative_pose is not None + assert pick_target.relative_pose[0, 3].item() == 0.0 + assert type(handover_options.middle_object_pose) is SceneEntityPose + assert handover_options.middle_object_pose.relative_pose is not None + assert handover_options.middle_object_pose.relative_pose[0, 3].item() == 0.0 + + def test_joint_position_goal_rejects_unsupported_target_type() -> None: with pytest.raises(TypeError, match="torch.Tensor or str"): JointPositionGoal(target=1.0) # type: ignore[arg-type] @@ -468,7 +759,7 @@ def test_move_end_effector_returns_full_robot_timed_plan() -> None: plan = _plan_action( action, _invocation( - "move_end_effector", + action, EndEffectorPoseGoal(torch.eye(4)), sample_count=10, ), @@ -476,8 +767,9 @@ def test_move_end_effector_returns_full_robot_timed_plan() -> None: ) assert plan.plan_success.tolist() == [True, True] - assert plan.trajectory.positions.shape == (NUM_ENVS, 10, ROBOT_DOF) - assert plan.trajectory.duration.tolist() == pytest.approx([0.15, 0.15]) + assert plan.commands.frame_count == 10 + assert [target.target_id for target in plan.commands.targets] == ["arm"] + assert _joint_trajectory(plan).duration.tolist() == pytest.approx([0.15, 0.15]) assert plan.expected_effects.is_empty @@ -498,34 +790,42 @@ def test_move_joints_uses_binding_and_preserves_uncontrolled_joints() -> None: task=TaskState.empty(NUM_ENVS, "cpu"), scene=SceneSnapshot.empty(), env_ids=torch.arange(NUM_ENVS), + control_dt=CONTROL_DT, ) plan = _plan_action( action, - _invocation("move_joints", JointPositionGoal("ready"), sample_count=8), + _invocation(action, JointPositionGoal("ready"), sample_count=8), context, ) - assert torch.allclose(plan.trajectory.positions[:, -1, :ARM_DOF], named["ready"]) - assert torch.all(plan.trajectory.positions[:, :, ARM_DOF:] == 0.7) + arm_positions = _joint_command_positions(plan, "arm") + assert torch.allclose(arm_positions[:, -1], named["ready"]) + assert [target.target_id for target in plan.commands.targets] == ["arm"] def test_pick_and_place_declare_effects_without_mutating_context() -> None: generator = _motion_generator() pick = _bind_action(generator, PickUp()) - initial = _context() - semantics = _semantics() + object_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + initial = _context(scene=_target_scene(object_pose, timestamp=0.0, version=0)) + semantics = _semantics(entity_id="target") grasp = torch.eye(4).repeat(NUM_ENVS, 1, 1) pick_plan = _plan_action( pick, - _invocation("pick_up", GraspGoal(semantics=semantics, grasp_xpos=grasp)), + ActionInvocation( + skill_id="pick_up", + goal=GraspGoal(semantics=semantics, grasp_xpos=grasp), + binding=_binding(pick, task_state_key="logical_arm"), + ), initial, ) picked_task = pick_plan.expected_effects.apply(initial.task, pick_plan.plan_success) - assert initial.task.get_held_object("arm") is None - assert picked_task.get_held_object("arm") is not None + assert initial.task.get_held_object("logical_arm") is None + assert picked_task.get_held_object("logical_arm") is not None + assert picked_task.get_held_object("arm") is None place = _bind_action(generator, Place()) picked_context = PlanningContext( @@ -533,56 +833,353 @@ def test_pick_and_place_declare_effects_without_mutating_context() -> None: task=picked_task, scene=initial.scene, env_ids=initial.env_ids, + control_dt=initial.control_dt, ) place_plan = _plan_action( place, - _invocation("place", PlaceGoal(torch.eye(4))), + ActionInvocation( + skill_id="place", + goal=PlaceGoal(torch.eye(4)), + binding=_binding(place, task_state_key="logical_arm"), + ), picked_context, ) placed_task = place_plan.expected_effects.apply( picked_task, place_plan.plan_success ) - assert picked_task.get_held_object("arm") is not None - assert placed_task.get_held_object("arm") is None + assert picked_task.get_held_object("logical_arm") is not None + assert placed_task.get_held_object("logical_arm") is None + + +def test_place_releases_only_exclusively_held_rows() -> None: + generator = _motion_generator() + + def move_ik( + pose: torch.Tensor, + name: str, + joint_seed: torch.Tensor, + **_: object, + ) -> tuple[torch.Tensor, torch.Tensor]: + return torch.ones(NUM_ENVS, dtype=torch.bool), joint_seed + 0.1 + + generator.robot.compute_ik.side_effect = move_ik + action = _bind_action(generator, Place()) + semantics = _semantics() + task = TaskState( + batch_size=NUM_ENVS, + device="cpu", + held_objects={ + "arm": _held(semantics), + "alternate_arm": _held( + semantics, + env_mask=torch.tensor([True, False]), + ), + }, + ) + context = _context(task) + + plan = _plan_action( + action, + _invocation(action, PlaceGoal(torch.eye(4))), + context, + ) + projected = plan.expected_effects.apply(task, plan.plan_success) + + assert plan.plan_success.tolist() == [False, True] + trajectory = _joint_trajectory(plan) + assert torch.allclose( + trajectory.positions[0], + context.robot.qpos[0].unsqueeze(0).expand(trajectory.waypoint_count, -1), + ) + primary = projected.get_held_object("arm") + alternate = projected.get_held_object("alternate_arm") + assert primary is not None and primary.env_mask.tolist() == [True, False] + assert alternate is not None and alternate.env_mask.tolist() == [True, False] def test_move_held_object_requires_projected_attachment() -> None: generator = _motion_generator() action = _bind_action(generator, MoveHeldObject()) invocation = _invocation( - "move_held_object", + action, HeldObjectPoseGoal(torch.eye(4)), sample_count=10, ) + invocation = replace( + invocation, + binding=_binding(action, task_state_key="logical_arm"), + ) with pytest.raises(ValueError, match="requires an object held"): _plan_action(action, invocation, _context()) - held = _held() + semantics = _semantics() + held = _held(semantics) + held.object_to_eef[:, :3, :3] = torch.tensor( + [[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]] + ) + held.object_to_eef[:, 0, 3] = torch.tensor([0.1, 0.2]) task = TaskState( batch_size=NUM_ENVS, device="cpu", - held_objects={"arm": held}, + held_objects={"logical_arm": held}, ) - plan = _plan_action(action, invocation, _context(task)) + eef_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + eef_pose[:, :3, :3] = torch.tensor( + [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]] + ) + eef_pose[:, 0, 3] = torch.tensor([0.5, 0.8]) + generator.robot.compute_fk.return_value = eef_pose + generator.robot.compute_fk.side_effect = None + action._apply_configured_upright_rotation = Mock() + configured_invocation = ActionInvocation( + skill_id="move_held_object", + goal=HeldObjectPoseGoal(torch.eye(4)), + binding=_binding(action, task_state_key="logical_arm"), + motion_policy=MotionPolicy(sample_count=10), + skill_options=MoveHeldObjectOptions(pick_rotate_upright=0.25), + ) + + plan = _plan_action(action, configured_invocation, _context(task)) + assert plan.plan_success.all() assert plan.expected_effects.is_empty + assert generator.robot.compute_fk.call_args.kwargs["name"] == "arm" + current_object_pose = action._apply_configured_upright_rotation.call_args.args[2] + assert torch.allclose( + current_object_pose, + torch.bmm(eef_pose, pose_inv(held.object_to_eef)), + ) + semantics.entity.get_local_pose.assert_not_called() -def test_press_uses_invocation_sample_budget() -> None: +def test_move_held_object_moves_only_exclusively_held_rows() -> None: generator = _motion_generator() - action = _bind_action(generator, Press()) + + def move_ik( + pose: torch.Tensor, + name: str, + joint_seed: torch.Tensor, + **_: object, + ) -> tuple[torch.Tensor, torch.Tensor]: + return torch.ones(NUM_ENVS, dtype=torch.bool), joint_seed + 0.1 + + generator.robot.compute_ik.side_effect = move_ik + action = _bind_action(generator, MoveHeldObject()) + semantics = _semantics() + task = TaskState( + batch_size=NUM_ENVS, + device="cpu", + held_objects={ + "arm": _held(semantics), + "alternate_arm": _held( + semantics, + env_mask=torch.tensor([True, False]), + ), + }, + ) + context = _context(task) plan = _plan_action( action, - _invocation("press", PressGoal(torch.eye(4)), sample_count=12), - _context(), + _invocation(action, HeldObjectPoseGoal(torch.eye(4))), + context, + ) + + assert plan.plan_success.tolist() == [False, True] + trajectory = _joint_trajectory(plan) + assert torch.allclose( + trajectory.positions[0], + context.robot.qpos[0].unsqueeze(0).expand(trajectory.waypoint_count, -1), + ) + assert not torch.allclose(trajectory.positions[1], context.robot.qpos[1]) + + +def test_operate_articulation_builds_named_verified_interaction() -> None: + generator = _motion_generator() + action = _bind_action( + generator, + OperateArticulation( + OperateArticulationOptions(engage_steps=2, release_steps=2) + ), + ) + goal = OperateArticulationGoal( + articulation_id="drawer", + joint_id="slide", + geometry=_articulation_geometry(), + source_position=torch.tensor([0.0]), + target_position=torch.tensor([0.4]), + target_displacement=0.4, + ) + + plan = _plan_action( + action, + _invocation(action, goal, sample_count=16), + _context(scene=_articulation_scene(torch.tensor([0.0]))), ) assert plan.plan_success.tolist() == [True, True] - assert plan.trajectory.waypoint_count == 12 - assert plan.expected_effects.is_empty + assert plan.commands.frame_count == 16 + assert tuple(segment.name for segment in plan.segments) == ( + "approach", + "engage", + "operate", + "release", + "retract", + ) + assert plan.requires_effect_verification + assert plan.scene_dependency_monitor_until == { + "drawer_handle": plan.segment("operate").start + } + assert plan.effect_verification is not None + assert plan.effect_verification.kind == "articulation.joint_progress" + update = plan.expected_effects.articulation_joint_updates[("drawer", "slide")] + assert update is not None + assert torch.equal(update.position, torch.tensor([0.4])) + interaction = _joint_command_positions(plan, "hand") + assert torch.all(interaction[:, -1] == 0.0) + assert torch.any(interaction == 1.0) + + +def test_operate_articulation_reports_per_phase_planning_failures() -> None: + generator = _motion_generator() + action = _bind_action( + generator, + OperateArticulation( + OperateArticulationOptions(engage_steps=2, release_steps=2) + ), + ) + phase_results = [] + for index in range(4): + phase_results.append( + PlanResult( + success=torch.tensor([index != 2, True]), + positions=torch.zeros(NUM_ENVS, 3, ARM_DOF), + dt=torch.tensor([[0.0, 0.1, 0.2]]).repeat(NUM_ENVS, 1), + ) + ) + generator.generate = Mock(side_effect=phase_results) + goal = OperateArticulationGoal( + articulation_id="drawer", + joint_id="slide", + geometry=_articulation_geometry(), + source_position=torch.tensor([0.0]), + target_position=torch.tensor([0.4]), + target_displacement=0.4, + ) + + plan = _plan_action( + action, + _invocation(action, goal, sample_count=16), + _context(scene=_articulation_scene(torch.tensor([0.0]))), + ) + + assert plan.plan_success.tolist() == [False, True] + assert plan.diagnostics.messages == ( + "Articulation motion phase 'operate' failed for rows [0].", + ) + phases = plan.diagnostics.metadata["motion_phases"] + assert phases["operate"] == { + "success": [False, True], + "failed_rows": [0], + "waypoint_count": 3, + } + + +def test_operate_articulation_replan_uses_fresh_handle_and_remaining_stroke() -> None: + generator = _motion_generator() + action = _bind_action( + generator, + OperateArticulation( + OperateArticulationOptions(engage_steps=2, release_steps=2) + ), + ) + goal = OperateArticulationGoal( + articulation_id="drawer", + joint_id="slide", + geometry=_articulation_geometry(), + source_position=torch.tensor([[0.0], [0.0]]), + target_position=torch.tensor([[0.4], [0.4]]), + target_displacement=0.4, + ) + invocation = _invocation(action, goal, sample_count=16) + initial_context = _context( + scene=_articulation_scene( + torch.tensor([[0.0], [0.0]]), + handle_x=0.3, + ) + ) + session = _ACTION_ENGINES[id(action)].start((invocation,), initial_context) + first_operation = generator.robot.compute_ik.call_args_list[2].kwargs["pose"] + assert torch.allclose(first_operation[:, 0, 3], torch.tensor([0.7, 0.7])) + session.tick(initial_context) + + generator.robot.compute_ik.reset_mock() + recovered = session.tick( + _context( + scene=_articulation_scene( + torch.tensor([[0.2], [0.4]]), + handle_x=0.55, + timestamp=1.0, + version=1, + ), + timestamp=1.0, + ), + ) + recovered_operation = generator.robot.compute_ik.call_args_list[2].kwargs["pose"] + + assert torch.allclose(recovered_operation[:, 0, 3], torch.tensor([0.75, 0.55])) + event_kinds = {event.kind for event in recovered.events} + assert ExecutionEventKind.DYNAMIC_GOAL_CHANGED in event_kinds + assert ExecutionEventKind.REPLANNED in event_kinds + assert session.trajectory_segment("operate").name == "operate" + + +def test_operate_articulation_requires_live_joint_observation() -> None: + generator = _motion_generator() + action = _bind_action(generator, OperateArticulation()) + goal = OperateArticulationGoal( + articulation_id="drawer", + joint_id="slide", + geometry=_articulation_geometry(), + source_position=torch.tensor([0.0]), + target_position=torch.tensor([0.4]), + target_displacement=0.4, + ) + handle = torch.eye(4).repeat(NUM_ENVS, 1, 1) + scene = SceneSnapshot( + timestamp=0.0, + version=0, + entities={"drawer_handle": EntityState(handle)}, + ) + + with pytest.raises(ValueError, match="ObservedArticulationJointState"): + _plan_action( + action, + _invocation(action, goal, sample_count=20), + _context(scene=scene), + ) + + +def test_operate_articulation_rejects_insufficient_motion_budget() -> None: + generator = _motion_generator() + action = _bind_action(generator, OperateArticulation()) + goal = OperateArticulationGoal( + articulation_id="drawer", + joint_id="slide", + geometry=_articulation_geometry(), + source_position=torch.tensor([0.0]), + target_position=torch.tensor([0.4]), + target_displacement=0.4, + ) + + with pytest.raises(ValueError, match="at least two waypoints"): + _plan_action( + action, + _invocation(action, goal, sample_count=17), + _context(scene=_articulation_scene(torch.tensor([0.0]))), + ) def test_strategy_and_sample_count_are_not_action_config_fields() -> None: @@ -598,12 +1195,42 @@ def test_move_joints_rejects_binding_with_wrong_goal_skill() -> None: invocation = ActionInvocation( skill_id="move_end_effector", goal=JointPositionGoal(torch.zeros(ARM_DOF)), - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=_binding(action), ) with pytest.raises(ValueError, match="skill_id"): action.resolve_request(invocation) +def test_move_joints_rejects_incompatible_goal_at_action_boundary() -> None: + action = _bind_action(_motion_generator(), MoveJoints()) + invocation = ActionInvocation( + skill_id="move_joints", + goal=object(), # type: ignore[arg-type] + binding=_binding(action), + ) + + with pytest.raises(TypeError, match="expects goal JointPositionGoal"): + action.resolve_request(invocation) + + +def test_builtin_action_validates_resolved_request_once() -> None: + generator = _motion_generator() + action = _bind_action(generator, MoveEndEffector()) + validator = Mock(wraps=action.require_goal) + action.require_goal = validator # type: ignore[method-assign] + + _plan_action( + action, + _invocation( + action, + EndEffectorPoseGoal(torch.eye(4)), + ), + _context(), + ) + + validator.assert_called_once() + + def test_planner_timing_is_preserved_in_simple_action() -> None: generator = _motion_generator() generator.planner.cfg.planner_type = "toppra" @@ -616,22 +1243,24 @@ def test_planner_timing_is_preserved_in_simple_action() -> None: velocities=torch.full((NUM_ENVS, 3, ARM_DOF), 0.5), accelerations=torch.zeros(NUM_ENVS, 3, ARM_DOF), dt=torch.tensor([[0.0, 0.1, 0.2]]).repeat(NUM_ENVS, 1), - duration=torch.full((NUM_ENVS,), 0.3), ) action = _bind_action(generator, MoveJoints()) invocation = ActionInvocation( skill_id="move_joints", goal=JointPositionGoal(torch.ones(ARM_DOF)), - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=_binding(action), motion_policy=MotionPolicy(strategy="motion_gen", sample_count=3), ) plan = _plan_action(action, invocation, _context()) - assert plan.trajectory.duration.tolist() == pytest.approx([0.3, 0.3]) - assert plan.trajectory.velocities is not None - assert torch.all(plan.trajectory.velocities[:, :, :ARM_DOF] == 0.5) - assert torch.all(plan.trajectory.velocities[:, :, ARM_DOF:] == 0.0) + trajectory = _joint_trajectory(plan) + payloads = _joint_command_payloads(plan, "arm") + assert trajectory.duration.tolist() == pytest.approx([0.3, 0.3]) + assert all(payload.velocities is not None for payload in payloads) + assert torch.all( + torch.stack([payload.velocities for payload in payloads], dim=1) == 0.5 + ) def test_move_end_effector_visits_batched_waypoints_in_order() -> None: @@ -656,7 +1285,7 @@ def compute_ik( plan = _plan_action( action, _invocation( - "move_end_effector", + action, EndEffectorPoseGoal(waypoints), sample_count=9, ), @@ -689,19 +1318,20 @@ def test_move_joints_visits_waypoints_and_rejects_unknown_names() -> None: plan = _plan_action( action, _invocation( - "move_joints", + action, JointPositionGoal(waypoints), sample_count=7, ), _context(), ) - assert torch.allclose(plan.trajectory.positions[:, 3, :ARM_DOF], waypoints[:, 0]) - assert torch.allclose(plan.trajectory.positions[:, -1, :ARM_DOF], waypoints[:, 1]) + arm_positions = _joint_command_positions(plan, "arm") + assert torch.allclose(arm_positions[:, 3], waypoints[:, 0]) + assert torch.allclose(arm_positions[:, -1], waypoints[:, 1]) with pytest.raises(KeyError, match="has no command"): _plan_action( action, - _invocation("move_joints", JointPositionGoal("missing")), + _invocation(action, JointPositionGoal("missing")), _context(), ) @@ -711,38 +1341,50 @@ def test_pick_explicit_grasp_bypasses_sampling_and_records_grasp() -> None: affordance = AntipodalAffordance() affordance.get_valid_grasp_poses = Mock() entity = Mock() - entity.get_local_pose.return_value = torch.eye(4).repeat(NUM_ENVS, 1, 1) + entity.get_local_pose.return_value = torch.full((NUM_ENVS, 4, 4), 9.0) semantics = ObjectSemantics( affordance=affordance, geometry={}, label="explicit-grasp-object", entity=entity, + entity_id="target", ) grasp = torch.eye(4).repeat(NUM_ENVS, 1, 1) grasp[:, 0, 3] = torch.tensor([0.1, 0.2]) action = _bind_action(generator, PickUp()) + object_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + object_pose[:, :3, :3] = torch.tensor( + [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]] + ) + object_pose[:, 0, 3] = torch.tensor([0.03, 0.07]) + context = _context(scene=_target_scene(object_pose, timestamp=0.0, version=0)) - plan = _plan_action( - action, + request = action.resolve_request( _invocation( - "pick_up", + action, GraspGoal(semantics=semantics, grasp_xpos=grasp), sample_count=20, - ), - _context(), + ) ) - projected = plan.expected_effects.apply(_context().task, plan.plan_success) + plan = action.plan(request, context) + projected = plan.expected_effects.apply(context.task, plan.plan_success) - affordance.get_valid_grasp_poses.assert_not_called() + request.goal.semantics.affordance.get_valid_grasp_poses.assert_not_called() + request.goal.semantics.entity.get_local_pose.assert_not_called() held = projected.get_held_object("arm") assert held is not None assert torch.allclose(held.grasp_xpos, grasp) + assert torch.allclose(held.object_to_eef, torch.bmm(pose_inv(object_pose), grasp)) + assert plan.scene_dependencies == ("target",) assert [segment.name for segment in plan.segments] == [ "approach", "close", "lift", ] assert plan.segment("close").stop == plan.segment("lift").start + assert plan.scene_dependency_monitor_until == { + "target": plan.segment("close").start + } def test_pick_holds_only_environment_without_a_feasible_grasp() -> None: @@ -754,6 +1396,7 @@ def test_pick_holds_only_environment_without_a_feasible_grasp() -> None: geometry={}, label="partially-graspable-object", entity=entity, + entity_id="target", ) action = _bind_action(generator, PickUp()) action._resolve_grasp_pose = Mock( @@ -762,21 +1405,29 @@ def test_pick_holds_only_environment_without_a_feasible_grasp() -> None: torch.eye(4).repeat(NUM_ENVS, 1, 1), ) ) - context = _context() + context = _context( + scene=_target_scene( + torch.eye(4).repeat(NUM_ENVS, 1, 1), + timestamp=0.0, + version=0, + ) + ) plan = _plan_action( action, - _invocation("pick_up", GraspGoal(semantics=semantics), sample_count=20), + _invocation(action, GraspGoal(semantics=semantics), sample_count=20), context, ) projected = plan.expected_effects.apply(context.task, plan.plan_success) assert plan.plan_success.tolist() == [True, False] - assert not torch.allclose(plan.trajectory.positions[0], context.robot.qpos[0]) + trajectory = _joint_trajectory(plan) + assert not torch.allclose(trajectory.positions[0], context.robot.qpos[0]) assert torch.allclose( - plan.trajectory.positions[1], + trajectory.positions[1], context.robot.qpos[1].unsqueeze(0).expand(20, -1), ) + assert all(not frame.active_mask[1].item() for frame in plan.commands.frames) held = projected.get_held_object("arm") assert held is not None assert held.env_mask.tolist() == [True, False] @@ -795,6 +1446,7 @@ def test_pick_resolves_late_bound_scene_grasp_and_declares_dependency() -> None: geometry={}, label="late-bound-grasp-object", entity=entity, + entity_id="target", ) action = _bind_action(generator, PickUp()) context = _context(scene=_target_scene(target_pose, timestamp=0.0, version=0)) @@ -802,7 +1454,7 @@ def test_pick_resolves_late_bound_scene_grasp_and_declares_dependency() -> None: plan = _plan_action( action, _invocation( - "pick_up", + action, GraspGoal( semantics=semantics, grasp_xpos=SceneEntityPose( @@ -824,6 +1476,9 @@ def test_pick_resolves_late_bound_scene_grasp_and_declares_dependency() -> None: assert held is not None assert torch.allclose(held.grasp_xpos, expected_grasp) assert plan.scene_dependencies == ("target",) + assert plan.scene_dependency_monitor_until == { + "target": plan.segment("approach").stop + } def test_pick_session_replans_when_late_bound_target_moves() -> None: @@ -838,6 +1493,7 @@ def test_pick_session_replans_when_late_bound_target_moves() -> None: geometry={}, label="moving-grasp-object", entity=entity, + entity_id="target", ) engine = AtomicActionEngine( generator, @@ -849,9 +1505,11 @@ def test_pick_session_replans_when_late_bound_target_moves() -> None: }, load_builtins=False, ) - engine.register(PickUp()) + action = PickUp() + engine.register(action) + _ACTION_ENGINES[id(action)] = engine invocation = _invocation( - "pick_up", + action, GraspGoal( semantics=semantics, grasp_xpos=SceneEntityPose("target"), @@ -877,61 +1535,249 @@ def test_pick_session_replans_when_late_bound_target_moves() -> None: assert ExecutionEventKind.REPLANNED in event_kinds -def test_pick_uses_binding_control_part_as_effect_resource() -> None: +@pytest.mark.parametrize( + ("waypoint_offset", "expects_replan"), + ((-1, True), (0, False)), +) +def test_pick_scene_monitoring_window_is_exclusive_at_close_boundary( + waypoint_offset: int, + expects_replan: bool, +) -> None: + """External motion replans before close, while grasp-induced motion does not.""" + generator = _motion_generator() + action = _bind_action(generator, PickUp()) + engine = _ACTION_ENGINES[id(action)] + initial_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + moved_pose = initial_pose.clone() + moved_pose[:, 1, 3] = 0.3 + invocation = _invocation( + action, + GraspGoal( + semantics=_semantics(entity_id="target"), + grasp_xpos=torch.eye(4), + ), + sample_count=20, + ) + task_state = TaskState.empty(batch_size=NUM_ENVS, device="cpu") + qpos = torch.zeros(NUM_ENVS, ROBOT_DOF) + + def context_at( + pose: torch.Tensor, + *, + timestamp: float, + version: int, + ) -> PlanningContext: + return PlanningContext( + robot=RobotObservation( + timestamp=timestamp, + qpos=qpos, + qvel=torch.zeros_like(qpos), + ), + task=task_state, + scene=_target_scene(pose, timestamp=timestamp, version=version), + env_ids=torch.arange(NUM_ENVS), + control_dt=1.0 / 60.0, + ) + + session = engine.start( + (invocation,), context_at(initial_pose, timestamp=0.0, version=0) + ) + tick = session.tick(context_at(initial_pose, timestamp=0.0, version=0)) + close_start = session.plan_attempts[0].plan.segment("close").start + commands_to_issue = close_start + waypoint_offset + issued = 1 + while issued < commands_to_issue: + assert tick.command is not None + for command in tick.command.commands: + assert isinstance(command.target, JointPositionTarget) + assert isinstance(command.payload, JointPositionPayload) + qpos[:, list(command.target.joint_ids)] = command.payload.positions + tick = session.tick( + context_at( + initial_pose, + timestamp=0.04 * issued, + version=0, + ) + ) + issued += 1 + + assert tick.command is not None + for command in tick.command.commands: + assert isinstance(command.target, JointPositionTarget) + assert isinstance(command.payload, JointPositionPayload) + qpos[:, list(command.target.joint_ids)] = command.payload.positions + moved = session.tick( + context_at( + moved_pose, + timestamp=0.04 * commands_to_issue, + version=1, + ) + ) + + event_kinds = {event.kind for event in moved.events} + assert (ExecutionEventKind.DYNAMIC_GOAL_CHANGED in event_kinds) is expects_replan + assert (ExecutionEventKind.REPLANNED in event_kinds) is expects_replan + assert len(session.plan_attempts) == (2 if expects_replan else 1) + + +def test_pick_uses_logical_task_state_key_and_physical_control_target() -> None: generator = _motion_generator() action = _bind_action(generator, PickUp()) invocation = ActionInvocation( skill_id="pick_up", - goal=GraspGoal(semantics=_semantics(), grasp_xpos=torch.eye(4)), - binding=ActionBinding( - manipulators={"primary": "alternate_arm"}, - end_effectors={"primary": "alternate_hand"}, + goal=GraspGoal( + semantics=_semantics(entity_id="target"), + grasp_xpos=torch.eye(4), + ), + binding=_binding( + action, + motion="alternate_arm", + grasp="alternate_hand", + task_state_key="logical_picker", ), motion_policy=MotionPolicy(sample_count=20), ) - context = _context() + context = _context( + scene=_target_scene( + torch.eye(4).repeat(NUM_ENVS, 1, 1), + timestamp=0.0, + version=0, + ) + ) - plan = _plan_action(action, invocation, context) + request = action.resolve_request(invocation) + plan = action.plan(request, context) projected = plan.expected_effects.apply(context.task, plan.plan_success) - assert projected.get_held_object("alternate_arm") is not None + assert projected.get_held_object("logical_picker") is not None + assert projected.get_held_object("alternate_arm") is None assert projected.get_held_object("arm") is None + assert {target.target_id for target in plan.commands.targets} == { + "alternate_arm", + "alternate_hand", + } + + +def test_participant_motion_and_grasp_must_share_task_state_key() -> None: + generator = _motion_generator() + action = _bind_action(generator, PickUp()) + binding = _binding(action) + mismatched = ActionBinding( + owner_id=binding.owner_id, + endpoints=tuple( + ( + replace(endpoint, task_state_key="other_participant") + if endpoint.endpoint_id == "grasp" + else endpoint + ) + for endpoint in binding.endpoints + ), + ) + invocation = ActionInvocation( + skill_id="pick_up", + goal=GraspGoal( + semantics=_semantics(entity_id="target"), + grasp_xpos=torch.eye(4), + ), + binding=mismatched, + ) + context = _context( + scene=_target_scene( + torch.eye(4).repeat(NUM_ENVS, 1, 1), + timestamp=0.0, + version=0, + ) + ) + + with pytest.raises(ValueError, match="must share one task_state_key"): + _plan_action(action, invocation, context) + + +def test_handover_participants_must_use_distinct_task_state_keys() -> None: + generator = _dual_motion_generator() + action = _bind_action( + generator, + HandOver( + default_options=HandOverOptions( + middle_object_pose=torch.eye(4), + final_object_pose=torch.eye(4), + ) + ), + ) + invocation = ActionInvocation( + skill_id="hand_over", + goal=GraspGoal(semantics=_semantics()), + binding=_dual_binding( + action, + "source", + "destination", + task_state_keys={"source": "same", "destination": "same"}, + ), + ) + + with pytest.raises(ValueError, match="different task_state_key"): + _plan_action(action, invocation, _dual_context()) def test_press_closes_hand_without_changing_projected_attachment() -> None: - held = _held() + semantics = ObjectSemantics( + affordance=PressAffordance( + press_axis=torch.tensor([1.0, 0.0, 0.0]), + press_position=(0.0, 0.0, 0.0), + ), + geometry={}, + label="button", + ) + held = _held(semantics) task = TaskState( batch_size=NUM_ENVS, device="cpu", held_objects={"arm": held}, ) - generator = _motion_generator() action = _bind_action( - generator, + _motion_generator(), Press(default_options=PressOptions(hand_interp_steps=4)), ) plan = _plan_action( action, - _invocation("press", PressGoal(torch.eye(4)), sample_count=12), + _invocation( + action, + PressGoal(semantics, torch.eye(4)), + sample_count=12, + ), _context(task), ) projected = plan.expected_effects.apply(task, plan.plan_success) - assert torch.all(plan.trajectory.positions[:, -1, ARM_DOF:] == 1.0) + assert torch.all(_joint_command_positions(plan, "hand")[:, -1] == 1.0) projected_held = projected.get_held_object("arm") assert projected_held is not None assert projected_held.semantics is held.semantics assert torch.equal(projected_held.object_to_eef, held.object_to_eef) -def test_handover_does_not_mutate_cached_final_pose() -> None: +@pytest.mark.parametrize( + ("hold_steps", "expected_segments"), + ( + (0, ("transfer", "approach", "close", "release", "deliver")), + ( + 2, + ("transfer", "approach", "close", "hold", "release", "deliver"), + ), + ), +) +def test_handover_does_not_mutate_cached_final_pose_and_omits_empty_hold( + hold_steps: int, + expected_segments: tuple[str, ...], + monkeypatch: pytest.MonkeyPatch, +) -> None: generator = _dual_motion_generator() handover_options = HandOverOptions( middle_object_pose=torch.eye(4), final_object_pose=torch.eye(4), hand_interp_steps=4, - hold_steps=2, + hold_steps=hold_steps, retreat_steps=5, ) action = _bind_action( @@ -940,49 +1786,137 @@ def test_handover_does_not_mutate_cached_final_pose() -> None: ) assert handover_options.final_object_pose is not None original_final_pose = handover_options.final_object_pose.clone() - current_object_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) - current_object_pose[:, :3, :3] = torch.diag(torch.tensor([-1.0, -1.0, 1.0])) - semantics = _semantics() - semantics.entity.get_local_pose.return_value = current_object_pose + semantics = _semantics(entity_id="handover_object") + held = _held(semantics) + held.object_to_eef[:, :3, :3] = torch.tensor( + [[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]] + ) + held.object_to_eef[:, 0, 3] = torch.tensor([0.1, 0.2]) task = TaskState( batch_size=NUM_ENVS, device="cpu", - held_objects={"left_arm": _held(semantics)}, + held_objects={"left_arm": held}, ) + current_eef = torch.eye(4).repeat(NUM_ENVS, 1, 1) + current_eef[:, :3, :3] = torch.tensor( + [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]] + ) + current_eef[:, 1, 3] = torch.tensor([0.3, 0.5]) + generator.robot.compute_fk.return_value = current_eef + generator.robot.compute_fk.side_effect = None receive_grasp = torch.eye(4).repeat(NUM_ENVS, 1, 1) action._resolve_receive_grasp = Mock( return_value=(receive_grasp, torch.ones(NUM_ENVS, dtype=torch.bool)) ) def plan_from_start( + motion_generator: MotionGenerator, control_part: str, start_qpos: torch.Tensor, target_poses: torch.Tensor, n_waypoints: int, motion_policy: MotionPolicy, + interpolation_dt: float | None, ) -> tuple[bool, torch.Tensor]: + del interpolation_dt return True, start_qpos.unsqueeze(1).repeat(1, n_waypoints, 1) - action._plan_named_arm_trajectory = Mock(side_effect=plan_from_start) + monkeypatch.setattr( + "embodichain.lab.sim.atomic_actions.primitives.hand_over." + "plan_named_arm_trajectory", + plan_from_start, + ) invocation = ActionInvocation( skill_id="hand_over", - goal=GraspGoal(semantics=semantics), - binding=_dual_binding("source", "destination"), + goal=GraspGoal( + semantics=semantics, + grasp_xpos=SceneEntityPose("unused_grasp_pose"), + ), + binding=_dual_binding(action, "source", "destination"), motion_policy=MotionPolicy(sample_count=30), ) plan = _plan_action(action, invocation, _dual_context(task)) assert plan.plan_success.all() + assert plan.scene_dependencies == () + handover_object_pose = action._resolve_receive_grasp.call_args.args[1] + expected_current_object_pose = torch.bmm( + current_eef, + pose_inv(held.object_to_eef), + ) + assert torch.allclose( + handover_object_pose[:, :3, :3], + expected_current_object_pose[:, :3, :3], + ) assert torch.equal(handover_options.final_object_pose, original_final_pose) - assert [segment.name for segment in plan.segments] == [ - "transfer", - "approach", - "close", - "hold", - "release", - "deliver", - ] + semantics.entity.get_local_pose.assert_not_called() + assert tuple(segment.name for segment in plan.segments) == expected_segments + assert plan.segments[0].start == 0 + assert all( + previous.stop == current.start + for previous, current in zip(plan.segments, plan.segments[1:]) + ) + assert plan.segments[-1].stop == plan.commands.frame_count + + +def test_handover_replan_resolves_named_targets_from_latest_snapshot() -> None: + generator = _dual_motion_generator() + action = _bind_action( + generator, + HandOver( + default_options=HandOverOptions( + middle_object_pose=SceneEntityPose("target"), + final_object_pose=SceneEntityPose("target"), + ) + ), + ) + semantics = _semantics(entity_id="handover_object") + task = TaskState( + batch_size=NUM_ENVS, + device="cpu", + held_objects={"left_arm": _held(semantics)}, + ) + invocation = ActionInvocation( + skill_id="hand_over", + goal=GraspGoal(semantics=semantics), + binding=_dual_binding(action, "source", "destination"), + ) + request = action.resolve_request(invocation) + assert action._scene_dependencies(request) == ("target",) + captured: list[torch.Tensor] = [] + original_resolve_matrix = action._resolve_matrix + + def capture_middle(matrix: torch.Tensor, name: str) -> torch.Tensor: + if name == "middle_object_pose": + captured.append(matrix.clone()) + raise RuntimeError("captured target") + return original_resolve_matrix(matrix, name) + + action._resolve_matrix = capture_middle # type: ignore[method-assign] + first_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + first_pose[:, 0, 3] = 0.3 + with pytest.raises(RuntimeError, match="captured target"): + action.plan( + request, + _dual_context( + task, + scene=_target_scene(first_pose, timestamp=0.0, version=0), + ), + ) + second_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + second_pose[:, 0, 3] = 0.7 + with pytest.raises(RuntimeError, match="captured target"): + action.plan( + request, + _dual_context( + task, + scene=_target_scene(second_pose, timestamp=0.0, version=1), + ), + ) + + torch.testing.assert_close(captured[0], first_pose) + torch.testing.assert_close(captured[1], second_pose) def test_handover_holds_only_environment_with_ik_failure() -> None: @@ -1007,11 +1941,11 @@ def fail_second_receiving_arm( return success, qpos generator.robot.compute_ik.side_effect = fail_second_receiving_arm - semantics = _semantics() + semantics = _semantics(entity_id="handover_object") task = TaskState( batch_size=NUM_ENVS, device="cpu", - held_objects={"left_arm": _held(semantics)}, + held_objects={"logical_source": _held(semantics)}, ) action = _bind_action( generator, @@ -1035,40 +1969,100 @@ def fail_second_receiving_arm( invocation = ActionInvocation( skill_id="hand_over", goal=GraspGoal(semantics=semantics), - binding=_dual_binding("source", "destination"), + binding=_dual_binding( + action, + "source", + "destination", + task_state_keys={ + "source": "logical_source", + "destination": "logical_destination", + }, + ), motion_policy=MotionPolicy(sample_count=30), ) - plan = _plan_action(action, invocation, context) + request = action.resolve_request(invocation) + plan = action.plan(request, context) projected = plan.expected_effects.apply(context.task, plan.plan_success) assert plan.plan_success.tolist() == [True, False] - assert not torch.allclose(plan.trajectory.positions[0], context.robot.qpos[0]) + trajectory = _joint_trajectory(plan) + assert not torch.allclose(trajectory.positions[0], context.robot.qpos[0]) assert torch.allclose( - plan.trajectory.positions[1], + trajectory.positions[1], context.robot.qpos[1].unsqueeze(0).expand(30, -1), ) - received = projected.get_held_object("right_arm") + assert all(not frame.active_mask[1].item() for frame in plan.commands.frames) + received = projected.get_held_object("logical_destination") assert received is not None assert received.env_mask.tolist() == [True, False] + assert projected.get_held_object("right_arm") is None + semantics.entity.get_local_pose.assert_not_called() -def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None: +def test_handover_rejects_goal_for_a_different_held_object() -> None: + generator = _dual_motion_generator() + action = _bind_action( + generator, + HandOver( + default_options=HandOverOptions( + middle_object_pose=torch.eye(4), + final_object_pose=torch.eye(4), + ) + ), + ) + held_semantics = _semantics(entity_id="held_object") + goal_semantics = _semantics(entity_id="other_object") + task = TaskState( + batch_size=NUM_ENVS, + device="cpu", + held_objects={"left_arm": _held(held_semantics)}, + ) + invocation = ActionInvocation( + skill_id="hand_over", + goal=GraspGoal(semantics=goal_semantics), + binding=_dual_binding(action, "source", "destination"), + ) + + with pytest.raises(ValueError, match="must identify the object held"): + _plan_action(action, invocation, _dual_context(task)) + + held_semantics.entity.get_local_pose.assert_not_called() + goal_semantics.entity.get_local_pose.assert_not_called() + + +@pytest.mark.parametrize( + ("hold_steps", "expected_segments"), + ( + (0, ("approach", "close", "lift", "move")), + (2, ("approach", "close", "lift", "move", "hold")), + ), +) +def test_coordinated_pick_returns_full_dof_plan_and_omits_empty_hold( + hold_steps: int, + expected_segments: tuple[str, ...], +) -> None: generator = _dual_motion_generator() action = _bind_action( generator, CoordinatedPickment( default_options=CoordinatedPickmentOptions( hand_interp_steps=4, - hold_steps=2, + hold_steps=hold_steps, object_motion_keyframes=3, ), ), ) affordance = AntipodalAffordance() _stub_dual_arm_grasp_poses(affordance) + entity = Mock() + entity.get_local_pose.return_value = torch.full((NUM_ENVS, 4, 4), 9.0) semantics = ObjectSemantics( - affordance=affordance, geometry={}, label="coordinated-object" + affordance=affordance, + geometry={}, + label="coordinated-object", + entity=entity, + entity_id="coordinated_object", ) invocation = ActionInvocation( skill_id="coordinated_pickment", @@ -1077,29 +2071,178 @@ def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None object_target_pose=torch.eye(4), object_initial_pose=torch.eye(4), ), - binding=_dual_binding("left", "right"), + binding=_dual_binding( + action, + "left", + "right", + task_state_keys={ + "left": "logical_left", + "right": "logical_right", + }, + ), motion_policy=MotionPolicy(sample_count=30), ) context = _dual_context() - plan = _plan_action(action, invocation, context) + request = action.resolve_request(invocation) + plan = action.plan(request, context) projected = plan.expected_effects.apply(context.task, plan.plan_success) assert plan.plan_success.tolist() == [True, True] - assert plan.trajectory.positions.shape == (NUM_ENVS, 30, DUAL_ROBOT_DOF) - assert projected.get_held_object("left_arm") is None - assert projected.get_held_object("right_arm") is None + assert _joint_trajectory(plan).positions.shape == ( + NUM_ENVS, + 30, + DUAL_ROBOT_DOF, + ) + assert plan.commands.frame_count == 30 + assert {target.target_id for target in plan.commands.targets} == { + "left_arm", + "left_hand", + "right_arm", + "right_hand", + } + assert plan.scene_dependencies == () + request.goal.semantics.entity.get_local_pose.assert_not_called() + assert projected.get_held_object("logical_left") is None + assert projected.get_held_object("logical_right") is None assert isinstance( - projected.get_coordinated_held_object("left_arm", "right_arm"), + projected.get_coordinated_held_object("logical_left", "logical_right"), CoordinatedHeldObjectState, ) - assert [segment.name for segment in plan.segments] == [ - "approach", - "close", - "lift", - "move", - "hold", + assert projected.get_coordinated_held_object("left_arm", "right_arm") is None + assert tuple(segment.name for segment in plan.segments) == expected_segments + assert plan.segments[0].start == 0 + assert all( + previous.stop == current.start + for previous, current in zip(plan.segments, plan.segments[1:]) + ) + assert plan.segments[-1].stop == plan.commands.frame_count + + +def test_coordinated_pick_implicit_initial_pose_uses_scene_snapshot() -> None: + generator = _dual_motion_generator() + action = _bind_action( + generator, + CoordinatedPickment( + default_options=CoordinatedPickmentOptions( + hand_interp_steps=4, + hold_steps=2, + object_motion_keyframes=3, + ), + ), + ) + affordance = AntipodalAffordance() + _stub_dual_arm_grasp_poses(affordance) + entity = Mock() + entity.get_local_pose.return_value = torch.full((NUM_ENVS, 4, 4), 9.0) + semantics = ObjectSemantics( + affordance=affordance, + geometry={}, + label="snapshot-coordinated-object", + entity=entity, + entity_id="target", + ) + object_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + object_pose[:, :3, :3] = torch.tensor( + [[0.0, 0.0, 1.0], [0.0, 1.0, 0.0], [-1.0, 0.0, 0.0]] + ) + object_pose[:, 1, 3] = torch.tensor([0.2, 0.4]) + invocation = ActionInvocation( + skill_id="coordinated_pickment", + goal=CoordinatedPickGoal( + semantics=semantics, + object_target_pose=object_pose, + ), + binding=_dual_binding(action, "left", "right"), + motion_policy=MotionPolicy(sample_count=30), + ) + context = _dual_context(scene=_target_scene(object_pose, timestamp=0.0, version=0)) + + request = action.resolve_request(invocation) + plan = action.plan(request, context) + projected = plan.expected_effects.apply(context.task, plan.plan_success) + + resolved_affordance = request.goal.semantics.affordance + sampled_pose = resolved_affordance.get_dual_arm_valid_grasp_poses.call_args.kwargs[ + "obj_poses" ] + assert torch.equal(sampled_pose, object_pose) + assert plan.scene_dependencies == ("target",) + request.goal.semantics.entity.get_local_pose.assert_not_called() + coordinated = projected.get_coordinated_held_object("left_arm", "right_arm") + assert coordinated is not None + assert torch.allclose(coordinated.left_object_to_eef, pose_inv(object_pose)) + assert torch.allclose(coordinated.right_object_to_eef, pose_inv(object_pose)) + + +def test_assemble_place_uses_explicit_base_snapshot() -> None: + generator = _motion_generator() + action = _bind_action(generator, Place()) + base_entity = Mock() + base_entity.get_local_pose.return_value = torch.full((NUM_ENVS, 4, 4), 9.0) + relative_pose = torch.eye(4) + relative_pose[2, 3] = 0.05 + affordance = AssembleAffordance( + base_object_entity=base_entity, + assemble_to_base_pose=relative_pose, + ) + task = TaskState( + batch_size=NUM_ENVS, + device="cpu", + held_objects={"logical_arm": _held(_semantics(entity_id="assemble_object"))}, + ) + base_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + base_pose[:, 0, 3] = torch.tensor([0.2, 0.4]) + context = _context( + task, + scene=SceneSnapshot( + timestamp=0.0, + version=0, + entities={"base": EntityState(base_pose)}, + ), + ) + + request = action.resolve_request( + replace( + _invocation( + action, + AssembleGoal( + affordance=affordance, + base_pose=SceneEntityPose("base"), + ), + ), + binding=_binding(action, task_state_key="logical_arm"), + ) + ) + plan = action.plan(request, context) + + assert plan.plan_success.all() + assert plan.scene_dependencies == ("base",) + request.goal.affordance.base_object_entity.get_local_pose.assert_not_called() + + +def test_assemble_place_legacy_base_entity_warns() -> None: + generator = _motion_generator() + action = _bind_action(generator, Place()) + base_entity = Mock() + base_entity.get_local_pose.return_value = torch.eye(4) + affordance = AssembleAffordance(base_object_entity=base_entity) + task = TaskState( + batch_size=NUM_ENVS, + device="cpu", + held_objects={"arm": _held()}, + ) + + request = action.resolve_request( + _invocation(action, AssembleGoal(affordance=affordance)) + ) + with pytest.warns(DeprecationWarning, match="base_pose"): + plan = action.plan(request, _context(task)) + + assert plan.scene_dependencies == () + request.goal.affordance.base_object_entity.get_local_pose.assert_called_once_with( + to_matrix=True + ) def test_coordinated_pick_holds_only_environment_with_ik_failure() -> None: @@ -1145,7 +2288,7 @@ def fail_second_environment( object_target_pose=target_pose, object_initial_pose=torch.eye(4), ), - binding=_dual_binding("left", "right"), + binding=_dual_binding(action, "left", "right"), motion_policy=MotionPolicy(sample_count=30), ) context = _dual_context() @@ -1154,14 +2297,16 @@ def fail_second_environment( projected = plan.expected_effects.apply(context.task, plan.plan_success) assert plan.plan_success.tolist() == [True, False] - assert not torch.allclose(plan.trajectory.positions[0], context.robot.qpos[0]) + trajectory = _joint_trajectory(plan) + assert not torch.allclose(trajectory.positions[0], context.robot.qpos[0]) assert torch.allclose( - plan.trajectory.positions[1], + trajectory.positions[1], context.robot.qpos[1].unsqueeze(0).repeat(30, 1), ) - held = projected.get_coordinated_held_object("left_arm", "right_arm") - assert held is not None - assert held.env_mask.tolist() == [True, False] + assert all(not frame.active_mask[1].item() for frame in plan.commands.frames) + coordinated = projected.get_coordinated_held_object("left_arm", "right_arm") + assert coordinated is not None + assert coordinated.env_mask.tolist() == [True, False] def test_coordinated_pick_fails_when_affordance_has_no_grasp() -> None: @@ -1204,24 +2349,41 @@ def test_coordinated_pick_fails_when_affordance_has_no_grasp() -> None: object_target_pose=torch.eye(4), object_initial_pose=torch.eye(4), ), - binding=_dual_binding("left", "right"), + binding=_dual_binding(action, "left", "right"), motion_policy=MotionPolicy(sample_count=30), ) plan = _plan_action(action, invocation, _dual_context()) assert plan.plan_success.tolist() == [False, False] - assert plan.trajectory.positions.shape == (NUM_ENVS, 0, DUAL_ROBOT_DOF) + assert plan.commands.frame_count == 0 + assert _joint_trajectory(plan).positions.shape == ( + NUM_ENVS, + 0, + DUAL_ROBOT_DOF, + ) -def test_coordinated_placement_projects_release_and_support_attachment() -> None: +@pytest.mark.parametrize( + ("release", "hold_steps", "expected_segments"), + ( + (False, 0, ("approach", "retreat")), + (True, 3, ("approach", "hold", "release", "retreat")), + ), +) +def test_coordinated_placement_projects_effects_and_omits_empty_segments( + release: bool, + hold_steps: int, + expected_segments: tuple[str, ...], +) -> None: generator = _dual_motion_generator() action = _bind_action( generator, CoordinatedPlacement( default_options=CoordinatedPlacementOptions( + release=release, hand_interp_steps=4, - hold_steps=3, + hold_steps=hold_steps, retreat_steps=5, ), ), @@ -1235,7 +2397,10 @@ def test_coordinated_placement_projects_release_and_support_attachment() -> None task = TaskState( batch_size=NUM_ENVS, device="cpu", - held_objects={"left_arm": placing, "right_arm": support}, + held_objects={ + "logical_placing": placing, + "logical_support": support, + }, ) invocation = ActionInvocation( skill_id="coordinated_placement", @@ -1243,7 +2408,15 @@ def test_coordinated_placement_projects_release_and_support_attachment() -> None placing_object_target_pose=torch.eye(4), support_object_target_pose=torch.eye(4), ), - binding=_dual_binding("placing", "support"), + binding=_dual_binding( + action, + "placing", + "support", + task_state_keys={ + "placing": "logical_placing", + "support": "logical_support", + }, + ), motion_policy=MotionPolicy(sample_count=30), ) context = _dual_context(task) @@ -1252,16 +2425,59 @@ def test_coordinated_placement_projects_release_and_support_attachment() -> None projected = plan.expected_effects.apply(context.task, plan.plan_success) assert plan.plan_success.tolist() == [True, True] - assert plan.trajectory.positions.shape == (NUM_ENVS, 30, DUAL_ROBOT_DOF) - assert projected.get_held_object("left_arm") is None - assert projected.get_held_object("right_arm") is not None - assert projected.get_held_object("right_arm").semantics is support.semantics - assert [segment.name for segment in plan.segments] == [ - "approach", - "hold", - "release", - "retreat", - ] + assert plan.commands.frame_count == 30 + assert {target.target_id for target in plan.commands.targets} == { + "left_arm", + "left_hand", + "right_arm", + "right_hand", + } + projected_placing = projected.get_held_object("logical_placing") + if release: + assert projected_placing is None + else: + assert projected_placing is not None + assert projected_placing.semantics is placing.semantics + assert torch.equal(projected_placing.object_to_eef, placing.object_to_eef) + assert projected.get_held_object("logical_support") is not None + assert projected.get_held_object("logical_support").semantics is support.semantics + assert projected.get_held_object("right_arm") is None + assert tuple(segment.name for segment in plan.segments) == expected_segments + assert plan.segments[0].start == 0 + assert all( + previous.stop == current.start + for previous, current in zip(plan.segments, plan.segments[1:]) + ) + assert plan.segments[-1].stop == plan.commands.frame_count + + +def test_coordinated_placement_rejects_one_object_held_by_both_arms() -> None: + generator = _dual_motion_generator() + action = _bind_action(generator, CoordinatedPlacement()) + semantics = _semantics() + task = TaskState( + batch_size=NUM_ENVS, + device="cpu", + held_objects={ + "left_arm": _held(semantics), + "right_arm": _held(semantics), + }, + ) + invocation = ActionInvocation( + skill_id="coordinated_placement", + goal=CoordinatedPlacementGoal( + placing_object_target_pose=torch.eye(4), + support_object_target_pose=torch.eye(4), + ), + binding=_dual_binding(action, "placing", "support"), + motion_policy=MotionPolicy(sample_count=30), + ) + + plan = _plan_action(action, invocation, _dual_context(task)) + + assert plan.plan_success.tolist() == [False, False] + assert _joint_trajectory(plan).waypoint_count == 0 + generator.robot.compute_ik.assert_not_called() def test_coordinated_placement_holds_only_environment_with_ik_failure() -> None: @@ -1314,7 +2530,7 @@ def fail_second_support_arm( placing_object_target_pose=torch.eye(4), support_object_target_pose=torch.eye(4), ), - binding=_dual_binding("placing", "support"), + binding=_dual_binding(action, "placing", "support"), motion_policy=MotionPolicy(sample_count=30), ) @@ -1322,11 +2538,13 @@ def fail_second_support_arm( projected = plan.expected_effects.apply(context.task, plan.plan_success) assert plan.plan_success.tolist() == [True, False] - assert not torch.allclose(plan.trajectory.positions[0], context.robot.qpos[0]) + trajectory = _joint_trajectory(plan) + assert not torch.allclose(trajectory.positions[0], context.robot.qpos[0]) assert torch.allclose( - plan.trajectory.positions[1], + trajectory.positions[1], context.robot.qpos[1].unsqueeze(0).expand(30, -1), ) + assert all(not frame.active_mask[1].item() for frame in plan.commands.frames) supported = projected.get_held_object("right_arm") assert supported is not None assert supported.env_mask.tolist() == [True, True] @@ -1352,7 +2570,7 @@ def test_coordinated_actions_reject_curobo_motion_generation() -> None: object_target_pose=torch.eye(4), object_initial_pose=torch.eye(4), ), - binding=_dual_binding("left", "right"), + binding=_dual_binding(pick, "left", "right"), motion_policy=policy, ) @@ -1366,7 +2584,7 @@ def test_coordinated_actions_reject_curobo_motion_generation() -> None: placement_invocation = ActionInvocation( skill_id="coordinated_placement", goal=CoordinatedPlacementGoal(torch.eye(4), torch.eye(4)), - binding=_dual_binding("placing", "support"), + binding=_dual_binding(placement, "placing", "support"), motion_policy=policy, ) with pytest.raises(ValueError, match="not supported"): diff --git a/tests/sim/atomic_actions/test_affordance.py b/tests/sim/atomic_actions/test_affordance.py index 4c05e0844..3835c801b 100644 --- a/tests/sim/atomic_actions/test_affordance.py +++ b/tests/sim/atomic_actions/test_affordance.py @@ -18,14 +18,19 @@ from __future__ import annotations -import torch from unittest.mock import Mock +import pytest +import torch + from embodichain.lab.sim.atomic_actions.affordance import ( Affordance, AntipodalAffordance, AssembleAffordance, InteractionPoints, + PressAffordance, + SlideAffordance, + TwistAffordance, ) @@ -99,6 +104,29 @@ def test_valid_grasp_poses_casts_approach_direction_to_generator_device(self): assert approach_direction.dtype == torch.float32 assert approach_direction.device == generator.device + def test_valid_grasp_poses_applies_object_aware_cost_callback(self): + aff = AntipodalAffordance() + generator = Mock() + generator.device = torch.device("cpu") + object_pose = torch.eye(4) + object_pose[0, 3] = 0.25 + grasp_poses = torch.eye(4).repeat(2, 1, 1) + costs = torch.tensor([0.2, 0.4]) + + def get_valid_grasp_poses(**kwargs): + adjusted = kwargs["pose_cost_fn"](grasp_poses, costs) + return True, grasp_poses, 0.0, adjusted + + generator.get_valid_grasp_poses.side_effect = get_valid_grasp_poses + aff._generator = generator + + results = aff.get_valid_grasp_poses( + object_pose.unsqueeze(0), + grasp_cost_fn=lambda obj, _grasps, current: current + obj[0, 3], + ) + + assert torch.allclose(results[0][1], torch.tensor([0.45, 0.65])) + def test_best_grasp_poses_casts_approach_direction_to_generator_device(self): aff = AntipodalAffordance() generator = Mock() @@ -116,6 +144,186 @@ def test_best_grasp_poses_casts_approach_direction_to_generator_device(self): assert approach_direction.device == generator.device +class TestTwistAffordance: + def test_requires_explicit_grasp_position_and_axis_origin(self): + with pytest.raises(TypeError, match="grasp_position"): + TwistAffordance() # type: ignore[call-arg] + + @pytest.mark.parametrize( + "twist_axis", + ( + torch.tensor([1.0, 0.0, 1.0]), + torch.tensor([0.0, 0.0, 1.0]), + torch.tensor([0.0, 0.0, -1.0]), + ), + ) + def test_builds_right_handed_orthonormal_grasp_frame(self, twist_axis): + link_pose = torch.eye(4).repeat(2, 1, 1) + link_pose[:, :3, 3] = torch.tensor([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]]) + affordance = TwistAffordance( + grasp_position=(0.25, -0.5, 0.75), + axis_origin=(0.1, 0.2, 0.3), + twist_axis=twist_axis, + ) + + grasp_pose = affordance.get_grasp_pose(link_pose) + rotation = grasp_pose[:, :3, :3] + + assert torch.allclose( + grasp_pose[:, :3, 3], + link_pose[:, :3, 3] + torch.tensor([0.25, -0.5, 0.75]).expand(2, -1), + ) + assert torch.allclose( + torch.matmul(rotation.transpose(1, 2), rotation), + torch.eye(3).expand(2, -1, -1), + atol=1.0e-6, + ) + assert torch.allclose(torch.linalg.det(rotation), torch.ones(2), atol=1.0e-6) + + +class TestSlideAffordance: + def test_uses_local_antipodal_mesh_with_batched_directions(self): + vertices = torch.tensor( + [ + [-1.0, 0.0, 0.0], + [1.0, 0.0, 0.0], + [0.0, 0.0, 0.0], + ] + ) + triangles = torch.tensor([[0, 1, 2]]) + link_pose = torch.eye(4).repeat(2, 1, 1) + link_pose[:, :3, 3] = torch.tensor([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]]) + affordance = SlideAffordance( + mesh_vertices=vertices, + mesh_triangles=triangles, + translation_axis=torch.tensor([0.0, -1.0, 0.0]), + ) + generator = Mock() + generator.device = torch.device("cpu") + first_grasp = torch.eye(4) + first_grasp[:3, 3] = torch.tensor([1.0, 2.0, 3.0]) + second_grasp = torch.eye(4) + second_grasp[:3, 3] = torch.tensor([4.0, 5.0, 6.0]) + generator.get_grasp_poses.side_effect = ( + (True, first_grasp, 0.03), + (True, second_grasp, 0.04), + ) + affordance._generator = generator + approach_directions = torch.tensor([[0.0, -1.0, 0.0], [1.0, 0.0, 0.0]]) + + success, grasp_poses, open_lengths = affordance.get_best_grasp_poses( + link_pose, + approach_direction=approach_directions, + ) + + assert isinstance(affordance, AntipodalAffordance) + assert success.tolist() == [True, True] + assert torch.allclose(grasp_poses, torch.stack([first_grasp, second_grasp])) + assert torch.allclose(open_lengths, torch.tensor([0.03, 0.04])) + assert torch.equal( + generator.get_grasp_poses.call_args_list[0].args[1], + approach_directions[0], + ) + assert torch.equal( + generator.get_grasp_poses.call_args_list[1].args[1], + approach_directions[1], + ) + + def test_requires_local_antipodal_geometry(self): + with pytest.raises(TypeError, match="mesh_vertices"): + SlideAffordance() + + @pytest.mark.parametrize( + "translation_axis", + ( + torch.zeros(3), + torch.tensor([float("nan"), 0.0, 0.0]), + torch.zeros(2), + ), + ) + def test_rejects_invalid_translation_axis(self, translation_axis): + with pytest.raises(ValueError, match="translation_axis"): + SlideAffordance( + mesh_vertices=torch.ones(3, 3), + mesh_triangles=torch.tensor([[0, 1, 2]]), + translation_axis=translation_axis, + ) + + +class TestPressAffordance: + def test_requires_explicit_surface_press_position(self): + with pytest.raises(TypeError, match="press_position"): + PressAffordance() # type: ignore[call-arg] + + @pytest.mark.parametrize( + "press_axis", + ( + torch.tensor([1.0, 0.0, 1.0]), + torch.tensor([0.0, 0.0, 1.0]), + torch.tensor([0.0, 0.0, -1.0]), + ), + ) + def test_builds_right_handed_orthonormal_press_frame(self, press_axis): + link_pose = torch.eye(4).repeat(2, 1, 1) + link_pose[:, :3, 3] = torch.tensor([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]]) + affordance = PressAffordance( + press_axis=press_axis, + press_position=(0.25, -0.5, 0.75), + ) + + press_pose = affordance.get_press_pose(link_pose) + rotation = press_pose[:, :3, :3] + + assert torch.allclose( + press_pose[:, :3, 3], + link_pose[:, :3, 3] + torch.tensor([0.25, -0.5, 0.75]).expand(2, -1), + ) + assert torch.allclose( + torch.matmul(rotation.transpose(1, 2), rotation), + torch.eye(3).expand(2, -1, -1), + atol=1.0e-6, + ) + assert torch.allclose(torch.linalg.det(rotation), torch.ones(2), atol=1.0e-6) + + def test_rejects_zero_press_axis(self): + with pytest.raises(ValueError, match="press_axis must be non-zero"): + PressAffordance( + press_axis=torch.zeros(3), + press_position=(0.0, 0.0, 0.0), + ) + + def test_uses_configured_press_position(self): + object_pose = torch.eye(4).repeat(2, 1, 1) + object_pose[:, :3, 3] = torch.tensor([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]]) + affordance = PressAffordance( + press_axis=torch.tensor([1.0, 0.0, 0.0]), + press_position=(0.25, -0.5, 0.75), + ) + + press_pose = affordance.get_press_pose(object_pose) + + assert torch.allclose( + press_pose[:, :3, 3], + object_pose[:, :3, 3] + torch.tensor([0.25, -0.5, 0.75]).expand(2, -1), + ) + + def test_per_call_press_position_overrides_affordance_position(self): + affordance = PressAffordance( + press_axis=torch.tensor([1.0, 0.0, 0.0]), + press_position=(1.0, 1.0, 1.0), + ) + + press_pose = affordance.get_press_pose( + torch.eye(4).unsqueeze(0), + press_position=(0.1, 0.2, 0.3), + ) + + assert torch.allclose( + press_pose[0, :3, 3], + torch.tensor([0.1, 0.2, 0.3]), + ) + + class TestInteractionPoints: def test_default_points_shape(self): assert InteractionPoints().points.shape == (1, 3) @@ -173,23 +381,36 @@ def test_get_assemble_object_pose_single_base_pose(self): assert torch.allclose(result[0], base_pose @ self._rel_pose()) def test_get_assemble_object_pose_broadcasts_across_envs(self): - n_envs = 3 + num_envs = 3 aff = AssembleAffordance(assemble_to_base_pose=self._rel_pose()) - base_pose = torch.eye(4).unsqueeze(0).repeat(n_envs, 1, 1) - base_pose[:, 0, 3] = torch.arange(n_envs, dtype=torch.float32) + base_pose = torch.eye(4).unsqueeze(0).repeat(num_envs, 1, 1) + base_pose[:, 0, 3] = torch.arange(num_envs, dtype=torch.float32) result = aff.get_assemble_object_pose(base_pose) - assert result.shape == (n_envs, 4, 4) + assert result.shape == (num_envs, 4, 4) expected = torch.bmm( - base_pose, self._rel_pose().unsqueeze(0).repeat(n_envs, 1, 1) + base_pose, self._rel_pose().unsqueeze(0).repeat(num_envs, 1, 1) ) assert torch.allclose(result, expected) def test_get_assemble_object_pose_broadcasts_batched_relative_pose(self): - n_envs = 2 - rel = self._rel_pose().unsqueeze(0).repeat(n_envs, 1, 1) + num_envs = 2 + rel = self._rel_pose().unsqueeze(0).repeat(num_envs, 1, 1) aff = AssembleAffordance(assemble_to_base_pose=rel) - base_pose = torch.eye(4).unsqueeze(0).repeat(n_envs, 1, 1) + base_pose = torch.eye(4).unsqueeze(0).repeat(num_envs, 1, 1) base_pose[:, 2, 3] = 0.5 result = aff.get_assemble_object_pose(base_pose) - assert result.shape == (n_envs, 4, 4) + assert result.shape == (num_envs, 4, 4) assert torch.allclose(result, torch.bmm(base_pose, rel)) + + def test_get_assemble_object_pose_rejects_relative_batch_mismatch(self): + aff = AssembleAffordance(assemble_to_base_pose=torch.eye(4).repeat(3, 1, 1)) + base_pose = torch.eye(4).repeat(2, 1, 1) + + with pytest.raises(ValueError, match="batch size must match"): + aff.get_assemble_object_pose(base_pose) + + def test_get_assemble_object_pose_rejects_invalid_base_shape(self): + aff = AssembleAffordance() + + with pytest.raises(ValueError, match="base_pose must have shape"): + aff.get_assemble_object_pose(torch.eye(4).repeat(2, 1, 1, 1)) diff --git a/tests/sim/atomic_actions/test_articulation_effects.py b/tests/sim/atomic_actions/test_articulation_effects.py new file mode 100644 index 000000000..26603cb89 --- /dev/null +++ b/tests/sim/atomic_actions/test_articulation_effects.py @@ -0,0 +1,120 @@ +# ---------------------------------------------------------------------------- +# 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 verified articulation state and masked symbolic effects.""" + +from __future__ import annotations + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + ArticulationJointState, + StateDelta, + TaskState, +) + + +def test_task_state_normalizes_and_owns_articulation_joint_state() -> None: + position = torch.tensor([0.35], dtype=torch.float32) + state = TaskState( + batch_size=2, + device="cpu", + articulation_joints={ + ("drawer", "slide"): ArticulationJointState(position), + }, + ) + + position.fill_(99.0) + observed = state.get_articulation_joint_state("drawer", "slide") + assert observed is not None + assert torch.equal(observed.position, torch.tensor([[0.35], [0.35]])) + assert torch.equal(observed.env_mask, torch.tensor([True, True])) + + +def test_state_delta_merges_articulation_rows_without_overwriting_others() -> None: + state = TaskState( + batch_size=3, + device="cpu", + articulation_joints={ + ("drawer", "slide"): ArticulationJointState( + torch.tensor([[0.0], [0.1], [0.2]]), + ) + }, + ) + candidate = ArticulationJointState( + torch.tensor([[0.5], [0.6], [0.7]]), + env_mask=torch.tensor([True, True, False]), + ) + + updated = StateDelta( + articulation_joint_updates={("drawer", "slide"): candidate} + ).apply(state, torch.tensor([True, False, True])) + + joint = updated.get_articulation_joint_state("drawer", "slide") + assert joint is not None + assert torch.equal(joint.position, torch.tensor([[0.5], [0.1], [0.7]])) + assert torch.equal(joint.env_mask, torch.tensor([True, True, False])) + + +def test_state_delta_removes_only_selected_articulation_rows() -> None: + state = TaskState( + batch_size=2, + device="cpu", + articulation_joints={ + ("drawer", "slide"): ArticulationJointState(torch.tensor([0.4])) + }, + ) + updated = StateDelta(articulation_joint_updates={("drawer", "slide"): None}).apply( + state, torch.tensor([False, True]) + ) + + joint = updated.get_articulation_joint_state("drawer", "slide") + assert joint is not None + assert torch.equal(joint.env_mask, torch.tensor([True, False])) + + removed = StateDelta(articulation_joint_updates={("drawer", "slide"): None}).apply( + updated, torch.tensor([True, False]) + ) + assert removed.get_articulation_joint_state("drawer", "slide") is None + + +def test_articulation_state_and_delta_validate_strictly() -> None: + with pytest.raises(TypeError, match="floating"): + ArticulationJointState(torch.tensor([1], dtype=torch.long)) + with pytest.raises(ValueError, match="finite"): + ArticulationJointState(torch.tensor([float("nan")])) + with pytest.raises(ValueError, match="articulation/joint pairs"): + StateDelta(articulation_joint_updates={("drawer", ""): None}) + with pytest.raises(TypeError, match="ArticulationJointState"): + StateDelta( + articulation_joint_updates={("drawer", "slide"): torch.tensor([0.1])} + ) + + +def test_articulation_state_delta_snapshot_is_independently_owned() -> None: + source = ArticulationJointState(torch.tensor([[0.2], [0.3]])) + delta = StateDelta(articulation_joint_updates={("drawer", "slide"): source}) + snapshot = delta.snapshot() + copied = snapshot.articulation_joint_updates[("drawer", "slide")] + + assert copied is not None + assert copied is not source + assert copied.position.data_ptr() != source.position.data_ptr() + assert torch.equal(copied.position, source.position) + + +__all__: list[str] = [] diff --git a/tests/sim/atomic_actions/test_control.py b/tests/sim/atomic_actions/test_control.py index ee056aeaa..16fd6649e 100644 --- a/tests/sim/atomic_actions/test_control.py +++ b/tests/sim/atomic_actions/test_control.py @@ -24,14 +24,42 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionControlOverrides, ActionPlanningServices, + ControlCommand, ControlPartCommandProfile, + DisjointSlotEndpoints, JointPositionCommand, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, ) +class _BrokenSnapshotCommand(ControlCommand): + """Command double whose snapshot violates the public command contract.""" + + def snapshot(self) -> ControlCommand: + """Return an invalid snapshot for validation coverage.""" + return "invalid" # type: ignore[return-value] + + def equivalent_to(self, other: ControlCommand) -> bool: + """Return whether another command has this test-only type.""" + return isinstance(other, _BrokenSnapshotCommand) + + +class _SelfSnapshotCommand(ControlCommand): + """Command double that leaks its source instance as the snapshot.""" + + def snapshot(self) -> ControlCommand: + """Return this instance in violation of ownership isolation.""" + return self + + def equivalent_to(self, other: ControlCommand) -> bool: + """Return whether another command has this test-only type.""" + return isinstance(other, _SelfSnapshotCommand) + + def _services() -> ActionPlanningServices: robot = Mock() robot.device = torch.device("cpu") @@ -54,12 +82,39 @@ def _services() -> ActionPlanningServices: ) +def _contract() -> SkillBindingContract: + """Return the endpoint contract used by the direct-binding tests.""" + return SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement(endpoint_id="motion"), + SkillEndpointRequirement( + endpoint_id="grasp", + required_commands={"grasp": JointPositionCommand}, + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + ), + ), + ) + + +def _binding(services: ActionPlanningServices): + """Bind the test contract to concrete robot control parts.""" + return services.bind_control_parts( + _contract(), + {"primary": {"motion": "arm", "grasp": "hand"}}, + ) + + def test_joint_position_command_broadcasts_owned_batch() -> None: source = torch.tensor([0.1, 0.2]) command = JointPositionCommand(source) source.fill_(9.0) - resolved = command.resolve(n_envs=3, control_dof=2, device="cpu") + resolved = command.resolve(num_envs=3, control_dof=2, device="cpu") resolved[0].fill_(7.0) assert torch.allclose(resolved[1:], torch.tensor([[0.1, 0.2], [0.1, 0.2]])) @@ -70,68 +125,154 @@ def test_joint_position_command_rejects_incompatible_control_part() -> None: command = JointPositionCommand(torch.zeros(2)) with pytest.raises(ValueError, match="resolved control part has 3"): - command.resolve(n_envs=1, control_dof=3, device="cpu") + command.resolve(num_envs=1, control_dof=3, device="cpu") -def test_control_profile_is_resolved_from_robot_control_part() -> None: - resolved = _services().resolve_binding( - ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, +def test_control_profile_rejects_invalid_command_snapshot_type() -> None: + with pytest.raises(TypeError, match="snapshot.*ControlCommand"): + ControlPartCommandProfile(commands={"stop": _BrokenSnapshotCommand()}) + + +def test_control_profile_rejects_command_snapshot_alias() -> None: + with pytest.raises(TypeError, match="independently owned"): + ControlPartCommandProfile(commands={"stop": _SelfSnapshotCommand()}) + + +def test_control_profile_rejects_command_name_outer_whitespace() -> None: + with pytest.raises(ValueError, match="outer whitespace"): + ControlPartCommandProfile( + commands={" stop ": JointPositionCommand(torch.zeros(1))} ) - ) - grasp = resolved.end_effector().joint_positions( + +def test_resource_free_contract_does_not_require_robot_control_parts() -> None: + robot = object() + generator = Mock(robot=robot, device=torch.device("cpu")) + services = ActionPlanningServices(generator) + + binding = services.bind_control_parts(SkillBindingContract(), {}) + + assert binding.owner_id == services.binding_owner_id + assert binding.endpoints == () + + +def test_control_profile_is_resolved_from_robot_control_part() -> None: + resolved = _binding(_services()) + + grasp = resolved.endpoint("primary", "grasp").joint_positions( "grasp", - n_envs=2, + num_envs=2, device="cpu", ) assert grasp.tolist() == [[1.0, 1.0], [1.0, 1.0]] - with pytest.raises(KeyError, match="Available commands"): - resolved.end_effector().joint_positions( + with pytest.raises(KeyError, match="available commands"): + resolved.endpoint("primary", "grasp").joint_positions( "pinch", - n_envs=2, + num_envs=2, device="cpu", ) -def test_invocation_override_replaces_only_resolved_role_snapshot() -> None: +def test_direct_binding_shares_motion_task_state_key_across_slot_endpoints() -> None: + resolved = _binding(_services()) + + assert resolved.endpoint("primary", "motion").task_state_key == "arm" + assert resolved.endpoint("primary", "grasp").task_state_key == "arm" + + +def test_direct_binding_accepts_explicit_stable_task_state_key() -> None: + resolved = _services().bind_control_parts( + _contract(), + {"primary": {"motion": "arm", "grasp": "hand"}}, + task_state_keys={"primary": "logical_manipulator"}, + ) + + assert {endpoint.task_state_key for endpoint in resolved.endpoints} == { + "logical_manipulator" + } + + +def test_direct_binding_without_motion_requires_unambiguous_task_state_key() -> None: + contract = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement(endpoint_id="grasp"), + SkillEndpointRequirement(endpoint_id="support"), + ), + ), + ) + ) + endpoints = {"primary": {"grasp": "hand", "support": "arm"}} + services = _services() + + with pytest.raises(ValueError, match="no 'motion'.*task_state_keys"): + services.bind_control_parts(contract, endpoints) + + resolved = services.bind_control_parts( + contract, + endpoints, + task_state_keys={"primary": "logical_manipulator"}, + ) + assert {endpoint.task_state_key for endpoint in resolved.endpoints} == { + "logical_manipulator" + } + + +def test_direct_binding_requires_exact_task_state_key_slot_coverage() -> None: + services = _services() + + with pytest.raises(ValueError, match="cover the binding slots exactly"): + services.bind_control_parts( + _contract(), + {"primary": {"motion": "arm", "grasp": "hand"}}, + task_state_keys={}, + ) + + +def test_invocation_override_replaces_only_resolved_endpoint_snapshot() -> None: services = _services() override_source = torch.full((2,), 0.4) overrides = ActionControlOverrides( - end_effectors={ - "primary": {"grasp": JointPositionCommand(override_source)}, + endpoints={ + "primary": { + "grasp": {"grasp": JointPositionCommand(override_source)}, + }, } ) override_source.fill_(8.0) - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ) + binding = _binding(services) - overridden = services.resolve_binding(binding, overrides) - base = services.resolve_binding(binding) - overrides.end_effectors["primary"]["grasp"].positions.fill_(6.0) # type: ignore[attr-defined] + overridden = services.apply_command_overrides(binding, overrides) + base = services.apply_command_overrides(binding, ActionControlOverrides()) + overrides.endpoints["primary"]["grasp"]["grasp"].positions.fill_(6.0) # type: ignore[attr-defined] assert torch.allclose( - overridden.end_effector().joint_positions("grasp", n_envs=1, device="cpu"), + overridden.endpoint("primary", "grasp").joint_positions( + "grasp", num_envs=1, device="cpu" + ), torch.full((1, 2), 0.4), ) assert torch.equal( - base.end_effector().joint_positions("grasp", n_envs=1, device="cpu"), + base.endpoint("primary", "grasp").joint_positions( + "grasp", num_envs=1, device="cpu" + ), torch.ones(1, 2), ) -def test_override_rejects_role_not_present_in_binding() -> None: +def test_override_rejects_endpoint_not_present_in_binding() -> None: services = _services() - binding = ActionBinding(end_effectors={"primary": "hand"}) + binding = _binding(services) overrides = ActionControlOverrides( - end_effectors={ - "destination": {"open": JointPositionCommand(torch.zeros(2))}, + endpoints={ + "destination": { + "grasp": {"open": JointPositionCommand(torch.zeros(2))}, + }, } ) - with pytest.raises(KeyError, match="unbound end effector roles"): - services.resolve_binding(binding, overrides) + with pytest.raises(KeyError, match="unbound endpoints"): + services.apply_command_overrides(binding, overrides) diff --git a/tests/sim/atomic_actions/test_core.py b/tests/sim/atomic_actions/test_core.py index 7383cde33..be26157dc 100644 --- a/tests/sim/atomic_actions/test_core.py +++ b/tests/sim/atomic_actions/test_core.py @@ -18,7 +18,8 @@ from __future__ import annotations -from dataclasses import FrozenInstanceError +from dataclasses import dataclass, FrozenInstanceError, replace +from unittest.mock import Mock import pytest import torch @@ -26,78 +27,440 @@ from embodichain.lab.sim.atomic_actions import ( ActionBinding, ActionInvocation, + ActionOptions, + ActionPlan, Affordance, + AtomicAction, + AtomicActionEngine, DynamicCollisionMode, + EndpointBinding, + EndpointCommand, + EndpointTrackingChannelBinding, + EndpointTrackingFeedbackAddress, EndEffectorPoseGoal, EntityState, + EffectVerificationRequirement, HeldObjectState, + JointPositionPayload, + JointPositionTarget, MotionPolicy, ObjectSemantics, + PlannerDiagnostics, PlanningContext, RecoveryPolicy, + ResolvedActionRequest, RobotObservation, + RuntimeCommandFrame, + RuntimeEndpointTarget, SceneEntityPose, SceneSnapshot, + SkillBindingContract, StateDelta, TaskState, + TimedCommandSequence, + TimedTerminalAcceptance, + TimedTrackingSequence, TimedTrajectory, + TrackingFeedbackSourceRef, + TrackingFrame, + TrackingPolicy, + TrackingProjectorRef, + TrackingSetpoint, + JointPositionTrackingState, ) from embodichain.lab.sim.atomic_actions.goals import ( + _resolve_object_pose, collect_scene_dependencies, resolve_pose_goal, ) +from embodichain.lab.sim.common import BatchEntity +from embodichain.lab.sim.planners import ToppraPlanOptions -def _semantics(label: str = "object") -> ObjectSemantics: - return ObjectSemantics(affordance=Affordance(), geometry={}, label=label) +def _semantics( + label: str = "object", + *, + entity: BatchEntity | None = None, + entity_id: str | None = None, +) -> ObjectSemantics: + return ObjectSemantics( + affordance=Affordance(), + geometry={}, + label=label, + entity=entity, + entity_id=entity_id, + ) -def _held(batch_size: int = 2) -> HeldObjectState: +def _held( + batch_size: int = 2, + *, + semantics: ObjectSemantics | None = None, + env_mask: torch.Tensor | None = None, +) -> HeldObjectState: pose = torch.eye(4).repeat(batch_size, 1, 1) return HeldObjectState( - semantics=_semantics(), + semantics=semantics or _semantics(), object_to_eef=pose, grasp_xpos=pose, + env_mask=env_mask, ) -def _context(scene: SceneSnapshot | None = None) -> PlanningContext: +def _context( + scene: SceneSnapshot | None = None, + *, + control_dt: float | None = None, +) -> PlanningContext: qpos = torch.zeros(2, 4) return PlanningContext( robot=RobotObservation(timestamp=1.0, qpos=qpos, qvel=torch.zeros_like(qpos)), task=TaskState.empty(batch_size=2, device="cpu"), scene=scene or SceneSnapshot.empty(), env_ids=torch.tensor([4, 7], dtype=torch.long), + control_dt=control_dt, + ) + + +def _command_sequence( + *, + env_ids: torch.Tensor, + frame_count: int, + targets: tuple[JointPositionTarget, ...] | None = None, + positions: tuple[torch.Tensor, ...] | None = None, + velocities: tuple[torch.Tensor | None, ...] | None = None, +) -> TimedCommandSequence: + batch_size = int(env_ids.shape[0]) + if targets is None: + target = JointPositionTarget("arm", (0, 1)) + targets = (target,) * frame_count + if len(targets) != frame_count: + raise ValueError("targets must contain one value per command frame.") + if positions is not None and len(positions) != frame_count: + raise ValueError("positions must contain one value per command frame.") + if velocities is not None and len(velocities) != frame_count: + raise ValueError("velocities must contain one value per command frame.") + frames = tuple( + RuntimeCommandFrame( + commands=( + EndpointCommand( + target=targets[index], + payload=JointPositionPayload( + ( + torch.full( + (batch_size, len(targets[index].joint_ids)), + float(index + 1), + device=env_ids.device, + ) + if positions is None + else positions[index] + ), + velocities=(None if velocities is None else velocities[index]), + ), + ), + ), + active_mask=torch.ones( + batch_size, + dtype=torch.bool, + device=env_ids.device, + ), + env_ids=env_ids, + hold_duration=torch.full( + (batch_size,), + 0.1, + device=env_ids.device, + ), + ) + for index in range(frame_count) + ) + return TimedCommandSequence(frames=frames, env_ids=env_ids) + + +class _AlternateJointPositionTarget(JointPositionTarget): + """Distinct exact target type sharing joint-position transport semantics.""" + + +@dataclass(frozen=True, slots=True) +class _ClaimedTarget(RuntimeEndpointTarget): + """Non-joint target used to verify binding claim authorization.""" + + name: str + + @property + def transport_id(self) -> str: + return JointPositionTarget.TRANSPORT_ID + + @property + def target_id(self) -> str: + return self.name + + +def _action_plan( + commands: TimedCommandSequence, + *, + plan_success: torch.Tensor | None = None, + joint_trajectory: TimedTrajectory | None = None, + tracking_policy: TrackingPolicy | None = None, + tracking: TimedTrackingSequence | None = None, + expected_effects: StateDelta | None = None, + effect_verification: EffectVerificationRequirement | None = None, + diagnostics: PlannerDiagnostics | None = None, + scene_dependencies: tuple[str, ...] = (), + scene_dependency_monitor_until: dict[str, int] | None = None, +) -> ActionPlan: + if plan_success is None: + plan_success = torch.ones( + commands.batch_size, + dtype=torch.bool, + device=commands.device, + ) + return ActionPlan( + skill_id="test", + plan_success=plan_success, + commands=commands, + recovery_policy=RecoveryPolicy(), + tracking_policy=( + TrackingPolicy.timed() if tracking_policy is None else tracking_policy + ), + planned_scene_version=0, + planned_collision_world_revision=(0,) * commands.batch_size, + diagnostics=( + PlannerDiagnostics(backend="test") if diagnostics is None else diagnostics + ), + tracking=tracking, + joint_trajectory=joint_trajectory, + scene_dependencies=scene_dependencies, + scene_dependency_monitor_until=( + {} + if scene_dependency_monitor_until is None + else scene_dependency_monitor_until + ), + expected_effects=StateDelta() if expected_effects is None else expected_effects, + effect_verification=effect_verification, ) -def test_action_binding_is_role_based_and_immutable() -> None: +def _joint_tracking_sequence( + commands: TimedCommandSequence, +) -> TimedTrackingSequence: + frames: list[TrackingFrame] = [] + for command_frame in commands.frames: + setpoints: list[TrackingSetpoint] = [] + for command in command_frame.commands: + assert isinstance(command.target, JointPositionTarget) + assert isinstance(command.payload, JointPositionPayload) + channel = EndpointTrackingChannelBinding( + channel_id="joint.position", + source=TrackingFeedbackSourceRef( + provider_id="planning_context.robot", + revision="1", + address=EndpointTrackingFeedbackAddress( + target=command.target, + channel_id="joint.position", + ), + ), + projector=TrackingProjectorRef( + projector_id="joint_position_payload", + revision="1", + ), + ) + setpoints.append( + TrackingSetpoint( + endpoint_key=("primary", "motion"), + binding=channel, + desired=JointPositionTrackingState(command.payload.positions), + ) + ) + frames.append(TrackingFrame(tuple(setpoints))) + return TimedTrackingSequence(commands.env_ids, tuple(frames)) + + +@pytest.mark.parametrize("kind", ("", " physical", "physical ", 1, True)) +def test_effect_verification_requirement_rejects_invalid_kind(kind: object) -> None: + with pytest.raises(ValueError, match="kind"): + EffectVerificationRequirement(kind=kind) # type: ignore[arg-type] + + +def test_action_plan_owns_explicit_effect_verification_requirement() -> None: + commands = _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, + ) + requirement = EffectVerificationRequirement(kind="articulation.joint_progress") + + plan = _action_plan(commands, effect_verification=requirement) + requirement_snapshot = plan.effect_verification + + assert plan.requires_effect_verification is True + assert requirement_snapshot is not None + assert requirement_snapshot is not requirement + assert requirement_snapshot.kind == requirement.kind + assert requirement_snapshot.snapshot() is not requirement_snapshot + + +def test_action_plan_implicitly_verifies_nonempty_state_delta() -> None: + commands = _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, + ) + effects = StateDelta(held_object_updates={"arm": _held(batch_size=1)}) + + implicit = _action_plan(commands, expected_effects=effects) + no_effect = _action_plan(commands) + + assert implicit.effect_verification is None + assert implicit.requires_effect_verification is True + assert no_effect.requires_effect_verification is False + + +def test_action_plan_rejects_untyped_effect_verification_requirement() -> None: + commands = _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, + ) + + with pytest.raises(TypeError, match="EffectVerificationRequirement"): + _action_plan( + commands, + effect_verification=object(), # type: ignore[arg-type] + ) + + +class _DependencyAction(AtomicAction[EndEffectorPoseGoal, ActionOptions]): + """Minimal action proving that build_plan delegates dependencies to its hook.""" + + skill_id = "dependency_test" + GoalType = EndEffectorPoseGoal + OptionsType = ActionOptions + binding_contract = SkillBindingContract() + + @property + def device(self) -> torch.device: + return torch.device("cpu") + + def _uses_collision_world( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> bool: + del request, context + return False + + def _scene_dependencies( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + ) -> tuple[str, ...]: + dependencies = set(super()._scene_dependencies(request)) + dependencies.add("extra") + return tuple(sorted(dependencies)) + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + raise NotImplementedError + + +class _RawCommandAction(AtomicAction[EndEffectorPoseGoal, ActionOptions]): + """Action that deliberately bypasses build_command_plan for validation.""" + + skill_id = "raw_command_test" + GoalType = EndEffectorPoseGoal + OptionsType = ActionOptions + binding_contract = SkillBindingContract() + + @property + def device(self) -> torch.device: + return torch.device("cpu") + + def _uses_collision_world( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> bool: + del request, context + return False + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + del request + return ActionPlan( + skill_id=self.skill_id, + plan_success=torch.ones(context.batch_size, dtype=torch.bool), + commands=_command_sequence( + env_ids=context.env_ids, + frame_count=1, + ), + recovery_policy=RecoveryPolicy(), + tracking_policy=TrackingPolicy.timed(), + planned_scene_version=context.scene.version, + planned_collision_world_revision=(0,) * context.batch_size, + diagnostics=PlannerDiagnostics(backend="test"), + ) + + +def test_action_binding_is_endpoint_based_and_immutable() -> None: + endpoint = EndpointBinding( + slot_id="primary", + endpoint_id="motion", + resource_id="left_actor", + adapter_id="control_part", + target=JointPositionTarget("left_arm", (0, 1)), + capabilities=frozenset({"motion.test"}), + claim_tokens=frozenset({"robot.control_part:left_arm"}), + ) binding = ActionBinding( - manipulators={"primary": "left_arm"}, - end_effectors={"primary": "left_hand"}, + owner_id="test-engine", + endpoints=(endpoint,), ) - assert binding.manipulator() == "left_arm" - assert binding.end_effector() == "left_hand" - with pytest.raises(TypeError): - binding.manipulators["primary"] = "right_arm" - with pytest.raises(KeyError, match="destination"): - binding.manipulator("destination") + resolved = binding.endpoint("primary", "motion") + target = resolved.require_target(JointPositionTarget) + assert resolved is not binding.endpoints[0] + assert resolved.target is not binding.endpoints[0].target + assert target.control_part == "left_arm" + assert target.joint_ids == (0, 1) + assert resolved.joint_ids == (0, 1) + assert resolved.capabilities == frozenset({"motion.test"}) + with pytest.raises(FrozenInstanceError): + binding.owner_id = "other-engine" # type: ignore[misc] + with pytest.raises(KeyError, match="destination.motion"): + binding.endpoint("destination", "motion") + with pytest.raises(ValueError, match="must match"): + EndpointBinding( + slot_id="primary", + endpoint_id="motion", + resource_id="left_actor", + adapter_id="control_part", + target=JointPositionTarget("left_arm", (0, 1)), + joint_ids=(1, 2), + ) -def test_invocation_rejects_values_without_goal_contract() -> None: - with pytest.raises(TypeError, match="goal_kind"): - ActionInvocation( - skill_id="move_end_effector", - goal=object(), # type: ignore[arg-type] - binding=ActionBinding(manipulators={"primary": "arm"}), +@pytest.mark.parametrize("entity_id", ["", " ", 7]) +def test_object_semantics_rejects_invalid_entity_id(entity_id: object) -> None: + with pytest.raises(ValueError, match="entity_id"): + ObjectSemantics( + affordance=Affordance(), + geometry={}, + entity_id=entity_id, # type: ignore[arg-type] ) +def test_object_semantics_identity_fields_are_frozen() -> None: + semantics = _semantics(entity_id="cube") + + with pytest.raises(FrozenInstanceError): + semantics.entity_id = "other" # type: ignore[misc] + + def test_motion_and_recovery_policy_validate_shared_parameters() -> None: - policy = MotionPolicy(sample_count=24, control_dt=0.01) + policy = MotionPolicy(sample_count=24) assert policy.sample_count == 24 - assert policy.control_dt == 0.01 assert policy.dynamic_collision_mode is DynamicCollisionMode.AUTO with pytest.raises(ValueError, match="sample_count"): MotionPolicy(sample_count=1) @@ -119,18 +482,22 @@ def test_motion_policy_normalizes_dynamic_collision_mode() -> None: def test_motion_policy_maps_to_motion_generator_strategy() -> None: + planner_options = ToppraPlanOptions( + constraints={"velocity": 0.2, "acceleration": 0.5} + ) policy = MotionPolicy( strategy="ik_interp", sample_count=24, - velocity_limit=0.2, - acceleration_limit=0.5, + plan_opts=planner_options, ) + planner_options.constraints["velocity"] = 1.0 start_qpos = torch.zeros(2, 6) options = policy.to_motion_gen_options( start_qpos=start_qpos, control_part="arm", sample_count=12, + interpolation_dt=0.02, ) assert options.strategy == "ik_interp" @@ -138,8 +505,11 @@ def test_motion_policy_maps_to_motion_generator_strategy() -> None: assert options.start_qpos is not start_qpos assert torch.equal(options.start_qpos, start_qpos) assert options.control_part == "arm" - assert options.velocity_limit == 0.2 - assert options.acceleration_limit == 0.5 + assert options.interpolation_dt == pytest.approx(0.02) + assert options.velocity_limit is None + assert options.acceleration_limit is None + assert isinstance(options.plan_opts, ToppraPlanOptions) + assert options.plan_opts.constraints == {"velocity": 0.2, "acceleration": 0.5} def test_task_state_normalizes_held_relations_and_masks_updates() -> None: @@ -164,6 +534,167 @@ def test_task_state_normalizes_held_relations_and_masks_updates() -> None: assert state.get_held_object("right_arm") is None +def test_task_state_reports_per_environment_exclusive_holds() -> None: + entity = Mock(spec=BatchEntity) + shared = _semantics("shared", entity=entity) + same_entity = ObjectSemantics( + affordance=Affordance(), + geometry={}, + label="same-entity-alias", + entity=entity, + ) + independent = _semantics("shared") + state = TaskState( + batch_size=2, + device="cpu", + held_objects={ + "left_arm": _held(semantics=shared), + "right_arm": _held( + semantics=same_entity, + env_mask=torch.tensor([True, False]), + ), + "third_arm": _held( + semantics=independent, + env_mask=torch.tensor([False, True]), + ), + }, + ) + + assert state.held_object_mask("left_arm").tolist() == [True, True] + assert state.exclusive_held_object_mask("left_arm").tolist() == [False, True] + assert state.exclusive_held_object_mask("right_arm").tolist() == [False, False] + assert state.exclusive_held_object_mask("third_arm").tolist() == [False, True] + assert state.held_object_mask("missing").tolist() == [False, False] + + +def test_task_state_treats_matching_entity_ids_as_shared() -> None: + state = TaskState( + batch_size=1, + device="cpu", + held_objects={ + "left_arm": _held( + batch_size=1, + semantics=_semantics(entity_id="tray"), + ), + "right_arm": _held( + batch_size=1, + semantics=_semantics(entity_id="tray"), + ), + }, + ) + + assert state.exclusive_held_object_mask("left_arm").tolist() == [False] + assert state.exclusive_held_object_mask("right_arm").tolist() == [False] + + +def test_state_delta_merges_distinct_semantics_with_same_entity_id() -> None: + previous_semantics = _semantics(entity_id="cube") + candidate_semantics = _semantics(entity_id="cube") + state = TaskState( + batch_size=2, + device="cpu", + held_objects={"arm": _held(semantics=previous_semantics)}, + ) + + updated = StateDelta( + held_object_updates={ + "arm": _held(semantics=candidate_semantics), + } + ).apply(state, torch.tensor([True, False])) + + held = updated.get_held_object("arm") + assert previous_semantics is not candidate_semantics + assert held is not None and held.semantics is previous_semantics + + +def test_state_delta_replaces_semantics_when_all_rows_are_updated() -> None: + previous_semantics = _semantics(entity_id="cube") + candidate_semantics = _semantics(entity_id="cube") + state = TaskState( + batch_size=2, + device="cpu", + held_objects={"arm": _held(semantics=previous_semantics)}, + ) + + updated = StateDelta( + held_object_updates={ + "arm": _held(semantics=candidate_semantics), + } + ).apply(state, torch.tensor([True, True])) + + held = updated.get_held_object("arm") + assert held is not None and held.semantics is candidate_semantics + + +def test_state_delta_rejects_partial_merge_of_different_entity_ids() -> None: + state = TaskState( + batch_size=2, + device="cpu", + held_objects={"arm": _held(semantics=_semantics(entity_id="cube"))}, + ) + delta = StateDelta( + held_object_updates={ + "arm": _held(semantics=_semantics(entity_id="cup")), + } + ) + + with pytest.raises(ValueError, match="different held-object semantics"): + delta.apply(state, torch.tensor([True, False])) + + +def test_state_delta_does_not_match_explicit_id_to_legacy_uid() -> None: + shared_entity = Mock(uid="cube") + previous_semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + entity=shared_entity, + entity_id="cube", + ) + candidate_semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + entity=shared_entity, + ) + state = TaskState( + batch_size=2, + device="cpu", + held_objects={"arm": _held(semantics=previous_semantics)}, + ) + delta = StateDelta( + held_object_updates={"arm": _held(semantics=candidate_semantics)} + ) + + with pytest.raises(ValueError, match="different held-object semantics"): + delta.apply(state, torch.tensor([True, False])) + + +def test_state_delta_merges_legacy_semantics_with_same_uid() -> None: + previous_semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + entity=Mock(uid="cube"), + ) + candidate_semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + entity=Mock(uid="cube"), + ) + state = TaskState( + batch_size=2, + device="cpu", + held_objects={"arm": _held(semantics=previous_semantics)}, + ) + + updated = StateDelta( + held_object_updates={ + "arm": _held(semantics=candidate_semantics), + } + ).apply(state, torch.tensor([True, False])) + + held = updated.get_held_object("arm") + assert held is not None and held.semantics is previous_semantics + + def test_robot_observation_owns_input_tensors() -> None: qpos = torch.zeros(2, 4) observation = RobotObservation( @@ -183,6 +714,7 @@ def test_scene_entity_pose_is_resolved_late_from_snapshot() -> None: offset = torch.eye(4) offset[2, 3] = 0.1 reference = SceneEntityPose("cup", relative_pose=offset) + offset[2, 3] = 9.0 context = _context( SceneSnapshot( timestamp=1.0, @@ -214,6 +746,581 @@ def test_scene_entity_pose_enforces_confidence() -> None: ) +def test_object_pose_uses_explicit_scene_id_without_live_fallback() -> None: + scene_pose = torch.eye(4).repeat(2, 1, 1) + scene_pose[:, 0, 3] = torch.tensor([0.2, 0.4]) + entity = Mock() + entity.get_local_pose.return_value = torch.full((2, 4, 4), 9.0) + semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + entity=entity, + entity_id="cup", + ) + context = _context( + SceneSnapshot( + timestamp=1.0, + version=1, + entities={"cup": EntityState(scene_pose)}, + ) + ) + + resolved = _resolve_object_pose(semantics, context) + + assert torch.equal(resolved, scene_pose) + entity.get_local_pose.assert_not_called() + + +def test_object_pose_missing_explicit_scene_id_does_not_fall_back() -> None: + entity = Mock() + entity.get_local_pose.return_value = torch.eye(4).repeat(2, 1, 1) + semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + entity=entity, + entity_id="missing", + ) + + with pytest.raises(KeyError, match="unknown scene entity"): + _resolve_object_pose(semantics, _context()) + entity.get_local_pose.assert_not_called() + + +def test_object_pose_legacy_entity_warns_and_broadcasts() -> None: + entity = Mock() + entity.get_local_pose.return_value = torch.eye(4) + semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + entity=entity, + ) + + with pytest.warns(DeprecationWarning, match="entity_id"): + resolved = _resolve_object_pose(semantics, _context()) + + assert resolved.shape == (2, 4, 4) + entity.get_local_pose.assert_called_once_with(to_matrix=True) + + +def test_dependency_collection_does_not_descend_object_semantics() -> None: + semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + properties={"unrelated_pose": SceneEntityPose("hidden")}, + entity_id="object", + ) + + assert collect_scene_dependencies(semantics) == () + + +def test_build_plan_uses_action_scene_dependency_hook() -> None: + context = _context() + generator = Mock() + generator.robot = Mock() + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "test" + engine = AtomicActionEngine(generator, load_builtins=False) + action = _DependencyAction() + engine.register(action) + request = ResolvedActionRequest( + skill_id="dependency_test", + goal=EndEffectorPoseGoal(SceneEntityPose("tracked")), + binding=ActionBinding(owner_id=engine.binding_owner_id), + motion_policy=MotionPolicy(), + tracking_policy=TrackingPolicy.timed(), + recovery_policy=RecoveryPolicy(), + skill_options=ActionOptions(), + ) + + with pytest.raises(TypeError, match="TimedTrajectory with explicit dt"): + action.build_plan( + request, + context, + success=True, + trajectory=context.robot.qpos.unsqueeze(1), # type: ignore[arg-type] + ) + + plan = action.build_command_plan( + request, + context, + success=True, + commands=TimedCommandSequence(frames=(), env_ids=context.env_ids), + diagnostics=PlannerDiagnostics(backend="test"), + ) + + assert plan.scene_dependencies == ("extra", "tracked") + + +def test_build_segments_omits_zero_length_entry_and_preserves_offsets() -> None: + approach_length = 2 + release_length = 3 + segment_lengths = { + "approach": approach_length, + "hold": 0, + "release": release_length, + } + + segments = AtomicAction._build_segments( + segment_lengths, + frame_count=sum(segment_lengths.values()), + ) + + assert tuple( + (segment.name, segment.start, segment.stop) for segment in segments + ) == ( + ("approach", 0, approach_length), + ("release", approach_length, approach_length + release_length), + ) + + +def test_build_command_plan_rejects_unbound_runtime_destination() -> None: + context = _context() + generator = Mock() + generator.robot = Mock() + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "test" + engine = AtomicActionEngine(generator, load_builtins=False) + action = _DependencyAction() + engine.register(action) + request = ResolvedActionRequest( + skill_id="dependency_test", + goal=EndEffectorPoseGoal(SceneEntityPose("tracked")), + binding=ActionBinding(owner_id=engine.binding_owner_id), + motion_policy=MotionPolicy(), + tracking_policy=TrackingPolicy.timed(), + recovery_policy=RecoveryPolicy(), + skill_options=ActionOptions(), + ) + + with pytest.raises(ValueError, match="not authorized"): + action.build_command_plan( + request, + context, + success=True, + commands=_command_sequence(env_ids=context.env_ids, frame_count=1), + diagnostics=PlannerDiagnostics(backend="test"), + ) + + +def test_public_plan_authorizes_raw_action_plan_destinations() -> None: + context = _context() + generator = Mock() + generator.robot = Mock() + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "test" + engine = AtomicActionEngine(generator, load_builtins=False) + action = _RawCommandAction() + engine.register(action) + request = ResolvedActionRequest( + skill_id=action.skill_id, + goal=EndEffectorPoseGoal(SceneEntityPose("tracked")), + binding=ActionBinding(owner_id=engine.binding_owner_id), + motion_policy=MotionPolicy(), + tracking_policy=TrackingPolicy.timed(), + recovery_policy=RecoveryPolicy(), + skill_options=ActionOptions(), + ) + + with pytest.raises(ValueError, match="not authorized"): + action.plan(request, context) + + +def test_command_target_authorization_rejects_altered_joint_claims() -> None: + context = _context() + request = ResolvedActionRequest( + skill_id="dependency_test", + goal=EndEffectorPoseGoal(SceneEntityPose("tracked")), + binding=ActionBinding( + owner_id="test-engine", + endpoints=( + EndpointBinding( + slot_id="primary", + endpoint_id="motion", + resource_id="arm", + adapter_id="control_part", + target=JointPositionTarget("arm", (0, 1)), + ), + ), + ), + motion_policy=MotionPolicy(), + tracking_policy=TrackingPolicy.timed(), + recovery_policy=RecoveryPolicy(), + skill_options=ActionOptions(), + ) + frame = RuntimeCommandFrame( + commands=( + EndpointCommand( + target=JointPositionTarget("arm", (2, 3)), + payload=JointPositionPayload(torch.ones(2, 2)), + ), + ), + active_mask=torch.ones(2, dtype=torch.bool), + env_ids=context.env_ids, + hold_duration=torch.full((2,), 0.1), + ) + + with pytest.raises(ValueError, match="bound joint-position target"): + _DependencyAction._authorize_command_targets( + request, + TimedCommandSequence(frames=(frame,), env_ids=context.env_ids), + ) + + +def test_command_target_authorization_rejects_custom_claim_conflicts() -> None: + context = _context() + endpoints = tuple( + EndpointBinding( + slot_id="primary", + endpoint_id=name, + resource_id=name, + adapter_id="test.claimed", + target=_ClaimedTarget(name), + claim_tokens=frozenset({"controller:shared"}), + ) + for name in ("first", "second") + ) + request = ResolvedActionRequest( + skill_id="dependency_test", + goal=EndEffectorPoseGoal(SceneEntityPose("tracked")), + binding=ActionBinding(owner_id="test-engine", endpoints=endpoints), + motion_policy=MotionPolicy(), + tracking_policy=TrackingPolicy.timed(), + recovery_policy=RecoveryPolicy(), + skill_options=ActionOptions(), + ) + frame = RuntimeCommandFrame( + commands=tuple( + EndpointCommand( + target=endpoint.target, + payload=JointPositionPayload(torch.ones(2, 1)), + ) + for endpoint in endpoints + ), + active_mask=torch.ones(2, dtype=torch.bool), + env_ids=context.env_ids, + hold_duration=torch.full((2,), 0.1), + ) + + with pytest.raises(ValueError, match="claim tokens.*controller:shared"): + _DependencyAction._authorize_command_targets( + request, + TimedCommandSequence(frames=(frame,), env_ids=context.env_ids), + ) + + +def test_action_plan_owns_commands_and_optional_joint_trajectory() -> None: + env_ids = torch.tensor([4, 7], dtype=torch.long) + commands = _command_sequence(env_ids=env_ids, frame_count=2) + trajectory_positions = torch.stack( + ( + torch.full((2, 2), 1.0), + torch.full((2, 2), 2.0), + ), + dim=1, + ) + trajectory = TimedTrajectory.from_uniform_step( + trajectory_positions, + env_ids=env_ids, + step_dt=0.1, + ) + plan_success = torch.tensor([True, False]) + + plan = _action_plan( + commands, + plan_success=plan_success, + joint_trajectory=trajectory, + ) + payload = commands.frames[0].commands[0].payload + assert isinstance(payload, JointPositionPayload) + plan_success.zero_() + payload.positions.zero_() + commands.frames[0].active_mask.zero_() + commands.frames[0].hold_duration.zero_() + commands.env_ids.zero_() + trajectory.positions.zero_() + + owned_payload = plan.commands.frames[0].commands[0].payload + assert isinstance(owned_payload, JointPositionPayload) + assert plan.plan_success.tolist() == [True, False] + assert torch.all(owned_payload.positions == 1.0) + assert plan.commands.frames[0].active_mask.tolist() == [True, True] + assert torch.all(plan.commands.frames[0].hold_duration == 0.1) + assert plan.commands.env_ids.tolist() == [4, 7] + assert plan.joint_trajectory is not None + assert torch.equal(plan.joint_trajectory.positions, trajectory_positions) + + +def test_planner_diagnostics_and_plan_snapshots_own_nested_metadata() -> None: + nested = {"solver": {"iterations": [3, 5]}} + diagnostics = PlannerDiagnostics(backend="test", metadata=nested) + plan = _action_plan( + _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, + ), + diagnostics=diagnostics, + ) + + nested["solver"]["iterations"][0] = 99 + diagnostics.metadata["solver"]["iterations"][1] = 77 + snapshot = plan.snapshot() + plan.diagnostics.metadata["solver"]["iterations"][0] = 42 + + assert snapshot.diagnostics.metadata["solver"]["iterations"] == [3, 5] + + +def test_planner_diagnostics_rejects_non_string_messages() -> None: + with pytest.raises(TypeError, match="messages must contain strings"): + PlannerDiagnostics( + backend="test", + messages=("valid", 1), # type: ignore[arg-type] + ) + + +def test_action_plan_owns_scene_dependency_monitor_cutoffs() -> None: + source = {"disabled": 0, "full_sequence": 2} + plan = _action_plan( + _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=2, + ), + scene_dependencies=("disabled", "full_sequence"), + scene_dependency_monitor_until=source, + ) + + source["disabled"] = 1 + source["full_sequence"] = 1 + snapshot = plan.snapshot() + + assert plan.scene_dependency_monitor_until == { + "disabled": 0, + "full_sequence": 2, + } + assert snapshot.scene_dependency_monitor_until == { + "disabled": 0, + "full_sequence": 2, + } + assert snapshot.scene_dependency_monitor_until is not ( + plan.scene_dependency_monitor_until + ) + + +@pytest.mark.parametrize("waypoint_index", (-1, 3, True, 1.5)) +def test_action_plan_rejects_invalid_scene_dependency_monitor_cutoff( + waypoint_index: object, +) -> None: + with pytest.raises(ValueError, match="waypoint indices"): + _action_plan( + _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=2, + ), + scene_dependencies=("tracked",), + scene_dependency_monitor_until={ + "tracked": waypoint_index # type: ignore[dict-item] + }, + ) + + +def test_action_plan_rejects_monitor_cutoff_for_non_dependency() -> None: + with pytest.raises(ValueError, match="keys must be scene dependencies"): + _action_plan( + _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=2, + ), + scene_dependencies=("tracked",), + scene_dependency_monitor_until={"other": 1}, + ) + + +def test_action_plan_allows_timed_commands_without_joint_trajectory() -> None: + commands = _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, + ) + + plan = _action_plan(commands) + + assert plan.commands.frame_count == 1 + assert plan.joint_trajectory is None + assert isinstance(plan.tracking_policy.terminal, TimedTerminalAcceptance) + assert plan.tracking is None + + +def test_action_plan_rejects_command_device_mismatch() -> None: + commands = _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, + ) + + with pytest.raises(ValueError, match="share a device"): + _action_plan( + commands, + plan_success=torch.ones(1, dtype=torch.bool, device="meta"), + ) + + +@pytest.mark.parametrize( + ("trajectory_env_ids", "trajectory_frame_count", "message"), + [ + (torch.tensor([7], dtype=torch.long), 1, "env_ids must match"), + (torch.tensor([4], dtype=torch.long), 2, "waypoints must match"), + ], +) +def test_action_plan_validates_joint_trajectory_against_commands( + trajectory_env_ids: torch.Tensor, + trajectory_frame_count: int, + message: str, +) -> None: + commands = _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, + ) + trajectory = TimedTrajectory.from_uniform_step( + torch.ones(1, trajectory_frame_count, 2), + env_ids=trajectory_env_ids, + step_dt=0.1, + ) + + with pytest.raises(ValueError, match=message): + _action_plan( + commands, + joint_trajectory=trajectory, + ) + + +def test_joint_position_plan_rejects_empty_commands_for_successful_rows() -> None: + env_ids = torch.tensor([4], dtype=torch.long) + commands = TimedCommandSequence(frames=(), env_ids=env_ids) + trajectory = TimedTrajectory.empty( + batch_size=1, + robot_dof=2, + device=env_ids.device, + env_ids=env_ids, + ) + + with pytest.raises(ValueError, match="requires command frames"): + _action_plan( + commands, + plan_success=torch.tensor([True]), + joint_trajectory=trajectory, + tracking_policy=TrackingPolicy.joint_position(), + tracking=_joint_tracking_sequence(commands), + ) + + +@pytest.mark.parametrize("changed_route", ["source", "projector"]) +def test_tracking_plan_rejects_route_changes_between_frames( + changed_route: str, +) -> None: + commands = _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=2, + ) + tracking = _joint_tracking_sequence(commands) + first_frame, second_frame = tracking.frames + original = second_frame.setpoints[0] + source = original.binding.source + projector = original.binding.projector + if changed_route == "source": + source = TrackingFeedbackSourceRef( + provider_id=source.provider_id, + revision="alternate", + address=source.address, + ) + else: + projector = TrackingProjectorRef( + projector_id=projector.projector_id, + revision="alternate", + ) + changed = TrackingSetpoint( + endpoint_key=original.endpoint_key, + binding=EndpointTrackingChannelBinding( + channel_id=original.binding.channel_id, + source=source, + projector=projector, + ), + desired=original.desired, + ) + changed_tracking = TimedTrackingSequence( + commands.env_ids, + (first_frame, TrackingFrame((changed,))), + ) + + with pytest.raises(ValueError, match="source fingerprint and projector route"): + _action_plan( + commands, + tracking_policy=TrackingPolicy.joint_position(), + tracking=changed_tracking, + ) + + +def test_joint_position_plan_allows_empty_commands_when_all_rows_fail() -> None: + env_ids = torch.tensor([4], dtype=torch.long) + commands = TimedCommandSequence(frames=(), env_ids=env_ids) + trajectory = TimedTrajectory.empty( + batch_size=1, + robot_dof=2, + device=env_ids.device, + env_ids=env_ids, + ) + + plan = _action_plan( + commands, + plan_success=torch.tensor([False]), + joint_trajectory=trajectory, + tracking_policy=TrackingPolicy.joint_position(), + tracking=_joint_tracking_sequence(commands), + ) + + assert plan.commands.frame_count == 0 + + +def test_action_plan_requires_stable_destination_set() -> None: + env_ids = torch.tensor([4], dtype=torch.long) + commands = _command_sequence( + env_ids=env_ids, + frame_count=2, + targets=( + JointPositionTarget("arm", (0, 1)), + JointPositionTarget("other_arm", (0, 1)), + ), + ) + with pytest.raises(ValueError, match="same destination set"): + _action_plan(commands) + + +def test_action_plan_requires_stable_exact_target_type() -> None: + env_ids = torch.tensor([4], dtype=torch.long) + commands = _command_sequence( + env_ids=env_ids, + frame_count=2, + targets=( + JointPositionTarget("arm", (0, 1)), + _AlternateJointPositionTarget("arm", (0, 1)), + ), + ) + + with pytest.raises(ValueError, match="exact target type"): + _action_plan(commands) + + +def test_action_plan_requires_stable_target_address_fingerprint() -> None: + env_ids = torch.tensor([4], dtype=torch.long) + commands = _command_sequence( + env_ids=env_ids, + frame_count=2, + targets=( + JointPositionTarget("arm", (0, 1)), + JointPositionTarget("arm", (1, 0)), + ), + ) + + with pytest.raises(ValueError, match="target address fingerprint"): + _action_plan(commands) + + def test_scene_snapshot_expands_global_collision_world_revision() -> None: pose = torch.eye(4).repeat(2, 1, 1) snapshot = SceneSnapshot( @@ -233,6 +1340,54 @@ def test_scene_snapshot_expands_global_collision_world_revision() -> None: assert torch.equal(obstacle_poses["obstacle"], pose) +def test_scene_snapshot_owns_entity_state_storage() -> None: + pose = torch.eye(4) + state = EntityState(pose) + snapshot = SceneSnapshot( + timestamp=0.0, + version=0, + entities={"object": state}, + ) + + pose.fill_(2.0) + state.pose.fill_(3.0) + + assert torch.equal(snapshot.entities["object"].pose, torch.eye(4)) + + +def test_scene_snapshot_entity_reads_are_defensive() -> None: + snapshot = SceneSnapshot( + timestamp=0.0, + version=0, + entities={"object": EntityState(torch.eye(4))}, + ) + + first_read = snapshot.entities["object"] + first_read.pose.fill_(7.0) + + assert torch.equal(snapshot.entities["object"].pose, torch.eye(4)) + with pytest.raises(TypeError): + snapshot.entities["other"] = EntityState(torch.eye(4)) # type: ignore[index] + + +def test_scene_snapshot_collision_pose_reads_are_defensive() -> None: + snapshot = SceneSnapshot( + timestamp=0.0, + version=0, + entities={"obstacle": EntityState(torch.eye(4))}, + collision_entity_ids=("obstacle",), + ) + + obstacle_poses = snapshot.collision_obstacle_poses( + batch_size=1, + device=torch.device("cpu"), + dtype=torch.float32, + ) + obstacle_poses["obstacle"].fill_(5.0) + + assert torch.equal(snapshot.entities["obstacle"].pose, torch.eye(4)) + + def test_scene_snapshot_rejects_unknown_collision_entity() -> None: with pytest.raises(ValueError, match="missing scene entities"): SceneSnapshot( @@ -242,12 +1397,12 @@ def test_scene_snapshot_rejects_unknown_collision_entity() -> None: ) -def test_timed_trajectory_synthesizes_timing_and_holds_selected_rows() -> None: +def test_timed_trajectory_uses_explicit_uniform_timing_and_holds_rows() -> None: positions = torch.arange(24, dtype=torch.float32).reshape(2, 3, 4) - trajectory = TimedTrajectory.from_positions( + trajectory = TimedTrajectory.from_uniform_step( positions, env_ids=torch.tensor([4, 7]), - control_dt=0.02, + step_dt=0.02, ) held = trajectory.hold_rows( torch.tensor([True, False]), @@ -259,19 +1414,64 @@ def test_timed_trajectory_synthesizes_timing_and_holds_selected_rows() -> None: assert torch.all(held.positions[1] == -1.0) +def test_timed_trajectory_constructor_detaches_and_owns_all_tensor_fields() -> None: + positions = torch.tensor([[[1.0, 2.0], [3.0, 4.0]]], requires_grad=True) + velocities = torch.full_like(positions, 0.5, requires_grad=True) + accelerations = torch.full_like(positions, 0.25, requires_grad=True) + dt = torch.tensor([[0.0, 0.1]], requires_grad=True) + env_ids = torch.tensor([4], dtype=torch.long) + inputs = { + "positions": positions, + "velocities": velocities, + "accelerations": accelerations, + "dt": dt, + "env_ids": env_ids, + } + expected = {name: value.detach().clone() for name, value in inputs.items()} + + trajectory = TimedTrajectory(**inputs) + + with torch.no_grad(): + for value in inputs.values(): + value.zero_() + for name, value in expected.items(): + owned = getattr(trajectory, name) + assert torch.equal(owned, value) + assert owned.grad_fn is None + assert not owned.requires_grad + + +def test_timed_trajectory_rejects_duplicate_environment_ids() -> None: + with pytest.raises(ValueError, match="unique"): + TimedTrajectory.from_positions( + torch.zeros(2, 1, 2), + env_ids=torch.tensor([4, 4], dtype=torch.long), + dt=torch.zeros(2, 1), + ) + + +def test_planning_context_requires_explicit_interpolation_period() -> None: + with pytest.raises(ValueError, match="explicit PlanningContext.control_dt"): + _context().require_control_dt() + + assert _context(control_dt=0.02).require_control_dt() == pytest.approx(0.02) + with pytest.raises(ValueError, match="finite and greater than zero"): + _context(control_dt=0.0) + + def test_timed_trajectory_snapshot_owns_its_tensor_storage() -> None: - trajectory = TimedTrajectory.from_positions( + trajectory = TimedTrajectory.from_uniform_step( torch.arange(12, dtype=torch.float32).reshape(1, 3, 4), env_ids=torch.tensor([4]), - control_dt=0.02, + step_dt=0.02, ) snapshot = trajectory.snapshot() snapshot.positions.zero_() snapshot.dt.zero_() - snapshot.duration.zero_() snapshot.env_ids.zero_() + assert snapshot.duration.item() == 0.0 assert torch.count_nonzero(trajectory.positions).item() > 0 assert torch.count_nonzero(trajectory.dt).item() > 0 assert trajectory.duration.item() > 0.0 @@ -279,15 +1479,15 @@ def test_timed_trajectory_snapshot_owns_its_tensor_storage() -> None: def test_timed_trajectory_concatenates_metadata() -> None: - first = TimedTrajectory.from_positions( + first = TimedTrajectory.from_uniform_step( torch.zeros(2, 2, 4), env_ids=torch.tensor([0, 1]), - control_dt=0.1, + step_dt=0.1, ) - second = TimedTrajectory.from_positions( + second = TimedTrajectory.from_uniform_step( torch.ones(2, 3, 4), env_ids=torch.tensor([0, 1]), - control_dt=0.2, + step_dt=0.2, ) result = TimedTrajectory.concatenate((first, second)) diff --git a/tests/sim/atomic_actions/test_curobo_motion_strategy_e2e.py b/tests/sim/atomic_actions/test_curobo_motion_strategy_e2e.py index da4e87a41..7ea946be9 100644 --- a/tests/sim/atomic_actions/test_curobo_motion_strategy_e2e.py +++ b/tests/sim/atomic_actions/test_curobo_motion_strategy_e2e.py @@ -46,7 +46,6 @@ CuroboWorldCfg, ) from embodichain.lab.sim.atomic_actions import ( # noqa: E402 - ActionBinding, ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, @@ -128,12 +127,16 @@ def test_atomic_move_end_effector_uses_curobo_v2(): sim, robot, engine = _make_franka_curobo_engine() try: target = _reachable_target_beyond_demo_block(robot) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": CONTROL_PART}}, + ) result = engine.compile( ( ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(xpos=target), - binding=ActionBinding(manipulators={"primary": CONTROL_PART}), + binding=binding, motion_policy=MotionPolicy( strategy="motion_gen", sample_count=SAMPLE_INTERVAL, @@ -141,7 +144,10 @@ def test_atomic_move_end_effector_uses_curobo_v2(): ), ) ) - trajectory = result.trajectory.positions + plan = result.action_plans[0] + assert plan.joint_trajectory is not None + assert plan.commands.frame_count == plan.joint_trajectory.waypoint_count + trajectory = plan.joint_trajectory.positions assert result.plan_success.shape == (1,) assert bool(result.plan_success.item()) assert trajectory.shape[2] == robot.dof diff --git a/tests/sim/atomic_actions/test_endpoint_runtime_e2e.py b/tests/sim/atomic_actions/test_endpoint_runtime_e2e.py new file mode 100644 index 000000000..44d8217bb --- /dev/null +++ b/tests/sim/atomic_actions/test_endpoint_runtime_e2e.py @@ -0,0 +1,536 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""End-to-end coverage for generic atomic-action runtime endpoints.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import ClassVar +from unittest.mock import Mock + +import torch + +from embodichain.lab.sim.atomic_actions import ( + ActionInvocation, + ActionOptions, + ActionPlan, + AtomicAction, + AtomicActionEngine, + CommandAcknowledgement, + EndpointCommand, + EndpointCommandRouter, + ExecutionRunner, + ExecutionStatus, + JOINT_POSITION_CAPABILITY, + JointPositionGoal, + JointPositionPayload, + JointPositionTarget, + MoveJoints, + PlanningContext, + RobotObservation, + RunnerStatus, + RuntimeCommandFrame, + RuntimeCommandPayload, + RuntimeEndpointTarget, + SceneSnapshot, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, + TaskState, + TimedCommandSequence, + TrackingPolicy, +) +from embodichain.lab.sim.atomic_actions.invocation import ResolvedActionRequest +from embodichain.lab.sim.planners import PlanResult +from embodichain.lab.sim.skills import ( + EndpointResolution, + ResourceBinding, + ResourceEndpoint, + ResourceEndpointAdapter, + RobotResource, + RobotSkillProfile, +) + + +class _Clock: + """Deterministic clock used by the runner.""" + + def __init__(self) -> None: + self.value = 0.0 + + def now(self) -> float: + """Return simulated time.""" + return self.value + + def sleep(self, duration: float) -> None: + """Advance simulated time.""" + self.value += duration + + +class _Robot: + """Small stateful robot with one whole-body control part.""" + + def __init__(self) -> None: + self.device = torch.device("cpu") + self.dof = 4 + self.control_parts = {"whole_body": object()} + self.qpos = torch.zeros(2, self.dof) + + def get_qpos(self, name: str | None = None) -> torch.Tensor: + """Return observed joint positions.""" + if name is not None and name != "whole_body": + raise KeyError(name) + return self.qpos.clone() + + def get_qvel(self, name: str | None = None) -> torch.Tensor: + """Return zero joint velocities.""" + return torch.zeros_like(self.get_qpos(name)) + + def get_joint_ids(self, name: str) -> list[int]: + """Resolve the whole-body control part.""" + if name != "whole_body": + raise KeyError(name) + return list(range(self.dof)) + + +class _Provider: + """Observe the stateful robot at the injected clock time.""" + + def __init__(self, robot: _Robot, clock: _Clock) -> None: + self.robot = robot + self.clock = clock + self.env_ids = torch.tensor([3, 7], dtype=torch.long) + + def observe(self, task_state: TaskState) -> PlanningContext: + """Return one fresh, correlated planning context.""" + qpos = self.robot.get_qpos() + timestamp = self.clock.now() + return PlanningContext( + robot=RobotObservation( + timestamp=timestamp, + qpos=qpos, + qvel=torch.zeros_like(qpos), + ), + task=task_state, + scene=SceneSnapshot(timestamp=timestamp, version=0), + env_ids=self.env_ids, + ) + + +def _engine(robot: _Robot) -> AtomicActionEngine: + """Build a core engine around a controllable planning stub.""" + generator = Mock() + generator.robot = robot + generator.device = robot.device + generator.planner.cfg.planner_type = "stub" + + def generate(states: list[object], *, options: object) -> PlanResult: + target = states[-1].qpos + assert isinstance(target, torch.Tensor) + start = options.start_qpos + assert isinstance(start, torch.Tensor) + positions = torch.stack((start, target), dim=1) + dt = torch.zeros(positions.shape[:2], dtype=torch.float32) + dt[:, 1] = 0.01 + return PlanResult( + success=torch.ones(positions.shape[0], dtype=torch.bool), + positions=positions, + dt=dt, + ) + + generator.generate.side_effect = generate + return AtomicActionEngine(generator, load_builtins=False) + + +class _JointTransport: + """Apply joint endpoint payloads to the stateful test robot.""" + + transport_id = JointPositionTarget.TRANSPORT_ID + payload_type = JointPositionPayload + + def __init__(self, robot: _Robot) -> None: + self.robot = robot + self.sent: list[RuntimeCommandFrame] = [] + self.held: list[tuple[RuntimeEndpointTarget, ...]] = [] + + def send( + self, + frame: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Apply each addressed joint subset.""" + del timeout + self.sent.append(frame.snapshot()) + for command in frame.commands: + assert isinstance(command.target, JointPositionTarget) + assert isinstance(command.payload, JointPositionPayload) + joint_ids = list(command.target.joint_ids) + self.robot.qpos[:, joint_ids] = torch.where( + frame.active_mask[:, None], + command.payload.positions, + self.robot.qpos[:, joint_ids], + ) + return CommandAcknowledgement.accepted_ack() + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Hold only the joint subsets addressed by the runner.""" + del timeout + self.held.append(tuple(target.snapshot() for target in targets)) + for target in targets: + assert isinstance(target, JointPositionTarget) + joint_ids = list(target.joint_ids) + self.robot.qpos[:, joint_ids] = context.robot.qpos[:, joint_ids] + return CommandAcknowledgement.accepted_ack() + + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + """Acknowledge synchronous cancellation.""" + del targets, timeout + return CommandAcknowledgement.accepted_ack() + + +def test_whole_body_joint_endpoint_executes_without_arm_or_tool_roles() -> None: + robot = _Robot() + engine = _engine(robot) + engine.register(MoveJoints()) + binding = engine.bind_control_parts( + "move_joints", + {"primary": {"motion": "whole_body"}}, + ) + invocation = ActionInvocation( + skill_id="move_joints", + goal=JointPositionGoal(torch.full((2, robot.dof), 0.5)), + binding=binding, + ) + clock = _Clock() + provider = _Provider(robot, clock) + context = provider.observe(TaskState.empty(batch_size=2, device="cpu")) + + plan = engine.plan(invocation, context) + target = binding.endpoint("primary", "motion").require_target(JointPositionTarget) + assert target.control_part == "whole_body" + assert plan.joint_trajectory is not None + assert plan.commands.targets[0].target_id == "whole_body" + + transport = _JointTransport(robot) + runner = ExecutionRunner( + engine.start((invocation,), context), + provider, + EndpointCommandRouter((transport,)), + clock=clock, + ) + result = runner.run_until_blocked() + + assert result.status is RunnerStatus.COMPLETED + assert result.tick is not None + assert result.tick.status is ExecutionStatus.COMPLETED + assert len(transport.sent) == 2 + assert len(transport.held) == 1 + assert transport.held[0][0].target_id == "whole_body" + assert torch.allclose(robot.qpos, torch.full((2, robot.dof), 0.5)) + + +@dataclass(frozen=True, slots=True) +class _PlanarVelocityTarget(RuntimeEndpointTarget): + """Address one planar velocity controller.""" + + controller_id: str + + @property + def transport_id(self) -> str: + """Return the custom transport identifier.""" + return "test.planar_velocity" + + @property + def target_id(self) -> str: + """Return the controller-local target identifier.""" + return self.controller_id + + +@dataclass(frozen=True, slots=True, eq=False) +class _PlanarVelocityPayload(RuntimeCommandPayload): + """Batched ``(vx, vy, yaw_rate)`` commands.""" + + twist: torch.Tensor + + def __post_init__(self) -> None: + if not isinstance(self.twist, torch.Tensor) or self.twist.dim() != 2: + raise ValueError("twist must have shape (batch_size, 3).") + if self.twist.shape[0] < 1 or self.twist.shape[1] != 3: + raise ValueError("twist must have shape (batch_size, 3).") + object.__setattr__(self, "twist", self.twist.clone()) + + @property + def batch_size(self) -> int: + """Return the number of environment rows.""" + return int(self.twist.shape[0]) + + @property + def device(self) -> torch.device: + """Return the payload device.""" + return self.twist.device + + @property + def transport_id(self) -> str: + """Return the custom transport identifier.""" + return "test.planar_velocity" + + def snapshot(self) -> _PlanarVelocityPayload: + """Return an independently owned payload.""" + return _PlanarVelocityPayload(self.twist) + + +@dataclass(frozen=True, slots=True) +class _PlanarVelocityEndpoint(ResourceEndpoint): + """Profile declaration for a planar velocity controller.""" + + controller_id: str + + +class _PlanarVelocityAdapter(ResourceEndpointAdapter): + """Resolve the custom profile endpoint to a runtime target.""" + + adapter_id: ClassVar[str] = "test.planar_velocity" + endpoint_type: ClassVar[type[ResourceEndpoint]] = _PlanarVelocityEndpoint + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: AtomicActionEngine, + ) -> EndpointResolution: + """Resolve immutable addressing and an exclusive controller claim.""" + del engine + assert isinstance(endpoint, _PlanarVelocityEndpoint) + return EndpointResolution( + runtime_target=_PlanarVelocityTarget(endpoint.controller_id), + claim_tokens=frozenset({f"controller:{endpoint.controller_id}"}), + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class _DriveGoal: + """Planar velocity command used by the custom atomic action.""" + + goal_kind: ClassVar[str] = "planar_velocity" + twist: torch.Tensor + + +class _DriveVelocity(AtomicAction[_DriveGoal, ActionOptions]): + """Custom action proving non-joint commands cross the full runtime.""" + + skill_id: ClassVar[str] = "drive_velocity" + GoalType: ClassVar[type] = _DriveGoal + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + "body", + endpoints=( + SkillEndpointRequirement( + "motion", + capabilities=frozenset({"motion.base.planar_velocity"}), + ), + ), + ), + ) + ) + + def _plan( + self, + request: ResolvedActionRequest[_DriveGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + """Emit one drive frame followed by an explicit zero-velocity frame.""" + goal = self.require_goal(request) + target = request.binding.endpoint("body", "motion").require_target( + _PlanarVelocityTarget + ) + active = torch.ones(context.batch_size, dtype=torch.bool, device=self.device) + frames = tuple( + RuntimeCommandFrame( + commands=( + EndpointCommand( + target=target, + payload=_PlanarVelocityPayload(twist), + ), + ), + active_mask=active, + env_ids=context.env_ids, + hold_duration=torch.full( + (context.batch_size,), + duration, + dtype=torch.float32, + device=self.device, + ), + ) + for twist, duration in ( + (goal.twist.to(self.device), 0.02), + (torch.zeros_like(goal.twist, device=self.device), 0.0), + ) + ) + return self.build_command_plan( + request, + context, + success=True, + commands=TimedCommandSequence(frames, context.env_ids), + segment_lengths={"drive": 1, "stop": 1}, + ) + + +class _PlanarVelocityTransport: + """Record velocity frames and own the zero-velocity safe state.""" + + transport_id = "test.planar_velocity" + payload_type = _PlanarVelocityPayload + + def __init__(self) -> None: + self.sent: list[torch.Tensor] = [] + self.hold_targets: tuple[RuntimeEndpointTarget, ...] = () + self.last_twist: torch.Tensor | None = None + + def send( + self, + frame: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Record active twists and neutralize every inactive row.""" + del timeout + payload = frame.commands[0].payload + assert isinstance(payload, _PlanarVelocityPayload) + self.last_twist = torch.where( + frame.active_mask[:, None], + payload.twist, + torch.zeros_like(payload.twist), + ) + self.sent.append(self.last_twist) + return CommandAcknowledgement.accepted_ack() + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Apply the velocity transport's safe zero command.""" + del context, timeout + self.hold_targets = tuple(target.snapshot() for target in targets) + assert self.last_twist is not None + self.last_twist = torch.zeros_like(self.last_twist) + return CommandAcknowledgement.accepted_ack() + + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + """Acknowledge cancellation.""" + del targets, timeout + return CommandAcknowledgement.accepted_ack() + + +def test_custom_planar_velocity_endpoint_runs_from_profile_through_router() -> None: + robot = _Robot() + engine = _engine(robot) + engine.register(_DriveVelocity()) + profile = RobotSkillProfile( + profile_id="mobile", + resources={ + "mobile_base": RobotResource( + "mobile_base", + endpoints={ + "motion": _PlanarVelocityEndpoint( + "base_controller", + capabilities=frozenset({"motion.base.planar_velocity"}), + ) + }, + ) + }, + defaults={"drive_velocity": ResourceBinding({"body": "mobile_base"})}, + ) + bound = engine.bind_skill_profile( + profile, + endpoint_adapters={_PlanarVelocityEndpoint: _PlanarVelocityAdapter()}, + ) + binding = bound.resolve("drive_velocity").action_binding + goal_twist = torch.tensor([[0.5, 0.0, 0.1], [0.2, 0.0, -0.1]]) + invocation = ActionInvocation( + skill_id="drive_velocity", + goal=_DriveGoal(goal_twist), + binding=binding, + tracking_policy=TrackingPolicy.timed(), + ) + clock = _Clock() + provider = _Provider(robot, clock) + context = provider.observe(TaskState.empty(batch_size=2, device="cpu")) + + plan = engine.plan(invocation, context) + assert plan.joint_trajectory is None + assert plan.segment("drive").waypoint_count == 1 + assert plan.commands.targets[0].transport_id == "test.planar_velocity" + + transport = _PlanarVelocityTransport() + runner = ExecutionRunner( + engine.start((invocation,), context), + provider, + EndpointCommandRouter((transport,)), + clock=clock, + ) + result = runner.run_until_blocked() + + assert result.status is RunnerStatus.COMPLETED + assert len(transport.sent) == 2 + assert torch.allclose(transport.sent[0], goal_twist) + assert torch.count_nonzero(transport.sent[1]) == 0 + assert transport.last_twist is not None + assert torch.count_nonzero(transport.last_twist) == 0 + assert transport.hold_targets[0].target_id == "base_controller" + + +def test_planar_velocity_transport_neutralizes_inactive_rows() -> None: + transport = _PlanarVelocityTransport() + frame = RuntimeCommandFrame( + commands=( + EndpointCommand( + target=_PlanarVelocityTarget("base_controller"), + payload=_PlanarVelocityPayload(torch.ones(2, 3)), + ), + ), + active_mask=torch.tensor([True, False]), + env_ids=torch.tensor([0, 1]), + hold_duration=torch.zeros(2), + ) + + acknowledgement = transport.send(frame, timeout=1.0) + + assert acknowledgement.accepted + assert transport.last_twist is not None + assert torch.equal(transport.last_twist[0], torch.ones(3)) + assert torch.count_nonzero(transport.last_twist[1]) == 0 diff --git a/tests/sim/atomic_actions/test_engine.py b/tests/sim/atomic_actions/test_engine.py index e19035e56..6c57c9f07 100644 --- a/tests/sim/atomic_actions/test_engine.py +++ b/tests/sim/atomic_actions/test_engine.py @@ -37,15 +37,28 @@ ControlPartCommandProfile, JointPositionCommand, JointPositionGoal, + JointPositionTarget, + JOINT_POSITION_CAPABILITY, MotionPolicy, + ObjectSemantics, PlanningContext, + PressAffordance, PressGoal, PressOptions, ResolvedActionRequest, - register_action, - get_registered_actions, - unregister_action, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, + TimedTrajectory, ) +from embodichain.lab.sim.skills import ( + ControlPartEndpoint, + ResourceBinding, + RobotResource, + RobotSkillProfile, +) + +ACTION_DT = 0.02 class StubAction(AtomicAction[JointPositionGoal, ActionOptions]): @@ -53,7 +66,19 @@ class StubAction(AtomicAction[JointPositionGoal, ActionOptions]): skill_id: ClassVar[str] = "stub" GoalType: ClassVar[type] = JointPositionGoal - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset({JOINT_POSITION_CAPABILITY}), + ), + ), + ), + ), + ) def _plan( self, @@ -71,7 +96,11 @@ def _plan( if torch.isnan(target).any(dim=1).any(): success &= ~torch.isnan(target).any(dim=1) target = torch.nan_to_num(target) - trajectory = torch.stack([context.robot.qpos, target], dim=1) + trajectory = TimedTrajectory.from_uniform_step( + torch.stack([context.robot.qpos, target], dim=1), + env_ids=context.env_ids, + step_dt=ACTION_DT, + ) return self.build_plan( request, context, @@ -119,26 +148,20 @@ def _engine( def _invocation( + engine: AtomicActionEngine, qpos: torch.Tensor, ) -> ActionInvocation[JointPositionGoal, ActionOptions]: return ActionInvocation( skill_id="stub", goal=JointPositionGoal(qpos), - binding=ActionBinding(manipulators={"primary": "all"}), + binding=engine.bind_control_parts( + "stub", + {"primary": {"motion": "all"}}, + ), motion_policy=MotionPolicy(sample_count=2), ) -def test_global_registry_uses_stable_skill_id() -> None: - unregister_action("stub") - register_action(StubAction) - try: - assert get_registered_actions()["stub"] is StubAction - register_action(StubAction) - finally: - unregister_action("stub") - - def test_action_subclass_cannot_override_framework_plan() -> None: with pytest.raises(TypeError, match="must implement _plan"): @@ -179,20 +202,34 @@ def test_engine_can_disable_builtin_loading() -> None: def test_auto_registered_builtin_accepts_per_invocation_options() -> None: - engine = _engine(load_builtins=True) + generator = _motion_generator(robot_dof=3) + generator.robot.control_parts = {"arm": object(), "hand": object()} + generator.robot.get_joint_ids.side_effect = lambda name: ( + [0, 1] if name == "arm" else [2] + ) + engine = AtomicActionEngine( + generator, + control_profiles={ + "hand": ControlPartCommandProfile.joint_positions(grasp=torch.ones(1)) + }, + ) options = PressOptions(hand_interp_steps=7) + semantics = ObjectSemantics( + affordance=PressAffordance(press_position=(0.0, 0.0, 0.0)), + geometry={}, + ) invocation = ActionInvocation( skill_id="press", - goal=PressGoal(torch.eye(4)), - binding=ActionBinding( - manipulators={"primary": "all"}, - end_effectors={"primary": "all"}, + goal=PressGoal(semantics, torch.eye(4)), + binding=engine.bind_control_parts( + "press", + {"primary": {"motion": "arm", "grasp": "hand"}}, ), motion_policy=MotionPolicy(sample_count=20), skill_options=options, ) - request = engine.resolve(invocation) + request = engine.actions["press"].resolve_request(invocation) assert request.skill_options.hand_interp_steps == 7 assert request.skill_options is not options @@ -204,11 +241,13 @@ def test_engine_compile_projects_terminal_state_between_actions() -> None: first = torch.ones(2, 3) second = torch.full((2, 3), 2.0) - compiled = engine.compile((_invocation(first), _invocation(second))) + compiled = engine.compile((_invocation(engine, first), _invocation(engine, second))) assert compiled.plan_success.tolist() == [True, True] assert compiled.trajectory.positions.shape == (2, 4, 3) - assert torch.equal(compiled.action_plans[1].trajectory.positions[:, 0], first) + second_trajectory = compiled.action_plans[1].joint_trajectory + assert second_trajectory is not None + assert torch.equal(second_trajectory.positions[:, 0], first) assert torch.equal(compiled.projected_context.robot.qpos, second) assert torch.count_nonzero(engine.robot.get_qpos()) == 0 assert compiled.action_waypoint_offset(1) == 2 @@ -222,12 +261,14 @@ def test_engine_compile_holds_failed_rows_for_remaining_actions() -> None: first = torch.tensor([[1.0, 1.0, 1.0], [float("nan"), 2.0, 2.0]]) second = torch.full((2, 3), 4.0) - compiled = engine.compile((_invocation(first), _invocation(second))) + compiled = engine.compile((_invocation(engine, first), _invocation(engine, second))) assert compiled.plan_success.tolist() == [True, False] assert torch.all(compiled.projected_context.robot.qpos[0] == 4.0) assert torch.all(compiled.projected_context.robot.qpos[1] == 0.0) - assert torch.all(compiled.action_plans[0].trajectory.positions[1] == 0.0) + first_trajectory = compiled.action_plans[0].joint_trajectory + assert first_trajectory is not None + assert torch.all(first_trajectory.positions[1] == 0.0) assert torch.all(compiled.trajectory.positions[1] == 0.0) @@ -244,8 +285,14 @@ def test_engine_compile_empty_sequence_is_successful_noop() -> None: def test_engine_rejects_unknown_skill() -> None: engine = _engine() + invocation = ActionInvocation( + skill_id="stub", + goal=JointPositionGoal(torch.zeros(2, 3)), + binding=ActionBinding(owner_id=engine.binding_owner_id), + motion_policy=MotionPolicy(sample_count=2), + ) with pytest.raises(KeyError, match="stub"): - engine.compile((_invocation(torch.zeros(2, 3)),)) + engine.compile((invocation,)) def test_engine_rejects_duplicate_instance_registration() -> None: @@ -283,16 +330,132 @@ def test_engine_binds_one_planning_service_to_every_action() -> None: assert second.planning_services is engine.planning_services +def test_engine_preserves_custom_action_timing() -> None: + engine = _engine() + engine.register(StubAction()) + + plan = engine.plan(_invocation(engine, torch.ones(2, 3))) + + assert plan.joint_trajectory is not None + assert torch.allclose( + plan.joint_trajectory.dt, + torch.tensor([[0.0, ACTION_DT], [0.0, ACTION_DT]]), + ) + + def test_engine_resolves_action_binding_from_robot_control_parts() -> None: engine = _engine(robot_dof=3) + engine.register(StubAction()) - resolved = engine.planning_services.resolve_binding( - ActionBinding(manipulators={"primary": "all"}) + resolved = engine.bind_control_parts( + "stub", + {"primary": {"motion": "all"}}, + task_state_keys={"primary": "logical_robot"}, ) + endpoint = resolved.endpoint("primary", "motion") + target = endpoint.require_target(JointPositionTarget) + + assert target.control_part == "all" + assert target.joint_ids == (0, 1, 2) + assert endpoint.task_state_key == "logical_robot" - assert resolved.manipulator().name == "all" - assert resolved.manipulator().joint_ids == (0, 1, 2) - assert resolved.manipulator().dof == 3 + +def test_engine_make_invocation_binds_direct_control_parts() -> None: + engine = _engine(robot_dof=3) + engine.register(StubAction()) + goal = JointPositionGoal(torch.ones(2, 3)) + motion_policy = MotionPolicy(sample_count=2) + + invocation = engine.make_invocation( + "stub", + goal, + control_parts={"primary": {"motion": "all"}}, + motion_policy=motion_policy, + invocation_id="direct-call", + revision=1, + ) + target = invocation.binding.endpoint("primary", "motion").require_target( + JointPositionTarget + ) + + assert invocation.skill_id == "stub" + assert invocation.goal is goal + assert invocation.motion_policy is motion_policy + assert invocation.invocation_id == "direct-call" + assert invocation.revision == 1 + assert target.control_part == "all" + assert engine.plan(invocation).plan_success.tolist() == [True, True] + + +def test_engine_make_invocation_uses_profile_default_binding() -> None: + engine = _engine(robot_dof=3) + engine.register(StubAction()) + engine.bind_skill_profile( + RobotSkillProfile( + profile_id="stub-profile", + resources={ + "whole_robot": RobotResource( + "whole_robot", + endpoints={ + "motion": ControlPartEndpoint( + "all", + capabilities=frozenset({JOINT_POSITION_CAPABILITY}), + ) + }, + ) + }, + defaults={ + "stub": ResourceBinding({"primary": "whole_robot"}), + }, + ) + ) + + invocation = engine.make_invocation( + "stub", + JointPositionGoal(torch.ones(2, 3)), + motion_policy=MotionPolicy(sample_count=2), + ) + endpoint = invocation.binding.endpoint("primary", "motion") + + assert endpoint.resource_id == "whole_robot" + assert endpoint.require_target(JointPositionTarget).control_part == "all" + assert engine.plan(invocation).plan_success.tolist() == [True, True] + + +def test_engine_make_invocation_requires_direct_binding_without_profile() -> None: + engine = _engine() + engine.register(StubAction()) + + with pytest.raises(ValueError, match="control_parts is required"): + engine.make_invocation( + "stub", + JointPositionGoal(torch.ones(2, 3)), + ) + + +def test_engine_make_invocation_rejects_resources_without_profile() -> None: + engine = _engine() + engine.register(StubAction()) + + with pytest.raises(ValueError, match="requires a bound RobotSkillProfile"): + engine.make_invocation( + "stub", + JointPositionGoal(torch.ones(2, 3)), + resources={"primary": "whole_robot"}, + ) + + +def test_engine_make_invocation_rejects_conflicting_binding_sources() -> None: + engine = _engine() + engine.register(StubAction()) + + with pytest.raises(ValueError, match="mutually exclusive"): + engine.make_invocation( + "stub", + JointPositionGoal(torch.ones(2, 3)), + control_parts={"primary": {"motion": "all"}}, + resources={"primary": "whole_robot"}, + ) def test_engine_resolves_invocation_control_override_into_request() -> None: @@ -304,20 +467,24 @@ def test_engine_resolves_invocation_control_override_into_request() -> None: ) engine.register(StubAction()) invocation = replace( - _invocation(torch.ones(2, 3)), + _invocation(engine, torch.ones(2, 3)), control_overrides=ActionControlOverrides( - manipulators={ - "primary": {"ready": JointPositionCommand(torch.full((3,), 0.4))} + endpoints={ + "primary": { + "motion": {"ready": JointPositionCommand(torch.full((3,), 0.4))} + } } ), revision=2, ) - request = engine.resolve(invocation) + request = engine.actions["stub"].resolve_request(invocation) assert request.revision == 2 assert torch.allclose( - request.binding.manipulator().joint_positions("ready", n_envs=2, device="cpu"), + request.binding.endpoint("primary", "motion").joint_positions( + "ready", num_envs=2, device="cpu" + ), torch.full((2, 3), 0.4), ) @@ -325,15 +492,12 @@ def test_engine_resolves_invocation_control_override_into_request() -> None: def test_engine_rejects_binding_outside_robot_control_parts() -> None: engine = _engine() engine.register(StubAction()) - invocation = ActionInvocation( - skill_id="stub", - goal=JointPositionGoal(torch.zeros(2, 3)), - binding=ActionBinding(manipulators={"primary": "missing_arm"}), - motion_policy=MotionPolicy(sample_count=2), - ) with pytest.raises(ValueError, match="Robot.control_parts"): - engine.plan(invocation) + engine.bind_control_parts( + "stub", + {"primary": {"motion": "missing_arm"}}, + ) def test_engine_motion_generator_is_read_only() -> None: @@ -346,10 +510,20 @@ def test_engine_motion_generator_is_read_only() -> None: def test_engine_plan_action_supports_unregistered_configured_instance() -> None: engine = _engine() action = StubAction() + binding = engine.bind_control_parts( + action, + {"primary": {"motion": "all"}}, + ) + invocation = ActionInvocation( + skill_id="stub", + goal=JointPositionGoal(torch.ones(2, 3)), + binding=binding, + motion_policy=MotionPolicy(sample_count=2), + ) plan = engine.plan_action( action, - _invocation(torch.ones(2, 3)), + invocation, engine.initial_context(), ) @@ -358,6 +532,18 @@ def test_engine_plan_action_supports_unregistered_configured_instance() -> None: assert engine.actions == {} +def test_engine_cannot_build_binding_for_action_owned_by_another_engine() -> None: + action = StubAction() + first = _engine() + first.register(action) + + with pytest.raises(ValueError, match="belongs to another engine"): + _engine().bind_control_parts( + action, + {"primary": {"motion": "all"}}, + ) + + def test_action_cannot_be_rebound_to_another_engine() -> None: action = StubAction() _engine().register(action) @@ -366,11 +552,21 @@ def test_action_cannot_be_rebound_to_another_engine() -> None: _engine().register(action) +def test_bound_action_exposes_num_envs_property() -> None: + engine = _engine(batch_size=3) + action = StubAction() + engine.register(action) + + assert action.num_envs == 3 + + def test_unbound_action_rejects_direct_planning() -> None: action = StubAction() + donor_engine = _engine() + donor_engine.register(StubAction()) with pytest.raises(RuntimeError, match="not bound"): - action.resolve_request(_invocation(torch.ones(2, 3))) + action.resolve_request(_invocation(donor_engine, torch.ones(2, 3))) def test_engine_rejects_plan_for_a_different_skill() -> None: @@ -388,4 +584,4 @@ def wrong_skill_plan( engine.register(action) with pytest.raises(ValueError, match="must match its request"): - engine.compile((_invocation(torch.zeros(2, 3)),)) + engine.compile((_invocation(engine, torch.zeros(2, 3)),)) diff --git a/tests/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index 1458b7367..27ccb2468 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -20,6 +20,7 @@ from collections.abc import Sequence from dataclasses import replace +import math from typing import ClassVar from unittest.mock import Mock @@ -36,35 +37,93 @@ AtomicActionEngine, DynamicCollisionMode, EndEffectorPoseGoal, + EndpointBinding, + EndpointCommand, + EndpointTrackingChannelBinding, + EndpointTrackingFeedbackAddress, EntityState, + EffectExpectationResult, ExecutionEventKind, + ExecutionSession, ExecutionStatus, + ExecutionTick, + EffectVerificationRequirement, + EffectVerificationResult, GraspGoal, + HeldObjectGuardRequest, + HeldObjectGuardResult, HeldObjectState, + JointPositionPayload, + JointPositionTarget, MotionPolicy, ObjectSemantics, + PlannerDiagnostics, PlanningContext, + PhaseEffectGateRequest, + PhaseEffectGateRequirement, + PhaseEffectGateResult, RecoveryPolicy, - ResolvedActionBinding, ResolvedActionRequest, RobotObservation, + RuntimeCommandFrame, SceneEntityPose, SceneSnapshot, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, StateDelta, TaskState, + TimedCommandSequence, + TimedTrackingSequence, TimedTrajectory, + TrackingFeedbackSourceRef, + TrackingFrame, + TrackingPolicy, + TrackingProjectorRef, + TrackingSetpoint, ) from embodichain.lab.sim.common import BatchEntity from embodichain.lab.sim.atomic_actions.goals import resolve_pose_goal from embodichain.lab.sim.planners import PlanOptions +def _effect_result( + verification_id: int, + success_mask: torch.Tensor, + failure_mask: torch.Tensor, + *, + invalidation_mask: torch.Tensor | None = None, + retry_mask: torch.Tensor | None = None, + expectation_results: tuple[EffectExpectationResult, ...] = (), +) -> EffectVerificationResult: + """Build an explicit terminal decision with legacy retry semantics.""" + return EffectVerificationResult( + verification_id=verification_id, + success_mask=success_mask, + failure_mask=failure_mask, + invalidation_mask=( + torch.zeros_like(failure_mask) + if invalidation_mask is None + else invalidation_mask + ), + retry_mask=failure_mask if retry_mask is None else retry_mask, + expectation_results=expectation_results, + ) + + class DynamicAction(AtomicAction[EndEffectorPoseGoal, ActionOptions]): """Test action whose terminal joint command follows a scene entity x pose.""" skill_id: ClassVar[str] = "dynamic" GoalType: ClassVar[type] = EndEffectorPoseGoal - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=(SkillEndpointRequirement(endpoint_id="motion"),), + ), + ) + ) def __init__(self) -> None: super().__init__() @@ -81,11 +140,46 @@ def _plan( self.requests.append(request) pose = resolve_pose_goal(goal.xpos, context, name="xpos") target = pose[:, 0, 3].unsqueeze(1).expand_as(context.robot.qpos) + trajectory = TimedTrajectory.from_uniform_step( + torch.stack([context.robot.qpos, target], dim=1), + env_ids=context.env_ids, + step_dt=0.1, + ) + return self.build_plan( + request, + context, + success=True, + trajectory=trajectory, + ) + + +class PhaseGateAction(DynamicAction): + """Three-frame action with one gate before its terminal segment.""" + + skill_id: ClassVar[str] = "phase_gate" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + goal = self.require_goal(request) + self.plan_count += 1 + self.requests.append(request) + pose = resolve_pose_goal(goal.xpos, context, name="xpos") + target = pose[:, 0, 3].unsqueeze(1).expand_as(context.robot.qpos) + midpoint = torch.lerp(context.robot.qpos, target, 0.5) return self.build_plan( request, context, success=True, - trajectory=torch.stack([context.robot.qpos, target], dim=1), + trajectory=TimedTrajectory.from_uniform_step( + torch.stack([context.robot.qpos, midpoint, target], dim=1), + env_ids=context.env_ids, + step_dt=0.1, + ), + segment_lengths={"prepare": 2, "commit": 1}, ) @@ -93,6 +187,7 @@ class EffectAction(DynamicAction): """Dynamic test action that declares an attachment effect.""" skill_id: ClassVar[str] = "effect" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract def _plan( self, @@ -110,19 +205,54 @@ def _plan( object_to_eef=torch.eye(4), grasp_xpos=torch.eye(4), ) + trajectory = TimedTrajectory.from_uniform_step( + torch.stack([context.robot.qpos, target], dim=1), + env_ids=context.env_ids, + step_dt=0.1, + ) return self.build_plan( request, context, success=True, - trajectory=torch.stack([context.robot.qpos, target], dim=1), + trajectory=trajectory, expected_effects=StateDelta(held_object_updates={"arm": held}), ) +class VerificationOnlyAction(DynamicAction): + """Dynamic action requiring a physical check without symbolic effects.""" + + skill_id: ClassVar[str] = "verification_only" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + goal = self.require_goal(request) + pose = resolve_pose_goal(goal.xpos, context, name="xpos") + target = pose[:, 0, 3].unsqueeze(1).expand_as(context.robot.qpos) + return self.build_plan( + request, + context, + success=True, + trajectory=TimedTrajectory.from_uniform_step( + torch.stack([context.robot.qpos, target], dim=1), + env_ids=context.env_ids, + step_dt=0.1, + ), + effect_verification=EffectVerificationRequirement( + kind="physical.test_completion" + ), + ) + + class FailedEffectAction(EffectAction): """Effect-declaring action whose planner fails for every environment.""" skill_id: ClassVar[str] = "failed_effect" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract def _plan( self, @@ -133,10 +263,87 @@ def _plan( return replace(plan, plan_success=torch.zeros_like(plan.plan_success)) +class DiagnosticAction(DynamicAction): + """Dynamic action exposing its installed plan for snapshot isolation tests.""" + + skill_id: ClassVar[str] = "diagnostic" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract + + def __init__(self) -> None: + super().__init__() + self.metadata = {"solver": {"iterations": [3, 5]}} + self.returned_plans: list[ActionPlan] = [] + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + plan = super()._plan(request, context) + plan = replace( + plan, + diagnostics=PlannerDiagnostics( + backend="diagnostic", + metadata=self.metadata, + ), + ) + self.returned_plans.append(plan) + return plan + + +class WindowedDependencyAction(DynamicAction): + """Stop monitoring a goal pose once its first command was issued.""" + + skill_id: ClassVar[str] = "windowed_dependency" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + plan = super()._plan(request, context) + return replace( + plan, + scene_dependency_monitor_until={"target": 1}, + ) + + +class MultiDependencyAction(DynamicAction): + """Track the goal plus one auxiliary scene dependency.""" + + skill_id: ClassVar[str] = "multi_dependency" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract + + def _scene_dependencies( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + ) -> tuple[str, ...]: + return tuple(sorted((*super()._scene_dependencies(request), "obstacle"))) + + +class MixedEffectAction(EffectAction): + """Effect action whose final environment row always fails planning.""" + + skill_id: ClassVar[str] = "mixed_effect" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + plan = super()._plan(request, context) + plan_success = torch.ones_like(plan.plan_success) + plan_success[-1] = False + return replace(plan, plan_success=plan_success) + + class NonuniformTimingAction(DynamicAction): """Test action with explicit nonuniform waypoint arrival intervals.""" skill_id: ClassVar[str] = "nonuniform_timing" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract def _plan( self, @@ -156,9 +363,7 @@ def _plan( trajectory = TimedTrajectory.from_positions( positions, env_ids=context.env_ids, - control_dt=request.motion_policy.control_dt, dt=dt, - duration=dt.sum(dim=1), ) return self.build_plan( request, @@ -168,6 +373,116 @@ def _plan( ) +class DestinationSequenceAction(AtomicAction[EndEffectorPoseGoal, ActionOptions]): + """Emit a configured destination sequence across recovery plans.""" + + skill_id: ClassVar[str] = "destination_sequence" + GoalType: ClassVar[type] = EndEffectorPoseGoal + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement(endpoint_id="first"), + SkillEndpointRequirement(endpoint_id="second"), + ), + ), + ) + ) + + def __init__( + self, + destinations: tuple[str | None, ...], + tracking_provider_revisions: tuple[str | None, ...] | None = None, + ) -> None: + super().__init__() + self.destinations = destinations + self.tracking_provider_revisions = tracking_provider_revisions + self.plan_count = 0 + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + self.require_goal(request) + index = min(self.plan_count, len(self.destinations) - 1) + endpoint_id = self.destinations[index] + self.plan_count += 1 + if endpoint_id is None: + commands = TimedCommandSequence(frames=(), env_ids=context.env_ids) + return self.build_command_plan( + request, + context, + success=False, + commands=commands, + ) + + target = request.binding.endpoint("primary", endpoint_id).require_target( + JointPositionTarget + ) + joint_ids = list(target.joint_ids) + frame = RuntimeCommandFrame( + commands=( + EndpointCommand( + target=target, + payload=JointPositionPayload( + positions=context.robot.qpos[:, joint_ids] + ), + ), + ), + active_mask=torch.ones( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ), + env_ids=context.env_ids, + hold_duration=torch.zeros( + context.batch_size, + dtype=torch.float32, + device=context.robot.qpos.device, + ), + ) + plan = self.build_command_plan( + request, + context, + success=True, + commands=TimedCommandSequence( + frames=(frame,), + env_ids=context.env_ids, + ), + ) + if self.tracking_provider_revisions is None: + return plan + provider_revision = self.tracking_provider_revisions[index] + if provider_revision is None or plan.tracking is None: + return plan + original = plan.tracking.frames[0].setpoints[0] + changed = TrackingSetpoint( + endpoint_key=original.endpoint_key, + binding=EndpointTrackingChannelBinding( + channel_id=original.binding.channel_id, + source=TrackingFeedbackSourceRef( + provider_id=original.binding.source.provider_id, + revision=provider_revision, + address=original.binding.source.address, + ), + projector=TrackingProjectorRef( + projector_id=original.binding.projector.projector_id, + revision=original.binding.projector.revision, + ), + ), + desired=original.desired, + ) + return replace( + plan, + tracking=TimedTrackingSequence( + plan.tracking.env_ids, + (TrackingFrame((changed,)),), + ), + ) + + class UncopyableEntity(BatchEntity): """Minimal live entity whose simulator identity must not be copied.""" @@ -210,6 +525,31 @@ def _engine(batch_size: int = 1) -> tuple[AtomicActionEngine, DynamicAction]: return engine, action +def _destination_engine( + destinations: tuple[str | None, ...], + tracking_provider_revisions: tuple[str | None, ...] | None = None, +) -> tuple[AtomicActionEngine, DestinationSequenceAction]: + robot = Mock() + robot.device = torch.device("cpu") + robot.dof = 2 + robot.control_parts = {"arm_a": object(), "arm_b": object()} + robot.get_qpos.return_value = torch.zeros(1, 2) + robot.get_qvel.return_value = torch.zeros(1, 2) + robot.get_joint_ids.side_effect = lambda *, name: { + "arm_a": [0], + "arm_b": [1], + }[name] + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "stub" + generator.supports_dynamic_collision_world = False + engine = AtomicActionEngine(generator, load_builtins=False) + action = DestinationSequenceAction(destinations, tracking_provider_revisions) + engine.register(action) + return engine, action + + def _context( timestamp: float, qpos: float | tuple[float, ...], @@ -240,6 +580,109 @@ 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 _phase_gate_result( + request: PhaseEffectGateRequest, + *, + success_mask: torch.Tensor, + failure_mask: torch.Tensor, + retry_mask: torch.Tensor | None = None, +) -> PhaseEffectGateResult: + """Build one result exactly correlated with a pending segment-entry gate.""" + return PhaseEffectGateResult( + verification_id=request.verification_id, + gate_id=request.gate_id, + attempt_generation=request.attempt_generation, + invocation_index=request.invocation_index, + next_waypoint_index=request.next_waypoint_index, + success_mask=success_mask, + failure_mask=failure_mask, + retry_mask=(failure_mask.clone() if retry_mask is None else retry_mask), + ) + + +def _multi_dependency_context( + timestamp: float, + *, + target_x: float, + obstacle_x: float | None, + version: int, + target_yaw: float = 0.0, +) -> PlanningContext: + """Build one-row context with an optional auxiliary dependency.""" + context = _context(timestamp, 0.0, target_x, version) + entities = dict(context.scene.entities) + target_pose = entities["target"].pose + cosine = math.cos(target_yaw) + sine = math.sin(target_yaw) + target_pose[:, 0, 0] = cosine + target_pose[:, 0, 1] = -sine + target_pose[:, 1, 0] = sine + target_pose[:, 1, 1] = cosine + entities["target"] = EntityState(target_pose) + if obstacle_x is not None: + obstacle_pose = torch.eye(4).unsqueeze(0) + obstacle_pose[:, 0, 3] = obstacle_x + entities["obstacle"] = EntityState(obstacle_pose) + return PlanningContext( + robot=context.robot, + task=context.task, + scene=SceneSnapshot( + timestamp=timestamp, + version=version, + entities=entities, + ), + env_ids=context.env_ids, + ) + + def _collision_context( timestamp: float, qpos: torch.Tensor, @@ -274,29 +717,30 @@ def _collision_context( def _invocation( + engine: AtomicActionEngine, *, skill_id: str = "dynamic", max_replans: int = 2, max_action_retries: int = 2, action_timeout: float = 30.0, - control_dt: float = 1.0 / 60.0, strategy: str = "ik_interp", dynamic_collision_mode: DynamicCollisionMode = DynamicCollisionMode.AUTO, ) -> ActionInvocation[EndEffectorPoseGoal]: return ActionInvocation( skill_id=skill_id, goal=EndEffectorPoseGoal(SceneEntityPose("target")), - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=engine.planning_services.bind_control_parts( + DynamicAction.binding_contract, + {"primary": {"motion": "arm"}}, + ), motion_policy=MotionPolicy( sample_count=2, - control_dt=control_dt, strategy=strategy, dynamic_collision_mode=dynamic_collision_mode, ), recovery_policy=RecoveryPolicy( max_replans=max_replans, max_action_retries=max_action_retries, - tracking_error_threshold=0.05, goal_translation_threshold=0.02, action_timeout=action_timeout, ), @@ -304,65 +748,739 @@ def _invocation( ) +def _destination_invocation( + engine: AtomicActionEngine, +) -> ActionInvocation[EndEffectorPoseGoal]: + return ActionInvocation( + skill_id=DestinationSequenceAction.skill_id, + goal=EndEffectorPoseGoal(SceneEntityPose("target")), + binding=engine.bind_control_parts( + DestinationSequenceAction.skill_id, + { + "primary": { + "first": "arm_a", + "second": "arm_b", + } + }, + task_state_keys={"primary": "destination_resource"}, + ), + recovery_policy=RecoveryPolicy( + max_replans=2, + max_action_retries=1, + goal_translation_threshold=0.02, + ), + invocation_id="destination-call", + ) + + +def _phase_gate_invocation( + engine: AtomicActionEngine, + *, + segment_name: str = "commit", + max_action_retries: int = 2, +) -> ActionInvocation[EndEffectorPoseGoal]: + """Build a test invocation whose core owns one named segment gate.""" + base = _invocation( + engine, + skill_id=PhaseGateAction.skill_id, + max_action_retries=max_action_retries, + ) + return replace( + base, + phase_effect_gates=( + PhaseEffectGateRequirement( + gate_id="physical_ready", + segment_name=segment_name, + ), + ), + ) + + +def _effect_session( + *, + batch_size: int = 1, + max_action_retries: int = 2, + action_timeout: float = 30.0, + eligible_mask: torch.Tensor | None = None, + action: DynamicAction | None = None, + task_state: TaskState | None = None, +) -> tuple[ExecutionSession, ExecutionTick]: + """Advance a test effect action to its verification boundary.""" + engine, _ = _engine(batch_size=batch_size) + selected_action = EffectAction() if action is None else action + engine.register(selected_action) + base = _invocation( + engine, + max_action_retries=max_action_retries, + action_timeout=action_timeout, + ) + invocation = ActionInvocation( + skill_id=selected_action.skill_id, + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + qpos = tuple(0.0 for _ in range(batch_size)) + target = tuple(0.2 for _ in range(batch_size)) + initial_context = _context(0.0, qpos, target, 0) + if task_state is not None: + initial_context = replace(initial_context, task=task_state) + session = engine.start( + (invocation,), + initial_context, + eligible_mask=eligible_mask, + ) + session.tick(_context(0.0, qpos, target, 0)) + session.tick(_context(0.1, qpos, target, 0)) + waiting = session.tick(_context(0.2, target, target, 0)) + assert waiting.pending_effect is not None + return session, waiting + + +def _joint_positions(command: RuntimeCommandFrame | None) -> torch.Tensor: + """Return the only joint-position payload emitted by the test action.""" + assert command is not None + assert len(command.commands) == 1 + payload = command.commands[0].payload + assert isinstance(payload, JointPositionPayload) + return payload.positions + + def test_session_completes_incremental_command_sequence() -> None: engine, _ = _engine() - session = engine.start((_invocation(),), _context(0.0, 0.0, 0.2, 0)) + session = engine.start((_invocation(engine),), _context(0.0, 0.0, 0.2, 0)) first = session.tick(_context(0.0, 0.0, 0.2, 0)) second = session.tick(_context(0.1, 0.0, 0.2, 0)) final = session.tick(_context(0.2, 0.2, 0.2, 0)) - assert first.command is not None and torch.all(first.command.positions == 0.0) + assert torch.all(_joint_positions(first.command) == 0.0) assert all(event.invocation_id == "dynamic-call" for event in first.events) - assert second.command is not None and torch.all(second.command.positions == 0.2) + assert torch.all(_joint_positions(second.command) == 0.2) assert final.status is ExecutionStatus.COMPLETED assert final.eligible_mask.tolist() == [True] -def test_session_commands_schedule_arrivals_and_final_settling() -> None: +@pytest.mark.parametrize( + ("segment_name", "message"), + (("missing", "missing segment"), ("prepare", "first trajectory segment")), +) +def test_phase_effect_gate_requires_a_noninitial_named_segment( + segment_name: str, + message: str, +) -> None: engine, _ = _engine() - engine.register(NonuniformTimingAction()) - session = engine.start( - (_invocation(skill_id="nonuniform_timing"),), - _context(0.0, 0.0, 0.2, 0), - ) + engine.register(PhaseGateAction()) - first = session.tick(_context(0.0, 0.0, 0.2, 0)) - second = session.tick(_context(0.0, 0.0, 0.2, 0)) - third = session.tick(_context(0.1, 0.1, 0.2, 0)) + with pytest.raises(ValueError, match=message): + engine.start( + (_phase_gate_invocation(engine, segment_name=segment_name),), + _context(0.0, 0.0, 0.2, 0), + ) - assert first.command is not None - assert second.command is not None - assert third.command is not None - command_durations = torch.stack( - [ - first.command.hold_duration, - second.command.hold_duration, - third.command.hold_duration, - ], - dim=1, - ) - assert torch.allclose(command_durations, torch.tensor([[0.1, 0.3, 0.3]])) - assert torch.allclose(command_durations[:, :-1].sum(dim=1), torch.tensor([0.4])) +def test_unresolved_phase_effect_gate_replays_preceding_command_for_full_cohort() -> ( + None +): + engine, _ = _engine(batch_size=2) + action = PhaseGateAction() + engine.register(action) + initial = _context(0.0, (0.0, 0.0), (0.2, 0.4), 0) + session = engine.start((_phase_gate_invocation(engine),), initial) + + first = session.tick(initial) + boundary = session.tick(_context(0.1, (0.0, 0.0), (0.2, 0.4), 0)) + request = boundary.pending_phase_effect_gate + assert request is not None + assert request.gate_id == "physical_ready" + assert request.segment_name == "commit" + assert request.next_waypoint_index == 2 + assert request.env_mask.tolist() == [True, True] + assert torch.allclose(_joint_positions(first.command), torch.zeros(2, 2)) + predecessor = _joint_positions(boundary.command) + assert torch.allclose(predecessor, torch.tensor([[0.1, 0.1], [0.2, 0.2]])) + + unresolved = session.tick( + _context(0.2, (0.1, 0.2), (0.2, 0.4), 0), + phase_effect_gate_result=_phase_gate_result( + request, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) -def test_request_snapshot_preserves_live_entity_identity() -> None: - entity = UncopyableEntity() - grasp_xpos = torch.eye(4).unsqueeze(0) - geometry_extent = torch.tensor([0.1, 0.2, 0.3]) - semantics = ObjectSemantics( - affordance=Affordance(), - geometry={"extent": geometry_extent}, - label="object", - entity=entity, + assert unresolved.status is ExecutionStatus.RUNNING + assert unresolved.pending_phase_effect_gate is not None + assert unresolved.pending_phase_effect_gate.verification_id == ( + request.verification_id + 1 ) - goal = GraspGoal(semantics=semantics, grasp_xpos=grasp_xpos) + assert unresolved.pending_phase_effect_gate.next_waypoint_index == 2 + assert torch.equal(_joint_positions(unresolved.command), predecessor) + assert unresolved.command is not None + assert unresolved.command.active_mask.tolist() == [True, True] + assert unresolved.task_state.held_objects == {} + kinds = [event.kind for event in (*boundary.events, *unresolved.events)] + assert kinds.count(ExecutionEventKind.PHASE_EFFECT_GATE_REQUIRED) == 1 + assert ExecutionEventKind.PHASE_EFFECT_GATE_SATISFIED not in kinds + assert action.plan_count == 1 - request = ResolvedActionRequest( - skill_id="pick_up", - goal=goal, - binding=ResolvedActionBinding(), + +def test_phase_effect_gate_success_unlocks_segment_without_committing_task_state() -> ( + None +): + engine, _ = _engine(batch_size=2) + engine.register(PhaseGateAction()) + initial = _context(0.0, (0.0, 0.0), (0.2, 0.4), 0) + session = engine.start((_phase_gate_invocation(engine),), initial) + session.tick(initial) + boundary = session.tick(_context(0.1, (0.0, 0.0), (0.2, 0.4), 0)) + request = boundary.pending_phase_effect_gate + assert request is not None + + released = session.tick( + _context(0.2, (0.1, 0.2), (0.2, 0.4), 0), + phase_effect_gate_result=_phase_gate_result( + request, + success_mask=torch.tensor([True, True]), + failure_mask=torch.tensor([False, False]), + ), + ) + + assert released.pending_phase_effect_gate is None + assert torch.allclose( + _joint_positions(released.command), + torch.tensor([[0.2, 0.2], [0.4, 0.4]]), + ) + assert released.task_state.held_objects == {} + satisfied = next( + event + for event in released.events + if event.kind is ExecutionEventKind.PHASE_EFFECT_GATE_SATISFIED + ) + assert satisfied.env_mask.tolist() == [True, True] + + +def test_phase_effect_gate_contradiction_retries_action_without_state_mutation() -> ( + None +): + engine, _ = _engine(batch_size=2) + action = PhaseGateAction() + engine.register(action) + initial = _context(0.0, (0.0, 0.0), (0.2, 0.4), 0) + session = engine.start( + (_phase_gate_invocation(engine, max_action_retries=1),), + initial, + ) + session.tick(initial) + boundary = session.tick(_context(0.1, (0.0, 0.0), (0.2, 0.4), 0)) + request = boundary.pending_phase_effect_gate + assert request is not None + + retried = session.tick( + _context(0.2, (0.1, 0.2), (0.2, 0.4), 0), + phase_effect_gate_result=_phase_gate_result( + request, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([True, False]), + retry_mask=torch.tensor([True, False]), + ), + ) + + assert retried.status is ExecutionStatus.RUNNING + assert retried.pending_phase_effect_gate is None + assert retried.command is not None + assert retried.command.active_mask.tolist() == [True, True] + assert action.plan_count == 2 + assert session.plan_attempts[-1].attempt_generation == 1 + assert session.plan_attempts[-1].action_retry_counts == (1, 0) + assert retried.task_state.held_objects == {} + kinds = [event.kind for event in retried.events] + assert ExecutionEventKind.PHASE_EFFECT_GATE_FAILED in kinds + assert ExecutionEventKind.ACTION_RETRY in kinds + + +def test_stale_phase_effect_gate_result_is_rejected_after_unresolved_poll() -> None: + engine, _ = _engine() + engine.register(PhaseGateAction()) + initial = _context(0.0, 0.0, 0.2, 0) + session = engine.start((_phase_gate_invocation(engine),), initial) + session.tick(initial) + boundary = session.tick(_context(0.1, 0.0, 0.2, 0)) + request = boundary.pending_phase_effect_gate + assert request is not None + unresolved = session.tick( + _context(0.2, 0.1, 0.2, 0), + phase_effect_gate_result=_phase_gate_result( + request, + success_mask=torch.tensor([False]), + failure_mask=torch.tensor([False]), + ), + ) + assert unresolved.pending_phase_effect_gate is not None + + with pytest.raises(ValueError, match="verification_id"): + session.tick( + _context(0.3, 0.1, 0.2, 0), + phase_effect_gate_result=_phase_gate_result( + request, + success_mask=torch.tensor([True]), + failure_mask=torch.tensor([False]), + ), + ) + + +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() + engine.register(action) + base = _invocation(engine, max_action_retries=0) + 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,), _context(0.0, 0.0, 0.2, 0)) + + failed = session.tick(_context(0.0, 0.0, 0.2, 0)) + + assert failed.command is None + assert failed.status is ExecutionStatus.FAILED + assert failed.eligible_mask.tolist() == [False] + assert any( + event.kind is ExecutionEventKind.ACTION_PLANNING_FAILED + for event in failed.events + ) + + +def test_plan_attempt_records_snapshot_nested_metadata_at_installation() -> None: + engine, _ = _engine() + action = DiagnosticAction() + engine.register(action) + 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,), _context(0.0, 0.0, 0.2, 0)) + + action.metadata["solver"]["iterations"][0] = 99 + action.returned_plans[0].diagnostics.metadata["solver"]["iterations"][1] = 77 + first_read = session.plan_attempts[0] + first_read.plan.diagnostics.metadata["solver"]["iterations"][0] = 42 + second_read = session.plan_attempts[0] + + assert second_read.plan.diagnostics.metadata["solver"]["iterations"] == [3, 5] + + +def test_scene_dependency_window_ignores_expected_self_motion() -> None: + engine, _ = _engine() + action = WindowedDependencyAction() + engine.register(action) + 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,), _context(0.0, 0.0, 0.2, 0)) + + first = session.tick(_context(0.0, 0.0, 0.2, 0)) + moved = session.tick(_context(0.1, 0.0, 0.8, 1)) + + assert first.command is not None + assert moved.command is not None + assert action.plan_count == 1 + assert not any( + event.kind is ExecutionEventKind.DYNAMIC_GOAL_CHANGED for event in moved.events + ) + + +def test_scene_dependency_window_reports_motion_before_cutoff() -> None: + engine, _ = _engine() + action = WindowedDependencyAction() + engine.register(action) + 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,), _context(0.0, 0.0, 0.2, 0)) + + moved = session.tick(_context(0.1, 0.0, 0.8, 1)) + + changed = next( + event + for event in moved.events + if event.kind is ExecutionEventKind.DYNAMIC_GOAL_CHANGED + ) + assert changed.message == ( + "Scene dependency invalidated the active plan at waypoint_index=0: " + "entity_id='target', monitor_cutoff=1, max_translation=0.600000, " + "translation_threshold=0.020000, max_rotation=0.000000, " + "rotation_threshold=0.087266." + ) + assert action.plan_count == 2 + + +def test_initial_eligibility_is_sticky_across_invocation_barriers() -> None: + engine, _ = _engine(batch_size=2) + invocation = _invocation(engine) + supplied_mask = torch.tensor([True, False]) + session = engine.start( + (invocation, invocation), + _context(0.0, (0.0, 0.0), (0.2, 0.2), 0), + eligible_mask=supplied_mask, + ) + supplied_mask.fill_(True) + + first = session.tick(_context(0.0, (0.0, 0.0), (0.2, 0.2), 0)) + session.tick(_context(0.1, (0.0, 0.0), (0.2, 0.2), 0)) + barrier = session.tick(_context(0.2, (0.2, 7.0), (0.2, 0.2), 0)) + second_action = session.tick(_context(0.3, (0.2, 7.0), (0.2, 0.2), 0)) + + assert first.command is not None + assert first.command.active_mask.tolist() == [True, False] + assert barrier.status is ExecutionStatus.RUNNING + assert second_action.command is not None + assert second_action.command.active_mask.tolist() == [True, False] + assert second_action.eligible_mask.tolist() == [True, False] + + +def test_empty_initial_eligibility_fails_without_planning() -> None: + engine, action = _engine(batch_size=2) + initial = _context(0.0, (0.0, 0.0), (0.2, 0.2), 0) + + session = engine.start( + (_invocation(engine),), + initial, + eligible_mask=torch.tensor([False, False]), + ) + terminal = session.tick(initial) + + assert action.plan_count == 0 + assert terminal.status is ExecutionStatus.FAILED + assert terminal.eligible_mask.tolist() == [False, False] + assert any( + event.kind is ExecutionEventKind.SESSION_FAILED for event in terminal.events + ) + + +def test_initial_eligibility_is_owned_and_validated() -> None: + engine, _ = _engine(batch_size=2) + initial = _context(0.0, (0.0, 0.0), (0.2, 0.2), 0) + + with pytest.raises(TypeError, match="eligible_mask must be a torch.Tensor"): + engine.start((_invocation(engine),), initial, eligible_mask=[True, False]) + with pytest.raises(ValueError, match="bool with shape"): + engine.start( + (_invocation(engine),), + initial, + eligible_mask=torch.tensor([1, 0]), + ) + with pytest.raises(ValueError, match="bool with shape"): + engine.start( + (_invocation(engine),), + initial, + eligible_mask=torch.tensor([True]), + ) + + supplied = torch.tensor([True, False]) + session = engine.start( + (_invocation(engine),), + initial, + eligible_mask=supplied, + ) + supplied.fill_(False) + observed = session.eligible_mask + observed.fill_(False) + + assert session.eligible_mask.tolist() == [True, False] + + +def test_deactivate_rows_is_sticky_and_masks_the_next_command() -> None: + engine, _ = _engine(batch_size=2) + initial = _context(0.0, (0.0, 0.0), (0.2, 0.2), 0) + session = engine.start((_invocation(engine),), initial) + session.tick(initial) + + changed = session.deactivate_rows( + torch.tensor([False, True]), + reason="environment terminated", + ) + unchanged = session.deactivate_rows( + torch.tensor([False, True]), + reason="duplicate termination", + ) + tick = session.tick(_context(0.1, (0.0, 0.0), (0.2, 0.2), 0)) + + assert changed.tolist() == [False, True] + assert unchanged.tolist() == [False, False] + assert tick.command is not None + assert tick.command.active_mask.tolist() == [True, False] + assert tick.eligible_mask.tolist() == [True, False] + deactivated = [ + event + for event in tick.events + if event.kind is ExecutionEventKind.ROWS_DEACTIVATED + ] + assert len(deactivated) == 1 + assert deactivated[0].env_mask.tolist() == [False, True] + assert deactivated[0].message == "environment terminated" + + +def test_session_commands_schedule_arrivals_and_final_settling() -> None: + engine, _ = _engine() + engine.register(NonuniformTimingAction()) + session = engine.start( + (_invocation(engine, skill_id="nonuniform_timing"),), + _context(0.0, 0.0, 0.2, 0), + ) + + first = session.tick(_context(0.0, 0.0, 0.2, 0)) + second = session.tick(_context(0.0, 0.0, 0.2, 0)) + third = session.tick(_context(0.1, 0.1, 0.2, 0)) + + assert first.command is not None + assert second.command is not None + assert third.command is not None + command_durations = torch.stack( + [ + first.command.hold_duration, + second.command.hold_duration, + third.command.hold_duration, + ], + dim=1, + ) + assert torch.allclose(command_durations, torch.tensor([[0.1, 0.3, 0.3]])) + assert torch.allclose(command_durations[:, :-1].sum(dim=1), torch.tensor([0.4])) + + +def test_request_snapshot_preserves_live_entity_identity() -> None: + entity = UncopyableEntity() + grasp_xpos = torch.eye(4).unsqueeze(0) + geometry_extent = torch.tensor([0.1, 0.2, 0.3]) + semantics = ObjectSemantics( + affordance=Affordance(), + geometry={"extent": geometry_extent}, + label="object", + entity=entity, + ) + goal = GraspGoal(semantics=semantics, grasp_xpos=grasp_xpos) + + request = ResolvedActionRequest( + skill_id="pick_up", + goal=goal, + binding=ActionBinding(owner_id="snapshot-test"), motion_policy=MotionPolicy(), + tracking_policy=TrackingPolicy.timed(), recovery_policy=RecoveryPolicy(), skill_options=ActionOptions(), ) @@ -381,19 +1499,173 @@ def test_request_snapshot_preserves_live_entity_identity() -> None: def test_scene_motion_replans_late_bound_goal() -> None: engine, action = _engine() - session = engine.start((_invocation(),), _context(0.0, 0.0, 0.1, 0)) + session = engine.start((_invocation(engine),), _context(0.0, 0.0, 0.1, 0)) session.tick(_context(0.0, 0.0, 0.1, 0)) tick = session.tick(_context(0.1, 0.0, 0.3, 1)) kinds = {event.kind for event in tick.events} + changed = next( + event + for event in tick.events + if event.kind is ExecutionEventKind.DYNAMIC_GOAL_CHANGED + ) assert ExecutionEventKind.DYNAMIC_GOAL_CHANGED in kinds assert ExecutionEventKind.REPLANNED in kinds + assert changed.message == ( + "Scene dependency invalidated the active plan at waypoint_index=1: " + "entity_id='target', monitor_cutoff=none, max_translation=0.200000, " + "translation_threshold=0.020000, max_rotation=0.000000, " + "rotation_threshold=0.087266." + ) assert action.plan_count == 2 assert action.requests[0] is action.requests[1] assert tick.command is not None +def test_scene_motion_diagnostic_orders_multiple_changed_entities() -> None: + engine, _ = _engine() + action = MultiDependencyAction() + engine.register(action) + initial = _multi_dependency_context( + 0.0, + target_x=0.1, + obstacle_x=0.4, + version=0, + ) + session = engine.start( + (_invocation(engine, skill_id=action.skill_id),), + initial, + ) + session.tick(initial) + + tick = session.tick( + _multi_dependency_context( + 0.1, + target_x=0.4, + obstacle_x=0.8, + version=1, + target_yaw=0.2, + ) + ) + + changed = next( + event + for event in tick.events + if event.kind is ExecutionEventKind.DYNAMIC_GOAL_CHANGED + ) + assert changed.message == ( + "Scene dependency invalidated the active plan at waypoint_index=1: " + "entity_id='obstacle', monitor_cutoff=none, max_translation=0.400000, " + "translation_threshold=0.020000, max_rotation=0.000000, " + "rotation_threshold=0.087266 | " + "entity_id='target', monitor_cutoff=none, max_translation=0.300000, " + "translation_threshold=0.020000, max_rotation=0.200000, " + "rotation_threshold=0.087266." + ) + + +def test_scene_motion_diagnostic_identifies_missing_entity() -> None: + engine, _ = _engine() + action = MultiDependencyAction() + engine.register(action) + initial = _multi_dependency_context( + 0.0, + target_x=0.1, + obstacle_x=0.4, + version=0, + ) + session = engine.start( + (_invocation(engine, skill_id=action.skill_id),), + initial, + ) + session.tick(initial) + + tick = session.tick( + _multi_dependency_context( + 0.1, + target_x=0.1, + obstacle_x=None, + version=1, + ) + ) + + changed = next( + event + for event in tick.events + if event.kind is ExecutionEventKind.DYNAMIC_GOAL_CHANGED + ) + assert changed.message == ( + "Scene dependency invalidated the active plan at waypoint_index=1: " + "entity_id='obstacle', monitor_cutoff=none, missing=current_scene, " + "max_translation=unavailable, translation_threshold=0.020000, " + "max_rotation=unavailable, rotation_threshold=0.087266." + ) + + +def test_recovery_replan_rejects_runtime_destination_change() -> None: + engine, action = _destination_engine(("first", "second")) + invocation = _destination_invocation(engine) + initial = _context(0.0, 0.0, 0.1, 0) + session = engine.start((invocation,), initial) + + activated = session.tick(initial) + assert activated.command is not None + assert activated.command.commands[0].target.target_id == "arm_a" + + with pytest.raises( + ValueError, + match="Recovery replans must preserve the active runtime destination set", + ) as exc_info: + session.tick(_context(0.1, 0.0, 0.3, 1)) + + assert "arm_a" in str(exc_info.value) + assert "arm_b" in str(exc_info.value) + assert action.plan_count == 2 + + +def test_empty_failed_replan_preserves_destination_for_same_target_retry() -> None: + engine, action = _destination_engine(("first", None, "first")) + invocation = _destination_invocation(engine) + initial = _context(0.0, 0.0, 0.1, 0) + session = engine.start((invocation,), initial) + + activated = session.tick(initial) + assert activated.command is not None + assert activated.command.commands[0].target.target_id == "arm_a" + + recovered = session.tick(_context(0.1, 0.0, 0.3, 1)) + + kinds = [event.kind for event in recovered.events] + assert ExecutionEventKind.DYNAMIC_GOAL_CHANGED in kinds + assert ExecutionEventKind.ACTION_RETRY in kinds + assert kinds.count(ExecutionEventKind.REPLANNED) == 2 + assert action.plan_count == 3 + assert recovered.command is None + assert [target.target_id for target in recovered.hold_targets] == ["arm_a"] + + resumed = session.tick(_context(0.2, 0.0, 0.3, 1)) + assert resumed.command is not None + assert resumed.command.commands[0].target.target_id == "arm_a" + + +def test_empty_failed_replan_does_not_erase_active_tracking_route() -> None: + engine, action = _destination_engine( + ("first", None, "first"), + tracking_provider_revisions=("1", None, "alternate"), + ) + invocation = _destination_invocation(engine) + initial = _context(0.0, 0.0, 0.1, 0) + session = engine.start((invocation,), initial) + + session.tick(initial) + + with pytest.raises(ValueError, match="tracking source fingerprints"): + session.tick(_context(0.1, 0.0, 0.3, 1)) + + assert action.plan_count == 3 + + def test_collision_world_change_replans_with_latest_obstacle_pose() -> None: engine, action = _engine() generator = engine.motion_generator @@ -409,7 +1681,7 @@ def test_collision_world_change_replans_with_latest_obstacle_pose() -> None: (0,), ) session = engine.start( - (_invocation(strategy="motion_gen"),), + (_invocation(engine, strategy="motion_gen"),), initial, ) session.tick(initial) @@ -429,6 +1701,18 @@ def test_collision_world_change_replans_with_latest_obstacle_pose() -> None: latest_obstacles = generator.bind_collision_world.call_args.kwargs["obstacle_poses"] assert latest_obstacles["obstacle"][0, 0, 3] == pytest.approx(0.6) assert tick.command is not None + attempts = session.plan_attempts + assert [attempt.attempt_generation for attempt in attempts] == [0, 1] + assert [attempt.event_kind for attempt in attempts] == [ + ExecutionEventKind.ACTION_PLANNED, + ExecutionEventKind.REPLANNED, + ] + assert [attempt.plan.planned_scene_version for attempt in attempts] == [0, 1] + assert [attempt.plan.planned_collision_world_revision for attempt in attempts] == [ + (0,), + (1,), + ] + assert [attempt.replan_counts for attempt in attempts] == [(0,), (1,)] def test_collision_world_exhaustion_only_disables_changed_environment() -> None: @@ -448,6 +1732,7 @@ def test_collision_world_exhaustion_only_disables_changed_environment() -> None: session = engine.start( ( _invocation( + engine, max_replans=0, strategy="motion_gen", ), @@ -496,6 +1781,7 @@ def test_dynamic_collision_off_skips_binding_and_revision_recovery() -> None: session = engine.start( ( _invocation( + engine, strategy="motion_gen", dynamic_collision_mode=DynamicCollisionMode.OFF, ), @@ -526,6 +1812,7 @@ def test_required_dynamic_collision_rejects_incompatible_strategy() -> None: with pytest.raises(ValueError, match="strategy='motion_gen'"): engine.plan( _invocation( + engine, dynamic_collision_mode=DynamicCollisionMode.REQUIRED, ), _collision_context( @@ -544,6 +1831,7 @@ def test_required_dynamic_collision_rejects_missing_scene_entities() -> None: with pytest.raises(ValueError, match="scene collision entities"): engine.plan( _invocation( + engine, strategy="motion_gen", dynamic_collision_mode=DynamicCollisionMode.REQUIRED, ), @@ -557,6 +1845,7 @@ def test_required_dynamic_collision_rejects_unsupported_planner() -> None: with pytest.raises(ValueError, match="dynamic collision-world support"): engine.plan( _invocation( + engine, strategy="motion_gen", dynamic_collision_mode=DynamicCollisionMode.REQUIRED, ), @@ -579,6 +1868,7 @@ def test_required_dynamic_collision_binds_supported_scene() -> None: plan = engine.plan( _invocation( + engine, strategy="motion_gen", dynamic_collision_mode=DynamicCollisionMode.REQUIRED, ), @@ -598,7 +1888,7 @@ def test_resolved_goal_snapshot_is_reused_during_recovery() -> None: engine, action = _engine() target = torch.eye(4).unsqueeze(0) target[:, 0, 3] = 0.2 - base = _invocation() + base = _invocation(engine) invocation = ActionInvocation( skill_id=base.skill_id, goal=EndEffectorPoseGoal(target), @@ -623,7 +1913,7 @@ def test_resolved_goal_snapshot_is_reused_during_recovery() -> None: def test_subset_replan_restarts_synchronized_active_cohort() -> None: engine, action = _engine(batch_size=2) session = engine.start( - (_invocation(),), + (_invocation(engine),), _context(0.0, (0.0, 0.0), (0.1, 0.2), 0), ) session.tick(_context(0.0, (0.0, 0.0), (0.1, 0.2), 0)) @@ -644,17 +1934,18 @@ def test_subset_replan_restarts_synchronized_active_cohort() -> None: assert changed.env_mask.tolist() == [True, False] assert cohort.env_mask.tolist() == [True, True] assert replanned.eligible_mask.tolist() == [True, True] - assert replanned.command is not None - assert torch.all(replanned.command.positions == 0.0) - assert next_command.command is not None - assert torch.equal(next_command.command.positions[:, 0], torch.tensor([0.4, 0.2])) + assert torch.all(_joint_positions(replanned.command) == 0.0) + assert torch.equal( + _joint_positions(next_command.command)[:, 0], + torch.tensor([0.4, 0.2]), + ) assert action.plan_count == 2 def test_replan_exhaustion_disables_only_triggering_row() -> None: engine, _ = _engine(batch_size=2) session = engine.start( - (_invocation(max_replans=1),), + (_invocation(engine, max_replans=1),), _context(0.0, (0.0, 0.0), (0.1, 0.2), 0), ) session.tick(_context(0.0, (0.0, 0.0), (0.1, 0.2), 0)) @@ -674,9 +1965,51 @@ def test_replan_exhaustion_disables_only_triggering_row() -> None: assert exhausted.command.active_mask.tolist() == [False, True] +def test_action_retry_resets_replan_budget_only_for_allowed_rows() -> None: + engine, _ = _engine(batch_size=2) + session = engine.start( + ( + _invocation( + engine, + max_replans=1, + max_action_retries=1, + ), + ), + _context(0.0, (0.0, 0.0), (0.1, 0.2), 0), + ) + session.tick(_context(0.0, (0.0, 0.0), (0.1, 0.2), 0)) + + row_b_replan = session.tick(_context(0.1, (0.0, 0.0), (0.1, 0.4), 1)) + changed = next( + event + for event in row_b_replan.events + if event.kind is ExecutionEventKind.DYNAMIC_GOAL_CHANGED + ) + assert changed.env_mask.tolist() == [False, True] + + retry_events = session._attempt_action_retry( + torch.tensor([True, False]), + ExecutionEventKind.ACTION_TIMEOUT, + "Row A starts a new action attempt.", + ) + retried = next( + event for event in retry_events if event.kind is ExecutionEventKind.ACTION_RETRY + ) + assert retried.env_mask.tolist() == [True, False] + + row_b_exhausted = session.tick(_context(0.2, (0.0, 0.0), (0.1, 0.6), 2)) + exhausted = next( + event + for event in row_b_exhausted.events + if event.kind is ExecutionEventKind.RECOVERY_EXHAUSTED + ) + assert exhausted.env_mask.tolist() == [False, True] + assert row_b_exhausted.eligible_mask.tolist() == [True, False] + + def test_session_revision_replans_from_latest_context() -> None: engine, action = _engine() - original = _invocation() + original = _invocation(engine) session = engine.start((original,), _context(0.0, 0.0, 0.1, 0)) revised_pose = torch.eye(4).unsqueeze(0) revised_pose[:, 0, 3] = 0.8 @@ -693,22 +2026,26 @@ def test_session_revision_replans_from_latest_context() -> None: session.revise_current(revised) first = session.tick(_context(0.0, 0.0, 0.1, 0)) second = session.tick(_context(0.1, 0.0, 0.1, 0)) + attempts = session.plan_attempts assert action.plan_count == 2 assert action.requests[0] is not action.requests[1] assert [request.revision for request in action.requests] == [0, 1] + assert [attempt.request.revision for attempt in attempts] == [0, 1] + assert attempts[0].request is not attempts[1].request + assert attempts[1].request.motion_policy == revised.motion_policy + assert attempts[1].request.recovery_policy == revised.recovery_policy assert any( event.kind is ExecutionEventKind.INVOCATION_REVISED and event.invocation_revision == 1 for event in first.events ) - assert second.command is not None - assert torch.all(second.command.positions == 0.8) + assert torch.all(_joint_positions(second.command) == 0.8) def test_session_revision_must_advance_same_invocation() -> None: engine, _ = _engine() - original = _invocation() + original = _invocation(engine) session = engine.start((original,), _context(0.0, 0.0, 0.1, 0)) with pytest.raises(ValueError, match="must advance"): @@ -728,10 +2065,115 @@ def test_session_revision_must_advance_same_invocation() -> None: ) +def test_session_revision_rejects_runtime_destination_change() -> None: + engine, action = _destination_engine(("first", "second")) + invocation = _destination_invocation(engine) + initial = _context(0.0, 0.0, 0.1, 0) + session = engine.start((invocation,), initial) + + with pytest.raises( + ValueError, + match="Invocation revisions must preserve the active runtime destination set", + ) as exc_info: + session.revise_current(replace(invocation, revision=1)) + + assert "Start a new invocation" in str(exc_info.value) + assert "arm_a" in str(exc_info.value) + assert "arm_b" in str(exc_info.value) + assert action.plan_count == 2 + + active = session.tick(initial) + assert active.command is not None + assert active.command.commands[0].target.target_id == "arm_a" + + +def test_session_revision_rejects_empty_target_plan() -> None: + engine, action = _destination_engine(("first", None)) + invocation = _destination_invocation(engine) + initial = _context(0.0, 0.0, 0.1, 0) + session = engine.start((invocation,), initial) + + with pytest.raises(ValueError, match="empty replacement plan"): + session.revise_current(replace(invocation, revision=1)) + + assert action.plan_count == 2 + active = session.tick(initial) + assert active.command is not None + assert active.command.commands[0].target.target_id == "arm_a" + + +def test_session_revision_rejects_changed_target_address_fingerprint() -> None: + engine, action = _engine() + invocation = _invocation(engine) + initial = _context(0.0, 0.0, 0.1, 0) + session = engine.start((invocation,), initial) + endpoint = invocation.binding.endpoint("primary", "motion") + changed_endpoint = EndpointBinding( + slot_id=endpoint.slot_id, + endpoint_id=endpoint.endpoint_id, + resource_id=endpoint.resource_id, + adapter_id=endpoint.adapter_id, + target=JointPositionTarget(control_part="arm", joint_ids=(0,)), + capabilities=endpoint.capabilities, + commands=endpoint.commands, + claim_tokens=endpoint.claim_tokens, + joint_ids=(0,), + ) + revised = replace( + invocation, + binding=ActionBinding( + owner_id=invocation.binding.owner_id, + endpoints=(changed_endpoint,), + ), + tracking_policy=TrackingPolicy.timed(), + revision=1, + ) + + with pytest.raises(ValueError, match="address fingerprint"): + session.revise_current(revised) + + assert action.plan_count == 2 + active = session.tick(initial) + assert active.command is not None + target = active.command.commands[0].target + assert isinstance(target, JointPositionTarget) + assert target.joint_ids == (0, 1) + + +def test_tracking_continuity_rejection_leaves_revision_state_transactional( + monkeypatch: pytest.MonkeyPatch, +) -> None: + engine, _ = _engine() + invocation = _invocation(engine) + initial = _context(0.0, 0.0, 0.1, 0) + replacement_context = _context(0.5, 0.2, 0.1, 0) + session = engine.start((invocation,), initial) + revised = replace(invocation, revision=1) + attempt_count = len(session.plan_attempts) + + with monkeypatch.context() as scoped: + scoped.setattr( + session, + "_validate_tracking_continuity", + Mock(side_effect=ValueError("tracking route changed")), + ) + with pytest.raises(ValueError, match="tracking route changed"): + session.revise_current(revised, context=replacement_context) + + assert len(session.plan_attempts) == attempt_count + assert session.active_plan.invocation_revision == 0 + assert session.latest_context.robot.timestamp == pytest.approx(0.0) + + session.revise_current(revised, context=replacement_context) + + assert session.active_plan.invocation_revision == 1 + assert session.latest_context.robot.timestamp == pytest.approx(0.5) + + def test_tracking_error_fails_when_replan_budget_is_zero() -> None: engine, _ = _engine() session = engine.start( - (_invocation(max_replans=0),), + (_invocation(engine, max_replans=0),), _context(0.0, 0.0, 0.2, 0), ) session.tick(_context(0.0, 0.0, 0.2, 0)) @@ -739,7 +2181,7 @@ def test_tracking_error_fails_when_replan_budget_is_zero() -> None: tick = session.tick(_context(0.1, 1.0, 0.2, 0)) kinds = {event.kind for event in tick.events} - assert ExecutionEventKind.TRACKING_ERROR in kinds + assert ExecutionEventKind.TRACKING_DIVERGED in kinds assert ExecutionEventKind.RECOVERY_EXHAUSTED in kinds assert tick.status is ExecutionStatus.FAILED assert tick.eligible_mask.tolist() == [False] @@ -750,6 +2192,7 @@ def test_action_timeout_retry_budget_is_bounded() -> None: session = engine.start( ( _invocation( + engine, max_action_retries=1, action_timeout=0.05, ), @@ -776,7 +2219,7 @@ def test_action_timeout_retry_budget_is_bounded() -> None: def test_session_rejects_changed_environment_identity() -> None: engine, _ = _engine() initial = _context(0.0, 0.0, 0.2, 0) - session = engine.start((_invocation(),), initial) + session = engine.start((_invocation(engine),), initial) changed = PlanningContext( robot=initial.robot, task=initial.task, @@ -788,33 +2231,186 @@ def test_session_rejects_changed_environment_identity() -> None: session.tick(changed) -def test_session_rejects_regressing_scene_snapshot() -> None: - engine, _ = _engine() - session = engine.start((_invocation(),), _context(1.0, 0.0, 0.2, 2)) +def test_session_rejects_regressing_scene_snapshot() -> None: + engine, _ = _engine() + session = engine.start((_invocation(engine),), _context(1.0, 0.0, 0.2, 2)) + + with pytest.raises(ValueError, match="versions must be monotonic"): + session.tick(_context(1.0, 0.0, 0.2, 1)) + + +def test_session_rejects_regressing_collision_world_revision() -> None: + engine, _ = _engine() + qpos = torch.zeros(1, 2) + initial = _collision_context(0.0, qpos, torch.tensor([0.4]), (2,)) + session = engine.start( + (_invocation(engine, strategy="motion_gen"),), + initial, + ) + regressed = _collision_context(0.1, qpos, torch.tensor([0.4]), (1,)) + + with pytest.raises(ValueError, match="Collision-world revisions"): + session.tick(regressed) + + +def test_explicit_verification_with_empty_delta_preserves_task_state() -> None: + session, waiting = _effect_session(action=VerificationOnlyAction()) + request = waiting.pending_effect + assert request is not None + assert request.expected_effects.is_empty + assert request.effect_verification is not None + assert request.effect_verification.kind == "physical.test_completion" + initial_task_state = waiting.task_state + + preserved = session.pending_effect + assert preserved is not None + assert preserved.effect_verification is not None + assert preserved.effect_verification is not request.effect_verification + with pytest.raises(ValueError, match="explicit physical-effect requirement"): + replace(request, effect_verification=None) + + completed = session.tick( + _context(0.21, 0.2, 0.2, 0), + effect_result=_effect_result( + request.verification_id, + success_mask=torch.tensor([True]), + failure_mask=torch.tensor([False]), + ), + ) + + assert completed.status is ExecutionStatus.COMPLETED + assert completed.pending_effect is None + assert completed.task_state is initial_task_state + assert not completed.task_state.held_objects + + +def test_explicit_verification_keeps_partial_and_retry_row_lifecycle() -> None: + session, waiting = _effect_session( + batch_size=2, + max_action_retries=1, + action=VerificationOnlyAction(), + ) + first_request = waiting.pending_effect + assert first_request is not None + initial_task_state = waiting.task_state + + retry = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=_effect_result( + first_request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, True]), + ), + ) + + assert retry.pending_effect is None + assert retry.task_state is initial_task_state + assert any( + event.kind is ExecutionEventKind.ACTION_RETRY + and event.env_mask.tolist() == [False, True] + for event in retry.events + ) + + first_command = session.tick(_context(0.22, (0.2, 0.2), (0.2, 0.2), 0)) + second_command = session.tick(_context(0.23, (0.2, 0.2), (0.2, 0.2), 0)) + second_wait = session.tick(_context(0.24, (0.2, 0.2), (0.2, 0.2), 0)) + assert first_command.command is not None + assert first_command.command.active_mask.tolist() == [False, True] + assert second_command.command is not None + assert second_command.command.active_mask.tolist() == [False, True] + second_request = second_wait.pending_effect + assert second_request is not None + assert second_request.env_mask.tolist() == [False, True] + assert second_request.verification_id != first_request.verification_id + assert second_request.deadline > first_request.deadline + + completed = session.tick( + _context(0.25, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=_effect_result( + second_request.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([False, False]), + ), + ) + + assert completed.status is ExecutionStatus.COMPLETED + assert completed.eligible_mask.tolist() == [True, True] + assert completed.task_state is initial_task_state + + +def test_explicit_verification_partial_success_shrinks_request_without_state_delta() -> ( + None +): + session, waiting = _effect_session( + batch_size=2, + action=VerificationOnlyAction(), + ) + first_request = waiting.pending_effect + assert first_request is not None + initial_task_state = waiting.task_state + + partial = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=_effect_result( + first_request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + + second_request = partial.pending_effect + assert second_request is not None + assert second_request.env_mask.tolist() == [False, True] + assert second_request.verification_id != first_request.verification_id + assert second_request.requested_at == first_request.requested_at + assert second_request.deadline == first_request.deadline + assert second_request.effect_verification is not None + assert second_request.effect_verification.kind == "physical.test_completion" + assert partial.task_state is initial_task_state + + completed = session.tick( + _context(0.22, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=_effect_result( + second_request.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([False, False]), + ), + ) + + assert completed.status is ExecutionStatus.COMPLETED + assert completed.task_state is initial_task_state + - with pytest.raises(ValueError, match="versions must be monotonic"): - session.tick(_context(1.0, 0.0, 0.2, 1)) +def test_explicit_verification_empty_delta_obeys_action_timeout() -> None: + session, waiting = _effect_session( + max_action_retries=0, + action_timeout=0.25, + action=VerificationOnlyAction(), + ) + request = waiting.pending_effect + assert request is not None + initial_task_state = waiting.task_state + timed_out = session.tick(_context(0.3, 0.2, 0.2, 0)) -def test_session_rejects_regressing_collision_world_revision() -> None: - engine, _ = _engine() - qpos = torch.zeros(1, 2) - initial = _collision_context(0.0, qpos, torch.tensor([0.4]), (2,)) - session = engine.start( - (_invocation(strategy="motion_gen"),), - initial, + assert timed_out.status is ExecutionStatus.FAILED + assert timed_out.pending_effect is None + assert timed_out.task_state is initial_task_state + assert any( + event.kind is ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT + for event in timed_out.events + ) + assert any( + event.kind is ExecutionEventKind.RECOVERY_EXHAUSTED + for event in timed_out.events ) - regressed = _collision_context(0.1, qpos, torch.tensor([0.4]), (1,)) - - with pytest.raises(ValueError, match="Collision-world revisions"): - session.tick(regressed) def test_nonempty_effect_is_committed_only_after_external_verification() -> None: engine, _ = _engine() effect = EffectAction() engine.register(effect) - invocation = _invocation() + invocation = _invocation(engine) invocation = ActionInvocation( skill_id="effect", goal=invocation.goal, @@ -830,7 +2426,11 @@ def test_nonempty_effect_is_committed_only_after_external_verification() -> None still_waiting = session.tick(_context(0.25, 0.2, 0.2, 0)) completed = session.tick( _context(0.3, 0.2, 0.2, 0), - effect_success=torch.tensor([True]), + effect_result=_effect_result( + waiting.pending_effect.verification_id, + torch.tensor([True]), + torch.tensor([False]), + ), ) assert waiting.status is ExecutionStatus.RUNNING @@ -858,10 +2458,502 @@ def test_nonempty_effect_is_committed_only_after_external_verification() -> None assert completed.task_state.get_held_object("arm") is not None +def test_initially_ineligible_rows_never_receive_effects() -> None: + session, waiting = _effect_session( + batch_size=2, + eligible_mask=torch.tensor([True, False]), + ) + request = waiting.pending_effect + assert request is not None + assert request.env_mask.tolist() == [True, False] + + completed = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=_effect_result( + request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + + held = completed.task_state.get_held_object("arm") + assert completed.status is ExecutionStatus.COMPLETED + assert completed.eligible_mask.tolist() == [True, False] + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [True, False] + + +def test_partial_effect_success_commits_resolved_rows_and_shrinks_request() -> None: + session, waiting = _effect_session(batch_size=2) + first_request = waiting.pending_effect + assert first_request is not None + + no_progress = session.tick( + _context(0.205, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=_effect_result( + first_request.verification_id, + success_mask=torch.tensor([False, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + assert no_progress.pending_effect is not None + assert no_progress.pending_effect.verification_id == first_request.verification_id + + partial = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=_effect_result( + first_request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + + 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() == [True, False] + assert partial.pending_effect is not None + assert partial.pending_effect.env_mask.tolist() == [False, True] + assert partial.pending_effect.verification_id != first_request.verification_id + assert partial.pending_effect.attempt_generation == first_request.attempt_generation + assert partial.pending_effect.requested_at == first_request.requested_at + assert partial.pending_effect.deadline == first_request.deadline + assert not any( + event.kind is ExecutionEventKind.ACTION_RETRY for event in partial.events + ) + + with pytest.raises(ValueError, match="verification_id"): + session.tick( + _context(0.22, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=_effect_result( + first_request.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([False, False]), + ), + ) + + current_request = partial.pending_effect + completed = session.tick( + _context(0.23, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=_effect_result( + current_request.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([False, False]), + ), + ) + + completed_held = completed.task_state.get_held_object("arm") + assert completed.status is ExecutionStatus.COMPLETED + assert completed_held is not None and completed_held.env_mask is not None + assert completed_held.env_mask.tolist() == [True, True] + + +def test_effect_result_masks_are_owned_disjoint_and_request_scoped() -> None: + success = torch.tensor([True, False]) + failure = torch.tensor([False, True]) + result = _effect_result(0, success, failure) + success.fill_(False) + failure.fill_(False) + assert result.success_mask.tolist() == [True, False] + assert result.failure_mask.tolist() == [False, True] + + with pytest.raises(ValueError, match="must not overlap"): + _effect_result( + 0, + torch.tensor([True, False]), + torch.tensor([True, False]), + ) + with pytest.raises(ValueError, match="invalidation_mask must be a subset"): + _effect_result( + 0, + torch.tensor([False, False]), + torch.tensor([True, False]), + invalidation_mask=torch.tensor([False, True]), + ) + with pytest.raises(ValueError, match="retry_mask must be a subset"): + _effect_result( + 0, + torch.tensor([False, False]), + torch.tensor([True, False]), + retry_mask=torch.tensor([False, True]), + ) + with pytest.raises(ValueError, match="conjunction"): + _effect_result( + 0, + torch.tensor([False, False]), + torch.tensor([False, True]), + expectation_results=( + EffectExpectationResult( + expectation_id="destination", + satisfied_mask=torch.tensor([True, False]), + contradicted_mask=torch.tensor([False, True]), + inverse_satisfied_mask=torch.tensor([False, False]), + ), + ), + ) + + session, waiting = _effect_session(batch_size=2) + request = waiting.pending_effect + assert request is not None + request.env_mask.fill_(False) + published_effect = request.expected_effects.held_object_updates["arm"] + assert published_effect is not None + published_effect.object_to_eef.fill_(9.0) + published_effect.grasp_xpos.fill_(8.0) + published_effect.semantics.affordance.set_custom_config("mutated", True) + preserved = session.pending_effect + assert preserved is not None + assert preserved.env_mask.tolist() == [True, True] + preserved_effect = preserved.expected_effects.held_object_updates["arm"] + assert preserved_effect is not None + assert torch.equal(preserved_effect.object_to_eef, torch.eye(4)) + assert torch.equal(preserved_effect.grasp_xpos, torch.eye(4)) + assert preserved_effect.semantics.affordance.get_custom_config("mutated") is None + + partial = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=_effect_result( + preserved.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + current = partial.pending_effect + assert current is not None + held = partial.task_state.get_held_object("arm") + assert held is not None + assert torch.equal(held.object_to_eef[0], torch.eye(4)) + + with pytest.raises(ValueError, match="subsets"): + session.tick( + _context(0.22, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=_effect_result( + current.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + + +def test_state_delta_snapshot_owns_effect_data_and_preserves_live_entity() -> None: + entity = UncopyableEntity() + semantics = ObjectSemantics( + affordance=Affordance(custom_config={"threshold": [1.0]}), + geometry={"size": torch.ones(3)}, + properties={"mass": torch.tensor(1.0)}, + label="snapshot-object", + entity=entity, + ) + held = HeldObjectState( + semantics=semantics, + object_to_eef=torch.eye(4), + grasp_xpos=torch.eye(4), + ) + delta = StateDelta(held_object_updates={"arm": held}) + + snapshot = delta.snapshot() + copied = snapshot.held_object_updates["arm"] + assert copied is not None + assert copied is not held + assert copied.semantics is not semantics + assert copied.semantics.entity is entity + assert copied.semantics.affordance is not semantics.affordance + assert copied.object_to_eef.data_ptr() != held.object_to_eef.data_ptr() + assert copied.grasp_xpos.data_ptr() != held.grasp_xpos.data_ptr() + + copied.object_to_eef.fill_(7.0) + copied.semantics.affordance.custom_config["threshold"].append(2.0) + copied.semantics.geometry["size"].zero_() + assert torch.equal(held.object_to_eef, torch.eye(4)) + assert semantics.affordance.custom_config["threshold"] == [1.0] + assert torch.equal(semantics.geometry["size"], torch.ones(3)) + + +def test_partial_effect_failure_waits_for_unresolved_rows_then_retries_failure() -> ( + None +): + session, waiting = _effect_session(batch_size=2, max_action_retries=1) + request = waiting.pending_effect + assert request is not None + + partial = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=_effect_result( + request.verification_id, + success_mask=torch.tensor([False, False]), + failure_mask=torch.tensor([True, False]), + ), + ) + + assert partial.pending_effect is not None + assert partial.pending_effect.env_mask.tolist() == [False, True] + assert not any( + event.kind is ExecutionEventKind.ACTION_RETRY for event in partial.events + ) + + unresolved_request = partial.pending_effect + resolved = session.tick( + _context(0.22, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=_effect_result( + unresolved_request.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([False, False]), + ), + ) + + held = resolved.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] + failed_event = next( + event + for event in resolved.events + if event.kind is ExecutionEventKind.EFFECT_VERIFICATION_FAILED + ) + retry_event = next( + event + for event in resolved.events + if event.kind is ExecutionEventKind.ACTION_RETRY + ) + assert failed_event.env_mask.tolist() == [True, False] + assert retry_event.env_mask.tolist() == [True, False] + + retry_command = session.tick(_context(0.23, (0.2, 0.2), (0.2, 0.2), 0)) + assert retry_command.command is not None + assert retry_command.command.active_mask.tolist() == [True, False] + + +def test_effect_failure_applies_request_owned_invalidation_before_recovery() -> None: + initial = _with_held_object(_context(0.0, (0.0, 0.0), (0.2, 0.2), 0)).task + session, waiting = _effect_session( + batch_size=2, + max_action_retries=1, + task_state=initial, + ) + request = waiting.pending_effect + assert request is not None + assert request.failure_invalidation.held_object_updates == {"arm": None} + + terminal = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=_effect_result( + request.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([True, False]), + invalidation_mask=torch.tensor([True, False]), + retry_mask=torch.tensor([False, False]), + ), + ) + + held = terminal.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] + assert terminal.eligible_mask.tolist() == [False, True] + assert any( + event.kind is ExecutionEventKind.RECOVERY_REQUIRED + and event.env_mask.tolist() == [True, False] + for event in terminal.events + ) + assert not any( + event.kind is ExecutionEventKind.ACTION_RETRY for event in terminal.events + ) + + +def test_inverse_proof_can_preserve_state_while_failure_requires_recovery() -> None: + initial = _with_held_object(_context(0.0, 0.0, 0.2, 0)).task + session, waiting = _effect_session(task_state=initial) + request = waiting.pending_effect + assert request is not None + failure = torch.tensor([True]) + + terminal = session.tick( + _context(0.21, 0.2, 0.2, 0), + effect_result=_effect_result( + request.verification_id, + success_mask=torch.tensor([False]), + failure_mask=failure, + invalidation_mask=torch.tensor([False]), + retry_mask=torch.tensor([False]), + expectation_results=( + EffectExpectationResult( + expectation_id="source", + satisfied_mask=torch.tensor([False]), + contradicted_mask=failure, + inverse_satisfied_mask=failure, + ), + ), + ), + ) + + held = terminal.task_state.get_held_object("arm") + assert held is not None and held.env_mask is not None and held.env_mask.all() + assert terminal.status is ExecutionStatus.FAILED + assert any( + event.kind is ExecutionEventKind.RECOVERY_REQUIRED for event in terminal.events + ) + + +def test_unresolved_effect_timeout_invalidates_active_state_fail_closed() -> None: + initial = _with_held_object(_context(0.0, 0.0, 0.2, 0)).task + session, waiting = _effect_session( + action_timeout=0.25, + max_action_retries=1, + task_state=initial, + ) + assert waiting.pending_effect is not None + + terminal = session.tick(_context(0.26, 0.2, 0.2, 0)) + + assert terminal.status is ExecutionStatus.FAILED + assert terminal.task_state.get_held_object("arm") is None + kinds = {event.kind for event in terminal.events} + assert ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT in kinds + assert ExecutionEventKind.RECOVERY_REQUIRED in kinds + assert ExecutionEventKind.ACTION_RETRY not in kinds + + +def test_effect_failure_exhaustion_advances_completed_rows_without_empty_request() -> ( + None +): + session, waiting = _effect_session(batch_size=2, max_action_retries=0) + request = waiting.pending_effect + assert request is not None + + terminal = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=_effect_result( + request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, True]), + ), + ) + + held = terminal.task_state.get_held_object("arm") + assert terminal.status is ExecutionStatus.COMPLETED + assert terminal.eligible_mask.tolist() == [True, False] + assert terminal.pending_effect is None + assert terminal.command is None + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [True, False] + assert any( + event.kind is ExecutionEventKind.RECOVERY_EXHAUSTED + and event.env_mask.tolist() == [False, True] + for event in terminal.events + ) + assert any( + event.kind is ExecutionEventKind.SESSION_COMPLETED for event in terminal.events + ) + + +def test_deactivating_last_unresolved_effect_row_advances_barrier() -> None: + session, waiting = _effect_session(batch_size=2) + request = waiting.pending_effect + assert request is not None + partial = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=_effect_result( + request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + assert partial.pending_effect is not None + + session.deactivate_rows( + torch.tensor([False, True]), + reason="effect observation terminated", + ) + terminal = session.tick(_context(0.22, (0.2, 0.2), (0.2, 0.2), 0)) + + assert terminal.status is ExecutionStatus.COMPLETED + assert terminal.eligible_mask.tolist() == [True, False] + assert terminal.pending_effect is None + assert not any( + event.kind is ExecutionEventKind.EFFECT_VERIFICATION_REQUIRED + for event in terminal.events + ) + + +def test_deactivating_all_effect_rows_is_terminal_and_clears_request() -> None: + session, _ = _effect_session(batch_size=2) + + changed = session.deactivate_rows( + torch.tensor([True, True]), + reason="all environments terminated", + ) + terminal = session.tick(_context(0.21, (0.2, 0.2), (0.2, 0.2), 0)) + + assert changed.tolist() == [True, True] + assert terminal.status is ExecutionStatus.FAILED + assert terminal.pending_effect is None + assert any( + event.kind is ExecutionEventKind.SESSION_FAILED for event in terminal.events + ) + assert not any( + event.kind is ExecutionEventKind.EFFECT_VERIFICATION_REQUIRED + for event in terminal.events + ) + + +def test_effect_request_deadline_is_stable_and_accepts_result_at_boundary() -> None: + session, waiting = _effect_session(action_timeout=0.25) + request = waiting.pending_effect + assert request is not None + assert request.requested_at == pytest.approx(0.2) + assert request.deadline == pytest.approx(0.25) + + polled = session.tick(_context(0.24, 0.2, 0.2, 0)) + assert polled.pending_effect is not None + assert polled.pending_effect.verification_id == request.verification_id + assert polled.pending_effect.requested_at == request.requested_at + assert polled.pending_effect.deadline == request.deadline + + completed = session.tick( + _context(0.25, 0.2, 0.2, 0), + effect_result=_effect_result( + request.verification_id, + success_mask=torch.tensor([True]), + failure_mask=torch.tensor([False]), + ), + ) + assert completed.status is ExecutionStatus.COMPLETED + + +def test_session_revision_cannot_abandon_pending_effect_verification() -> None: + engine, _ = _engine() + engine.register(EffectAction()) + base = _invocation(engine) + invocation = ActionInvocation( + skill_id="effect", + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) + session.tick(_context(0.0, 0.0, 0.2, 0)) + session.tick(_context(0.1, 0.0, 0.2, 0)) + waiting = session.tick(_context(0.2, 0.2, 0.2, 0)) + assert waiting.pending_effect is not None + + with pytest.raises(RuntimeError, match="physical-effect resolution"): + session.revise_current(replace(invocation, revision=1)) + + assert session.effect_verification_pending is True + completed = session.tick( + _context(0.3, 0.2, 0.2, 0), + effect_result=_effect_result( + waiting.pending_effect.verification_id, + torch.tensor([True]), + torch.tensor([False]), + ), + ) + assert completed.status is ExecutionStatus.COMPLETED + assert completed.task_state.get_held_object("arm") is not None + + def test_effect_failure_does_not_commit_and_exhausts_retry_budget() -> None: engine, _ = _engine() engine.register(EffectAction()) - base = _invocation(max_action_retries=0) + base = _invocation(engine, max_action_retries=0) invocation = ActionInvocation( skill_id="effect", goal=base.goal, @@ -873,9 +2965,15 @@ def test_effect_failure_does_not_commit_and_exhausts_retry_budget() -> None: session.tick(_context(0.0, 0.0, 0.2, 0)) session.tick(_context(0.1, 0.0, 0.2, 0)) + waiting = session.tick(_context(0.2, 0.2, 0.2, 0)) + assert waiting.pending_effect is not None failed = session.tick( - _context(0.2, 0.2, 0.2, 0), - effect_success=torch.tensor([False]), + _context(0.3, 0.2, 0.2, 0), + effect_result=_effect_result( + waiting.pending_effect.verification_id, + torch.tensor([False]), + torch.tensor([True]), + ), ) assert failed.status is ExecutionStatus.FAILED @@ -885,12 +2983,166 @@ def test_effect_failure_does_not_commit_and_exhausts_retry_budget() -> None: ) -def test_failed_effect_plan_retries_without_requesting_effect_verification() -> None: +def test_pending_effect_timeout_exhausts_without_committing_late_result() -> None: engine, _ = _engine() - engine.register(FailedEffectAction()) - base = _invocation(max_action_retries=0) + engine.register(EffectAction()) + base = _invocation( + engine, + max_action_retries=0, + action_timeout=0.25, + ) invocation = ActionInvocation( - skill_id="failed_effect", + skill_id="effect", + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) + session.tick(_context(0.0, 0.0, 0.2, 0)) + session.tick(_context(0.1, 0.0, 0.2, 0)) + waiting = session.tick(_context(0.2, 0.2, 0.2, 0)) + assert waiting.pending_effect is not None + + timed_out = session.tick( + _context(0.3, 0.2, 0.2, 0), + effect_result=_effect_result( + waiting.pending_effect.verification_id, + torch.tensor([True]), + torch.tensor([False]), + ), + ) + + kinds = {event.kind for event in timed_out.events} + assert ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT in kinds + assert ExecutionEventKind.RECOVERY_EXHAUSTED in kinds + assert timed_out.status is ExecutionStatus.FAILED + assert timed_out.pending_effect is None + assert timed_out.task_state.get_held_object("arm") is None + + +def test_effect_timeout_exhaustion_advances_rows_already_verified() -> None: + session, waiting = _effect_session( + batch_size=2, + max_action_retries=0, + action_timeout=0.25, + ) + request = waiting.pending_effect + assert request is not None + partial = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=_effect_result( + request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + assert partial.pending_effect is not None + + terminal = session.tick(_context(0.3, (0.2, 0.2), (0.2, 0.2), 0)) + + held = terminal.task_state.get_held_object("arm") + assert terminal.status is ExecutionStatus.COMPLETED + assert terminal.eligible_mask.tolist() == [True, False] + assert terminal.pending_effect is None + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [True, False] + timeout_event = next( + event + for event in terminal.events + if event.kind is ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT + ) + assert timeout_event.env_mask.tolist() == [False, True] + + +def test_effect_timeout_charges_concurrent_planning_failures() -> None: + session, _ = _effect_session( + batch_size=2, + max_action_retries=1, + action_timeout=0.25, + action=MixedEffectAction(), + ) + + first_retry = session.tick(_context(0.3, (0.2, 0.2), (0.2, 0.2), 0)) + retry_event = next( + event + for event in first_retry.events + if event.kind is ExecutionEventKind.ACTION_RETRY + ) + planning_event = next( + event + for event in first_retry.events + if event.kind is ExecutionEventKind.ACTION_PLANNING_FAILED + ) + assert retry_event.env_mask.tolist() == [True, True] + assert planning_event.env_mask.tolist() == [False, True] + + session.tick(_context(0.4, (0.2, 0.2), (0.2, 0.2), 0)) + second_wait = session.tick(_context(0.5, (0.2, 0.2), (0.2, 0.2), 0)) + assert second_wait.pending_effect is not None + terminal = session.tick(_context(0.6, (0.2, 0.2), (0.2, 0.2), 0)) + + assert terminal.status is ExecutionStatus.FAILED + assert terminal.eligible_mask.tolist() == [False, False] + exhausted = next( + event + for event in terminal.events + if event.kind is ExecutionEventKind.RECOVERY_EXHAUSTED + ) + assert exhausted.env_mask.tolist() == [True, True] + + +def test_deferred_effect_failure_charges_concurrent_planning_failures() -> None: + session, waiting = _effect_session( + batch_size=3, + max_action_retries=0, + action=MixedEffectAction(), + ) + request = waiting.pending_effect + assert request is not None + partial = session.tick( + _context(0.21, (0.2, 0.2, 0.2), (0.2, 0.2, 0.2), 0), + effect_result=_effect_result( + request.verification_id, + success_mask=torch.tensor([False, False, False]), + failure_mask=torch.tensor([True, False, False]), + ), + ) + assert partial.pending_effect is not None + assert partial.pending_effect.env_mask.tolist() == [False, True, False] + + session.deactivate_rows( + torch.tensor([False, True, False]), + reason="unresolved effect row terminated", + ) + terminal = session.tick(_context(0.22, (0.2, 0.2, 0.2), (0.2, 0.2, 0.2), 0)) + + assert terminal.status is ExecutionStatus.FAILED + assert terminal.eligible_mask.tolist() == [False, False, False] + planning_event = next( + event + for event in terminal.events + if event.kind is ExecutionEventKind.ACTION_PLANNING_FAILED + ) + exhausted = next( + event + for event in terminal.events + if event.kind is ExecutionEventKind.RECOVERY_EXHAUSTED + ) + assert planning_event.env_mask.tolist() == [False, False, True] + assert exhausted.env_mask.tolist() == [True, False, True] + + +def test_effect_retry_invalidates_previous_verification_id() -> None: + engine, _ = _engine() + engine.register(EffectAction()) + base = _invocation( + engine, + max_action_retries=1, + action_timeout=0.25, + ) + invocation = ActionInvocation( + skill_id="effect", goal=base.goal, binding=base.binding, motion_policy=base.motion_policy, @@ -899,10 +3151,73 @@ def test_failed_effect_plan_retries_without_requesting_effect_verification() -> session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) session.tick(_context(0.0, 0.0, 0.2, 0)) session.tick(_context(0.1, 0.0, 0.2, 0)) + first_wait = session.tick(_context(0.2, 0.2, 0.2, 0)) + assert first_wait.pending_effect is not None + old_id = first_wait.pending_effect.verification_id + old_deadline = first_wait.pending_effect.deadline + old_generation = first_wait.pending_effect.attempt_generation + + retry = session.tick(_context(0.3, 0.2, 0.2, 0)) + assert retry.command is not None + assert any(event.kind is ExecutionEventKind.ACTION_RETRY for event in retry.events) + session.tick(_context(0.4, 0.2, 0.2, 0)) + second_wait = session.tick(_context(0.5, 0.2, 0.2, 0)) + assert second_wait.pending_effect is not None + assert second_wait.pending_effect.verification_id != old_id + assert second_wait.pending_effect.attempt_generation == old_generation + 1 + assert second_wait.pending_effect.deadline > old_deadline + + with pytest.raises(ValueError, match="verification_id"): + session.tick( + _context(0.55, 0.2, 0.2, 0), + effect_result=_effect_result( + old_id, + torch.tensor([True]), + torch.tensor([False]), + ), + ) + + +def test_effect_request_generation_advances_after_tracking_replan() -> None: + engine, _ = _engine() + effect = EffectAction() + engine.register(effect) + base = _invocation(engine) + invocation = ActionInvocation( + skill_id=effect.skill_id, + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) + session.tick(_context(0.0, 0.0, 0.2, 0)) + + replanned = session.tick(_context(0.1, 1.0, 0.2, 0)) + session.tick(_context(0.2, 1.0, 0.2, 0)) + waiting = session.tick(_context(0.3, 0.2, 0.2, 0)) + + assert any(event.kind is ExecutionEventKind.REPLANNED for event in replanned.events) + assert waiting.pending_effect is not None + assert waiting.pending_effect.attempt_generation == 1 - failed = session.tick(_context(0.2, 0.0, 0.2, 0)) + +def test_failed_effect_plan_retries_without_requesting_effect_verification() -> None: + engine, _ = _engine() + engine.register(FailedEffectAction()) + base = _invocation(engine, max_action_retries=0) + invocation = ActionInvocation( + skill_id="failed_effect", + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) + failed = session.tick(_context(0.0, 0.0, 0.2, 0)) assert failed.status is ExecutionStatus.FAILED + assert failed.command is None assert failed.pending_effect is None assert not any( event.kind is ExecutionEventKind.EFFECT_VERIFICATION_REQUIRED diff --git a/tests/sim/atomic_actions/test_module_imports.py b/tests/sim/atomic_actions/test_module_imports.py new file mode 100644 index 000000000..23db8966c --- /dev/null +++ b/tests/sim/atomic_actions/test_module_imports.py @@ -0,0 +1,79 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Regression tests for atomic-action module imports and file entry points.""" + +from __future__ import annotations + +from pathlib import Path +import subprocess +import sys + +REPOSITORY_ROOT = Path(__file__).resolve().parents[3] +PRIMITIVES_DIRECTORY = ( + REPOSITORY_ROOT / "embodichain" / "lab" / "sim" / "atomic_actions" / "primitives" +) +TUTORIAL_DIRECTORY = REPOSITORY_ROOT / "scripts" / "tutorials" / "atomic_action" + +PUBLIC_PRIMITIVE_SCRIPTS = tuple( + path + for path in sorted(PRIMITIVES_DIRECTORY.glob("*.py")) + if not path.name.startswith("_") +) +TUTORIAL_SCRIPTS = tuple(sorted(TUTORIAL_DIRECTORY.glob("*.py"))) + +RUN_PUBLIC_PRIMITIVES_CODE = """ +import runpy +import sys + +for module_path in sys.argv[1:]: + runpy.run_path(module_path, run_name="__main__") +""" + +IMPORT_TUTORIALS_CODE = """ +import runpy +import sys + +for module_path in sys.argv[1:]: + runpy.run_path(module_path, run_name="__atomic_action_import_check__") +""" + + +def _run_import_check( + code: str, paths: tuple[Path, ...] +) -> subprocess.CompletedProcess[str]: + """Run multiple module files in one isolated Python process.""" + return subprocess.run( + [sys.executable, "-c", code, *(str(path) for path in paths)], + cwd=REPOSITORY_ROOT, + capture_output=True, + text=True, + check=False, + ) + + +def test_all_public_primitive_modules_can_run_as_files() -> None: + """Public primitive files should resolve package imports when run directly.""" + result = _run_import_check(RUN_PUBLIC_PRIMITIVES_CODE, PUBLIC_PRIMITIVE_SCRIPTS) + + assert result.returncode == 0, result.stderr + + +def test_all_atomic_action_tutorials_import_without_running_main() -> None: + """Tutorial modules should import without starting their simulations.""" + result = _run_import_check(IMPORT_TUTORIALS_CODE, TUTORIAL_SCRIPTS) + + assert result.returncode == 0, result.stderr diff --git a/tests/sim/atomic_actions/test_motion_strategy_e2e.py b/tests/sim/atomic_actions/test_motion_strategy_e2e.py index dd3e7e058..38bff128c 100644 --- a/tests/sim/atomic_actions/test_motion_strategy_e2e.py +++ b/tests/sim/atomic_actions/test_motion_strategy_e2e.py @@ -25,7 +25,6 @@ from embodichain.lab.sim.robots import CobotMagicCfg from embodichain.lab.sim.planners import MotionGenerator, MotionGenCfg, ToppraPlannerCfg from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, @@ -79,23 +78,29 @@ def _run_reach_test(self, strategy: str): sim, robot, engine = self._setup() try: target, arm_ids = self._reachable_target(robot) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": self.CONTROL_PART}}, + ) result = engine.compile( ( ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(xpos=target), - binding=ActionBinding( - manipulators={"primary": self.CONTROL_PART} - ), + binding=binding, motion_policy=MotionPolicy( strategy=strategy, sample_count=self.SAMPLE_INTERVAL, ), ), - ) + ), + engine.initial_context(control_dt=sim.sim_config.physics_dt), ) assert result.plan_success.all().item(), f"{strategy} reported failure" - final_q = result.trajectory.positions[0, -1, arm_ids] + plan = result.action_plans[0] + assert plan.joint_trajectory is not None + assert plan.commands.frame_count == plan.joint_trajectory.waypoint_count + final_q = plan.joint_trajectory.positions[0, -1, arm_ids] fk = robot.compute_fk( qpos=final_q[None], name=self.CONTROL_PART, to_matrix=True )[0] diff --git a/tests/sim/atomic_actions/test_primitives_helpers.py b/tests/sim/atomic_actions/test_primitives_helpers.py index 6985edda7..f5f297738 100644 --- a/tests/sim/atomic_actions/test_primitives_helpers.py +++ b/tests/sim/atomic_actions/test_primitives_helpers.py @@ -22,15 +22,84 @@ import torch from embodichain.lab.sim.atomic_actions.primitives._helpers import ( + assemble_full_robot_trajectory, + repeat_qpos, + resolve_batched_pose, resolve_object_target, ) +from embodichain.lab.sim.atomic_actions.primitives.pick_up import ( + PickUpOptions, + _upright_yaw_pose_variants, +) + +BATCH_SIZE = 2 +ROBOT_DOF = 6 +WAYPOINT_COUNT = 3 + + +def test_resolve_batched_pose_broadcasts_and_owns_global_pose() -> None: + source = torch.eye(4) + + result = resolve_batched_pose( + source, + num_envs=BATCH_SIZE, + device=torch.device("cpu"), + name="target_pose", + ) + result[0, 0, 0] = 2.0 + + assert result.shape == (BATCH_SIZE, 4, 4) + assert source[0, 0] == 1.0 + + +def test_assemble_full_robot_trajectory_overlays_control_parts() -> None: + base = torch.zeros(BATCH_SIZE, ROBOT_DOF) + first = torch.ones(BATCH_SIZE, WAYPOINT_COUNT, 2) + second = torch.full((BATCH_SIZE, WAYPOINT_COUNT, 1), 2.0) + + result = assemble_full_robot_trajectory( + base, + ( + ((0, 2), first), + ((5,), second), + ), + ) + + expected = repeat_qpos(base, WAYPOINT_COUNT) + expected[:, :, [0, 2]] = 1.0 + expected[:, :, 5] = 2.0 + assert torch.equal(result, expected) + + +def test_assemble_full_robot_trajectory_rejects_empty_parts() -> None: + with pytest.raises(ValueError, match="must not be empty"): + assemble_full_robot_trajectory( + torch.zeros(BATCH_SIZE, ROBOT_DOF), + (), + ) def test_resolve_object_target_uses_custom_name_in_shape_error() -> None: with pytest.raises(ValueError, match="placing_object_target_pose"): resolve_object_target( torch.zeros(2, 4, 4), - n_envs=3, + num_envs=3, device=torch.device("cpu"), name="placing_object_target_pose", ) + + +def test_upright_yaw_pose_variants_preserve_translation() -> None: + pose = torch.eye(4).repeat(2, 1, 1) + pose[:, :3, 3] = torch.tensor([[0.2, -0.1, 0.8], [-0.3, 0.4, 0.7]]) + + variants = _upright_yaw_pose_variants(pose, 4) + + assert variants.shape == (2, 4, 4, 4) + assert torch.allclose(variants[:, :, :3, 3], pose[:, None, :3, 3].expand(-1, 4, -1)) + assert torch.allclose(variants[:, 0], pose) + + +def test_upright_yaw_samples_must_be_positive() -> None: + with pytest.raises(ValueError, match="upright_yaw_samples"): + PickUpOptions(upright_yaw_samples=0) diff --git a/tests/sim/atomic_actions/test_runner.py b/tests/sim/atomic_actions/test_runner.py index bfeccc04b..1b88f769d 100644 --- a/tests/sim/atomic_actions/test_runner.py +++ b/tests/sim/atomic_actions/test_runner.py @@ -37,22 +37,46 @@ CommandAckStatus, CommandOperation, EndEffectorPoseGoal, + EffectVerificationRequest, + EffectVerificationResult, ExecutionEventKind, ExecutionRunner, ExecutionRunnerCfg, + HeldObjectGuardRequest, HeldObjectState, - JointCommand, + JOINT_POSITION_CAPABILITY, + JointPositionPayload, + JointPositionTrackingMetric, + JointPositionTarget, MotionPolicy, ObjectSemantics, + PhaseEffectGateRequest, + PhaseEffectGateRequirement, + PhaseEffectGateResult, + PlanningContextTrackingFeedbackProvider, PlanningContext, RecoveryPolicy, ResolvedActionRequest, RobotObservation, + RuntimeCommandFrame, + RuntimeEndpointTarget, RunnerStatus, + RunnerStep, SceneSnapshot, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, StateDelta, TaskState, TimedTrajectory, + TrackingEvaluation, + TrackingEvaluatorRegistry, + TrackingFeedbackBatch, + TrackingFeedbackProviderRegistry, + TrackingFeedbackSourceRef, + TrackingMetricCfg, + TrackingRuntime, + TrackingState, ) BATCH_SIZE = 1 @@ -63,6 +87,49 @@ TARGET_POSITION = 1.0 +def _effect_result( + verification_id: int, + success_mask: torch.Tensor, + failure_mask: torch.Tensor, + *, + invalidation_mask: torch.Tensor | None = None, + retry_mask: torch.Tensor | None = None, +) -> EffectVerificationResult: + """Build an explicit terminal decision with legacy retry semantics.""" + return EffectVerificationResult( + verification_id=verification_id, + success_mask=success_mask, + failure_mask=failure_mask, + invalidation_mask=( + torch.zeros_like(failure_mask) + if invalidation_mask is None + else invalidation_mask + ), + retry_mask=failure_mask if retry_mask is None else retry_mask, + ) + + +def _phase_gate_result( + request: PhaseEffectGateRequest, + *, + success: bool, + batch_size: int, +) -> PhaseEffectGateResult: + """Build one all-row gate decision correlated with a runner request.""" + success_mask = torch.full((batch_size,), success, dtype=torch.bool) + failure_mask = torch.zeros(batch_size, dtype=torch.bool) + return PhaseEffectGateResult( + verification_id=request.verification_id, + gate_id=request.gate_id, + attempt_generation=request.attempt_generation, + invocation_index=request.invocation_index, + next_waypoint_index=request.next_waypoint_index, + success_mask=success_mask, + failure_mask=failure_mask, + retry_mask=failure_mask, + ) + + class FakeClock: """Deterministic clock used by non-blocking runner tests.""" @@ -115,14 +182,15 @@ def __init__(self, provider: FakeObservationProvider) -> None: self.provider = provider self.send_statuses: deque[CommandAckStatus] = deque() self.follow_commands: deque[bool] = deque() - self.sent: list[JointCommand] = [] + self.sent: list[RuntimeCommandFrame] = [] self.send_times: list[float] = [] - self.held: list[JointCommand] = [] + self.held: list[tuple[tuple[RuntimeEndpointTarget, ...], PlanningContext]] = [] + self.cancelled: list[tuple[RuntimeEndpointTarget, ...]] = [] self.cancel_count = 0 def send( self, - command: JointCommand, + command: RuntimeCommandFrame, *, timeout: float, ) -> CommandAcknowledgement: @@ -136,36 +204,133 @@ def send( ) follows = self.follow_commands.popleft() if self.follow_commands else True if status is CommandAckStatus.ACCEPTED and follows: - self.provider.qpos = command.positions.clone() + positions = self.provider.qpos.clone() + for endpoint_command in command.commands: + target = endpoint_command.target + payload = endpoint_command.payload + assert isinstance(target, JointPositionTarget) + assert isinstance(payload, JointPositionPayload) + joint_ids = list(target.joint_ids) + positions[:, joint_ids] = torch.where( + command.active_mask[:, None], + payload.positions, + positions[:, joint_ids], + ) + self.provider.qpos = positions return CommandAcknowledgement(status) def hold( self, - command: JointCommand, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, *, timeout: float, ) -> CommandAcknowledgement: - """Record and apply a hold command.""" - self.held.append(command) - self.provider.qpos = command.positions.clone() + """Record targets and apply the supplied observed-state hold.""" + self.held.append((tuple(targets), context)) + self.provider.qpos = context.robot.qpos.clone() return CommandAcknowledgement.accepted_ack() - def cancel(self, *, timeout: float) -> CommandAcknowledgement: + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: """Record controller cancellation.""" + self.cancelled.append(tuple(targets)) self.cancel_count += 1 return CommandAcknowledgement.accepted_ack() +class RaisingFeedbackProvider: + """Built-in-source replacement that simulates a provider failure.""" + + provider_id = "planning_context.robot" + revision = "1" + + def observe( + self, + source: TrackingFeedbackSourceRef, + context: PlanningContext, + ) -> TrackingFeedbackBatch: + """Raise instead of returning required feedback.""" + del source, context + raise RuntimeError("provider unavailable") + + +class RaisingJointTrackingEvaluator: + """Joint evaluator replacement that simulates an evaluation failure.""" + + metric_id = JointPositionTrackingMetric.metric_id + revision = JointPositionTrackingMetric.revision + metric_type = JointPositionTrackingMetric + + def evaluate( + self, + desired: TrackingState, + observed: TrackingState, + valid_mask: torch.Tensor, + metric: TrackingMetricCfg, + ) -> TrackingEvaluation: + """Raise instead of evaluating required feedback.""" + del desired, observed, valid_mask, metric + raise RuntimeError("evaluator unavailable") + + +class MaskedFeedbackProvider(PlanningContextTrackingFeedbackProvider): + """Context provider exposing a deterministic per-row validity mask.""" + + def __init__(self, valid_mask: tuple[bool, ...]) -> None: + self.valid_mask = valid_mask + + def observe( + self, + source: TrackingFeedbackSourceRef, + context: PlanningContext, + ) -> TrackingFeedbackBatch: + """Return built-in feedback with selected rows marked invalid.""" + feedback = super().observe(source, context) + return TrackingFeedbackBatch( + source=feedback.source, + state=feedback.state, + valid_mask=torch.tensor( + self.valid_mask, + dtype=torch.bool, + device=feedback.state.device, + ), + timestamp=feedback.timestamp, + ) + + class TimedAction(AtomicAction[EndEffectorPoseGoal, ActionOptions]): """Test action with explicit non-uniform command intervals.""" skill_id: ClassVar[str] = "timed" GoalType: ClassVar[type] = EndEffectorPoseGoal - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset({JOINT_POSITION_CAPABILITY}), + ), + ), + ), + ) + ) - def __init__(self, *, with_effect: bool = False) -> None: + def __init__( + self, + *, + with_effect: bool = False, + with_phase_gate: bool = False, + ) -> None: super().__init__() self.with_effect = with_effect + self.with_phase_gate = with_phase_gate self.plan_count = 0 def _plan( @@ -190,7 +355,6 @@ def _plan( trajectory = TimedTrajectory.from_positions( positions, env_ids=context.env_ids, - control_dt=request.motion_policy.control_dt, dt=dt, ) effects = StateDelta() @@ -210,13 +374,31 @@ def _plan( success=True, trajectory=trajectory, expected_effects=effects, + segment_lengths=( + {"prepare": 2, "commit": 1} if self.with_phase_gate else None + ), ) +def _timed_action_binding(action: TimedAction) -> ActionBinding: + """Bind the timed action's generic motion endpoint to the fake arm.""" + return action.planning_services.bind_control_parts( + TimedAction.binding_contract, + {"primary": {"motion": "arm"}}, + ) + + def _make_runner( *, with_effect: bool = False, + with_phase_gate: bool = False, batch_size: int = BATCH_SIZE, + control_joint_ids: tuple[int, ...] | None = None, + 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, @@ -232,13 +414,18 @@ def _make_runner( robot.dof = ROBOT_DOF robot.control_parts = {"arm": object()} robot.get_qpos.return_value = torch.zeros(batch_size, ROBOT_DOF) - robot.get_joint_ids.return_value = list(range(ROBOT_DOF)) + robot.get_joint_ids.return_value = list( + range(ROBOT_DOF) if control_joint_ids is None else control_joint_ids + ) generator = Mock() generator.robot = robot generator.device = torch.device("cpu") generator.planner.cfg.planner_type = "stub" - action = TimedAction(with_effect=with_effect) - engine = AtomicActionEngine(generator) + action = TimedAction( + with_effect=with_effect, + with_phase_gate=with_phase_gate, + ) + engine = AtomicActionEngine(generator, tracking_runtime=tracking_runtime) engine.register(action) initial_task = TaskState.empty(batch_size, "cpu") initial_context = provider.observe(initial_task) @@ -247,12 +434,22 @@ def _make_runner( invocation = ActionInvocation( skill_id="timed", goal=EndEffectorPoseGoal(goal_pose), - binding=ActionBinding(manipulators={"primary": "arm"}), - motion_policy=MotionPolicy(sample_count=3, control_dt=FIRST_INTERVAL), + binding=_timed_action_binding(action), + motion_policy=MotionPolicy(sample_count=3), recovery_policy=RecoveryPolicy( max_replans=2, - tracking_error_threshold=0.05, - action_timeout=10.0, + max_action_retries=max_action_retries, + action_timeout=action_timeout, + ), + phase_effect_gates=( + ( + PhaseEffectGateRequirement( + gate_id="physical_ready", + segment_name="commit", + ), + ) + if with_phase_gate + else () ), ) session = engine.start((invocation,), initial_context) @@ -261,11 +458,79 @@ 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 +def _successful_effect_result( + context: PlanningContext, + request: EffectVerificationRequest, +) -> EffectVerificationResult: + """Correlate a successful result with the pending effect boundary.""" + return _effect_result( + verification_id=request.verification_id, + success_mask=torch.ones( + 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 _unresolved_effect_result( + context: PlanningContext, + request: EffectVerificationRequest, +) -> EffectVerificationResult: + """Keep every row pending at the current effect boundary.""" + return _effect_result( + 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,)) + + runner.step() + provider.qpos[:, 1] = 42.0 + clock.advance(FIRST_INTERVAL) + second = runner.step() + clock.advance(SECOND_INTERVAL) + runner.step() + clock.advance(SECOND_INTERVAL) + completed = runner.step() + + assert action.plan_count == 1 + assert len(sink.sent) == 3 + assert not any( + event.kind is ExecutionEventKind.TRACKING_DIVERGED + for step in (second, completed) + if step.tick is not None + for event in step.tick.events + ) + assert completed.status is RunnerStatus.COMPLETED + assert provider.qpos[0, 1].item() == 42.0 + + def test_runner_dispatches_only_when_timed_waypoint_is_due() -> None: runner, clock, _, sink, _ = _make_runner() @@ -289,13 +554,175 @@ def test_runner_dispatches_only_when_timed_waypoint_is_due() -> None: assert third.wait_duration == pytest.approx(SECOND_INTERVAL) -def test_session_active_trajectory_returns_an_owned_snapshot() -> None: +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_phase_gate_polls_fresh_state_and_replays_preceding_command() -> None: + runner, clock, _, sink, _ = _make_runner(with_phase_gate=True) + requests: list[tuple[float, PhaseEffectGateRequest]] = [] + + first = runner.step() + clock.advance(first.wait_duration) + boundary = runner.step() + assert boundary.tick is not None + assert boundary.tick.pending_phase_effect_gate is not None + clock.advance(boundary.wait_duration) + + def verifier( + context: PlanningContext, + request: PhaseEffectGateRequest, + ) -> PhaseEffectGateResult: + requests.append((context.robot.timestamp, request)) + return _phase_gate_result( + request, + success=len(requests) == 2, + batch_size=context.batch_size, + ) + + unresolved = runner.step(phase_effect_gate_verifier=verifier) + assert unresolved.tick is not None + assert unresolved.tick.pending_phase_effect_gate is not None + clock.advance(unresolved.wait_duration) + released = runner.step(phase_effect_gate_verifier=verifier) + + assert released.status is RunnerStatus.RUNNING + assert released.tick is not None + assert released.tick.pending_phase_effect_gate is None + assert [value[1].verification_id for value in requests] == [0, 1] + assert [value[1].next_waypoint_index for value in requests] == [2, 2] + assert [value[1].segment_name for value in requests] == ["commit", "commit"] + assert requests[0][0] < requests[1][0] + assert len(sink.sent) == 4 + boundary_payload = sink.sent[1].commands[0].payload + replay_payload = sink.sent[2].commands[0].payload + released_payload = sink.sent[3].commands[0].payload + assert isinstance(boundary_payload, JointPositionPayload) + assert isinstance(replay_payload, JointPositionPayload) + assert isinstance(released_payload, JointPositionPayload) + assert torch.equal(replay_payload.positions, boundary_payload.positions) + assert torch.allclose( + released_payload.positions, + torch.full((BATCH_SIZE, ROBOT_DOF), TARGET_POSITION), + ) + assert any( + event.kind is ExecutionEventKind.PHASE_EFFECT_GATE_SATISFIED + for event in released.tick.events + ) + + +def test_blocking_runner_returns_unverified_phase_gate_boundary() -> None: + runner, _, _, sink, _ = _make_runner(with_phase_gate=True) + + blocked = runner.run_until_blocked() + + assert blocked.status is RunnerStatus.RUNNING + assert blocked.tick is not None + assert blocked.tick.pending_phase_effect_gate is not None + assert blocked.tick.pending_phase_effect_gate.segment_name == "commit" + assert len(sink.sent) == 2 + + +def test_runner_phase_gate_verifier_exception_performs_safe_stop() -> None: + runner, clock, _, sink, _ = _make_runner(with_phase_gate=True) + first = runner.step() + clock.advance(first.wait_duration) + boundary = runner.step() + clock.advance(boundary.wait_duration) + + def verifier( + context: PlanningContext, + request: PhaseEffectGateRequest, + ) -> PhaseEffectGateResult: + del context, request + raise RuntimeError("gate evidence unavailable") + + failed = runner.step(phase_effect_gate_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 failed.message is not None + assert "gate evidence unavailable" in failed.message + + +def test_runner_dispatches_transport_neutral_endpoint_frames() -> None: + runner, _, _, sink, _ = _make_runner() + + runner.step() + + frame = sink.sent[0] + assert isinstance(frame, RuntimeCommandFrame) + assert len(frame.commands) == 1 + endpoint_command = frame.commands[0] + assert isinstance(endpoint_command.target, JointPositionTarget) + assert endpoint_command.target.transport_id == "robot.joint_position" + assert endpoint_command.target.target_id == "arm" + assert endpoint_command.target.joint_ids == (0, 1) + assert isinstance(endpoint_command.payload, JointPositionPayload) + assert endpoint_command.payload.transport_id == endpoint_command.target.transport_id + + +def test_session_active_commands_return_an_owned_endpoint_snapshot() -> None: runner, _, _, _, _ = _make_runner() - trajectory = runner.session.active_trajectory - trajectory.positions.fill_(-1.0) + commands = runner.session.active_commands + payload = commands.frames[0].commands[0].payload + assert isinstance(payload, JointPositionPayload) + payload.positions.fill_(-1.0) - assert torch.all(runner.session.active_trajectory.positions >= 0.0) + current_payload = runner.session.active_commands.frames[0].commands[0].payload + assert isinstance(current_payload, JointPositionPayload) + assert torch.all(current_payload.positions >= 0.0) def test_runner_uses_the_longest_active_batch_interval_as_a_barrier() -> None: @@ -324,6 +751,11 @@ def test_runner_completes_and_holds_after_last_command_settles() -> None: assert completed.command_count == 3 assert [item.operation for item in completed.dispatches] == [CommandOperation.HOLD] assert len(sink.held) == 1 + held_targets, hold_context = sink.held[0] + assert [(target.transport_id, target.target_id) for target in held_targets] == [ + ("robot.joint_position", "arm") + ] + assert torch.equal(hold_context.robot.qpos, sink.provider.qpos) @pytest.mark.parametrize( @@ -345,6 +777,8 @@ def test_runner_safely_stops_when_command_is_not_accepted( CommandOperation.HOLD, ] assert sink.cancel_count == 1 + assert [target.target_id for target in sink.cancelled[0]] == ["arm"] + assert [target.target_id for target in sink.held[0][0]] == ["arm"] assert failed.message is not None and status.value in failed.message @@ -363,6 +797,8 @@ def test_runner_cancel_performs_cancel_then_hold() -> None: assert repeated.status is RunnerStatus.CANCELLED assert repeated.dispatches == () assert sink.cancel_count == 1 + assert sink.cancelled == [()] + assert sink.held[0][0] == () def test_runner_replans_from_observation_after_tracking_error() -> None: @@ -378,33 +814,168 @@ def test_runner_replans_from_observation_after_tracking_error() -> None: assert action.plan_count == 2 assert recovered.tick is not None event_kinds = {event.kind for event in recovered.tick.events} - assert ExecutionEventKind.TRACKING_ERROR in event_kinds + assert ExecutionEventKind.TRACKING_DIVERGED in event_kinds assert ExecutionEventKind.REPLANNED in event_kinds assert recovered.status is RunnerStatus.RUNNING -def test_runner_surfaces_explicit_invocation_revision() -> None: - runner, _, _, _, action = _make_runner() +@pytest.mark.parametrize("failure_kind", ["provider", "evaluator"]) +def test_runner_fails_closed_when_required_tracking_runtime_raises( + failure_kind: str, +) -> None: + builtins = TrackingRuntime.with_builtins() + if failure_kind == "provider": + tracking_runtime = TrackingRuntime( + TrackingFeedbackProviderRegistry((RaisingFeedbackProvider(),)), + builtins.projectors, + builtins.evaluators, + ) + else: + tracking_runtime = TrackingRuntime( + builtins.providers, + builtins.projectors, + TrackingEvaluatorRegistry((RaisingJointTrackingEvaluator(),)), + ) + runner, clock, _, sink, action = _make_runner(tracking_runtime=tracking_runtime) + + runner.step() + clock.advance(FIRST_INTERVAL) + failed = runner.step() + + assert failed.status is RunnerStatus.FAILED + assert failed.tick is not None + event_kinds = {event.kind for event in failed.tick.events} + assert ExecutionEventKind.TRACKING_FEEDBACK_FAILED in event_kinds + assert ExecutionEventKind.REPLANNED not in event_kinds + assert action.plan_count == 1 + assert sink.cancel_count == 1 + + +def test_runner_deactivates_only_rows_with_invalid_required_feedback() -> None: + builtins = TrackingRuntime.with_builtins() + tracking_runtime = TrackingRuntime( + TrackingFeedbackProviderRegistry((MaskedFeedbackProvider((True, False)),)), + builtins.projectors, + builtins.evaluators, + ) + runner, clock, _, _, _ = _make_runner( + batch_size=2, + tracking_runtime=tracking_runtime, + ) + + runner.step() + clock.advance(2.0 * FIRST_INTERVAL) + partial = runner.step() + + assert partial.status is RunnerStatus.RUNNING + assert partial.tick is not None + assert partial.tick.command is not None + assert partial.tick.command.active_mask.tolist() == [True, False] + feedback_failure = next( + event + for event in partial.tick.events + if event.kind is ExecutionEventKind.TRACKING_FEEDBACK_FAILED + ) + assert feedback_failure.env_mask.tolist() == [False, True] + + +def test_runner_maintains_final_target_while_terminal_acceptance_is_pending() -> None: + runner, clock, _, sink, action = _make_runner() + sink.follow_commands.extend([True, True, False]) + + runner.step() + clock.advance(FIRST_INTERVAL) + runner.step() + clock.advance(SECOND_INTERVAL) + runner.step() + final_command = sink.sent[-1] + + clock.advance(SECOND_INTERVAL) + settling = runner.step() + + assert action.plan_count == 1 + assert settling.status is RunnerStatus.RUNNING + assert settling.tick is not None + assert settling.tick.command is not None + assert len(sink.sent) == 4 + assert sink.sent[-1] is settling.tick.command + assert torch.equal(sink.sent[-1].active_mask, final_command.active_mask) + final_payload = final_command.commands[0].payload + settling_payload = sink.sent[-1].commands[0].payload + assert isinstance(final_payload, JointPositionPayload) + assert isinstance(settling_payload, JointPositionPayload) + assert torch.equal(settling_payload.positions, final_payload.positions) + event_kinds = {event.kind for event in settling.tick.events} + assert ExecutionEventKind.TERMINAL_ACCEPTANCE_PENDING in event_kinds + assert ExecutionEventKind.REPLANNED not in event_kinds + + +def test_terminal_settle_reemits_final_target_only_for_pending_rows() -> None: + runner, clock, provider, sink, action = _make_runner(batch_size=2) + + runner.step() + clock.advance(2.0 * FIRST_INTERVAL) + runner.step() + clock.advance(2.0 * SECOND_INTERVAL) + runner.step() + provider.qpos[1].zero_() + + clock.advance(2.0 * SECOND_INTERVAL) + settling = runner.step() + + assert action.plan_count == 1 + assert settling.status is RunnerStatus.RUNNING + assert settling.tick is not None + assert settling.tick.command is not None + assert settling.tick.command.active_mask.tolist() == [False, True] + pending = next( + event + for event in settling.tick.events + if event.kind is ExecutionEventKind.TERMINAL_ACCEPTANCE_PENDING + ) + assert pending.env_mask.tolist() == [False, True] + assert not any( + event.kind is ExecutionEventKind.REPLANNED for event in settling.tick.events + ) + + +def test_runner_revision_waits_for_deadline_and_plans_from_fresh_observation() -> None: + runner, clock, provider, sink, action = _make_runner() + first = runner.step() + assert first.wait_duration == pytest.approx(FIRST_INTERVAL) + revised_pose = torch.eye(4) revised_pose[0, 3] = 2.0 * TARGET_POSITION revised = ActionInvocation( skill_id="timed", goal=EndEffectorPoseGoal(revised_pose), - binding=ActionBinding(manipulators={"primary": "arm"}), - motion_policy=MotionPolicy(sample_count=3, control_dt=FIRST_INTERVAL), + binding=_timed_action_binding(action), + motion_policy=MotionPolicy(sample_count=3), recovery_policy=RecoveryPolicy( max_replans=2, - tracking_error_threshold=0.05, action_timeout=10.0, ), revision=1, ) - runner.session.revise_current(revised) + runner.revise_current(revised) + provider.qpos.fill_(0.4) + waiting = runner.step() + + assert waiting.is_waiting is True + assert action.plan_count == 1 + assert sink.send_times == [0.0] + + clock.advance(FIRST_INTERVAL) result = runner.step() assert action.plan_count == 2 + assert result.command_count == 2 + assert sink.send_times == pytest.approx([0.0, FIRST_INTERVAL]) assert result.tick is not None + revised_payload = result.tick.command.commands[0].payload + assert isinstance(revised_payload, JointPositionPayload) + assert torch.allclose(revised_payload.positions, torch.full((1, 2), 0.4)) assert any( event.kind is ExecutionEventKind.INVOCATION_REVISED and event.invocation_revision == 1 @@ -412,6 +983,37 @@ def test_runner_surfaces_explicit_invocation_revision() -> None: ) +def test_runner_revision_rejects_pending_effect_verification() -> None: + runner, _, _, _, action = _make_runner(with_effect=True) + blocked = runner.run_until_blocked() + assert blocked.tick is not None + assert blocked.tick.pending_effect is not None + assert runner.effect_verification_pending is True + + revised_pose = torch.eye(4) + revised_pose[0, 3] = 2.0 * TARGET_POSITION + revised = ActionInvocation( + skill_id="timed", + goal=EndEffectorPoseGoal(revised_pose), + binding=_timed_action_binding(action), + motion_policy=MotionPolicy(sample_count=3), + recovery_policy=RecoveryPolicy( + max_replans=2, + action_timeout=10.0, + ), + revision=1, + ) + + with pytest.raises(RuntimeError, match="physical-effect resolution"): + runner.revise_current(revised) + + assert runner.effect_verification_pending is True + completed = runner.run_until_blocked( + effect_verifier=_successful_effect_result, + ) + assert completed.status is RunnerStatus.COMPLETED + + def test_runner_fails_safely_when_observation_provider_raises() -> None: runner, _, provider, sink, _ = _make_runner() provider.fail = True @@ -425,6 +1027,8 @@ def test_runner_fails_safely_when_observation_provider_raises() -> None: ] assert len(sink.held) == 1 assert sink.cancel_count == 1 + assert sink.cancelled == [()] + assert sink.held[0][0] == () assert failed.message is not None and "observation unavailable" in failed.message @@ -463,9 +1067,7 @@ def test_blocking_runner_verifies_effect_before_committing_task_state() -> None: runner, _, _, _, _ = _make_runner(with_effect=True) completed = runner.run_until_blocked( - effect_verifier=lambda context, tick: torch.ones( - context.batch_size, dtype=torch.bool - ) + effect_verifier=_successful_effect_result, ) assert completed.status is RunnerStatus.COMPLETED @@ -484,12 +1086,423 @@ def test_blocking_runner_resumes_a_stored_effect_verification_boundary() -> None assert runner.effect_verification_pending is True completed = runner.run_until_blocked( - effect_verifier=lambda context, tick: torch.ones( - context.batch_size, dtype=torch.bool - ) + effect_verifier=_successful_effect_result, ) assert runner.effect_verification_pending is False assert completed.status is RunnerStatus.COMPLETED assert completed.tick is not 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() + assert blocked.context is not None + blocked_at = blocked.context.robot.timestamp + clock.advance(0.5) + resumed_at = clock.now() + observed_at: list[float] = [] + + def record_fresh_context( + context: PlanningContext, + request: EffectVerificationRequest, + ) -> EffectVerificationResult: + observed_at.append(context.robot.timestamp) + return _successful_effect_result(context, request) + + completed = runner.run_until_blocked(effect_verifier=record_fresh_context) + + assert completed.status is RunnerStatus.COMPLETED + assert observed_at and observed_at[0] >= resumed_at + assert observed_at[0] > blocked_at + + +def test_due_effect_verifier_consumes_fresh_observation_in_the_same_step() -> None: + runner, clock, _, _, _ = _make_runner(with_effect=True) + blocked = runner.run_until_blocked() + assert blocked.context is not None + assert blocked.tick is not None and blocked.tick.pending_effect is not None + blocked_at = blocked.context.robot.timestamp + clock.advance(MINIMUM_CYCLE_TIME) + observed_at: list[float] = [] + + def verify_fresh_observation( + context: PlanningContext, + request: EffectVerificationRequest, + ) -> EffectVerificationResult: + observed_at.append(context.robot.timestamp) + return _successful_effect_result(context, request) + + completed = runner.step(effect_verifier=verify_fresh_observation) + + assert completed.status is RunnerStatus.COMPLETED + assert completed.tick is not None and completed.tick.pending_effect is None + assert completed.tick.task_state.get_held_object("arm") is not None + assert completed.context is not None + assert observed_at == [completed.context.robot.timestamp] + assert observed_at[0] > blocked_at + + +def test_effect_verifier_runs_and_succeeds_at_the_request_deadline() -> None: + runner, clock, _, _, _ = _make_runner( + with_effect=True, + action_timeout=2.0, + ) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + request = blocked.tick.pending_effect + clock.advance(request.deadline - clock.now()) + observed_at: list[float] = [] + + def verify_at_deadline( + context: PlanningContext, + current_request: EffectVerificationRequest, + ) -> EffectVerificationResult: + observed_at.append(context.robot.timestamp) + return _successful_effect_result(context, current_request) + + completed = runner.step(effect_verifier=verify_at_deadline) + + assert completed.status is RunnerStatus.COMPLETED + assert observed_at == pytest.approx([request.deadline]) + + +def test_effect_verifier_is_not_called_after_deadline_and_session_retries() -> None: + runner, clock, _, _, action = _make_runner( + with_effect=True, + max_action_retries=1, + action_timeout=2.0, + ) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + request = blocked.tick.pending_effect + plan_count = action.plan_count + clock.advance(request.deadline - clock.now() + MINIMUM_CYCLE_TIME) + verifier = Mock() + + retry = runner.step(effect_verifier=verifier) + + verifier.assert_not_called() + assert retry.status is RunnerStatus.RUNNING + assert retry.tick is not None and retry.tick.command is not None + assert action.plan_count == plan_count + 1 + assert { + ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT, + ExecutionEventKind.ACTION_RETRY, + ExecutionEventKind.REPLANNED, + }.issubset({event.kind for event in retry.tick.events}) + + +def test_effect_result_and_effect_verifier_are_mutually_exclusive() -> None: + runner, _, _, sink, action = _make_runner(with_effect=True) + result = _effect_result( + verification_id=0, + success_mask=torch.tensor([True]), + failure_mask=torch.tensor([False]), + ) + + with pytest.raises(ValueError, match="mutually exclusive"): + runner.step( + effect_result=result, + effect_verifier=_successful_effect_result, + ) + + assert action.plan_count == 1 + assert sink.sent == [] + + +@pytest.mark.parametrize( + "invalid_result", + [None, True], + ids=["none", "wrong-type"], +) +def test_effect_verifier_invalid_result_fails_with_cancel_then_hold( + invalid_result: object | None, +) -> None: + runner, clock, _, sink, _ = _make_runner(with_effect=True) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + clock.advance(MINIMUM_CYCLE_TIME) + + def invalid_verifier( + context: PlanningContext, + request: EffectVerificationRequest, + ) -> object | None: + del context, request + return invalid_result + + failed = runner.step(effect_verifier=invalid_verifier) + + assert failed.status is RunnerStatus.FAILED + assert [item.operation for item in failed.dispatches] == [ + CommandOperation.CANCEL, + CommandOperation.HOLD, + ] + assert sink.cancel_count == 1 + assert failed.message is not None + assert "must return exactly EffectVerificationResult" in failed.message + + +def test_all_false_effect_updates_keep_polling_the_same_request() -> None: + runner, clock, _, sink, _ = _make_runner(with_effect=True) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + initial_request = blocked.tick.pending_effect + observed_requests: list[tuple[int, int]] = [] + + def report_no_progress( + context: PlanningContext, + request: EffectVerificationRequest, + ) -> EffectVerificationResult: + observed_requests.append((request.verification_id, request.attempt_generation)) + return _effect_result( + verification_id=request.verification_id, + success_mask=torch.zeros(context.batch_size, dtype=torch.bool), + failure_mask=torch.zeros(context.batch_size, dtype=torch.bool), + ) + + clock.advance(MINIMUM_CYCLE_TIME) + first_poll = runner.step(effect_verifier=report_no_progress) + clock.advance(MINIMUM_CYCLE_TIME) + second_poll = runner.step(effect_verifier=report_no_progress) + + assert first_poll.status is RunnerStatus.RUNNING + assert second_poll.status is RunnerStatus.RUNNING + assert first_poll.tick is not None and first_poll.tick.pending_effect is not None + assert second_poll.tick is not None and second_poll.tick.pending_effect is not None + assert observed_requests == [ + (initial_request.verification_id, initial_request.attempt_generation), + (initial_request.verification_id, initial_request.attempt_generation), + ] + assert sink.cancel_count == 0 + assert second_poll.tick.task_state.get_held_object("arm") is None + + +def test_partial_effect_verifier_receives_the_committed_task_state() -> None: + runner, _, _, _, _ = _make_runner(with_effect=True, batch_size=2) + observations: list[list[bool] | None] = [] + + def verify_in_two_updates( + context: PlanningContext, + request: EffectVerificationRequest, + ) -> EffectVerificationResult: + held = context.task.get_held_object("arm") + observations.append( + None if held is None or held.env_mask is None else held.env_mask.tolist() + ) + if request.env_mask.tolist() == [True, True]: + return _effect_result( + verification_id=request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ) + assert request.env_mask.tolist() == [False, True] + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [True, False] + assert torch.equal(context.task.held_objects["arm"].env_mask, held.env_mask) + return _effect_result( + verification_id=request.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([False, False]), + ) + + completed = runner.run_until_blocked(effect_verifier=verify_in_two_updates) + + assert completed.status is RunnerStatus.COMPLETED + assert observations == [None, [True, False]] + assert completed.context is not None and completed.tick is not None + assert completed.context.task is completed.tick.task_state + + +def test_runner_effect_timeout_replans_and_invalidates_cached_request() -> None: + runner, clock, _, _, action = _make_runner( + with_effect=True, + max_action_retries=1, + action_timeout=2.0, + ) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + request = blocked.tick.pending_effect + plan_count = action.plan_count + clock.advance(request.deadline - clock.now() + 0.01) + + retry = runner.step() + + assert retry.status is RunnerStatus.RUNNING + assert retry.tick is not None and retry.tick.command is not None + assert runner.effect_verification_pending is False + assert action.plan_count == plan_count + 1 + kinds = {event.kind for event in retry.tick.events} + assert ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT in kinds + assert ExecutionEventKind.ACTION_RETRY in kinds + assert ExecutionEventKind.REPLANNED in kinds + + +def test_runner_effect_timeout_exhaustion_cancels_and_holds() -> None: + runner, clock, _, sink, _ = _make_runner( + with_effect=True, + max_action_retries=0, + action_timeout=2.0, + ) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + request = blocked.tick.pending_effect + clock.advance(request.deadline - clock.now() + 0.01) + + failed = runner.step() + + assert failed.status is RunnerStatus.FAILED + assert runner.effect_verification_pending is False + assert failed.tick is not None and failed.tick.pending_effect is None + assert failed.tick.task_state.get_held_object("arm") is None + assert [item.operation for item in failed.dispatches[-2:]] == [ + CommandOperation.CANCEL, + CommandOperation.HOLD, + ] + 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() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + old_id = blocked.tick.pending_effect.verification_id + old_generation = blocked.tick.pending_effect.attempt_generation + + changed = runner.deactivate_rows( + torch.tensor([False, True]), + reason="environment terminated", + ) + refreshed = runner.run_until_blocked() + + assert changed.tolist() == [False, True] + assert refreshed.tick is not None and refreshed.tick.pending_effect is not None + assert refreshed.tick.pending_effect.env_mask.tolist() == [True, False] + assert refreshed.tick.pending_effect.verification_id != old_id + assert refreshed.tick.pending_effect.attempt_generation == old_generation + + def verify_remaining( + context: PlanningContext, + request: EffectVerificationRequest, + ) -> EffectVerificationResult: + return _effect_result( + verification_id=request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ) + + completed = runner.run_until_blocked(effect_verifier=verify_remaining) + + assert completed.status is RunnerStatus.COMPLETED + assert completed.tick is not None + assert completed.tick.eligible_mask.tolist() == [True, False] + + +def test_blocking_runner_fails_safely_for_a_mismatched_effect_result() -> None: + runner, _, _, sink, _ = _make_runner(with_effect=True) + + def mismatched_effect_result( + context: PlanningContext, + request: EffectVerificationRequest, + ) -> EffectVerificationResult: + return _effect_result( + verification_id=request.verification_id + 1, + success_mask=torch.ones(context.batch_size, dtype=torch.bool), + failure_mask=torch.zeros(context.batch_size, dtype=torch.bool), + ) + + failed = runner.run_until_blocked(effect_verifier=mismatched_effect_result) + + assert failed.status is RunnerStatus.FAILED + assert [item.operation for item in failed.dispatches[-2:]] == [ + CommandOperation.CANCEL, + CommandOperation.HOLD, + ] + assert sink.cancel_count == 1 + assert failed.message is not None + assert "verification_id does not match" in failed.message diff --git a/tests/sim/atomic_actions/test_runtime_commands.py b/tests/sim/atomic_actions/test_runtime_commands.py new file mode 100644 index 000000000..fb0e6bd62 --- /dev/null +++ b/tests/sim/atomic_actions/test_runtime_commands.py @@ -0,0 +1,379 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Pure value-object tests for transport-neutral runtime commands.""" + +from __future__ import annotations + +from dataclasses import dataclass + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions.bindings import ( + JointPositionTarget, + RuntimeEndpointTarget, +) +from embodichain.lab.sim.atomic_actions.runtime_commands import ( + EndpointCommand, + JointPositionPayload, + RuntimeCommandFrame, + RuntimeCommandPayload, + TimedCommandSequence, +) + + +@dataclass(frozen=True, slots=True) +class _TestTarget(RuntimeEndpointTarget): + """Small target used to exercise custom transports.""" + + _transport_id: str + _target_id: str + + @property + def transport_id(self) -> str: + """Return the test transport identifier.""" + return self._transport_id + + @property + def target_id(self) -> str: + """Return the test destination identifier.""" + return self._target_id + + +@dataclass(frozen=True, slots=True) +class _OpaquePayload(RuntimeCommandPayload): + """Metadata-only payload used for transport and device validation.""" + + rows: int + payload_device: torch.device + payload_transport: str + + @property + def batch_size(self) -> int: + """Return the configured row count.""" + return self.rows + + @property + def device(self) -> torch.device: + """Return the configured device.""" + return self.payload_device + + @property + def transport_id(self) -> str: + """Return the configured transport identifier.""" + return self.payload_transport + + def snapshot(self) -> _OpaquePayload: + """Return an independently owned payload.""" + return _OpaquePayload( + rows=self.rows, + payload_device=self.payload_device, + payload_transport=self.payload_transport, + ) + + +class _SelfSnapshotPayload(RuntimeCommandPayload): + """Invalid payload whose snapshot aliases the source.""" + + @property + def batch_size(self) -> int: + """Return one row.""" + return 1 + + @property + def device(self) -> torch.device: + """Return the CPU device.""" + return torch.device("cpu") + + @property + def transport_id(self) -> str: + """Return the test transport.""" + return "test.transport" + + def snapshot(self) -> _SelfSnapshotPayload: + """Incorrectly return this same payload.""" + return self + + +def _joint_command( + control_part: str, + joint_ids: tuple[int, ...], + positions: torch.Tensor, +) -> EndpointCommand: + """Build one joint endpoint command for a test.""" + return EndpointCommand( + target=JointPositionTarget(control_part, joint_ids), + payload=JointPositionPayload(positions), + ) + + +def _frame( + commands: tuple[EndpointCommand, ...], + *, + active_mask: torch.Tensor | None = None, + env_ids: torch.Tensor | None = None, + hold_duration: torch.Tensor | None = None, +) -> RuntimeCommandFrame: + """Build a two-row CPU frame with optional field replacements.""" + return RuntimeCommandFrame( + commands=commands, + active_mask=( + torch.tensor([True, False]) if active_mask is None else active_mask + ), + env_ids=torch.tensor([4, 9]) if env_ids is None else env_ids, + hold_duration=( + torch.tensor([0.0, 0.1]) if hold_duration is None else hold_duration + ), + ) + + +def test_runtime_command_payload_is_abstract() -> None: + with pytest.raises(TypeError): + RuntimeCommandPayload() # type: ignore[abstract] + + +def test_joint_position_payload_owns_tensors_and_snapshots() -> None: + positions = torch.tensor([[1.0, 2.0], [3.0, 4.0]]) + velocities = torch.tensor([[0.1, 0.2], [0.3, 0.4]]) + payload = JointPositionPayload(positions, velocities) + + positions.fill_(9.0) + velocities.fill_(8.0) + snapshot = payload.snapshot() + snapshot.positions.fill_(7.0) + assert payload.positions.tolist() == [[1.0, 2.0], [3.0, 4.0]] + assert payload.velocities is not None + assert torch.allclose( + payload.velocities, + torch.tensor([[0.1, 0.2], [0.3, 0.4]]), + ) + assert payload.batch_size == 2 + assert payload.dof == 2 + assert payload.device == torch.device("cpu") + assert payload.transport_id == JointPositionTarget.TRANSPORT_ID + + +@pytest.mark.parametrize( + "positions, message", + [ + (torch.empty(0, 2), "non-zero"), + (torch.empty(2, 0), "non-zero"), + (torch.zeros(2), "shape"), + (torch.tensor([[float("nan")]]), "finite"), + ], +) +def test_joint_position_payload_rejects_invalid_positions( + positions: torch.Tensor, + message: str, +) -> None: + with pytest.raises(ValueError, match=message): + JointPositionPayload(positions) + + +def test_joint_position_payload_validates_velocities() -> None: + positions = torch.zeros(2, 2) + with pytest.raises(ValueError, match="match positions shape"): + JointPositionPayload(positions, torch.zeros(2, 3)) + with pytest.raises(ValueError, match="finite"): + JointPositionPayload( + positions, + torch.tensor([[0.0, float("inf")], [0.0, 0.0]]), + ) + + +def test_endpoint_command_requires_matching_transport() -> None: + with pytest.raises(ValueError, match="does not accept"): + EndpointCommand( + target=_TestTarget("test.target", "base"), + payload=_OpaquePayload(2, torch.device("cpu"), "test.payload"), + ) + + +def test_endpoint_command_owns_target_and_payload_snapshots() -> None: + target = _TestTarget("test.transport", "base") + payload = _OpaquePayload(2, torch.device("cpu"), "test.transport") + command = EndpointCommand(target=target, payload=payload) + + assert command.target is not target + assert command.payload is not payload + assert command.transport_id == "test.transport" + assert command.destination_key == ("test.transport", "base") + assert command.batch_size == 2 + assert command.device == torch.device("cpu") + assert command.snapshot().payload is not command.payload + + +def test_endpoint_command_rejects_aliased_payload_snapshot() -> None: + with pytest.raises(TypeError, match="independently owned"): + EndpointCommand( + target=_TestTarget("test.transport", "base"), + payload=_SelfSnapshotPayload(), + ) + + +def test_runtime_command_frame_accepts_disjoint_joint_destinations() -> None: + frame = _frame( + ( + _joint_command("left", (0, 2), torch.zeros(2, 2)), + _joint_command("right", (1, 3), torch.ones(2, 2)), + ) + ) + + assert frame.batch_size == 2 + assert frame.device == torch.device("cpu") + assert [target.target_id for target in frame.targets] == ["left", "right"] + assert frame.active_mask.tolist() == [True, False] + assert frame.env_ids.tolist() == [4, 9] + + +def test_runtime_command_frame_rejects_payload_batch_mismatch() -> None: + with pytest.raises(ValueError, match="batch size 1, expected 2"): + _frame((_joint_command("arm", (0,), torch.zeros(1, 1)),)) + + +def test_runtime_command_frame_rejects_payload_device_mismatch() -> None: + command = EndpointCommand( + target=_TestTarget("test.transport", "base"), + payload=_OpaquePayload(2, torch.device("meta"), "test.transport"), + ) + with pytest.raises(ValueError, match="share the frame device"): + _frame((command,)) + + +def test_runtime_command_frame_rejects_duplicate_destination() -> None: + target = _TestTarget("test.transport", "base") + command = EndpointCommand( + target=target, + payload=_OpaquePayload(2, torch.device("cpu"), "test.transport"), + ) + with pytest.raises(ValueError, match="duplicate destination"): + _frame((command, command)) + + +def test_runtime_command_frame_requires_joint_payload_for_joint_target() -> None: + command = EndpointCommand( + target=JointPositionTarget("arm", (0,)), + payload=_OpaquePayload( + 2, + torch.device("cpu"), + JointPositionTarget.TRANSPORT_ID, + ), + ) + with pytest.raises(TypeError, match="requires a JointPositionPayload"): + _frame((command,)) + + +def test_runtime_command_frame_rejects_joint_target_dof_mismatch() -> None: + with pytest.raises(ValueError, match="DOF 1, expected 2"): + _frame((_joint_command("arm", (0, 1), torch.zeros(2, 1)),)) + + +def test_runtime_command_frame_rejects_overlapping_joint_ids() -> None: + with pytest.raises(ValueError, match=r"overlaps joint IDs \[2\]"): + _frame( + ( + _joint_command("left", (0, 2), torch.zeros(2, 2)), + _joint_command("right", (2, 3), torch.zeros(2, 2)), + ) + ) + + +def test_runtime_command_frame_validates_batch_metadata() -> None: + command = _joint_command("arm", (0,), torch.zeros(2, 1)) + with pytest.raises(ValueError, match="active_mask"): + _frame((command,), active_mask=torch.tensor([1, 0])) + with pytest.raises(ValueError, match="env_ids"): + _frame((command,), env_ids=torch.tensor([4.0, 9.0])) + with pytest.raises(ValueError, match="hold_duration"): + _frame((command,), hold_duration=torch.tensor([0.0, float("nan")])) + with pytest.raises(ValueError, match="non-negative"): + _frame((command,), hold_duration=torch.tensor([0.0, -0.1])) + with pytest.raises(ValueError, match="unique"): + _frame((command,), env_ids=torch.tensor([4, 4])) + + +def test_runtime_command_frame_with_active_mask_returns_owned_frame() -> None: + frame = _frame((_joint_command("arm", (0,), torch.zeros(2, 1)),)) + replacement = torch.tensor([False, True]) + updated = frame.with_active_mask(replacement) + + replacement.fill_(False) + updated.commands[0].payload.positions.fill_(4.0) + assert updated.active_mask.tolist() == [False, True] + assert frame.active_mask.tolist() == [True, False] + assert isinstance(frame.commands[0].payload, JointPositionPayload) + assert frame.commands[0].payload.positions.tolist() == [[0.0], [0.0]] + + +def test_timed_command_sequence_preserves_empty_batch_and_device() -> None: + env_ids = torch.tensor([3, 7], dtype=torch.long) + sequence = TimedCommandSequence(frames=(), env_ids=env_ids) + + env_ids.fill_(0) + assert sequence.frame_count == 0 + assert sequence.batch_size == 2 + assert sequence.device == torch.device("cpu") + assert sequence.env_ids.tolist() == [3, 7] + assert sequence.targets == () + + +def test_timed_command_sequence_requires_matching_frame_env_ids() -> None: + frame = _frame((_joint_command("arm", (0,), torch.zeros(2, 1)),)) + with pytest.raises(ValueError, match="env_ids do not match"): + TimedCommandSequence( + frames=(frame,), + env_ids=torch.tensor([4, 8], dtype=torch.long), + ) + + +def test_timed_command_sequence_owns_frames_and_returns_unique_targets() -> None: + first = _frame( + ( + _joint_command("left", (0,), torch.zeros(2, 1)), + _joint_command("right", (1,), torch.ones(2, 1)), + ) + ) + second = _frame((_joint_command("left", (0,), torch.full((2, 1), 2.0)),)) + sequence = TimedCommandSequence( + frames=(first, second), + env_ids=torch.tensor([4, 9]), + ) + snapshot = sequence.snapshot() + + snapshot.frames[0].active_mask.fill_(False) + targets = sequence.targets + assert sequence.frame_count == 2 + assert sequence.frames[0].active_mask.tolist() == [True, False] + assert [target.target_id for target in targets] == ["left", "right"] + assert targets[0] is not sequence.frames[0].commands[0].target + + +def test_timed_command_sequence_rejects_invalid_frame_values() -> None: + with pytest.raises(TypeError, match="RuntimeCommandFrame"): + TimedCommandSequence( + frames=(object(),), # type: ignore[arg-type] + env_ids=torch.tensor([0], dtype=torch.long), + ) + + +def test_timed_command_sequence_requires_nonempty_int64_batch() -> None: + with pytest.raises(ValueError, match="int64"): + TimedCommandSequence(frames=(), env_ids=torch.empty(0, dtype=torch.long)) + with pytest.raises(ValueError, match="int64"): + TimedCommandSequence(frames=(), env_ids=torch.tensor([0.0])) + with pytest.raises(ValueError, match="unique"): + TimedCommandSequence(frames=(), env_ids=torch.tensor([2, 2])) diff --git a/tests/sim/atomic_actions/test_sim_adapter.py b/tests/sim/atomic_actions/test_sim_adapter.py index 01356abde..5b29109c0 100644 --- a/tests/sim/atomic_actions/test_sim_adapter.py +++ b/tests/sim/atomic_actions/test_sim_adapter.py @@ -25,9 +25,13 @@ from embodichain.lab.sim.atomic_actions import ( CommandAckStatus, - JointCommand, + EndpointCommand, + EndpointCommandTransport, + JointPositionPayload, + JointPositionTarget, RigidObjectSceneProvider, RigidObjectSceneProviderCfg, + RuntimeCommandFrame, SceneSnapshot, SimulationExecutionAdapter, TaskState, @@ -53,10 +57,20 @@ def _command( *, env_ids: torch.Tensor | None = None, active_mask: torch.Tensor | None = None, -) -> JointCommand: - return JointCommand( - positions=torch.ones(BATCH_SIZE, ROBOT_DOF), - velocities=torch.full((BATCH_SIZE, ROBOT_DOF), 0.5), +) -> RuntimeCommandFrame: + return RuntimeCommandFrame( + commands=( + EndpointCommand( + target=JointPositionTarget( + control_part="arm", + joint_ids=tuple(range(ROBOT_DOF)), + ), + payload=JointPositionPayload( + positions=torch.ones(BATCH_SIZE, ROBOT_DOF), + velocities=torch.full((BATCH_SIZE, ROBOT_DOF), 0.5), + ), + ), + ), active_mask=( torch.tensor([True, False]) if active_mask is None else active_mask ), @@ -80,6 +94,15 @@ def test_simulation_adapter_observes_full_robot_state() -> None: assert context.scene.version == 0 +def test_simulation_adapter_is_joint_position_transport() -> None: + simulation, robot = _simulation_and_robot() + adapter = SimulationExecutionAdapter(simulation, robot) + + assert isinstance(adapter, EndpointCommandTransport) + assert adapter.transport_id == JointPositionTarget.TRANSPORT_ID + assert adapter.payload_type is JointPositionPayload + + @pytest.mark.parametrize("error", [AttributeError, NotImplementedError]) def test_simulation_adapter_treats_unavailable_effort_as_optional( error: type[Exception], @@ -115,10 +138,86 @@ def test_simulation_adapter_sends_active_rows_and_inactive_holds_together() -> N assert acknowledgement.status is CommandAckStatus.ACCEPTED sent_qpos = robot.set_qpos.call_args.args[0] sent_qvel = robot.set_qvel.call_args.args[0] - assert torch.equal(sent_qpos, command.positions) - assert torch.equal(sent_qvel, command.velocities) + expected_qpos = torch.zeros(BATCH_SIZE, ROBOT_DOF) + expected_qpos[0] = 1.0 + expected_qvel = torch.zeros(BATCH_SIZE, ROBOT_DOF) + expected_qvel[0] = 0.5 + assert torch.equal(sent_qpos, expected_qpos) + assert torch.equal(sent_qvel, expected_qvel) + endpoint_command = command.commands[0] + assert isinstance(endpoint_command.target, JointPositionTarget) + assert endpoint_command.target.target_id == "arm" + assert endpoint_command.target.joint_ids == tuple(range(ROBOT_DOF)) + assert isinstance(endpoint_command.payload, JointPositionPayload) assert robot.set_qpos.call_args.kwargs["env_ids"] == [0, 1] + assert robot.set_qpos.call_args.kwargs["joint_ids"] == [0, 1, 2] assert robot.set_qvel.call_args.kwargs["env_ids"] == [0, 1] + assert robot.set_qvel.call_args.kwargs["joint_ids"] == [0, 1, 2] + + +def test_simulation_adapter_writes_disjoint_joint_endpoints_independently() -> None: + simulation, robot = _simulation_and_robot() + adapter = SimulationExecutionAdapter(simulation, robot) + command = RuntimeCommandFrame( + commands=( + EndpointCommand( + target=JointPositionTarget("arm", (0, 2)), + payload=JointPositionPayload(torch.tensor([[1.0, 3.0], [4.0, 6.0]])), + ), + EndpointCommand( + target=JointPositionTarget("tool", (1,)), + payload=JointPositionPayload(torch.tensor([[2.0], [5.0]])), + ), + ), + active_mask=torch.ones(BATCH_SIZE, dtype=torch.bool), + env_ids=torch.arange(BATCH_SIZE, dtype=torch.long), + hold_duration=torch.zeros(BATCH_SIZE), + ) + + acknowledgement = adapter.send(command, timeout=1.0) + + assert acknowledgement.status is CommandAckStatus.ACCEPTED + assert robot.set_qpos.call_count == 2 + arm_call, tool_call = robot.set_qpos.call_args_list + assert torch.equal( + arm_call.args[0], + torch.tensor([[1.0, 3.0], [4.0, 6.0]]), + ) + assert arm_call.kwargs == {"joint_ids": [0, 2], "env_ids": [0, 1]} + assert torch.equal(tool_call.args[0], torch.tensor([[2.0], [5.0]])) + assert tool_call.kwargs == {"joint_ids": [1], "env_ids": [0, 1]} + robot.set_qvel.assert_not_called() + + +def test_simulation_adapter_neutralizes_inactive_rows_without_velocity_payload() -> ( + None +): + simulation, robot = _simulation_and_robot() + robot.get_qvel.return_value = torch.tensor([[0.1, 0.2, 0.3], [0.4, 0.5, 0.6]]) + adapter = SimulationExecutionAdapter(simulation, robot) + command = RuntimeCommandFrame( + commands=( + EndpointCommand( + target=JointPositionTarget("arm", (0, 2)), + payload=JointPositionPayload(torch.ones(BATCH_SIZE, 2)), + ), + ), + active_mask=torch.tensor([True, False]), + env_ids=torch.arange(BATCH_SIZE, dtype=torch.long), + hold_duration=torch.zeros(BATCH_SIZE), + ) + + acknowledgement = adapter.send(command, timeout=1.0) + + assert acknowledgement.accepted + assert torch.equal( + robot.set_qvel.call_args.args[0], + torch.tensor([[0.1, 0.3], [0.0, 0.0]]), + ) + assert robot.set_qvel.call_args.kwargs == { + "joint_ids": [0, 2], + "env_ids": [0, 1], + } def test_simulation_adapter_send_writes_a_pure_hold_batch() -> None: @@ -129,8 +228,18 @@ def test_simulation_adapter_send_writes_a_pure_hold_batch() -> None: acknowledgement = adapter.send(command, timeout=1.0) assert acknowledgement.status is CommandAckStatus.ACCEPTED - robot.set_qpos.assert_called_once_with(command.positions, env_ids=[0, 1]) - robot.set_qvel.assert_called_once_with(command.velocities, env_ids=[0, 1]) + assert torch.equal( + robot.set_qpos.call_args.args[0], + torch.zeros(BATCH_SIZE, ROBOT_DOF), + ) + assert torch.equal( + robot.set_qvel.call_args.args[0], + torch.zeros(BATCH_SIZE, ROBOT_DOF), + ) + assert robot.set_qpos.call_args.kwargs["env_ids"] == [0, 1] + assert robot.set_qpos.call_args.kwargs["joint_ids"] == [0, 1, 2] + assert robot.set_qvel.call_args.kwargs["env_ids"] == [0, 1] + assert robot.set_qvel.call_args.kwargs["joint_ids"] == [0, 1, 2] def test_simulation_adapter_keeps_stable_ids_separate_from_robot_indices() -> None: @@ -147,14 +256,70 @@ def test_simulation_adapter_keeps_stable_ids_separate_from_robot_indices() -> No def test_simulation_adapter_hold_targets_every_environment() -> None: simulation, robot = _simulation_and_robot() + observed_positions = torch.full((BATCH_SIZE, ROBOT_DOF), 0.25) + robot.get_qpos.return_value = observed_positions adapter = SimulationExecutionAdapter(simulation, robot) command = _command() + context = adapter.observe(TaskState.empty(BATCH_SIZE, "cpu")) + + acknowledgement = adapter.hold(command.targets, context, timeout=1.0) + + assert acknowledgement.status is CommandAckStatus.ACCEPTED + assert torch.equal(robot.set_qpos.call_args.args[0], observed_positions) + assert torch.equal( + robot.set_qvel.call_args.args[0], + torch.zeros_like(observed_positions), + ) + assert robot.set_qpos.call_args.kwargs["env_ids"] == [0, 1] + assert robot.set_qpos.call_args.kwargs["joint_ids"] == [0, 1, 2] + assert robot.set_qvel.call_args.kwargs["env_ids"] == [0, 1] + assert robot.set_qvel.call_args.kwargs["joint_ids"] == [0, 1, 2] + + +def test_simulation_adapter_hold_scopes_write_to_target_joint_ids() -> None: + simulation, robot = _simulation_and_robot() + observed_positions = torch.tensor([[0.1, 0.2, 0.3], [0.4, 0.5, 0.6]]) + robot.get_qpos.return_value = observed_positions + adapter = SimulationExecutionAdapter(simulation, robot) + context = adapter.observe(TaskState.empty(BATCH_SIZE, "cpu")) + + acknowledgement = adapter.hold( + (JointPositionTarget("tool", (1,)),), + context, + timeout=1.0, + ) + + assert acknowledgement.status is CommandAckStatus.ACCEPTED + assert torch.equal( + robot.set_qpos.call_args.args[0], + torch.tensor([[0.2], [0.5]]), + ) + assert robot.set_qpos.call_args.kwargs == { + "joint_ids": [1], + "env_ids": [0, 1], + } + assert torch.equal(robot.set_qvel.call_args.args[0], torch.zeros(BATCH_SIZE, 1)) - acknowledgement = adapter.hold(command, timeout=1.0) + +def test_simulation_adapter_cancel_validates_transport_targets() -> None: + simulation, robot = _simulation_and_robot() + adapter = SimulationExecutionAdapter(simulation, robot) + targets = _command().targets + + acknowledgement = adapter.cancel(targets, timeout=1.0) assert acknowledgement.status is CommandAckStatus.ACCEPTED - robot.set_qpos.assert_called_once_with(command.positions, env_ids=[0, 1]) - robot.set_qvel.assert_called_once_with(command.velocities, env_ids=[0, 1]) + assert [(target.transport_id, target.target_id) for target in targets] == [ + (JointPositionTarget.TRANSPORT_ID, "arm") + ] + robot.set_qpos.assert_not_called() + + invalid = adapter.cancel( + (JointPositionTarget("invalid", (ROBOT_DOF,)),), + timeout=1.0, + ) + assert invalid.status is CommandAckStatus.REJECTED + assert "outside robot DOF" in invalid.message def test_simulation_adapter_sleep_advances_integral_physics_steps() -> None: diff --git a/tests/sim/atomic_actions/test_tracking.py b/tests/sim/atomic_actions/test_tracking.py new file mode 100644 index 000000000..3bff07f3e --- /dev/null +++ b/tests/sim/atomic_actions/test_tracking.py @@ -0,0 +1,263 @@ +# ---------------------------------------------------------------------------- +# 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 + +from dataclasses import dataclass +from typing import ClassVar + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget +from embodichain.lab.sim.atomic_actions.runtime_commands import ( + EndpointCommand, + JointPositionPayload, +) +from embodichain.lab.sim.atomic_actions.state import ( + PlanningContext, + RobotObservation, + SceneSnapshot, + TaskState, +) +from embodichain.lab.sim.atomic_actions.tracking import ( + BASE_POSE_CHANNEL, + JOINT_POSITION_CHANNEL, + EndpointTrackingChannelBinding, + EndpointTrackingFeedbackAddress, + FeedbackTerminalAcceptance, + InFlightTrackingPolicy, + JointPositionTrackingMetric, + JointPositionTrackingState, + PoseTrackingEvaluator, + PoseTrackingMetric, + PoseTrackingState, + TimedTerminalAcceptance, + TimedTrackingSequence, + TrackingFeedbackSourceRef, + TrackingFrame, + TrackingMetricCfg, + TrackingPolicy, + TrackingProjectorRef, + TrackingRuntime, + TrackingSetpoint, + WholeBodyPoseTrackingEvaluator, + WholeBodyPoseTrackingMetric, + WholeBodyPoseTrackingState, +) + + +@dataclass(frozen=True, slots=True) +class _AlternateJointMetric(TrackingMetricCfg): + """Different metric identity deliberately sharing the joint channel.""" + + metric_id: ClassVar[str] = "joint.alternate" + channel_id: ClassVar[str] = JOINT_POSITION_CHANNEL + + +def _joint_binding(target: JointPositionTarget) -> EndpointTrackingChannelBinding: + return EndpointTrackingChannelBinding( + channel_id=JOINT_POSITION_CHANNEL, + source=TrackingFeedbackSourceRef( + provider_id="planning_context.robot", + revision="1", + address=EndpointTrackingFeedbackAddress( + target=target, + channel_id=JOINT_POSITION_CHANNEL, + ), + ), + projector=TrackingProjectorRef( + projector_id="joint_position_payload", + revision="1", + ), + ) + + +def _context(qpos: torch.Tensor) -> PlanningContext: + batch_size = qpos.shape[0] + device = qpos.device + return PlanningContext( + robot=RobotObservation( + timestamp=1.0, + qpos=qpos, + qvel=torch.zeros_like(qpos), + root_pose=torch.eye(4, device=device).repeat(batch_size, 1, 1), + ), + task=TaskState.empty(batch_size=batch_size, device=device), + scene=SceneSnapshot.empty(), + env_ids=torch.arange(batch_size, dtype=torch.long, device=device), + ) + + +def test_joint_policy_factory_separates_in_flight_and_terminal_contracts() -> None: + policy = TrackingPolicy.joint_position( + in_flight_max_abs_error=0.1, + terminal_max_abs_error=0.08, + terminal_settle_timeout=0.25, + ) + + assert policy.in_flight is not None + assert policy.in_flight.metrics == (JointPositionTrackingMetric(0.1),) + assert isinstance(policy.terminal, FeedbackTerminalAcceptance) + assert policy.terminal.metrics == (JointPositionTrackingMetric(0.08),) + assert policy.terminal.settle_timeout == pytest.approx(0.25) + + +def test_policy_rejects_ambiguous_metric_id_for_a_shared_channel() -> None: + with pytest.raises(ValueError, match="same exact metric ID"): + TrackingPolicy( + in_flight=InFlightTrackingPolicy( + metrics=(JointPositionTrackingMetric(0.1),) + ), + terminal=FeedbackTerminalAcceptance(metrics=(_AlternateJointMetric(),)), + ) + + +def test_timed_policy_is_an_explicit_no_feedback_contract() -> None: + policy = TrackingPolicy.timed(settle_duration=0.2) + + assert policy.in_flight is None + assert isinstance(policy.terminal, TimedTerminalAcceptance) + assert policy.terminal.settle_duration == pytest.approx(0.2) + + +def test_tracking_values_and_routes_own_tensor_and_target_snapshots() -> None: + positions = torch.tensor([[0.1, 0.2]]) + target = JointPositionTarget(control_part="arm", joint_ids=(0, 1)) + setpoint = TrackingSetpoint( + endpoint_key=("arm", "controller"), + binding=_joint_binding(target), + desired=JointPositionTrackingState(positions), + ) + + positions.add_(1.0) + + assert torch.equal(setpoint.desired.positions, torch.tensor([[0.1, 0.2]])) + assert setpoint.binding.source.address.target is not target + assert setpoint.key == ("arm", "controller", JOINT_POSITION_CHANNEL) + + +def test_timed_tracking_sequence_owns_env_ids_and_validates_batches() -> None: + env_ids = torch.tensor([2, 5], dtype=torch.long) + target = JointPositionTarget(control_part="arm", joint_ids=(0, 1)) + frame = TrackingFrame( + ( + TrackingSetpoint( + endpoint_key=("arm", "controller"), + binding=_joint_binding(target), + desired=JointPositionTrackingState(torch.zeros(2, 2)), + ), + ) + ) + sequence = TimedTrackingSequence(env_ids=env_ids, frames=(frame,)) + + env_ids[0] = 99 + + assert sequence.env_ids.tolist() == [2, 5] + assert sequence.batch_size == 2 + assert sequence.frame_count == 1 + + +def test_timed_tracking_sequence_rejects_mismatched_setpoint_batch() -> None: + target = JointPositionTarget(control_part="arm", joint_ids=(0, 1)) + frame = TrackingFrame( + ( + TrackingSetpoint( + endpoint_key=("arm", "controller"), + binding=_joint_binding(target), + desired=JointPositionTrackingState(torch.zeros(1, 2)), + ), + ) + ) + + with pytest.raises(ValueError, match="setpoint batch"): + TimedTrackingSequence( + env_ids=torch.tensor([0, 1], dtype=torch.long), + frames=(frame,), + ) + + +def test_builtin_runtime_projects_observes_and_evaluates_joint_positions() -> None: + target = JointPositionTarget(control_part="arm", joint_ids=(1, 3)) + binding = _joint_binding(target) + command = EndpointCommand( + target=target, + payload=JointPositionPayload(positions=torch.tensor([[0.3, 0.5], [0.1, 0.2]])), + ) + runtime = TrackingRuntime.with_builtins() + desired = runtime.project(command, binding) + setpoint = TrackingSetpoint(("arm", "controller"), binding, desired) + context = _context( + torch.tensor( + [ + [0.0, 0.32, 0.0, 0.49], + [0.0, 0.25, 0.0, 0.2], + ] + ) + ) + + feedback = runtime.observe(setpoint, context) + evaluation = runtime.evaluate( + setpoint, + feedback, + JointPositionTrackingMetric(tolerance=0.05), + ) + + assert torch.equal(evaluation.accepted_mask, torch.tensor([True, False])) + assert torch.allclose( + evaluation.component_errors["joint_max_abs"], + torch.tensor([0.02, 0.15]), + ) + + +def test_pose_metric_preserves_translation_and_rotation_components() -> None: + desired = torch.eye(4).repeat(2, 1, 1) + observed = desired.clone() + observed[0, 0, 3] = 0.01 + observed[1, :2, :2] = torch.tensor([[0.0, -1.0], [1.0, 0.0]]) + evaluator = PoseTrackingEvaluator() + + evaluation = evaluator.evaluate( + PoseTrackingState(desired), + PoseTrackingState(observed), + torch.ones(2, dtype=torch.bool), + PoseTrackingMetric(translation_tolerance=0.02, rotation_tolerance=0.1), + ) + + assert evaluation.channel_id == BASE_POSE_CHANNEL + assert evaluation.accepted_mask.tolist() == [True, False] + assert set(evaluation.component_errors) == {"translation", "rotation"} + + +def test_whole_body_metric_requires_pose_and_joint_acceptance() -> None: + root = torch.eye(4).repeat(2, 1, 1) + desired = WholeBodyPoseTrackingState(root, torch.zeros(2, 2)) + observed = WholeBodyPoseTrackingState( + root, + torch.tensor([[0.01, 0.0], [0.0, 0.2]]), + ) + + evaluation = WholeBodyPoseTrackingEvaluator().evaluate( + desired, + observed, + torch.ones(2, dtype=torch.bool), + WholeBodyPoseTrackingMetric(joint_position_tolerance=0.05), + ) + + assert evaluation.accepted_mask.tolist() == [True, False] + assert torch.allclose( + evaluation.component_errors["joint_max_abs"], torch.tensor([0.01, 0.2]) + ) diff --git a/tests/sim/atomic_actions/test_trajectory_ops.py b/tests/sim/atomic_actions/test_trajectory_ops.py index e3dae34a6..24362b8b9 100644 --- a/tests/sim/atomic_actions/test_trajectory_ops.py +++ b/tests/sim/atomic_actions/test_trajectory_ops.py @@ -28,6 +28,7 @@ resolve_object_target, ) from embodichain.lab.sim.atomic_actions.trajectory_ops import ( + axis_translation_keyframes, build_joint_plan_states, build_pose_plan_states, interpolate_hand_qpos, @@ -46,7 +47,7 @@ class TestNormalizeSuccessMask: def test_python_bool_is_expanded_without_collapsing_batch(self): success = normalize_success_mask( True, - n_envs=2, + num_envs=2, device=CPU, name="IK success", ) @@ -56,7 +57,7 @@ def test_python_bool_is_expanded_without_collapsing_batch(self): def test_per_environment_tensor_is_preserved(self): success = normalize_success_mask( torch.tensor([True, False]), - n_envs=2, + num_envs=2, device=CPU, name="IK success", ) @@ -66,7 +67,7 @@ def test_per_environment_tensor_is_preserved(self): def test_binary_integer_success_is_normalized_at_planner_boundary(self): success = normalize_success_mask( torch.tensor([1, 0], dtype=torch.int32), - n_envs=2, + num_envs=2, device=CPU, name="IK success", ) @@ -78,55 +79,70 @@ def test_non_binary_integer_success_is_rejected(self): with pytest.raises(TypeError, match="binary integer"): normalize_success_mask( torch.tensor([1, 2], dtype=torch.int32), - n_envs=2, + num_envs=2, device=CPU, name="IK success", ) + def test_cuda_device_requires_available_runtime(self, monkeypatch): + def unexpected_current_device(): + raise AssertionError("current_device must not be queried") + + monkeypatch.setattr(torch.cuda, "is_available", lambda: False) + monkeypatch.setattr(torch.cuda, "current_device", unexpected_current_device) + + with pytest.raises(ValueError, match="CUDA device requested"): + normalize_success_mask( + True, + num_envs=2, + device="cuda", + name="IK success", + ) + class TestResolvePoseTarget: def test_unbatched_pose_broadcasts(self): pose = torch.eye(4) - out = resolve_pose_target(pose, n_envs=2, device=CPU) + out = resolve_pose_target(pose, num_envs=2, device=CPU) assert out.shape == (2, 4, 4) def test_batched_pose_passes_through(self): pose = torch.eye(4).unsqueeze(0).repeat(2, 1, 1) - out = resolve_pose_target(pose, n_envs=2, device=CPU) + out = resolve_pose_target(pose, num_envs=2, device=CPU) assert torch.equal(out, pose) def test_batched_pose_returns_owned_tensor(self): pose = torch.eye(4).unsqueeze(0).repeat(2, 1, 1) - out = resolve_pose_target(pose, n_envs=2, device=CPU) + out = resolve_pose_target(pose, num_envs=2, device=CPU) out[:, 0, 0] = 2.0 assert torch.equal(pose, torch.eye(4).unsqueeze(0).repeat(2, 1, 1)) def test_pose_converts_to_float32_on_requested_device(self): pose = torch.eye(4, dtype=torch.float64) - out = resolve_pose_target(pose, n_envs=2, device=CPU) + out = resolve_pose_target(pose, num_envs=2, device=CPU) assert out.dtype == torch.float32 assert out.device == CPU def test_wrong_shape_raises(self): with pytest.raises(ValueError): - resolve_pose_target(torch.eye(3), n_envs=2, device=CPU) + resolve_pose_target(torch.eye(3), num_envs=2, device=CPU) def test_multi_waypoint_passes_through(self): pose = torch.eye(4).unsqueeze(0).unsqueeze(0).repeat(2, 3, 1, 1) pose[0, 1, :3, 3] = torch.tensor([1.0, 0.0, 0.0]) - out = resolve_pose_target(pose, n_envs=2, device=CPU) + out = resolve_pose_target(pose, num_envs=2, device=CPU) assert out.shape == (2, 3, 4, 4) assert torch.equal(out, pose.to(torch.float32)) def test_multi_waypoint_wrong_envs_raises(self): pose = torch.eye(4).unsqueeze(0).unsqueeze(0).repeat(3, 2, 1, 1) with pytest.raises(ValueError): - resolve_pose_target(pose, n_envs=2, device=CPU) + resolve_pose_target(pose, num_envs=2, device=CPU) def test_multi_waypoint_empty_raises(self): empty = torch.zeros((2, 0, 4, 4), dtype=torch.float32) with pytest.raises(ValueError, match="zero waypoints"): - resolve_pose_target(empty, n_envs=2, device=CPU) + resolve_pose_target(empty, num_envs=2, device=CPU) class TestResolveObjectTarget: @@ -134,7 +150,7 @@ def test_batched_pose_returns_owned_tensor(self): pose = torch.eye(4).unsqueeze(0).repeat(2, 1, 1) out = resolve_object_target( pose, - n_envs=2, + num_envs=2, device=CPU, ) @@ -148,7 +164,7 @@ def test_unbatched_qpos_broadcasts(self): qpos = torch.arange(6, dtype=torch.float32) out = resolve_joint_target( qpos, - n_envs=2, + num_envs=2, joint_dof=6, control_part="arm", device=CPU, @@ -161,7 +177,7 @@ def test_batched_qpos_passes_through(self): qpos = torch.arange(12, dtype=torch.float32).reshape(2, 6) out = resolve_joint_target( qpos, - n_envs=2, + num_envs=2, joint_dof=6, control_part="arm", device=CPU, @@ -173,7 +189,7 @@ def test_batched_qpos_returns_owned_tensor(self): expected = qpos.clone() out = resolve_joint_target( qpos, - n_envs=2, + num_envs=2, joint_dof=6, control_part="arm", device=CPU, @@ -185,7 +201,7 @@ def test_wrong_shape_raises(self): with pytest.raises(ValueError): resolve_joint_target( torch.zeros(5), - n_envs=2, + num_envs=2, joint_dof=6, control_part="arm", device=CPU, @@ -195,7 +211,7 @@ def test_multi_waypoint_passes_through(self): qpos = torch.arange(24, dtype=torch.float32).reshape(2, 2, 6) out = resolve_joint_target( qpos, - n_envs=2, + num_envs=2, joint_dof=6, control_part="arm", device=CPU, @@ -207,7 +223,7 @@ def test_multi_waypoint_wrong_envs_raises(self): with pytest.raises(ValueError): resolve_joint_target( torch.zeros(3, 2, 6), - n_envs=2, + num_envs=2, joint_dof=6, control_part="arm", device=CPU, @@ -217,7 +233,7 @@ def test_multi_waypoint_wrong_dof_raises(self): with pytest.raises(ValueError): resolve_joint_target( torch.zeros(2, 2, 5), - n_envs=2, + num_envs=2, joint_dof=6, control_part="arm", device=CPU, @@ -228,7 +244,7 @@ def test_multi_waypoint_empty_raises(self): with pytest.raises(ValueError, match="zero waypoints"): resolve_joint_target( empty, - n_envs=2, + num_envs=2, joint_dof=6, control_part="arm", device=CPU, @@ -279,6 +295,11 @@ def test_raises_when_first_segment_too_small(self): with pytest.raises(ValueError): split_three_segments(6, 5) + def test_ratio_is_rounded_after_multiplication(self): + first, hand, third = split_three_segments(10, 2) + + assert (first, hand, third) == (5, 2, 3) + class TestTranslatePoseWorld: def test_offset_adds_to_translation(self): @@ -302,6 +323,45 @@ def test_incompatible_offset_batch_raises(self): translate_pose_world(pose, offset) +class TestAxisTranslationKeyframes: + def test_excludes_start_includes_end_and_stays_on_axis(self): + start = torch.eye(4).repeat(2, 1, 1) + start[:, :3, 3] = torch.tensor([[-0.1, 0.2, 0.3], [0.4, -0.2, 0.1]]) + axis = torch.tensor([[1.0, 0.0, 1.0], [0.0, -1.0, 0.0]]) + axis = torch.nn.functional.normalize(axis, dim=1) + end = start.clone() + end[:, :3, 3] += axis * torch.tensor([[0.5], [-0.3]]) + + keyframes = axis_translation_keyframes( + start, + end, + axis, + n_waypoints=5, + ) + + displacement = keyframes[:, :, :3, 3] - start[:, None, :3, 3] + orthogonal = ( + displacement + - (displacement * axis[:, None]).sum(dim=-1, keepdim=True) * axis[:, None] + ) + assert keyframes.shape == (2, 5, 4, 4) + assert torch.allclose(keyframes[:, -1], end) + assert torch.allclose(orthogonal, torch.zeros_like(orthogonal), atol=1.0e-6) + + def test_rejects_off_axis_displacement(self): + start = torch.eye(4).unsqueeze(0) + end = start.clone() + end[:, 1, 3] = 0.1 + + with pytest.raises(ValueError, match="parallel to axis"): + axis_translation_keyframes( + start, + end, + torch.tensor([1.0, 0.0, 0.0]), + n_waypoints=2, + ) + + def test_interpolate_hand_qpos_preserves_endpoints(): start = torch.tensor([[0.0, 0.0]]) end = torch.tensor([[1.0, 1.0]]) diff --git a/tests/sim/atomic_actions/test_transports.py b/tests/sim/atomic_actions/test_transports.py new file mode 100644 index 000000000..32b74c3a2 --- /dev/null +++ b/tests/sim/atomic_actions/test_transports.py @@ -0,0 +1,522 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Pure routing tests for endpoint-command transports.""" + +from __future__ import annotations + +from dataclasses import dataclass + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions.bindings import RuntimeEndpointTarget +from embodichain.lab.sim.atomic_actions.runner import ( + CommandAcknowledgement, + CommandAckStatus, + CommandSink, +) +from embodichain.lab.sim.atomic_actions.runtime_commands import ( + EndpointCommand, + RuntimeCommandFrame, + RuntimeCommandPayload, +) +from embodichain.lab.sim.atomic_actions.transports import ( + EndpointCommandRouter, + EndpointCommandTransport, +) + + +@dataclass(frozen=True, slots=True) +class _Target(RuntimeEndpointTarget): + """Test-only runtime target.""" + + _transport_id: str + _target_id: str + + @property + def transport_id(self) -> str: + """Return the addressed transport.""" + return self._transport_id + + @property + def target_id(self) -> str: + """Return the local destination.""" + return self._target_id + + +@dataclass(frozen=True, slots=True) +class _Payload(RuntimeCommandPayload): + """Test-only payload with transport-neutral scalar data.""" + + _transport_id: str + values: torch.Tensor + + @property + def batch_size(self) -> int: + """Return the payload batch size.""" + return int(self.values.shape[0]) + + @property + def device(self) -> torch.device: + """Return the payload device.""" + return self.values.device + + @property + def transport_id(self) -> str: + """Return the addressed transport.""" + return self._transport_id + + def snapshot(self) -> _Payload: + """Return an independently owned payload.""" + return _Payload(self._transport_id, self.values.clone()) + + +@dataclass(frozen=True, slots=True) +class _OtherPayload(RuntimeCommandPayload): + """Different payload type used to exercise compatibility checks.""" + + _transport_id: str + values: torch.Tensor + + @property + def batch_size(self) -> int: + """Return the payload batch size.""" + return int(self.values.shape[0]) + + @property + def device(self) -> torch.device: + """Return the payload device.""" + return self.values.device + + @property + def transport_id(self) -> str: + """Return the addressed transport.""" + return self._transport_id + + def snapshot(self) -> _OtherPayload: + """Return an independently owned payload.""" + return _OtherPayload(self._transport_id, self.values.clone()) + + +class _FakeTransport: + """Recording transport with configurable acknowledgements.""" + + def __init__( + self, + transport_id: str, + *, + payload_type: type[RuntimeCommandPayload] = _Payload, + ) -> None: + self._transport_id = transport_id + self._payload_type = payload_type + self.send_ack: object = CommandAcknowledgement.accepted_ack() + self.hold_ack: object = CommandAcknowledgement.accepted_ack() + self.cancel_ack: object = CommandAcknowledgement.accepted_ack() + self.send_error: Exception | None = None + self.hold_error: Exception | None = None + self.cancel_error: Exception | None = None + self.send_calls: list[tuple[RuntimeCommandFrame, float]] = [] + self.hold_calls: list[ + tuple[tuple[RuntimeEndpointTarget, ...], object, float] + ] = [] + self.cancel_calls: list[tuple[tuple[RuntimeEndpointTarget, ...], float]] = [] + + @property + def transport_id(self) -> str: + """Return the fake registration identifier.""" + return self._transport_id + + @property + def payload_type(self) -> type[RuntimeCommandPayload]: + """Return the accepted fake payload type.""" + return self._payload_type + + def send( + self, + frame: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Record one transport-local frame.""" + self.send_calls.append((frame, timeout)) + if self.send_error is not None: + raise self.send_error + return self.send_ack # type: ignore[return-value] + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: object, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Record one transport-local hold.""" + self.hold_calls.append((targets, context, timeout)) + if self.hold_error is not None: + raise self.hold_error + return self.hold_ack # type: ignore[return-value] + + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + """Record one transport-local cancellation.""" + self.cancel_calls.append((targets, timeout)) + if self.cancel_error is not None: + raise self.cancel_error + return self.cancel_ack # type: ignore[return-value] + + +def _command( + transport_id: str, + target_id: str, + *, + payload_type: type[RuntimeCommandPayload] = _Payload, +) -> EndpointCommand: + """Build one two-row endpoint command.""" + return EndpointCommand( + target=_Target(transport_id, target_id), + payload=payload_type( # type: ignore[call-arg] + transport_id, + torch.tensor([[1.0], [2.0]]), + ), + ) + + +def _frame(*commands: EndpointCommand) -> RuntimeCommandFrame: + """Build one two-row command frame.""" + return RuntimeCommandFrame( + commands=commands, + active_mask=torch.tensor([True, False]), + env_ids=torch.tensor([3, 8]), + hold_duration=torch.tensor([0.1, 0.2]), + ) + + +def test_transport_protocol_is_runtime_checkable() -> None: + assert isinstance(_FakeTransport("alpha"), EndpointCommandTransport) + assert not isinstance(object(), EndpointCommandTransport) + + +def test_router_structurally_implements_command_sink() -> None: + assert isinstance(EndpointCommandRouter([]), CommandSink) + + +def test_router_builds_owned_exact_registry_from_mapping() -> None: + alpha = _FakeTransport("alpha") + registrations = {"alpha": alpha} + router = EndpointCommandRouter(registrations) + + registrations.clear() + assert dict(router.transports) == {"alpha": alpha} + with pytest.raises(TypeError): + router.transports["beta"] = _FakeTransport("beta") # type: ignore[index] + + +def test_router_rejects_non_exact_mapping_key() -> None: + with pytest.raises(ValueError, match="exactly match"): + EndpointCommandRouter({"alias": _FakeTransport("alpha")}) + + +def test_router_rejects_duplicate_transport_registration() -> None: + with pytest.raises(ValueError, match="more than once"): + EndpointCommandRouter([_FakeTransport("alpha"), _FakeTransport("alpha")]) + + +def test_router_rejects_invalid_transport_contract_and_payload_type() -> None: + with pytest.raises(TypeError, match="EndpointCommandTransport"): + EndpointCommandRouter([object()]) # type: ignore[list-item] + + invalid_payload = _FakeTransport("alpha") + invalid_payload._payload_type = str # type: ignore[assignment] + with pytest.raises(TypeError, match="payload_type"): + EndpointCommandRouter([invalid_payload]) + + +def test_send_groups_subframes_and_preserves_frame_metadata() -> None: + alpha = _FakeTransport("alpha") + beta = _FakeTransport("beta") + router = EndpointCommandRouter({"alpha": alpha, "beta": beta}) + frame = _frame( + _command("alpha", "a0"), + _command("beta", "b0"), + _command("alpha", "a1"), + ) + + acknowledgement = router.send(frame, timeout=0.75) + + assert acknowledgement.accepted + assert len(alpha.send_calls) == 1 + assert len(beta.send_calls) == 1 + alpha_frame, alpha_timeout = alpha.send_calls[0] + beta_frame, beta_timeout = beta.send_calls[0] + assert [command.target.target_id for command in alpha_frame.commands] == [ + "a0", + "a1", + ] + assert [command.target.target_id for command in beta_frame.commands] == ["b0"] + assert torch.equal(alpha_frame.active_mask, frame.active_mask) + assert torch.equal(alpha_frame.env_ids, frame.env_ids) + assert torch.equal(alpha_frame.hold_duration, frame.hold_duration) + assert alpha_frame.active_mask.data_ptr() != frame.active_mask.data_ptr() + assert alpha_timeout == beta_timeout == 0.75 + + +def test_send_unknown_transport_rejects_before_any_dispatch() -> None: + alpha = _FakeTransport("alpha") + router = EndpointCommandRouter([alpha]) + + acknowledgement = router.send( + _frame(_command("alpha", "a0"), _command("missing", "x0")), + timeout=1.0, + ) + + assert acknowledgement.status is CommandAckStatus.REJECTED + assert "missing" in acknowledgement.message + assert alpha.send_calls == [] + + +def test_send_incompatible_payload_rejects_before_dispatch() -> None: + alpha = _FakeTransport("alpha", payload_type=_Payload) + router = EndpointCommandRouter([alpha]) + + acknowledgement = router.send( + _frame(_command("alpha", "a0", payload_type=_OtherPayload)), + timeout=1.0, + ) + + assert acknowledgement.status is CommandAckStatus.REJECTED + assert "alpha" in acknowledgement.message + assert "_Payload" in acknowledgement.message + assert "_OtherPayload" in acknowledgement.message + assert alpha.send_calls == [] + + +def test_send_aggregates_partial_rejection_with_transport_id() -> None: + alpha = _FakeTransport("alpha") + beta = _FakeTransport("beta") + alpha.send_ack = CommandAcknowledgement.accepted_ack("queued") + beta.send_ack = CommandAcknowledgement( + CommandAckStatus.REJECTED, + "controller busy", + ) + router = EndpointCommandRouter([alpha, beta]) + + acknowledgement = router.send( + _frame(_command("alpha", "a0"), _command("beta", "b0")), + timeout=1.0, + ) + + assert acknowledgement.status is CommandAckStatus.REJECTED + assert "beta" in acknowledgement.message + assert "controller busy" in acknowledgement.message + assert len(alpha.send_calls) == len(beta.send_calls) == 1 + + +def test_send_timed_out_status_takes_failure_precedence() -> None: + alpha = _FakeTransport("alpha") + beta = _FakeTransport("beta") + alpha.send_ack = CommandAcknowledgement(CommandAckStatus.REJECTED, "rejected") + beta.send_ack = CommandAcknowledgement(CommandAckStatus.TIMED_OUT, "late") + router = EndpointCommandRouter([alpha, beta]) + + acknowledgement = router.send( + _frame(_command("alpha", "a0"), _command("beta", "b0")), + timeout=1.0, + ) + + assert acknowledgement.status is CommandAckStatus.TIMED_OUT + assert "alpha" in acknowledgement.message + assert "beta" in acknowledgement.message + + +def test_send_converts_transport_exception_and_continues_dispatch() -> None: + alpha = _FakeTransport("alpha") + beta = _FakeTransport("beta") + alpha.send_error = RuntimeError("send exploded") + router = EndpointCommandRouter([alpha, beta]) + + acknowledgement = router.send( + _frame(_command("alpha", "a0"), _command("beta", "b0")), + timeout=1.0, + ) + + assert acknowledgement.status is CommandAckStatus.REJECTED + assert "alpha" in acknowledgement.message + assert "RuntimeError" in acknowledgement.message + assert "send exploded" in acknowledgement.message + assert len(alpha.send_calls) == len(beta.send_calls) == 1 + + +def test_hold_groups_targets_and_forwards_observation_context() -> None: + alpha = _FakeTransport("alpha") + beta = _FakeTransport("beta") + router = EndpointCommandRouter([alpha, beta]) + context = object() + + acknowledgement = router.hold( + ( + _Target("alpha", "a0"), + _Target("beta", "b0"), + _Target("alpha", "a1"), + ), + context, # type: ignore[arg-type] + timeout=0.4, + ) + + assert acknowledgement.accepted + alpha_targets, alpha_context, alpha_timeout = alpha.hold_calls[0] + beta_targets, beta_context, beta_timeout = beta.hold_calls[0] + assert [target.target_id for target in alpha_targets] == ["a0", "a1"] + assert [target.target_id for target in beta_targets] == ["b0"] + assert alpha_context is beta_context is context + assert alpha_timeout == beta_timeout == 0.4 + + +def test_cancel_groups_targets_and_aggregates_partial_failure() -> None: + alpha = _FakeTransport("alpha") + beta = _FakeTransport("beta") + beta.cancel_ack = CommandAcknowledgement(CommandAckStatus.TIMED_OUT, "late") + router = EndpointCommandRouter([alpha, beta]) + + acknowledgement = router.cancel( + ( + _Target("beta", "b0"), + _Target("alpha", "a0"), + _Target("beta", "b1"), + ), + timeout=0.2, + ) + + assert acknowledgement.status is CommandAckStatus.TIMED_OUT + assert "beta" in acknowledgement.message + assert [target.target_id for target in beta.cancel_calls[0][0]] == ["b0", "b1"] + assert [target.target_id for target in alpha.cancel_calls[0][0]] == ["a0"] + + +@pytest.mark.parametrize("operation", ["hold", "cancel"]) +def test_safe_stop_transport_exception_does_not_block_later_transport( + operation: str, +) -> None: + alpha = _FakeTransport("alpha") + beta = _FakeTransport("beta") + setattr(alpha, f"{operation}_error", RuntimeError(f"{operation} exploded")) + router = EndpointCommandRouter([alpha, beta]) + targets = (_Target("alpha", "a0"), _Target("beta", "b0")) + + if operation == "hold": + acknowledgement = router.hold( + targets, + object(), # type: ignore[arg-type] + timeout=1.0, + ) + alpha_calls = alpha.hold_calls + beta_calls = beta.hold_calls + else: + acknowledgement = router.cancel(targets, timeout=1.0) + alpha_calls = alpha.cancel_calls + beta_calls = beta.cancel_calls + + assert acknowledgement.status is CommandAckStatus.REJECTED + assert "alpha" in acknowledgement.message + assert "RuntimeError" in acknowledgement.message + assert f"{operation} exploded" in acknowledgement.message + assert len(alpha_calls) == len(beta_calls) == 1 + + +@pytest.mark.parametrize("operation", ["hold", "cancel"]) +def test_target_operation_unknown_transport_rejects_before_dispatch( + operation: str, +) -> None: + alpha = _FakeTransport("alpha") + router = EndpointCommandRouter([alpha]) + if operation == "hold": + acknowledgement = router.hold( + (_Target("missing", "x0"),), + object(), # type: ignore[arg-type] + timeout=1.0, + ) + calls = alpha.hold_calls + else: + acknowledgement = router.cancel( + (_Target("missing", "x0"),), + timeout=1.0, + ) + calls = alpha.cancel_calls + + assert acknowledgement.status is CommandAckStatus.REJECTED + assert "missing" in acknowledgement.message + assert calls == [] + + +@pytest.mark.parametrize("operation", ["send", "hold", "cancel"]) +def test_router_converts_invalid_return_type_and_continues_dispatch( + operation: str, +) -> None: + alpha = _FakeTransport("alpha") + beta = _FakeTransport("beta") + setattr(alpha, f"{operation}_ack", object()) + router = EndpointCommandRouter([alpha, beta]) + + if operation == "send": + acknowledgement = router.send( + _frame(_command("alpha", "a0"), _command("beta", "b0")), + timeout=1.0, + ) + beta_calls = beta.send_calls + elif operation == "hold": + acknowledgement = router.hold( + (_Target("alpha", "a0"), _Target("beta", "b0")), + object(), # type: ignore[arg-type] + timeout=1.0, + ) + beta_calls = beta.hold_calls + else: + acknowledgement = router.cancel( + (_Target("alpha", "a0"), _Target("beta", "b0")), + timeout=1.0, + ) + beta_calls = beta.cancel_calls + + assert acknowledgement.status is CommandAckStatus.REJECTED + assert "alpha" in acknowledgement.message + assert "CommandAcknowledgement" in acknowledgement.message + assert len(beta_calls) == 1 + + +@pytest.mark.parametrize("timeout", [0.0, -1.0, float("inf"), float("nan")]) +@pytest.mark.parametrize("operation", ["send", "hold", "cancel"]) +def test_router_rejects_invalid_timeout(operation: str, timeout: float) -> None: + router = EndpointCommandRouter([]) + + with pytest.raises(ValueError, match="timeout"): + if operation == "send": + router.send(_frame(), timeout=timeout) + elif operation == "hold": + router.hold((), object(), timeout=timeout) # type: ignore[arg-type] + else: + router.cancel((), timeout=timeout) + + +def test_empty_operations_are_accepted() -> None: + router = EndpointCommandRouter([]) + + assert router.send(_frame(), timeout=1.0).accepted + assert router.hold((), object(), timeout=1.0).accepted # type: ignore[arg-type] + assert router.cancel((), timeout=1.0).accepted diff --git a/tests/sim/atomic_actions/test_tutorial_utils.py b/tests/sim/atomic_actions/test_tutorial_utils.py index 07d101939..c40b330a8 100644 --- a/tests/sim/atomic_actions/test_tutorial_utils.py +++ b/tests/sim/atomic_actions/test_tutorial_utils.py @@ -18,7 +18,9 @@ from __future__ import annotations +import importlib from argparse import Namespace +from types import SimpleNamespace from unittest.mock import MagicMock, call, patch import pytest @@ -31,11 +33,24 @@ _maximum_path_deviation, _minimum_cuboid_clearance, ) +from scripts.tutorials.atomic_action.coordinated_pickment import ( + compute_left_to_right_arm_direction, +) +from scripts.tutorials.atomic_action.scenario_utils import ( + create_dual_tutorial_robot_cfg, +) from scripts.tutorials.atomic_action.tutorial_utils import ( + TUTORIAL_ROBOTS, broadcast_pose_batch, broadcast_waypoint_pose_batch, clone_local_pose_from_first_env, create_antipodal_semantics, + create_curobo_motion_generator, + create_franka_panda_robot_cfg, + create_tutorial_argument_parser, + create_tutorial_robot_cfg, + create_ur5_gripper_robot_cfg, + get_hand_open_close_qpos, replay_trajectory, should_open_tutorial_window, should_wait_for_tutorial_input, @@ -46,6 +61,30 @@ Y_OFFSET = 0.18 EXPECTED_STEP_COUNT = 3 CUBOID_SIZE = (0.2, 0.2, 0.2) +FRANKA_TUTORIAL_BASE_ROTATION = (0.0, 0.0, 180.0) +DUAL_FRANKA_MOUNT_X_AXIS = torch.tensor([0.0, -1.0, 0.0]) +PGI_TUTORIAL_TCP = torch.tensor( + [ + [1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.17], + [0.0, 0.0, 0.0, 1.0], + ] +) +ATOMIC_ACTION_TUTORIAL_MODULES = ( + "assemble", + "coordinated_pickment", + "coordinated_placement", + "dynamic_obstacle_recovery", + "hand_over", + "move_end_effector", + "move_held_object", + "move_joints", + "moving_target_recovery", + "pickup", + "place", + "press", +) def _run_obstacle_animation(*, pace_wall_time: bool) -> tuple[MagicMock, MagicMock]: @@ -199,6 +238,173 @@ def test_create_antipodal_semantics_keeps_mesh_data_on_affordance() -> None: assert semantics.affordance.generator_cfg.antipodal_sampler_cfg.n_sample == 64 +def test_franka_tutorial_config_uses_ur5_gripper_component() -> None: + ur5_cfg = create_ur5_gripper_robot_cfg() + franka_cfg = create_franka_panda_robot_cfg() + + assert franka_cfg.urdf_cfg.components["hand"]["urdf_path"] == ( + ur5_cfg.urdf_cfg.components["hand"]["urdf_path"] + ) + assert franka_cfg.urdf_cfg.components["arm"]["urdf_path"].endswith( + "/Franka/Panda/Panda.urdf" + ) + assert franka_cfg.init_qpos[-2:] == [0.0, 0.0] + assert franka_cfg.init_rot == FRANKA_TUTORIAL_BASE_ROTATION + for property_name in ("stiffness", "damping", "max_effort"): + ur5_values = getattr(ur5_cfg.drive_pros, property_name) + franka_values = getattr(franka_cfg.drive_pros, property_name) + assert franka_values["gripper_finger1_joint_1"] == ( + ur5_values["gripper_finger1_joint_1"] + ) + assert "fr3_finger_joint[1-2]" not in franka_values + + +def test_ur5_and_franka_configs_share_place_binding_contract() -> None: + configs = ( + create_ur5_gripper_robot_cfg(), + create_franka_panda_robot_cfg(), + ) + + assert all(set(cfg.control_parts) == {"arm", "hand"} for cfg in configs) + assert all(set(cfg.solver_cfg) == {"arm"} for cfg in configs) + assert configs[0].control_parts["hand"] == configs[1].control_parts["hand"] + assert all( + torch.allclose(torch.as_tensor(cfg.solver_cfg["arm"].tcp), PGI_TUTORIAL_TCP) + for cfg in configs + ) + + +@pytest.mark.parametrize( + ("robot_type", "arm_dof", "solver_name"), + (("ur5", 6, "URSolverCfg"), ("franka", 7, "PytorchSolverCfg")), +) +def test_dual_tutorial_configs_share_pgi_binding_contract( + robot_type: str, + arm_dof: int, + solver_name: str, +) -> None: + single_cfg = create_tutorial_robot_cfg(robot_type) + dual_cfg = create_dual_tutorial_robot_cfg( + robot_type=robot_type, + uid=f"test_{robot_type}", + urdf_name=f"test_dual_{robot_type}", + tcp_z=0.121, + ) + expected_tcp = PGI_TUTORIAL_TCP.clone() + expected_tcp[2, 3] = 0.121 + + assert tuple(dual_cfg.urdf_cfg.components) == ( + "left_arm", + "right_arm", + "left_hand", + "right_hand", + ) + expected_arm_home = list(single_cfg.init_qpos[:arm_dof]) + assert dual_cfg.init_qpos[: 2 * arm_dof : 2] == expected_arm_home + assert dual_cfg.init_qpos[1 : 2 * arm_dof : 2] == expected_arm_home + for side in ("left", "right"): + assert len(dual_cfg.control_parts[f"{side}_arm"]) == arm_dof + assert dual_cfg.control_parts[f"{side}_hand"] == [ + f"{side}_gripper_finger1_joint_1" + ] + assert dual_cfg.urdf_cfg.components[f"{side}_hand"]["urdf_path"] == ( + single_cfg.urdf_cfg.components["hand"]["urdf_path"] + ) + solver = dual_cfg.solver_cfg[f"{side}_arm"] + assert type(solver).__name__ == solver_name + assert torch.allclose(torch.as_tensor(solver.tcp), expected_tcp) + + +def test_dual_franka_mount_preserves_single_arm_facing_direction() -> None: + cfg = create_dual_tutorial_robot_cfg( + robot_type="franka", + uid="test_franka_orientation", + urdf_name="test_dual_franka_orientation", + tcp_z=0.121, + ) + + for side in ("left", "right"): + mount = torch.as_tensor( + cfg.urdf_cfg.components[f"{side}_arm"]["transform"], + dtype=torch.float32, + ) + assert torch.allclose( + mount[:3, 0], + DUAL_FRANKA_MOUNT_X_AXIS, + atol=1e-6, + ) + + +def test_hand_commands_use_pgi_open_limit() -> None: + robot = MagicMock() + robot.device = torch.device("cpu") + robot.get_qpos_limits.return_value = torch.tensor([[[0.0, 0.04]]]) + + hand_open, hand_close = get_hand_open_close_qpos(robot) + + assert torch.allclose(hand_open, torch.tensor([0.0])) + assert torch.allclose(hand_close, torch.tensor([0.024])) + + +def test_curobo_motion_generator_factory_selects_curobo_backend() -> None: + robot = MagicMock(uid="tutorial_robot") + + with patch( + "scripts.tutorials.atomic_action.tutorial_utils.MotionGenerator" + ) as motion_generator_cls: + result = create_curobo_motion_generator(robot) + + cfg = motion_generator_cls.call_args.kwargs["cfg"] + assert result is motion_generator_cls.return_value + assert cfg.planner_cfg.planner_type == "curobo" + assert cfg.planner_cfg.robot_uid == "tutorial_robot" + + +def test_shared_robot_selection_keeps_ur5_default_and_accepts_franka() -> None: + parser = create_tutorial_argument_parser("test parser") + default_args = parser.parse_args([]) + franka_args = parser.parse_args(["--robot", "franka"]) + + assert TUTORIAL_ROBOTS == ("ur5", "franka") + assert default_args.robot == "ur5" + assert franka_args.robot == "franka" + + +def test_arm_direction_uses_selected_robot_solver_roots() -> None: + robot = MagicMock() + robot.cfg.solver_cfg = { + "left_arm": SimpleNamespace(root_link_name="left_franka_root"), + "right_arm": SimpleNamespace(root_link_name="right_franka_root"), + } + left_pose = torch.eye(4).unsqueeze(0) + right_pose = torch.eye(4).unsqueeze(0) + right_pose[0, 1, 3] = 2.0 + robot.get_link_pose.side_effect = (left_pose, right_pose) + + direction = compute_left_to_right_arm_direction(robot, "cpu") + + assert torch.allclose(direction, torch.tensor([0.0, 1.0, 0.0])) + assert robot.get_link_pose.call_args_list == [ + call(link_name="left_franka_root", env_ids=[0], to_matrix=True), + call(link_name="right_franka_root", env_ids=[0], to_matrix=True), + ] + + +@pytest.mark.parametrize("module_name", ATOMIC_ACTION_TUTORIAL_MODULES) +def test_all_atomic_action_tutorials_accept_both_robot_choices( + module_name: str, +) -> None: + module = importlib.import_module(f"scripts.tutorials.atomic_action.{module_name}") + + with patch("sys.argv", [f"{module_name}.py"]): + default_args = module.parse_arguments() + with patch("sys.argv", [f"{module_name}.py", "--robot", "franka"]): + franka_args = module.parse_arguments() + + assert default_args.robot == "ur5" + assert franka_args.robot == "franka" + + def test_replay_timed_trajectory_uses_arrival_intervals() -> None: sim = MagicMock() sim.sim_config.physics_dt = 0.1 @@ -206,7 +412,6 @@ def test_replay_timed_trajectory_uses_arrival_intervals() -> None: trajectory = TimedTrajectory.from_positions( torch.zeros(1, 3, 2), env_ids=torch.tensor([0], dtype=torch.long), - control_dt=0.1, dt=torch.tensor([[0.0, 0.2, 0.25]]), ) diff --git a/tests/sim/objects/test_articulation.py b/tests/sim/objects/test_articulation.py index 8a731a6f8..e294f3c0a 100644 --- a/tests/sim/objects/test_articulation.py +++ b/tests/sim/objects/test_articulation.py @@ -17,8 +17,10 @@ from __future__ import annotations import os -import torch +from types import SimpleNamespace + import pytest +import torch from embodichain.lab.sim import ( SimulationManager, @@ -41,6 +43,16 @@ NUM_ARENAS = 10 +def test_get_qf_returns_all_articulation_joint_efforts(): + expected_qf = torch.tensor([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]], dtype=torch.float32) + articulation = object.__new__(Articulation) + articulation._data = SimpleNamespace(qf=expected_qf) + + actual_qf = articulation.get_qf() + + assert torch.equal(actual_qf, expected_qf) + + def _link_static_friction(art: Articulation, link_name: str, env_idx: int = 0) -> float: return art._entities[env_idx].get_physical_attr(link_name).static_friction diff --git a/tests/sim/objects/test_cloth_object.py b/tests/sim/objects/test_cloth_object.py index efa1ce54d..8ddaecaa6 100644 --- a/tests/sim/objects/test_cloth_object.py +++ b/tests/sim/objects/test_cloth_object.py @@ -78,7 +78,7 @@ def setup_simulation(self): self.sim = SimulationManager(sim_cfg) # Enable manual physics update for precise control - self.n_envs = 4 + self.num_envs = 4 cloth_verts, cloth_faces = create_2d_grid_mesh( width=0.3, height=0.3, nx=12, ny=12 diff --git a/tests/sim/objects/test_robot.py b/tests/sim/objects/test_robot.py index 876781a60..e6e050f91 100644 --- a/tests/sim/objects/test_robot.py +++ b/tests/sim/objects/test_robot.py @@ -17,9 +17,11 @@ from __future__ import annotations import os -import torch -import pytest +from types import SimpleNamespace + import numpy as np +import pytest +import torch from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.sim.objects import Robot @@ -49,6 +51,20 @@ } +def test_get_qf_selects_control_part_joint_efforts(): + full_qf = torch.tensor( + [[1.0, 2.0, 3.0, 4.0], [5.0, 6.0, 7.0, 8.0]], dtype=torch.float32 + ) + robot = object.__new__(Robot) + robot._data = SimpleNamespace(qf=full_qf) + robot.cfg = SimpleNamespace(control_parts={"arm": ["joint_3", "joint_1"]}) + robot._joint_ids = {"arm": [3, 1]} + + actual_qf = robot.get_qf(name="arm") + + assert torch.equal(actual_qf, full_qf[:, [3, 1]]) + + # Base test class for CPU and CUDA class BaseRobotTest: @classmethod diff --git a/tests/sim/objects/test_soft_object.py b/tests/sim/objects/test_soft_object.py index dc785402e..d7334bb9e 100644 --- a/tests/sim/objects/test_soft_object.py +++ b/tests/sim/objects/test_soft_object.py @@ -65,7 +65,7 @@ def setup_simulation(self): assert os.path.isfile(COW_PATH) # Enable manual physics update for precise control - self.n_envs = 4 + self.num_envs = 4 # add softbody to the scene self.cow: SoftObject = self.sim.add_soft_object( diff --git a/tests/sim/planners/test_base_planner.py b/tests/sim/planners/test_base_planner.py new file mode 100644 index 000000000..0bf1d09c2 --- /dev/null +++ b/tests/sim/planners/test_base_planner.py @@ -0,0 +1,76 @@ +# ---------------------------------------------------------------------------- +# 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 + +from dataclasses import FrozenInstanceError + +import pytest + +from embodichain.lab.sim.planners.base_planner import CollisionWorldInfo + + +def test_collision_world_info_represents_one_contract() -> None: + info = CollisionWorldInfo( + entity_ids=("cube", "table"), + dynamic_entity_ids=("cube",), + batch_mode="per_env", + supports_updates=True, + ) + + assert info.entity_ids == ("cube", "table") + assert info.dynamic_entity_ids == ("cube",) + + +def test_collision_world_info_is_immutable() -> None: + info = CollisionWorldInfo() + + with pytest.raises(FrozenInstanceError): + info.supports_updates = False # type: ignore[misc] + + +@pytest.mark.parametrize( + ("entity_ids", "error_type", "match"), + [ + (("cube", "cube"), ValueError, "unique"), + ((" cube",), TypeError, "outer whitespace"), + ], +) +def test_collision_world_info_rejects_invalid_entity_ids( + entity_ids: tuple[str, ...], + error_type: type[Exception], + match: str, +) -> None: + with pytest.raises(error_type, match=match): + CollisionWorldInfo(entity_ids=entity_ids) + + +def test_collision_world_info_requires_dynamic_ids_in_complete_world() -> None: + with pytest.raises(ValueError, match="subset"): + CollisionWorldInfo( + entity_ids=("table",), + dynamic_entity_ids=("cube",), + ) + + +def test_collision_world_info_rejects_invalid_batch_mode() -> None: + with pytest.raises(ValueError, match="batch_mode"): + CollisionWorldInfo(batch_mode="batched") # type: ignore[arg-type] + + +def test_collision_world_info_requires_boolean_update_capability() -> None: + with pytest.raises(TypeError, match="supports_updates"): + CollisionWorldInfo(supports_updates=1) # type: ignore[arg-type] diff --git a/tests/sim/planners/test_curobo_planner.py b/tests/sim/planners/test_curobo_planner.py index f62563d2c..552ef6dfc 100644 --- a/tests/sim/planners/test_curobo_planner.py +++ b/tests/sim/planners/test_curobo_planner.py @@ -27,6 +27,7 @@ import importlib import logging import math +from types import SimpleNamespace import pytest import torch @@ -218,6 +219,111 @@ def test_curobo_world_cfg_uses_v2_safe_default_collision_cache(): assert cfg.obstacle_representation == "sphere" +def test_curobo_world_cfg_accepts_registered_dynamic_obstacle(): + obstacle = type("NamedObstacle", (), {"uid": "known"})() + + cfg = CuroboWorldCfg( + rigid_objects=[obstacle], + dynamic_obstacle_names=["known"], + ) + + assert cfg.dynamic_obstacle_names == ["known"] + + +def test_curobo_world_cfg_mapping_uses_registry_id_for_dynamic_obstacle(): + obstacle = type("NamedObstacle", (), {"uid": "legacy_uid"})() + + cfg = CuroboWorldCfg( + rigid_objects={"registry_cube": obstacle}, + dynamic_obstacle_names=["registry_cube"], + ) + + assert cfg.dynamic_obstacle_names == ["registry_cube"] + assert cfg.rigid_objects["registry_cube"] is obstacle + + +def test_curobo_world_cfg_mapping_does_not_accept_object_uid_as_alias(): + obstacle = type("NamedObstacle", (), {"uid": "legacy_uid"})() + + with pytest.raises(ValueError, match="not present in rigid_objects"): + CuroboWorldCfg( + rigid_objects={"registry_cube": obstacle}, + dynamic_obstacle_names=["legacy_uid"], + ) + + +def test_curobo_world_cfg_rejects_unregistered_dynamic_obstacle(): + obstacle = type("NamedObstacle", (), {"uid": "known"})() + + with pytest.raises(ValueError, match="not present in rigid_objects"): + CuroboWorldCfg( + rigid_objects=[obstacle], + dynamic_obstacle_names=["unknown"], + ) + + +def test_curobo_world_cfg_rejects_duplicate_dynamic_obstacle_names(): + obstacle = type("NamedObstacle", (), {"uid": "known"})() + + with pytest.raises(ValueError, match="unique non-empty"): + CuroboWorldCfg( + rigid_objects=[obstacle], + dynamic_obstacle_names=["known", "known"], + ) + + +def test_curobo_world_cfg_rejects_outer_whitespace_in_obstacle_ids(): + obstacle = type("NamedObstacle", (), {"uid": "known"})() + + with pytest.raises(ValueError, match="without outer whitespace"): + CuroboWorldCfg( + rigid_objects={"registry_cube": obstacle}, + dynamic_obstacle_names=[" registry_cube"], + ) + with pytest.raises(ValueError, match="without outer whitespace"): + CuroboWorldCfg(rigid_objects={" registry_cube": obstacle}) + + +def test_curobo_world_cfg_rejects_string_dynamic_obstacle_collection(): + obstacle = type("NamedObstacle", (), {"uid": "known"})() + + with pytest.raises(TypeError, match="not a string"): + CuroboWorldCfg( + rigid_objects={"registry_cube": obstacle}, + dynamic_obstacle_names="registry_cube", # type: ignore[arg-type] + ) + + +def test_curobo_world_cfg_rejects_duplicate_rigid_object_names(): + obstacle_type = type("NamedObstacle", (), {"uid": "duplicate"}) + + with pytest.raises(ValueError, match="unique obstacle names"): + CuroboWorldCfg(rigid_objects=[obstacle_type(), obstacle_type()]) + + +@pytest.mark.parametrize( + ("multi_env", "expected_mode"), + [(False, "shared"), (True, "per_env")], +) +def test_curobo_planner_exposes_collision_world_contract(multi_env, expected_mode): + planner = object.__new__(CuroboPlanner) + planner.cfg = CuroboPlannerCfg( + robot_uid="robot", + world=CuroboWorldCfg( + rigid_objects={"registry_cube": object()}, + dynamic_obstacle_names=["registry_cube"], + multi_env=multi_env, + ), + ) + + info = planner.collision_world_info + + assert info.dynamic_entity_ids == ("registry_cube",) + assert info.entity_ids == ("registry_cube",) + assert info.batch_mode == expected_mode + assert info.supports_updates is True + + def test_curobo_collision_world_binding_merges_owned_obstacle_poses(): planner = object.__new__(CuroboPlanner) configured_pose = torch.eye(4).unsqueeze(0) @@ -513,6 +619,156 @@ def test_generate_cuboid_world_yaml_assembles_schema(tmp_path): assert data["cuboid"]["demo_block"]["pose"][:3] == pytest.approx([0.45, 0.0, 0.18]) +def test_generate_world_yaml_uses_mapping_key_instead_of_object_uid(tmp_path): + rigid_object = _FakeRigidObject( + "legacy_uid", + _unit_cube_vertices(), + _cube_faces(), + _identity_pose(), + ) + output_path = tmp_path / "registry_world.yml" + + generate_curobo_world_yaml( + {"registry_cube": rigid_object}, + str(output_path), + representation="cuboid", + ) + data = yaml.safe_load(output_path.read_text(encoding="utf-8")) + + assert set(data["cuboid"]) == {"registry_cube"} + + +def test_world_yaml_cache_key_includes_registry_id(): + rigid_object = _FakeRigidObject( + "legacy_uid", + _unit_cube_vertices(), + _cube_faces(), + _identity_pose(), + ) + planner = object.__new__(CuroboPlanner) + planner.cfg = CuroboPlannerCfg( + robot_uid="robot", + world=CuroboWorldCfg(rigid_objects={"registry_cube": rigid_object}), + ) + registry_key = planner._world_yaml_cache_key(planner.cfg.world) + planner.cfg.world = CuroboWorldCfg( + rigid_objects={"renamed_registry_cube": rigid_object} + ) + + renamed_key = planner._world_yaml_cache_key(planner.cfg.world) + + assert registry_key != renamed_key + + +def test_dynamic_update_uses_registry_id_in_curobo_backend(): + rigid_object = _FakeRigidObject( + "legacy_uid", + _unit_cube_vertices(), + _cube_faces(), + _identity_pose(), + ) + planner = object.__new__(CuroboPlanner) + planner.cfg = CuroboPlannerCfg( + robot_uid="robot", + world=CuroboWorldCfg( + rigid_objects={"registry_cube": rigid_object}, + obstacle_representation="cuboid", + dynamic_obstacle_names=["registry_cube"], + ), + ) + planner._curobo_device = torch.device("cpu") + planner._bindings = SimpleNamespace(Pose=lambda **kwargs: kwargs) + updates = [] + collision_checker = SimpleNamespace( + update_obstacle_pose=lambda name, pose, env_idx: updates.append( + (name, pose, env_idx) + ) + ) + backend = SimpleNamespace( + batch_size=1, + profile=SimpleNamespace(sim_base_to_curobo_base=None), + sim_base_to_curobo_base_matrix=None, + planner=SimpleNamespace(scene_collision_checker=collision_checker), + ) + identity = torch.eye(4).unsqueeze(0) + + planner.update_dynamic_obstacles( + {"registry_cube": identity}, + backend=backend, + sim_base_pose_inv=identity, + ) + + assert [(name, env_idx) for name, _, env_idx in updates] == [("registry_cube", 0)] + + +def test_validate_joint_trajectory_checks_every_exact_sample_in_curobo_order(): + """The collision gate preserves samples and maps simulator joint order.""" + planner = object.__new__(CuroboPlanner) + planner.cfg = CuroboPlannerCfg( + robot_uid="robot", + world=CuroboWorldCfg(multi_env=True), + ) + planner._curobo_device = torch.device("cpu") + joint_states = [] + collision_queries = [] + + def from_position(position, *, joint_names): + joint_states.append((position.clone(), tuple(joint_names))) + return SimpleNamespace(position=position) + + def validate(sample, *, env_query_idx): + collision_queries.append((sample.clone(), env_query_idx.clone())) + validity = torch.ones(sample.shape[:2], dtype=torch.bool) + if len(collision_queries) == 2: + validity[1, 0] = False + return validity + + planner._bindings = SimpleNamespace( + JointState=SimpleNamespace(from_position=from_position), + ) + backend = SimpleNamespace( + sim_joint_names=["sim_left", "sim_right"], + sim_to_curobo_col_idx=None, + collision_checker=SimpleNamespace(validate=validate), + profile=SimpleNamespace( + sim_to_curobo_joint_names={ + "sim_left": "curobo_left", + "sim_right": "curobo_right", + }, + ), + planner=SimpleNamespace( + joint_names=["curobo_right", "curobo_left"], + ), + ) + planner._get_backend = lambda control_part, batch_size, move_type: backend + trajectory = torch.tensor( + ( + ((0.0, 1.0), (0.1, 1.1), (0.2, 1.2)), + ((2.0, 3.0), (2.1, 3.1), (2.2, 3.2)), + ), + dtype=torch.float32, + ) + + validity = planner.validate_joint_trajectory( + trajectory, + control_part="dual_arm", + ) + + assert torch.equal( + validity, + torch.tensor(((True, True, True), (True, False, True))), + ) + assert len(collision_queries) == trajectory.shape[1] + for sample_index, (sample, env_query_idx) in enumerate(collision_queries): + torch.testing.assert_close( + sample[:, 0], + trajectory[:, sample_index].flip(dims=(-1,)), + ) + assert torch.equal(env_query_idx, torch.tensor((0, 1), dtype=torch.int32)) + torch.testing.assert_close(joint_states[0][0], trajectory[:, 0].flip(dims=(-1,))) + assert joint_states[0][1] == ("curobo_right", "curobo_left") + + def test_generate_mesh_world_yaml_assembles_schema(tmp_path): rigid_object = _FakeRigidObject( "demo_block", @@ -566,6 +822,21 @@ def test_generate_world_yaml_rejects_empty_input(tmp_path): generate_curobo_world_yaml([], str(tmp_path / "world.yml")) +def test_registry_world_yaml_rejects_empty_geometry_instead_of_skipping(tmp_path): + rigid_object = _FakeRigidObject( + "legacy_uid", + torch.zeros((0, 3), dtype=torch.float32), + torch.zeros((0, 3), dtype=torch.int64), + _identity_pose(), + ) + + with pytest.raises(ValueError, match="Registry-backed obstacle.*no mesh"): + generate_curobo_world_yaml( + {"registry_cube": rigid_object}, + str(tmp_path / "world.yml"), + ) + + def test_generate_world_yaml_rejects_duplicate_names(tmp_path): pose = _identity_pose() first = _FakeRigidObject( @@ -588,6 +859,21 @@ def test_generate_world_yaml_rejects_duplicate_names(tmp_path): ) +def test_generate_world_yaml_rejects_outer_whitespace_in_mapping_id(tmp_path): + rigid_object = _FakeRigidObject( + "legacy_uid", + _unit_cube_vertices(), + _cube_faces(), + _identity_pose(), + ) + + with pytest.raises(ValueError, match="without outer whitespace"): + generate_curobo_world_yaml( + {" registry_cube": rigid_object}, + str(tmp_path / "world.yml"), + ) + + def test_generated_cuboid_yaml_loads_in_curobo_scene_cfg(tmp_path): pytest.importorskip("curobo") from curobo._src.geom.types import SceneCfg @@ -713,7 +999,6 @@ def _make_curobo_engine( def test_curobo_reuses_non_graph_backend(): from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, EndEffectorPoseGoal, MotionPolicy, @@ -724,13 +1009,17 @@ def test_curobo_reuses_non_graph_backend(): try: engine = _make_curobo_engine(block) target = _target_beyond_block(robot) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": _SIM_CONTROL_PART}}, + ) result = engine.compile( ( ActionInvocation( "move_end_effector", EndEffectorPoseGoal(xpos=target), - ActionBinding(manipulators={"primary": _SIM_CONTROL_PART}), + binding, MotionPolicy(strategy="motion_gen", sample_count=80), ), ) @@ -749,7 +1038,7 @@ def test_curobo_reuses_non_graph_backend(): ActionInvocation( "move_end_effector", EndEffectorPoseGoal(xpos=target), - ActionBinding(manipulators={"primary": _SIM_CONTROL_PART}), + binding, MotionPolicy(strategy="motion_gen", sample_count=80), ), ) @@ -767,7 +1056,6 @@ def test_curobo_reuses_non_graph_backend(): def test_curobo_uses_accelerator_with_cpu_physics(): from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, EndEffectorPoseGoal, MotionPolicy, @@ -778,13 +1066,17 @@ def test_curobo_uses_accelerator_with_cpu_physics(): try: engine = _make_curobo_engine(block, use_cuda_graph=True) target = _target_beyond_block(robot) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": _SIM_CONTROL_PART}}, + ) result = engine.compile( ( ActionInvocation( "move_end_effector", EndEffectorPoseGoal(xpos=target), - ActionBinding(manipulators={"primary": _SIM_CONTROL_PART}), + binding, MotionPolicy(strategy="motion_gen", sample_count=80), ), ) diff --git a/tests/sim/planners/test_motion_generator_batched.py b/tests/sim/planners/test_motion_generator_batched.py index a2adbdb49..4b84350d9 100644 --- a/tests/sim/planners/test_motion_generator_batched.py +++ b/tests/sim/planners/test_motion_generator_batched.py @@ -16,20 +16,56 @@ from __future__ import annotations -import torch -import pytest +from typing import Literal from unittest.mock import Mock, patch +import pytest +import torch + +from embodichain.lab.sim.planners.base_planner import CollisionWorldInfo, PlanOptions from embodichain.lab.sim.planners.motion_generator import ( MotionGenerator, MotionGenOptions, ) -from embodichain.lab.sim.planners.base_planner import PlanOptions from embodichain.lab.sim.planners.utils import PlanState, PlanResult, MoveType BATCH_SIZE = 2 CONTROLLED_DOF = 6 SAMPLE_COUNT = 8 +STEP_DT = 0.05 + + +def _collision_world_info( + dynamic_entity_ids: tuple[str, ...] = (), + *, + entity_ids: tuple[str, ...] | None = None, + batch_mode: Literal["shared", "per_env"] | None = "shared", + supports_updates: bool = True, +) -> CollisionWorldInfo: + """Build a valid collision-world contract for planner test doubles.""" + return CollisionWorldInfo( + entity_ids=dynamic_entity_ids if entity_ids is None else entity_ids, + dynamic_entity_ids=dynamic_entity_ids, + batch_mode=batch_mode, + supports_updates=supports_updates, + ) + + +def _timed_result( + positions: torch.Tensor, + *, + success: bool | torch.Tensor = True, + step_dt: float = STEP_DT, +) -> PlanResult: + """Build a planner result that satisfies the explicit timing contract.""" + dt = torch.zeros(positions.shape[:2], device=positions.device) + if positions.shape[1] > 1: + dt[:, 1:] = step_dt + return PlanResult( + success=success, + positions=positions, + dt=dt, + ) @pytest.fixture(autouse=True) @@ -81,9 +117,9 @@ def with_motion_context(self, options, *, start_qpos, control_part): def plan(self, target_states, options): self.target_states = target_states - return PlanResult( + return _timed_result( + torch.zeros(1, 3, 2), success=torch.tensor([True]), - positions=torch.zeros(1, 3, 2), ) @@ -128,7 +164,7 @@ def test_direct_cartesian_planner_requires_joint_fallback_inputs(): def test_bind_collision_world_copies_caller_options() -> None: planner = Mock() - planner.supports_collision_world_updates = True + planner.collision_world_info = _collision_world_info(("obstacle",)) original = PlanOptions() obstacle_pose = torch.eye(4).unsqueeze(0) @@ -152,9 +188,136 @@ def bind(options, *, obstacle_poses): planner.with_collision_world.assert_called_once() +@pytest.mark.parametrize( + ("configured_ids", "obstacle_poses", "expected"), + [ + (("cube", "tray"), {"cube": torch.eye(4).unsqueeze(0)}, "missing"), + ( + ("cube",), + { + "cube": torch.eye(4).unsqueeze(0), + "tray": torch.eye(4).unsqueeze(0), + }, + "extra", + ), + ], +) +def test_bind_collision_world_requires_exact_planner_entity_ids( + configured_ids, obstacle_poses, expected +) -> None: + planner = Mock() + planner.collision_world_info = _collision_world_info(configured_ids) + generator = object.__new__(MotionGenerator) + generator.planner = planner + + with pytest.raises(ValueError, match=expected): + generator.bind_collision_world(None, obstacle_poses=obstacle_poses) + + planner.with_collision_world.assert_not_called() + + +def test_bind_collision_world_rejects_extra_ids_in_caller_options() -> None: + planner = Mock() + planner.collision_world_info = _collision_world_info(("cube",)) + generator = object.__new__(MotionGenerator) + generator.planner = planner + options = PlanOptions() + options.dynamic_obstacle_poses = {"legacy_cube": torch.eye(4).unsqueeze(0)} + + with pytest.raises(ValueError, match="Caller planning options.*legacy_cube"): + generator.bind_collision_world( + options, + obstacle_poses={"cube": torch.eye(4).unsqueeze(0)}, + ) + + planner.with_collision_world.assert_not_called() + + +def test_bind_collision_world_rejects_ids_injected_by_backend() -> None: + planner = Mock() + planner.collision_world_info = _collision_world_info(("cube",)) + + def bind(options, *, obstacle_poses): + options.dynamic_obstacle_poses = { + **obstacle_poses, + "legacy_cube": torch.eye(4).unsqueeze(0), + } + return options + + planner.with_collision_world.side_effect = bind + generator = object.__new__(MotionGenerator) + generator.planner = planner + + with pytest.raises(ValueError, match="Bound dynamic collision.*legacy_cube"): + generator.bind_collision_world( + PlanOptions(), + obstacle_poses={"cube": torch.eye(4).unsqueeze(0)}, + ) + + +def test_bind_collision_world_allows_none_for_empty_configured_world() -> None: + planner = Mock() + planner.collision_world_info = _collision_world_info() + planner.default_plan_options.return_value = PlanOptions() + + def bind(options, *, obstacle_poses): + assert obstacle_poses == {} + options.dynamic_obstacle_poses = None + return options + + planner.with_collision_world.side_effect = bind + generator = object.__new__(MotionGenerator) + generator.planner = planner + + bound = generator.bind_collision_world(None, obstacle_poses={}) + + assert bound.dynamic_obstacle_poses is None + + +def test_bind_collision_world_rejects_non_string_option_keys() -> None: + planner = Mock() + planner.collision_world_info = _collision_world_info() + generator = object.__new__(MotionGenerator) + generator.planner = planner + options = PlanOptions() + options.dynamic_obstacle_poses = {1: torch.eye(4).unsqueeze(0)} + + with pytest.raises(TypeError, match="keys must be non-empty strings"): + generator.bind_collision_world(options, obstacle_poses={}) + + planner.with_collision_world.assert_not_called() + + +def test_motion_generator_exposes_collision_integration_metadata() -> None: + planner = Mock() + info = _collision_world_info( + ("cube", "tray"), + entity_ids=("cube", "tray", "table"), + batch_mode="per_env", + ) + planner.collision_world_info = info + generator = object.__new__(MotionGenerator) + generator.planner = planner + + assert generator.collision_world_info is info + assert generator.dynamic_collision_entity_ids == ("cube", "tray") + assert generator.collision_world_entity_ids == ("cube", "tray", "table") + assert generator.collision_world_batch_mode == "per_env" + + +def test_motion_generator_rejects_invalid_collision_world_contract() -> None: + planner = Mock() + planner.collision_world_info = object() + generator = object.__new__(MotionGenerator) + generator.planner = planner + + with pytest.raises(TypeError, match="CollisionWorldInfo"): + _ = generator.collision_world_info + + def test_bind_collision_world_rejects_unsupported_planner() -> None: planner = Mock() - planner.supports_collision_world_updates = False + planner.collision_world_info = None generator = object.__new__(MotionGenerator) generator.planner = planner @@ -170,7 +333,7 @@ def test_bind_collision_world_rejects_unsupported_planner() -> None: def test_bind_collision_world_uses_backend_default_options() -> None: planner = Mock() - planner.supports_collision_world_updates = True + planner.collision_world_info = _collision_world_info(("obstacle",)) defaults = PlanOptions() planner.default_plan_options.return_value = defaults planner.with_collision_world.return_value = defaults @@ -195,9 +358,9 @@ def _mock_planner(b=3, n=15, dofs=6): ) planner.robot.num_instances = b planner.robot.device = torch.device("cpu") - planner.plan.return_value = PlanResult( + planner.plan.return_value = _timed_result( + torch.zeros(b, n, dofs), success=torch.ones(b, dtype=torch.bool), - positions=torch.zeros(b, n, dofs), ) planner.preserve_plan_samples = False planner.default_plan_options.return_value = PlanOptions() @@ -236,9 +399,9 @@ def _mock_generator( planner.with_motion_context.side_effect = ( lambda options, *, start_qpos, control_part: options ) - planner.plan.return_value = result or PlanResult( + planner.plan.return_value = result or _timed_result( + torch.zeros(batch_size, 5, controlled_dof), success=torch.ones(batch_size, dtype=torch.bool), - positions=torch.zeros(batch_size, 5, controlled_dof), ) generator = object.__new__(MotionGenerator) generator.planner = planner @@ -311,6 +474,21 @@ def test_options_accept_only_declared_strategy_values(self): assert MotionGenOptions(strategy="ik_interp").strategy == "ik_interp" with pytest.raises(ValueError, match="strategy"): MotionGenOptions(strategy="planner") # type: ignore[arg-type] + with pytest.raises(ValueError, match="interpolation_dt"): + MotionGenOptions(interpolation_dt=0.0) + + def test_ik_interp_rejects_missing_timing(self): + generator = _mock_generator() + with pytest.raises(ValueError, match="explicit interpolation_dt"): + generator.generate( + [PlanState.from_qpos(torch.ones(BATCH_SIZE, CONTROLLED_DOF))], + MotionGenOptions( + strategy="ik_interp", + sample_count=SAMPLE_COUNT, + start_qpos=torch.zeros(BATCH_SIZE, CONTROLLED_DOF), + control_part="arm", + ), + ) def test_ik_interp_solves_batched_poses_without_calling_backend(self): generator = _mock_generator() @@ -329,6 +507,7 @@ def test_ik_interp_solves_batched_poses_without_calling_backend(self): sample_count=SAMPLE_COUNT, start_qpos=start, control_part="arm", + interpolation_dt=STEP_DT, ), ) @@ -346,12 +525,57 @@ def test_ik_interp_solves_batched_poses_without_calling_backend(self): ) generator.planner.plan.assert_not_called() + def test_linear_cartesian_motion_grounds_every_output_sample_with_ik(self): + generator = _mock_generator() + + def encode_position( + pose: torch.Tensor, + name: str, + joint_seed: torch.Tensor, + ) -> tuple[torch.Tensor, torch.Tensor]: + qpos = joint_seed.clone() + qpos[:, :3] = pose[:, :3, 3] + return torch.ones(BATCH_SIZE, dtype=torch.bool), qpos + + generator.robot.compute_ik.side_effect = encode_position + weights = torch.linspace(1.0 / (SAMPLE_COUNT - 1), 1.0, SAMPLE_COUNT - 1) + targets = [] + for weight in weights: + pose = torch.eye(4).repeat(BATCH_SIZE, 1, 1) + pose[:, 0, 3] = weight + targets.append(PlanState.from_xpos(pose)) + + result = generator.generate( + targets, + MotionGenOptions( + strategy="motion_gen", + sample_count=SAMPLE_COUNT, + start_qpos=torch.zeros(BATCH_SIZE, CONTROLLED_DOF), + control_part="arm", + is_linear=True, + interpolation_dt=STEP_DT, + preserve_cartesian_samples=True, + ), + ) + + assert result.positions is not None + assert result.positions.shape == ( + BATCH_SIZE, + SAMPLE_COUNT, + CONTROLLED_DOF, + ) + expected_x = torch.linspace(0.0, 1.0, SAMPLE_COUNT) + assert torch.allclose( + result.positions[:, :, 0], expected_x.expand(BATCH_SIZE, -1) + ) + assert generator.robot.compute_ik.call_count == SAMPLE_COUNT - 1 + generator.planner.plan.assert_not_called() + def test_motion_gen_delegates_and_resamples_backend_result(self): raw_sample_count = 5 generator = _mock_generator( - result=PlanResult( - success=True, - positions=torch.zeros( + result=_timed_result( + torch.zeros( BATCH_SIZE, raw_sample_count, CONTROLLED_DOF, @@ -376,15 +600,20 @@ def test_motion_gen_delegates_and_resamples_backend_result(self): SAMPLE_COUNT, CONTROLLED_DOF, ) + assert result.dt is not None + assert result.duration is not None + assert result.dt.shape == (BATCH_SIZE, SAMPLE_COUNT) + assert result.duration.tolist() == pytest.approx( + [STEP_DT * (raw_sample_count - 1)] * BATCH_SIZE + ) generator.planner.plan.assert_called_once() def test_motion_gen_preserves_backend_samples_when_required(self): raw_sample_count = 5 generator = _mock_generator( preserve_plan_samples=True, - result=PlanResult( - success=True, - positions=torch.zeros( + result=_timed_result( + torch.zeros( BATCH_SIZE, raw_sample_count, CONTROLLED_DOF, @@ -417,6 +646,7 @@ def test_joint_target_falls_back_when_backend_has_no_joint_capability(self): sample_count=SAMPLE_COUNT, start_qpos=start, control_part="arm", + interpolation_dt=STEP_DT, ), ) @@ -455,9 +685,7 @@ class TestNormalizedPlanResult: def test_non_finite_positions_are_rejected(self): positions = torch.zeros(BATCH_SIZE, 5, CONTROLLED_DOF) positions[0, 0, 0] = float("nan") - generator = _mock_generator( - result=PlanResult(success=True, positions=positions) - ) + generator = _mock_generator(result=_timed_result(positions)) with pytest.raises(ValueError, match="non-finite"): generator.generate( @@ -489,9 +717,9 @@ def test_failed_rows_hold_start_qpos(self): positions = torch.zeros(BATCH_SIZE, 5, CONTROLLED_DOF) positions[1] = 1.0 generator = _mock_generator( - result=PlanResult( + result=_timed_result( + positions, success=torch.tensor([True, False]), - positions=positions, ) ) start = torch.zeros(BATCH_SIZE, CONTROLLED_DOF) diff --git a/tests/sim/planners/test_plan_state_batched.py b/tests/sim/planners/test_plan_state_batched.py index a48e8e2a0..fc195655b 100644 --- a/tests/sim/planners/test_plan_state_batched.py +++ b/tests/sim/planners/test_plan_state_batched.py @@ -51,17 +51,24 @@ def test_is_all_success_scalar(self): assert r.is_all_success() is True def test_batched_shapes(self): + dt = torch.zeros(2, 10) + dt[:, 1:] = 0.1 r = PlanResult( success=torch.tensor([True, False]), positions=torch.zeros(2, 10, 7), velocities=torch.zeros(2, 10, 7), accelerations=torch.zeros(2, 10, 7), - dt=torch.zeros(2, 10), - duration=torch.tensor([1.0, 0.0]), + dt=dt, ) assert r.positions.shape == (2, 10, 7) assert r.dt.shape == (2, 10) assert r.duration.shape == (2,) + assert torch.equal(r.duration, dt.sum(dim=1)) + + def test_positions_require_explicit_timing(self): + positions = torch.zeros(2, 3, 7) + with pytest.raises(ValueError, match="explicit dt"): + PlanResult(success=True, positions=positions) class TestValidateBatchConsistency: diff --git a/tests/sim/planners/test_toppra_batched.py b/tests/sim/planners/test_toppra_batched.py index f9d331770..a769327ce 100644 --- a/tests/sim/planners/test_toppra_batched.py +++ b/tests/sim/planners/test_toppra_batched.py @@ -85,7 +85,7 @@ def test_solve_one_env_same_waypoint_shortcut(self): ) assert out["success"] is True assert out["n"] == 2 - assert out["duration"] == 0.0 + assert out["dt"].sum() == 0.0 def test_solve_one_env_duplicate_plateau(self): # Long plateaus of identical waypoints (e.g. from interpolating a diff --git a/tests/sim/skills/__init__.py b/tests/sim/skills/__init__.py new file mode 100644 index 000000000..8dc25c19d --- /dev/null +++ b/tests/sim/skills/__init__.py @@ -0,0 +1,19 @@ +# ---------------------------------------------------------------------------- +# 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 semantic-skill integration contracts.""" + +from __future__ import annotations diff --git a/tests/sim/skills/test_articulation_semantics.py b/tests/sim/skills/test_articulation_semantics.py new file mode 100644 index 000000000..ec8910bdc --- /dev/null +++ b/tests/sim/skills/test_articulation_semantics.py @@ -0,0 +1,602 @@ +# ---------------------------------------------------------------------------- +# 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 first-class semantic articulation operations.""" + +from __future__ import annotations + +from unittest.mock import Mock + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + Affordance, + ArticulationOperationAffordance, + ArticulationOperationTarget, + AtomicActionEngine, + CARTESIAN_POSE_CAPABILITY, + ControlPartCommandProfile, + EntityState, + GRASP_CAPABILITY, + JOINT_POSITION_CAPABILITY, + ObservedArticulationJointState, + OperateArticulationGoal, + OperateArticulationOptions, + PlanningContext, + RobotObservation, + SceneSnapshot, + TaskState, +) +from embodichain.lab.sim.skills.calls import ( + OperateArticulation, + builtin_semantic_call_catalog, +) +from embodichain.lab.sim.skills.compiler import SemanticSkillCompiler +from embodichain.lab.sim.skills.effects import ( + ArticulationJointStateExpectation, + JointStateEffectClause, + SemanticEffectKind, + SymbolicStateKey, +) +from embodichain.lab.sim.skills.integration import ( + SceneManifest, + SemanticIntegrationManifest, + SemanticValidationError, +) +from embodichain.lab.sim.skills.profiles import ( + ControlPartEndpoint, + RobotResource, + RobotSkillProfile, + SkillPolicyPreset, +) +from embodichain.lab.sim.skills.scene import ( + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, + ArticulationJointEvidenceAddress, + SceneAffordanceRef, + SceneArticulationRef, + SceneEntityRegistration, + SceneRegistry, +) + +_BATCH_SIZE = 2 +_TARGET_POSITION = 0.42 +_TARGET_DISPLACEMENT = 0.4 +_POSITION_SCALE = 0.5 + + +class _MutablePoseProvider: + """Expose a mutable pose and count semantic observation calls.""" + + def __init__( + self, + pose: torch.Tensor, + *, + joint_position: torch.Tensor | None = None, + ) -> None: + self.pose = pose.clone() + self.joint_position = None if joint_position is None else joint_position.clone() + self.calls = 0 + self.joint_calls = 0 + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp, env_ids + self.calls += 1 + return EntityState(self.pose) + + def observe_joints( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> dict[str, ObservedArticulationJointState]: + del timestamp, env_ids + self.joint_calls += 1 + if self.joint_position is None: + raise RuntimeError("This provider has no articulation joint fixture.") + return {"drawer_slide": ObservedArticulationJointState(self.joint_position)} + + +def _translated_offset(x: float, y: float, z: float) -> torch.Tensor: + """Build one test-only proper local offset.""" + pose = torch.eye(4, dtype=torch.float32) + pose[:3, 3] = torch.tensor((x, y, z), dtype=torch.float32) + return pose + + +def _operation_affordance() -> ArticulationOperationAffordance: + """Build the canonical drawer-handle fixture.""" + return ArticulationOperationAffordance( + joint_id="drawer_slide", + approach_offset=_translated_offset(0.0, 0.0, -0.1), + contact_offset=torch.eye(4), + operation_offset=_translated_offset(0.0, 0.02, 0.0), + retract_offset=_translated_offset(0.0, 0.0, -0.1), + operation_axis=torch.tensor((1.0, 0.0, 0.0)), + position_scale=_POSITION_SCALE, + semantic_targets={ + "open": ArticulationOperationTarget( + target_position=_TARGET_POSITION, + displacement=_TARGET_DISPLACEMENT, + ) + }, + ) + + +def _registry() -> tuple[SceneRegistry, _MutablePoseProvider, _MutablePoseProvider]: + """Build an articulation plus one directly registered handle affordance.""" + articulation = SceneArticulationRef("drawer") + handle = SceneAffordanceRef("drawer_handle") + articulation_provider = _MutablePoseProvider( + torch.eye(4).repeat(_BATCH_SIZE, 1, 1), + joint_position=torch.zeros(_BATCH_SIZE, 1), + ) + handle_pose = torch.eye(4).repeat(_BATCH_SIZE, 1, 1) + handle_pose[:, 0, 3] = 0.3 + handle_provider = _MutablePoseProvider(handle_pose) + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=articulation, + state_provider=articulation_provider, + joint_state_provider=articulation_provider, + semantic_type="drawer", + default_affordances={ + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY: handle + }, + ), + SceneEntityRegistration( + ref=handle, + state_provider=handle_provider, + parent=articulation, + native_name="handle", + affordance=_operation_affordance(), + affordance_capabilities=frozenset( + {ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY} + ), + affordance_revision="drawer-handle-v1", + ), + ) + ) + return registry, articulation_provider, handle_provider + + +def _profile() -> RobotSkillProfile: + """Build one resource satisfying motion and interaction endpoints.""" + return RobotSkillProfile( + profile_id="articulation_test_robot", + resources={ + "manipulator": RobotResource( + resource_id="manipulator", + endpoints={ + "motion": ControlPartEndpoint( + control_part="arm", + capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + JOINT_POSITION_CAPABILITY, + } + ), + ), + "interaction": ControlPartEndpoint( + control_part="hand", + capabilities=frozenset({GRASP_CAPABILITY}), + ), + }, + ) + }, + command_profiles={ + "hand": ControlPartCommandProfile.joint_positions( + open=torch.tensor((0.0,)), + grasp=torch.tensor((1.0,)), + ) + }, + presets={ + "safe": SkillPolicyPreset( + "safe", + action_option_templates={ + "operate_articulation": OperateArticulationOptions(), + }, + ) + }, + default_preset="safe", + ) + + +def _engine(profile: RobotSkillProfile) -> AtomicActionEngine: + """Construct a CPU-only engine with the minimum typed robot surface.""" + robot = Mock() + robot.device = torch.device("cpu") + robot.dof = 2 + robot.control_parts = {"arm": object(), "hand": object()} + robot.get_qpos.return_value = torch.zeros(_BATCH_SIZE, robot.dof) + robot.get_qvel.return_value = torch.zeros(_BATCH_SIZE, robot.dof) + robot.get_joint_ids.side_effect = lambda name: {"arm": [0], "hand": [1]}[name] + robot.get_solver.return_value = object() + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "stub_planner" + return AtomicActionEngine(generator, skill_profile=profile) + + +def _compiler(registry: SceneRegistry) -> SemanticSkillCompiler: + """Bind the curated semantic catalog to the test scene and profile.""" + profile = _profile() + engine = _engine(profile) + manifest = SemanticIntegrationManifest( + scene=SceneManifest.from_registry(registry), + robot_profile=profile, + call_catalog=builtin_semantic_call_catalog(), + ) + return SemanticSkillCompiler(manifest.bind(registry, engine)) + + +def _context(scene: SceneSnapshot, *, timestamp: float) -> PlanningContext: + """Build one immutable planning observation around a supplied scene.""" + env_ids = torch.arange(_BATCH_SIZE, dtype=torch.long) + return PlanningContext( + robot=RobotObservation( + timestamp=timestamp, + qpos=torch.zeros(_BATCH_SIZE, 2), + qvel=torch.zeros(_BATCH_SIZE, 2), + ), + task=TaskState.empty(_BATCH_SIZE, "cpu"), + scene=scene, + env_ids=env_ids, + ) + + +def test_articulation_affordance_owns_configuration_and_grounds_geometry() -> None: + axis = torch.tensor((2.0, 0.0, 0.0)) + operation_offset = _translated_offset(0.0, 0.02, 0.0) + targets = { + "open": ArticulationOperationTarget( + _TARGET_POSITION, + _TARGET_DISPLACEMENT, + ) + } + affordance = ArticulationOperationAffordance( + joint_id="drawer_slide", + operation_axis=axis, + operation_offset=operation_offset, + position_scale=_POSITION_SCALE, + semantic_targets=targets, + ) + axis.zero_() + operation_offset.zero_() + targets.clear() + + handle = torch.eye(4).repeat(_BATCH_SIZE, 1, 1) + handle[:, 0, 3] = 0.3 + _, _, operation, _ = affordance.ground_poses( + handle, + displacement=_TARGET_DISPLACEMENT, + ) + + assert tuple(affordance.semantic_targets) == ("open",) + assert torch.allclose(affordance.operation_axis, torch.tensor((1.0, 0.0, 0.0))) + assert torch.allclose( + operation[:, :3, 3], + torch.tensor((0.5, 0.02, 0.0)).repeat(_BATCH_SIZE, 1), + ) + + +def test_registry_returns_owned_articulation_affordance_snapshots() -> None: + registry, _, _ = _registry() + + first = registry.lookup( + SceneAffordanceRef("drawer_handle"), + expected_type=SceneAffordanceRef, + ).affordance + second = registry.lookup( + SceneAffordanceRef("drawer_handle"), + expected_type=SceneAffordanceRef, + ).affordance + + assert type(first) is ArticulationOperationAffordance + assert type(second) is ArticulationOperationAffordance + assert first is not second + first.operation_offset[0, 3] = 99.0 + assert second.operation_offset[0, 3].item() == 0.0 + + +@pytest.mark.parametrize( + "kwargs", + ( + {}, + {"target_position": _TARGET_POSITION}, + {"target_displacement": _TARGET_DISPLACEMENT}, + { + "target": "open", + "target_position": _TARGET_POSITION, + "target_displacement": _TARGET_DISPLACEMENT, + }, + ), +) +def test_articulation_call_requires_exactly_one_complete_target(kwargs: dict) -> None: + with pytest.raises(ValueError): + OperateArticulation( + articulation=SceneArticulationRef("drawer"), + **kwargs, + ) + + +def test_static_link_selects_default_without_observing_scene() -> None: + registry, articulation_provider, handle_provider = _registry() + compiler = _compiler(registry) + + workflow = compiler.analyze( + ( + OperateArticulation( + articulation=SceneArticulationRef("drawer"), + target="open", + ), + ) + ) + + analyzed = workflow.calls[0] + assert analyzed.effect_kind is SemanticEffectKind.ARTICULATION + assert analyzed.bound.linked.affordances["handle"] == SceneAffordanceRef( + "drawer_handle" + ) + assert analyzed.bound.linked.descriptor.skill_id == "operate_articulation" + assert analyzed.symbolic_writes == frozenset( + {SymbolicStateKey.articulation_joint("drawer", "drawer_slide")} + ) + assert not analyzed.opaque_symbolic_effect + assert articulation_provider.calls == 0 + assert handle_provider.calls == 0 + + +def test_static_link_rejects_unknown_explicit_handle_with_path() -> None: + registry, _, _ = _registry() + compiler = _compiler(registry) + + with pytest.raises(SemanticValidationError) as error: + compiler.analyze( + ( + OperateArticulation( + articulation=SceneArticulationRef("drawer"), + handle=SceneAffordanceRef("missing_handle"), + target="open", + ), + ) + ) + + assert error.value.diagnostic.path == ("workflow", 0, "call", "handle") + + +def test_grounding_uses_fresh_handle_pose_and_lowers_typed_effect() -> None: + registry, articulation_provider, handle_provider = _registry() + compiler = _compiler(registry) + workflow = compiler.analyze( + ( + OperateArticulation( + articulation=SceneArticulationRef("drawer"), + target="open", + ), + ) + ) + env_ids = torch.arange(_BATCH_SIZE, dtype=torch.long) + scene_provider = registry.make_scene_provider( + translation_threshold=0.0, + rotation_threshold=0.0, + ) + first_context = _context( + scene_provider.snapshot(timestamp=0.0, env_ids=env_ids), + timestamp=0.0, + ) + first = compiler.ground(workflow, 0, first_context) + handle_provider.pose[:, 0, 3] = 0.7 + assert articulation_provider.joint_position is not None + articulation_provider.joint_position[:, 0] = 0.1 + second_context = _context( + scene_provider.snapshot(timestamp=1.0, env_ids=env_ids), + timestamp=1.0, + ) + second = compiler.ground(workflow, 0, second_context, revision=1) + + first_goal = first.invocation.goal + second_goal = second.invocation.goal + assert type(first_goal) is OperateArticulationGoal + assert type(second_goal) is OperateArticulationGoal + first_poses = first_goal.geometry.resolve( + first_context, + displacement=torch.full((_BATCH_SIZE,), _TARGET_DISPLACEMENT), + ) + second_poses = second_goal.geometry.resolve( + second_context, + displacement=torch.full((_BATCH_SIZE,), _TARGET_DISPLACEMENT), + ) + assert torch.allclose(first_poses[0][:, 0, 3], torch.full((2,), 0.3)) + assert torch.allclose(second_poses[0][:, 0, 3], torch.full((2,), 0.7)) + assert torch.allclose(second_poses[2][:, 0, 3], torch.full((2,), 0.9)) + assert torch.equal( + first_goal.source_position, + torch.zeros(_BATCH_SIZE, 1), + ) + assert torch.equal( + second_goal.source_position, + torch.full((_BATCH_SIZE, 1), 0.1), + ) + assert second_goal.target_displacement == _TARGET_DISPLACEMENT + assert torch.allclose( + second_goal.target_position, + torch.full((_BATCH_SIZE, 1), _TARGET_POSITION), + ) + + effect = second.effect_spec + assert effect is not None + assert effect.effect_kind is SemanticEffectKind.ARTICULATION + expectation = effect.state_expectations[0] + clause = effect.clauses[0] + assert type(expectation) is ArticulationJointStateExpectation + assert expectation.articulation_id == "drawer" + assert expectation.joint_id == "drawer_slide" + assert type(clause) is JointStateEffectClause + assert torch.equal(clause.target_position, second_goal.target_position) + assert clause.source.provider_id == SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID + assert clause.source.revision == SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION + assert type(clause.source.address) is ArticulationJointEvidenceAddress + assert clause.source.address.articulation_id == "drawer" + assert clause.source.address.joint_id == "drawer_slide" + + +def test_explicit_target_pair_records_live_source_joint_state() -> None: + registry, _, _ = _registry() + compiler = _compiler(registry) + workflow = compiler.analyze( + ( + OperateArticulation( + articulation=SceneArticulationRef("drawer"), + target_position=0.25, + target_displacement=-0.1, + ), + ) + ) + env_ids = torch.arange(_BATCH_SIZE, dtype=torch.long) + scene = registry.make_scene_provider( + translation_threshold=0.0, + rotation_threshold=0.0, + ).snapshot(timestamp=0.0, env_ids=env_ids) + + grounded = compiler.ground(workflow, 0, _context(scene, timestamp=0.0)) + + goal = grounded.invocation.goal + assert type(goal) is OperateArticulationGoal + assert torch.allclose(goal.target_position, torch.full((_BATCH_SIZE, 1), 0.25)) + assert torch.equal(goal.source_position, torch.zeros(_BATCH_SIZE, 1)) + assert goal.target_displacement == -0.1 + operation = goal.geometry.resolve( + _context(scene, timestamp=0.0), + displacement=torch.full((_BATCH_SIZE,), -0.1), + )[2] + assert torch.allclose(operation[:, 0, 3], torch.full((2,), 0.25)) + + +def test_unknown_named_target_has_strict_grounding_diagnostic() -> None: + registry, _, _ = _registry() + compiler = _compiler(registry) + workflow = compiler.analyze( + ( + OperateArticulation( + articulation=SceneArticulationRef("drawer"), + target="closed", + ), + ) + ) + env_ids = torch.arange(_BATCH_SIZE, dtype=torch.long) + scene = registry.make_scene_provider( + translation_threshold=0.0, + rotation_threshold=0.0, + ).snapshot(timestamp=0.0, env_ids=env_ids) + + with pytest.raises(SemanticValidationError) as error: + compiler.ground(workflow, 0, _context(scene, timestamp=0.0)) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "unknown_articulation_target" + assert diagnostic.path == ("workflow", 0, "call", "target") + assert diagnostic.candidates == ("open",) + + +def test_missing_handle_pose_has_strict_grounding_diagnostic() -> None: + registry, _, _ = _registry() + compiler = _compiler(registry) + workflow = compiler.analyze( + ( + OperateArticulation( + articulation=SceneArticulationRef("drawer"), + target="open", + ), + ) + ) + scene = SceneSnapshot( + timestamp=0.0, + version=0, + entities={"drawer": EntityState(torch.eye(4).repeat(_BATCH_SIZE, 1, 1))}, + ) + + with pytest.raises(SemanticValidationError) as error: + compiler.ground(workflow, 0, _context(scene, timestamp=0.0)) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "missing_handle_observation" + assert diagnostic.path == ("workflow", 0, "call", "handle") + + +def test_missing_live_joint_state_has_strict_grounding_diagnostic() -> None: + registry, _, _ = _registry() + compiler = _compiler(registry) + workflow = compiler.analyze( + ( + OperateArticulation( + articulation=SceneArticulationRef("drawer"), + target="open", + ), + ) + ) + handle = torch.eye(4).repeat(_BATCH_SIZE, 1, 1) + scene = SceneSnapshot( + timestamp=0.0, + version=0, + entities={"drawer_handle": EntityState(handle)}, + ) + + with pytest.raises(SemanticValidationError) as error: + compiler.ground(workflow, 0, _context(scene, timestamp=0.0)) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "missing_articulation_joint_observation" + assert diagnostic.path == ("workflow", 0, "call", "articulation") + + +def test_articulation_capability_rejects_untyped_affordance_payload() -> None: + articulation = SceneArticulationRef("drawer") + handle = SceneAffordanceRef("drawer_handle") + provider = _MutablePoseProvider(torch.eye(4).repeat(_BATCH_SIZE, 1, 1)) + + with pytest.raises(TypeError, match="ArticulationOperationAffordance"): + SceneRegistry( + ( + SceneEntityRegistration( + ref=articulation, + state_provider=provider, + default_affordances={ + ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY: handle + }, + ), + SceneEntityRegistration( + ref=handle, + state_provider=provider, + parent=articulation, + native_name="handle", + affordance=Affordance(), + affordance_capabilities=frozenset( + {ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY} + ), + affordance_revision="bad-v1", + ), + ) + ) diff --git a/tests/sim/skills/test_calls.py b/tests/sim/skills/test_calls.py new file mode 100644 index 000000000..cde90d8ad --- /dev/null +++ b/tests/sim/skills/test_calls.py @@ -0,0 +1,445 @@ +# ---------------------------------------------------------------------------- +# 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 immutable, declarative semantic call values.""" + +from __future__ import annotations + +from collections.abc import Callable, Mapping +import json +import math + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + SkillBindingContract, + SkillDescriptor, + SkillEndpointRequirement, + SkillResourceSlot, +) +from embodichain.lab.sim.skills.calls import ( + HandOver, + Pick, + Place, + RegisteredSemanticCall, + SemanticCallCatalog, + SemanticCallDescriptor, + SemanticCallSpec, + SemanticPose, + builtin_semantic_call_catalog, +) +from embodichain.lab.sim.skills.scene import ( + SceneAffordanceRef, + SceneEntityRef, + SceneObjectRef, +) + + +def _identity_pose() -> SemanticPose: + return SemanticPose((0.0, 0.0, 0.0), (1.0, 0.0, 0.0, 0.0)) + + +def _call_descriptor( + call_id: str, + spec_type: type[SemanticCallSpec], +) -> SemanticCallDescriptor: + if spec_type is not RegisteredSemanticCall: + return builtin_semantic_call_catalog().discover(call_id) + target = builtin_semantic_call_catalog().discover("pick").target_descriptor + assert target is not None + assert target.binding_contract is not None + return SemanticCallDescriptor( + call_id=call_id, + spec_type=spec_type, + target_descriptor=target, + ) + + +def test_semantic_pose_owns_inputs_and_returns_independent_tensors() -> None: + position = torch.tensor([1.0, 2.0, 3.0]) + quaternion = torch.tensor([1.0, 0.0, 0.0, 0.0]) + pose = SemanticPose(position, quaternion) + + position.zero_() + quaternion.zero_() + returned_position = pose.position + returned_quaternion = pose.quaternion_wxyz + returned_position.fill_(9.0) + returned_quaternion.fill_(9.0) + + torch.testing.assert_close(pose.position, torch.tensor([1.0, 2.0, 3.0])) + torch.testing.assert_close( + pose.quaternion_wxyz, + torch.tensor([1.0, 0.0, 0.0, 0.0]), + ) + + +def test_semantic_pose_normalizes_wxyz_quaternion() -> None: + pose = SemanticPose((0.0, 0.0, 0.0), (2.0, 0.0, 0.0, 2.0)) + + expected = torch.tensor( + [math.sqrt(0.5), 0.0, 0.0, math.sqrt(0.5)], + dtype=torch.float32, + ) + torch.testing.assert_close(pose.quaternion_wxyz, expected) + + +def test_semantic_pose_converts_to_homogeneous_matrix() -> None: + pose = SemanticPose((1.0, 2.0, 3.0), (2.0, 0.0, 0.0, 2.0)) + + expected = torch.tensor( + [ + [0.0, -1.0, 0.0, 1.0], + [1.0, 0.0, 0.0, 2.0], + [0.0, 0.0, 1.0, 3.0], + [0.0, 0.0, 0.0, 1.0], + ] + ) + torch.testing.assert_close(pose.to_matrix(), expected, atol=1.0e-6, rtol=1.0e-6) + + +def test_semantic_call_metadata_is_deterministic_and_json_safe() -> None: + call = Place( + object=SceneObjectRef("cube"), + at=SemanticPose((1.0, 2.0, 3.0), (1.0, 0.0, 0.0, 0.0)), + resources={"primary": "left_arm"}, + ) + + metadata = call.to_metadata() + + json.dumps(metadata, allow_nan=False, sort_keys=True) + assert metadata["semantic_id"] == "place" + assert metadata["resources"] == {"primary": "left_arm"} + assert metadata["arguments"]["object"] == { + "entity_type": "SceneObjectRef", + "entity_id": "cube", + } + assert metadata["arguments"]["at"]["position"] == [1.0, 2.0, 3.0] + + +@pytest.mark.parametrize( + "factory", + ( + pytest.param( + lambda resources: Pick( + object=SceneObjectRef("cube"), + resources=resources, + ), + id="pick", + ), + pytest.param( + lambda resources: Place( + object=SceneObjectRef("cube"), + at=_identity_pose(), + resources=resources, + ), + id="place", + ), + pytest.param( + lambda resources: HandOver( + object=SceneObjectRef("cube"), + resources=resources, + ), + id="hand-over", + ), + pytest.param( + lambda resources: RegisteredSemanticCall( + call_id="vendor.navigate", + resources=resources, + ), + id="registered", + ), + ), +) +def test_semantic_calls_snapshot_and_freeze_resources( + factory: Callable[[Mapping[str, str]], SemanticCallSpec], +) -> None: + source = {"actor": "left_arm"} + call = factory(source) + + source["actor"] = "right_arm" + + assert call.resources == {"actor": "left_arm"} + with pytest.raises(TypeError): + call.resources["actor"] = "right_arm" # type: ignore[index] + + +def test_pick_requires_typed_object_and_affordance_references() -> None: + with pytest.raises(TypeError, match="Pick.object"): + Pick(object=SceneEntityRef("cube")) # type: ignore[arg-type] + + with pytest.raises(TypeError, match="Pick.grasp"): + Pick( + object=SceneObjectRef("cube"), + grasp=SceneObjectRef("cube.grasp"), # type: ignore[arg-type] + ) + + +def test_place_requires_exactly_one_destination() -> None: + object_ref = SceneObjectRef("cube") + + with pytest.raises(ValueError, match="exactly one"): + Place(object=object_ref) + with pytest.raises(ValueError, match="exactly one"): + Place( + object=object_ref, + at=_identity_pose(), + on=SceneObjectRef("table"), + ) + + +def test_place_snapshots_absolute_destination_pose() -> None: + destination = _identity_pose() + + call = Place(object=SceneObjectRef("cube"), at=destination) + + assert call.at is not destination + assert call.at is not None + torch.testing.assert_close(call.at.to_matrix(), destination.to_matrix()) + + +def test_handover_uses_destination_resource_selection() -> None: + call = HandOver( + object=SceneObjectRef("cube"), + resources={"destination": "right_actor"}, + ) + + assert call.resources == {"destination": "right_actor"} + + +def test_handover_snapshots_optional_final_target() -> None: + final_target = _identity_pose() + + call = HandOver( + object=SceneObjectRef("cube"), + final_target=final_target, + ) + + assert call.final_target is not final_target + assert call.final_target is not None + torch.testing.assert_close(call.final_target.to_matrix(), final_target.to_matrix()) + + +def test_registered_call_recursively_snapshots_declarative_arguments() -> None: + step = {"object": SceneObjectRef("cube")} + steps = [step] + pose = _identity_pose() + arguments = {"steps": steps, "target": pose} + + call = RegisteredSemanticCall( + call_id="vendor.navigate", + arguments=arguments, + ) + step["object"] = SceneObjectRef("changed") + steps.append({"object": SceneObjectRef("extra")}) + + saved_steps = call.arguments["steps"] + assert isinstance(saved_steps, tuple) + assert len(saved_steps) == 1 + assert saved_steps[0] == {"object": SceneObjectRef("cube")} + saved_target = call.arguments["target"] + assert isinstance(saved_target, SemanticPose) + assert saved_target is not pose + with pytest.raises(TypeError): + call.arguments["new"] = 1 # type: ignore[index] + + +@pytest.mark.parametrize( + "unsafe_value", + ( + pytest.param(lambda: None, id="callable"), + pytest.param(torch.tensor([1.0]), id="tensor"), + pytest.param(object(), id="live-object"), + ), +) +def test_registered_call_rejects_executable_or_live_payloads( + unsafe_value: object, +) -> None: + with pytest.raises(TypeError, match="non-declarative"): + RegisteredSemanticCall( + call_id="vendor.navigate", + arguments={"unsafe": unsafe_value}, + ) + + +def test_registered_call_rejects_non_finite_payload_numbers() -> None: + with pytest.raises(ValueError, match="finite"): + RegisteredSemanticCall( + call_id="vendor.navigate", + arguments={"speed": float("nan")}, + ) + + +@pytest.mark.parametrize( + "call_id", + (".", "vendor.", ".inspect", "vendor..inspect", "Vendor.inspect"), +) +def test_registered_call_rejects_malformed_namespace(call_id: str) -> None: + with pytest.raises(ValueError, match="segments"): + RegisteredSemanticCall(call_id=call_id) + + +def test_registered_call_rejects_cyclic_payload() -> None: + payload: dict[str, object] = {} + payload["self"] = payload + + with pytest.raises(ValueError, match="cyclic"): + RegisteredSemanticCall( + call_id="vendor.inspect", + arguments=payload, + ) + + +def test_registered_call_rejects_string_subclass_identifier() -> None: + class LiveString(str): + live_handle = object() + + with pytest.raises(ValueError, match="non-empty string"): + RegisteredSemanticCall(call_id=LiveString("vendor.inspect")) + + +def test_semantic_call_catalog_discovers_without_mutable_runtime_state() -> None: + pick_descriptor = _call_descriptor(Pick.call_kind, Pick) + catalog = SemanticCallCatalog([pick_descriptor]) + + assert catalog.discover("pick") is pick_descriptor + assert catalog.discover(Pick(object=SceneObjectRef("cube"))) is pick_descriptor + with pytest.raises(TypeError): + catalog.descriptors["other"] = pick_descriptor # type: ignore[index] + + +def test_semantic_call_catalog_extension_does_not_mutate_original() -> None: + pick_descriptor = _call_descriptor(Pick.call_kind, Pick) + extension = _call_descriptor("vendor.navigate", RegisteredSemanticCall) + original = SemanticCallCatalog([pick_descriptor]) + + extended = original.with_descriptor(extension) + + with pytest.raises(KeyError, match="Unknown semantic call"): + original.discover("vendor.navigate") + assert ( + extended.discover(RegisteredSemanticCall(call_id="vendor.navigate")) + is extension + ) + + +def test_semantic_call_catalog_rejects_duplicate_ids() -> None: + descriptor = _call_descriptor(Pick.call_kind, Pick) + + with pytest.raises(ValueError, match="Duplicate semantic call ID"): + SemanticCallCatalog([descriptor, descriptor]) + + +def test_catalog_rejects_executable_call_subclasses() -> None: + class UnsafeRegisteredCall(RegisteredSemanticCall): + pass + + with pytest.raises(TypeError, match="exactly"): + SemanticCallDescriptor( + call_id="vendor.unsafe", + spec_type=UnsafeRegisteredCall, + ) + + +def test_registered_payload_rejects_value_subclasses() -> None: + class LiveInteger(int): + live_handle = object() + + with pytest.raises(TypeError, match="non-declarative"): + RegisteredSemanticCall( + call_id="vendor.unsafe", + arguments={"value": LiveInteger(1)}, + ) + + +def test_builtin_descriptor_target_cannot_be_remapped() -> None: + target = builtin_semantic_call_catalog().discover("pick").target_descriptor + assert target is not None + remapped = SkillDescriptor( + skill_id="move_joints", + goal_type=target.goal_type, + options_type=target.options_type, + binding_contract=SkillBindingContract(), + ) + + with pytest.raises(ValueError, match="exact curated"): + SemanticCallDescriptor( + call_id=Pick.call_kind, + spec_type=Pick, + target_descriptor=remapped, + ) + + +def test_catalog_rejects_descriptor_subclass_with_live_state() -> None: + class LiveDescriptor(SemanticCallDescriptor): + live_handle = object() + + source = _call_descriptor("vendor.inspect", RegisteredSemanticCall) + descriptor = LiveDescriptor( + call_id=source.call_id, + spec_type=source.spec_type, + target_descriptor=source.target_descriptor, + ) + + with pytest.raises(TypeError, match="exact SemanticCallDescriptor"): + SemanticCallCatalog((descriptor,)) + + +def test_descriptor_rejects_runtime_bearing_binding_contract_subclasses() -> None: + class LiveSlot(SkillResourceSlot): + live_handle = object() + + target = builtin_semantic_call_catalog().discover("pick").target_descriptor + assert target is not None + endpoint = SkillEndpointRequirement("motion") + contract = SkillBindingContract(slots=(LiveSlot("primary", (endpoint,)),)) + remapped_target = SkillDescriptor( + skill_id=target.skill_id, + goal_type=target.goal_type, + options_type=target.options_type, + binding_contract=contract, + ) + + with pytest.raises(TypeError, match="exact SkillResourceSlot"): + SemanticCallDescriptor( + call_id="vendor.inspect", + spec_type=RegisteredSemanticCall, + target_descriptor=remapped_target, + ) + + +def test_descriptor_rejects_target_descriptor_subclass() -> None: + class LiveTarget(SkillDescriptor): + live_handle = object() + + target = builtin_semantic_call_catalog().discover("pick").target_descriptor + assert target is not None + live_target = LiveTarget( + skill_id=target.skill_id, + goal_type=target.goal_type, + options_type=target.options_type, + agent_visible=target.agent_visible, + binding_contract=target.binding_contract, + ) + assert target.binding_contract is not None + + with pytest.raises(TypeError, match="exactly SkillDescriptor"): + SemanticCallDescriptor( + call_id="vendor.inspect", + spec_type=RegisteredSemanticCall, + target_descriptor=live_target, + ) diff --git a/tests/sim/skills/test_compiler.py b/tests/sim/skills/test_compiler.py new file mode 100644 index 000000000..b5de61a65 --- /dev/null +++ b/tests/sim/skills/test_compiler.py @@ -0,0 +1,1686 @@ +# ---------------------------------------------------------------------------- +# 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 static semantic analysis and JIT invocation lowering.""" + +from __future__ import annotations + +from types import MethodType +from typing import ClassVar +from unittest.mock import Mock + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + ActionOptions, + Affordance, + AntipodalAffordance, + AtomicActionEngine, + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + ControlPartCommandProfile, + DynamicCollisionMode, + EntityState, + ExecutionStatus, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + GraspGoal, + HandOverOptions, + HeldObjectState, + MotionPolicy, + ObjectSemantics, + OperateArticulationOptions, + PickUp, + PickUpOptions, + PlaceGoal, + PlaceOptions, + PlanningContext, + RobotObservation, + SceneEntityPose, + TaskState, +) +from embodichain.lab.sim.atomic_actions.tracking import ( + JointPositionTrackingMetric, + TrackingPolicy, +) +from embodichain.lab.sim.skills.calls import ( + HandOver, + Pick, + Place, + RegisteredSemanticCall, + SemanticCallDescriptor, + SemanticPose, + builtin_semantic_call_catalog, +) +from embodichain.lab.sim.skills.compiler import ( + ContainerRelationTargetGrounder, + GroundedSemanticCall, + HandOverPoseProvider, + HandOverPoseTargets, + HeldObjectGuardBaseline, + RegisteredSemanticLowerer, + RelationTargetGrounder, + SemanticLowering, + SemanticObjectTarget, + SemanticRelationTarget, + SemanticSkillCompiler, + SemanticWorkflow, + SupportSurfaceRelationTargetGrounder, +) +from embodichain.lab.sim.skills.effects import ( + BinaryEffectClause, + BinaryEvidenceKind, + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + CompositeEffectMonitorFactory, + ControlPartEvidenceAddress, + EffectMonitor, + EffectMonitorRef, + EffectMonitorRegistry, + HeldObjectRelation, + HeldObjectStateExpectation, + PoseRelationClause, + PoseRelationExpectation, + SemanticEffectKind, + SemanticEffectSpec, + SymbolicStateKey, +) +from embodichain.lab.sim.skills.integration import ( + BoundSemanticCall, + SceneManifest, + SemanticIntegrationManifest, + SemanticValidationError, +) +from embodichain.lab.sim.skills.profiles import ( + ControlPartEndpoint, + ResourceBinding, + RobotResource, + RobotSkillProfile, + SkillPolicyPreset, +) +from embodichain.lab.sim.skills.scene import ( + ContainerAffordance, + GRASP_AFFORDANCE_CAPABILITY, + PLACEMENT_TARGET_AFFORDANCE_REVISION, + PLACE_IN_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, + SceneAffordanceRef, + SceneCollisionRole, + SceneCollisionWorldMode, + SceneEntityRegistration, + SceneObjectRef, + SceneRegistry, + SupportSurfaceAffordance, +) + +_MOTION_CAPABILITIES = frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } +) +_PICK_TARGET = PickUp.descriptor() + + +def _action_option_templates(*, registered: bool = False) -> dict[str, object]: + """Return complete exact option declarations for the selected catalog.""" + templates: dict[str, object] = { + "pick": PickUpOptions(), + "place": PlaceOptions(), + "hand_over": HandOverOptions(), + "operate_articulation": OperateArticulationOptions(), + } + if registered: + templates["vendor.inspect"] = PickUpOptions() + return templates + + +def _preset( + preset_id: str, + *, + registered: bool = False, + **kwargs: object, +) -> SkillPolicyPreset: + """Build one complete schema-v3 test preset.""" + kwargs.setdefault( + "action_option_templates", + _action_option_templates(registered=registered), + ) + return SkillPolicyPreset(preset_id, **kwargs) + + +class _PoseProvider: + """Return a fixed pose while exposing observation call count.""" + + def __init__(self, pose: torch.Tensor) -> None: + self.pose = pose + self.calls = 0 + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp, env_ids + self.calls += 1 + return EntityState(self.pose) + + +class _GeometryProvider: + """Return one opaque planner-facing geometry descriptor.""" + + def get_geometry(self) -> object: + return object() + + +class _FrameRelationGrounder(RelationTargetGrounder): + """Explicit test contract: relation frame equals target object frame.""" + + capability: ClassVar[str] = PLACE_ON_AFFORDANCE_CAPABILITY + affordance_type: ClassVar[type[Affordance]] = Affordance + affordance_revision: ClassVar[str] = "relation-v1" + + def ground( + self, + relation: SemanticRelationTarget, + *, + affordance: Affordance, + context: PlanningContext, + ) -> SceneEntityPose: + del affordance, context + return SceneEntityPose(relation.affordance.entity_id) + + +class _InspectLowerer(RegisteredSemanticLowerer): + """Test extension proving a lowerer cannot replace compiler ownership.""" + + call_id: ClassVar[str] = "vendor.inspect" + schema_version: ClassVar[int] = 1 + + def __init__(self) -> None: + self.option_templates: list[ActionOptions] = [] + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: object, + option_template: ActionOptions, + ) -> SemanticLowering: + del call, context, bound + self.option_templates.append(option_template) + return SemanticLowering( + goal=GraspGoal( + semantics=ObjectSemantics( + affordance=AntipodalAffordance(), + geometry={}, + entity_id="cube", + ) + ), + ) + + +class _DerivedGraspGoal(GraspGoal): + """Executable subclass that an extension must not smuggle into the core.""" + + +class _SubclassOutputLowerer(RegisteredSemanticLowerer): + """Try to bypass exact goal or preset-owned options contracts.""" + + call_id: ClassVar[str] = "vendor.inspect" + schema_version: ClassVar[int] = 1 + + def __init__(self, output: str) -> None: + self.output = output + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + option_template: ActionOptions, + ) -> SemanticLowering: + del call, context, bound, option_template + semantics = ObjectSemantics( + affordance=AntipodalAffordance(), + geometry={}, + entity_id="cube", + ) + if self.output == "goal": + return SemanticLowering( + goal=_DerivedGraspGoal(semantics=semantics), + ) + return SemanticLowering( + goal=GraspGoal(semantics=semantics), + skill_options=PickUpOptions(pre_grasp_distance=0.99), + ) + + +class _DualCenterHandOverProvider(HandOverPoseProvider): + """Resolve named dual-arm handover poses without observing during analysis.""" + + provider_id: ClassVar[str] = "dual_center" + + def __init__(self) -> None: + self.calls = 0 + + def resolve( + self, + call: HandOver, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> HandOverPoseTargets: + del call, context, bound + self.calls += 1 + return HandOverPoseTargets( + middle=SemanticObjectTarget(SceneEntityPose("table_top")), + final=SemanticObjectTarget( + SemanticPose( + (0.5, 0.0, 0.4), + (1.0, 0.0, 0.0, 0.0), + ) + ), + ) + + +class _CountingRelationMonitorFactory(CompositeEffectMonitorFactory): + """Count monitor construction without changing built-in behavior.""" + + def __init__(self) -> None: + self.calls = 0 + + def create( + self, + spec: SemanticEffectSpec, + ref: EffectMonitorRef, + ) -> EffectMonitor: + self.calls += 1 + return super().create(spec, ref) + + +class _BadCreatingRelationMonitorFactory(CompositeEffectMonitorFactory): + """Return an invalid monitor value after successful static validation.""" + + def create( + self, + spec: SemanticEffectSpec, + ref: EffectMonitorRef, + ) -> EffectMonitor: + del spec, ref + return object() # type: ignore[return-value] + + +def _scene_registry( + *, + dynamic_collision: bool = False, +) -> tuple[SceneRegistry, tuple[_PoseProvider, _PoseProvider]]: + cube_provider = _PoseProvider(torch.eye(4).repeat(2, 1, 1)) + table_pose = torch.eye(4).repeat(2, 1, 1) + table_pose[:, 0, 3] = 0.6 + table_provider = _PoseProvider(table_pose) + cube = SceneObjectRef("cube") + table = SceneObjectRef("table") + grasp = SceneAffordanceRef("cube_grasp") + table_top = SceneAffordanceRef("table_top") + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=cube, + state_provider=cube_provider, + semantic_type="cube", + default_affordances={GRASP_AFFORDANCE_CAPABILITY: grasp}, + geometry_provider=(_GeometryProvider() if dynamic_collision else None), + collision_role=( + SceneCollisionRole.DYNAMIC + if dynamic_collision + else SceneCollisionRole.NONE + ), + ), + SceneEntityRegistration( + ref=grasp, + parent=cube, + native_name="grasp", + affordance=AntipodalAffordance(), + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + affordance_revision="grasp-v1", + relative_pose=torch.eye(4), + ), + SceneEntityRegistration( + ref=table, + state_provider=table_provider, + semantic_type="table", + default_affordances={PLACE_ON_AFFORDANCE_CAPABILITY: table_top}, + ), + SceneEntityRegistration( + ref=table_top, + parent=table, + native_name="top", + affordance=Affordance(), + affordance_capabilities=frozenset({PLACE_ON_AFFORDANCE_CAPABILITY}), + affordance_revision="relation-v1", + relative_pose=torch.eye(4), + ), + ), + collision_world_mode=( + SceneCollisionWorldMode.PER_ENV if dynamic_collision else None + ), + ) + return registry, (cube_provider, table_provider) + + +def _profile( + *, + preset: SkillPolicyPreset | None = None, + registered: bool = False, +) -> RobotSkillProfile: + return RobotSkillProfile( + profile_id="test_robot", + resources={ + "manipulator": RobotResource( + resource_id="manipulator", + endpoints={ + "motion": ControlPartEndpoint( + control_part="arm", + capabilities=_MOTION_CAPABILITIES, + ), + "grasp": ControlPartEndpoint( + control_part="hand", + capabilities=frozenset({GRASP_CAPABILITY}), + ), + }, + ) + }, + command_profiles={ + "hand": ControlPartCommandProfile.joint_positions( + open=torch.tensor([0.0]), + grasp=torch.tensor([1.0]), + ) + }, + presets={ + "safe": ( + _preset("safe", registered=registered) if preset is None else preset + ) + }, + default_preset="safe", + ) + + +def _dual_profile( + *, + provider_id: str | None = "dual_center", + preset: SkillPolicyPreset | None = None, +) -> RobotSkillProfile: + resources = { + side: RobotResource( + resource_id=side, + endpoints={ + "motion": ControlPartEndpoint( + control_part=f"{side}_arm", + capabilities=_MOTION_CAPABILITIES, + ), + "grasp": ControlPartEndpoint( + control_part=f"{side}_hand", + capabilities=frozenset({GRASP_CAPABILITY}), + ), + }, + ) + for side in ("left", "right") + } + return RobotSkillProfile( + profile_id="dual_robot", + resources=resources, + command_profiles={ + f"{side}_hand": ControlPartCommandProfile.joint_positions( + open=torch.tensor([0.0]), + grasp=torch.tensor([1.0]), + ) + for side in ("left", "right") + }, + defaults={ + "pick_up": ResourceBinding({"primary": "left"}), + "hand_over": ResourceBinding({"source": "left", "destination": "right"}), + }, + presets={"safe": _preset("safe") if preset is None else preset}, + default_preset="safe", + grounding_providers=({} if provider_id is None else {"hand_over": provider_id}), + ) + + +def _engine( + profile: RobotSkillProfile, + *, + supports_dynamic_collision_world: bool = False, +) -> AtomicActionEngine: + robot = Mock() + robot.device = torch.device("cpu") + control_parts = tuple( + sorted( + { + endpoint.control_part + for resource in profile.resources.values() + for endpoint in resource.endpoints.values() + if type(endpoint) is ControlPartEndpoint + } + ) + ) + joint_ids = {name: [index] for index, name in enumerate(control_parts)} + robot.dof = len(control_parts) + robot.control_parts = {name: object() for name in control_parts} + robot.get_qpos.return_value = torch.zeros(2, robot.dof) + robot.get_qvel.return_value = torch.zeros(2, robot.dof) + robot.get_joint_ids.side_effect = lambda name: joint_ids[name] + robot.get_solver.return_value = object() + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "stub_planner" + generator.supports_dynamic_collision_world = supports_dynamic_collision_world + return AtomicActionEngine(generator, skill_profile=profile) + + +def _integration( + registry: SceneRegistry, + *, + registered: bool = False, + profile: RobotSkillProfile | None = None, + supports_dynamic_collision_world: bool = False, +) -> tuple[SemanticIntegrationManifest, AtomicActionEngine]: + selected_profile = _profile(registered=registered) if profile is None else profile + catalog = builtin_semantic_call_catalog() + if registered: + assert _PICK_TARGET.binding_contract is not None + catalog = catalog.with_descriptor( + SemanticCallDescriptor( + call_id="vendor.inspect", + spec_type=RegisteredSemanticCall, + target_descriptor=_PICK_TARGET, + ) + ) + manifest = SemanticIntegrationManifest( + scene=SceneManifest.from_registry(registry), + robot_profile=selected_profile, + call_catalog=catalog, + ) + return manifest, _engine( + selected_profile, + supports_dynamic_collision_world=supports_dynamic_collision_world, + ) + + +def _compiler( + registry: SceneRegistry, + *, + registered: bool = False, + relation_grounders: tuple[RelationTargetGrounder, ...] = ( + _FrameRelationGrounder(), + ), + registered_lowerers: tuple[RegisteredSemanticLowerer, ...] = (), + handover_pose_providers: tuple[HandOverPoseProvider, ...] = (), + profile: RobotSkillProfile | None = None, + effect_monitor_registry: EffectMonitorRegistry | None = None, + supports_dynamic_collision_world: bool = False, +) -> tuple[SemanticSkillCompiler, AtomicActionEngine]: + manifest, engine = _integration( + registry, + registered=registered, + profile=profile, + supports_dynamic_collision_world=supports_dynamic_collision_world, + ) + bound = manifest.bind(registry, engine) + return ( + SemanticSkillCompiler( + bound, + relation_grounders=relation_grounders, + registered_lowerers=registered_lowerers, + handover_pose_providers=handover_pose_providers, + effect_monitor_registry=effect_monitor_registry, + ), + engine, + ) + + +def _dual_compiler( + registry: SceneRegistry, +) -> tuple[ + SemanticSkillCompiler, + AtomicActionEngine, + _DualCenterHandOverProvider, +]: + profile = _dual_profile() + manifest = SemanticIntegrationManifest( + scene=SceneManifest.from_registry(registry), + robot_profile=profile, + call_catalog=builtin_semantic_call_catalog(), + ) + engine = _engine(profile) + provider = _DualCenterHandOverProvider() + return ( + SemanticSkillCompiler( + manifest.bind(registry, engine), + relation_grounders=(_FrameRelationGrounder(),), + handover_pose_providers=(provider,), + ), + engine, + provider, + ) + + +def _context( + registry: SceneRegistry, + *, + task: TaskState | None = None, + timestamp: float = 0.0, + robot_dof: int = 2, +) -> PlanningContext: + env_ids = torch.tensor([0, 1], dtype=torch.long) + scene = registry.make_scene_provider( + translation_threshold=0.0, + rotation_threshold=0.0, + ).snapshot(timestamp=timestamp, env_ids=env_ids) + return PlanningContext( + robot=RobotObservation( + timestamp=timestamp, + qpos=torch.zeros(2, robot_dof), + qvel=torch.zeros(2, robot_dof), + ), + task=TaskState.empty(2, "cpu") if task is None else task, + scene=scene, + env_ids=env_ids, + ) + + +def _held_context( + registry: SceneRegistry, + semantics: ObjectSemantics, + object_to_eef: torch.Tensor, + *, + env_mask: torch.Tensor | None = None, + task_state_key: str = "manipulator", + robot_dof: int = 2, +) -> PlanningContext: + held = HeldObjectState( + semantics=semantics, + object_to_eef=object_to_eef, + grasp_xpos=torch.eye(4).repeat(2, 1, 1), + env_mask=env_mask, + ) + return _context( + registry, + task=TaskState( + batch_size=2, + device="cpu", + held_objects={task_state_key: held}, + ), + robot_dof=robot_dof, + ) + + +@pytest.mark.parametrize( + ("grounder", "capability", "affordance_type"), + ( + ( + SupportSurfaceRelationTargetGrounder(), + PLACE_ON_AFFORDANCE_CAPABILITY, + SupportSurfaceAffordance, + ), + ( + ContainerRelationTargetGrounder(), + PLACE_IN_AFFORDANCE_CAPABILITY, + ContainerAffordance, + ), + ), +) +def test_builtin_relation_grounders_preserve_late_pose_and_confidence( + grounder: RelationTargetGrounder, + capability: str, + affordance_type: type[Affordance], +) -> None: + """Production relation grounders keep target frames live and typed.""" + registry, _ = _scene_registry() + relation = SemanticRelationTarget( + capability=capability, + affordance=SceneAffordanceRef("declared_target"), + payload_type=affordance_type, + payload_revision=PLACEMENT_TARGET_AFFORDANCE_REVISION, + ) + + target = grounder.ground( + relation, + affordance=affordance_type(minimum_confidence=0.65), + context=_context(registry), + ) + + assert type(target) is SceneEntityPose + assert target.entity_id == "declared_target" + assert target.relative_pose is None + assert target.minimum_confidence == pytest.approx(0.65) + + +def test_curated_analysis_selects_exact_preset_monitor_without_creating_it() -> None: + registry, providers = _scene_registry() + factory = _CountingRelationMonitorFactory() + compiler, _ = _compiler( + registry, + effect_monitor_registry=EffectMonitorRegistry((factory,)), + ) + + workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + + monitor_ref = workflow.calls[0].effect_monitor_ref + assert monitor_ref is not None + assert monitor_ref.monitor_id == COMPOSITE_EFFECT_MONITOR_ID + assert monitor_ref.revision == COMPOSITE_EFFECT_MONITOR_REVISION + assert workflow.calls[0].symbolic_writes == frozenset( + {SymbolicStateKey.held_object("manipulator")} + ) + assert not workflow.calls[0].opaque_symbolic_effect + assert factory.calls == 0 + assert [provider.calls for provider in providers] == [0, 0] + + +def test_curated_analysis_rejects_explicitly_missing_monitor() -> None: + registry, _ = _scene_registry() + profile = _profile( + preset=_preset("safe", effect_monitors={}), + ) + compiler, _ = _compiler(registry, profile=profile) + + with pytest.raises(SemanticValidationError) as error: + compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + + assert error.value.diagnostic.code == "missing_effect_monitor" + + +def test_uninstalled_effect_monitor_fails_analysis_without_factory_creation() -> None: + registry, providers = _scene_registry() + factory = _CountingRelationMonitorFactory() + profile = _profile( + preset=_preset( + "safe", + effect_monitors={ + "pick": EffectMonitorRef("test.not_installed", "1"), + }, + ), + ) + compiler, _ = _compiler( + registry, + profile=profile, + effect_monitor_registry=EffectMonitorRegistry((factory,)), + ) + + with pytest.raises(SemanticValidationError) as error: + compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + + assert error.value.diagnostic.code == "effect_monitor_not_installed" + assert factory.calls == 0 + assert [provider.calls for provider in providers] == [0, 0] + + +def test_invalid_effect_monitor_config_fails_analysis_without_side_effects() -> None: + registry, providers = _scene_registry() + factory = _CountingRelationMonitorFactory() + profile = _profile( + preset=_preset( + "safe", + effect_monitors={ + "pick": EffectMonitorRef( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + { + "attached_translation_threshold": 0.10, + "detached_translation_threshold": 0.05, + }, + ), + }, + ), + ) + compiler, _ = _compiler( + registry, + profile=profile, + effect_monitor_registry=EffectMonitorRegistry((factory,)), + ) + + with pytest.raises(SemanticValidationError) as error: + compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "invalid_effect_monitor_config" + assert diagnostic.path == ("workflow", 0, "effect_monitor") + assert factory.calls == 0 + assert [provider.calls for provider in providers] == [0, 0] + + +def test_pick_effect_spec_binds_destination_and_fresh_monitor_per_grounding() -> None: + registry, _ = _scene_registry() + compiler, _ = _compiler(registry) + workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + context = _context(registry) + + first = compiler.ground(workflow, 0, context) + repeated = compiler.ground(workflow, 0, context) + revised = compiler.ground(workflow, 0, context, revision=1) + + spec = first.effect_spec + assert spec is not None + assert spec.semantic_id == "pick" + assert spec.effect_kind is SemanticEffectKind.ATTACH + assert spec.skill_id == first.invocation.skill_id + assert spec.invocation_id == first.invocation.invocation_id + assert spec.invocation_revision == 0 + torch.testing.assert_close(spec.env_ids, context.env_ids) + assert len(spec.state_expectations) == 1 + relation = spec.state_expectations[0] + assert isinstance(relation, HeldObjectStateExpectation) + assert relation.expectation_id == "destination" + assert relation.relation is HeldObjectRelation.ATTACHED + assert relation.object_id == "cube" + assert relation.slot_id == "primary" + assert relation.resource_id == "manipulator" + assert relation.task_state_key == "manipulator" + pose, constraint = spec.clauses + assert isinstance(pose, PoseRelationClause) + assert pose.expectation is PoseRelationExpectation.MATCHED + assert pose.baseline_object_to_endpoint is None + assert pose.source.address == ControlPartEvidenceAddress("arm", "pose_relation") + assert isinstance(constraint, BinaryEffectClause) + assert constraint.evidence_kind is BinaryEvidenceKind.CONSTRAINT + assert constraint.expected is True + assert constraint.source.address == ControlPartEvidenceAddress("hand", "constraint") + assert ( + first.analyzed.bound.binding.action_binding.endpoint( + "primary", "motion" + ).task_state_key + == "manipulator" + ) + assert first.effect_monitor is not None + assert repeated.effect_monitor is not None + assert revised.effect_monitor is not None + assert repeated.effect_monitor is not first.effect_monitor + assert revised.effect_monitor is not first.effect_monitor + assert repeated.effect_spec is not None + assert repeated.effect_spec.invocation_revision == 0 + 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 + assert len(first.effect_gates) == 1 + gate = first.effect_gates[0] + assert gate.gate_id == "destination_acquired" + assert gate.segment_name == "lift" + assert gate.retry_action is True + assert gate.effect_monitor is not first.effect_monitor + assert gate.effect_monitor is not guard.effect_monitor + assert gate.effect_spec.state_expectations[0].expectation_id == "destination" + assert gate.effect_spec.effect_kind is SemanticEffectKind.ATTACH + assert first.invocation.phase_effect_gates == (gate.requirement,) + assert repeated.effect_gates[0].effect_monitor is not gate.effect_monitor + + +def test_place_effect_spec_binds_source_and_verified_detach_baseline() -> None: + registry, _ = _scene_registry() + compiler, _ = _compiler(registry) + pick_workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + pick = compiler.ground(pick_workflow, 0, _context(registry)) + semantics = pick.invocation.goal.semantics + object_to_eef = torch.eye(4).repeat(2, 1, 1) + object_to_eef[:, 2, 3] = 0.12 + context = _held_context(registry, semantics, object_to_eef) + workflow = compiler.analyze( + ( + Place( + object=SceneObjectRef("cube"), + at=SemanticPose( + (0.5, -0.2, 0.4), + (1.0, 0.0, 0.0, 0.0), + ), + ), + ) + ) + + assert workflow.calls[0].symbolic_writes == frozenset( + {SymbolicStateKey.held_object("manipulator")} + ) + grounded = compiler.ground(workflow, 0, context) + + spec = grounded.effect_spec + assert spec is not None + assert spec.semantic_id == "place" + assert spec.effect_kind is SemanticEffectKind.RELEASE + assert len(spec.state_expectations) == 1 + relation = spec.state_expectations[0] + assert isinstance(relation, HeldObjectStateExpectation) + assert relation.expectation_id == "source" + assert relation.relation is HeldObjectRelation.DETACHED + assert relation.object_id == "cube" + assert relation.slot_id == "primary" + assert relation.resource_id == "manipulator" + assert relation.task_state_key == "manipulator" + pose, constraint = spec.clauses + assert isinstance(pose, PoseRelationClause) + assert pose.expectation is PoseRelationExpectation.SEPARATED + assert pose.baseline_object_to_endpoint is not None + torch.testing.assert_close( + pose.baseline_object_to_endpoint, + object_to_eef, + ) + 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 + assert len(grounded.effect_gates) == 1 + gate = grounded.effect_gates[0] + assert gate.gate_id == "source_released" + assert gate.segment_name == "retract" + assert gate.retry_action is True + assert gate.effect_spec.effect_kind is SemanticEffectKind.RELEASE + gate_relation = gate.effect_spec.state_expectations[0] + assert isinstance(gate_relation, HeldObjectStateExpectation) + assert gate_relation.expectation_id == "source" + assert gate_relation.relation is HeldObjectRelation.DETACHED + assert grounded.invocation.phase_effect_gates == (gate.requirement,) + + +def test_handover_effect_spec_binds_source_and_destination_relations() -> None: + registry, _ = _scene_registry() + provider = _DualCenterHandOverProvider() + compiler, _ = _compiler( + registry, + profile=_dual_profile(), + handover_pose_providers=(provider,), + ) + pick_workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + pick = compiler.ground( + pick_workflow, + 0, + _context(registry, robot_dof=4), + ) + semantics = pick.invocation.goal.semantics + object_to_source = torch.eye(4).repeat(2, 1, 1) + object_to_source[:, 0, 3] = 0.08 + context = _held_context( + registry, + semantics, + object_to_source, + task_state_key="left", + robot_dof=4, + ) + workflow = compiler.analyze((HandOver(object=SceneObjectRef("cube")),)) + + assert workflow.calls[0].symbolic_writes == frozenset( + { + SymbolicStateKey.held_object("left"), + SymbolicStateKey.held_object("right"), + } + ) + grounded = compiler.ground(workflow, 0, context) + + spec = grounded.effect_spec + assert spec is not None + assert spec.semantic_id == "hand_over" + assert spec.effect_kind is SemanticEffectKind.TRANSFER + assert tuple(relation.expectation_id for relation in spec.state_expectations) == ( + "source", + "destination", + ) + source, destination = spec.state_expectations + assert isinstance(source, HeldObjectStateExpectation) + assert source.relation is HeldObjectRelation.DETACHED + assert source.object_id == "cube" + assert source.slot_id == "source" + assert source.resource_id == "left" + assert source.task_state_key == "left" + source_pose, source_constraint, destination_pose, destination_constraint = ( + spec.clauses + ) + assert isinstance(source_pose, PoseRelationClause) + assert source_pose.expectation is PoseRelationExpectation.SEPARATED + assert source_pose.baseline_object_to_endpoint is not None + torch.testing.assert_close( + source_pose.baseline_object_to_endpoint, + object_to_source, + ) + assert isinstance(source_constraint, BinaryEffectClause) + assert source_constraint.expected is False + assert isinstance(destination, HeldObjectStateExpectation) + assert destination.relation is HeldObjectRelation.ATTACHED + assert destination.object_id == "cube" + assert destination.slot_id == "destination" + assert destination.resource_id == "right" + assert destination.task_state_key == "right" + assert isinstance(destination_pose, PoseRelationClause) + assert destination_pose.expectation is PoseRelationExpectation.MATCHED + 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 + assert len(grounded.effect_gates) == 1 + gate = grounded.effect_gates[0] + assert gate.gate_id == "destination_acquired" + assert gate.segment_name == "release" + assert gate.retry_action is True + assert gate.effect_monitor is not destination_guard.effect_monitor + gate_relation = gate.effect_spec.state_expectations[0] + assert isinstance(gate_relation, HeldObjectStateExpectation) + assert gate_relation.expectation_id == "destination" + assert gate_relation.relation is HeldObjectRelation.ATTACHED + assert grounded.invocation.phase_effect_gates == (gate.requirement,) + + +def test_registered_call_without_monitor_has_no_effect_contract() -> None: + registry, _ = _scene_registry() + factory = _CountingRelationMonitorFactory() + templates = _action_option_templates(registered=True) + templates["vendor.inspect"] = PickUpOptions(pre_grasp_distance=0.07) + profile = _profile( + preset=_preset( + "safe", + registered=True, + action_option_templates=templates, + ) + ) + lowerer = _InspectLowerer() + compiler, _ = _compiler( + registry, + registered=True, + registered_lowerers=(lowerer,), + profile=profile, + effect_monitor_registry=EffectMonitorRegistry((factory,)), + ) + workflow = compiler.analyze((RegisteredSemanticCall(call_id="vendor.inspect"),)) + + grounded = compiler.ground(workflow, 0, _context(registry)) + + assert workflow.calls[0].symbolic_writes == frozenset() + assert workflow.calls[0].opaque_symbolic_effect + assert workflow.calls[0].effect_monitor_ref is None + assert grounded.effect_spec is None + assert grounded.effect_monitor is None + assert grounded.effect_gates == () + assert grounded.invocation.phase_effect_gates == () + options = grounded.invocation.skill_options + assert type(options) is PickUpOptions + assert options.pre_grasp_distance == 0.07 + assert len(lowerer.option_templates) == 1 + assert lowerer.option_templates[0] is not options + assert type(lowerer.option_templates[0]) is PickUpOptions + assert factory.calls == 0 + + +def test_registered_monitor_without_effect_grounder_fails_during_analysis() -> None: + registry, _ = _scene_registry() + profile = _profile( + preset=_preset( + "safe", + registered=True, + effect_monitors={ + "vendor.inspect": EffectMonitorRef( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + ) + }, + ) + ) + compiler, _ = _compiler( + registry, + registered=True, + registered_lowerers=(_InspectLowerer(),), + profile=profile, + ) + + with pytest.raises(SemanticValidationError) as error: + compiler.analyze((RegisteredSemanticCall(call_id="vendor.inspect"),)) + + assert error.value.diagnostic.code == "registered_effect_contract_not_installed" + assert error.value.diagnostic.path == ("workflow", 0, "effect_monitor") + + +def test_ground_wraps_effect_monitor_factory_contract_failure_with_path() -> None: + registry, _ = _scene_registry() + compiler, _ = _compiler( + registry, + effect_monitor_registry=EffectMonitorRegistry( + (_BadCreatingRelationMonitorFactory(),) + ), + ) + workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + + with pytest.raises(SemanticValidationError) as error: + compiler.ground(workflow, 0, _context(registry)) + + assert error.value.diagnostic.code == "effect_monitor_creation_failed" + assert error.value.diagnostic.path == ("workflow", 0, "effect_monitor") + + +def test_analysis_is_provider_free_and_propagates_object_target() -> None: + registry, providers = _scene_registry() + templates = _action_option_templates() + templates["pick"] = PickUpOptions( + pick_object_part="top", + pre_grasp_distance=0.08, + ) + compiler, engine = _compiler( + registry, + profile=_profile( + preset=_preset("safe", action_option_templates=templates), + ), + ) + drop = SemanticPose((0.4, 0.2, 0.3), (1.0, 0.0, 0.0, 0.0)) + + workflow = compiler.analyze( + ( + Pick(object=SceneObjectRef("cube")), + Place(object=SceneObjectRef("cube"), at=drop), + ), + workflow_id="pick_place", + ) + + assert [provider.calls for provider in providers] == [0, 0] + assert len(workflow.calls[0].downstream_object_targets) == 1 + assert workflow.calls[0].downstream_object_targets[0].pose is not drop + assert workflow.effect_dependencies[0].producer_index == 0 + context = _context(registry) + grounded = compiler.ground(workflow, 0, context) + assert type(grounded.invocation.goal) is GraspGoal + assert grounded.invocation.goal.semantics.entity_id == "cube" + options = grounded.invocation.skill_options + assert type(options) is PickUpOptions + assert options.pick_object_part == "top" + assert options.pre_grasp_distance == 0.08 + torch.testing.assert_close( + options.downstream_object_target_poses[0], + drop.to_matrix(), + ) + engine.resolve(grounded.invocation) + + +def test_grounded_safe_invocation_requires_registered_dynamic_collision() -> None: + registry, _ = _scene_registry(dynamic_collision=True) + profile = _profile( + preset=_preset( + "safe", + motion_policy=MotionPolicy(strategy="motion_gen"), + tracking_policy=TrackingPolicy.joint_position( + in_flight_max_abs_error=0.125, + terminal_max_abs_error=0.125, + ), + ) + ) + compiler, engine = _compiler( + registry, + profile=profile, + supports_dynamic_collision_world=True, + ) + workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + + grounded = compiler.ground(workflow, 0, _context(registry)) + + assert ( + grounded.invocation.motion_policy.dynamic_collision_mode + is DynamicCollisionMode.REQUIRED + ) + assert ( + engine.resolve(grounded.invocation).motion_policy.dynamic_collision_mode + is DynamicCollisionMode.REQUIRED + ) + invocation_tracking = grounded.invocation.tracking_policy.in_flight + resolved_tracking = engine.resolve(grounded.invocation).tracking_policy.in_flight + assert invocation_tracking is not None + assert resolved_tracking is not None + assert isinstance(invocation_tracking.metrics[0], JointPositionTrackingMetric) + assert isinstance(resolved_tracking.metrics[0], JointPositionTrackingMetric) + assert invocation_tracking.metrics[0].tolerance == 0.125 + assert resolved_tracking.metrics[0].tolerance == 0.125 + + +def test_place_inherits_known_pick_resource_when_primary_is_omitted() -> None: + registry, _ = _scene_registry() + compiler, _, _ = _dual_compiler(registry) + place = Place( + object=SceneObjectRef("cube"), + at=SemanticPose((0.4, 0.2, 0.3), (1.0, 0.0, 0.0, 0.0)), + ) + + workflow = compiler.analyze( + ( + Pick( + object=SceneObjectRef("cube"), + resources={"primary": "right"}, + ), + place, + ) + ) + + assert place.resources == {} + assert workflow.calls[1].call.resources == {"primary": "right"} + assert workflow.calls[1].bound.binding.resource_ids == {"primary": "right"} + + +def test_handover_source_inherits_known_pick_resource() -> None: + registry, _ = _scene_registry() + compiler, _, provider = _dual_compiler(registry) + handover = HandOver( + object=SceneObjectRef("cube"), + resources={"destination": "left"}, + ) + + workflow = compiler.analyze( + ( + Pick( + object=SceneObjectRef("cube"), + resources={"primary": "right"}, + ), + handover, + ) + ) + + assert handover.resources == {"destination": "left"} + assert workflow.calls[1].call.resources == { + "source": "right", + "destination": "left", + } + assert workflow.calls[1].bound.binding.resource_ids == { + "source": "right", + "destination": "left", + } + assert provider.calls == 0 + + +def test_explicit_consumer_resource_must_match_known_holder() -> None: + registry, _ = _scene_registry() + compiler, _, _ = _dual_compiler(registry) + + with pytest.raises(SemanticValidationError) as error: + compiler.analyze( + ( + Pick( + object=SceneObjectRef("cube"), + resources={"primary": "right"}, + ), + Place( + object=SceneObjectRef("cube"), + at=SemanticPose( + (0.4, 0.2, 0.3), + (1.0, 0.0, 0.0, 0.0), + ), + resources={"primary": "left"}, + ), + ) + ) + + assert error.value.diagnostic.code == "held_resource_mismatch" + assert error.value.diagnostic.rendered_path == ( + "workflow[1].call.resources.primary" + ) + + +def test_pick_relation_lookahead_stays_late_bound_scene_dependency() -> None: + registry, _ = _scene_registry() + compiler, engine = _compiler(registry) + workflow = compiler.analyze( + ( + Pick(object=SceneObjectRef("cube")), + Place( + object=SceneObjectRef("cube"), + on=SceneObjectRef("table"), + ), + ) + ) + + grounded = compiler.ground(workflow, 0, _context(registry)) + + options = grounded.invocation.skill_options + assert type(options) is PickUpOptions + assert len(options.downstream_object_target_poses) == 1 + downstream = options.downstream_object_target_poses[0] + assert type(downstream) is SceneEntityPose + assert downstream.entity_id == "table_top" + request = engine.resolve(grounded.invocation) + action = engine.actions["pick_up"] + assert "table_top" in action._scene_dependencies(request) + + +def test_pick_replan_resolves_downstream_target_from_latest_snapshot() -> None: + registry, providers = _scene_registry() + compiler, engine = _compiler(registry) + workflow = compiler.analyze( + ( + Pick(object=SceneObjectRef("cube")), + Place( + object=SceneObjectRef("cube"), + on=SceneObjectRef("table"), + ), + ) + ) + first_context = _context(registry, timestamp=0.0) + invocation = compiler.ground(workflow, 0, first_context).invocation + action = engine.actions["pick_up"] + captured: list[torch.Tensor] = [] + + def fail_after_capture( + self: object, + semantics: object, + object_pose: torch.Tensor, + start_qpos: torch.Tensor, + manipulator: object, + options: PickUpOptions, + approach_direction: torch.Tensor, + ) -> tuple[torch.Tensor, torch.Tensor]: + del self, semantics, start_qpos, manipulator, approach_direction + target = options.downstream_object_target_poses[0] + assert isinstance(target, torch.Tensor) + captured.append(target.clone()) + return ( + torch.zeros(2, dtype=torch.bool), + object_pose.clone(), + ) + + action._resolve_grasp_pose = MethodType( # type: ignore[method-assign] + fail_after_capture, + action, + ) + request = engine.resolve(invocation) + engine.plan_request(request, first_context) + moved_table_pose = torch.eye(4).repeat(2, 1, 1) + moved_table_pose[:, 0, 3] = 0.9 + providers[1].pose = moved_table_pose + second_context = _context(registry, timestamp=1.0) + engine.plan_request(request, second_context) + + assert captured[0][0, 0, 3].item() == pytest.approx(0.6) + assert captured[1][0, 0, 3].item() == pytest.approx(0.9) + + +def test_handover_uses_profile_selected_named_provider_and_stops_lookahead() -> None: + registry, providers = _scene_registry() + templates = _action_option_templates() + templates["hand_over"] = HandOverOptions( + receive_pick_object_part="top", + pre_grasp_distance=0.06, + ) + profile = _dual_profile( + preset=_preset("safe", action_option_templates=templates), + ) + manifest = SemanticIntegrationManifest( + scene=SceneManifest.from_registry(registry), + robot_profile=profile, + call_catalog=builtin_semantic_call_catalog(), + ) + engine = _engine(profile) + provider = _DualCenterHandOverProvider() + compiler = SemanticSkillCompiler( + manifest.bind(registry, engine), + relation_grounders=(_FrameRelationGrounder(),), + handover_pose_providers=(provider,), + ) + final_target = SemanticPose( + (0.8, 0.0, 0.4), + (1.0, 0.0, 0.0, 0.0), + ) + workflow = compiler.analyze( + ( + Pick(object=SceneObjectRef("cube")), + HandOver( + object=SceneObjectRef("cube"), + final_target=final_target, + ), + Place( + object=SceneObjectRef("cube"), + on=SceneObjectRef("table"), + resources={"primary": "right"}, + ), + ) + ) + + assert provider.calls == 0 + assert [scene_provider.calls for scene_provider in providers] == [0, 0] + assert workflow.calls[0].downstream_object_targets + pick = compiler.ground(workflow, 0, _context(registry, robot_dof=4)) + assert provider.calls == 1 + pick_options = pick.invocation.skill_options + assert type(pick_options) is PickUpOptions + assert type(pick_options.downstream_object_target_poses[0]) is SceneEntityPose + assert pick_options.downstream_object_target_poses[0].entity_id == "table_top" + + held_context = _held_context( + registry, + pick.invocation.goal.semantics, + torch.eye(4).repeat(2, 1, 1), + task_state_key="left", + robot_dof=4, + ) + handover = compiler.ground(workflow, 1, held_context) + assert provider.calls == 2 + options = handover.invocation.skill_options + assert type(options) is HandOverOptions + assert options.receive_pick_object_part == "top" + assert options.pre_grasp_distance == 0.06 + assert type(options.middle_object_pose) is SceneEntityPose + assert options.middle_object_pose.entity_id == "table_top" + assert options.final_object_pose[0, 3].item() == pytest.approx(0.8) + request = engine.resolve(handover.invocation) + action = engine.actions["hand_over"] + assert action._scene_dependencies(request) == ("table_top",) + action._resolve_start_qpos = Mock( # type: ignore[method-assign] + return_value=(torch.zeros(2, 1), torch.zeros(2, 1)) + ) + captured: list[torch.Tensor] = [] + original_resolve_matrix = action._resolve_matrix + + def capture_middle(matrix: torch.Tensor, name: str) -> torch.Tensor: + if name == "middle_object_pose": + captured.append(matrix.clone()) + raise RuntimeError("captured target") + return original_resolve_matrix(matrix, name) + + action._resolve_matrix = capture_middle # type: ignore[method-assign] + with pytest.raises(RuntimeError, match="captured target"): + engine.plan_request(request, held_context) + moved_table_pose = torch.eye(4).repeat(2, 1, 1) + moved_table_pose[:, 0, 3] = 0.9 + providers[1].pose = moved_table_pose + moved_context = _held_context( + registry, + pick.invocation.goal.semantics, + torch.eye(4).repeat(2, 1, 1), + task_state_key="left", + robot_dof=4, + ) + with pytest.raises(RuntimeError, match="captured target"): + engine.plan_request(request, moved_context) + + assert captured[0][0, 0, 3].item() == pytest.approx(0.6) + assert captured[1][0, 0, 3].item() == pytest.approx(0.9) + + +def test_handover_requires_profile_selection_and_installed_provider() -> None: + registry, _ = _scene_registry() + call = HandOver(object=SceneObjectRef("cube")) + + unconfigured_profile = _dual_profile(provider_id=None) + unconfigured_manifest = SemanticIntegrationManifest( + scene=SceneManifest.from_registry(registry), + robot_profile=unconfigured_profile, + call_catalog=builtin_semantic_call_catalog(), + ) + unconfigured_engine = _engine(unconfigured_profile) + unconfigured = SemanticSkillCompiler( + unconfigured_manifest.bind(registry, unconfigured_engine) + ) + with pytest.raises(SemanticValidationError) as unconfigured_error: + unconfigured.analyze((call,)) + assert unconfigured_error.value.diagnostic.code == "handover_grounding_unconfigured" + + missing_profile = _dual_profile(provider_id="not_installed") + missing_manifest = SemanticIntegrationManifest( + scene=SceneManifest.from_registry(registry), + robot_profile=missing_profile, + call_catalog=builtin_semantic_call_catalog(), + ) + missing_engine = _engine(missing_profile) + missing = SemanticSkillCompiler(missing_manifest.bind(registry, missing_engine)) + with pytest.raises(SemanticValidationError) as missing_error: + missing.analyze((call,)) + assert ( + missing_error.value.diagnostic.code + == "handover_grounding_provider_not_installed" + ) + + +def test_relation_call_requires_exact_typed_versioned_grounder() -> None: + registry, _ = _scene_registry() + compiler, _ = _compiler(registry, relation_grounders=()) + + with pytest.raises(SemanticValidationError) as error: + compiler.analyze( + ( + Place( + object=SceneObjectRef("cube"), + on=SceneObjectRef("table"), + ), + ) + ) + + assert error.value.diagnostic.code == "relation_grounder_not_installed" + + +def test_place_uses_verified_object_to_eef_transform() -> None: + registry, _ = _scene_registry() + templates = _action_option_templates() + templates["place"] = PlaceOptions( + lift_height=0.22, + cartesian_waypoint_count=3, + ) + compiler, engine = _compiler( + registry, + profile=_profile( + preset=_preset("safe", action_option_templates=templates), + ), + ) + drop = SemanticPose((0.5, -0.2, 0.4), (1.0, 0.0, 0.0, 0.0)) + workflow = compiler.analyze((Place(object=SceneObjectRef("cube"), at=drop),)) + pick_workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + semantics = compiler.ground( + pick_workflow, + 0, + _context(registry), + ).invocation.goal.semantics + object_to_eef = torch.eye(4).repeat(2, 1, 1) + object_to_eef[:, 2, 3] = 0.12 + context = _held_context(registry, semantics, object_to_eef) + + grounded = compiler.ground(workflow, 0, context) + + assert type(grounded.invocation.goal) is PlaceGoal + options = grounded.invocation.skill_options + assert type(options) is PlaceOptions + assert options.lift_height == 0.22 + assert options.cartesian_waypoint_count == 3 + expected = torch.bmm(drop.to_matrix().repeat(2, 1, 1), object_to_eef) + torch.testing.assert_close(grounded.invocation.goal.xpos, expected) + engine.resolve(grounded.invocation) + + +def test_relation_place_composes_late_target_with_verified_transform() -> None: + registry, _ = _scene_registry() + compiler, _ = _compiler(registry) + pick_workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + semantics = compiler.ground( + pick_workflow, + 0, + _context(registry), + ).invocation.goal.semantics + object_to_eef = torch.eye(4).repeat(2, 1, 1) + object_to_eef[:, 0, 3] = 0.08 + context = _held_context(registry, semantics, object_to_eef) + workflow = compiler.analyze( + ( + Place( + object=SceneObjectRef("cube"), + on=SceneObjectRef("table"), + ), + ) + ) + + grounded = compiler.ground(workflow, 0, context) + + goal = grounded.invocation.goal + assert type(goal) is PlaceGoal + assert type(goal.xpos) is SceneEntityPose + assert goal.xpos.entity_id == "table_top" + torch.testing.assert_close(goal.xpos.relative_pose, object_to_eef) + + +def test_place_rejects_wrong_or_inactive_verified_holder() -> None: + registry, _ = _scene_registry() + compiler, _ = _compiler(registry) + workflow = compiler.analyze( + ( + Place( + object=SceneObjectRef("cube"), + at=SemanticPose((0.0, 0.0, 0.0), (1.0, 0.0, 0.0, 0.0)), + ), + ) + ) + wrong = ObjectSemantics( + affordance=AntipodalAffordance(), + geometry={}, + entity_id="other", + ) + wrong_context = _held_context( + registry, + wrong, + torch.eye(4).repeat(2, 1, 1), + ) + + with pytest.raises(SemanticValidationError) as wrong_error: + compiler.ground(workflow, 0, wrong_context) + assert wrong_error.value.diagnostic.code == "verified_held_object_required" + assert wrong_error.value.diagnostic.rendered_path == "workflow[0].call.object" + + pick_workflow = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + semantics = compiler.ground( + pick_workflow, + 0, + _context(registry), + ).invocation.goal.semantics + partial_context = _held_context( + registry, + semantics, + torch.eye(4).repeat(2, 1, 1), + env_mask=torch.tensor([True, False]), + ) + with pytest.raises(SemanticValidationError) as inactive_error: + compiler.ground(workflow, 0, partial_context) + assert inactive_error.value.diagnostic.code == "verified_held_object_required" + assert inactive_error.value.diagnostic.rendered_path == "workflow[0].call.object" + + grounded = compiler.ground( + workflow, + 0, + partial_context, + eligible_mask=torch.tensor([True, False]), + ) + assert grounded.eligible_mask.tolist() == [True, False] + + +def test_registered_lowerer_is_explicit_and_opaque_to_lookahead() -> None: + registry, _ = _scene_registry() + without_lowerer, _ = _compiler(registry, registered=True) + registered = RegisteredSemanticCall(call_id="vendor.inspect") + + with pytest.raises(SemanticValidationError) as error: + without_lowerer.analyze((registered,)) + assert error.value.diagnostic.code == "semantic_lowerer_not_installed" + + compiler, engine = _compiler( + registry, + registered=True, + registered_lowerers=(_InspectLowerer(),), + ) + workflow = compiler.analyze( + ( + Pick(object=SceneObjectRef("cube")), + registered, + Place( + object=SceneObjectRef("cube"), + at=SemanticPose((0.3, 0.0, 0.2), (1.0, 0.0, 0.0, 0.0)), + ), + ) + ) + + assert workflow.calls[0].downstream_object_targets == () + assert workflow.effect_dependencies[0].producer_index is None + grounded = compiler.ground(workflow, 1, _context(registry)) + assert grounded.invocation.skill_id == "pick_up" + engine.resolve(grounded.invocation) + + +@pytest.mark.parametrize( + ("output", "message"), + (("goal", "produced"), ("options", "must not return skill_options")), +) +def test_registered_lowerer_cannot_replace_owned_contracts( + output: str, + message: str, +) -> None: + registry, _ = _scene_registry() + compiler, _ = _compiler( + registry, + registered=True, + registered_lowerers=(_SubclassOutputLowerer(output),), + ) + workflow = compiler.analyze((RegisteredSemanticCall(call_id="vendor.inspect"),)) + + with pytest.raises(TypeError, match=message): + compiler.ground(workflow, 0, _context(registry)) + + +def test_workflow_cannot_cross_compilers() -> None: + registry, _ = _scene_registry() + first, _ = _compiler(registry) + second, _ = _compiler(registry) + workflow = first.analyze((Pick(object=SceneObjectRef("cube")),)) + + with pytest.raises(SemanticValidationError) as error: + second.ground(workflow, 0, _context(registry)) + assert error.value.diagnostic.code == "semantic_program_stale" diff --git a/tests/sim/skills/test_curobo_semantic_runtime_dynamic_recovery_gpu.py b/tests/sim/skills/test_curobo_semantic_runtime_dynamic_recovery_gpu.py new file mode 100644 index 000000000..90b4a0924 --- /dev/null +++ b/tests/sim/skills/test_curobo_semantic_runtime_dynamic_recovery_gpu.py @@ -0,0 +1,384 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Real semantic-runtime recovery gate for a dynamic cuRobo collision world.""" + +from __future__ import annotations + +from typing import ClassVar + +import pytest +import torch + +# Module-level guards must precede cuRobo-only imports. +pytest.importorskip("curobo") +if not torch.cuda.is_available(): + pytest.skip("cuRobo V2 requires CUDA", allow_module_level=True) + +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg # noqa: E402 +from embodichain.lab.sim.atomic_actions import ( # noqa: E402 + CARTESIAN_POSE_CAPABILITY, + AtomicActionEngine, + CommandAcknowledgement, + DynamicCollisionMode, + EndEffectorPoseGoal, + ExecutionEventKind, + ExecutionRunnerCfg, + MotionPolicy, + MoveEndEffector, + MoveEndEffectorOptions, + PlanningContext, + RecoveryPolicy, + RuntimeCommandFrame, + RuntimeEndpointTarget, + SimulationExecutionAdapter, + SkillDescriptor, +) +from embodichain.lab.sim.atomic_actions.tracking import TrackingPolicy # noqa: E402 +from embodichain.lab.sim.cfg import RigidBodyAttributesCfg # noqa: E402 +from embodichain.lab.sim.objects import RigidObjectCfg # noqa: E402 +from embodichain.lab.sim.planners import MotionGenCfg, MotionGenerator # noqa: E402 +from embodichain.lab.sim.planners.curobo.curobo_planner import ( # noqa: E402 + CuroboAutoGenCfg, + CuroboPlannerCfg, + CuroboWorldCfg, +) +from embodichain.lab.sim.robots import FrankaPandaCfg # noqa: E402 +from embodichain.lab.sim.shapes import CubeCfg # noqa: E402 +from embodichain.lab.sim.skills import ( # noqa: E402 + BoundSemanticCall, + ControlPartEndpoint, + EffectEvidenceCollector, + EffectEvidenceProviderRegistry, + RegisteredSemanticCall, + RegisteredSemanticLowerer, + RobotResource, + RobotSkillProfile, + SceneCollisionRole, + SceneCollisionWorldMode, + SceneManifest, + SceneRegistry, + SemanticCallDescriptor, + SemanticIntegrationManifest, + SemanticLowering, + SemanticSkillCompiler, + SkillPolicyPreset, + SkillRuntime, + SkillStatus, + builtin_semantic_call_catalog, +) + +pytestmark = [ + pytest.mark.requires_sim, + pytest.mark.gpu, + pytest.mark.slow, +] + +ROBOT_UID = "semantic_dynamic_scene_franka" +OBSTACLE_UID = "semantic_dynamic_obstacle" +CONTROL_PART = "arm" +CALL_ID = "test.move_end_effector" +SAMPLE_COUNT = 80 +COMMAND_CYCLE_TIME = 0.1 +MOVE_AFTER_COMMAND = 12 +OBSTACLE_SIZE = [0.10, 0.10, 0.12] +OBSTACLE_START_POSITION = [0.59, -0.20, 0.455] +MAXIMUM_FINAL_EEF_ERROR = 0.04 + +_MOVE_TARGET = MoveEndEffector.descriptor() +assert _MOVE_TARGET.binding_contract is not None + + +class _MoveEndEffectorLowerer(RegisteredSemanticLowerer): + """Lower a declarative matrix into the built-in Cartesian motion goal.""" + + call_id: ClassVar[str] = CALL_ID + schema_version: ClassVar[int] = 1 + target_descriptor: ClassVar[SkillDescriptor] = _MOVE_TARGET + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + option_template: MoveEndEffectorOptions, + ) -> SemanticLowering: + del bound, option_template + values = call.arguments.get("xpos") + if type(values) is not tuple or len(values) != 16: + raise ValueError("xpos must contain one flattened 4x4 pose matrix.") + pose = torch.tensor( + values, + dtype=context.robot.qpos.dtype, + device=context.robot.qpos.device, + ).reshape(4, 4) + return SemanticLowering(goal=EndEffectorPoseGoal(xpos=pose)) + + +class _CountingCommandSink: + """Count accepted real-simulation command frames while delegating transport.""" + + def __init__(self, delegate: SimulationExecutionAdapter) -> None: + self.delegate = delegate + self.command_count = 0 + + def send( + self, + command: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + acknowledgement = self.delegate.send(command, timeout=timeout) + if acknowledgement.accepted: + self.command_count += 1 + return acknowledgement + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + return self.delegate.hold(targets, context, timeout=timeout) + + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + return self.delegate.cancel(targets, timeout=timeout) + + +def _profile() -> RobotSkillProfile: + """Declare the exact robot resource and bounded safe recovery policy.""" + return RobotSkillProfile( + profile_id="semantic_dynamic_scene_franka", + resources={ + "manipulator": RobotResource( + resource_id="manipulator", + endpoints={ + "motion": ControlPartEndpoint( + control_part=CONTROL_PART, + capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + ) + }, + ) + }, + presets={ + "safe": SkillPolicyPreset( + "safe", + action_option_templates={ + CALL_ID: MoveEndEffectorOptions(), + }, + motion_policy=MotionPolicy( + strategy="motion_gen", + sample_count=SAMPLE_COUNT, + control_dt=COMMAND_CYCLE_TIME, + ), + tracking_policy=TrackingPolicy.joint_position( + in_flight_max_abs_error=0.1, + terminal_max_abs_error=0.1, + ), + recovery_policy=RecoveryPolicy( + max_replans=2, + action_timeout=30.0, + ), + runner_cfg=ExecutionRunnerCfg(minimum_cycle_time=COMMAND_CYCLE_TIME), + ) + }, + default_preset="safe", + ) + + +def _compiler( + registry: SceneRegistry, + engine: AtomicActionEngine, +) -> SemanticSkillCompiler: + """Bind the test semantic extension to the real engine and scene registry.""" + catalog = builtin_semantic_call_catalog().with_descriptor( + SemanticCallDescriptor( + call_id=CALL_ID, + spec_type=RegisteredSemanticCall, + skill_id=_MOVE_TARGET.skill_id, + binding_contract=_MOVE_TARGET.binding_contract, + target_descriptor=_MOVE_TARGET, + ) + ) + integration = SemanticIntegrationManifest( + scene=SceneManifest.from_registry(registry), + robot_profile=_profile(), + call_catalog=catalog, + ).bind(registry, engine) + return SemanticSkillCompiler( + integration, + registered_lowerers=(_MoveEndEffectorLowerer(),), + ) + + +def test_semantic_runtime_replans_after_dynamic_curobo_world_change() -> None: + """Run semantic lowering, real cuRobo planning, world update, and recovery.""" + sim = SimulationManager( + SimulationManagerCfg(headless=True, sim_device="cuda", num_envs=1) + ) + planner = None + try: + robot = sim.add_robot( + cfg=FrankaPandaCfg.from_dict({"uid": ROBOT_UID, "robot_type": "panda"}) + ) + obstacle = sim.add_rigid_object( + cfg=RigidObjectCfg( + uid=OBSTACLE_UID, + shape=CubeCfg(size=OBSTACLE_SIZE), + attrs=RigidBodyAttributesCfg(), + body_type="kinematic", + init_pos=OBSTACLE_START_POSITION, + init_rot=[0.0, 0.0, 0.0], + ) + ) + sim.update(step=10) + + motion_generator = MotionGenerator( + MotionGenCfg( + planner_cfg=CuroboPlannerCfg( + robot_uid=ROBOT_UID, + auto_gen=CuroboAutoGenCfg( + fit_type="morphit", + sphere_density=0.3, + collision_sphere_buffer=0.005, + ), + world=CuroboWorldCfg( + rigid_objects=[obstacle], + obstacle_representation="cuboid", + dynamic_obstacle_names=[OBSTACLE_UID], + multi_env=False, + ), + warmup_iterations=0, + ) + ) + ) + planner = motion_generator.planner + registry = SceneRegistry.from_simulation( + sim, + rigid_objects={OBSTACLE_UID: OBSTACLE_UID}, + collision_roles={OBSTACLE_UID: SceneCollisionRole.DYNAMIC}, + collision_world_mode=SceneCollisionWorldMode.SHARED, + ) + scene_provider = registry.make_planning_scene_provider( + motion_generator, + batch_size=1, + ) + adapter = SimulationExecutionAdapter( + sim, + robot, + scene_provider=scene_provider, + ) + sink = _CountingCommandSink(adapter) + engine = AtomicActionEngine(motion_generator) + runtime = SkillRuntime.from_components( + _compiler(registry, engine), + adapter, + sink, + EffectEvidenceCollector(EffectEvidenceProviderRegistry()), + clock=adapter, + ) + + start_pose = robot.compute_fk( + qpos=robot.get_qpos(name=CONTROL_PART), + name=CONTROL_PART, + to_matrix=True, + ) + target_pose = start_pose.clone() + target_pose[:, :3, 3] += torch.tensor( + [0.22, 0.24, 0.12], + dtype=target_pose.dtype, + device=target_pose.device, + ) + call = RegisteredSemanticCall( + call_id=CALL_ID, + arguments={ + "xpos": tuple( + float(value) + for value in target_pose[0].detach().cpu().reshape(-1).tolist() + ) + }, + resources={"primary": "manipulator"}, + ) + + result = runtime.start(call, workflow_id="dynamic_curobo_recovery") + assert result.status is SkillStatus.RUNNING + obstacle_moved = False + for _ in range(2_000): + if result.terminal: + break + if result.wait_duration > 0.0: + adapter.sleep(result.wait_duration) + result = runtime.step() + if not obstacle_moved and sink.command_count >= MOVE_AFTER_COMMAND: + blocking_pose = obstacle.get_local_pose(to_matrix=True).clone() + blocking_pose[:, :3, 3] = 0.5 * ( + start_pose[:, :3, 3] + target_pose[:, :3, 3] + ) + obstacle.set_local_pose(blocking_pose) + adapter.sleep(adapter.physics_dt) + obstacle_moved = True + + assert obstacle_moved + assert result.status is SkillStatus.COMPLETED, result.message + assert result.success_mask.tolist() == [True] + assert len(result.calls) == 1 + trace = result.calls[0] + assert trace.semantic_id == CALL_ID + assert trace.skill_id == MoveEndEffector.skill_id + assert len(trace.plan_attempts) >= 2 + + event_kinds = tuple(event.kind for event in result.events) + assert ExecutionEventKind.COLLISION_WORLD_CHANGED in event_kinds + assert ExecutionEventKind.REPLANNED in event_kinds + + initial_attempt = trace.plan_attempts[0] + changed_attempts = tuple( + attempt + for attempt in trace.plan_attempts[1:] + if attempt.planned_collision_world_revision[0] + > initial_attempt.planned_collision_world_revision[0] + ) + assert changed_attempts + assert changed_attempts[0].trigger == ExecutionEventKind.REPLANNED.value + assert changed_attempts[0].planner_backend == "curobo" + assert initial_attempt.collision_world_sensitive + assert ( + initial_attempt.resolved_core_policy.motion_policy.dynamic_collision_mode + is DynamicCollisionMode.REQUIRED + ) + + final_pose = robot.compute_fk( + qpos=robot.get_qpos(name=CONTROL_PART), + name=CONTROL_PART, + to_matrix=True, + ) + final_error = torch.linalg.vector_norm( + final_pose[:, :3, 3] - target_pose[:, :3, 3], + dim=1, + ) + assert bool((final_error < MAXIMUM_FINAL_EEF_ERROR).all().item()) + finally: + if planner is not None: + planner.close() + sim.destroy() + SimulationManager.flush_cleanup_queue() diff --git a/tests/sim/skills/test_effects.py b/tests/sim/skills/test_effects.py new file mode 100644 index 000000000..72e479ea4 --- /dev/null +++ b/tests/sim/skills/test_effects.py @@ -0,0 +1,1163 @@ +# ---------------------------------------------------------------------------- +# 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 typed semantic-effect contracts and raw evidence monitors.""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import dataclass, replace +import json +import math +from types import MappingProxyType + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + Affordance, + ArticulationJointState, + EffectVerificationRequest, + HeldObjectState, + ObjectSemantics, + StateDelta, +) +from embodichain.lab.sim.skills.effects import ( + ArticulationJointStateExpectation, + BinaryEffectClause, + BinaryEffectEvidenceBatch, + BinaryEvidenceKind, + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + CompositeEffectMonitor, + CompositeEffectMonitorCfg, + CompositeEffectMonitorFactory, + CoordinatedHeldObjectCleanupExpectation, + EffectEvidenceAddress, + EffectEvidenceBatch, + EffectEvidenceSourceRef, + EffectExpectationDecision, + EffectMonitor, + EffectMonitorDecision, + EffectMonitorFactory, + EffectMonitorRef, + EffectMonitorRegistry, + HeldObjectRelation, + HeldObjectStateExpectation, + JointStateEffectClause, + JointStateEvidenceBatch, + PoseRelationClause, + PoseRelationEvidenceBatch, + PoseRelationExpectation, + ScalarEffectClause, + ScalarEffectEvidenceBatch, + ScalarEvidenceKind, + ScalarExpectation, + SemanticEffectKind, + SemanticEffectSpec, +) + +_ENV_IDS = torch.tensor([101, 205, 309], dtype=torch.long) +_OBJECT_ID = "scene/cube" +_STATE_KEY = "left_actor" +_SOURCE_STATE_KEY = "source_actor" +_DESTINATION_STATE_KEY = "destination_actor" +_SKILL_ID = "pick_up" +_INVOCATION_ID = "call-7" +_ATTACHED_OFFSET = 0.0 +_DETACHED_OFFSET = 0.1 # Above the built-in 0.06 translation threshold. +_UNRESOLVED_OFFSET = 0.04 # Between the attached and detached thresholds. + + +@dataclass(frozen=True, slots=True) +class _EvidenceAddress(EffectEvidenceAddress): + """Minimal custom observation address used by contract tests.""" + + endpoint: str + channel: str + + @property + def address_fingerprint(self) -> tuple[type, str, str]: + return type(self), self.endpoint, self.channel + + +class _AliasingAddress(_EvidenceAddress): + """Address intentionally violating snapshot ownership.""" + + def snapshot(self) -> EffectEvidenceAddress: + return self + + +def _source(channel: str) -> EffectEvidenceSourceRef: + return EffectEvidenceSourceRef( + "test.raw_evidence", + "1", + _EvidenceAddress("left_actor", channel), + ) + + +def _poses(*x_offsets: float) -> torch.Tensor: + poses = torch.eye(4).repeat(len(x_offsets), 1, 1) + poses[:, 0, 3] = torch.tensor(x_offsets) + return poses + + +def _semantics(object_id: str = _OBJECT_ID) -> ObjectSemantics: + return ObjectSemantics( + affordance=Affordance(), + geometry={}, + label="object", + entity_id=object_id, + ) + + +def _held( + *, + object_id: str = _OBJECT_ID, + baseline: torch.Tensor | None = None, + env_mask: torch.Tensor | None = None, +) -> HeldObjectState: + poses = _poses(0.0, 0.0, 0.0) if baseline is None else baseline + if env_mask is None: + env_mask = torch.ones(3, dtype=torch.bool) + return HeldObjectState( + semantics=_semantics(object_id), + object_to_eef=poses, + grasp_xpos=_poses(0.0, 0.0, 0.0), + env_mask=env_mask, + ) + + +def _expectation( + relation: HeldObjectRelation = HeldObjectRelation.ATTACHED, + *, + expectation_id: str = "destination", + state_key: str = _STATE_KEY, +) -> HeldObjectStateExpectation: + return HeldObjectStateExpectation( + expectation_id=expectation_id, + relation=relation, + object_id=_OBJECT_ID, + slot_id="primary", + resource_id="left_actor", + task_state_key=state_key, + ) + + +def _attach_spec() -> SemanticEffectSpec: + return SemanticEffectSpec( + semantic_id="pick", + effect_kind=SemanticEffectKind.ATTACH, + skill_id=_SKILL_ID, + invocation_id=_INVOCATION_ID, + invocation_revision=2, + env_ids=_ENV_IDS, + state_expectations=(_expectation(),), + clauses=( + PoseRelationClause( + "destination.pose", + "destination", + _source("pose_relation"), + PoseRelationExpectation.MATCHED, + ), + BinaryEffectClause( + "destination.constraint", + "destination", + _source("constraint"), + BinaryEvidenceKind.CONSTRAINT, + True, + ), + ), + ) + + +def _transfer_spec() -> SemanticEffectSpec: + source = _expectation( + HeldObjectRelation.DETACHED, + expectation_id="source", + state_key=_SOURCE_STATE_KEY, + ) + destination = _expectation( + expectation_id="destination", + state_key=_DESTINATION_STATE_KEY, + ) + return SemanticEffectSpec( + semantic_id="hand_over", + effect_kind=SemanticEffectKind.TRANSFER, + skill_id=_SKILL_ID, + invocation_id=_INVOCATION_ID, + invocation_revision=2, + env_ids=_ENV_IDS, + state_expectations=(source, destination), + clauses=( + PoseRelationClause( + "source.pose", + "source", + _source("source_pose_relation"), + PoseRelationExpectation.SEPARATED, + baseline_object_to_endpoint=_poses(0.0, 0.0, 0.0), + ), + BinaryEffectClause( + "source.constraint", + "source", + _source("source_constraint"), + BinaryEvidenceKind.CONSTRAINT, + False, + ), + PoseRelationClause( + "destination.pose", + "destination", + _source("destination_pose_relation"), + PoseRelationExpectation.MATCHED, + ), + BinaryEffectClause( + "destination.constraint", + "destination", + _source("destination_constraint"), + BinaryEvidenceKind.CONSTRAINT, + True, + ), + ), + ) + + +def _transfer_request() -> EffectVerificationRequest: + return _request( + effects=StateDelta( + held_object_updates={ + _SOURCE_STATE_KEY: None, + _DESTINATION_STATE_KEY: _held(), + } + ) + ) + + +def _transfer_evidence( + *, + source_offsets: tuple[float, ...], + source_constraints: tuple[bool, ...], + destination_offsets: tuple[float, ...], + destination_constraints: tuple[bool, ...], + timestamp: float, + revision: int, +) -> Mapping[str, EffectEvidenceBatch]: + valid = torch.ones(len(source_offsets), dtype=torch.bool) + errors = tuple(None for _ in source_offsets) + return { + "source.pose": PoseRelationEvidenceBatch( + "source.pose", + _poses(*source_offsets), + valid, + errors, + timestamp, + _ENV_IDS, + revision, + ), + "source.constraint": BinaryEffectEvidenceBatch( + "source.constraint", + BinaryEvidenceKind.CONSTRAINT, + torch.tensor(source_constraints, dtype=torch.bool), + valid, + errors, + timestamp, + _ENV_IDS, + revision, + ), + "destination.pose": PoseRelationEvidenceBatch( + "destination.pose", + _poses(*destination_offsets), + valid, + errors, + timestamp, + _ENV_IDS, + revision, + ), + "destination.constraint": BinaryEffectEvidenceBatch( + "destination.constraint", + BinaryEvidenceKind.CONSTRAINT, + torch.tensor(destination_constraints, dtype=torch.bool), + valid, + errors, + timestamp, + _ENV_IDS, + revision, + ), + } + + +def _request( + *, + env_mask: torch.Tensor | None = None, + attempt_generation: int = 0, + verification_id: int = 1, + effects: StateDelta | None = None, +) -> EffectVerificationRequest: + if env_mask is None: + env_mask = torch.ones(3, dtype=torch.bool) + if effects is None: + effects = StateDelta(held_object_updates={_STATE_KEY: _held()}) + return EffectVerificationRequest( + verification_id=verification_id, + skill_id=_SKILL_ID, + invocation_id=_INVOCATION_ID, + invocation_revision=2, + invocation_index=0, + attempt_generation=attempt_generation, + terminal_segment="close", + requested_at=1.0, + deadline=10.0, + env_mask=env_mask, + expected_effects=effects, + ) + + +def _pose_evidence( + offsets: tuple[float, ...], + *, + timestamp: float, + env_ids: torch.Tensor = _ENV_IDS, + valid: torch.Tensor | None = None, + revision: int = 4, +) -> PoseRelationEvidenceBatch: + if valid is None: + valid = torch.ones(len(offsets), dtype=torch.bool) + return PoseRelationEvidenceBatch( + evidence_id="destination.pose", + object_to_endpoint=_poses(*offsets), + valid=valid, + acquisition_errors=tuple( + None if row_valid else "pose unavailable" for row_valid in valid + ), + timestamp=timestamp, + env_ids=env_ids, + observation_revision=revision, + ) + + +def _binary_evidence( + values: tuple[bool, ...], + *, + timestamp: float, + env_ids: torch.Tensor = _ENV_IDS, + valid: torch.Tensor | None = None, + revision: int = 4, +) -> BinaryEffectEvidenceBatch: + if valid is None: + valid = torch.ones(len(values), dtype=torch.bool) + return BinaryEffectEvidenceBatch( + evidence_id="destination.constraint", + evidence_kind=BinaryEvidenceKind.CONSTRAINT, + values=torch.tensor(values, dtype=torch.bool), + valid=valid, + acquisition_errors=tuple( + None if row_valid else "constraint unavailable" for row_valid in valid + ), + timestamp=timestamp, + env_ids=env_ids, + observation_revision=revision, + ) + + +def _evidence( + offsets: tuple[float, ...], + constraints: tuple[bool, ...], + *, + timestamp: float, + env_ids: torch.Tensor = _ENV_IDS, + valid: torch.Tensor | None = None, + revision: int = 4, +) -> Mapping[str, object]: + return { + "destination.pose": _pose_evidence( + offsets, + timestamp=timestamp, + env_ids=env_ids, + valid=valid, + revision=revision, + ), + "destination.constraint": _binary_evidence( + constraints, + timestamp=timestamp, + env_ids=env_ids, + valid=valid, + revision=revision, + ), + } + + +def test_monitor_ref_owns_bounded_non_executable_params() -> None: + params = {"limits": [1, {"enabled": True}]} + ref = EffectMonitorRef("monitor", "v1", params) + params["limits"][1]["enabled"] = False # type: ignore[index] + + assert isinstance(ref.params, MappingProxyType) + assert ref.params["limits"] == (1, MappingProxyType({"enabled": True})) + assert ref.snapshot().params is not ref.params + + +@pytest.mark.parametrize("value", [torch.tensor(1.0), lambda: None, math.inf]) +def test_monitor_ref_rejects_live_or_nonfinite_params(value: object) -> None: + with pytest.raises((TypeError, ValueError)): + EffectMonitorRef("monitor", "v1", {"bad": value}) + + +def test_monitor_ref_rejects_cyclic_params() -> None: + params: dict[str, object] = {} + params["cycle"] = params + + with pytest.raises(ValueError, match="cyclic"): + EffectMonitorRef("monitor", "v1", params) + + +def test_evidence_source_is_independent_from_runtime_command_addresses() -> None: + address = _EvidenceAddress("left_actor", "pose_relation") + source = EffectEvidenceSourceRef("provider", "2", address) + + assert source.address is not address + assert source.source_fingerprint == ( + "provider", + "2", + _EvidenceAddress, + (_EvidenceAddress, "left_actor", "pose_relation"), + ) + assert not hasattr(source, "transport_id") + + +def test_evidence_source_enforces_snapshot_ownership() -> None: + with pytest.raises(TypeError, match="independently owned"): + EffectEvidenceSourceRef( + "provider", + "1", + _AliasingAddress("left_actor", "pose_relation"), + ) + + +def test_semantic_spec_owns_typed_state_and_heterogeneous_clauses() -> None: + env_ids = _ENV_IDS.clone() + spec = _attach_spec() + env_ids[0] = -1 + + assert torch.equal(spec.env_ids, _ENV_IDS) + assert type(spec.state_expectations[0]) is HeldObjectStateExpectation + assert tuple(type(clause) for clause in spec.clauses) == ( + PoseRelationClause, + BinaryEffectClause, + ) + assert spec.snapshot().clauses[0] is not spec.clauses[0] + + +def test_spec_rejects_clause_without_typed_state_expectation() -> None: + with pytest.raises(ValueError, match="unknown state expectations"): + replace( + _attach_spec(), + clauses=(replace(_attach_spec().clauses[0], expectation_id="missing"),), + ) + + +def test_articulation_and_joint_clause_are_first_class_typed_contracts() -> None: + target = torch.tensor([0.42]) + spec = SemanticEffectSpec( + semantic_id="operate_articulation", + effect_kind=SemanticEffectKind.ARTICULATION, + skill_id="operate_articulation", + invocation_id="drawer-1", + invocation_revision=0, + env_ids=_ENV_IDS, + state_expectations=( + ArticulationJointStateExpectation( + "drawer_joint", + "drawer", + "slide", + target, + ), + ), + clauses=( + JointStateEffectClause( + "drawer_joint.position", + "drawer_joint", + _source("joint_state"), + target, + ), + ), + ) + target.fill_(9.0) + + expectation = spec.state_expectations[0] + clause = spec.clauses[0] + assert isinstance(expectation, ArticulationJointStateExpectation) + assert isinstance(clause, JointStateEffectClause) + torch.testing.assert_close(expectation.target_position, torch.tensor([0.42])) + torch.testing.assert_close(clause.target_position, torch.tensor([0.42])) + + request = EffectVerificationRequest( + verification_id=1, + skill_id="operate_articulation", + invocation_id="drawer-1", + invocation_revision=0, + invocation_index=0, + attempt_generation=0, + terminal_segment="operate", + requested_at=1.0, + deadline=10.0, + env_mask=torch.ones(3, dtype=torch.bool), + expected_effects=StateDelta( + articulation_joint_updates={ + ("drawer", "slide"): ArticulationJointState(torch.tensor([0.42])) + } + ), + ) + spec.validate_request(request) + + wrong = replace( + request, + expected_effects=StateDelta( + articulation_joint_updates={ + ("drawer", "slide"): ArticulationJointState(torch.tensor([0.7])) + } + ), + ) + with pytest.raises(ValueError, match="target position"): + spec.validate_request(wrong) + + +def test_request_validation_uses_logical_state_key() -> None: + _attach_spec().validate_request(_request()) + + wrong_key = _request( + effects=StateDelta(held_object_updates={"arm_control_part": _held()}) + ) + with pytest.raises(ValueError, match="exactly match"): + _attach_spec().validate_request(wrong_key) + + +def test_request_validation_declares_coordinated_cleanup_explicitly() -> None: + cleanup = CoordinatedHeldObjectCleanupExpectation( + "cleanup:left_actor:support", + (_STATE_KEY, "support"), + ) + spec = replace( + _attach_spec(), + state_expectations=(*_attach_spec().state_expectations, cleanup), + ) + request = _request( + effects=StateDelta( + held_object_updates={_STATE_KEY: _held()}, + coordinated_held_object_updates={(_STATE_KEY, "support"): None}, + ) + ) + + spec.validate_request(request) + + +def test_pose_evidence_owns_rows_and_allows_invalid_nonfinite_payload() -> None: + poses = _poses(0.0, 0.1) + poses[1].fill_(math.nan) + valid = torch.tensor([True, False]) + batch = PoseRelationEvidenceBatch( + "pose", + poses, + valid, + (None, "occluded"), + 2.0, + torch.tensor([101, 205]), + 3, + ) + poses.zero_() + valid.fill_(True) + + assert torch.isnan(batch.object_to_endpoint[1]).all() + assert batch.valid.tolist() == [True, False] + + +def test_effect_contract_evidence_and_resolved_thresholds_are_json_safe() -> None: + poses = _poses(0.0, 0.1) + poses[1].fill_(math.nan) + batch = PoseRelationEvidenceBatch( + "pose", + poses, + torch.tensor([True, False]), + (None, "occluded"), + 2.0, + torch.tensor([101, 205]), + 3, + ) + monitor = CompositeEffectMonitor( + _attach_spec(), + CompositeEffectMonitorCfg(consecutive_samples=3), + ) + + metadata = { + "spec": _attach_spec().to_metadata(), + "evidence": batch.to_metadata(), + "thresholds": dict(monitor.resolved_params), + } + + json.dumps(metadata, allow_nan=False, sort_keys=True) + assert metadata["evidence"]["object_to_endpoint"][1][0][0] is None + assert metadata["thresholds"]["attached_translation_threshold"] == 0.02 + assert metadata["thresholds"]["consecutive_samples"] == 3 + + +def test_binary_scalar_and_joint_evidence_are_distinct_raw_batches() -> None: + valid = torch.tensor([True, True]) + env_ids = torch.tensor([101, 205]) + binary = BinaryEffectEvidenceBatch( + "contact", + BinaryEvidenceKind.CONTACT, + torch.tensor([True, False]), + valid, + (None, None), + 2.0, + env_ids, + 3, + ) + scalar = ScalarEffectEvidenceBatch( + "force", + ScalarEvidenceKind.FORCE, + torch.tensor([2.0, 0.0]), + valid, + (None, None), + 2.0, + env_ids, + 3, + ) + joint = JointStateEvidenceBatch( + "joint", + torch.tensor([[0.4], [0.5]]), + torch.zeros(2, 1), + valid, + (None, None), + 2.0, + env_ids, + 3, + ) + + assert binary.values.dtype == torch.bool + assert scalar.values.tolist() == [2.0, 0.0] + assert joint.positions.shape == (2, 1) + + +def test_valid_raw_evidence_rejects_nonfinite_payload() -> None: + with pytest.raises(ValueError, match="finite"): + ScalarEffectEvidenceBatch( + "force", + ScalarEvidenceKind.FORCE, + torch.tensor([math.nan]), + torch.tensor([True]), + (None,), + 2.0, + torch.tensor([101]), + 3, + ) + + +def test_expectation_decision_owns_all_outcome_masks() -> None: + satisfied = torch.tensor([True, False, False]) + contradicted = torch.tensor([False, True, False]) + inverse_satisfied = torch.tensor([False, True, False]) + + decision = EffectExpectationDecision( + "source", + satisfied, + contradicted, + inverse_satisfied, + ) + satisfied.zero_() + contradicted.zero_() + inverse_satisfied.zero_() + + assert decision.satisfied_mask.tolist() == [True, False, False] + assert decision.contradicted_mask.tolist() == [False, True, False] + assert decision.inverse_satisfied_mask.tolist() == [False, True, False] + + aggregate = EffectMonitorDecision( + decision.satisfied_mask, + decision.contradicted_mask, + (decision,), + ) + decision.satisfied_mask.zero_() + assert aggregate.expectation_decisions[0].satisfied_mask.tolist() == [ + True, + False, + False, + ] + + +def test_expectation_decision_requires_complete_inverse_to_be_contradicted() -> None: + with pytest.raises(ValueError, match="subset of contradicted_mask"): + EffectExpectationDecision( + "source", + torch.tensor([False]), + torch.tensor([False]), + torch.tensor([True]), + ) + + +def test_transfer_monitor_reports_each_expectation_and_strong_inverse() -> None: + monitor = CompositeEffectMonitor( + _transfer_spec(), + CompositeEffectMonitorCfg(consecutive_samples=1), + ) + + decision = monitor.observe( + _transfer_request(), + _transfer_evidence( + source_offsets=( + _DETACHED_OFFSET, + _ATTACHED_OFFSET, + _DETACHED_OFFSET, + ), + source_constraints=(False, True, False), + destination_offsets=( + _ATTACHED_OFFSET, + _ATTACHED_OFFSET, + _DETACHED_OFFSET, + ), + destination_constraints=(True, True, False), + timestamp=2.0, + revision=4, + ), + ) + + outcomes = { + outcome.expectation_id: outcome for outcome in decision.expectation_decisions + } + assert tuple(outcomes) == ("source", "destination") + assert outcomes["source"].satisfied_mask.tolist() == [True, False, True] + assert outcomes["source"].contradicted_mask.tolist() == [False, True, False] + assert outcomes["source"].inverse_satisfied_mask.tolist() == [False, True, False] + assert outcomes["destination"].satisfied_mask.tolist() == [True, True, False] + assert outcomes["destination"].contradicted_mask.tolist() == [False, False, True] + assert outcomes["destination"].inverse_satisfied_mask.tolist() == [ + False, + False, + True, + ] + assert decision.success_mask.tolist() == [True, False, False] + assert decision.failure_mask.tolist() == [False, True, True] + + +def test_transfer_contradictions_are_counted_per_expectation() -> None: + monitor = CompositeEffectMonitor( + _transfer_spec(), + CompositeEffectMonitorCfg(consecutive_samples=2), + ) + request = _transfer_request() + monitor.observe( + request, + _transfer_evidence( + source_offsets=(_ATTACHED_OFFSET,) * 3, + source_constraints=(True,) * 3, + destination_offsets=(_ATTACHED_OFFSET,) * 3, + destination_constraints=(True,) * 3, + timestamp=2.0, + revision=4, + ), + ) + + alternating = monitor.observe( + request, + _transfer_evidence( + source_offsets=(_DETACHED_OFFSET,) * 3, + source_constraints=(False,) * 3, + destination_offsets=(_DETACHED_OFFSET,) * 3, + destination_constraints=(False,) * 3, + timestamp=3.0, + revision=5, + ), + ) + persistent = monitor.observe( + request, + _transfer_evidence( + source_offsets=(_DETACHED_OFFSET,) * 3, + source_constraints=(False,) * 3, + destination_offsets=(_DETACHED_OFFSET,) * 3, + destination_constraints=(False,) * 3, + timestamp=4.0, + revision=6, + ), + ) + + assert not alternating.failure_mask.any() + assert persistent.failure_mask.all() + persistent_outcomes = { + outcome.expectation_id: outcome for outcome in persistent.expectation_decisions + } + assert persistent_outcomes["source"].satisfied_mask.all() + assert persistent_outcomes["destination"].contradicted_mask.all() + + +def test_transfer_success_never_stitches_expectations_across_ticks() -> None: + monitor = CompositeEffectMonitor( + _transfer_spec(), + CompositeEffectMonitorCfg(consecutive_samples=1), + ) + request = _transfer_request() + source_only = monitor.observe( + request, + _transfer_evidence( + source_offsets=(_DETACHED_OFFSET,) * 3, + source_constraints=(False,) * 3, + destination_offsets=(_UNRESOLVED_OFFSET,) * 3, + destination_constraints=(True,) * 3, + timestamp=2.0, + revision=4, + ), + ) + destination_only = monitor.observe( + request, + _transfer_evidence( + source_offsets=(_UNRESOLVED_OFFSET,) * 3, + source_constraints=(False,) * 3, + destination_offsets=(_ATTACHED_OFFSET,) * 3, + destination_constraints=(True,) * 3, + timestamp=3.0, + revision=5, + ), + ) + + assert not source_only.success_mask.any() + assert not source_only.failure_mask.any() + assert not destination_only.success_mask.any() + assert not destination_only.failure_mask.any() + destination_outcomes = { + outcome.expectation_id: outcome + for outcome in destination_only.expectation_decisions + } + assert not destination_outcomes["source"].satisfied_mask.any() + assert destination_outcomes["destination"].satisfied_mask.all() + + +def test_monitor_requires_pose_and_binary_physical_evidence() -> None: + monitor = CompositeEffectMonitor( + _attach_spec(), + CompositeEffectMonitorCfg(consecutive_samples=1), + ) + request = _request() + pose_only = monitor.observe( + request, + _evidence( + (0.0, 0.0, 0.0), + (False, False, False), + timestamp=2.0, + ), # type: ignore[arg-type] + ) + + assert not pose_only.success_mask.any() + assert pose_only.failure_mask.all() + outcome = pose_only.expectation_decisions[0] + assert outcome.contradicted_mask.all() + assert not outcome.inverse_satisfied_mask.any() + + +def test_monitor_reports_success_only_for_complete_consecutive_evidence() -> None: + monitor = CompositeEffectMonitor( + _attach_spec(), + CompositeEffectMonitorCfg(consecutive_samples=2), + ) + request = _request() + first = monitor.observe( + request, + _evidence( + (0.0, 0.01, 0.019), + (True, True, True), + timestamp=2.0, + ), # type: ignore[arg-type] + ) + second = monitor.observe( + request, + _evidence( + (0.0, 0.01, 0.019), + (True, True, True), + timestamp=3.0, + revision=5, + ), # type: ignore[arg-type] + ) + + assert not first.success_mask.any() + assert second.success_mask.all() + assert not second.failure_mask.any() + + +def test_invalid_evidence_is_unresolved_and_resets_hysteresis() -> None: + monitor = CompositeEffectMonitor( + _attach_spec(), + CompositeEffectMonitorCfg(consecutive_samples=2), + ) + request = _request() + monitor.observe( + request, + _evidence( + (0.0, 0.0, 0.0), + (True, True, True), + timestamp=2.0, + ), # type: ignore[arg-type] + ) + invalid = monitor.observe( + request, + _evidence( + (0.0, 0.0, 0.0), + (True, True, True), + timestamp=3.0, + valid=torch.tensor([False, True, True]), + revision=5, + ), # type: ignore[arg-type] + ) + after_reset = monitor.observe( + request, + _evidence( + (0.0, 0.0, 0.0), + (True, True, True), + timestamp=4.0, + revision=6, + ), # type: ignore[arg-type] + ) + + assert invalid.success_mask.tolist() == [False, True, True] + assert after_reset.success_mask.tolist() == [False, True, True] + + +def test_request_shrink_preserves_counts_and_generation_change_resets() -> None: + monitor = CompositeEffectMonitor( + _attach_spec(), + CompositeEffectMonitorCfg(consecutive_samples=2), + ) + monitor.observe( + _request(), + _evidence( + (0.0, 0.0, 0.0), + (True, True, True), + timestamp=2.0, + ), # type: ignore[arg-type] + ) + shrunk = _request( + env_mask=torch.tensor([False, True, True]), + verification_id=2, + ) + preserved = monitor.observe( + shrunk, + _evidence( + (0.0, 0.0), + (True, True), + timestamp=3.0, + env_ids=torch.tensor([205, 309]), + revision=5, + ), # type: ignore[arg-type] + ) + reset = monitor.observe( + _request(attempt_generation=1, verification_id=3), + _evidence( + (0.0, 0.0, 0.0), + (True, True, True), + timestamp=3.0, + revision=5, + ), # type: ignore[arg-type] + ) + + assert preserved.success_mask.tolist() == [False, True, True] + assert not reset.success_mask.any() + + +def test_monitor_rejects_expansion_duplicate_counting_and_late_evidence() -> None: + monitor = CompositeEffectMonitor( + _attach_spec(), + CompositeEffectMonitorCfg(consecutive_samples=2), + ) + shrunk = _request(env_mask=torch.tensor([False, True, True])) + sample = _evidence( + (0.0, 0.0), + (True, True), + timestamp=2.0, + env_ids=torch.tensor([205, 309]), + ) + monitor.observe(shrunk, sample) # type: ignore[arg-type] + repeated = monitor.observe(shrunk, sample) # type: ignore[arg-type] + + assert not repeated.success_mask.any() + with pytest.raises(ValueError, match="only shrink"): + monitor.observe( + _request(verification_id=2), + _evidence( + (0.0, 0.0, 0.0), + (True, True, True), + timestamp=3.0, + revision=5, + ), # type: ignore[arg-type] + ) + with pytest.raises(ValueError, match="deadline"): + CompositeEffectMonitor( + _attach_spec(), + CompositeEffectMonitorCfg(consecutive_samples=1), + ).observe( + _request(), + _evidence( + (0.0, 0.0, 0.0), + (True, True, True), + timestamp=10.01, + ), # type: ignore[arg-type] + ) + + +def test_scalar_and_joint_clauses_use_monitor_owned_policy() -> None: + spec = replace( + _attach_spec(), + clauses=( + ScalarEffectClause( + "destination.force", + "destination", + _source("force"), + ScalarEvidenceKind.FORCE, + ScalarExpectation.PRESENT, + ), + JointStateEffectClause( + "destination.joint", + "destination", + _source("joint_state"), + torch.tensor([0.5]), + ), + ), + ) + monitor = CompositeEffectMonitor( + spec, + CompositeEffectMonitorCfg(consecutive_samples=1), + ) + valid = torch.ones(3, dtype=torch.bool) + errors = (None, None, None) + evidence = { + "destination.force": ScalarEffectEvidenceBatch( + "destination.force", + ScalarEvidenceKind.FORCE, + torch.tensor([2.0, 0.0, 0.5]), + valid, + errors, + 2.0, + _ENV_IDS, + 4, + ), + "destination.joint": JointStateEvidenceBatch( + "destination.joint", + torch.tensor([[0.5], [0.5], [0.7]]), + None, + valid, + errors, + 2.0, + _ENV_IDS, + 4, + ), + } + + decision = monitor.observe(_request(), evidence) + + assert decision.success_mask.tolist() == [True, False, False] + assert decision.failure_mask.tolist() == [False, True, True] + + +class _BoundMonitor(EffectMonitor): + def __init__(self, spec: SemanticEffectSpec, *, alias: bool = False) -> None: + self._spec = spec if alias else spec.snapshot() + self._alias = alias + + @property + def spec(self) -> SemanticEffectSpec: + return self._spec if self._alias else self._spec.snapshot() + + def observe( + self, + request: EffectVerificationRequest, + evidence: Mapping[str, EffectEvidenceBatch], + ) -> EffectMonitorDecision: + del evidence + return EffectMonitorDecision( + torch.zeros_like(request.env_mask), + torch.zeros_like(request.env_mask), + ) + + +class _BoundFactory(EffectMonitorFactory): + monitor_id = "test.bound" + revision = "1" + + def __init__(self, spec: SemanticEffectSpec, *, alias: bool = False) -> None: + self._spec = spec if alias else spec.snapshot() + self._alias = alias + + def validate_ref(self, ref: EffectMonitorRef) -> None: + if (ref.monitor_id, ref.revision) != (self.monitor_id, self.revision): + raise ValueError("wrong key") + + def create( + self, + spec: SemanticEffectSpec, + ref: EffectMonitorRef, + ) -> EffectMonitor: + del spec, ref + return _BoundMonitor(self._spec, alias=self._alias) + + +def test_registry_is_exact_versioned_and_enforces_bound_spec() -> None: + factory = CompositeEffectMonitorFactory() + registry = EffectMonitorRegistry((factory,)) + ref = EffectMonitorRef( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + {"consecutive_samples": 1}, + ) + + first = registry.create(_attach_spec(), ref) + second = registry.create(_attach_spec(), ref) + + assert isinstance(first, CompositeEffectMonitor) + assert first is not second + with pytest.raises(KeyError): + registry.resolve(EffectMonitorRef(factory.monitor_id, "unknown")) + with pytest.raises(ValueError, match="Duplicate"): + EffectMonitorRegistry((factory, CompositeEffectMonitorFactory())) + + +def test_registry_rejects_factory_spec_drift_or_aliasing() -> None: + requested = _attach_spec() + changed = replace(requested, semantic_id="other") + drift = _BoundFactory(changed) + with pytest.raises(ValueError, match="different effect spec"): + EffectMonitorRegistry((drift,)).create( + requested, + EffectMonitorRef(drift.monitor_id, drift.revision), + ) + + alias = _BoundFactory(requested, alias=True) + with pytest.raises(TypeError, match="independently owned"): + EffectMonitorRegistry((alias,)).create( + requested, + EffectMonitorRef(alias.monitor_id, alias.revision), + ) + + +def test_composite_config_requires_real_hysteresis_gaps() -> None: + with pytest.raises(ValueError, match="less than"): + CompositeEffectMonitorCfg( + attached_translation_threshold=0.05, + detached_translation_threshold=0.05, + ) + with pytest.raises(ValueError, match="positive integer"): + CompositeEffectMonitorCfg(consecutive_samples=True) + with pytest.raises(ValueError, match="Unknown"): + CompositeEffectMonitorFactory().validate_ref( + EffectMonitorRef( + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + {"typo": 1}, + ) + ) diff --git a/tests/sim/skills/test_evidence.py b/tests/sim/skills/test_evidence.py new file mode 100644 index 000000000..3fb097383 --- /dev/null +++ b/tests/sim/skills/test_evidence.py @@ -0,0 +1,666 @@ +# ---------------------------------------------------------------------------- +# 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 synchronized semantic-effect evidence acquisition.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + EntityState, + ObservedArticulationJointState, + SceneSnapshot, +) +from embodichain.lab.sim.skills.effects import ( + ArticulationJointStateExpectation, + BinaryEffectClause, + BinaryEffectEvidenceBatch, + BinaryEvidenceKind, + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + ControlPartEvidenceAddress, + EffectEvidenceBatch, + EffectEvidenceSourceRef, + FORCE_EFFECT_CHANNEL, + HeldObjectRelation, + HeldObjectStateExpectation, + JOINT_STATE_EFFECT_CHANNEL, + JointStateEffectClause, + POSE_RELATION_EFFECT_CHANNEL, + PoseRelationClause, + PoseRelationExpectation, + ScalarEffectClause, + ScalarEvidenceKind, + ScalarExpectation, + SemanticEffectKind, + SemanticEffectSpec, +) +from embodichain.lab.sim.skills.evidence import ( + BinaryEffectEvidenceQuery, + BinaryEffectObservation, + ControlPartSimulationEvidenceProvider, + EffectEvidenceCollectionContext, + EffectEvidenceCollector, + EffectEvidenceProvider, + EffectEvidenceProviderRegistry, + JointStateEvidenceQuery, + JointStateObservation, + PoseRelationEvidenceQuery, + ScalarEffectEvidenceQuery, + ScalarEffectObservation, + SceneArticulationEvidenceProvider, + build_effect_evidence_queries, +) +from embodichain.lab.sim.skills.scene import ( + SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, + ArticulationJointEvidenceAddress, +) + + +def _source(channel: str, *, provider_id: str | None = None) -> EffectEvidenceSourceRef: + return EffectEvidenceSourceRef( + provider_id or CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + ControlPartEvidenceAddress("arm", channel), + ) + + +def _held_expectation() -> HeldObjectStateExpectation: + return HeldObjectStateExpectation( + "held", + HeldObjectRelation.ATTACHED, + "cube", + "actor", + "arm_resource", + "arm_resource", + ) + + +def _attach_spec( + *clauses: object, + env_ids: torch.Tensor | None = None, +) -> SemanticEffectSpec: + return SemanticEffectSpec( + semantic_id="pick:cube", + effect_kind=SemanticEffectKind.ATTACH, + skill_id="PickUp", + invocation_id="pick-1", + invocation_revision=0, + env_ids=torch.tensor([0, 1], dtype=torch.long) if env_ids is None else env_ids, + state_expectations=(_held_expectation(),), + clauses=clauses, + ) + + +def _pose_clause(clause_id: str = "pose") -> PoseRelationClause: + return PoseRelationClause( + clause_id, + "held", + _source(POSE_RELATION_EFFECT_CHANNEL), + PoseRelationExpectation.MATCHED, + ) + + +def _binary_clause( + clause_id: str = "contact", + *, + provider_id: str | None = None, +) -> BinaryEffectClause: + return BinaryEffectClause( + clause_id, + "held", + _source(CONTACT_EFFECT_CHANNEL, provider_id=provider_id), + BinaryEvidenceKind.CONTACT, + True, + ) + + +def _scalar_clause(clause_id: str = "force") -> ScalarEffectClause: + return ScalarEffectClause( + clause_id, + "held", + _source(FORCE_EFFECT_CHANNEL), + ScalarEvidenceKind.FORCE, + ScalarExpectation.PRESENT, + ) + + +class _FakeSceneProvider: + def __init__(self, poses: torch.Tensor, *, confidence: float = 1.0) -> None: + self.poses = poses + self.confidence = confidence + self.calls = 0 + self.received_env_ids: torch.Tensor | None = None + + def snapshot(self, *, timestamp: float, env_ids: torch.Tensor) -> SceneSnapshot: + self.calls += 1 + self.received_env_ids = env_ids.clone() + poses = self.poses.index_select(0, env_ids.to(device=self.poses.device)) + return SceneSnapshot( + timestamp=timestamp, + version=self.calls, + entities={"cube": EntityState(poses, confidence=self.confidence)}, + ) + + +class _FakeRobot: + def __init__(self, qpos: torch.Tensor, qvel: torch.Tensor | None = None) -> None: + self.qpos = qpos + self.qvel = torch.zeros_like(qpos) if qvel is None else qvel + self.fk_calls = 0 + self.qpos_calls = 0 + self.qvel_calls = 0 + + def get_qpos(self, name: str | None = None, target: bool = False) -> torch.Tensor: + assert name == "arm" + assert target is False + self.qpos_calls += 1 + return self.qpos + + def get_qvel(self, name: str | None = None, target: bool = False) -> torch.Tensor: + assert name == "arm" + assert target is False + self.qvel_calls += 1 + return self.qvel + + def compute_fk( + self, + qpos: torch.Tensor, + name: str | None = None, + env_ids: Sequence[int] | None = None, + to_matrix: bool = False, + ) -> torch.Tensor: + assert name == "arm" + assert env_ids is not None + assert to_matrix is True + self.fk_calls += 1 + poses = torch.eye(4, dtype=qpos.dtype, device=qpos.device).repeat( + qpos.shape[0], 1, 1 + ) + poses[:, 0, 3] = qpos[:, 0] + return poses + + +class _WrongTimestampProvider(EffectEvidenceProvider): + provider_id = "test.provider" + revision = "1" + + def collect( + self, + queries: tuple[object, ...], + context: EffectEvidenceCollectionContext, + ) -> Mapping[str, EffectEvidenceBatch]: + query = queries[0] + assert isinstance(query, BinaryEffectEvidenceQuery) + batch_size = int(context.env_ids.numel()) + return { + query.evidence_id: BinaryEffectEvidenceBatch( + query.evidence_id, + BinaryEvidenceKind.CONTACT, + torch.ones(batch_size, dtype=torch.bool), + torch.ones(batch_size, dtype=torch.bool), + (None,) * batch_size, + context.timestamp + 1.0, + context.env_ids, + context.observation_revision, + ) + } + + +class _SecondRevisionProvider(_WrongTimestampProvider): + revision = "2" + + +def test_collection_context_validates_and_owns_env_ids() -> None: + env_ids = torch.tensor([3, 1], dtype=torch.long) + context = EffectEvidenceCollectionContext(1.25, 7, env_ids) + env_ids[0] = 99 + + assert context.timestamp == 1.25 + assert context.observation_revision == 7 + assert context.env_ids.tolist() == [3, 1] + assert context.snapshot().env_ids.data_ptr() != context.env_ids.data_ptr() + + with pytest.raises(ValueError, match="unique"): + EffectEvidenceCollectionContext(0.0, 0, torch.tensor([1, 1])) + with pytest.raises(ValueError, match="non-negative"): + EffectEvidenceCollectionContext(-0.1, 0, torch.tensor([0])) + + +def test_build_queries_preserves_clause_order_and_exact_types() -> None: + spec = _attach_spec(_pose_clause(), _binary_clause(), _scalar_clause()) + + queries = build_effect_evidence_queries(spec) + + assert tuple(type(query) for query in queries) == ( + PoseRelationEvidenceQuery, + BinaryEffectEvidenceQuery, + ScalarEffectEvidenceQuery, + ) + assert tuple(query.evidence_id for query in queries) == ( + "pose", + "contact", + "force", + ) + assert all(query.expectation.expectation_id == "held" for query in queries) + + +def test_provider_registry_requires_exact_unique_versions() -> None: + first = _WrongTimestampProvider() + second = _SecondRevisionProvider() + registry = EffectEvidenceProviderRegistry((first, second)) + + source_v1 = _source(CONTACT_EFFECT_CHANNEL, provider_id="test.provider") + assert registry.resolve(source_v1) is first + assert registry.providers[("test.provider", "2")] is second + + with pytest.raises(ValueError, match="Duplicate"): + EffectEvidenceProviderRegistry((first, _WrongTimestampProvider())) + with pytest.raises(KeyError, match="exact versions"): + registry.resolve( + EffectEvidenceSourceRef( + "test.provider", + "missing", + ControlPartEvidenceAddress("arm", CONTACT_EFFECT_CHANNEL), + ) + ) + + +def test_collector_rejects_provider_metadata_drift() -> None: + spec = _attach_spec(_binary_clause(provider_id="test.provider")) + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry((_WrongTimestampProvider(),)) + ) + + with pytest.raises(ValueError, match="collection timestamp"): + collector.collect(spec, timestamp=2.0, observation_revision=4) + + +def test_control_part_provider_collects_pose_and_joint_state_once() -> None: + object_poses = torch.eye(4).repeat(2, 1, 1) + object_poses[:, 0, 3] = torch.tensor([0.25, 0.5]) + robot = _FakeRobot(torch.tensor([[0.75, 1.0], [1.5, 2.0]])) + scene = _FakeSceneProvider(object_poses) + joint_clause = JointStateEffectClause( + "joints", + "held", + _source(JOINT_STATE_EFFECT_CHANNEL), + torch.tensor([0.0, 0.0]), + ) + spec = _attach_spec(_pose_clause(), joint_clause) + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry( + (ControlPartSimulationEvidenceProvider(robot, scene_provider=scene),) + ) + ) + + evidence = collector.collect(spec, timestamp=3.5, observation_revision=11) + + assert list(evidence) == ["pose", "joints"] + assert evidence["pose"].timestamp == 3.5 + assert evidence["joints"].observation_revision == 11 + assert torch.allclose( + evidence["pose"].object_to_endpoint[:, 0, 3], + torch.tensor([0.5, 1.0]), + ) + assert torch.equal(evidence["joints"].positions, robot.qpos) + assert torch.equal(evidence["joints"].velocities, robot.qvel) + assert scene.calls == 1 + assert robot.qpos_calls == 1 + assert robot.qvel_calls == 1 + assert robot.fk_calls == 1 + + +def test_control_part_provider_selects_requested_simulator_rows() -> None: + env_ids = torch.tensor([2, 0], dtype=torch.long) + object_poses = torch.eye(4).repeat(3, 1, 1) + robot = _FakeRobot(torch.tensor([[1.0], [2.0], [3.0]])) + scene = _FakeSceneProvider(object_poses) + spec = _attach_spec(_pose_clause(), env_ids=env_ids) + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry( + (ControlPartSimulationEvidenceProvider(robot, scene_provider=scene),) + ) + ) + + evidence = collector.collect(spec, timestamp=0.0, observation_revision=0) + + assert evidence["pose"].env_ids.tolist() == [2, 0] + assert evidence["pose"].object_to_endpoint[:, 0, 3].tolist() == [3.0, 1.0] + assert scene.received_env_ids is not None + assert scene.received_env_ids.tolist() == [2, 0] + + +def test_pose_queries_share_one_scene_and_fk_snapshot() -> None: + robot = _FakeRobot(torch.tensor([[0.0], [0.0]])) + scene = _FakeSceneProvider(torch.eye(4).repeat(2, 1, 1)) + spec = _attach_spec(_pose_clause("first"), _pose_clause("second")) + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry( + (ControlPartSimulationEvidenceProvider(robot, scene_provider=scene),) + ) + ) + + evidence = collector.collect(spec, timestamp=1.0, observation_revision=1) + + assert set(evidence) == {"first", "second"} + assert scene.calls == 1 + assert robot.fk_calls == 1 + assert robot.qpos_calls == 1 + + +def test_missing_backend_specific_callbacks_return_explicit_invalid_rows() -> None: + robot = _FakeRobot(torch.zeros((2, 1))) + spec = _attach_spec(_binary_clause(), _scalar_clause()) + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry((ControlPartSimulationEvidenceProvider(robot),)) + ) + + evidence = collector.collect(spec, timestamp=1.0, observation_revision=2) + + assert not evidence["contact"].valid.any() + assert not evidence["force"].valid.any() + assert all("callback" in error for error in evidence["contact"].acquisition_errors) + assert all("callback" in error for error in evidence["force"].acquisition_errors) + + +def test_callbacks_receive_owned_queries_and_propagate_row_validity() -> None: + robot = _FakeRobot(torch.zeros((2, 1))) + binary_values = torch.tensor([True, False]) + scalar_values = torch.tensor([3.0, 0.0]) + received_query: BinaryEffectEvidenceQuery | None = None + + def observe_contact( + query: BinaryEffectEvidenceQuery, + context: EffectEvidenceCollectionContext, + ) -> BinaryEffectObservation: + nonlocal received_query + received_query = query + assert context.env_ids.tolist() == [0, 1] + return BinaryEffectObservation( + binary_values, + torch.tensor([True, False]), + (None, "contact sensor unavailable"), + ) + + def observe_force( + query: ScalarEffectEvidenceQuery, + context: EffectEvidenceCollectionContext, + ) -> ScalarEffectObservation: + del query, context + return ScalarEffectObservation(scalar_values) + + provider = ControlPartSimulationEvidenceProvider( + robot, + contact_observer=observe_contact, + force_observer=observe_force, + ) + collector = EffectEvidenceCollector(EffectEvidenceProviderRegistry((provider,))) + + evidence = collector.collect( + _attach_spec(_binary_clause(), _scalar_clause()), + timestamp=1.0, + observation_revision=2, + ) + binary_values[:] = False + scalar_values[:] = 99.0 + + assert received_query is not None + assert received_query.evidence_id == "contact" + assert evidence["contact"].values.tolist() == [True, False] + assert evidence["contact"].valid.tolist() == [True, False] + assert evidence["force"].values.tolist() == [3.0, 0.0] + + +def test_pose_without_scene_provider_is_invalid_not_fabricated() -> None: + robot = _FakeRobot(torch.zeros((2, 1))) + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry((ControlPartSimulationEvidenceProvider(robot),)) + ) + + evidence = collector.collect( + _attach_spec(_pose_clause()), + timestamp=0.0, + observation_revision=0, + ) + + assert not evidence["pose"].valid.any() + assert all( + "scene provider" in error for error in evidence["pose"].acquisition_errors + ) + assert robot.fk_calls == 0 + + +def test_channel_mismatch_fails_before_callback() -> None: + wrong_clause = BinaryEffectClause( + "contact", + "held", + _source(CONSTRAINT_EFFECT_CHANNEL), + BinaryEvidenceKind.CONTACT, + True, + ) + robot = _FakeRobot(torch.zeros((2, 1))) + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry((ControlPartSimulationEvidenceProvider(robot),)) + ) + + with pytest.raises(ValueError, match="requires channel"): + collector.collect( + _attach_spec(wrong_clause), + timestamp=0.0, + observation_revision=0, + ) + + +def test_joint_query_type_is_built_for_articulation_expectation() -> None: + expectation = ArticulationJointStateExpectation( + "drawer_joint", + "drawer", + "slide", + torch.tensor([0.4]), + ) + clause = JointStateEffectClause( + "joint", + "drawer_joint", + _source(JOINT_STATE_EFFECT_CHANNEL), + torch.tensor([0.4]), + ) + spec = SemanticEffectSpec( + semantic_id="open:drawer", + effect_kind=SemanticEffectKind.ARTICULATION, + skill_id="OperateArticulation", + invocation_id="open-1", + invocation_revision=0, + env_ids=torch.tensor([0]), + state_expectations=(expectation,), + clauses=(clause,), + ) + + query = build_effect_evidence_queries(spec)[0] + + assert isinstance(query, JointStateEvidenceQuery) + assert query.expectation.articulation_id == "drawer" + + +def _articulation_spec( + *clauses: JointStateEffectClause, + expectation_joint: str = "slide", +) -> SemanticEffectSpec: + expectation = ArticulationJointStateExpectation( + "drawer_joint", + "drawer", + expectation_joint, + torch.tensor([[0.4], [0.4]]), + ) + return SemanticEffectSpec( + semantic_id="open:drawer", + effect_kind=SemanticEffectKind.ARTICULATION, + skill_id="OperateArticulation", + invocation_id="open-1", + invocation_revision=0, + env_ids=torch.tensor([0, 1]), + state_expectations=(expectation,), + clauses=clauses, + ) + + +def _articulation_clause(clause_id: str = "joint") -> JointStateEffectClause: + return JointStateEffectClause( + clause_id, + "drawer_joint", + EffectEvidenceSourceRef( + SCENE_ARTICULATION_EVIDENCE_PROVIDER_ID, + SCENE_ARTICULATION_EVIDENCE_PROVIDER_REVISION, + ArticulationJointEvidenceAddress("drawer", "slide"), + ), + torch.tensor([[0.4], [0.4]]), + ) + + +def test_scene_articulation_provider_uses_explicit_typed_observer() -> None: + calls = 0 + + def observe_joint( + query: JointStateEvidenceQuery, + context: EffectEvidenceCollectionContext, + ) -> JointStateObservation: + nonlocal calls + calls += 1 + address = query.source.address + assert isinstance(address, ArticulationJointEvidenceAddress) + assert (address.articulation_id, address.joint_id) == ("drawer", "slide") + assert context.observation_revision == 8 + return JointStateObservation( + positions=torch.tensor([[0.4], [0.3]]), + velocities=torch.tensor([[0.0], [0.1]]), + valid=torch.tensor([True, False]), + acquisition_errors=(None, "joint sensor unavailable"), + ) + + provider = SceneArticulationEvidenceProvider(observe_joint) + collector = EffectEvidenceCollector(EffectEvidenceProviderRegistry((provider,))) + + evidence = collector.collect( + _articulation_spec(_articulation_clause()), + timestamp=4.0, + observation_revision=8, + ) + + assert calls == 1 + assert torch.allclose( + evidence["joint"].positions, + torch.tensor([[0.4], [0.3]]), + ) + assert evidence["joint"].valid.tolist() == [True, False] + assert evidence["joint"].acquisition_errors == ( + None, + "joint sensor unavailable", + ) + + +def test_scene_articulation_provider_reads_typed_scene_snapshot_once() -> None: + class _JointSceneProvider: + calls = 0 + + def snapshot( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> SceneSnapshot: + self.calls += 1 + return SceneSnapshot( + timestamp=timestamp, + version=self.calls, + articulation_joints={ + ("drawer", "slide"): ObservedArticulationJointState( + torch.tensor([[0.4], [0.3]]), + torch.tensor([True, False]), + ) + }, + ) + + scene_provider = _JointSceneProvider() + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry( + (SceneArticulationEvidenceProvider(scene_provider=scene_provider),) + ) + ) + + evidence = collector.collect( + _articulation_spec( + _articulation_clause("position"), + _articulation_clause("settled_position"), + ), + timestamp=2.0, + observation_revision=5, + ) + + assert scene_provider.calls == 1 + assert torch.equal(evidence["position"].positions, torch.tensor([[0.4], [0.3]])) + assert evidence["settled_position"].valid.tolist() == [True, False] + + +def test_scene_articulation_provider_samples_same_address_once() -> None: + calls = 0 + + def observe_joint( + query: JointStateEvidenceQuery, + context: EffectEvidenceCollectionContext, + ) -> JointStateObservation: + nonlocal calls + del query, context + calls += 1 + return JointStateObservation(torch.tensor([[0.4], [0.4]])) + + collector = EffectEvidenceCollector( + EffectEvidenceProviderRegistry( + (SceneArticulationEvidenceProvider(observe_joint),) + ) + ) + spec = _articulation_spec( + _articulation_clause("position"), + _articulation_clause("settled_position"), + ) + + evidence = collector.collect(spec, timestamp=1.0, observation_revision=1) + + assert set(evidence) == {"position", "settled_position"} + assert calls == 1 + + +def test_scene_articulation_provider_rejects_address_expectation_drift() -> None: + provider = SceneArticulationEvidenceProvider( + lambda query, context: JointStateObservation(torch.tensor([[0.4], [0.4]])) + ) + collector = EffectEvidenceCollector(EffectEvidenceProviderRegistry((provider,))) + + with pytest.raises(ValueError, match="exactly match"): + collector.collect( + _articulation_spec( + _articulation_clause(), + expectation_joint="other_joint", + ), + timestamp=1.0, + observation_revision=1, + ) diff --git a/tests/sim/skills/test_integration.py b/tests/sim/skills/test_integration.py new file mode 100644 index 000000000..b9d204dd3 --- /dev/null +++ b/tests/sim/skills/test_integration.py @@ -0,0 +1,1028 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Pure-Python tests for static semantic integration.""" + +from __future__ import annotations + +from unittest.mock import Mock + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + Affordance, + AntipodalAffordance, + AtomicActionEngine, + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + ControlPartCommandProfile, + DynamicCollisionMode, + EntityState, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + HandOverOptions, + MotionPolicy, + OperateArticulationOptions, + PickUpOptions, + PlaceOptions, +) +from embodichain.lab.sim.skills.calls import ( + Pick, + RegisteredSemanticCall, + SemanticCallCatalog, + SemanticCallDescriptor, + builtin_semantic_call_catalog, +) +from embodichain.lab.sim.skills.effects import EffectMonitorRef +from embodichain.lab.sim.skills.integration import ( + BoundSemanticCall, + BoundSemanticIntegration, + SceneEntityManifest, + SceneManifest, + SemanticIntegrationManifest, + SemanticValidationError, +) +from embodichain.lab.sim.skills.profiles import ( + ControlPartEndpoint, + RobotResource, + RobotSkillProfile, + SkillPolicyPreset, + WorkflowRecoveryPolicy, +) +from embodichain.lab.sim.skills.scene import ( + AmbiguousSceneAffordanceError, + GRASP_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, + SceneAffordanceRef, + SceneCollisionRole, + SceneCollisionWorldMode, + SceneEntityRegistration, + SceneObjectRef, + SceneRegistry, + UnsupportedSceneAffordanceError, +) + +_MOTION_CAPABILITIES = frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } +) + + +def _action_option_templates() -> dict[str, object]: + """Return exact built-in semantic-call option declarations.""" + return { + "pick": PickUpOptions(), + "place": PlaceOptions(), + "hand_over": HandOverOptions(), + "operate_articulation": OperateArticulationOptions(), + } + + +def _preset(preset_id: str, **kwargs: object) -> SkillPolicyPreset: + """Build one complete schema-v3 test preset.""" + kwargs.setdefault("action_option_templates", _action_option_templates()) + return SkillPolicyPreset(preset_id, **kwargs) + + +class _NeverObservedStateProvider: + """Fail if provider-backed state leaks into static validation.""" + + def __init__(self) -> None: + self.calls = 0 + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp, env_ids + self.calls += 1 + raise AssertionError("static semantic validation must not observe providers") + + +class _CopyTrackedAffordance(AntipodalAffordance): + """Count payload copies so metadata projection can prove it performs none.""" + + copies = 0 + + def __deepcopy__(self, memo: dict[int, object]) -> _CopyTrackedAffordance: + del memo + type(self).copies += 1 + return _CopyTrackedAffordance() + + +class _GeometryProvider: + """Return one opaque planner-facing geometry descriptor.""" + + def get_geometry(self) -> object: + return object() + + +def _scene_registry( + *, + with_default: bool, + dynamic_collision: bool = False, +) -> tuple[SceneRegistry, _NeverObservedStateProvider]: + provider = _NeverObservedStateProvider() + object_ref = SceneObjectRef("cube") + side_grasp = SceneAffordanceRef("cube.grasp.side") + top_grasp = SceneAffordanceRef("cube.grasp.top") + defaults = {GRASP_AFFORDANCE_CAPABILITY: top_grasp} if with_default else {} + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=object_ref, + state_provider=provider, + aliases=("sim_cube",), + default_affordances=defaults, + geometry_provider=(_GeometryProvider() if dynamic_collision else None), + collision_role=( + SceneCollisionRole.DYNAMIC + if dynamic_collision + else SceneCollisionRole.NONE + ), + ), + SceneEntityRegistration( + ref=side_grasp, + parent=object_ref, + native_name="side_grasp", + affordance=AntipodalAffordance(), + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + affordance_revision="grasp-v1", + relative_pose=torch.eye(4), + ), + SceneEntityRegistration( + ref=top_grasp, + parent=object_ref, + native_name="top_grasp", + affordance=AntipodalAffordance(), + affordance_capabilities=frozenset( + { + GRASP_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, + } + ), + affordance_revision="grasp-v1", + relative_pose=torch.eye(4), + ), + ), + collision_world_mode=( + SceneCollisionWorldMode.PER_ENV if dynamic_collision else None + ), + ) + return registry, provider + + +def _semantic_integration( + registry: SceneRegistry, + *, + preset: SkillPolicyPreset | None = None, + additional_presets: tuple[SkillPolicyPreset, ...] = (), + default_preset: str | None = None, + skill_presets: dict[str, str] | None = None, + runtime_preset: str | None = None, +) -> SemanticIntegrationManifest: + selected_preset = _preset("safe") if preset is None else preset + presets = {selected_preset.preset_id: selected_preset} + presets.update( + { + additional_preset.preset_id: additional_preset + for additional_preset in additional_presets + } + ) + robot_profile = RobotSkillProfile( + profile_id="test_robot", + resources={ + "manipulator": RobotResource( + resource_id="manipulator", + endpoints={ + "motion": ControlPartEndpoint( + control_part="arm", + capabilities=_MOTION_CAPABILITIES, + ), + "grasp": ControlPartEndpoint( + control_part="hand", + capabilities=frozenset({GRASP_CAPABILITY}), + ), + }, + ) + }, + command_profiles={ + "hand": ControlPartCommandProfile.joint_positions( + open=torch.tensor([0.0]), + grasp=torch.tensor([1.0]), + ) + }, + presets=presets, + default_preset=( + selected_preset.preset_id if default_preset is None else default_preset + ), + skill_presets={} if skill_presets is None else skill_presets, + ) + return SemanticIntegrationManifest( + scene=SceneManifest.from_registry(registry), + robot_profile=robot_profile, + call_catalog=builtin_semantic_call_catalog(), + runtime_preset=runtime_preset, + ) + + +def _engine_for_integration( + integration: SemanticIntegrationManifest, + *, + supports_dynamic_collision_world: bool = False, +) -> AtomicActionEngine: + """Build a minimal live engine whose resource graph matches the manifest.""" + robot = Mock() + robot.device = torch.device("cpu") + robot.dof = 2 + robot.control_parts = {"arm": object(), "hand": object()} + robot.get_qpos.return_value = torch.zeros(1, 2) + robot.get_qvel.return_value = torch.zeros(1, 2) + robot.get_joint_ids.side_effect = lambda name: { + "arm": [0], + "hand": [1], + }[name] + robot.get_solver.side_effect = lambda name=None: ( + object() if name == "arm" else None + ) + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "stub_planner" + generator.supports_dynamic_collision_world = supports_dynamic_collision_world + return AtomicActionEngine( + generator, + skill_profile=integration.robot_profile, + ) + + +def test_scene_registry_filters_capabilities_and_uses_scoped_default() -> None: + registry, _ = _scene_registry(with_default=True) + + assert registry.affordances( + "sim_cube", + capability=GRASP_AFFORDANCE_CAPABILITY, + ) == ( + SceneAffordanceRef("cube.grasp.side"), + SceneAffordanceRef("cube.grasp.top"), + ) + assert registry.affordances( + "cube", + capability=PLACE_ON_AFFORDANCE_CAPABILITY, + ) == (SceneAffordanceRef("cube.grasp.top"),) + assert registry.resolve_affordance( + "cube", + capability=GRASP_AFFORDANCE_CAPABILITY, + ) == SceneAffordanceRef("cube.grasp.top") + assert registry.resolve_affordance( + "cube", + capability=GRASP_AFFORDANCE_CAPABILITY, + explicit="cube.grasp.side", + ) == SceneAffordanceRef("cube.grasp.side") + + +def test_scene_registry_rejects_ambiguous_or_unsupported_affordance() -> None: + registry, _ = _scene_registry(with_default=False) + + with pytest.raises(AmbiguousSceneAffordanceError, match="multiple affordances"): + registry.resolve_affordance( + "cube", + capability=GRASP_AFFORDANCE_CAPABILITY, + ) + with pytest.raises(UnsupportedSceneAffordanceError, match="no affordance"): + registry.resolve_affordance( + "cube", + capability="affordance.place.inside", + ) + + +def test_scene_registry_rejects_untyped_or_unversioned_grasp_capability() -> None: + object_ref = SceneObjectRef("cube") + base = dict( + ref=SceneAffordanceRef("cube.grasp"), + parent=object_ref, + native_name="grasp", + relative_pose=torch.eye(4), + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + ) + + with pytest.raises(TypeError, match="AntipodalAffordance"): + SceneEntityRegistration( + **base, + affordance=Affordance(), + affordance_revision="v1", + ) + with pytest.raises(ValueError, match="affordance_revision"): + SceneEntityRegistration( + **base, + affordance=AntipodalAffordance(), + ) + + +def test_scene_registry_rejects_default_reference_subclass() -> None: + class SpecialAffordanceRef(SceneAffordanceRef): + pass + + with pytest.raises(TypeError, match="SceneAffordanceRef"): + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_NeverObservedStateProvider(), + default_affordances={ + GRASP_AFFORDANCE_CAPABILITY: SpecialAffordanceRef("cube.grasp") + }, + ) + + +def test_scene_manifest_projection_does_not_copy_affordance_payload() -> None: + provider = _NeverObservedStateProvider() + object_ref = SceneObjectRef("cube") + _CopyTrackedAffordance.copies = 0 + registry = SceneRegistry( + ( + SceneEntityRegistration(ref=object_ref, state_provider=provider), + SceneEntityRegistration( + ref=SceneAffordanceRef("cube.grasp"), + parent=object_ref, + native_name="grasp", + relative_pose=torch.eye(4), + affordance=_CopyTrackedAffordance(), + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + affordance_revision="v1", + ), + ) + ) + _CopyTrackedAffordance.copies = 0 + + SceneManifest.from_registry(registry) + + assert _CopyTrackedAffordance.copies == 0 + assert provider.calls == 0 + + +def test_scene_manifest_detects_grounding_metadata_drift() -> None: + provider = _NeverObservedStateProvider() + object_ref = SceneObjectRef("cube") + + def registry(native_name: str, revision: str) -> SceneRegistry: + return SceneRegistry( + ( + SceneEntityRegistration(ref=object_ref, state_provider=provider), + SceneEntityRegistration( + ref=SceneAffordanceRef("cube.grasp"), + parent=object_ref, + native_name=native_name, + relative_pose=torch.eye(4), + affordance=AntipodalAffordance(), + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + affordance_revision=revision, + ), + ) + ) + + manifest = SceneManifest.from_registry(registry("grasp", "v1")) + + with pytest.raises(SemanticValidationError) as error: + manifest.validate_registry(registry("changed", "v2")) + + assert error.value.diagnostic.code == "scene_manifest_mismatch" + + +def test_scene_manifest_detects_collision_world_mode_drift() -> None: + manifest = SceneManifest.from_registry( + SceneRegistry((), collision_world_mode=SceneCollisionWorldMode.SHARED) + ) + + with pytest.raises(SemanticValidationError) as error: + manifest.validate_registry( + SceneRegistry((), collision_world_mode=SceneCollisionWorldMode.PER_ENV) + ) + + assert error.value.diagnostic.code == "scene_manifest_mismatch" + assert error.value.diagnostic.rendered_path.endswith("collision_world_mode") + + +def test_scene_manifest_rejects_impossible_typed_topology() -> None: + affordance = SceneAffordanceRef("self") + + with pytest.raises(ValueError, match="object, articulation, or link"): + SceneEntityManifest( + ref=affordance, + parent=affordance, + native_name="self", + affordance_payload_type=AntipodalAffordance, + affordance_revision="v1", + ) + + +def test_scene_manifest_rejects_entry_subclass_with_live_state() -> None: + class LiveManifest(SceneEntityManifest): + live_handle = object() + + with pytest.raises(TypeError, match="exact SceneEntityManifest"): + SceneManifest((LiveManifest(ref=SceneObjectRef("cube")),)) + + +def test_semantic_integration_rejects_catalog_subclass_with_behavior() -> None: + class LiveCatalog(SemanticCallCatalog): + live_handle = object() + + registry, _ = _scene_registry(with_default=True) + integration = _semantic_integration(registry) + live_catalog = LiveCatalog(integration.call_catalog.descriptors.values()) + + with pytest.raises(TypeError, match="exactly SemanticCallCatalog"): + SemanticIntegrationManifest( + scene=integration.scene, + robot_profile=integration.robot_profile, + call_catalog=live_catalog, + ) + + +def test_semantic_integration_rejects_monitor_for_unknown_call_with_path() -> None: + registry, _ = _scene_registry(with_default=True) + unknown_semantic_id = "not_catalogued" + + with pytest.raises(SemanticValidationError) as error: + _semantic_integration( + registry, + preset=_preset( + "safe", + effect_monitors={ + unknown_semantic_id: EffectMonitorRef("test.monitor", "1") + }, + ), + ) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "unknown_effect_monitor_call" + assert diagnostic.path == ( + "integration", + "robot_profile", + "presets", + "safe", + "effect_monitors", + unknown_semantic_id, + ) + assert diagnostic.rendered_path == ( + "integration.robot_profile.presets.safe.effect_monitors.not_catalogued" + ) + + +def test_semantic_integration_rejects_unknown_action_option_call() -> None: + registry, _ = _scene_registry(with_default=True) + + with pytest.raises(SemanticValidationError) as error: + _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + action_option_templates={"vendor.unknown": PickUpOptions()}, + ), + ) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "unknown_action_option_call" + assert diagnostic.path[-2:] == ( + "action_option_templates", + "vendor.unknown", + ) + + +def test_semantic_integration_validates_exact_action_option_type() -> None: + registry, _ = _scene_registry(with_default=True) + + with pytest.raises(SemanticValidationError) as error: + _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + action_option_templates={"pick": PlaceOptions()}, + ), + ) + + assert error.value.diagnostic.code == "incompatible_action_option_template" + assert error.value.diagnostic.path[-1] == "pick" + + +def test_semantic_integration_rejects_compiler_owned_option_fields() -> None: + registry, _ = _scene_registry(with_default=True) + + with pytest.raises(SemanticValidationError) as pick_error: + _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + action_option_templates={ + "pick": PickUpOptions( + downstream_object_target_poses=(torch.eye(4),) + ) + }, + ), + ) + assert pick_error.value.diagnostic.code == "reserved_action_option_field" + assert pick_error.value.diagnostic.path[-1] == ("downstream_object_target_poses") + + with pytest.raises(SemanticValidationError) as handover_error: + _semantic_integration( + registry, + preset=SkillPolicyPreset( + "safe", + action_option_templates={ + "hand_over": HandOverOptions( + middle_object_pose=torch.eye(4), + ) + }, + ), + ) + assert handover_error.value.diagnostic.code == "reserved_action_option_field" + assert handover_error.value.diagnostic.path[-1] == "middle_object_pose" + + +def test_static_link_requires_selected_preset_action_option_template() -> None: + registry, _ = _scene_registry(with_default=True) + integration = _semantic_integration( + registry, + preset=SkillPolicyPreset("safe", action_option_templates={}), + ) + + with pytest.raises(SemanticValidationError) as error: + integration.link_call(Pick(object=SceneObjectRef("cube"))) + + assert error.value.diagnostic.code == "missing_action_option_template" + assert error.value.diagnostic.path == ( + "integration", + "robot_profile", + "presets", + "safe", + "action_option_templates", + "pick", + ) + assert "selected at call" in error.value.diagnostic.message + + +def test_scene_manifest_reports_structured_pathful_diagnostic() -> None: + manifest = SceneManifest((SceneEntityManifest(ref=SceneObjectRef("cube")),)) + + with pytest.raises(SemanticValidationError) as error: + manifest.resolve( + "missing", + expected_type=SceneObjectRef, + path=("program", 2, "object"), + ) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "unknown_entity" + assert diagnostic.path == ("program", 2, "object") + assert diagnostic.rendered_path == "program[2].object" + assert diagnostic.candidates == ("cube",) + assert str(error.value).startswith("program[2].object:") + + +def test_static_integration_links_resources_and_affordances_without_observation() -> ( + None +): + registry, provider = _scene_registry(with_default=True) + integration = _semantic_integration(registry) + + linked = integration.link_call( + Pick( + object=SceneObjectRef("cube"), + resources={"primary": "manipulator"}, + ), + path=("program", 0), + ) + integration.scene.validate_registry(registry) + + assert linked.descriptor.skill_id == "pick_up" + assert linked.preset_id == "safe" + assert linked.call.resources == {"primary": "manipulator"} + assert isinstance(linked.call, Pick) + assert linked.call.grasp == SceneAffordanceRef("cube.grasp.top") + assert linked.affordances == {"grasp": SceneAffordanceRef("cube.grasp.top")} + assert provider.calls == 0 + + +def test_static_integration_rejects_unknown_resource_with_complete_path() -> None: + registry, provider = _scene_registry(with_default=True) + integration = _semantic_integration(registry) + + with pytest.raises(SemanticValidationError) as error: + integration.link_call( + Pick( + object=SceneObjectRef("cube"), + resources={"primary": "missing"}, + ), + path=("program", 3), + ) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "unknown_resource" + assert diagnostic.path == ("program", 3, "resources", "primary") + assert diagnostic.rendered_path == "program[3].resources.primary" + assert diagnostic.candidates == ("manipulator",) + assert provider.calls == 0 + + +def test_static_integration_preserves_scene_path_without_observing_provider() -> None: + registry, provider = _scene_registry(with_default=True) + integration = _semantic_integration(registry) + + with pytest.raises(SemanticValidationError) as error: + integration.link_call( + Pick( + object=SceneObjectRef("missing"), + resources={"primary": "manipulator"}, + ), + path=("program", 4), + ) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "unknown_entity" + assert diagnostic.rendered_path == "program[4].object" + assert provider.calls == 0 + + +def test_registered_payload_scene_refs_are_statically_resolved() -> None: + registry, provider = _scene_registry(with_default=True) + integration = _semantic_integration(registry) + pick = integration.call_catalog.discover("pick") + extension = SemanticCallDescriptor( + call_id="vendor.inspect", + spec_type=RegisteredSemanticCall, + target_descriptor=pick.target_descriptor, + ) + integration = SemanticIntegrationManifest( + scene=integration.scene, + robot_profile=integration.robot_profile, + call_catalog=integration.call_catalog.with_descriptor(extension), + ) + + with pytest.raises(SemanticValidationError) as error: + integration.link_call( + RegisteredSemanticCall( + call_id="vendor.inspect", + arguments={"object": SceneObjectRef("missing")}, + resources={"primary": "manipulator"}, + ), + path=("program", 5, "call"), + ) + + assert error.value.diagnostic.code == "unknown_entity" + assert error.value.diagnostic.rendered_path == ("program[5].call.arguments.object") + assert provider.calls == 0 + + +def test_bound_semantic_call_retains_installed_profile_ownership() -> None: + registry, _ = _scene_registry(with_default=True) + integration = _semantic_integration(registry) + engine = _engine_for_integration(integration) + bound_integration = integration.bind(registry, engine) + + result = bound_integration.link_call(Pick(object=SceneObjectRef("cube"))) + + assert result.robot_profile is bound_integration.robot_profile + assert result.binding.action_binding.owner_id == engine.binding_owner_id + + +@pytest.mark.parametrize( + "source_mode", + [ + DynamicCollisionMode.AUTO, + DynamicCollisionMode.OFF, + DynamicCollisionMode.REQUIRED, + ], +) +def test_safe_preset_requires_dynamic_collision_for_dynamic_scene( + source_mode: DynamicCollisionMode, +) -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + integration = _semantic_integration( + registry, + preset=_preset( + "safe", + motion_policy=MotionPolicy( + strategy="motion_gen", + dynamic_collision_mode=source_mode, + ), + workflow_recovery_policy=WorkflowRecoveryPolicy( + max_recovery_attempts=2, + ), + ), + ) + engine = _engine_for_integration( + integration, + supports_dynamic_collision_world=True, + ) + + bound = integration.bind(registry, engine).link_call( + Pick(object=SceneObjectRef("cube")) + ) + + assert ( + bound.preset.motion_policy.dynamic_collision_mode + is DynamicCollisionMode.REQUIRED + ) + assert ( + integration.robot_profile.presets["safe"].motion_policy.dynamic_collision_mode + is source_mode + ) + assert bound.preset.workflow_recovery_policy.max_recovery_attempts == 2 + assert provider.calls == 0 + + +def test_safe_preset_rejects_unsupported_dynamic_planner_before_observation() -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + integration = _semantic_integration( + registry, + preset=_preset( + "safe", + motion_policy=MotionPolicy(strategy="motion_gen"), + ), + ) + engine = _engine_for_integration(integration) + bind_skill_profile = Mock(wraps=engine.bind_skill_profile) + engine.bind_skill_profile = bind_skill_profile # type: ignore[method-assign] + + with pytest.raises(SemanticValidationError) as error: + integration.bind(registry, engine) + + diagnostic = error.value.diagnostic + assert diagnostic.code == "safe_dynamic_collision_unsupported" + assert diagnostic.path == ( + "integration", + "robot_profile", + "presets", + "safe", + "motion_policy", + "dynamic_collision_mode", + ) + assert diagnostic.candidates == () + assert "('cube',)" in diagnostic.message + bind_skill_profile.assert_not_called() + assert provider.calls == 0 + + +def test_per_skill_safe_preset_is_conservatively_preflighted() -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + pick_skill_id = builtin_semantic_call_catalog().descriptors["pick"].skill_id + integration = _semantic_integration( + registry, + preset=_preset("fast"), + additional_presets=( + _preset( + "safe", + motion_policy=MotionPolicy(strategy="motion_gen"), + ), + ), + skill_presets={pick_skill_id: "safe"}, + ) + engine = _engine_for_integration(integration) + + with pytest.raises(SemanticValidationError) as error: + integration.bind(registry, engine) + + assert error.value.diagnostic.code == "safe_dynamic_collision_unsupported" + assert provider.calls == 0 + + +def test_fully_overridden_safe_default_is_not_reachable() -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + catalog = builtin_semantic_call_catalog() + integration = _semantic_integration( + registry, + preset=_preset( + "safe", + motion_policy=MotionPolicy(strategy="motion_gen"), + ), + additional_presets=(_preset("fast"),), + skill_presets={ + descriptor.skill_id: "fast" for descriptor in catalog.descriptors.values() + }, + ) + engine = _engine_for_integration(integration) + + bound = integration.bind(registry, engine) + + assert ( + bound.link_call(Pick(object=SceneObjectRef("cube"))).preset.preset_id == "fast" + ) + assert provider.calls == 0 + + +def test_runtime_non_safe_override_makes_safe_default_unreachable() -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + integration = _semantic_integration( + registry, + preset=_preset( + "safe", + motion_policy=MotionPolicy(strategy="motion_gen"), + ), + additional_presets=(_preset("fast"),), + runtime_preset="fast", + ) + engine = _engine_for_integration(integration) + + bound = integration.bind(registry, engine) + + assert ( + bound.link_call(Pick(object=SceneObjectRef("cube"))).preset.preset_id == "fast" + ) + assert provider.calls == 0 + + +def test_bound_integration_cannot_bypass_safe_dynamic_planner_preflight() -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + integration = _semantic_integration( + registry, + preset=_preset( + "safe", + motion_policy=MotionPolicy(strategy="motion_gen"), + ), + ) + engine = _engine_for_integration(integration) + bound_profile = engine.bind_skill_profile(integration.robot_profile) + + with pytest.raises(SemanticValidationError) as error: + BoundSemanticIntegration( + manifest=integration, + scene_registry=registry, + robot_profile=bound_profile, + engine=engine, + ) + + assert error.value.diagnostic.code == "safe_dynamic_collision_unsupported" + assert error.value.diagnostic.path[-1] == "dynamic_collision_mode" + assert provider.calls == 0 + + +def test_bind_rejects_invalid_engine_before_safe_capability_lookup() -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + integration = _semantic_integration( + registry, + preset=_preset( + "safe", + motion_policy=MotionPolicy(strategy="motion_gen"), + ), + ) + + with pytest.raises(TypeError, match="engine must be an AtomicActionEngine"): + integration.bind(registry, object()) # type: ignore[arg-type] + + assert provider.calls == 0 + + +def test_safe_preset_rejects_non_motion_generator_strategy_for_dynamic_scene() -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + integration = _semantic_integration( + registry, + preset=_preset( + "safe", + motion_policy=MotionPolicy(strategy="ik_interp"), + ), + ) + engine = _engine_for_integration( + integration, + supports_dynamic_collision_world=True, + ) + + with pytest.raises(SemanticValidationError) as error: + integration.bind(registry, engine) + + assert error.value.diagnostic.code == "safe_dynamic_collision_unsupported" + assert error.value.diagnostic.path[-1] == "strategy" + assert provider.calls == 0 + + +@pytest.mark.parametrize( + "source_mode", + [DynamicCollisionMode.AUTO, DynamicCollisionMode.OFF], +) +def test_non_safe_preset_preserves_dynamic_collision_policy( + source_mode: DynamicCollisionMode, +) -> None: + registry, provider = _scene_registry( + with_default=True, + dynamic_collision=True, + ) + integration = _semantic_integration( + registry, + preset=_preset( + "fast", + motion_policy=MotionPolicy(dynamic_collision_mode=source_mode), + ), + ) + engine = _engine_for_integration(integration) + + bound = integration.bind(registry, engine).link_call( + Pick(object=SceneObjectRef("cube")) + ) + + assert bound.preset.preset_id == "fast" + assert bound.preset.motion_policy.dynamic_collision_mode is source_mode + assert provider.calls == 0 + + +@pytest.mark.parametrize( + "source_mode", + [DynamicCollisionMode.AUTO, DynamicCollisionMode.OFF], +) +def test_safe_preset_preserves_policy_without_dynamic_collision( + source_mode: DynamicCollisionMode, +) -> None: + registry, provider = _scene_registry(with_default=True) + integration = _semantic_integration( + registry, + preset=_preset( + "safe", + motion_policy=MotionPolicy(dynamic_collision_mode=source_mode), + ), + ) + engine = _engine_for_integration(integration) + + bound = integration.bind(registry, engine).link_call( + Pick(object=SceneObjectRef("cube")) + ) + + assert bound.preset.motion_policy.dynamic_collision_mode is source_mode + assert provider.calls == 0 + + +def test_bound_semantic_integration_rejects_engine_profile_rebind() -> None: + registry, _ = _scene_registry(with_default=True) + integration = _semantic_integration(registry) + engine = _engine_for_integration(integration) + stale = integration.bind(registry, engine) + + engine.bind_skill_profile(integration.robot_profile) + + with pytest.raises(SemanticValidationError) as error: + stale.link_call(Pick(object=SceneObjectRef("cube"))) + + assert error.value.diagnostic.code == "semantic_profile_stale" + + +def test_bound_semantic_integration_rejects_manifest_subclass_behavior() -> None: + class LiveManifest(SemanticIntegrationManifest): + live_handle = object() + + registry, _ = _scene_registry(with_default=True) + integration = _semantic_integration(registry) + engine = _engine_for_integration(integration) + bound_profile = engine.skill_profile + assert bound_profile is not None + live_manifest = LiveManifest( + scene=integration.scene, + robot_profile=integration.robot_profile, + call_catalog=integration.call_catalog, + ) + + with pytest.raises(TypeError, match="exactly SemanticIntegrationManifest"): + type(integration.bind(registry, engine))( + manifest=live_manifest, + scene_registry=registry, + robot_profile=bound_profile, + engine=engine, + ) diff --git a/tests/sim/skills/test_parallel.py b/tests/sim/skills/test_parallel.py new file mode 100644 index 000000000..9c6044288 --- /dev/null +++ b/tests/sim/skills/test_parallel.py @@ -0,0 +1,251 @@ +# ---------------------------------------------------------------------------- +# 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 deterministic parallel-skill contracts.""" + +from __future__ import annotations + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + ArticulationJointState, + EndpointCommand, + JointPositionPayload, + JointPositionTarget, + RuntimeCommandFrame, + StateDelta, + TaskState, + TimedCommandSequence, +) +from embodichain.lab.sim.skills.parallel import ( + ParallelBranchPlan, + ParallelConflictError, + ParallelStateConflictError, + ParallelTimingError, + ParallelTimingPolicy, + align_parallel_commands, + merge_parallel_effects, + resolve_parallel_barrier, +) +from embodichain.lab.sim.skills.profiles import ResourceClaim + +ENV_IDS = torch.tensor([3, 7], dtype=torch.long) + + +def _sequence( + control_part: str, + joint_id: int, + frame_count: int, + *, + duration: float = 0.1, +) -> TimedCommandSequence: + target = JointPositionTarget(control_part, (joint_id,)) + frames = tuple( + RuntimeCommandFrame( + commands=( + EndpointCommand( + target, + JointPositionPayload(torch.full((2, 1), float(index + joint_id))), + ), + ), + active_mask=torch.tensor([True, True]), + env_ids=ENV_IDS, + hold_duration=torch.full((2,), duration), + ) + for index in range(frame_count) + ) + return TimedCommandSequence(frames, ENV_IDS) + + +def _branch( + branch_id: str, + control_part: str, + joint_id: int, + frame_count: int, + *, + duration: float = 0.1, +) -> ParallelBranchPlan: + return ParallelBranchPlan( + branch_id=branch_id, + claim=ResourceClaim(frozenset({control_part}), (joint_id,)), + commands=_sequence( + control_part, + joint_id, + frame_count, + duration=duration, + ), + ) + + +def test_parallel_alignment_hold_pads_shorter_disjoint_branch() -> None: + merged = align_parallel_commands( + ( + _branch("left", "left_arm", 0, 2), + _branch("right", "right_arm", 1, 3), + ), + ParallelTimingPolicy(step_dt=0.1), + ) + + assert merged.frame_count == 3 + assert all(len(frame.commands) == 2 for frame in merged.frames) + left_final = merged.frames[-1].commands[0].payload + assert isinstance(left_final, JointPositionPayload) + assert torch.equal(left_final.positions, torch.full((2, 1), 1.0)) + assert torch.equal(merged.frames[-1].active_mask, torch.tensor([True, True])) + + +def test_parallel_alignment_rejects_claim_and_grid_conflicts() -> None: + with pytest.raises(ParallelConflictError, match="overlapping"): + align_parallel_commands( + ( + _branch("one", "arm", 0, 2), + _branch("two", "arm", 1, 2), + ), + ParallelTimingPolicy(0.1), + ) + + +def test_parallel_alignment_rejects_different_lane_active_masks() -> None: + left = _branch("left", "left", 0, 1) + right = _branch("right", "right", 1, 1) + right_frame = right.commands.frames[0].with_active_mask(torch.tensor([False, True])) + right = ParallelBranchPlan( + branch_id=right.branch_id, + claim=right.claim, + commands=TimedCommandSequence((right_frame,), ENV_IDS), + ) + + with pytest.raises(ParallelTimingError, match="active masks"): + align_parallel_commands( + (left, right), + ParallelTimingPolicy(0.1), + ) + + +def test_parallel_alignment_validates_inactive_row_durations_on_same_grid() -> None: + left = _branch("left", "left", 0, 1) + left_frame = RuntimeCommandFrame( + commands=left.commands.frames[0].commands, + active_mask=torch.tensor([False, True]), + env_ids=ENV_IDS, + hold_duration=torch.tensor([0.2, 0.1]), + ) + left = ParallelBranchPlan( + branch_id=left.branch_id, + claim=left.claim, + commands=TimedCommandSequence((left_frame,), ENV_IDS), + ) + right = _branch("right", "right", 1, 1) + right_frame = RuntimeCommandFrame( + commands=right.commands.frames[0].commands, + active_mask=torch.tensor([False, True]), + env_ids=ENV_IDS, + hold_duration=torch.tensor([0.1, 0.1]), + ) + right = ParallelBranchPlan( + branch_id=right.branch_id, + claim=right.claim, + commands=TimedCommandSequence((right_frame,), ENV_IDS), + ) + + with pytest.raises(ParallelTimingError, match="step_dt"): + align_parallel_commands((left, right), ParallelTimingPolicy(0.1)) + + +def test_parallel_alignment_rejects_off_grid_duration() -> None: + with pytest.raises(ParallelTimingError, match="step_dt"): + align_parallel_commands( + ( + _branch("left", "left", 0, 2, duration=0.05), + _branch("right", "right", 1, 2), + ), + ParallelTimingPolicy(0.1), + ) + + +def test_parallel_effects_merge_disjoint_keys_by_verified_row() -> None: + state = TaskState.empty(batch_size=2, device="cpu") + merged = merge_parallel_effects( + state, + { + "drawer": ( + StateDelta( + articulation_joint_updates={ + ("drawer", "slide"): ArticulationJointState(torch.tensor([0.4])) + } + ), + torch.tensor([True, False]), + ), + "door": ( + StateDelta( + articulation_joint_updates={ + ("door", "hinge"): ArticulationJointState(torch.tensor([1.0])) + } + ), + torch.tensor([False, True]), + ), + }, + ) + + drawer = merged.get_articulation_joint_state("drawer", "slide") + door = merged.get_articulation_joint_state("door", "hinge") + assert drawer is not None and door is not None + assert torch.equal(drawer.env_mask, torch.tensor([True, False])) + assert torch.equal(door.env_mask, torch.tensor([False, True])) + + +def test_parallel_effects_reject_same_key_on_same_row() -> None: + delta = StateDelta( + articulation_joint_updates={ + ("drawer", "slide"): ArticulationJointState(torch.tensor([0.4])) + } + ) + with pytest.raises(ParallelStateConflictError, match="same symbolic keys"): + merge_parallel_effects( + TaskState.empty(2, "cpu"), + { + "one": (delta, torch.tensor([True, False])), + "two": (delta, torch.tensor([True, True])), + }, + ) + + +def test_parallel_barrier_cancels_pending_siblings_per_failed_row() -> None: + update = resolve_parallel_barrier( + pending_masks={ + "left": torch.tensor([False, True, True]), + "right": torch.tensor([True, False, True]), + }, + success_masks={ + "left": torch.tensor([True, False, False]), + "right": torch.tensor([False, True, False]), + }, + failure_masks={ + "left": torch.tensor([False, False, True]), + "right": torch.tensor([False, False, False]), + }, + ) + + assert torch.equal(update.failure_mask, torch.tensor([False, False, True])) + assert torch.equal(update.completed_mask, torch.tensor([False, False, True])) + assert torch.equal( + update.cancellation_masks["right"], + torch.tensor([False, False, True]), + ) + + +__all__: list[str] = [] diff --git a/tests/sim/skills/test_parallel_runtime.py b/tests/sim/skills/test_parallel_runtime.py new file mode 100644 index 000000000..ec21d8913 --- /dev/null +++ b/tests/sim/skills/test_parallel_runtime.py @@ -0,0 +1,1406 @@ +# ---------------------------------------------------------------------------- +# 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 branch-local semantic execution at a parallel barrier.""" + +from __future__ import annotations + +from dataclasses import dataclass +import json + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + ArticulationJointState, + CommandAcknowledgement, + EndpointCommand, + ExecutionRunnerCfg, + JointPositionPayload, + JointPositionTarget, + PlanningContext, + RobotObservation, + RuntimeCommandFrame, + SceneSnapshot, + StateDelta, + TaskState, +) +from embodichain.lab.sim.skills.calls import RegisteredSemanticCall +from embodichain.lab.sim.skills.parallel import ParallelTimingPolicy +from embodichain.lab.sim.skills.parallel_runtime import ( + ParallelLaneCommandSink, + ParallelRuntimeBranch, + ParallelSkillRuntime, +) +from embodichain.lab.sim.skills.profiles import ResourceClaim +from embodichain.lab.sim.skills.runtime import SkillResult, SkillStatus + +ENV_IDS = torch.tensor([4, 9], dtype=torch.long) + + +class _Clock: + """Deterministic environment-grid clock.""" + + def __init__(self) -> None: + self.time = 0.0 + + def now(self) -> float: + return self.time + + def sleep(self, duration: float) -> None: + self.time += duration + + +class _OutboundSink: + """Record the coordinator's one merged transport transaction.""" + + def __init__( + self, + *, + reject: bool = False, + reject_cancel: bool = False, + reject_hold: bool = False, + raise_send: bool = False, + ) -> None: + self.reject = reject + self.reject_cancel = reject_cancel + self.reject_hold = reject_hold + self.raise_send = raise_send + self.frames: list[RuntimeCommandFrame] = [] + self.hold_targets: list[tuple[str, ...]] = [] + self.hold_fingerprints: list[tuple[object, ...]] = [] + self.operations: list[str] = [] + self.timeouts: list[tuple[str, float]] = [] + self.holds = 0 + self.cancels = 0 + + def send( + self, + command: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + self.timeouts.append(("send", timeout)) + if self.raise_send: + raise RuntimeError("send exploded") + self.operations.append("send") + self.frames.append(command.snapshot()) + if self.reject: + return CommandAcknowledgement.rejected_ack("test rejection") + return CommandAcknowledgement.accepted_ack() + + def hold( + self, + targets: tuple[object, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + del context + self.timeouts.append(("hold", timeout)) + self.operations.append("hold") + self.holds += 1 + self.hold_targets.append( + tuple(getattr(target, "target_id") for target in targets) + ) + self.hold_fingerprints.append( + tuple(getattr(target, "address_fingerprint") for target in targets) + ) + if self.reject_hold: + return CommandAcknowledgement.rejected_ack("hold rejected") + return CommandAcknowledgement.accepted_ack() + + def cancel( + self, + targets: tuple[object, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + del targets + self.timeouts.append(("cancel", timeout)) + self.operations.append("cancel") + self.cancels += 1 + if self.reject_cancel: + return CommandAcknowledgement.rejected_ack("cancel rejected") + return CommandAcknowledgement.accepted_ack() + + +class _AcceptSafety: + """Accept fake joint commands while recording validation calls.""" + + def __init__(self) -> None: + self.calls = 0 + + def validate( + self, + *, + branch_frames: dict[str, RuntimeCommandFrame], + merged_frame: RuntimeCommandFrame, + ) -> None: + assert branch_frames + assert merged_frame.commands + self.calls += 1 + + +class _RejectSafety: + """Reject every synchronized motion as physically unsafe.""" + + def validate( + self, + *, + branch_frames: dict[str, RuntimeCommandFrame], + merged_frame: RuntimeCommandFrame, + ) -> None: + del branch_frames, merged_frame + raise RuntimeError("predicted self collision") + + +@dataclass(frozen=True, slots=True) +class _ScriptStep: + """One fake lane cycle.""" + + status: SkillStatus + eligible: torch.Tensor + success: torch.Tensor + failure: torch.Tensor + cancelled: torch.Tensor + frame: RuntimeCommandFrame | None = None + task_state: TaskState | None = None + wait_duration: float = 0.0 + emit_hold: bool = False + hold_targets: tuple[JointPositionTarget, ...] = () + + +class _BranchRuntime: + """Small deterministic implementation of the parallel runtime protocol.""" + + def __init__( + self, + script: tuple[_ScriptStep, ...], + sink: ParallelLaneCommandSink, + *, + initial_state: TaskState | None = None, + emit_terminal_hold: bool = True, + ) -> None: + self._script = script + self._sink = sink + self._index = 0 + self._state = initial_state or TaskState.empty(2, "cpu") + self._emit_terminal_hold = emit_terminal_hold + self._result = self._make_result( + SkillStatus.IDLE, + eligible=torch.ones(2, dtype=torch.bool), + ) + + @property + def result(self) -> SkillResult: + return self._result + + @property + def step_count(self) -> int: + return self._index + + def start( + self, + *calls: RegisteredSemanticCall, + workflow_id: str, + eligible_mask: torch.Tensor | None = None, + ) -> SkillResult: + del calls + eligible = ( + torch.ones(2, dtype=torch.bool) + if eligible_mask is None + else eligible_mask.clone() + ) + self._result = self._make_result( + SkillStatus.RUNNING, + workflow_id=workflow_id, + eligible=eligible, + ) + return self._result + + def step(self) -> SkillResult: + scripted = self._script[min(self._index, len(self._script) - 1)] + self._index += 1 + if scripted.frame is not None: + self._sink.send(scripted.frame, timeout=1.0) + self._state = scripted.task_state or self._state + if scripted.hold_targets: + self._sink.hold( + scripted.hold_targets, + _context(self._state), + timeout=1.0, + ) + elif scripted.emit_hold or ( + scripted.status is not SkillStatus.RUNNING and self._emit_terminal_hold + ): + last_frame = scripted.frame or self._sink.last_frame + targets = () if last_frame is None else last_frame.targets + self._sink.hold(targets, _context(self._state), timeout=1.0) + self._result = self._make_result( + scripted.status, + workflow_id=self._result.workflow_id, + eligible=scripted.eligible & ~self._result.cancelled_mask, + success=scripted.success & ~self._result.cancelled_mask, + failure=scripted.failure, + cancelled=self._result.cancelled_mask | scripted.cancelled, + wait_duration=scripted.wait_duration, + ) + return self._result + + def deactivate_rows( + self, + env_mask: torch.Tensor, + *, + reason: str, + ) -> SkillResult: + del reason + changed = env_mask & self._result.eligible_mask + self._result = self._make_result( + self._result.status, + workflow_id=self._result.workflow_id, + eligible=self._result.eligible_mask & ~changed, + success=self._result.success_mask & ~changed, + failure=self._result.failure_mask, + cancelled=self._result.cancelled_mask | changed, + wait_duration=self._result.wait_duration, + ) + return self._result + + def cancel(self, reason: str) -> SkillResult: + del reason + active = self._result.eligible_mask & ~self._result.failure_mask + last_frame = self._sink.last_frame + targets = () if last_frame is None else last_frame.targets + self._sink.cancel(targets, timeout=1.0) + self._sink.hold(targets, _context(self._state), timeout=1.0) + self._result = self._make_result( + SkillStatus.CANCELLED, + workflow_id=self._result.workflow_id, + eligible=self._result.eligible_mask & ~active, + failure=self._result.failure_mask, + cancelled=self._result.cancelled_mask | active, + ) + return self._result + + def _make_result( + self, + status: SkillStatus, + *, + workflow_id: str | None = None, + eligible: torch.Tensor | None = None, + success: torch.Tensor | None = None, + failure: torch.Tensor | None = None, + cancelled: torch.Tensor | None = None, + wait_duration: float = 0.0, + ) -> SkillResult: + zeros = torch.zeros(2, dtype=torch.bool) + return SkillResult( + status=status, + workflow_id=workflow_id, + current_call_index=0 if status is SkillStatus.RUNNING else None, + env_ids=ENV_IDS, + success_mask=zeros if success is None else success, + failure_mask=zeros if failure is None else failure, + cancelled_mask=zeros if cancelled is None else cancelled, + eligible_mask=( + torch.ones(2, dtype=torch.bool) if eligible is None else eligible + ), + task_state=self._state, + wait_duration=wait_duration, + ) + + +def _mask(first: bool, second: bool) -> torch.Tensor: + return torch.tensor([first, second], dtype=torch.bool) + + +def _context(task_state: TaskState) -> PlanningContext: + return PlanningContext( + robot=RobotObservation( + timestamp=1.0, + qpos=torch.zeros(2, 3), + qvel=torch.zeros(2, 3), + ), + task=task_state, + scene=SceneSnapshot.empty(), + env_ids=ENV_IDS, + ) + + +def _frame(joint_id: int, values: tuple[float, float]) -> RuntimeCommandFrame: + target = JointPositionTarget(f"resource_{joint_id}", (joint_id,)) + return RuntimeCommandFrame( + commands=( + EndpointCommand( + target, + JointPositionPayload(torch.tensor(values).reshape(2, 1)), + ), + ), + active_mask=_mask(True, True), + env_ids=ENV_IDS, + hold_duration=torch.full((2,), 0.1), + ) + + +def _branch( + branch_id: str, + joint_id: int, + script: tuple[_ScriptStep, ...], + *, + initial_state: TaskState | None = None, + emit_terminal_hold: bool = True, +) -> ParallelRuntimeBranch: + sink = ParallelLaneCommandSink() + return ParallelRuntimeBranch( + branch_id=branch_id, + calls=(RegisteredSemanticCall(f"test.{branch_id}"),), + claim=ResourceClaim(frozenset({f"resource_{joint_id}"}), (joint_id,)), + runtime=_BranchRuntime( + script, + sink, + initial_state=initial_state, + emit_terminal_hold=emit_terminal_hold, + ), + command_sink=sink, + ) + + +def _running_step( + *, + frame: RuntimeCommandFrame | None = None, + eligible: torch.Tensor | None = None, + failure: torch.Tensor | None = None, + task_state: TaskState | None = None, + wait_duration: float = 0.0, + emit_hold: bool = False, + hold_targets: tuple[JointPositionTarget, ...] = (), +) -> _ScriptStep: + return _ScriptStep( + SkillStatus.RUNNING, + _mask(True, True) if eligible is None else eligible, + _mask(False, False), + _mask(False, False) if failure is None else failure, + _mask(False, False), + frame, + task_state, + wait_duration=wait_duration, + emit_hold=emit_hold, + hold_targets=hold_targets, + ) + + +def _completed_step( + *, + frame: RuntimeCommandFrame | None = None, + success: torch.Tensor | None = None, + failure: torch.Tensor | None = None, + task_state: TaskState | None = None, +) -> _ScriptStep: + succeeded = _mask(True, True) if success is None else success + failed = _mask(False, False) if failure is None else failure + return _ScriptStep( + SkillStatus.COMPLETED, + succeeded, + succeeded, + failed, + _mask(False, False), + frame, + task_state, + ) + + +def test_parallel_runtime_merges_one_frame_and_hold_pads_short_lane() -> None: + left_state = TaskState.empty(2, "cpu") + left_state = StateDelta( + articulation_joint_updates={ + ("left_fixture", "joint"): ArticulationJointState(torch.full((2, 1), 0.5)) + } + ).apply(left_state, _mask(True, True)) + right_state = TaskState.empty(2, "cpu") + right_state = StateDelta( + articulation_joint_updates={ + ("right_fixture", "joint"): ArticulationJointState(torch.full((2, 1), 1.0)) + } + ).apply(right_state, _mask(True, True)) + left = _branch( + "left", + 0, + ( + _running_step(frame=_frame(0, (1.0, 1.0))), + _completed_step(task_state=left_state), + ), + ) + right = _branch( + "right", + 1, + ( + _running_step(frame=_frame(1, (2.0, 2.0))), + _running_step(frame=_frame(1, (3.0, 3.0))), + _completed_step(task_state=right_state), + ), + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=8, + ) + + result = runtime.start() + assert result.status is SkillStatus.RUNNING + result = runtime.step() + assert len(outbound.frames) == 1 + assert len(outbound.frames[0].commands) == 2 + assert outbound.operations == ["send"] + assert isinstance(left.runtime, _BranchRuntime) + assert isinstance(right.runtime, _BranchRuntime) + first_lane_steps = (left.runtime.step_count, right.runtime.step_count) + same_tick = runtime.step() + assert same_tick.wait_duration == pytest.approx(0.1) + assert outbound.operations == ["send"] + assert (left.runtime.step_count, right.runtime.step_count) == first_lane_steps + + clock.time = 0.1 + result = runtime.step() + assert result.status is SkillStatus.RUNNING + assert outbound.operations == ["send", "hold"] + assert len(outbound.frames) == 1 + branch_steps = (left.runtime.step_count, right.runtime.step_count) + same_tick = runtime.step() + assert same_tick.wait_duration == pytest.approx(0.1) + assert outbound.operations == ["send", "hold"] + assert (left.runtime.step_count, right.runtime.step_count) == branch_steps + + clock.time = 0.2 + result = runtime.step() + assert result.status is SkillStatus.RUNNING + assert outbound.operations == ["send", "hold", "send"] + assert len(outbound.frames[1].commands) == 1 + assert outbound.frames[1].commands[0].target.target_id == "resource_1" + assert (left.runtime.step_count, right.runtime.step_count) == branch_steps + + clock.time = 0.3 + result = runtime.step() + + assert result.status is SkillStatus.COMPLETED + assert result.command_count == 2 + assert outbound.holds == 2 + assert outbound.operations == ["send", "hold", "send", "hold"] + assert ( + result.task_state.get_articulation_joint_state("left_fixture", "joint") + is not None + ) + assert ( + result.task_state.get_articulation_joint_state("right_fixture", "joint") + is not None + ) + metadata = result.to_metadata() + json.dumps(metadata, allow_nan=False, sort_keys=True) + assert metadata["kind"] == "parallel_skill_result" + assert list(metadata["branches"]) == ["left", "right"] + assert metadata["elapsed_steps"] == 3 + + +def test_deferred_command_waits_for_clock_after_padding_hold() -> None: + left = _branch( + "left", + 0, + ( + _running_step( + frame=_frame(0, (1.0, 1.0)), + emit_hold=True, + ), + ), + ) + right = _branch( + "right", + 1, + (_running_step(frame=_frame(1, (2.0, 2.0))),), + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + padded = runtime.step() + lane_steps = (left.runtime.step_count, right.runtime.step_count) + same_tick = runtime.step() + + assert padded.status is SkillStatus.RUNNING + assert same_tick.wait_duration == pytest.approx(0.1) + assert outbound.operations == ["hold"] + assert not outbound.frames + assert (left.runtime.step_count, right.runtime.step_count) == lane_steps + + clock.time = 0.1 + runtime.step() + + assert outbound.operations == ["hold", "send"] + assert len(outbound.frames) == 1 + assert (left.runtime.step_count, right.runtime.step_count) == lane_steps + + +def test_completion_hold_waits_for_clock_after_accepted_command() -> None: + left = _branch( + "left", + 0, + ( + _running_step(frame=_frame(0, (1.0, 1.0))), + _completed_step(), + ), + ) + right = _branch( + "right", + 1, + ( + _running_step(frame=_frame(1, (2.0, 2.0))), + _completed_step(), + ), + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + runtime.step() + lane_steps = (left.runtime.step_count, right.runtime.step_count) + same_tick = runtime.step() + + assert same_tick.status is SkillStatus.RUNNING + assert same_tick.wait_duration == pytest.approx(0.1) + assert outbound.operations == ["send"] + assert (left.runtime.step_count, right.runtime.step_count) == lane_steps + + clock.time = 0.1 + completed = runtime.step() + + assert completed.status is SkillStatus.COMPLETED + assert outbound.operations == ["send", "hold"] + + +def test_parallel_runtime_uses_runner_transport_timeouts() -> None: + """Merged sends and safe stops share the selected preset runner policy.""" + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + ( + _branch("left", 0, (_running_step(frame=_frame(0, (1.0, 1.0))),)), + _branch("right", 1, (_running_step(frame=_frame(1, (2.0, 2.0))),)), + ), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + runner_cfg=ExecutionRunnerCfg( + command_timeout=0.25, + safe_stop_timeout=0.75, + hold_on_completion=False, + ), + ) + + runtime.start() + runtime.step() + runtime.cancel("operator stop") + + assert outbound.timeouts == [ + ("send", pytest.approx(0.25)), + ("cancel", pytest.approx(0.75)), + ("hold", pytest.approx(0.75)), + ] + + +def test_parallel_failure_safe_holds_when_completion_hold_is_disabled() -> None: + """Failure policy always cancels and holds independently of success policy.""" + outbound = _OutboundSink() + runtime = ParallelSkillRuntime( + ( + _branch("left", 0, (_running_step(frame=_frame(0, (1.0, 1.0))),)), + _branch("right", 1, (_running_step(frame=_frame(1, (2.0, 2.0))),)), + ), + outbound, + _Clock(), + ParallelTimingPolicy(0.1), + _RejectSafety(), + timeout_steps=5, + runner_cfg=ExecutionRunnerCfg(hold_on_completion=False), + ) + + runtime.start() + result = runtime.step() + + assert result.status is SkillStatus.FAILED + assert outbound.operations == ["cancel", "hold"] + + +def test_parallel_completion_respects_disabled_completion_hold() -> None: + """Successful completion does not synthesize a hold when policy disables it.""" + left = _branch( + "left", + 0, + ( + _running_step(frame=_frame(0, (1.0, 1.0))), + _completed_step(), + ), + emit_terminal_hold=False, + ) + right = _branch( + "right", + 1, + ( + _running_step(frame=_frame(1, (2.0, 2.0))), + _completed_step(), + ), + emit_terminal_hold=False, + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + runner_cfg=ExecutionRunnerCfg(hold_on_completion=False), + ) + + runtime.start() + runtime.step() + clock.time = 0.1 + result = runtime.step() + + assert result.status is SkillStatus.COMPLETED + assert outbound.operations == ["send"] + + +def test_parallel_minimum_cycle_time_limits_coordinator_cadence() -> None: + """Coordinator dispatches no faster than the preset's minimum cycle time.""" + left = _branch( + "left", + 0, + ( + _running_step(frame=_frame(0, (1.0, 1.0))), + _running_step(frame=_frame(0, (3.0, 3.0))), + ), + ) + right = _branch( + "right", + 1, + ( + _running_step(frame=_frame(1, (2.0, 2.0))), + _running_step(frame=_frame(1, (4.0, 4.0))), + ), + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + runner_cfg=ExecutionRunnerCfg(minimum_cycle_time=0.25), + ) + + runtime.start() + first = runtime.step() + clock.time = 0.1 + waiting = runtime.step() + clock.time = 0.25 + runtime.step() + + assert first.wait_duration == pytest.approx(0.25) + assert waiting.wait_duration == pytest.approx(0.15) + assert len(outbound.frames) == 2 + + +def test_parallel_runtime_fail_fast_is_row_local() -> None: + left = _branch( + "left", + 0, + ( + _running_step( + frame=_frame(0, (1.0, 1.0)), + eligible=_mask(False, True), + failure=_mask(True, False), + ), + _completed_step( + success=_mask(False, True), + failure=_mask(True, False), + ), + ), + ) + right = _branch( + "right", + 1, + ( + _running_step(frame=_frame(1, (3.0, 3.0))), + _completed_step( + success=_mask(False, True), + ), + ), + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + first = runtime.step() + + assert torch.equal(first.failure_mask, _mask(True, False)) + assert torch.equal( + first.branch_results["right"].cancelled_mask, + _mask(True, False), + ) + assert torch.equal(outbound.frames[0].active_mask, _mask(False, True)) + + clock.time = 0.1 + result = runtime.step() + assert result.status is SkillStatus.FAILED + assert torch.equal(result.failure_mask, _mask(True, False)) + assert torch.equal(result.success_mask, _mask(False, True)) + + +def test_parallel_failure_without_fresh_peer_frame_forces_masked_dispatch() -> None: + left = _branch( + "left", + 0, + ( + _running_step(frame=_frame(0, (1.0, 1.0))), + _running_step( + eligible=_mask(False, True), + failure=_mask(True, False), + ), + ), + ) + right = _branch( + "right", + 1, + ( + _running_step(frame=_frame(1, (2.0, 2.0))), + _running_step(wait_duration=0.1), + ), + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + runtime.step() + assert torch.equal(outbound.frames[-1].active_mask, _mask(True, True)) + + # The failure update has no fresh frame from either lane. The coordinator + # still replays the last transaction with row 0 inactive. + clock.time = 0.1 + runtime.step() + assert len(outbound.frames) == 2 + assert torch.equal(outbound.frames[-1].active_mask, _mask(False, True)) + + +def test_parallel_timeout_counts_completed_environment_steps() -> None: + left = _branch("left", 0, (_running_step(wait_duration=0.1),)) + right = _branch("right", 1, (_running_step(wait_duration=0.1),)) + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + _OutboundSink(), + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=1, + ) + + runtime.start() + before_step = runtime.step() + assert before_step.status is SkillStatus.RUNNING + assert before_step.elapsed_steps == 0 + + clock.time = 0.1 + timed_out = runtime.step() + assert timed_out.status is SkillStatus.FAILED + assert timed_out.elapsed_steps == 1 + assert torch.equal(timed_out.failure_mask, _mask(True, True)) + + +def test_parallel_timeout_does_not_execute_deadline_tick() -> None: + left_runtime_steps = ( + _running_step(frame=_frame(0, (1.0, 1.0)), wait_duration=0.1), + _running_step(frame=_frame(0, (2.0, 2.0))), + ) + right_runtime_steps = ( + _running_step(frame=_frame(1, (3.0, 3.0)), wait_duration=0.1), + _running_step(frame=_frame(1, (4.0, 4.0))), + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + ( + _branch("left", 0, left_runtime_steps), + _branch("right", 1, right_runtime_steps), + ), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=1, + ) + + runtime.start() + runtime.step() + assert len(outbound.frames) == 1 + clock.time = 0.1 + result = runtime.step() + + assert result.status is SkillStatus.FAILED + assert len(outbound.frames) == 1 + assert outbound.cancels == 1 + assert outbound.holds == 1 + + +def test_parallel_timeout_discards_frame_deferred_behind_completion_hold() -> None: + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + ( + _branch( + "left", + 0, + ( + _running_step( + frame=_frame(0, (1.0, 1.0)), + emit_hold=True, + ), + ), + ), + _branch( + "right", + 1, + (_running_step(frame=_frame(1, (2.0, 2.0))),), + ), + ), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=1, + ) + + runtime.start() + padded = runtime.step() + assert padded.status is SkillStatus.RUNNING + assert outbound.operations == ["hold"] + assert not outbound.frames + + clock.time = 0.1 + timed_out = runtime.step() + + assert timed_out.status is SkillStatus.FAILED + assert torch.equal(timed_out.failure_mask, _mask(True, True)) + assert not timed_out.success_mask.any() + assert not outbound.frames + assert outbound.operations == ["hold", "cancel", "hold"] + + +def test_parallel_cancel_discards_deferred_frame_and_covers_started_rows() -> None: + outbound = _OutboundSink() + runtime = ParallelSkillRuntime( + ( + _branch( + "left", + 0, + ( + _running_step( + frame=_frame(0, (1.0, 1.0)), + emit_hold=True, + ), + ), + ), + _branch( + "right", + 1, + (_running_step(frame=_frame(1, (2.0, 2.0))),), + ), + ), + outbound, + _Clock(), + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + runtime.step() + cancelled = runtime.cancel("operator stop during padding") + + assert cancelled.status is SkillStatus.CANCELLED + assert torch.equal(cancelled.cancelled_mask, _mask(True, True)) + assert not cancelled.success_mask.any() + assert not cancelled.failure_mask.any() + assert not outbound.frames + assert outbound.operations == ["hold", "cancel", "hold"] + + +def test_deferred_frame_validation_failure_does_not_advance_lanes() -> None: + outbound = _OutboundSink() + clock = _Clock() + left = _branch( + "left", + 0, + ( + _running_step( + frame=_frame(0, (1.0, 1.0)), + emit_hold=True, + ), + ), + ) + right = _branch( + "right", + 1, + (_running_step(frame=_frame(1, (2.0, 2.0))),), + ) + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _RejectSafety(), + timeout_steps=5, + ) + + runtime.start() + runtime.step() + assert isinstance(left.runtime, _BranchRuntime) + assert isinstance(right.runtime, _BranchRuntime) + steps_before_dispatch = (left.runtime.step_count, right.runtime.step_count) + + clock.time = 0.1 + failed = runtime.step() + + assert failed.status is SkillStatus.FAILED + assert (left.runtime.step_count, right.runtime.step_count) == steps_before_dispatch + assert not outbound.frames + assert outbound.operations == ["hold", "cancel", "hold"] + + +def test_terminal_fresh_frames_fail_closed_without_post_command_observation() -> None: + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + ( + _branch( + "left", + 0, + (_completed_step(frame=_frame(0, (1.0, 1.0))),), + ), + _branch( + "right", + 1, + (_completed_step(frame=_frame(1, (2.0, 2.0))),), + ), + ), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + result = runtime.step() + + assert result.status is SkillStatus.FAILED + assert torch.equal(result.failure_mask, _mask(True, True)) + assert not result.success_mask.any() + assert not outbound.frames + assert outbound.operations == ["cancel", "hold"] + assert "post-command observation" in (result.message or "") + + +def test_hold_aggregation_preserves_same_destination_distinct_fingerprints() -> None: + target_a = JointPositionTarget("shared_arm", (0,)) + target_b = JointPositionTarget("shared_arm", (1,)) + lane_sink = ParallelLaneCommandSink() + lane_sink.hold( + (target_a, target_b), + _context(TaskState.empty(2, "cpu")), + timeout=1.0, + ) + pending_targets, _ = lane_sink.hold_request + assert tuple(target.address_fingerprint for target in pending_targets) == ( + target_a.address_fingerprint, + target_b.address_fingerprint, + ) + + outbound = _OutboundSink() + runtime = ParallelSkillRuntime( + ( + _branch( + "left", + 0, + (_running_step(hold_targets=(target_a, target_b)),), + ), + _branch("right", 2, (_running_step(wait_duration=0.1),)), + ), + outbound, + _Clock(), + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + runtime.step() + + assert outbound.hold_targets == [("shared_arm", "shared_arm")] + assert outbound.hold_fingerprints == [ + (target_a.address_fingerprint, target_b.address_fingerprint) + ] + + +def test_parallel_lane_does_not_drop_prior_call_completion_hold() -> None: + left_sink = ParallelLaneCommandSink() + left = ParallelRuntimeBranch( + branch_id="left", + calls=( + RegisteredSemanticCall("test.left_first"), + RegisteredSemanticCall("test.left_second"), + ), + claim=ResourceClaim(frozenset({"left"}), (0, 2)), + runtime=_BranchRuntime( + ( + _running_step( + frame=_frame(0, (1.0, 1.0)), + emit_hold=True, + ), + _running_step(frame=_frame(2, (2.0, 2.0))), + _completed_step(), + ), + left_sink, + ), + command_sink=left_sink, + ) + right = _branch( + "right", + 1, + ( + _running_step(frame=_frame(1, (3.0, 3.0))), + _running_step(frame=_frame(1, (4.0, 4.0))), + _completed_step(), + ), + ) + outbound = _OutboundSink() + clock = _Clock() + runtime = ParallelSkillRuntime( + (left, right), + outbound, + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + runtime.step() + result = runtime.result + for step_index in range(1, 8): + if result.terminal: + break + clock.time = step_index * 0.1 + result = runtime.step() + + assert result.status is SkillStatus.COMPLETED + assert any("resource_0" in targets for targets in outbound.hold_targets) + assert any("resource_2" in targets for targets in outbound.hold_targets) + + +def test_parallel_runtime_rejects_overlapping_claims_before_start() -> None: + script = (_running_step(),) + left = _branch("left", 0, script) + right_sink = ParallelLaneCommandSink() + right = ParallelRuntimeBranch( + branch_id="right", + calls=(RegisteredSemanticCall("test.right"),), + claim=ResourceClaim(frozenset({"different_name"}), (0,)), + runtime=_BranchRuntime(script, right_sink), + command_sink=right_sink, + ) + + with pytest.raises(ValueError, match="overlapping resource claims"): + ParallelSkillRuntime( + (left, right), + _OutboundSink(), + _Clock(), + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + +def test_parallel_runtime_requires_equal_branch_barrier_state() -> None: + changed = StateDelta( + articulation_joint_updates={ + ("fixture", "joint"): ArticulationJointState(torch.ones(2, 1)) + } + ).apply(TaskState.empty(2, "cpu"), _mask(True, True)) + + with pytest.raises(ValueError, match="same verified TaskState"): + ParallelSkillRuntime( + ( + _branch("left", 0, (_running_step(),)), + _branch( + "right", + 1, + (_running_step(),), + initial_state=changed, + ), + ), + _OutboundSink(), + _Clock(), + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + +def test_terminal_targets_without_hold_context_fail_closed() -> None: + clock = _Clock() + runtime = ParallelSkillRuntime( + ( + _branch( + "left", + 0, + ( + _running_step(frame=_frame(0, (1.0, 1.0))), + _completed_step(), + ), + emit_terminal_hold=False, + ), + _branch( + "right", + 1, + ( + _running_step(frame=_frame(1, (2.0, 2.0))), + _completed_step(), + ), + emit_terminal_hold=False, + ), + ), + _OutboundSink(), + clock, + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + + runtime.start() + result = runtime.step() + assert result.status is SkillStatus.RUNNING + clock.time = 0.1 + result = runtime.step() + + assert result.status is SkillStatus.FAILED + assert torch.equal(result.failure_mask, _mask(True, True)) + assert "no synchronized planning context" in (result.message or "") + + +@pytest.mark.parametrize( + ("sink_kwargs", "expected_status"), + [ + ({}, SkillStatus.CANCELLED), + ({"reject_cancel": True}, SkillStatus.FAILED), + ({"reject_hold": True}, SkillStatus.FAILED), + ], +) +def test_parallel_caller_cancel_checks_cancel_and_hold_acknowledgements( + sink_kwargs: dict[str, bool], + expected_status: SkillStatus, +) -> None: + outbound = _OutboundSink(**sink_kwargs) + runtime = ParallelSkillRuntime( + ( + _branch("left", 0, (_running_step(frame=_frame(0, (1.0, 1.0))),)), + _branch("right", 1, (_running_step(frame=_frame(1, (2.0, 2.0))),)), + ), + outbound, + _Clock(), + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + runtime.start() + runtime.step() + + result = runtime.cancel("operator stop") + + assert result.status is expected_status + assert outbound.cancels == 1 + assert outbound.holds >= 1 + if expected_status is SkillStatus.CANCELLED: + assert torch.equal(result.cancelled_mask, _mask(True, True)) + assert not result.failure_mask.any() + else: + assert torch.equal(result.failure_mask, _mask(True, True)) + assert not result.cancelled_mask.any() + + +@pytest.mark.parametrize( + ("safety", "sink"), + [ + (_RejectSafety(), _OutboundSink()), + (_AcceptSafety(), _OutboundSink(raise_send=True)), + ], +) +def test_parallel_tick_exception_safe_stops_with_disjoint_failure_masks( + safety: object, + sink: _OutboundSink, +) -> None: + runtime = ParallelSkillRuntime( + ( + _branch("left", 0, (_running_step(frame=_frame(0, (1.0, 1.0))),)), + _branch("right", 1, (_running_step(frame=_frame(1, (2.0, 2.0))),)), + ), + sink, + _Clock(), + ParallelTimingPolicy(0.1), + safety, + timeout_steps=5, + ) + runtime.start() + + result = runtime.step() + + assert result.status is SkillStatus.FAILED + assert torch.equal(result.failure_mask, _mask(True, True)) + assert not result.success_mask.any() + assert not result.cancelled_mask.any() + assert sink.cancels == 1 + assert sink.holds == 1 + + +def test_cancel_preserves_verified_state_from_an_earlier_branch_call() -> None: + changed = StateDelta( + articulation_joint_updates={ + ("fixture", "joint"): ArticulationJointState(torch.ones(2, 1)) + } + ).apply(TaskState.empty(2, "cpu"), _mask(True, True)) + runtime = ParallelSkillRuntime( + ( + _branch( + "left", + 0, + ( + _running_step( + frame=_frame(0, (1.0, 1.0)), + task_state=changed, + ), + ), + ), + _branch("right", 1, (_running_step(frame=_frame(1, (2.0, 2.0))),)), + ), + _OutboundSink(), + _Clock(), + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + runtime.start() + runtime.step() + + result = runtime.cancel() + + assert result.status is SkillStatus.CANCELLED + assert ( + result.task_state.get_articulation_joint_state("fixture", "joint") is not None + ) + + +def test_disjoint_intrinsic_rows_still_conflict_on_same_unpartitioned_key() -> None: + initial = TaskState.empty(2, "cpu") + left_state = StateDelta( + articulation_joint_updates={ + ("fixture", "joint"): ArticulationJointState(torch.ones(2, 1)) + } + ).apply(initial, _mask(True, False)) + right_state = StateDelta( + articulation_joint_updates={ + ("fixture", "joint"): ArticulationJointState(torch.full((2, 1), 2.0)) + } + ).apply(initial, _mask(False, True)) + runtime = ParallelSkillRuntime( + ( + _branch( + "left", + 0, + (_running_step(frame=_frame(0, (1.0, 1.0)), task_state=left_state),), + ), + _branch( + "right", + 1, + (_running_step(frame=_frame(1, (2.0, 2.0)), task_state=right_state),), + ), + ), + _OutboundSink(), + _Clock(), + ParallelTimingPolicy(0.1), + _AcceptSafety(), + timeout_steps=5, + ) + runtime.start() + runtime.step() + + result = runtime.cancel() + + assert result.status is SkillStatus.FAILED + assert torch.equal(result.failure_mask, _mask(True, True)) + assert not result.cancelled_mask.any() + + +__all__: list[str] = [] diff --git a/tests/sim/skills/test_profiles.py b/tests/sim/skills/test_profiles.py new file mode 100644 index 000000000..6aaef9844 --- /dev/null +++ b/tests/sim/skills/test_profiles.py @@ -0,0 +1,1762 @@ +# ---------------------------------------------------------------------------- +# 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 generic robot resources and declarative skill profiles.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import ClassVar +from unittest.mock import Mock + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + ActionOptions, + ActionPlan, + AtomicAction, + AtomicActionEngine, + BATCH_INVERSE_KINEMATICS_CAPABILITY, + BUILTIN_ACTION_TYPES, + CARTESIAN_POSE_CAPABILITY, + ControlCommand, + ControlPartCommandProfile, + DisjointSlotEndpoints, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + GRASP_COMMAND, + INVERSE_KINEMATICS_CAPABILITY, + JOINT_POSITION_CAPABILITY, + JointPositionCommand, + JointPositionGoal, + MotionPolicy, + OPEN_COMMAND, + PickUpOptions, + ResolvedActionRequest, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) +from embodichain.lab.sim.atomic_actions.bindings import ( + JointPositionTarget, + RuntimeEndpointTarget, +) +from embodichain.lab.sim.atomic_actions.state import PlanningContext +from embodichain.lab.sim.atomic_actions.tracking import ( + JOINT_POSITION_CHANNEL, + EndpointTrackingFeedbackAddress, + JointPositionTrackingMetric, + TrackingPolicy, +) +from embodichain.lab.sim.skills import ( + AmbiguousSkillBindingError, + COMPOSITE_EFFECT_MONITOR_ID, + COMPOSITE_EFFECT_MONITOR_REVISION, + CONSTRAINT_EFFECT_CHANNEL, + CONTACT_EFFECT_CHANNEL, + ControlPartEndpoint, + ControlPartEndpointAdapter, + ControlPartEvidenceAddress, + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + EffectEvidenceSourceRef, + EffectMonitorRef, + EndpointResolution, + FORCE_EFFECT_CHANNEL, + JOINT_STATE_EFFECT_CHANNEL, + POSE_RELATION_EFFECT_CHANNEL, + ProfileValidationError, + ResourceBinding, + ResourceEndpoint, + ResourceEndpointAdapter, + RobotResource, + RobotSkillProfile, + SkillPolicyPreset, + UnsupportedSkillError, + WorkflowRecoveryPolicy, +) + +_JOINT_IDS = { + "left_arm": [0, 1], + "left_hand": [2], + "right_arm": [3, 4], + "right_hand": [5], + "base": [6, 7], + "torso": [8], + "full_body": [0, 1, 3, 4, 6, 7, 8], +} + +_MOTION_CAPABILITIES = frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + INVERSE_KINEMATICS_CAPABILITY, + JOINT_POSITION_CAPABILITY, + } +) + + +def _command_profiles() -> dict[str, ControlPartCommandProfile]: + return { + hand: ControlPartCommandProfile.joint_positions( + open=torch.tensor([0.0]), + grasp=torch.tensor([1.0]), + ) + for hand in ("left_hand", "right_hand") + } + + +def _engine( + *, + control_profiles: dict[str, ControlPartCommandProfile] | None = None, + load_builtins: bool = True, +) -> AtomicActionEngine: + robot = Mock() + robot.device = torch.device("cpu") + robot.dof = 9 + robot.control_parts = {name: object() for name in _JOINT_IDS} + robot.get_qpos.return_value = torch.zeros(2, 9) + robot.get_qvel.return_value = torch.zeros(2, 9) + robot.get_joint_ids.side_effect = lambda name: list(_JOINT_IDS[name]) + robot.get_solver.side_effect = lambda name=None: ( + object() if name in {"left_arm", "right_arm"} else None + ) + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "stub_planner" + return AtomicActionEngine( + generator, + control_profiles=control_profiles, + load_builtins=load_builtins, + ) + + +def _resources(*, include_right: bool = True) -> dict[str, RobotResource]: + resources = { + "left_arm": RobotResource( + "left_arm", + endpoints={"control": ControlPartEndpoint("left_arm")}, + ), + "left_hand": RobotResource( + "left_hand", + endpoints={"control": ControlPartEndpoint("left_hand")}, + ), + "left_actor": RobotResource( + "left_actor", + endpoints={ + "motion": ControlPartEndpoint( + "left_arm", capabilities=_MOTION_CAPABILITIES + ), + "grasp": ControlPartEndpoint( + "left_hand", capabilities=frozenset({GRASP_CAPABILITY}) + ), + }, + members=("left_arm", "left_hand"), + ), + "base": RobotResource( + "base", + endpoints={ + "motion": ControlPartEndpoint( + "base", capabilities=frozenset({"motion.base.se2"}) + ) + }, + ), + "torso": RobotResource( + "torso", + endpoints={"control": ControlPartEndpoint("torso")}, + ), + } + if include_right: + resources.update( + { + "right_arm": RobotResource( + "right_arm", + endpoints={"control": ControlPartEndpoint("right_arm")}, + ), + "right_hand": RobotResource( + "right_hand", + endpoints={"control": ControlPartEndpoint("right_hand")}, + ), + "right_actor": RobotResource( + "right_actor", + endpoints={ + "motion": ControlPartEndpoint( + "right_arm", capabilities=_MOTION_CAPABILITIES + ), + "grasp": ControlPartEndpoint( + "right_hand", + capabilities=frozenset({GRASP_CAPABILITY}), + ), + }, + members=("right_arm", "right_hand"), + ), + } + ) + whole_body_members = ["base", "torso", "left_arm"] + if include_right: + whole_body_members.append("right_arm") + if include_right: + resources["whole_body"] = RobotResource( + "whole_body", + endpoints={ + "motion": ControlPartEndpoint( + "full_body", capabilities=frozenset({"motion.whole_body"}) + ) + }, + members=tuple(whole_body_members), + ) + return resources + + +def _profile( + *, + defaults: dict[str, ResourceBinding] | None = None, + resources: dict[str, RobotResource] | None = None, + command_profiles: dict[str, ControlPartCommandProfile] | None = None, +) -> RobotSkillProfile: + return RobotSkillProfile( + profile_id="test_robot", + resources=_resources() if resources is None else resources, + command_profiles=( + _command_profiles() if command_profiles is None else command_profiles + ), + defaults={} if defaults is None else defaults, + ) + + +class _WholeBodyAction(AtomicAction[JointPositionGoal, ActionOptions]): + skill_id: ClassVar[str] = "whole_body_reach" + GoalType: ClassVar[type] = JointPositionGoal + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + "body", + endpoints=( + SkillEndpointRequirement( + "motion", + capabilities=frozenset({"motion.whole_body"}), + ), + ), + ), + ) + ) + + def _plan( + self, + request: ResolvedActionRequest[JointPositionGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + raise NotImplementedError + + +class _NavigateAction(AtomicAction[JointPositionGoal, ActionOptions]): + skill_id: ClassVar[str] = "navigate" + GoalType: ClassVar[type] = JointPositionGoal + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + "body", + endpoints=( + SkillEndpointRequirement( + "motion", + capabilities=frozenset({"motion.base.se2"}), + ), + ), + ), + ) + ) + + def _plan( + self, + request: ResolvedActionRequest[JointPositionGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + raise NotImplementedError + + +@dataclass(frozen=True, slots=True) +class _BaseVelocityEndpoint(ResourceEndpoint): + """Future non-joint endpoint used to prove the resource API stays generic.""" + + controller_id: str + claim_id: str | None = None + + +@dataclass(frozen=True, slots=True) +class _MutableMetadataEndpoint(ResourceEndpoint): + """Endpoint with mutable metadata used to verify ownership snapshots.""" + + controller_id: str + aliases: list[str] + + +@dataclass(frozen=True, slots=True) +class _BaseVelocityTarget(RuntimeEndpointTarget): + """Typed runtime destination for the test mobile controller.""" + + controller_id: str + + @property + def transport_id(self) -> str: + """Return the fake base-velocity transport kind.""" + return "test.base_velocity" + + @property + def target_id(self) -> str: + """Return the addressed controller ID.""" + return self.controller_id + + +@dataclass(frozen=True, slots=True) +class _MutableRuntimeTarget(RuntimeEndpointTarget): + """Target with nested mutable data used to prove snapshot ownership.""" + + controller_id: str + aliases: list[str] + + @property + def transport_id(self) -> str: + """Return the fake mutable-target transport kind.""" + return "test.mutable" + + @property + def target_id(self) -> str: + """Return the addressed controller ID.""" + return self.controller_id + + +@dataclass(frozen=True, slots=True) +class _TwistCommand(ControlCommand): + """Test-only non-joint command for a mobile controller.""" + + value: tuple[float, float, float] + + def snapshot(self) -> _TwistCommand: + """Return an independently owned immutable command.""" + return _TwistCommand(tuple(self.value)) + + def equivalent_to(self, other: ControlCommand) -> bool: + """Return whether another twist command has the same value.""" + return isinstance(other, _TwistCommand) and self.value == other.value + + +class _BaseVelocityEndpointAdapter(ResourceEndpointAdapter): + """Resolve the test mobile controller without profile-resolver changes.""" + + adapter_id: ClassVar[str] = "test.base_velocity" + endpoint_type: ClassVar[type[ResourceEndpoint]] = _BaseVelocityEndpoint + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: AtomicActionEngine, + ) -> EndpointResolution: + """Resolve one mobile controller to a generic exclusive claim.""" + del engine + assert isinstance(endpoint, _BaseVelocityEndpoint) + claim_id = ( + endpoint.controller_id if endpoint.claim_id is None else endpoint.claim_id + ) + return EndpointResolution( + runtime_target=_BaseVelocityTarget(endpoint.controller_id), + command_profile_key=endpoint.controller_id, + claim_tokens=frozenset({f"controller:{claim_id}"}), + ) + + +class _VelocityNavigateAction(AtomicAction[JointPositionGoal, ActionOptions]): + """Semantic test skill consuming a non-core controller endpoint.""" + + skill_id: ClassVar[str] = "navigate_velocity" + GoalType: ClassVar[type] = JointPositionGoal + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + "body", + endpoints=( + SkillEndpointRequirement( + "motion", + capabilities=frozenset({"motion.base.velocity"}), + required_commands={"stop": _TwistCommand}, + ), + ), + ), + ) + ) + + def _plan( + self, + request: ResolvedActionRequest[JointPositionGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + raise NotImplementedError + + +def test_engine_skills_only_exposes_visible_explicit_installed_contracts() -> None: + engine = _engine(control_profiles=_command_profiles()) + expected = { + action_type.skill_id + for action_type in BUILTIN_ACTION_TYPES + if action_type.agent_visible + } + + assert set(engine.skills) == expected + assert "move_joints" in engine.actions + assert "move_joints" not in engine.skills + + +def test_new_skill_subclass_must_redeclare_binding_contract() -> None: + base_contract = BUILTIN_ACTION_TYPES[0].descriptor().binding_contract + + class Derived(BUILTIN_ACTION_TYPES[0]): + skill_id: ClassVar[str] = "derived_without_explicit_contract" + + assert base_contract is not None + assert Derived.descriptor().binding_contract is None + + +def test_profile_owns_input_mappings_and_command_tensors() -> None: + resources = _resources() + open_positions = torch.tensor([0.0]) + profiles = { + "left_hand": ControlPartCommandProfile.joint_positions(open=open_positions) + } + profile = _profile(resources=resources, command_profiles=profiles) + + resources.clear() + profiles.clear() + open_positions.fill_(9.0) + + assert "left_actor" in profile.resources + command = profile.command_profiles["left_hand"].commands[OPEN_COMMAND] + assert isinstance(command, JointPositionCommand) + assert command.positions.tolist() == [0.0] + + +def test_profile_owns_custom_endpoint_nested_payloads() -> None: + source_aliases = ["base"] + resource = RobotResource( + "mobile_base", + endpoints={ + "motion": _MutableMetadataEndpoint( + "base_controller", + aliases=source_aliases, + ) + }, + ) + profile = RobotSkillProfile( + "mobile", + resources={"mobile_base": resource}, + ) + + source_aliases.append("source_mutation") + resource_endpoint = resource.endpoints["motion"] + assert isinstance(resource_endpoint, _MutableMetadataEndpoint) + resource_endpoint.aliases.append("resource_mutation") + profile_endpoint = profile.resources["mobile_base"].endpoints["motion"] + assert isinstance(profile_endpoint, _MutableMetadataEndpoint) + + assert profile_endpoint.aliases == ["base"] + + +def test_endpoint_resolution_requires_a_runtime_target() -> None: + with pytest.raises(TypeError, match="runtime_target"): + EndpointResolution( + runtime_target=None, # type: ignore[arg-type] + exclusive=False, + ) + + +def test_endpoint_resolution_owns_runtime_target_snapshot() -> None: + aliases = ["base"] + target = _MutableRuntimeTarget("base_controller", aliases) + + resolution = EndpointResolution(runtime_target=target, exclusive=False) + aliases.append("source_mutation") + target.aliases.append("target_mutation") + + assert resolution.runtime_target is not target + assert type(resolution.runtime_target) is _MutableRuntimeTarget + assert resolution.runtime_target.aliases == ["base"] + + +def test_endpoint_resolution_owns_and_freezes_effect_sources() -> None: + source = EffectEvidenceSourceRef( + "test.provider", + "1", + ControlPartEvidenceAddress("left_arm", POSE_RELATION_EFFECT_CHANNEL), + ) + sources = {POSE_RELATION_EFFECT_CHANNEL: source} + + resolution = EndpointResolution( + runtime_target=_BaseVelocityTarget("base_controller"), + task_state_key="mobile_actor", + effect_sources=sources, + exclusive=False, + ) + sources.clear() + + assert resolution.task_state_key == "mobile_actor" + assert tuple(resolution.effect_sources) == (POSE_RELATION_EFFECT_CHANNEL,) + assert resolution.effect_sources[POSE_RELATION_EFFECT_CHANNEL] is not source + assert resolution.effect_sources[POSE_RELATION_EFFECT_CHANNEL].address == ( + ControlPartEvidenceAddress("left_arm", POSE_RELATION_EFFECT_CHANNEL) + ) + with pytest.raises(TypeError): + resolution.effect_sources["new"] = source # type: ignore[index] + + +def test_control_part_adapter_declares_every_builtin_integration_route() -> None: + adapter = ControlPartEndpointAdapter + + assert adapter.runtime_transport_ids == frozenset( + {JointPositionTarget.TRANSPORT_ID} + ) + assert adapter.runtime_target_types == (JointPositionTarget,) + assert adapter.tracking_feedback_source_keys == frozenset( + {("planning_context.robot", "1")} + ) + assert adapter.tracking_projector_keys == frozenset( + {("joint_position_payload", "1")} + ) + assert adapter.effect_evidence_source_keys == frozenset( + { + ( + CONTROL_PART_EVIDENCE_PROVIDER_ID, + CONTROL_PART_EVIDENCE_PROVIDER_REVISION, + ) + } + ) + + +@pytest.mark.parametrize("returns_self", [False, True]) +def test_endpoint_resolution_rejects_invalid_target_snapshot( + returns_self: bool, +) -> None: + @dataclass(frozen=True, slots=True) + class InvalidSnapshotTarget(RuntimeEndpointTarget): + controller_id: str + + @property + def transport_id(self) -> str: + return "test.invalid_snapshot" + + @property + def target_id(self) -> str: + return self.controller_id + + def snapshot(self) -> RuntimeEndpointTarget: + if returns_self: + return self + return _BaseVelocityTarget(self.controller_id) + + with pytest.raises(TypeError, match="same target type"): + EndpointResolution( + runtime_target=InvalidSnapshotTarget("base_controller"), + exclusive=False, + ) + + +def test_resource_graph_rejects_unknown_member_and_cycle() -> None: + with pytest.raises(ValueError, match="unknown members"): + RobotSkillProfile( + "unknown_member", + resources={ + "group": RobotResource("group", members=("missing",)), + }, + ) + + with pytest.raises(ValueError, match="contains a cycle"): + RobotSkillProfile( + "cycle", + resources={ + "a": RobotResource("a", members=("b",)), + "b": RobotResource("b", members=("a",)), + }, + ) + + +def test_identifier_sets_do_not_accept_one_string_as_characters() -> None: + with pytest.raises(TypeError, match="not a string"): + ControlPartEndpoint("left_arm", capabilities="motion.cartesian_pose") + with pytest.raises(TypeError, match="not a string"): + RobotResource( + "left_arm", + endpoints={"control": ControlPartEndpoint("left_arm")}, + members="left_arm", + ) + with pytest.raises(TypeError, match="iterable of endpoint IDs"): + DisjointSlotEndpoints("motion") + + +def test_slot_constraint_rejects_unknown_endpoint() -> None: + with pytest.raises(ValueError, match="unknown endpoints"): + SkillResourceSlot( + "primary", + endpoints=(SkillEndpointRequirement("motion"),), + constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + ) + + +def test_bind_rejects_unknown_control_part() -> None: + resources = _resources() + resources["camera_gimbal"] = RobotResource( + "camera_gimbal", + endpoints={"motion": ControlPartEndpoint("missing")}, + ) + + with pytest.raises(ProfileValidationError, match="unknown control part 'missing'"): + _profile(resources=resources).bind( + _engine(control_profiles=_command_profiles()) + ) + + +def test_resource_graph_accepts_extensible_endpoint_before_adapter_installation() -> ( + None +): + resource = RobotResource( + "mobile_base", + endpoints={ + "motion": _BaseVelocityEndpoint( + "base_controller", + capabilities=frozenset({"motion.base.velocity"}), + ) + }, + ) + profile = RobotSkillProfile("mobile", resources={"mobile_base": resource}) + + assert ( + profile.resources["mobile_base"].endpoints["motion"] + == resource.endpoints["motion"] + ) + with pytest.raises(ProfileValidationError, match="ResourceEndpointAdapter"): + profile.bind(_engine(control_profiles={}, load_builtins=False)) + + +def test_custom_endpoint_adapter_resolves_commands_and_physical_claim() -> None: + resource = RobotResource( + "mobile_base", + endpoints={ + "motion": _BaseVelocityEndpoint( + "base_velocity", + capabilities=frozenset({"motion.base.velocity"}), + ) + }, + ) + profile = RobotSkillProfile( + "mobile", + resources={"mobile_base": resource}, + command_profiles={ + "base_velocity": ControlPartCommandProfile( + commands={"stop": _TwistCommand((0.0, 0.0, 0.0))} + ) + }, + ) + engine = _engine(control_profiles={}, load_builtins=False) + engine.register(_VelocityNavigateAction()) + + bound = engine.bind_skill_profile( + profile, + endpoint_adapters={_BaseVelocityEndpoint: _BaseVelocityEndpointAdapter()}, + ) + resolved = bound.resolve("navigate_velocity") + endpoint = resolved.resources["body"].endpoints["motion"] + binding_endpoint = resolved.action_binding.endpoint("body", "motion") + + assert endpoint.adapter_id == "test.base_velocity" + assert isinstance(endpoint.runtime_target, _BaseVelocityTarget) + assert isinstance(endpoint.commands["stop"], _TwistCommand) + assert resolved.claim.claim_tokens == frozenset({"controller:base_velocity"}) + assert resolved.action_binding.owner_id == engine.binding_owner_id + assert binding_endpoint.resource_id == "mobile_base" + assert binding_endpoint.require_target(_BaseVelocityTarget).controller_id == ( + "base_velocity" + ) + assert isinstance(binding_endpoint.command("stop"), _TwistCommand) + + +def test_custom_endpoint_joint_claim_survives_action_binding_lowering() -> None: + class JointClaimAdapter(_BaseVelocityEndpointAdapter): + """Attach robot-joint ownership to a non-joint runtime target.""" + + adapter_id: ClassVar[str] = "test.base_velocity_joint_claim" + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: AtomicActionEngine, + ) -> EndpointResolution: + del engine + assert isinstance(endpoint, _BaseVelocityEndpoint) + return EndpointResolution( + runtime_target=_BaseVelocityTarget(endpoint.controller_id), + command_profile_key=endpoint.controller_id, + joint_ids=(6, 7), + ) + + profile = RobotSkillProfile( + "mobile", + resources={ + "mobile_base": RobotResource( + "mobile_base", + endpoints={ + "motion": _BaseVelocityEndpoint( + "base_velocity", + capabilities=frozenset({"motion.base.velocity"}), + ) + }, + ) + }, + command_profiles={ + "base_velocity": ControlPartCommandProfile( + commands={"stop": _TwistCommand((0.0, 0.0, 0.0))} + ) + }, + ) + engine = _engine(control_profiles={}, load_builtins=False) + engine.register(_VelocityNavigateAction()) + bound = engine.bind_skill_profile( + profile, + endpoint_adapters={_BaseVelocityEndpoint: JointClaimAdapter()}, + ) + + binding_endpoint = bound.resolve("navigate_velocity").action_binding.endpoint( + "body", "motion" + ) + + assert binding_endpoint.joint_ids == (6, 7) + + +def test_custom_endpoint_joint_claim_must_fit_robot_dof() -> None: + class OutOfRangeJointClaimAdapter(_BaseVelocityEndpointAdapter): + adapter_id: ClassVar[str] = "test.out_of_range_joint_claim" + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: AtomicActionEngine, + ) -> EndpointResolution: + del engine + assert isinstance(endpoint, _BaseVelocityEndpoint) + return EndpointResolution( + runtime_target=_BaseVelocityTarget(endpoint.controller_id), + joint_ids=(9,), + ) + + profile = RobotSkillProfile( + "mobile", + resources={ + "mobile_base": RobotResource( + "mobile_base", + endpoints={ + "motion": _BaseVelocityEndpoint( + "base_velocity", + capabilities=frozenset({"motion.base.velocity"}), + ) + }, + ) + }, + ) + + with pytest.raises(ProfileValidationError, match="outside robot DOF 9"): + profile.bind( + _engine(control_profiles={}, load_builtins=False), + endpoint_adapters={ + _BaseVelocityEndpoint: OutOfRangeJointClaimAdapter(), + }, + ) + + +def test_engine_constructor_forwards_custom_endpoint_adapters() -> None: + source = _engine(control_profiles={}, load_builtins=False) + profile = RobotSkillProfile( + "mobile", + resources={ + "mobile_base": RobotResource( + "mobile_base", + endpoints={ + "motion": _BaseVelocityEndpoint( + "base_velocity", + capabilities=frozenset({"motion.base.velocity"}), + ) + }, + ) + }, + ) + + engine = AtomicActionEngine( + source.motion_generator, + load_builtins=False, + skill_profile=profile, + endpoint_adapters={_BaseVelocityEndpoint: _BaseVelocityEndpointAdapter()}, + ) + + assert engine.skill_profile is not None + assert engine.skill_profile.resources["mobile_base"].claim.claim_tokens == ( + frozenset({"controller:base_velocity"}) + ) + + +def test_custom_endpoint_claim_tokens_protect_distinct_leaf_aliases() -> None: + endpoint = _BaseVelocityEndpoint( + "base_velocity", + capabilities=frozenset({"motion.base.velocity"}), + ) + profile = RobotSkillProfile( + "aliased_mobile", + resources={ + "base_a": RobotResource("base_a", endpoints={"motion": endpoint}), + "base_b": RobotResource("base_b", endpoints={"motion": endpoint}), + }, + ) + + with pytest.raises(ProfileValidationError, match="adapter claims"): + profile.bind( + _engine(control_profiles={}, load_builtins=False), + endpoint_adapters={_BaseVelocityEndpoint: _BaseVelocityEndpointAdapter()}, + ) + + +def test_distinct_physical_leaves_cannot_share_one_runtime_target() -> None: + profile = RobotSkillProfile( + "duplicate_runtime_target", + resources={ + "base_a": RobotResource( + "base_a", + endpoints={ + "motion": _BaseVelocityEndpoint("shared", claim_id="base_a") + }, + ), + "base_b": RobotResource( + "base_b", + endpoints={ + "motion": _BaseVelocityEndpoint("shared", claim_id="base_b") + }, + ), + }, + ) + + with pytest.raises(ProfileValidationError, match="share runtime targets"): + profile.bind( + _engine(control_profiles={}, load_builtins=False), + endpoint_adapters={_BaseVelocityEndpoint: _BaseVelocityEndpointAdapter()}, + ) + + +def test_endpoint_adapter_cannot_omit_runtime_target() -> None: + class MissingRuntimeTargetAdapter(ResourceEndpointAdapter): + adapter_id: ClassVar[str] = "test.missing_runtime_target" + endpoint_type: ClassVar[type[ResourceEndpoint]] = _BaseVelocityEndpoint + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: AtomicActionEngine, + ) -> EndpointResolution: + del endpoint, engine + return EndpointResolution( + runtime_target=None, # type: ignore[arg-type] + exclusive=False, + ) + + profile = RobotSkillProfile( + "missing_runtime_target", + resources={ + "mobile_base": RobotResource( + "mobile_base", + endpoints={"motion": _BaseVelocityEndpoint("base_velocity")}, + ) + }, + ) + + with pytest.raises( + ProfileValidationError, + match="test.missing_runtime_target.*mobile_base.*motion.*runtime_target", + ): + profile.bind( + _engine(control_profiles={}, load_builtins=False), + endpoint_adapters={_BaseVelocityEndpoint: MissingRuntimeTargetAdapter()}, + ) + + +def test_exclusive_custom_endpoint_requires_a_physical_claim() -> None: + class EmptyClaimAdapter(ResourceEndpointAdapter): + adapter_id: ClassVar[str] = "test.empty_claim" + endpoint_type: ClassVar[type[ResourceEndpoint]] = _BaseVelocityEndpoint + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: AtomicActionEngine, + ) -> EndpointResolution: + del endpoint, engine + return EndpointResolution( + runtime_target=_BaseVelocityTarget("base_velocity") + ) + + profile = RobotSkillProfile( + "mobile", + resources={ + "mobile_base": RobotResource( + "mobile_base", + endpoints={"motion": _BaseVelocityEndpoint("base_velocity")}, + ) + }, + ) + + with pytest.raises( + ProfileValidationError, + match="test.empty_claim.*mobile_base.*motion.*joint_ids or claim_tokens", + ): + profile.bind( + _engine(control_profiles={}, load_builtins=False), + endpoint_adapters={_BaseVelocityEndpoint: EmptyClaimAdapter()}, + ) + + +def test_nonexclusive_custom_endpoint_may_omit_a_physical_claim() -> None: + class VirtualEndpointAdapter(ResourceEndpointAdapter): + adapter_id: ClassVar[str] = "test.virtual" + endpoint_type: ClassVar[type[ResourceEndpoint]] = _BaseVelocityEndpoint + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: AtomicActionEngine, + ) -> EndpointResolution: + del endpoint, engine + return EndpointResolution( + runtime_target=_BaseVelocityTarget("virtual"), + exclusive=False, + ) + + profile = RobotSkillProfile( + "virtual", + resources={ + "virtual_channel": RobotResource( + "virtual_channel", + endpoints={"motion": _BaseVelocityEndpoint("virtual")}, + ) + }, + ) + + bound = profile.bind( + _engine(control_profiles={}, load_builtins=False), + endpoint_adapters={_BaseVelocityEndpoint: VirtualEndpointAdapter()}, + ) + + assert not bound.resources["virtual_channel"].endpoints["motion"].exclusive + + +def test_endpoint_adapter_registration_validates_declared_type() -> None: + class MissingMetadataAdapter(ResourceEndpointAdapter): + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: AtomicActionEngine, + ) -> EndpointResolution: + del endpoint, engine + return EndpointResolution( + runtime_target=_BaseVelocityTarget("base_velocity"), + exclusive=False, + ) + + profile = RobotSkillProfile( + "mobile", + resources={ + "mobile_base": RobotResource( + "mobile_base", + endpoints={"motion": _BaseVelocityEndpoint("base_velocity")}, + ) + }, + ) + + with pytest.raises(TypeError, match="must declare.*endpoint_type"): + profile.bind( + _engine(control_profiles={}, load_builtins=False), + endpoint_adapters={_BaseVelocityEndpoint: MissingMetadataAdapter()}, + ) + + +def test_builtin_control_part_adapter_cannot_be_overridden() -> None: + with pytest.raises(ValueError, match="cannot be overridden"): + _profile().bind( + _engine(control_profiles=_command_profiles()), + endpoint_adapters={ControlPartEndpoint: ControlPartEndpointAdapter()}, + ) + + +def test_endpoint_adapter_must_return_endpoint_resolution() -> None: + class WrongReturnAdapter(ResourceEndpointAdapter): + adapter_id: ClassVar[str] = "test.wrong_return" + endpoint_type: ClassVar[type[ResourceEndpoint]] = _BaseVelocityEndpoint + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: AtomicActionEngine, + ) -> EndpointResolution: + del endpoint, engine + return object() # type: ignore[return-value] + + profile = RobotSkillProfile( + "mobile", + resources={ + "mobile_base": RobotResource( + "mobile_base", + endpoints={"motion": _BaseVelocityEndpoint("base_velocity")}, + ) + }, + ) + + with pytest.raises(ProfileValidationError, match="expected EndpointResolution"): + profile.bind( + _engine(control_profiles={}, load_builtins=False), + endpoint_adapters={_BaseVelocityEndpoint: WrongReturnAdapter()}, + ) + + +def test_bind_rejects_overlapping_physical_leaves() -> None: + resources = _resources() + resources["left_arm_alias"] = RobotResource( + "left_arm_alias", + endpoints={"control": ControlPartEndpoint("left_arm")}, + ) + + with pytest.raises(ProfileValidationError, match="overlap on robot joints"): + _profile(resources=resources).bind( + _engine(control_profiles=_command_profiles()) + ) + + +def test_bind_rejects_composite_endpoint_outside_member_claim() -> None: + resources = _resources() + resources["bad_composite"] = RobotResource( + "bad_composite", + endpoints={"motion": ControlPartEndpoint("right_arm")}, + members=("left_arm",), + ) + + with pytest.raises(ProfileValidationError, match="not claimed by its members"): + _profile(resources=resources).bind( + _engine(control_profiles=_command_profiles()) + ) + + +def test_bind_rejects_profile_commands_not_installed_on_engine() -> None: + with pytest.raises(ProfileValidationError, match="is not installed"): + _profile().bind(_engine(control_profiles={})) + + +def test_explicit_endpoint_command_profile_must_exist() -> None: + profile = RobotSkillProfile( + "missing_commands", + resources={ + "arm": RobotResource( + "arm", + endpoints={ + "motion": ControlPartEndpoint( + "left_arm", + command_profile="missing_profile", + ) + }, + ) + }, + ) + + with pytest.raises(ProfileValidationError, match="required command profile"): + profile.bind(_engine(control_profiles={}, load_builtins=False)) + + +def test_bind_rejects_engine_command_payload_that_differs_from_profile() -> None: + engine_profiles = _command_profiles() + engine_profiles["left_hand"] = ControlPartCommandProfile.joint_positions( + open=torch.tensor([0.5]), + grasp=torch.tensor([1.0]), + ) + + with pytest.raises(ProfileValidationError, match="not semantically equivalent"): + _profile().bind(_engine(control_profiles=engine_profiles)) + + +def test_profile_rejects_conflicting_endpoint_command_profiles() -> None: + resources = { + "hand": RobotResource( + "hand", + endpoints={ + "first": ControlPartEndpoint( + "left_hand", + command_profile="first_hand", + ), + "second": ControlPartEndpoint( + "left_hand", + command_profile="second_hand", + ), + }, + ) + } + with pytest.raises(ValueError, match="non-equivalent 'grasp'"): + RobotSkillProfile( + "conflicting_commands", + resources=resources, + command_profiles={ + "first_hand": ControlPartCommandProfile.joint_positions( + grasp=torch.tensor([0.5]) + ), + "second_hand": ControlPartCommandProfile.joint_positions( + grasp=torch.tensor([1.0]) + ), + }, + ) + + +def test_bind_rejects_joint_command_with_wrong_endpoint_dof() -> None: + profiles = _command_profiles() + profiles["left_hand"] = ControlPartCommandProfile.joint_positions( + open=torch.zeros(2), + grasp=torch.ones(2), + ) + + with pytest.raises(ProfileValidationError, match="2 joints, expected 1"): + _profile(command_profiles=profiles).bind(_engine(control_profiles=profiles)) + + +def test_profile_commands_must_be_broadcastable_across_environments() -> None: + profiles = _command_profiles() + profiles["left_hand"] = ControlPartCommandProfile.joint_positions( + open=torch.zeros(2, 1), + grasp=torch.ones(2, 1), + ) + + with pytest.raises(ProfileValidationError, match="must be one-dimensional"): + _profile(command_profiles=profiles).bind(_engine(control_profiles=profiles)) + + +def test_bind_rejects_unverified_standard_solver_capability() -> None: + engine = _engine(control_profiles=_command_profiles()) + engine.robot.get_solver.side_effect = lambda name=None: None + + with pytest.raises(ProfileValidationError, match="has no configured solver"): + _profile().bind(engine) + + +def test_unique_capability_binding_lowers_to_exact_action_binding() -> None: + profile = _profile(resources=_resources(include_right=False)) + engine = _engine(control_profiles=_command_profiles()) + bound = profile.bind(engine) + + resolved = bound.resolve("pick_up") + motion = resolved.action_binding.endpoint("primary", "motion") + grasp = resolved.action_binding.endpoint("primary", "grasp") + + assert resolved.resource_ids == {"primary": "left_actor"} + assert resolved.action_binding.owner_id == engine.binding_owner_id + assert resolved.action_binding.endpoint_keys == ( + ("primary", "motion"), + ("primary", "grasp"), + ) + assert motion.require_target(JointPositionTarget).control_part == "left_arm" + assert grasp.require_target(JointPositionTarget).control_part == "left_hand" + assert motion.task_state_key == "left_actor" + assert grasp.task_state_key == "left_actor" + motion_tracking = motion.tracking_channel(JOINT_POSITION_CHANNEL) + assert motion_tracking.source.provider_id == "planning_context.robot" + assert motion_tracking.source.revision == "1" + assert motion_tracking.projector.projector_id == "joint_position_payload" + assert motion_tracking.projector.revision == "1" + assert isinstance( + motion_tracking.source.address, + EndpointTrackingFeedbackAddress, + ) + assert ( + motion_tracking.source.address.target.address_fingerprint + == motion.target.address_fingerprint + ) + resource = resolved.resources["primary"] + motion_sources = resource.endpoints["motion"].effect_sources + grasp_sources = resource.endpoints["grasp"].effect_sources + assert set(motion_sources) == { + POSE_RELATION_EFFECT_CHANNEL, + JOINT_STATE_EFFECT_CHANNEL, + } + assert set(grasp_sources) == { + POSE_RELATION_EFFECT_CHANNEL, + JOINT_STATE_EFFECT_CHANNEL, + CONTACT_EFFECT_CHANNEL, + CONSTRAINT_EFFECT_CHANNEL, + FORCE_EFFECT_CHANNEL, + } + assert motion_sources[POSE_RELATION_EFFECT_CHANNEL].address == ( + ControlPartEvidenceAddress("left_arm", POSE_RELATION_EFFECT_CHANNEL) + ) + assert grasp_sources[CONSTRAINT_EFFECT_CHANNEL].address == ( + ControlPartEvidenceAddress("left_hand", CONSTRAINT_EFFECT_CHANNEL) + ) + assert resolved.claim.leaf_resource_ids == frozenset({"left_arm", "left_hand"}) + assert resolved.claim.joint_ids == (0, 1, 2) + + +def test_ambiguous_binding_requires_complete_per_skill_default() -> None: + engine = _engine(control_profiles=_command_profiles()) + bound = _profile().bind(engine) + + with pytest.raises(AmbiguousSkillBindingError, match="2 valid resource bindings"): + bound.resolve("pick_up") + + selected = _profile( + defaults={ + "pick_up": ResourceBinding({"primary": "right_actor"}), + } + ).bind(engine) + assert selected.resolve("pick_up").resource_ids == {"primary": "right_actor"} + + +@pytest.mark.parametrize( + "default", + [ + ResourceBinding({}), + ResourceBinding({"primary": "left_actor", "stale": "right_actor"}), + ], +) +def test_profile_rejects_partial_or_extra_default_slots( + default: ResourceBinding, +) -> None: + with pytest.raises(ProfileValidationError, match="must cover exactly"): + _profile(defaults={"pick_up": default}).bind( + _engine(control_profiles=_command_profiles()) + ) + + +def test_explicit_slot_selection_overrides_profile_default() -> None: + bound = _profile( + defaults={ + "pick_up": ResourceBinding({"primary": "left_actor"}), + } + ).bind(_engine(control_profiles=_command_profiles())) + + resolved = bound.resolve("pick_up", {"primary": "right_actor"}) + + assert resolved.resource_ids == {"primary": "right_actor"} + + +def test_missing_required_command_filters_skill_and_reports_reason() -> None: + profiles = _command_profiles() + profiles = { + name: ControlPartCommandProfile.joint_positions(grasp=torch.tensor([1.0])) + for name in profiles + } + bound = _profile(command_profiles=profiles).bind(_engine(control_profiles=profiles)) + + assert "pick_up" not in bound.skills + with pytest.raises(UnsupportedSkillError, match="missing command 'open'"): + bound.resolve("pick_up") + + +def test_one_participant_cannot_use_overlapping_required_endpoints() -> None: + resources = _resources(include_right=False) + resources["left_actor"] = RobotResource( + "left_actor", + endpoints={ + "motion": ControlPartEndpoint( + "left_arm", capabilities=_MOTION_CAPABILITIES + ), + "grasp": ControlPartEndpoint( + "left_arm", capabilities=frozenset({GRASP_CAPABILITY}) + ), + }, + members=("left_arm",), + ) + profiles = _command_profiles() + profiles["left_arm"] = ControlPartCommandProfile.joint_positions( + open=torch.zeros(2), + grasp=torch.ones(2), + ) + bound = _profile(resources=resources, command_profiles=profiles).bind( + _engine(control_profiles=profiles) + ) + + assert "pick_up" not in bound.skills + with pytest.raises(UnsupportedSkillError, match="overlap on joints"): + bound.resolve("pick_up") + + +def test_coupled_endpoint_views_are_allowed_without_disjoint_constraint() -> None: + class CoupledWholeBodyAction(AtomicAction[JointPositionGoal, ActionOptions]): + skill_id: ClassVar[str] = "coupled_whole_body" + GoalType: ClassVar[type] = JointPositionGoal + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + "body", + endpoints=( + SkillEndpointRequirement( + "motion", + capabilities=frozenset({JOINT_POSITION_CAPABILITY}), + ), + SkillEndpointRequirement( + "posture", + capabilities=frozenset({"control.posture"}), + ), + ), + ), + ) + ) + + def _plan( + self, + request: ResolvedActionRequest[JointPositionGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + raise NotImplementedError + + resources = _resources() + resources["coupled_body"] = RobotResource( + "coupled_body", + endpoints={ + "motion": ControlPartEndpoint( + "full_body", + capabilities=frozenset({JOINT_POSITION_CAPABILITY}), + ), + "posture": ControlPartEndpoint( + "full_body", + capabilities=frozenset({"control.posture"}), + ), + }, + members=("base", "torso", "left_arm", "right_arm"), + ) + engine = _engine(control_profiles=_command_profiles(), load_builtins=False) + engine.register(CoupledWholeBodyAction()) + bound = _profile(resources=resources).bind(engine) + + assert bound.resolve("coupled_whole_body").resource_ids == {"body": "coupled_body"} + + +def test_disjoint_slot_constraint_rejects_one_actor_for_two_participants() -> None: + profile = _profile(resources=_resources(include_right=False)) + bound = profile.bind(_engine(control_profiles=_command_profiles())) + + assert "hand_over" not in bound.skills + with pytest.raises(UnsupportedSkillError, match="violate constraints"): + bound.resolve("hand_over") + + +def test_composite_claim_conflicts_with_nested_arm_but_not_hand() -> None: + bound = _profile().bind(_engine(control_profiles=_command_profiles())) + + whole_body = bound.resources["whole_body"].claim + left_actor = bound.resources["left_actor"].claim + left_hand = bound.resources["left_hand"].claim + + assert whole_body.conflicts_with(left_actor) + assert not whole_body.conflicts_with(left_hand) + + +def test_generic_profile_supports_base_and_whole_body_without_arm_tool_fields() -> None: + engine = _engine(control_profiles=_command_profiles(), load_builtins=False) + engine.register(_WholeBodyAction()) + engine.register(_NavigateAction()) + bound = _profile().bind(engine) + + whole_body = bound.resolve("whole_body_reach") + navigation = bound.resolve("navigate") + + assert set(bound.skills) == {"navigate", "whole_body_reach"} + assert whole_body.resource_ids == {"body": "whole_body"} + assert ( + whole_body.action_binding.endpoint("body", "motion") + .require_target(JointPositionTarget) + .control_part + == "full_body" + ) + assert whole_body.claim.leaf_resource_ids == frozenset( + {"base", "torso", "left_arm", "right_arm"} + ) + assert navigation.resource_ids == {"body": "base"} + assert ( + navigation.action_binding.endpoint("body", "motion") + .require_target(JointPositionTarget) + .control_part + == "base" + ) + + +def test_presets_are_versioned_snapshots_and_validate_planner() -> None: + preset = SkillPolicyPreset( + "safe", + action_option_templates={ + "pick": PickUpOptions(pre_grasp_distance=0.08), + }, + motion_policy=MotionPolicy(sample_count=80), + required_planner="stub_planner", + tracking_policy=TrackingPolicy.joint_position( + in_flight_max_abs_error=0.125, + terminal_max_abs_error=0.125, + ), + ) + profile = RobotSkillProfile( + "presets", + resources=_resources(), + command_profiles=_command_profiles(), + presets={"safe": preset}, + default_preset="safe", + skill_presets={"pick_up": "safe"}, + ) + bound = profile.bind(_engine(control_profiles=_command_profiles())) + + first = bound.preset(skill_id="pick_up") + second = bound.preset() + + assert first is not second + assert first.schema_version == 3 + assert first.motion_policy.sample_count == 80 + assert first.tracking_policy is not second.tracking_policy + assert first.action_option_templates["pick"] is not ( + second.action_option_templates["pick"] + ) + assert ( + first.action_option_templates["pick"].pre_grasp_distance # type: ignore[attr-defined] + == 0.08 + ) + first_tracking = first.tracking_policy.in_flight + assert first_tracking is not None + assert isinstance(first_tracking.metrics[0], JointPositionTrackingMetric) + assert first_tracking.metrics[0].tolerance == 0.125 + mutable_runner = first.runner_cfg + mutable_runner.command_timeout = 99.0 + assert bound.preset().runner_cfg.command_timeout == 1.0 + with pytest.raises(KeyError, match="not an installed"): + bound.preset(skill_id="typo") + with pytest.raises(KeyError, match="not an installed"): + bound.preset("safe", skill_id="typo") + with pytest.raises(ValueError, match=r"supported versions are \[3\]"): + SkillPolicyPreset("legacy", action_option_templates={}, schema_version=2) + + incompatible = RobotSkillProfile( + "bad_preset", + resources=_resources(), + command_profiles=_command_profiles(), + presets={ + "other": SkillPolicyPreset( + "other", + action_option_templates={}, + required_planner="other_planner", + ) + }, + ) + with pytest.raises(ProfileValidationError, match="requires planner"): + incompatible.bind(_engine(control_profiles=_command_profiles())) + + +def test_workflow_recovery_policy_is_bounded_and_snapshotted() -> None: + source = WorkflowRecoveryPolicy(max_recovery_attempts=2) + preset = SkillPolicyPreset( + "recovering", + action_option_templates={}, + workflow_recovery_policy=source, + ) + + first = preset.workflow_recovery_policy + second = preset.snapshot().workflow_recovery_policy + + assert first.max_recovery_attempts == 2 + assert second == first + assert first is not source + assert second is not first + assert ( + SkillPolicyPreset( + "disabled", action_option_templates={} + ).workflow_recovery_policy.max_recovery_attempts + == 0 + ) + for invalid in (True, 1.5, "2"): + with pytest.raises(TypeError, match="must be an integer"): + WorkflowRecoveryPolicy(invalid) # type: ignore[arg-type] + for invalid in (-1, 101): + with pytest.raises(ValueError, match=r"\[0, 100\]"): + WorkflowRecoveryPolicy(invalid) + + +def test_policy_preset_defaults_exact_builtin_effect_monitor_refs() -> None: + preset = SkillPolicyPreset("safe", action_option_templates={}) + + assert set(preset.effect_monitors) == { + "pick", + "place", + "hand_over", + "operate_articulation", + } + for monitor_ref in preset.effect_monitors.values(): + assert monitor_ref.monitor_id == COMPOSITE_EFFECT_MONITOR_ID + assert monitor_ref.revision == COMPOSITE_EFFECT_MONITOR_REVISION + assert dict(monitor_ref.params) == {} + + +def test_policy_preset_distinguishes_explicit_empty_effect_monitor_mapping() -> None: + preset = SkillPolicyPreset( + "unmonitored", + action_option_templates={}, + effect_monitors={}, + ) + + assert dict(preset.effect_monitors) == {} + assert dict(preset.snapshot().effect_monitors) == {} + + +def test_policy_preset_owns_and_snapshots_effect_monitor_refs() -> None: + source_params = { + "consecutive_samples": 3, + "metadata": ["strict", {"source": "profile"}], + } + source_ref = EffectMonitorRef("test.monitor", "2", source_params) + source_mapping = {"pick": source_ref} + preset = SkillPolicyPreset( + "custom", + action_option_templates={}, + effect_monitors=source_mapping, + ) + + source_params["consecutive_samples"] = 99 + source_params["metadata"][1]["source"] = "mutated" # type: ignore[index] + source_mapping["pick"] = EffectMonitorRef("replacement", "1") + first = preset.effect_monitors + snapshot = preset.snapshot() + second = snapshot.effect_monitors + + assert first["pick"] is not source_ref + assert first["pick"].monitor_id == "test.monitor" + assert first["pick"].params["consecutive_samples"] == 3 + assert first["pick"].params["metadata"] == ( + "strict", + {"source": "profile"}, + ) + assert second["pick"] is not first["pick"] + assert second["pick"].params == first["pick"].params + with pytest.raises(TypeError): + first["place"] = source_ref # type: ignore[index] + with pytest.raises(TypeError): + first["pick"].params["consecutive_samples"] = 4 # type: ignore[index] + + +def test_policy_preset_owns_and_freezes_action_option_templates() -> None: + direction = torch.tensor([0.0, 1.0, 0.0]) + source = PickUpOptions( + pick_object_part="top", + approach_direction=direction, + ) + source_mapping = {"pick": source} + preset = SkillPolicyPreset( + "custom", + action_option_templates=source_mapping, + ) + + direction.fill_(9.0) + source.approach_direction.fill_(8.0) + source_mapping.clear() + first = preset.action_option_templates + second = preset.snapshot().action_option_templates + selected = preset.action_option_template("pick") + + assert type(first["pick"]) is PickUpOptions + assert first["pick"] is not source + assert second["pick"] is not first["pick"] + assert selected is not first["pick"] + assert first["pick"].pick_object_part == "top" # type: ignore[attr-defined] + torch.testing.assert_close( + first["pick"].approach_direction, # type: ignore[attr-defined] + torch.tensor([0.0, 1.0, 0.0]), + ) + with pytest.raises(TypeError): + first["place"] = PickUpOptions() # type: ignore[index] + with pytest.raises(KeyError, match="no action-option template"): + preset.action_option_template("place") + + +def test_policy_preset_allows_empty_templates_but_rejects_invalid_values() -> None: + with pytest.raises(TypeError, match="action_option_templates"): + SkillPolicyPreset("missing") # type: ignore[call-arg] + + assert ( + dict( + SkillPolicyPreset( + "empty", action_option_templates={} + ).action_option_templates + ) + == {} + ) + + with pytest.raises(TypeError, match="ActionOptions"): + SkillPolicyPreset( + "invalid", + action_option_templates={"pick": object()}, # type: ignore[dict-item] + ) + + +def test_policy_preset_rejects_inherited_action_options_with_extra_slot_state() -> None: + class InheritedOptions(PickUpOptions): + __slots__ = ("runtime_cache",) + + options = InheritedOptions() + object.__setattr__(options, "runtime_cache", ["live"]) + + with pytest.raises(TypeError, match="exact frozen @dataclass"): + SkillPolicyPreset( + "invalid", + action_option_templates={"pick": options}, + ) + + +def test_policy_preset_rejects_deepcopy_with_nested_mutable_aliases() -> None: + @dataclass(frozen=True, slots=True) + class AliasingOptions(ActionOptions): + values: list[int] + + def __deepcopy__(self, memo: dict[int, object]) -> AliasingOptions: + del memo + return type(self)(self.values) + + with pytest.raises(TypeError, match="without shared mutable objects"): + SkillPolicyPreset( + "invalid", + action_option_templates={"vendor.alias": AliasingOptions([1])}, + ) + + +def test_policy_preset_rejects_deepcopy_with_shared_tensor_storage() -> None: + @dataclass(frozen=True, slots=True) + class TensorViewOptions(ActionOptions): + values: torch.Tensor + + def __deepcopy__(self, memo: dict[int, object]) -> TensorViewOptions: + del memo + return type(self)(self.values.view_as(self.values)) + + with pytest.raises(TypeError, match="tensor storage"): + SkillPolicyPreset( + "invalid", + action_option_templates={ + "vendor.tensor_alias": TensorViewOptions(torch.ones(2)) + }, + ) + + +def test_profile_owns_named_grounding_provider_selections() -> None: + selections = {"hand_over": "dual_center"} + profile = RobotSkillProfile( + "grounding", + resources=_resources(), + command_profiles=_command_profiles(), + grounding_providers=selections, + ) + + selections["hand_over"] = "source_mutation" + + assert profile.grounding_providers == {"hand_over": "dual_center"} + with pytest.raises(TypeError): + profile.grounding_providers["pick"] = "invalid" # type: ignore[index] + with pytest.raises(ValueError, match="grounding_providers"): + RobotSkillProfile( + "invalid_grounding", + resources=_resources(), + grounding_providers={"hand_over": " provider"}, + ) + + +def test_profile_rejects_default_for_uninstalled_skill() -> None: + with pytest.raises(ProfileValidationError, match="not installed"): + _profile(defaults={"missing": ResourceBinding({"primary": "left_actor"})}).bind( + _engine(control_profiles=_command_profiles()) + ) + + +def test_engine_can_install_profile_as_authoritative_command_source() -> None: + source_engine = _engine(control_profiles=_command_profiles()) + profile = _profile(defaults={"pick_up": ResourceBinding({"primary": "left_actor"})}) + + engine = AtomicActionEngine(source_engine.motion_generator, skill_profile=profile) + + assert engine.skill_profile is not None + assert engine.skill_profile.resolve("pick_up").resource_ids == { + "primary": "left_actor" + } + assert set(engine.control_profiles) == {"left_hand", "right_hand"} + + +def test_engine_rejects_endpoint_adapters_without_skill_profile() -> None: + source = _engine(control_profiles={}, load_builtins=False) + + with pytest.raises(ValueError, match="requires skill_profile"): + AtomicActionEngine( + source.motion_generator, + load_builtins=False, + endpoint_adapters={_BaseVelocityEndpoint: _BaseVelocityEndpointAdapter()}, + ) + + +def test_bound_profile_rejects_stale_engine_skill_catalog() -> None: + engine = _engine(control_profiles=_command_profiles()) + bound = _profile().bind(engine) + action_type = BUILTIN_ACTION_TYPES[0] + + class Replacement(action_type): + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + "primary", + endpoints=( + SkillEndpointRequirement( + "motion", + capabilities=frozenset({JOINT_POSITION_CAPABILITY}), + ), + ), + ), + ) + ) + + engine.register(Replacement(), replace=True) + + assert engine.skill_profile is None + with pytest.raises(RuntimeError, match="changed after"): + _ = bound.skills + + +def test_bound_profile_rejects_equal_descriptor_implementation_replacement() -> None: + engine = _engine(control_profiles=_command_profiles()) + bound = _profile().bind(engine) + action_type = BUILTIN_ACTION_TYPES[0] + + class EquivalentReplacement(action_type): + binding_contract: ClassVar[SkillBindingContract] = action_type.binding_contract + + assert EquivalentReplacement.descriptor() == action_type.descriptor() + + engine.register(EquivalentReplacement(), replace=True) + + with pytest.raises(RuntimeError, match="changed after"): + _ = bound.skills + + +__all__ = [] diff --git a/tests/sim/skills/test_runtime.py b/tests/sim/skills/test_runtime.py new file mode 100644 index 000000000..0a980f6fd --- /dev/null +++ b/tests/sim/skills/test_runtime.py @@ -0,0 +1,2022 @@ +# ---------------------------------------------------------------------------- +# 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 canonical semantic-skill execution and its public facade.""" + +from __future__ import annotations + +from dataclasses import dataclass, replace +import json +from types import MethodType, SimpleNamespace +from typing import ClassVar +from unittest.mock import Mock + +import pytest +import torch + +import embodichain.lab.sim.skills.runtime as runtime_module +from embodichain.lab.sim.atomic_actions import ( + ActionBinding, + ActionInvocation, + ActionOptions, + ActionPlan, + Affordance, + ArticulationJointState, + AtomicAction, + AtomicActionEngine, + CommandAcknowledgement, + EffectVerificationRequirement, + EffectVerificationRequest, + EndpointBinding, + EndpointTrackingChannelBinding, + EndpointTrackingFeedbackAddress, + ExecutionEventKind, + HeldObjectGuardRequest, + HeldObjectState, + JointPositionTarget, + MotionPolicy, + ObjectSemantics, + PhaseEffectGateRequest, + PlanningContext, + RecoveryPolicy, + ResolvedActionRequest, + RobotObservation, + SceneSnapshot, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, + StateDelta, + TaskState, + TimedCommandSequence, + TrackingFeedbackSourceRef, + TrackingProjectorRef, +) +from embodichain.lab.sim.atomic_actions.tracking import TrackingPolicy +from embodichain.lab.sim.skills.calls import ( + HandOver, + Pick, + Place, + RegisteredSemanticCall, + SemanticCallSpec, +) +from embodichain.lab.sim.skills.compiler import ( + GroundedHeldObjectGuard, + GroundedPhaseEffectGate, + HeldObjectGuardBaseline, + SemanticSkillCompiler, +) +from embodichain.lab.sim.skills.effects import ( + ArticulationJointStateExpectation, + BinaryEffectClause, + BinaryEvidenceKind, + CONSTRAINT_EFFECT_CHANNEL, + ControlPartEvidenceAddress, + EffectEvidenceBatch, + EffectEvidenceSourceRef, + EffectExpectationDecision, + EffectMonitor, + EffectMonitorDecision, + HeldObjectRelation, + HeldObjectStateExpectation, + JOINT_STATE_EFFECT_CHANNEL, + JointStateEffectClause, + SemanticEffectKind, + SemanticEffectSpec, +) +from embodichain.lab.sim.skills.runtime import ( + AtomicSkills, + SkillEndpointBindingTrace, + SkillRuntime, + SkillStatus, + SkillWorkflowRecoveryRole, +) +from embodichain.lab.sim.skills.parallel import ParallelTimingPolicy +from embodichain.lab.sim.skills.parallel_runtime import ParallelSkillRuntime +from embodichain.lab.sim.skills.profiles import ( + ResourceClaim, + WorkflowRecoveryPolicy, +) +from embodichain.lab.sim.skills.scene import SceneObjectRef, SceneRegistry + +BATCH_SIZE = 2 + + +class _Clock: + """Deterministic execution clock.""" + + def __init__(self) -> None: + self.time = 0.0 + self.sleeps: list[float] = [] + + def now(self) -> float: + return self.time + + def sleep(self, duration: float) -> None: + self.sleeps.append(duration) + self.time += duration + + +class _ObservationProvider: + """Return a new timestamped context on every external observation.""" + + def __init__(self) -> None: + self.calls = 0 + self.task_states: list[TaskState] = [] + + def observe(self, task_state: TaskState) -> PlanningContext: + self.calls += 1 + self.task_states.append(task_state) + timestamp = float(self.calls) + return PlanningContext( + robot=RobotObservation( + timestamp=timestamp, + qpos=torch.zeros(BATCH_SIZE, 1), + qvel=torch.zeros(BATCH_SIZE, 1), + ), + task=task_state, + scene=SceneSnapshot(timestamp=timestamp, version=self.calls), + env_ids=torch.arange(BATCH_SIZE, dtype=torch.long), + ) + + +class _CommandSink: + """Accept every command while recording safe-stop operations.""" + + def __init__(self) -> None: + self.sent = 0 + self.held = 0 + self.cancelled = 0 + + def send(self, command: object, *, timeout: float) -> CommandAcknowledgement: + del command, timeout + self.sent += 1 + return CommandAcknowledgement.accepted_ack() + + def hold( + self, + targets: tuple[object, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + del targets, context, timeout + self.held += 1 + return CommandAcknowledgement.accepted_ack() + + def cancel( + self, + targets: tuple[object, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + del targets, timeout + self.cancelled += 1 + return CommandAcknowledgement.accepted_ack() + + +class _Collector: + """Fake acquisition boundary; the test monitor owns decisions.""" + + def __init__(self) -> None: + self.calls: list[tuple[int, float, torch.Tensor]] = [] + + def collect( + self, + spec: SemanticEffectSpec, + *, + timestamp: float, + observation_revision: int, + env_ids: torch.Tensor | None = None, + ) -> dict[str, EffectEvidenceBatch]: + assert env_ids is not None + self.calls.append((observation_revision, timestamp, env_ids.clone())) + del spec + return {} + + +class _DecisionMonitor(EffectMonitor): + """Return one deterministic row-local physical-effect decision.""" + + def __init__(self, spec: SemanticEffectSpec, decision: EffectMonitorDecision): + self._spec = spec + self._decision = decision + self.calls = 0 + self.requests: list[EffectVerificationRequest] = [] + + @property + def spec(self) -> SemanticEffectSpec: + return self._spec.snapshot() + + def observe( + self, + request: EffectVerificationRequest, + evidence: dict[str, EffectEvidenceBatch], + ) -> EffectMonitorDecision: + del evidence + self.calls += 1 + self.requests.append(request.snapshot()) + return EffectMonitorDecision( + self._decision.success_mask, + self._decision.failure_mask, + self._decision.expectation_decisions, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class _EffectGoal: + """Test-only goal carrying plan success and symbolic target value.""" + + goal_kind: ClassVar[str] = "runtime_test_effect" + + plan_success: torch.Tensor + target_position: float + + +class _EffectAction(AtomicAction[_EffectGoal, ActionOptions]): + """Zero-frame action with an explicit verified articulation effect.""" + + skill_id: ClassVar[str] = "runtime_test_effect" + GoalType: ClassVar[type] = _EffectGoal + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract() + + def __init__(self) -> None: + super().__init__() + self.plan_count = 0 + + def _scene_dependencies( + self, + request: ResolvedActionRequest[_EffectGoal, ActionOptions], + ) -> tuple[str, ...]: + del request + return ("fixture",) + + def _plan( + self, + request: ResolvedActionRequest[_EffectGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + goal = self.require_goal(request) + self.plan_count += 1 + position = torch.full( + (context.batch_size, 1), + goal.target_position, + dtype=context.robot.qpos.dtype, + device=context.robot.qpos.device, + ) + return self.build_command_plan( + request, + context, + success=goal.plan_success, + commands=TimedCommandSequence((), context.env_ids), + expected_effects=StateDelta( + articulation_joint_updates={ + ("fixture", "joint"): ArticulationJointState(position) + } + ), + effect_verification=EffectVerificationRequirement("semantic_effect"), + replannable=False, + scene_dependency_monitor_until={"fixture": 0}, + ) + + +@dataclass(frozen=True, slots=True) +class _WorkflowEffectGoal: + """Test-only held-object effect for workflow recovery.""" + + goal_kind: ClassVar[str] = "runtime_test_workflow_effect" + + object_id: str + attach: bool + + def __post_init__(self) -> None: + if type(self.object_id) is not str or not self.object_id: + raise ValueError("object_id must be a non-empty string.") + if type(self.attach) is not bool: + raise TypeError("attach must be exactly bool.") + + +class _WorkflowEffectAction(AtomicAction[_WorkflowEffectGoal, ActionOptions]): + """Zero-frame action that commits only a verified held-object effect.""" + + skill_id: ClassVar[str] = "runtime_test_workflow_primary" + GoalType: ClassVar[type] = _WorkflowEffectGoal + source_slot: ClassVar[str] = "primary" + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=(SkillEndpointRequirement(endpoint_id="motion"),), + ), + ) + ) + + def _plan( + self, + request: ResolvedActionRequest[_WorkflowEffectGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + goal = self.require_goal(request) + endpoint = request.binding.endpoint(self.source_slot, "motion") + task_state_key = endpoint.task_state_key + assert task_state_key is not None + if goal.attach: + poses = ( + torch.eye( + 4, + dtype=context.robot.qpos.dtype, + device=context.robot.qpos.device, + ) + .unsqueeze(0) + .repeat(context.batch_size, 1, 1) + ) + effect: HeldObjectState | None = HeldObjectState( + semantics=ObjectSemantics( + affordance=Affordance(), + geometry={}, + label=goal.object_id, + entity_id=goal.object_id, + ), + object_to_eef=poses, + grasp_xpos=poses, + env_mask=torch.ones( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ), + ) + else: + effect = None + return self.build_command_plan( + request, + context, + success=torch.ones( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ), + commands=TimedCommandSequence((), context.env_ids), + expected_effects=StateDelta( + held_object_updates={task_state_key: effect}, + ), + effect_verification=EffectVerificationRequirement("semantic_effect"), + replannable=False, + ) + + +class _WorkflowSourceEffectAction(_WorkflowEffectAction): + """Held-object effect addressed through a hand-over source slot.""" + + skill_id: ClassVar[str] = "runtime_test_workflow_source" + source_slot: ClassVar[str] = "source" + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="source", + endpoints=(SkillEndpointRequirement(endpoint_id="motion"),), + ), + ) + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class _WorkflowEffectDecision: + """One queued physical-effect result for a grounded recovery call.""" + + success_mask: torch.Tensor + failure_mask: torch.Tensor + inverse_satisfied_mask: torch.Tensor + + def __post_init__(self) -> None: + for name in ( + "success_mask", + "failure_mask", + "inverse_satisfied_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.shape != (BATCH_SIZE,): + raise ValueError(f"{name} must be bool with shape ({BATCH_SIZE},).") + object.__setattr__(self, name, value.clone()) + + +@dataclass(frozen=True, slots=True) +class _Workflow: + workflow_id: str + calls: tuple[SemanticCallSpec, ...] + + +@dataclass(frozen=True, slots=True) +class _Grounded: + analyzed: object + invocation: ActionInvocation + effect_spec: SemanticEffectSpec + effect_monitor: EffectMonitor + eligible_mask: torch.Tensor + + +@dataclass(frozen=True, slots=True) +class _Integration: + engine: AtomicActionEngine + scene_registry: SceneRegistry + + +class _Compiler(SemanticSkillCompiler): + """Semantic compiler test double retaining the production call boundaries.""" + + def __init__( + self, + engine: AtomicActionEngine, + decisions: tuple[EffectMonitorDecision, ...], + plan_success: tuple[torch.Tensor, ...], + ) -> None: + self._test_integration = _Integration(engine, SceneRegistry()) + self._decisions = decisions + self._plan_success = plan_success + self.analyze_count = 0 + self.ground_count = 0 + self.ground_timestamps: list[float] = [] + self.ground_task_masks: list[torch.Tensor | None] = [] + self.invocations: list[ActionInvocation] = [] + self.monitors: list[_DecisionMonitor] = [] + + @property + def integration(self) -> _Integration: + return self._test_integration + + def analyze( + self, + calls: tuple[SemanticCallSpec, ...], + *, + workflow_id: str = "semantic_workflow", + path: tuple[object, ...] = ("workflow",), + ) -> _Workflow: + del path + self.analyze_count += 1 + return _Workflow(workflow_id, tuple(calls)) + + def ground( + self, + workflow: _Workflow, + call_index: int, + context: PlanningContext, + *, + eligible_mask: torch.Tensor | None = None, + revision: int = 0, + path: tuple[object, ...] = ("workflow",), + ) -> _Grounded: + del path + assert eligible_mask is not None + self.ground_count += 1 + self.ground_timestamps.append(context.robot.timestamp) + state = context.task.get_articulation_joint_state("fixture", "joint") + self.ground_task_masks.append(None if state is None else state.env_mask.clone()) + call = workflow.calls[call_index] + invocation = ActionInvocation( + skill_id=_EffectAction.skill_id, + goal=_EffectGoal( + self._plan_success[call_index].clone(), + float(call_index + 1), + ), + binding=self.integration.engine.bind_control_parts( + _EffectAction.skill_id, + {}, + ), + motion_policy=MotionPolicy( + strategy="ik_interp", + sample_count=7, + ), + tracking_policy=TrackingPolicy.timed(), + recovery_policy=RecoveryPolicy( + max_replans=0, + max_action_retries=0, + action_timeout=100.0, + ), + invocation_id=f"{workflow.workflow_id}:{call_index}", + revision=revision, + ) + target = torch.full((BATCH_SIZE, 1), float(call_index + 1)) + expectation = ArticulationJointStateExpectation( + "joint_target", + "fixture", + "joint", + target, + ) + source = EffectEvidenceSourceRef( + "test.provider", + "1", + ControlPartEvidenceAddress("virtual", JOINT_STATE_EFFECT_CHANNEL), + ) + spec = SemanticEffectSpec( + semantic_id=call.semantic_id, + effect_kind=SemanticEffectKind.ARTICULATION, + skill_id=invocation.skill_id, + invocation_id=invocation.invocation_id, + invocation_revision=invocation.revision, + env_ids=context.env_ids, + state_expectations=(expectation,), + clauses=( + JointStateEffectClause( + "joint_position", + expectation.expectation_id, + source, + target, + ), + ), + ) + monitor = _DecisionMonitor(spec, self._decisions[call_index]) + self.invocations.append(invocation) + self.monitors.append(monitor) + analyzed = SimpleNamespace( + bound=SimpleNamespace( + robot_profile=SimpleNamespace(profile_id="runtime_test_profile"), + binding=SimpleNamespace(action_binding=invocation.binding), + linked=SimpleNamespace( + descriptor=SimpleNamespace(skill_id=invocation.skill_id) + ), + preset=SimpleNamespace( + preset_id="runtime_test_preset", + schema_version=1, + motion_policy=invocation.motion_policy, + recovery_policy=invocation.recovery_policy, + ), + ) + ) + return _Grounded( + analyzed, + invocation, + spec, + monitor, + eligible_mask.clone(), + ) + + +class _WorkflowRecoveryCompiler(SemanticSkillCompiler): + """Ground queued physical outcomes through real execution sessions.""" + + def __init__( + self, + engine: AtomicActionEngine, + decisions: tuple[_WorkflowEffectDecision, ...], + *, + max_recovery_attempts: int, + ) -> None: + self._test_integration = _Integration(engine, SceneRegistry()) + self._decisions = decisions + self._workflow_policy = WorkflowRecoveryPolicy(max_recovery_attempts) + self.analysis_windows: list[tuple[str, ...]] = [] + self.grounded_calls: list[SemanticCallSpec] = [] + self.grounded_masks: list[torch.Tensor] = [] + self.invocations: list[ActionInvocation] = [] + + @property + def integration(self) -> _Integration: + return self._test_integration + + def analyze( + self, + calls: tuple[SemanticCallSpec, ...], + *, + workflow_id: str = "semantic_workflow", + path: tuple[object, ...] = ("workflow",), + ) -> _Workflow: + del path + self.analysis_windows.append(tuple(call.semantic_id for call in calls)) + return _Workflow(workflow_id, tuple(calls)) + + def ground( + self, + workflow: _Workflow, + call_index: int, + context: PlanningContext, + *, + eligible_mask: torch.Tensor | None = None, + revision: int = 0, + path: tuple[object, ...] = ("workflow",), + ) -> _Grounded: + del path + if eligible_mask is None: + raise ValueError("eligible_mask is required by this compiler.") + decision_index = len(self.grounded_calls) + if decision_index >= len(self._decisions): + raise RuntimeError("No queued workflow-effect decision remains.") + decision = self._decisions[decision_index] + call = workflow.calls[call_index] + if type(call) is Pick: + action_type = _WorkflowEffectAction + source_slot = "primary" + expectation_id = "destination" + relation = HeldObjectRelation.ATTACHED + effect_kind = SemanticEffectKind.ATTACH + attach = True + expected_binary = True + elif type(call) is Place: + action_type = _WorkflowEffectAction + source_slot = "primary" + expectation_id = "source" + relation = HeldObjectRelation.DETACHED + effect_kind = SemanticEffectKind.RELEASE + attach = False + expected_binary = False + elif type(call) is HandOver: + action_type = _WorkflowSourceEffectAction + source_slot = "source" + expectation_id = "source" + relation = HeldObjectRelation.DETACHED + effect_kind = SemanticEffectKind.RELEASE + attach = False + expected_binary = False + else: + raise TypeError( + "Workflow-recovery test compiler accepts Pick, Place, or HandOver." + ) + object_id = call.object.entity_id + binding = ActionBinding( + owner_id=self.integration.engine.binding_owner_id, + endpoints=( + EndpointBinding( + slot_id=source_slot, + endpoint_id="motion", + resource_id="left_actor", + adapter_id="test", + target=JointPositionTarget("virtual", (0,)), + task_state_key="left_gripper", + joint_ids=(0,), + ), + ), + ) + motion_policy = MotionPolicy( + strategy="ik_interp", + sample_count=7, + ) + tracking_policy = TrackingPolicy.timed() + recovery_policy = RecoveryPolicy( + max_replans=0, + max_action_retries=0, + action_timeout=100.0, + ) + invocation = ActionInvocation( + skill_id=action_type.skill_id, + goal=_WorkflowEffectGoal(object_id=object_id, attach=attach), + binding=binding, + motion_policy=motion_policy, + tracking_policy=tracking_policy, + recovery_policy=recovery_policy, + invocation_id=f"{workflow.workflow_id}:{decision_index}", + revision=revision, + ) + expectation = HeldObjectStateExpectation( + expectation_id=expectation_id, + relation=relation, + object_id=object_id, + slot_id=source_slot, + resource_id="left_actor", + task_state_key="left_gripper", + ) + spec = SemanticEffectSpec( + semantic_id=call.semantic_id, + effect_kind=effect_kind, + skill_id=invocation.skill_id, + invocation_id=invocation.invocation_id, + invocation_revision=invocation.revision, + env_ids=context.env_ids, + state_expectations=(expectation,), + clauses=( + BinaryEffectClause( + clause_id=f"{expectation_id}.constraint", + expectation_id=expectation_id, + source=EffectEvidenceSourceRef( + "test.provider", + "1", + ControlPartEvidenceAddress( + "virtual", + CONSTRAINT_EFFECT_CHANNEL, + ), + ), + evidence_kind=BinaryEvidenceKind.CONSTRAINT, + expected=expected_binary, + ), + ), + ) + monitor = _DecisionMonitor( + spec, + EffectMonitorDecision( + success_mask=decision.success_mask, + failure_mask=decision.failure_mask, + expectation_decisions=( + EffectExpectationDecision( + expectation_id=expectation_id, + satisfied_mask=decision.success_mask, + contradicted_mask=decision.failure_mask, + inverse_satisfied_mask=decision.inverse_satisfied_mask, + ), + ), + ), + ) + analyzed = SimpleNamespace( + call=call, + bound=SimpleNamespace( + robot_profile=SimpleNamespace(profile_id="runtime_test_profile"), + binding=SimpleNamespace(action_binding=binding), + linked=SimpleNamespace( + descriptor=SimpleNamespace(skill_id=invocation.skill_id) + ), + preset=SimpleNamespace( + preset_id="runtime_test_recovery_preset", + schema_version=3, + motion_policy=motion_policy, + tracking_policy=tracking_policy, + recovery_policy=recovery_policy, + workflow_recovery_policy=self._workflow_policy, + ), + ), + ) + self.grounded_calls.append(call) + self.grounded_masks.append(eligible_mask.clone()) + self.invocations.append(invocation) + return _Grounded( + analyzed=analyzed, + invocation=invocation, + effect_spec=spec, + effect_monitor=monitor, + eligible_mask=eligible_mask.clone(), + ) + + +@dataclass(slots=True) +class _System: + runtime: SkillRuntime + compiler: _Compiler + engine: AtomicActionEngine + action: _EffectAction + observation: _ObservationProvider + sink: _CommandSink + collector: _Collector + clock: _Clock + + +@dataclass(slots=True) +class _WorkflowRecoverySystem: + runtime: SkillRuntime + compiler: _WorkflowRecoveryCompiler + engine: AtomicActionEngine + observation: _ObservationProvider + sink: _CommandSink + collector: _Collector + clock: _Clock + + +def _mask(*values: bool) -> torch.Tensor: + return torch.tensor(values, dtype=torch.bool) + + +def _workflow_decision( + success_mask: torch.Tensor, + failure_mask: torch.Tensor, + *, + inverse_satisfied_mask: torch.Tensor | None = None, +) -> _WorkflowEffectDecision: + return _WorkflowEffectDecision( + success_mask=success_mask, + failure_mask=failure_mask, + inverse_satisfied_mask=( + torch.zeros_like(failure_mask) + if inverse_satisfied_mask is None + else inverse_satisfied_mask + ), + ) + + +def _call(name: str) -> RegisteredSemanticCall: + return RegisteredSemanticCall(call_id=f"test.{name}") + + +def _system( + decisions: tuple[EffectMonitorDecision, ...], + *, + plan_success: tuple[torch.Tensor, ...] | None = None, +) -> _System: + robot = Mock() + robot.device = torch.device("cpu") + robot.dof = 1 + robot.control_parts = {} + robot.get_qpos.return_value = torch.zeros(BATCH_SIZE, 1) + robot.get_qvel.return_value = torch.zeros(BATCH_SIZE, 1) + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "runtime_test" + engine = AtomicActionEngine(generator, load_builtins=False) + action = _EffectAction() + engine.register(action) + selected_plan_success = plan_success or tuple(_mask(True, True) for _ in decisions) + compiler = _Compiler(engine, decisions, selected_plan_success) + observation = _ObservationProvider() + sink = _CommandSink() + collector = _Collector() + clock = _Clock() + runtime = SkillRuntime.from_components( + compiler, + observation, + sink, + collector, + task_state=TaskState.empty(BATCH_SIZE, "cpu"), + clock=clock, + ) + return _System( + runtime, + compiler, + engine, + action, + observation, + sink, + collector, + clock, + ) + + +def _workflow_recovery_system( + decisions: tuple[_WorkflowEffectDecision, ...], + *, + max_recovery_attempts: int = 2, + task_state: TaskState | None = None, +) -> _WorkflowRecoverySystem: + robot = Mock() + robot.device = torch.device("cpu") + robot.dof = 1 + robot.control_parts = {} + robot.get_qpos.return_value = torch.zeros(BATCH_SIZE, 1) + robot.get_qvel.return_value = torch.zeros(BATCH_SIZE, 1) + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "runtime_test" + engine = AtomicActionEngine(generator, load_builtins=False) + engine.register(_WorkflowEffectAction()) + engine.register(_WorkflowSourceEffectAction()) + compiler = _WorkflowRecoveryCompiler( + engine, + decisions, + max_recovery_attempts=max_recovery_attempts, + ) + observation = _ObservationProvider() + sink = _CommandSink() + collector = _Collector() + clock = _Clock() + runtime = SkillRuntime.from_components( + compiler, + observation, + sink, + collector, + task_state=( + TaskState.empty(BATCH_SIZE, "cpu") if task_state is None else task_state + ), + clock=clock, + ) + return _WorkflowRecoverySystem( + runtime=runtime, + compiler=compiler, + engine=engine, + observation=observation, + sink=sink, + collector=collector, + clock=clock, + ) + + +def test_runtime_analyzes_once_and_uses_one_fresh_session_per_call( + monkeypatch: pytest.MonkeyPatch, +) -> None: + system = _system( + ( + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + ) + ) + session_calls = 0 + runner_calls = 0 + original_start = system.engine.start + original_runner = runtime_module.ExecutionRunner + + def counted_start(self: AtomicActionEngine, *args: object, **kwargs: object): + nonlocal session_calls + del self + session_calls += 1 + return original_start(*args, **kwargs) + + system.engine.start = MethodType(counted_start, system.engine) + + def counted_runner(*args: object, **kwargs: object): + nonlocal runner_calls + runner_calls += 1 + return original_runner(*args, **kwargs) + + monkeypatch.setattr(runtime_module, "ExecutionRunner", counted_runner) + result = system.runtime.run((_call("first"), _call("second"))) + + assert result.status is SkillStatus.COMPLETED + assert system.compiler.analyze_count == 1 + assert system.compiler.ground_count == 2 + assert session_calls == 2 + assert runner_calls == 2 + assert system.action.plan_count == 2 + assert len(result.calls) == 2 + assert len(system.collector.calls) == 2 + assert system.compiler.ground_timestamps[1] > system.compiler.ground_timestamps[0] + assert system.observation.calls == 4 + + +def test_runtime_analyzes_downstream_calls_but_executes_only_requested_prefix() -> None: + system = _system( + ( + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + ) + ) + calls = (_call("current_segment"), _call("downstream_segment")) + + result = system.runtime.run(calls, execution_prefix_length=1) + + assert result.status is SkillStatus.COMPLETED + assert system.compiler.analyze_count == 1 + assert system.compiler.ground_count == 1 + assert len(system.compiler.invocations) == 1 + assert len(result.calls) == 1 + assert result.calls[0].semantic_id == "test.current_segment" + + +@pytest.mark.parametrize("prefix_length", (0, 3, True, 1.5)) +def test_runtime_rejects_invalid_execution_prefix_before_analysis( + prefix_length: object, +) -> None: + system = _system( + ( + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + ) + ) + + with pytest.raises((TypeError, ValueError), match="execution_prefix_length"): + system.runtime.start( + (_call("first"), _call("second")), + execution_prefix_length=prefix_length, # type: ignore[arg-type] + ) + + assert system.compiler.analyze_count == 0 + assert system.observation.calls == 0 + + +def test_runtime_keeps_partial_rows_at_the_shared_call_barrier() -> None: + system = _system( + ( + EffectMonitorDecision(_mask(True, False), _mask(False, True)), + EffectMonitorDecision(_mask(True, False), _mask(False, False)), + ) + ) + result = system.runtime.run(_call("first"), _call("second")) + + assert result.status is SkillStatus.COMPLETED + assert torch.equal(result.success_mask, _mask(True, False)) + assert torch.equal(result.failure_mask, _mask(False, True)) + assert torch.equal(result.calls[0].completed_mask, _mask(True, False)) + assert torch.equal(result.calls[0].failed_mask, _mask(False, True)) + assert torch.equal(result.calls[1].entered_mask, _mask(True, False)) + assert torch.equal(system.compiler.ground_task_masks[1], _mask(True, False)) + joint = result.task_state.get_articulation_joint_state("fixture", "joint") + assert joint is not None + assert torch.equal(joint.env_mask, _mask(True, False)) + assert torch.allclose(joint.position[0], torch.tensor([2.0])) + assert len(result.failures) == 1 + + +def test_runtime_reacquires_a_lost_source_with_a_real_pick_then_retries() -> None: + system = _workflow_recovery_system( + ( + _workflow_decision(_mask(True, True), _mask(False, False)), + _workflow_decision(_mask(True, False), _mask(False, True)), + _workflow_decision(_mask(False, True), _mask(False, False)), + _workflow_decision(_mask(False, True), _mask(False, False)), + ) + ) + cube = SceneObjectRef("cube") + + result = system.runtime.run( + Pick(object=cube), + Place(object=cube, inside=SceneObjectRef("bin")), + ) + + assert result.status is SkillStatus.COMPLETED + assert torch.equal(result.success_mask, _mask(True, True)) + assert torch.equal(result.failure_mask, _mask(False, False)) + assert len(result.calls) == 2 + assert [call.semantic_id for call in system.compiler.grounded_calls] == [ + "pick", + "place", + "pick", + "place", + ] + assert [mask.tolist() for mask in system.compiler.grounded_masks] == [ + [True, True], + [True, True], + [False, True], + [False, True], + ] + assert system.compiler.analysis_windows == [ + ("pick", "place"), + ("pick", "place"), + ("place",), + ] + assert [trace.role for trace in result.workflow_recoveries] == [ + SkillWorkflowRecoveryRole.REACQUIRE, + SkillWorkflowRecoveryRole.RETRY_REACQUIRED, + ] + assert all( + torch.equal(trace.entered_mask, _mask(False, True)) + for trace in result.workflow_recoveries + ) + assert result.workflow_recoveries[0].call is not None + assert result.workflow_recoveries[0].call.semantic_id == "pick" + assert result.workflow_recoveries[1].call is not None + assert result.workflow_recoveries[1].call.semantic_id == "place" + assert any( + event.kind is ExecutionEventKind.RECOVERY_REQUIRED + and torch.equal(event.env_mask, _mask(False, True)) + for event in result.events + ) + metadata = result.to_metadata() + json.dumps(metadata, allow_nan=False, sort_keys=True) + assert [entry["role"] for entry in metadata["workflow_recoveries"]] == [ + "reacquire", + "retry_reacquired", + ] + assert metadata["workflow_recoveries"][0]["source_resource_id"] == "left_actor" + assert metadata["workflow_recoveries"][0]["source_task_state_key"] == ( + "left_gripper" + ) + assert result.task_state.get_held_object("left_gripper") is None + + +def test_runtime_retries_directly_when_verified_source_relation_remains() -> None: + poses = torch.eye(4).unsqueeze(0).repeat(BATCH_SIZE, 1, 1) + initial_state = TaskState( + batch_size=BATCH_SIZE, + device="cpu", + held_objects={ + "left_gripper": HeldObjectState( + semantics=ObjectSemantics( + affordance=Affordance(), + geometry={}, + label="cube", + entity_id="cube", + ), + object_to_eef=poses, + grasp_xpos=poses, + env_mask=_mask(True, True), + ) + }, + ) + system = _workflow_recovery_system( + ( + _workflow_decision( + _mask(True, False), + _mask(False, True), + inverse_satisfied_mask=_mask(False, True), + ), + _workflow_decision(_mask(False, True), _mask(False, False)), + ), + task_state=initial_state, + ) + + result = system.runtime.run( + HandOver( + object=SceneObjectRef("cube"), + resources={"source": "left_actor", "destination": "right_actor"}, + ) + ) + + assert result.status is SkillStatus.COMPLETED + assert torch.equal(result.success_mask, _mask(True, True)) + assert [call.semantic_id for call in system.compiler.grounded_calls] == [ + "hand_over", + "hand_over", + ] + assert [mask.tolist() for mask in system.compiler.grounded_masks] == [ + [True, True], + [False, True], + ] + assert len(result.workflow_recoveries) == 1 + recovery = result.workflow_recoveries[0] + assert recovery.role is SkillWorkflowRecoveryRole.RETRY_RETAINED + assert recovery.attempt_index == 1 + assert torch.equal(recovery.entered_mask, _mask(False, True)) + assert result.task_state.get_held_object("left_gripper") is None + + +def test_runtime_partitions_retained_and_lost_source_rows_in_one_barrier() -> None: + poses = torch.eye(4).unsqueeze(0).repeat(BATCH_SIZE, 1, 1) + initial_state = TaskState( + batch_size=BATCH_SIZE, + device="cpu", + held_objects={ + "left_gripper": HeldObjectState( + semantics=ObjectSemantics( + affordance=Affordance(), + geometry={}, + label="cube", + entity_id="cube", + ), + object_to_eef=poses, + grasp_xpos=poses, + env_mask=_mask(True, True), + ) + }, + ) + system = _workflow_recovery_system( + ( + _workflow_decision( + _mask(False, False), + _mask(True, True), + inverse_satisfied_mask=_mask(True, False), + ), + _workflow_decision(_mask(True, False), _mask(False, False)), + _workflow_decision(_mask(False, True), _mask(False, False)), + _workflow_decision(_mask(False, True), _mask(False, False)), + ), + task_state=initial_state, + ) + + result = system.runtime.run( + HandOver( + object=SceneObjectRef("cube"), + resources={"source": "left_actor", "destination": "right_actor"}, + ) + ) + + assert result.status is SkillStatus.COMPLETED + assert torch.equal(result.success_mask, _mask(True, True)) + assert [call.semantic_id for call in system.compiler.grounded_calls] == [ + "hand_over", + "hand_over", + "pick", + "hand_over", + ] + assert [mask.tolist() for mask in system.compiler.grounded_masks] == [ + [True, True], + [True, False], + [False, True], + [False, True], + ] + assert [trace.role for trace in result.workflow_recoveries] == [ + SkillWorkflowRecoveryRole.RETRY_RETAINED, + SkillWorkflowRecoveryRole.REACQUIRE, + SkillWorkflowRecoveryRole.RETRY_REACQUIRED, + ] + assert [trace.attempt_index for trace in result.workflow_recoveries] == [1, 1, 1] + assert result.task_state.get_held_object("left_gripper") is None + + +def test_runtime_bounds_reacquisition_attempts_per_failed_row() -> None: + system = _workflow_recovery_system( + ( + _workflow_decision(_mask(True, True), _mask(False, False)), + _workflow_decision(_mask(True, False), _mask(False, True)), + _workflow_decision(_mask(False, False), _mask(False, True)), + _workflow_decision(_mask(False, False), _mask(False, True)), + ), + max_recovery_attempts=2, + ) + cube = SceneObjectRef("cube") + + result = system.runtime.run( + Pick(object=cube), + Place(object=cube, inside=SceneObjectRef("bin")), + ) + + assert result.status is SkillStatus.COMPLETED + assert torch.equal(result.success_mask, _mask(True, False)) + assert torch.equal(result.failure_mask, _mask(False, True)) + assert [call.semantic_id for call in system.compiler.grounded_calls] == [ + "pick", + "place", + "pick", + "pick", + ] + assert [trace.role for trace in result.workflow_recoveries] == [ + SkillWorkflowRecoveryRole.REACQUIRE, + SkillWorkflowRecoveryRole.REACQUIRE, + ] + assert [trace.attempt_index for trace in result.workflow_recoveries] == [1, 2] + assert len(result.failures) == 1 + assert "exhausted" in result.failures[0].message + + +def test_runtime_leaves_external_recovery_disabled_at_zero_budget() -> None: + system = _workflow_recovery_system( + ( + _workflow_decision(_mask(True, True), _mask(False, False)), + _workflow_decision(_mask(True, False), _mask(False, True)), + ), + max_recovery_attempts=0, + ) + cube = SceneObjectRef("cube") + + result = system.runtime.run( + Pick(object=cube), + Place(object=cube, inside=SceneObjectRef("bin")), + ) + + assert result.status is SkillStatus.COMPLETED + assert torch.equal(result.success_mask, _mask(True, False)) + assert torch.equal(result.failure_mask, _mask(False, True)) + assert result.workflow_recoveries == () + assert [call.semantic_id for call in system.compiler.grounded_calls] == [ + "pick", + "place", + ] + + +def test_runtime_resolves_workflow_policy_only_after_typed_core_handoff() -> None: + system = _workflow_recovery_system( + ( + _workflow_decision(_mask(True, True), _mask(False, False)), + _workflow_decision(_mask(True, True), _mask(False, False)), + ) + ) + system.compiler._workflow_policy = object() # type: ignore[assignment] + cube = SceneObjectRef("cube") + + result = system.runtime.run( + Pick(object=cube), + Place(object=cube, inside=SceneObjectRef("bin")), + ) + + assert result.status is SkillStatus.COMPLETED + assert result.workflow_recoveries == () + + +def test_cancel_during_reacquisition_safe_stops_every_barrier_row() -> None: + system = _workflow_recovery_system( + ( + _workflow_decision(_mask(True, True), _mask(False, False)), + _workflow_decision(_mask(True, False), _mask(False, True)), + _workflow_decision(_mask(False, True), _mask(False, False)), + ) + ) + cube = SceneObjectRef("cube") + result = system.runtime.start( + Pick(object=cube), + Place(object=cube, inside=SceneObjectRef("bin")), + ) + while len(system.compiler.grounded_calls) < 3: + if result.wait_duration: + system.clock.sleep(result.wait_duration) + result = system.runtime.step() + + held_before_cancel = system.sink.held + result = system.runtime.cancel("caller stopped recovery") + + assert result.status is SkillStatus.CANCELLED + assert torch.equal(result.cancelled_mask, _mask(True, True)) + assert len(result.workflow_recoveries) == 1 + assert result.workflow_recoveries[0].role is SkillWorkflowRecoveryRole.REACQUIRE + assert result.workflow_recoveries[0].call is not None + assert ( + result.workflow_recoveries[0].call.status + is runtime_module.RunnerStatus.CANCELLED + ) + assert system.sink.cancelled == 1 + assert system.sink.held == held_before_cancel + 1 + + +def test_deactivating_a_waiting_row_does_not_cancel_active_reacquisition() -> None: + system = _workflow_recovery_system( + ( + _workflow_decision(_mask(True, True), _mask(False, False)), + _workflow_decision(_mask(True, False), _mask(False, True)), + _workflow_decision(_mask(False, True), _mask(False, False)), + _workflow_decision(_mask(False, True), _mask(False, False)), + ) + ) + cube = SceneObjectRef("cube") + result = system.runtime.start( + Pick(object=cube), + Place(object=cube, inside=SceneObjectRef("bin")), + ) + while len(system.compiler.grounded_calls) < 3: + if result.wait_duration: + system.clock.sleep(result.wait_duration) + result = system.runtime.step() + + result = system.runtime.deactivate_rows( + _mask(True, False), + reason="parallel peer failed", + ) + while not result.terminal: + if result.wait_duration: + system.clock.sleep(result.wait_duration) + result = system.runtime.step() + + assert result.status is SkillStatus.COMPLETED + assert torch.equal(result.cancelled_mask, _mask(True, False)) + assert torch.equal(result.success_mask, _mask(False, True)) + assert torch.equal(result.failure_mask, _mask(False, False)) + assert [trace.role for trace in result.workflow_recoveries] == [ + SkillWorkflowRecoveryRole.REACQUIRE, + SkillWorkflowRecoveryRole.RETRY_REACQUIRED, + ] + + +def test_nonblocking_step_routes_effect_feedback_through_collector() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + result = system.runtime.start(_call("stepwise")) + + assert result.status is SkillStatus.RUNNING + while not result.terminal: + if result.wait_duration: + system.clock.sleep(result.wait_duration) + result = system.runtime.step() + + assert result.status is SkillStatus.COMPLETED + assert len(result.effects) == 1 + assert len(result.calls[0].effects) == 1 + assert system.collector.calls[0][0] == 0 + assert torch.equal(system.collector.calls[0][2], torch.tensor([0, 1])) + assert system.compiler.monitors[0].requests[0].verification_id == 0 + + +def test_runtime_preserves_per_expectation_effect_outcomes_in_trace() -> None: + expectation = EffectExpectationDecision( + expectation_id="joint_target", + satisfied_mask=_mask(True, True), + contradicted_mask=_mask(False, False), + inverse_satisfied_mask=_mask(False, False), + ) + system = _system( + ( + EffectMonitorDecision( + _mask(True, True), + _mask(False, False), + (expectation,), + ), + ) + ) + + result = system.runtime.run(_call("expectation_trace")) + + assert result.status is SkillStatus.COMPLETED + assert len(result.effects) == 1 + recorded = result.effects[0].expectation_decisions + assert len(recorded) == 1 + assert recorded[0].expectation_id == "joint_target" + assert result.to_metadata()["effects"][0]["decision"]["expectations"] == [ + { + "expectation_id": "joint_target", + "satisfied_mask": [True, True], + "contradicted_mask": [False, False], + "inverse_satisfied_mask": [False, False], + } + ] + + +@pytest.mark.parametrize( + ("call", "expected_invalidation", "expected_retry"), + ( + ( + Place( + object=SceneObjectRef("cube"), + inside=SceneObjectRef("bin"), + ), + _mask(False, True), + _mask(True, False), + ), + ( + HandOver(object=SceneObjectRef("cube")), + _mask(False, True), + _mask(False, False), + ), + ), +) +def test_terminal_failure_policy_only_retains_strongly_proven_source_attachment( + call: Place | HandOver, + expected_invalidation: torch.Tensor, + expected_retry: torch.Tensor, +) -> None: + failure = _mask(True, True) + source = EffectExpectationDecision( + expectation_id="source", + satisfied_mask=_mask(False, False), + contradicted_mask=failure, + inverse_satisfied_mask=_mask(True, False), + ) + destination = EffectExpectationDecision( + expectation_id="destination", + satisfied_mask=_mask(False, False), + contradicted_mask=failure, + inverse_satisfied_mask=_mask(False, False), + ) + grounded = SimpleNamespace(analyzed=SimpleNamespace(call=call)) + + invalidation, retry = SkillRuntime._terminal_failure_policy( + grounded, + failure, + (source, destination), + ) + + assert torch.equal(invalidation, expected_invalidation) + assert torch.equal(retry, expected_retry) + + +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_phase_effect_gate_uses_independent_monitor_and_records_boundary_trace() -> ( + 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), + ) + expectation = HeldObjectStateExpectation( + expectation_id="destination", + relation=HeldObjectRelation.ATTACHED, + object_id="cube", + slot_id="primary", + resource_id="arm", + task_state_key="arm", + ) + spec = SemanticEffectSpec( + semantic_id="pick", + effect_kind=SemanticEffectKind.ATTACH, + skill_id="pick_up", + invocation_id="workflow:0", + invocation_revision=0, + env_ids=torch.arange(BATCH_SIZE, dtype=torch.long), + state_expectations=(expectation,), + clauses=( + BinaryEffectClause( + clause_id="destination.constraint", + expectation_id="destination", + source=EffectEvidenceSourceRef( + "test.provider", + "1", + ControlPartEvidenceAddress("hand", "constraint"), + ), + evidence_kind=BinaryEvidenceKind.CONSTRAINT, + expected=True, + ), + ), + ) + terminal_monitor = _DecisionMonitor( + spec, + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + ) + gate_monitor = _DecisionMonitor( + spec, + EffectMonitorDecision(_mask(False, True), _mask(True, False)), + ) + gate = GroundedPhaseEffectGate( + gate_id="destination_acquired", + segment_name="lift", + effect_spec=spec, + effect_monitor=gate_monitor, + retry_action=True, + ) + system.runtime._grounded = SimpleNamespace( + analyzed=SimpleNamespace(effect_monitor_ref=None), + effect_monitor=terminal_monitor, + effect_gates=(gate,), + ) + system.runtime._runner = SimpleNamespace( + session=SimpleNamespace( + active_plan=SimpleNamespace( + expected_effects=StateDelta(held_object_updates={"arm": held}) + ) + ) + ) + system.runtime._current_call_index = 0 + context = system.observation.observe(TaskState.empty(BATCH_SIZE, "cpu")) + request = PhaseEffectGateRequest( + verification_id=7, + gate_id="destination_acquired", + skill_id="pick_up", + invocation_id="workflow:0", + invocation_revision=0, + invocation_index=0, + attempt_generation=3, + next_waypoint_index=4, + segment_name="lift", + requested_at=0.0, + deadline=10.0, + env_mask=_mask(True, True), + ) + + result = system.runtime._phase_effect_gate_verifier(context, request) + + assert result.verification_id == 7 + assert result.gate_id == "destination_acquired" + assert result.attempt_generation == 3 + assert result.next_waypoint_index == 4 + assert torch.equal(result.success_mask, _mask(False, True)) + assert torch.equal(result.failure_mask, _mask(True, False)) + assert torch.equal(result.retry_mask, _mask(True, False)) + assert terminal_monitor.calls == 0 + assert gate_monitor.calls == 1 + assert gate_monitor.requests[0].terminal_segment == "lift" + candidate = gate_monitor.requests[0].expected_effects.held_object_updates["arm"] + assert isinstance(candidate, HeldObjectState) + assert candidate.semantics.entity_id == "cube" + trace = system.runtime._effect_traces[0] + assert trace.boundary_kind == "phase_effect_gate" + assert trace.guard_id is None + assert trace.gate_id == "destination_acquired" + assert trace.segment_name == "lift" + 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)),)) + + result = system.runtime.run(_call("metadata")) + metadata = result.to_metadata() + + json.dumps(metadata, allow_nan=False, sort_keys=True) + assert metadata["schema_version"] == 2 + assert metadata["kind"] == "skill_result" + call = metadata["calls"][0] + assert call["semantic_id"] == "test.metadata" + assert call["call"]["arguments"]["call_id"] == "test.metadata" + assert call["active_plan_attempt_generation"] == 0 + attempt = call["plan_attempts"][0] + assert attempt["trigger"] == "action_planned" + assert attempt["planned_scene_version"] == 1 + assert attempt["planned_collision_world_revision"] == [0, 0] + assert attempt["scene_dependencies"] == ["fixture"] + assert attempt["scene_dependency_monitor_until"] == {"fixture": 0} + typed_attempt = result.calls[0].plan_attempts[0] + assert typed_attempt.scene_dependency_monitor_until == {"fixture": 0} + assert typed_attempt.snapshot().scene_dependency_monitor_until == {"fixture": 0} + resolved = call["resolved_core_policy"] + assert resolved["profile_id"] == "runtime_test_profile" + assert resolved["preset"] == { + "preset_id": "runtime_test_preset", + "schema_version": 1, + } + assert resolved["motion_policy"]["strategy"] == "ik_interp" + assert resolved["motion_policy"]["sample_count"] == 7 + assert resolved["tracking_policy"] == { + "in_flight": None, + "terminal": {"mode": "timed", "settle_duration": 0.0}, + } + assert resolved["recovery_policy"]["max_replans"] == 0 + assert resolved["endpoints"] == [] + assert attempt["resolved_core_policy"] == resolved + assert attempt["tracking_policy"] == resolved["tracking_policy"] + assert attempt["tracking_contract"] is 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"] == {} + assert metadata["workflow_recoveries"] == [] + + metadata["masks"]["success"][0] = False + assert system.runtime.result.to_metadata()["masks"]["success"] == [True, True] + + +@pytest.mark.parametrize("waypoint_index", (-1, 1, True, 1.5)) +def test_plan_attempt_trace_rejects_invalid_scene_dependency_monitor_cutoff( + waypoint_index: object, +) -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + result = system.runtime.run(_call("trace_cutoff")) + attempt = result.calls[0].plan_attempts[0] + + with pytest.raises(ValueError, match="waypoint indices"): + replace( + attempt, + scene_dependency_monitor_until={ + "fixture": waypoint_index # type: ignore[dict-item] + }, + ) + + +def test_plan_attempt_trace_rejects_monitor_cutoff_for_non_dependency() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + result = system.runtime.run(_call("trace_dependency")) + attempt = result.calls[0].plan_attempts[0] + + with pytest.raises(ValueError, match="keys must be scene dependencies"): + replace( + attempt, + scene_dependency_monitor_until={"other": 0}, + ) + + +def test_endpoint_binding_trace_records_only_stable_binding_choices() -> None: + target = JointPositionTarget("left_arm_control", (3, 1)) + binding = EndpointBinding( + slot_id="primary", + endpoint_id="motion", + resource_id="left_arm", + adapter_id="control_part", + target=target, + task_state_key="left_arm_state", + capabilities=frozenset({"cartesian_pose", "joint_position"}), + claim_tokens=frozenset({"arm_workspace", "left_side"}), + joint_ids=(3, 1), + tracking_channels={ + "joint.position": EndpointTrackingChannelBinding( + channel_id="joint.position", + source=TrackingFeedbackSourceRef( + provider_id="planning_context.robot", + revision="1", + address=EndpointTrackingFeedbackAddress( + target=target, + channel_id="joint.position", + ), + ), + projector=TrackingProjectorRef( + projector_id="joint_position_payload", + revision="1", + ), + ) + }, + ) + + trace = SkillEndpointBindingTrace.from_binding(binding) + metadata = trace.to_metadata() + + json.dumps(metadata, allow_nan=False, sort_keys=True) + assert metadata["resource_id"] == "left_arm" + assert metadata["adapter_id"] == "control_part" + assert metadata["transport_id"] == "robot.joint_position" + assert metadata["target_id"] == "left_arm_control" + assert metadata["capabilities"] == ["cartesian_pose", "joint_position"] + assert metadata["claim_tokens"] == ["arm_workspace", "left_side"] + assert metadata["joint_ids"] == [3, 1] + assert "target" not in metadata + tracking = metadata["tracking_channels"][0] + target_fingerprint = [ + { + "__type__": ( + "embodichain.lab.sim.atomic_actions.bindings." "JointPositionTarget" + ) + }, + "robot.joint_position", + "left_arm_control", + [3, 1], + ] + address_fingerprint = [target_fingerprint, "joint.position"] + assert tracking["feedback_source"]["address_fingerprint"] == address_fingerprint + assert tracking["route_fingerprint"] == [ + "joint.position", + ["planning_context.robot", "1", address_fingerprint], + "joint_position_payload", + "1", + ] + tracking["feedback_source"]["address_fingerprint"][0][1] = "mutated" + assert ( + trace.to_metadata()["tracking_channels"][0]["feedback_source"][ + "address_fingerprint" + ][0][1] + == "robot.joint_position" + ) + + +def test_preparation_failure_keeps_resolved_policy_without_plan_attempt( + monkeypatch: pytest.MonkeyPatch, +) -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + monkeypatch.setattr( + system.engine, + "start", + Mock(side_effect=RuntimeError("planner unavailable")), + ) + + result = system.runtime.start(_call("planning_failure")) + metadata = result.to_metadata() + + assert result.status is SkillStatus.FAILED + assert len(result.calls) == 1 + assert result.calls[0].plan_attempts == () + assert result.calls[0].resolved_core_policy.preset_id == "runtime_test_preset" + assert metadata["calls"][0]["active_plan_attempt_generation"] is None + assert ( + metadata["calls"][0]["resolved_core_policy"]["motion_policy"]["strategy"] + == "ik_interp" + ) + + +def test_cancel_inherits_runner_cancel_then_hold_safe_stop() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + system.runtime.start(_call("cancel")) + + result = system.runtime.cancel("operator stop") + + assert result.status is SkillStatus.CANCELLED + assert torch.equal(result.cancelled_mask, _mask(True, True)) + assert not result.eligible_mask.any() + assert system.sink.cancelled == 1 + assert system.sink.held == 1 + assert result.calls[0].status.value == "cancelled" + + +def test_facade_varargs_and_programmatic_iterable_share_runtime_path() -> None: + decisions = ( + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + ) + iterable_system = _system(decisions) + facade_system = _system(decisions) + calls = (_call("first"), _call("second")) + + iterable_result = iterable_system.runtime.run(calls) + facade_result = AtomicSkills(facade_system.runtime).run(*calls) + + assert iterable_result.status is facade_result.status + assert torch.equal(iterable_result.success_mask, facade_result.success_mask) + assert [trace.skill_id for trace in iterable_result.calls] == [ + trace.skill_id for trace in facade_result.calls + ] + assert iterable_system.compiler.analyze_count == 1 + assert facade_system.compiler.analyze_count == 1 + assert [item.skill_id for item in iterable_system.compiler.invocations] == [ + item.skill_id for item in facade_system.compiler.invocations + ] + + +def test_from_env_requires_an_explicit_runtime_provider() -> None: + class AttributeBag: + compiler = object() + robot = object() + scene = object() + + with pytest.raises(TypeError, match="no semantic-skill integration adapter"): + AtomicSkills.from_env(AttributeBag()) + + +def test_from_env_delegates_preset_to_installed_provider() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + + class Provider: + def __init__(self) -> None: + self.presets: list[str] = [] + + def create_skill_runtime(self, *, preset: str) -> SkillRuntime: + self.presets.append(preset) + return system.runtime + + provider = Provider() + skills = AtomicSkills.from_env(provider, preset="precise") + + assert skills.runtime is system.runtime + assert provider.presets == ["precise"] + + +def test_result_snapshots_do_not_expose_runtime_masks() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + result = system.runtime.run(_call("owned")) + + result.success_mask.zero_() + result.calls[0].completed_mask.zero_() + fresh = system.runtime.result + + assert torch.equal(fresh.success_mask, _mask(True, True)) + assert torch.equal(fresh.calls[0].completed_mask, _mask(True, True)) + + +def test_fork_creates_an_independent_lane_on_the_shared_clock() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + lane_sink = _CommandSink() + + lane = system.runtime.fork(lane_sink) + + assert lane is not system.runtime + assert lane.compiler is system.runtime.compiler + assert lane.clock is system.runtime.clock + assert lane.status is SkillStatus.IDLE + assert lane_sink.sent == 0 + + +def test_runner_failure_does_not_relabel_peer_cancelled_rows() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + system.runtime.start(_call("row_failure")) + system.runtime.deactivate_rows(_mask(True, False), reason="peer branch failed") + + def fail_observation(task_state: TaskState) -> PlanningContext: + del task_state + raise RuntimeError("observation unavailable") + + system.observation.observe = fail_observation + result = system.runtime.step() + if result.wait_duration > 0.0: + system.clock.sleep(result.wait_duration) + result = system.runtime.step() + + assert result.status is SkillStatus.FAILED + assert torch.equal(result.cancelled_mask, _mask(True, False)) + assert torch.equal(result.failure_mask, _mask(False, True)) + + +def test_deactivate_all_rows_safe_stops_immediately_before_due_time() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + system.runtime.start(_call("deactivate_all")) + + result = system.runtime.deactivate_rows( + _mask(True, True), + reason="parallel peer failed", + ) + + assert result.status is SkillStatus.CANCELLED + assert torch.equal(result.cancelled_mask, _mask(True, True)) + assert system.sink.cancelled == 1 + assert system.sink.held == 1 + + +def test_parallel_factory_analyzes_claims_and_forks_owned_shared_clock_lanes() -> None: + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + + def analyze_claims( + self: _Compiler, + calls: tuple[RegisteredSemanticCall, ...], + *, + workflow_id: str, + path: tuple[object, ...] = ("workflow",), + ) -> object: + del self, workflow_id, path + analyzed = [] + for call_index, call in enumerate(calls): + joint_id = 0 if call.call_id.endswith("left") else 1 + analyzed.append( + SimpleNamespace( + index=call_index, + symbolic_writes=frozenset(), + opaque_symbolic_effect=False, + bound=SimpleNamespace( + binding=SimpleNamespace( + claim=ResourceClaim( + frozenset({f"resource_{joint_id}"}), + (joint_id,), + ) + ) + ), + ) + ) + return SimpleNamespace(calls=tuple(analyzed)) + + class AcceptSafety: + def validate(self, *, branch_frames: object, merged_frame: object) -> None: + del branch_frames, merged_frame + + system.compiler.analyze = MethodType(analyze_claims, system.compiler) + parallel = ParallelSkillRuntime.from_template( + system.runtime, + { + "left": (_call("left"),), + "right": (_call("right"),), + }, + system.sink, + ParallelTimingPolicy(0.1), + AcceptSafety(), + timeout_steps=5, + ) + + assert parallel.clock is system.runtime.clock + assert parallel.branch_claims["left"].joint_ids == (0,) + assert parallel.branch_claims["right"].joint_ids == (1,) + + changed = StateDelta( + articulation_joint_updates={ + ("template", "joint"): ArticulationJointState(torch.ones(2, 1)) + } + ).apply(system.runtime.task_state, _mask(True, True)) + system.runtime.adopt_verified_task_state(changed) + assert all( + result.task_state.get_articulation_joint_state("template", "joint") is None + for result in parallel.result.branch_results.values() + ) diff --git a/tests/sim/skills/test_scene.py b/tests/sim/skills/test_scene.py new file mode 100644 index 000000000..fc14262a6 --- /dev/null +++ b/tests/sim/skills/test_scene.py @@ -0,0 +1,1170 @@ +# ---------------------------------------------------------------------------- +# 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 authoritative semantic-scene registrations.""" + +from __future__ import annotations + +from dataclasses import FrozenInstanceError +from typing import Literal + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + Affordance, + AntipodalAffordance, + EntityState, + ObservedArticulationJointState, + SceneSnapshot, +) +from embodichain.lab.sim.planners.base_planner import CollisionWorldInfo +from embodichain.lab.sim.skills import ( + AmbiguousSceneAffordanceError, + GRASP_AFFORDANCE_CAPABILITY, + SceneAffordanceRef, + SceneArticulationRef, + SceneCollisionRole, + SceneCollisionWorldMode, + SceneEntityRegistration, + SceneLinkRef, + SceneObjectRef, + SceneRegistry, + UnsupportedSceneAffordanceError, +) + + +class _StateProvider: + """Return one fixed identity pose for registration validation.""" + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp + return EntityState(torch.eye(4).repeat(env_ids.numel(), 1, 1)) + + +class _GeometryProvider: + """Return one opaque collision-geometry descriptor.""" + + def get_geometry(self) -> object: + return {"kind": "box"} + + +class _EmptyGeometryProvider: + """Satisfy the geometry protocol but fail to materialize a descriptor.""" + + def get_geometry(self) -> object: + return None + + +class _MutableStateProvider: + """Expose a mutable pose while recording provider calls.""" + + def __init__(self, pose: torch.Tensor) -> None: + self.pose = pose + self.calls = 0 + + def observe( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> EntityState: + del timestamp, env_ids + self.calls += 1 + return EntityState(self.pose) + + +class _MutableJointProvider: + """Expose one mutable canonical articulation joint observation.""" + + def __init__(self, position: torch.Tensor) -> None: + self.position = position + self.calls = 0 + + def observe_joints( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> dict[str, ObservedArticulationJointState]: + del timestamp, env_ids + self.calls += 1 + return {"slide": ObservedArticulationJointState(self.position)} + + +class _MotionGenerator: + """Minimal dynamic-collision integration surface.""" + + def __init__( + self, + *, + entity_ids: tuple[str, ...], + world_entity_ids: tuple[str, ...] | None = None, + supports_updates: bool = True, + batch_mode: Literal["shared", "per_env"] | None = "per_env", + ) -> None: + self.collision_world_info = CollisionWorldInfo( + entity_ids=entity_ids if world_entity_ids is None else world_entity_ids, + dynamic_entity_ids=entity_ids, + supports_updates=supports_updates, + batch_mode=batch_mode, + ) + + +class _ExternalSceneProvider: + """External provider with an explicit concrete collision declaration.""" + + def __init__(self, entity_ids: tuple[str, ...]) -> None: + self.collision_entity_ids = entity_ids + + def snapshot( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> SceneSnapshot: + del timestamp, env_ids + raise NotImplementedError + + +class _SimulationEntity: + """Simulation entity pose source used by the opt-in adapter tests.""" + + def __init__( + self, + pose: torch.Tensor, + *, + qpos: torch.Tensor | None = None, + joint_names: tuple[str, ...] = (), + ) -> None: + self.pose = pose + self.qpos = qpos + self.joint_names = joint_names + + def get_local_pose(self, *, to_matrix: bool) -> torch.Tensor: + assert to_matrix is True + return self.pose + + def get_qpos(self, *, target: bool) -> torch.Tensor: + assert target is False + if self.qpos is None: + raise RuntimeError("This simulation fixture has no articulation qpos.") + return self.qpos + + +class _Simulation: + """Minimal simulation lookup surface with selected and unselected assets.""" + + def __init__(self) -> None: + self.rigid_objects = { + "sim_cube": _SimulationEntity(torch.eye(4)), + "ignored": _SimulationEntity(torch.eye(4) * 2.0), + } + self.articulations = { + "sim_drawer": _SimulationEntity( + torch.eye(4), + qpos=torch.tensor([[0.25]]), + joint_names=("slide",), + ), + } + + def get_rigid_object(self, uid: str) -> _SimulationEntity | None: + return self.rigid_objects.get(uid) + + def get_articulation(self, uid: str) -> _SimulationEntity | None: + return self.articulations.get(uid) + + +class _CopyTrackedAffordance(AntipodalAffordance): + """Count payload copies so metadata projection can prove it performs none.""" + + copies = 0 + + def __deepcopy__(self, memo: dict[int, object]) -> _CopyTrackedAffordance: + del memo + type(self).copies += 1 + return _CopyTrackedAffordance() + + +class _SelfCopyAffordance(AntipodalAffordance): + """Malicious payload that violates deepcopy ownership.""" + + def __deepcopy__(self, memo: dict[int, object]) -> _SelfCopyAffordance: + del memo + return self + + +@pytest.mark.parametrize("entity_id", ["", " cube", "cube "]) +def test_scene_entity_ref_rejects_non_exact_identifier(entity_id: str) -> None: + with pytest.raises(ValueError, match="entity_id"): + SceneObjectRef(entity_id) + + +def test_scene_entity_refs_are_typed_and_immutable() -> None: + object_ref = SceneObjectRef("cube") + + assert object_ref != SceneArticulationRef("cube") + with pytest.raises(FrozenInstanceError): + object_ref.entity_id = "other" # type: ignore[misc] + + +def test_registration_normalizes_self_alias_without_rewriting_names() -> None: + registration = SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + aliases=("cube", "sim_cube"), + ) + + assert registration.aliases == ("sim_cube",) + + +def test_registration_rejects_duplicate_aliases() -> None: + with pytest.raises(ValueError, match="aliases"): + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + aliases=("sim_cube", "sim_cube"), + ) + + +def test_registration_rejects_string_as_alias_collection() -> None: + with pytest.raises(TypeError, match="aliases.*not a string"): + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + aliases="sim_cube", # type: ignore[arg-type] + ) + + +def test_root_registration_requires_explicit_state_provider() -> None: + with pytest.raises(ValueError, match="state_provider"): + SceneEntityRegistration(ref=SceneObjectRef("cube")) + + +def test_joint_state_provider_is_owned_by_articulation_registration() -> None: + joint_provider = _MutableJointProvider(torch.tensor([[0.1], [0.2]])) + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneArticulationRef("drawer"), + state_provider=_StateProvider(), + joint_state_provider=joint_provider, + ), + ) + ) + provider = registry.make_scene_provider() + env_ids = torch.tensor([0, 1], dtype=torch.long) + + first = provider.snapshot(timestamp=0.0, env_ids=env_ids) + returned = first.articulation_joints[("drawer", "slide")] + returned.position.zero_() + assert torch.equal( + first.articulation_joints[("drawer", "slide")].position, + torch.tensor([[0.1], [0.2]]), + ) + + joint_provider.position[:, 0] = torch.tensor([0.3, 0.4]) + second = provider.snapshot(timestamp=1.0, env_ids=env_ids) + assert second.version == first.version + 1 + assert torch.equal( + second.articulation_joints[("drawer", "slide")].position, + torch.tensor([[0.3], [0.4]]), + ) + assert joint_provider.calls == 2 + + +def test_joint_state_provider_rejects_non_articulation_registration() -> None: + with pytest.raises(ValueError, match="SceneArticulationRef"): + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + joint_state_provider=_MutableJointProvider(torch.tensor([0.0])), + ) + + +def test_link_registration_requires_parent_and_native_name() -> None: + with pytest.raises(ValueError, match="parent and native_name"): + SceneEntityRegistration( + ref=SceneLinkRef("drawer_handle_link"), + state_provider=_StateProvider(), + ) + + +def test_affordance_registration_owns_parent_relation_and_pose() -> None: + relative_pose = torch.eye(4) + registration = SceneEntityRegistration( + ref=SceneAffordanceRef("drawer_handle"), + parent=SceneLinkRef("drawer_handle_link"), + native_name="handle", + affordance=Affordance(), + relative_pose=relative_pose, + ) + relative_pose.fill_(4.0) + + assert registration.relative_pose is not None + assert torch.equal(registration.relative_pose, torch.eye(4)) + + +def test_affordance_registration_rejects_two_pose_sources() -> None: + with pytest.raises(ValueError, match="mutually exclusive"): + SceneEntityRegistration( + ref=SceneAffordanceRef("drawer_handle"), + state_provider=_StateProvider(), + parent=SceneLinkRef("drawer_handle_link"), + native_name="handle", + affordance=Affordance(), + relative_pose=torch.eye(4), + ) + + +def test_grasp_capability_requires_typed_versioned_payload() -> None: + object_ref = SceneObjectRef("cube") + common = { + "ref": SceneAffordanceRef("cube_grasp"), + "parent": object_ref, + "native_name": "grasp", + "relative_pose": torch.eye(4), + "affordance_capabilities": frozenset({GRASP_AFFORDANCE_CAPABILITY}), + } + + with pytest.raises(TypeError, match="AntipodalAffordance"): + SceneEntityRegistration( + **common, + affordance=Affordance(), + affordance_revision="v1", + ) + with pytest.raises(ValueError, match="affordance_revision"): + SceneEntityRegistration( + **common, + affordance=AntipodalAffordance(), + ) + + +def test_registry_selects_only_explicit_scoped_affordance_default() -> None: + object_ref = SceneObjectRef("cube") + first = SceneAffordanceRef("first_grasp") + second = SceneAffordanceRef("second_grasp") + + def registrations(*, with_default: bool) -> tuple[SceneEntityRegistration, ...]: + return ( + SceneEntityRegistration( + ref=object_ref, + state_provider=_StateProvider(), + default_affordances=( + {GRASP_AFFORDANCE_CAPABILITY: second} if with_default else {} + ), + ), + *tuple( + SceneEntityRegistration( + ref=ref, + parent=object_ref, + native_name=ref.entity_id, + affordance=AntipodalAffordance(), + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + affordance_revision="v1", + relative_pose=torch.eye(4), + ) + for ref in (first, second) + ), + ) + + ambiguous = SceneRegistry(registrations(with_default=False)) + with pytest.raises(AmbiguousSceneAffordanceError, match="multiple"): + ambiguous.resolve_affordance( + object_ref, + capability=GRASP_AFFORDANCE_CAPABILITY, + ) + with pytest.raises(UnsupportedSceneAffordanceError, match="no affordance"): + ambiguous.resolve_affordance( + object_ref, + capability="affordance.unknown", + ) + + registry = SceneRegistry(registrations(with_default=True)) + assert ( + registry.resolve_affordance( + object_ref, + capability=GRASP_AFFORDANCE_CAPABILITY, + ) + == second + ) + assert ( + registry.resolve_affordance( + object_ref, + capability=GRASP_AFFORDANCE_CAPABILITY, + explicit=first, + ) + == first + ) + + +def test_registry_metadata_projection_does_not_copy_affordance_payload() -> None: + object_ref = SceneObjectRef("cube") + _CopyTrackedAffordance.copies = 0 + registry = SceneRegistry( + ( + SceneEntityRegistration(ref=object_ref, state_provider=_StateProvider()), + SceneEntityRegistration( + ref=SceneAffordanceRef("cube_grasp"), + parent=object_ref, + native_name="grasp", + affordance=_CopyTrackedAffordance(), + affordance_capabilities=frozenset({GRASP_AFFORDANCE_CAPABILITY}), + affordance_revision="v1", + relative_pose=torch.eye(4), + ), + ) + ) + _CopyTrackedAffordance.copies = 0 + + metadata = registry.entity_metadata + + assert metadata[1].affordance_payload_type is _CopyTrackedAffordance + assert _CopyTrackedAffordance.copies == 0 + + +def test_registry_rejects_affordance_that_cannot_produce_owned_copy() -> None: + cube = SceneObjectRef("cube") + + with pytest.raises(TypeError, match="distinct value"): + SceneRegistry( + ( + SceneEntityRegistration(ref=cube, state_provider=_StateProvider()), + SceneEntityRegistration( + ref=SceneAffordanceRef("cube_grasp"), + parent=cube, + native_name="grasp", + affordance=_SelfCopyAffordance(), + relative_pose=torch.eye(4), + ), + ) + ) + + +def test_registry_builds_owned_object_semantics_from_direct_child() -> None: + cube = SceneObjectRef("cube") + table = SceneObjectRef("table") + cube_grasp = SceneAffordanceRef("cube_grasp") + table_grasp = SceneAffordanceRef("table_grasp") + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=cube, + state_provider=_StateProvider(), + semantic_type="cube", + ), + SceneEntityRegistration(ref=table, state_provider=_StateProvider()), + SceneEntityRegistration( + ref=cube_grasp, + parent=cube, + native_name="grasp", + affordance=AntipodalAffordance(), + relative_pose=torch.eye(4), + ), + SceneEntityRegistration( + ref=table_grasp, + parent=table, + native_name="grasp", + affordance=AntipodalAffordance(), + relative_pose=torch.eye(4), + ), + ) + ) + + first = registry.object_semantics(cube, affordance=cube_grasp) + second = registry.object_semantics("cube", affordance="cube_grasp") + + assert first.entity_id == "cube" + assert first.label == "cube" + assert first.affordance is not second.affordance + first.affordance.custom_config["mutated"] = True + assert "mutated" not in second.affordance.custom_config + with pytest.raises(ValueError, match="not a direct child"): + registry.object_semantics(cube, affordance=table_grasp) + + +def test_collision_registration_requires_geometry_provider() -> None: + with pytest.raises(ValueError, match="geometry_provider"): + SceneEntityRegistration( + ref=SceneObjectRef("obstacle"), + state_provider=_StateProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ) + + registration = SceneEntityRegistration( + ref=SceneObjectRef("obstacle"), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ) + assert registration.geometry_provider is not None + + +def test_registry_resolves_aliases_to_typed_canonical_refs() -> None: + cube_ref = SceneObjectRef("cube") + drawer_ref = SceneArticulationRef("drawer") + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=cube_ref, + state_provider=_StateProvider(), + aliases=("sim_cube",), + ), + SceneEntityRegistration( + ref=drawer_ref, + state_provider=_StateProvider(), + aliases=("sim_drawer",), + ), + ) + ) + + assert registry.resolve("sim_cube", expected_type=SceneObjectRef) is cube_ref + assert registry.lookup("sim_drawer").ref is drawer_ref + assert registry.aliases == { + "sim_cube": "cube", + "sim_drawer": "drawer", + } + + with pytest.raises(TypeError, match="SceneObjectRef"): + registry.resolve("sim_cube", expected_type=SceneArticulationRef) + with pytest.raises(TypeError, match="SceneArticulationRef"): + registry.resolve(SceneArticulationRef("cube")) + + +def test_registry_enforces_one_flat_global_id_namespace() -> None: + registrations = ( + SceneEntityRegistration( + ref=SceneObjectRef("shared"), + state_provider=_StateProvider(), + ), + SceneEntityRegistration( + ref=SceneArticulationRef("shared"), + state_provider=_StateProvider(), + ), + ) + + with pytest.raises(ValueError, match="Duplicate canonical"): + SceneRegistry(registrations) + + +def test_registry_rejects_alias_collision_with_canonical_id() -> None: + with pytest.raises(ValueError, match="collides with canonical"): + SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + aliases=("drawer",), + ), + SceneEntityRegistration( + ref=SceneArticulationRef("drawer"), + state_provider=_StateProvider(), + ), + ) + ) + + +def test_registry_rejects_ambiguous_aliases_across_types() -> None: + with pytest.raises(ValueError, match="ambiguous"): + SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + aliases=("legacy",), + ), + SceneEntityRegistration( + ref=SceneArticulationRef("drawer"), + state_provider=_StateProvider(), + aliases=("legacy",), + ), + ) + ) + + +def test_registry_requires_registered_exact_typed_parent() -> None: + link_registration = SceneEntityRegistration( + ref=SceneLinkRef("drawer_link"), + parent=SceneArticulationRef("drawer"), + native_name="link", + state_provider=_StateProvider(), + ) + + with pytest.raises(ValueError, match="unregistered parent"): + SceneRegistry((link_registration,)) + with pytest.raises(TypeError, match="registered as SceneObjectRef"): + SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("drawer"), + state_provider=_StateProvider(), + ), + link_registration, + ) + ) + + +@pytest.mark.parametrize( + "ref_type", + [SceneLinkRef, SceneAffordanceRef], +) +def test_registry_rejects_duplicate_parent_native_member(ref_type: type) -> None: + parent = SceneArticulationRef("drawer") + + def member_registration(entity_id: str) -> SceneEntityRegistration: + if ref_type is SceneLinkRef: + return SceneEntityRegistration( + ref=SceneLinkRef(entity_id), + parent=parent, + native_name="handle", + state_provider=_StateProvider(), + ) + return SceneEntityRegistration( + ref=SceneAffordanceRef(entity_id), + parent=parent, + native_name="handle", + affordance=Affordance(), + relative_pose=torch.eye(4), + ) + + with pytest.raises(ValueError, match="native_name.*already registered"): + SceneRegistry( + ( + SceneEntityRegistration( + ref=parent, + state_provider=_StateProvider(), + ), + member_registration("first"), + member_registration("second"), + ) + ) + + +def test_registry_is_structurally_immutable_and_owns_relative_pose() -> None: + parent = SceneObjectRef("drawer") + relative_pose = torch.eye(4) + affordance_registration = SceneEntityRegistration( + ref=SceneAffordanceRef("handle"), + parent=parent, + native_name="handle", + affordance=Affordance(), + relative_pose=relative_pose, + ) + registrations = [ + SceneEntityRegistration( + ref=parent, + state_provider=_StateProvider(), + ), + affordance_registration, + ] + registry = SceneRegistry(registrations) + + registrations.clear() + relative_pose.fill_(3.0) + assert len(registry) == 2 + returned_pose = registry.lookup("handle").relative_pose + assert returned_pose is not None + assert torch.equal(returned_pose, torch.eye(4)) + returned_pose.fill_(5.0) + assert torch.equal(registry.lookup("handle").relative_pose, torch.eye(4)) + with pytest.raises(TypeError): + registry.aliases["new"] = "drawer" # type: ignore[index] + with pytest.raises(FrozenInstanceError): + registry.collision_world_mode = SceneCollisionWorldMode.SHARED # type: ignore[misc] + + +def test_registry_owns_and_defensively_copies_affordance_metadata() -> None: + parent = SceneObjectRef("drawer") + affordance = Affordance(custom_config={"limits": {"opening": 0.3}}) + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=parent, + state_provider=_StateProvider(), + ), + SceneEntityRegistration( + ref=SceneAffordanceRef("handle"), + parent=parent, + native_name="handle", + affordance=affordance, + relative_pose=torch.eye(4), + ), + ) + ) + + affordance.custom_config["limits"]["opening"] = 0.8 + public_affordance = registry.lookup("handle").affordance + assert public_affordance is not None + assert public_affordance.custom_config["limits"]["opening"] == 0.3 + + public_affordance.custom_config["limits"]["opening"] = 1.0 + second_read = registry.lookup("handle").affordance + assert second_read is not None + assert second_read.custom_config["limits"]["opening"] == 0.3 + + +def test_registry_provider_uses_canonical_ids_and_derives_relative_pose() -> None: + parent_pose = torch.eye(4).repeat(2, 1, 1) + parent_pose[:, 0, 3] = torch.tensor([1.0, 2.0]) + relative_pose = torch.eye(4) + relative_pose[1, 3] = 0.25 + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("drawer"), + state_provider=_MutableStateProvider(parent_pose), + aliases=("sim_drawer",), + ), + SceneEntityRegistration( + ref=SceneAffordanceRef("handle"), + parent=SceneObjectRef("drawer"), + native_name="handle", + affordance=Affordance(), + relative_pose=relative_pose, + ), + ) + ) + + snapshot = registry.make_scene_provider().snapshot( + timestamp=0.0, + env_ids=torch.tensor([10, 20], dtype=torch.long), + ) + + assert set(snapshot.entities) == {"drawer", "handle"} + assert "sim_drawer" not in snapshot.entities + assert torch.equal( + snapshot.entities["handle"].pose, + torch.matmul(parent_pose, relative_pose), + ) + + +def test_registry_providers_have_independent_revisions() -> None: + state_provider = _MutableStateProvider(torch.eye(4)) + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=state_provider, + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + ), + collision_world_mode=SceneCollisionWorldMode.PER_ENV, + ) + first_provider = registry.make_scene_provider() + second_provider = registry.make_scene_provider() + env_ids = torch.tensor([0, 1], dtype=torch.long) + first_provider.snapshot(timestamp=0.0, env_ids=env_ids) + moved = torch.eye(4).repeat(2, 1, 1) + moved[1, 0, 3] = 0.1 + state_provider.pose = moved + + changed = first_provider.snapshot(timestamp=1.0, env_ids=env_ids) + independent_initial = second_provider.snapshot(timestamp=1.0, env_ids=env_ids) + + assert changed.version == 1 + assert changed.collision_world_revisions(2) == (0, 1) + assert independent_initial.version == 0 + assert independent_initial.collision_world_revisions(2) == (0, 0) + + +def test_registry_provider_accumulates_subthreshold_motion_per_row() -> None: + pose = torch.eye(4).repeat(2, 1, 1) + state_provider = _MutableStateProvider(pose) + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=state_provider, + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + ), + collision_world_mode=SceneCollisionWorldMode.PER_ENV, + ) + provider = registry.make_scene_provider(translation_threshold=0.01) + env_ids = torch.tensor([4, 8], dtype=torch.long) + provider.snapshot(timestamp=0.0, env_ids=env_ids) + first_motion = pose.clone() + first_motion[1, 0, 3] = 0.006 + state_provider.pose = first_motion + + below_threshold = provider.snapshot(timestamp=1.0, env_ids=env_ids) + second_motion = first_motion.clone() + second_motion[1, 0, 3] = 0.012 + state_provider.pose = second_motion + accumulated_change = provider.snapshot(timestamp=2.0, env_ids=env_ids) + + assert below_threshold.version == 0 + assert below_threshold.collision_world_revisions(2) == (0, 0) + assert accumulated_change.version == 1 + assert accumulated_change.collision_world_revisions(2) == (0, 1) + + +def test_multi_env_dynamic_collision_requires_explicit_mode_before_observation() -> ( + None +): + state_provider = _MutableStateProvider(torch.eye(4)) + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=state_provider, + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + ) + ) + + with pytest.raises(ValueError, match="explicit collision_world_mode"): + registry.make_scene_provider(batch_size=2) + provider = registry.make_scene_provider() + with pytest.raises(ValueError, match="explicit collision_world_mode"): + provider.snapshot( + timestamp=0.0, + env_ids=torch.tensor([0, 1], dtype=torch.long), + ) + assert state_provider.calls == 0 + + +def test_single_env_dynamic_collision_defaults_to_shared_mode() -> None: + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + ) + ) + provider = registry.make_scene_provider(batch_size=1) + + assert provider.collision_world_mode is SceneCollisionWorldMode.SHARED + + snapshot = provider.snapshot( + timestamp=0.0, + env_ids=torch.tensor([0], dtype=torch.long), + ) + + assert provider.collision_entity_ids == ("cube",) + assert snapshot.collision_world_revisions(1) == (0,) + + +def test_collision_integration_requires_exact_canonical_ids_and_mode() -> None: + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + aliases=("sim_cube",), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + ), + collision_world_mode=SceneCollisionWorldMode.PER_ENV, + ) + + assert ( + registry.validate_collision_integration( + _MotionGenerator(entity_ids=("cube",)), # type: ignore[arg-type] + batch_size=2, + scene_provider=_ExternalSceneProvider(("cube",)), + ) + is SceneCollisionWorldMode.PER_ENV + ) + with pytest.raises(ValueError, match="authoritative registry IDs"): + registry.validate_collision_integration( + _MotionGenerator(entity_ids=("sim_cube",)), # type: ignore[arg-type] + batch_size=2, + ) + with pytest.raises(ValueError, match="does not support"): + registry.validate_collision_integration( + _MotionGenerator( # type: ignore[arg-type] + entity_ids=("cube",), + supports_updates=False, + ), + batch_size=2, + ) + with pytest.raises(ValueError, match="mode mismatch"): + registry.validate_collision_integration( + _MotionGenerator( # type: ignore[arg-type] + entity_ids=("cube",), + batch_mode="shared", + ), + batch_size=2, + ) + + +def test_collision_integration_requires_exact_full_world_ids() -> None: + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + SceneEntityRegistration( + ref=SceneObjectRef("table"), + aliases=("legacy_table",), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.STATIC, + ), + ), + collision_world_mode=SceneCollisionWorldMode.PER_ENV, + ) + + assert registry.collision_world_entity_ids == ("cube", "table") + assert ( + registry.validate_collision_integration( + _MotionGenerator( + entity_ids=("cube",), + world_entity_ids=("cube", "table"), + ), # type: ignore[arg-type] + batch_size=2, + ) + is SceneCollisionWorldMode.PER_ENV + ) + with pytest.raises(ValueError, match="Collision world.*authoritative registry IDs"): + registry.validate_collision_integration( + _MotionGenerator( + entity_ids=("cube",), + world_entity_ids=("cube", "legacy_table"), + ), # type: ignore[arg-type] + batch_size=2, + ) + + +def test_static_only_collision_world_does_not_require_dynamic_updates() -> None: + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("table"), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.STATIC, + ), + ) + ) + + assert ( + registry.validate_collision_integration( + _MotionGenerator( + entity_ids=(), + world_entity_ids=("table",), + supports_updates=False, + batch_mode=None, + ), # type: ignore[arg-type] + batch_size=2, + ) + is None + ) + + +def test_collision_integration_rejects_external_provider_id_drift() -> None: + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + ), + collision_world_mode=SceneCollisionWorldMode.PER_ENV, + ) + + with pytest.raises(ValueError, match="provider.*authoritative registry IDs"): + registry.validate_collision_integration( + _MotionGenerator(entity_ids=("cube",)), # type: ignore[arg-type] + batch_size=2, + scene_provider=_ExternalSceneProvider(("legacy_cube",)), + ) + + +def test_planning_provider_factory_validates_before_returning_provider() -> None: + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + ), + collision_world_mode=SceneCollisionWorldMode.PER_ENV, + ) + + provider = registry.make_planning_scene_provider( + _MotionGenerator(entity_ids=("cube",)), # type: ignore[arg-type] + batch_size=2, + ) + assert provider.collision_entity_ids == ("cube",) + + with pytest.raises(ValueError, match="entity mismatch"): + registry.make_planning_scene_provider( + _MotionGenerator(entity_ids=("other",)), # type: ignore[arg-type] + batch_size=2, + ) + + with pytest.raises(ValueError, match="configured batch_size=2"): + provider.snapshot( + timestamp=0.0, + env_ids=torch.tensor([0], dtype=torch.long), + ) + + +def test_collision_geometry_is_materialized_under_canonical_ids() -> None: + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("dynamic_cube"), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + SceneEntityRegistration( + ref=SceneObjectRef("static_table"), + state_provider=_StateProvider(), + geometry_provider=_GeometryProvider(), + collision_role=SceneCollisionRole.STATIC, + ), + ), + collision_world_mode=SceneCollisionWorldMode.SHARED, + ) + + all_geometry = registry.collision_geometry_by_id() + dynamic_geometry = registry.collision_geometry_by_id(SceneCollisionRole.DYNAMIC) + + assert set(all_geometry) == {"dynamic_cube", "static_table"} + assert set(dynamic_geometry) == {"dynamic_cube"} + with pytest.raises(TypeError): + all_geometry["other"] = {} # type: ignore[index] + + +def test_collision_integration_rejects_empty_dynamic_geometry() -> None: + registry = SceneRegistry( + ( + SceneEntityRegistration( + ref=SceneObjectRef("cube"), + state_provider=_StateProvider(), + geometry_provider=_EmptyGeometryProvider(), + collision_role=SceneCollisionRole.DYNAMIC, + ), + ) + ) + + with pytest.raises(ValueError, match="scene entity 'cube'.*None"): + registry.validate_collision_integration( + _MotionGenerator( # type: ignore[arg-type] + entity_ids=("cube",), + batch_mode="shared", + ), + batch_size=1, + ) + + +def test_from_simulation_is_explicit_and_uses_uid_only_as_alias() -> None: + simulation = _Simulation() + + registry = SceneRegistry.from_simulation( + simulation, # type: ignore[arg-type] + rigid_objects={"cube": "sim_cube"}, + ) + snapshot = registry.make_scene_provider().snapshot( + timestamp=0.0, + env_ids=torch.tensor([0], dtype=torch.long), + ) + + assert len(registry) == 1 + assert registry.resolve("sim_cube") == SceneObjectRef("cube") + assert registry.lookup("cube").collision_role is SceneCollisionRole.NONE + assert registry.dynamic_collision_entity_ids == () + assert registry.collision_geometry_by_id() == {} + assert set(snapshot.entities) == {"cube"} + assert "ignored" not in snapshot.entities + + +def test_from_simulation_does_not_register_canonical_uid_as_alias() -> None: + simulation = _Simulation() + simulation.rigid_objects["cube"] = simulation.rigid_objects["sim_cube"] + + registry = SceneRegistry.from_simulation( + simulation, # type: ignore[arg-type] + rigid_objects={"cube": "cube"}, + ) + + assert registry.resolve("cube") == SceneObjectRef("cube") + assert registry.aliases == {} + + +def test_from_simulation_publishes_named_articulation_qpos() -> None: + registry = SceneRegistry.from_simulation( + _Simulation(), # type: ignore[arg-type] + articulations={"drawer": "sim_drawer"}, + ) + + snapshot = registry.make_scene_provider().snapshot( + timestamp=0.0, + env_ids=torch.tensor([0], dtype=torch.long), + ) + + state = snapshot.articulation_joints[("drawer", "slide")] + assert torch.equal(state.position, torch.tensor([[0.25]])) + assert state.valid_mask is not None and state.valid_mask.tolist() == [True] + + +def test_from_simulation_derives_live_geometry_only_for_explicit_collision_role() -> ( + None +): + simulation = _Simulation() + registry = SceneRegistry.from_simulation( + simulation, # type: ignore[arg-type] + rigid_objects={"cube": "sim_cube"}, + collision_roles={"cube": SceneCollisionRole.DYNAMIC}, + collision_world_mode=SceneCollisionWorldMode.SHARED, + ) + + assert registry.dynamic_collision_entity_ids == ("cube",) + assert registry.collision_geometry_by_id() == { + "cube": simulation.rigid_objects["sim_cube"] + } + + +def test_from_simulation_allows_geometry_provider_override() -> None: + registry = SceneRegistry.from_simulation( + _Simulation(), # type: ignore[arg-type] + rigid_objects={"cube": "sim_cube"}, + collision_roles={"cube": SceneCollisionRole.STATIC}, + geometry_providers={"cube": _GeometryProvider()}, + ) + + assert registry.collision_geometry_by_id() == {"cube": {"kind": "box"}} + + +def test_from_simulation_requires_selected_uid_to_exist() -> None: + with pytest.raises(KeyError, match="missing"): + SceneRegistry.from_simulation( + _Simulation(), # type: ignore[arg-type] + articulations={"drawer": "missing"}, + ) diff --git a/tests/sim/skills/test_scene_curobo_integration.py b/tests/sim/skills/test_scene_curobo_integration.py new file mode 100644 index 000000000..13facf1f3 --- /dev/null +++ b/tests/sim/skills/test_scene_curobo_integration.py @@ -0,0 +1,115 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Cross-layer CPU tests for registry-backed cuRobo obstacle identity.""" + +from __future__ import annotations + +import torch + +from embodichain.lab.sim.planners import ( + CuroboPlanOptions, + CuroboPlanner, + CuroboPlannerCfg, + CuroboWorldCfg, + MotionGenerator, +) +from embodichain.lab.sim.skills import ( + SceneCollisionRole, + SceneCollisionWorldMode, + SceneRegistry, +) + + +class _RigidObject: + """Minimal live rigid-object geometry and pose surface.""" + + def __init__(self) -> None: + self.uid = "legacy_cube" + self.pose = torch.eye(4).repeat(2, 1, 1) + + def get_local_pose(self, to_matrix: bool = False) -> torch.Tensor: + assert to_matrix is True + return self.pose + + def get_vertices( + self, + env_ids: list[int], + *, + scale: bool, + ) -> list[torch.Tensor]: + assert env_ids == [0] + assert scale is True + return [torch.zeros(8, 3)] + + def get_triangles(self, env_ids: list[int]) -> list[torch.Tensor]: + assert env_ids == [0] + return [torch.zeros(12, 3, dtype=torch.long)] + + +class _Simulation: + """Resolve one rigid object through its simulation-native UID.""" + + def __init__(self, rigid_object: _RigidObject) -> None: + self.rigid_object = rigid_object + + def get_rigid_object(self, uid: str) -> _RigidObject | None: + return self.rigid_object if uid == self.rigid_object.uid else None + + +def test_registry_id_remains_authoritative_through_curobo_binding() -> None: + rigid_object = _RigidObject() + registry = SceneRegistry.from_simulation( + _Simulation(rigid_object), # type: ignore[arg-type] + rigid_objects={"cube": rigid_object.uid}, + collision_roles={"cube": SceneCollisionRole.DYNAMIC}, + collision_world_mode=SceneCollisionWorldMode.PER_ENV, + ) + mode = registry.resolve_collision_world_mode(batch_size=2) + geometry = registry.collision_geometry_by_id() + world_cfg = CuroboWorldCfg( + rigid_objects=geometry, # type: ignore[arg-type] + obstacle_representation="cuboid", + dynamic_obstacle_names=list(registry.dynamic_collision_entity_ids), + multi_env=mode is SceneCollisionWorldMode.PER_ENV, + ) + planner = object.__new__(CuroboPlanner) + planner.cfg = CuroboPlannerCfg(robot_uid="unused", world=world_cfg) + motion_generator = object.__new__(MotionGenerator) + motion_generator.planner = planner + provider = registry.make_planning_scene_provider( + motion_generator, + batch_size=2, + ) + + snapshot = provider.snapshot( + timestamp=0.0, + env_ids=torch.tensor([0, 1], dtype=torch.long), + ) + obstacle_poses = snapshot.collision_obstacle_poses( + batch_size=2, + device=torch.device("cpu"), + dtype=torch.float32, + ) + bound = motion_generator.bind_collision_world( + CuroboPlanOptions(), + obstacle_poses=obstacle_poses, + ) + + assert set(world_cfg.rigid_objects or {}) == {"cube"} + assert set(obstacle_poses) == {"cube"} + assert set(bound.dynamic_obstacle_poses or {}) == {"cube"} + assert rigid_object.uid not in obstacle_poses 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."] diff --git a/tests/test_agent_context_map.py b/tests/test_agent_context_map.py new file mode 100644 index 000000000..6f9a45a57 --- /dev/null +++ b/tests/test_agent_context_map.py @@ -0,0 +1,181 @@ +# ---------------------------------------------------------------------------- +# 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 + +from pathlib import Path + +import yaml + +_REPOSITORY_ROOT = Path(__file__).resolve().parents[1] +_AGENT_CONTEXT_ROOT = _REPOSITORY_ROOT / "agent_context" +_MAP_PATH = _AGENT_CONTEXT_ROOT / "MAP.yaml" +_REQUIRED_TOPIC_FIELDS = { + "id", + "title", + "aliases", + "keywords", + "paths", + "source_of_truth", + "related_topics", + "status", +} + + +def _load_context_map() -> dict: + with _MAP_PATH.open(encoding="utf-8") as stream: + return yaml.safe_load(stream) + + +def _topics_by_id() -> dict[str, dict]: + return {topic["id"]: topic for topic in _load_context_map()["topics"]} + + +def test_topic_ids_are_unique() -> None: + topics = _load_context_map()["topics"] + topic_ids = [topic["id"] for topic in topics] + + assert len(topic_ids) == len(set(topic_ids)) + + +def test_topic_entries_use_the_required_schema() -> None: + errors: list[str] = [] + for topic in _load_context_map()["topics"]: + missing = _REQUIRED_TOPIC_FIELDS - set(topic) + if missing: + errors.append( + f"{topic.get('id', '')}: missing {sorted(missing)}" + ) + if topic.get("status") not in {"active", "deprecated"}: + errors.append( + f"{topic.get('id', '')}: invalid status " + f"{topic.get('status')!r}" + ) + + assert errors == [] + + +def test_registered_context_and_source_paths_exist() -> None: + context_map = _load_context_map() + missing_paths: list[str] = [] + + for context_path in context_map["defaults"]["contexts"]: + resolved = _AGENT_CONTEXT_ROOT / context_path + if not resolved.exists(): + missing_paths.append(str(resolved.relative_to(_REPOSITORY_ROOT))) + + for topic in context_map["topics"]: + for context_path in topic["paths"]: + resolved = _AGENT_CONTEXT_ROOT / context_path + if not resolved.exists(): + missing_paths.append(str(resolved.relative_to(_REPOSITORY_ROOT))) + for source_path in topic["source_of_truth"]: + resolved = _REPOSITORY_ROOT / source_path + if not resolved.exists(): + missing_paths.append(str(resolved.relative_to(_REPOSITORY_ROOT))) + + assert missing_paths == [] + + +def test_related_topics_reference_registered_ids() -> None: + topics = _topics_by_id() + invalid_relations = [ + f"{topic_id} -> {related_id}" + for topic_id, topic in topics.items() + for related_id in topic["related_topics"] + if related_id not in topics + ] + + assert invalid_relations == [] + + +def test_simulation_and_rl_topics_cover_their_primary_entry_points() -> None: + topics = _topics_by_id() + + assert { + "embodichain/lab/sim/__init__.py", + "embodichain/lab/sim/sim_manager.py", + "embodichain/lab/gym/envs/base_env.py", + } <= set(topics["simulation-system"]["source_of_truth"]) + assert { + "embodichain/__main__.py", + "embodichain/learning/rl/train.py", + "embodichain/learning/rl/utils/trainer.py", + "embodichain_tasks/configs/agents/rl/", + } <= set(topics["rl-learning"]["source_of_truth"]) + + +def test_simulation_and_rl_topics_have_operational_sections() -> None: + topics = _topics_by_id() + required_sections = { + "## Entry Points", + "## Invariants", + "## Common Failure Modes", + } + missing_sections: list[str] = [] + + for topic_id in ("simulation-system", "rl-learning"): + context_path = _AGENT_CONTEXT_ROOT / topics[topic_id]["paths"][0] + content = context_path.read_text(encoding="utf-8") + for section in required_sections: + if section not in content: + missing_sections.append(f"{topic_id}: {section}") + + assert missing_sections == [] + + +def test_representative_navigation_terms_have_one_owner() -> None: + expected_owners = { + "simulation manager": "simulation-system", + "SimulationManager": "simulation-system", + "viser": "sim-visualization", + "rl config": "rl-learning", + "train-rl": "rl-learning", + "ik solver": "ik-solvers", + } + topics = _load_context_map()["topics"] + actual_owners: dict[str, set[str]] = {} + + for term in expected_owners: + normalized_term = term.casefold() + actual_owners[term] = { + topic["id"] + for topic in topics + if normalized_term + in { + candidate.casefold() + for candidate in [*topic["aliases"], *topic["keywords"]] + } + } + + assert actual_owners == { + term: {topic_id} for term, topic_id in expected_owners.items() + } + + +def test_project_context_adapters_reference_the_canonical_skill() -> None: + canonical_path = ".agents/skills/project-dev-context/SKILL.md" + adapter_paths = ( + _REPOSITORY_ROOT / ".claude/skills/project-dev-context/SKILL.md", + _REPOSITORY_ROOT / ".github/copilot/project-dev-context.md", + ) + missing_references = [ + str(path.relative_to(_REPOSITORY_ROOT)) + for path in adapter_paths + if canonical_path not in path.read_text(encoding="utf-8") + ] + + assert missing_references == [] diff --git a/tests/test_expert_program_package_data.py b/tests/test_expert_program_package_data.py new file mode 100644 index 000000000..90c747645 --- /dev/null +++ b/tests/test_expert_program_package_data.py @@ -0,0 +1,198 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Focused setuptools coverage for packaged Expert Program resources.""" + +from __future__ import annotations + +import ast +import json +import os +from pathlib import Path +import shutil +import subprocess +import sys +from typing import NamedTuple + +import pytest +from setuptools import Distribution +from setuptools.command.build_py import build_py + +from setup import get_package_dir + +_REPOSITORY_ROOT = Path(__file__).resolve().parents[1] +_SETUP_PATH = _REPOSITORY_ROOT / "setup.py" +_CONFIG_PACKAGE = "embodichain_tasks.configs" +_CONFIG_SOURCE = _REPOSITORY_ROOT / "embodichain_tasks" / "configs" +_PROGRAMS = { + Path("expert_program/multi_segments/repeated_cube_pick_place.yaml"): ( + "repeated_cube_pick_place" + ), + Path("expert_program/tableware/open_drawer.json"): "open_drawer", +} + + +class _StagedConfigPackage(NamedTuple): + """Isolated setuptools output and the setup options that produced it.""" + + build_lib: Path + relative_outputs: frozenset[Path] + package_data: dict[str, list[str]] + include_package_data: bool + + +def _literal_setup_keyword(keyword_name: str) -> object: + """Read one literal keyword from the repository's setup() call.""" + tree = ast.parse(_SETUP_PATH.read_text(encoding="utf-8"), filename=str(_SETUP_PATH)) + setup_calls = tuple( + node + for node in ast.walk(tree) + if isinstance(node, ast.Call) + and isinstance(node.func, ast.Name) + and node.func.id == "setup" + ) + if len(setup_calls) != 1: + raise AssertionError("setup.py must contain exactly one setup() call.") + keywords = { + keyword.arg: keyword.value + for keyword in setup_calls[0].keywords + if keyword.arg is not None + } + if keyword_name not in keywords: + raise AssertionError(f"setup.py does not declare {keyword_name!r}.") + return ast.literal_eval(keywords[keyword_name]) + + +@pytest.fixture +def staged_config_package(tmp_path: Path) -> _StagedConfigPackage: + """Stage only the two official programs through the real build_py command.""" + package_data = _literal_setup_keyword("package_data") + include_package_data = _literal_setup_keyword("include_package_data") + assert type(package_data) is dict + assert type(include_package_data) is bool + + isolated_source = tmp_path / "source" / "embodichain_tasks" / "configs" + isolated_source.mkdir(parents=True) + shutil.copyfile(_CONFIG_SOURCE / "__init__.py", isolated_source / "__init__.py") + for relative_path in _PROGRAMS: + destination = isolated_source / relative_path + destination.parent.mkdir(parents=True, exist_ok=True) + shutil.copyfile(_CONFIG_SOURCE / relative_path, destination) + + build_lib = tmp_path / "build_lib" + distribution = Distribution( + { + "packages": [_CONFIG_PACKAGE], + "package_dir": {_CONFIG_PACKAGE: str(isolated_source)}, + "package_data": package_data, + "include_package_data": include_package_data, + } + ) + distribution.script_name = str(_SETUP_PATH) + command = build_py(distribution) + command.build_lib = str(build_lib) + command.ensure_finalized() + + def reject_manifest_command(command_name: str) -> None: + raise AssertionError( + f"Focused package-data staging must not run {command_name!r}." + ) + + command.run_command = reject_manifest_command + relative_outputs = frozenset( + Path(output).resolve().relative_to(build_lib.resolve()) + for output in command.get_outputs(include_bytecode=False) + ) + command.run() + return _StagedConfigPackage( + build_lib=build_lib, + relative_outputs=relative_outputs, + package_data=package_data, + include_package_data=include_package_data, + ) + + +def test_setup_stages_both_official_expert_program_formats( + staged_config_package: _StagedConfigPackage, +) -> None: + """The actual setup patterns put nested JSON and YAML in wheel staging.""" + assert staged_config_package.include_package_data is False + assert get_package_dir()[_CONFIG_PACKAGE] == "embodichain_tasks/configs" + assert staged_config_package.package_data[_CONFIG_PACKAGE] == [ + "**/*.json", + "**/*.yaml", + "**/*.yml", + ] + expected_outputs = { + Path("embodichain_tasks") / "configs" / relative_path + for relative_path in _PROGRAMS + } + assert expected_outputs <= staged_config_package.relative_outputs + + +def test_staged_programs_decode_through_installed_config_paths( + staged_config_package: _StagedConfigPackage, + tmp_path: Path, +) -> None: + """A clean process resolves and decodes both files from wheel staging.""" + runtime_dir = tmp_path / "runtime" + runtime_dir.mkdir() + expected_ids = { + relative_path.as_posix(): program_id + for relative_path, program_id in _PROGRAMS.items() + } + script = """ +import json +from pathlib import Path +import sys + +import embodichain_tasks.configs as config_package +from embodichain.lab.gym.envs.expert_program import load_expert_program +from embodichain_tasks.configs import get_config_path + +build_lib = Path(sys.argv[1]).resolve() +expected = json.loads(sys.argv[2]) +module_path = Path(config_package.__file__).resolve() +assert module_path.is_relative_to(build_lib), (module_path, build_lib) +decoded = {} +for relative_path, expected_program_id in expected.items(): + resource_path = get_config_path(relative_path).resolve() + assert resource_path.is_relative_to(build_lib), (resource_path, build_lib) + program = load_expert_program(resource_path) + assert program.program_id == expected_program_id + decoded[relative_path] = program.program_id +print(json.dumps(decoded, sort_keys=True)) + """ + environment = os.environ.copy() + environment["PYTHONPATH"] = os.pathsep.join( + (str(staged_config_package.build_lib), str(_REPOSITORY_ROOT)) + ) + completed = subprocess.run( + [ + sys.executable, + "-c", + script, + str(staged_config_package.build_lib), + json.dumps(expected_ids, sort_keys=True), + ], + cwd=runtime_dir, + env=environment, + check=True, + capture_output=True, + text=True, + ) + + assert json.loads(completed.stdout.splitlines()[-1]) == expected_ids diff --git a/tests/toolkits/test_grasp_pose_generator.py b/tests/toolkits/test_grasp_pose_generator.py index cf7e0f349..83d304d84 100644 --- a/tests/toolkits/test_grasp_pose_generator.py +++ b/tests/toolkits/test_grasp_pose_generator.py @@ -153,7 +153,7 @@ def create_mug(sim: SimulationManager): def get_grasp_traj(sim: SimulationManager, robot: Robot, grasp_xpos: torch.Tensor): - n_envs = sim.num_envs + num_envs = sim.num_envs rest_arm_qpos = robot.get_qpos("arm") approach_xpos = grasp_xpos.clone() @@ -183,12 +183,12 @@ def get_grasp_traj(sim: SimulationManager, robot: Robot, grasp_xpos: torch.Tenso ) hand_trajectory = torch.cat( [ - hand_open_qpos[None, None, :].repeat(n_envs, 1, 1), - hand_open_qpos[None, None, :].repeat(n_envs, 1, 1), - hand_open_qpos[None, None, :].repeat(n_envs, 1, 1), - hand_close_qpos[None, None, :].repeat(n_envs, 1, 1), - hand_close_qpos[None, None, :].repeat(n_envs, 1, 1), - hand_close_qpos[None, None, :].repeat(n_envs, 1, 1), + hand_open_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_open_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_open_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_close_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_close_qpos[None, None, :].repeat(num_envs, 1, 1), + hand_close_qpos[None, None, :].repeat(num_envs, 1, 1), ], dim=1, ) diff --git a/tests/utils/test_config_paths.py b/tests/utils/test_config_paths.py new file mode 100644 index 000000000..b05c9fe04 --- /dev/null +++ b/tests/utils/test_config_paths.py @@ -0,0 +1,68 @@ +# ---------------------------------------------------------------------------- +# 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 stable configuration-path resolution.""" + +from __future__ import annotations + +from pathlib import Path + +import pytest + +from embodichain.utils import resolve_config_path as exported_resolve_config_path +from embodichain.utils.config_paths import resolve_config_path + + +def test_resolve_config_path_preserves_existing_path(tmp_path: Path) -> None: + config_path = tmp_path / "config.yaml" + config_path.write_text("id: Test-v0\n", encoding="utf-8") + + assert resolve_config_path(config_path) == config_path + + +def test_resolve_config_path_is_exported_from_utils_package() -> None: + assert exported_resolve_config_path is resolve_config_path + + +def test_resolve_config_path_preserves_ordinary_relative_path( + tmp_path: Path, + monkeypatch, +) -> None: + monkeypatch.chdir(tmp_path) + + assert resolve_config_path("local/config.yaml") == Path("local/config.yaml") + + +def test_resolve_config_path_redirects_packaged_task_config( + tmp_path: Path, + monkeypatch, +) -> None: + monkeypatch.chdir(tmp_path) + + resolved = resolve_config_path("embodichain_tasks/configs/gym/cobotmagic.json") + + assert resolved.is_file() + assert resolved.name == "cobotmagic.json" + + +def test_resolve_config_path_rejects_packaged_path_escape( + tmp_path: Path, + monkeypatch, +) -> None: + monkeypatch.chdir(tmp_path) + + with pytest.raises(ValueError, match="stay within the package"): + resolve_config_path("embodichain_tasks/configs/../VERSION") diff --git a/tests/utils/test_logger.py b/tests/utils/test_logger.py new file mode 100644 index 000000000..a5856237d --- /dev/null +++ b/tests/utils/test_logger.py @@ -0,0 +1,159 @@ +# ---------------------------------------------------------------------------- +# 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 logging +from collections.abc import Callable + +import pytest + +from embodichain.utils import logger as logger_module + +_RESET_COLOR = "\033[0m" +_LEVEL_CASES = ( + (logger_module.log_debug, "debug", "DEBUG", "\033[96m", False), + (logger_module.log_info, "info", "INFO", "\033[92m", False), + (logger_module.log_warning, "warning", "WARNING", "\033[93m", True), +) + + +def test_default_formatter_uses_utc_datetime_and_aligned_layout(): + record = logging.LogRecord( + name="embodichain.test", + level=logging.INFO, + pathname=__file__, + lineno=1, + msg="INFO │ EmbodiChain │ Simulation initialized", + args=(), + exc_info=None, + ) + record.created = 0.123 + record.msecs = 123.0 + + formatted = logger_module._DEFAULT_FORMATTER.format(record) + + assert formatted == ( + "1970-01-01 00:00:00.123 UTC " + "│ INFO │ EmbodiChain │ Simulation initialized" + ) + + +def test_default_formatter_can_omit_prefix(): + message = "╭─ Environment initialized\n╰─ Ready" + record = logging.LogRecord( + name="embodichain.test", + level=logging.INFO, + pathname=__file__, + lineno=1, + msg=message, + args=(), + exc_info=None, + ) + record.embodichain_plain = True + + assert logger_module._DEFAULT_FORMATTER.format(record) == message + + +@pytest.mark.parametrize( + ("log_function", "logger_method", "level", "color_code", "colors_message"), + _LEVEL_CASES, +) +def test_log_methods_use_default_level_colors( + monkeypatch: pytest.MonkeyPatch, + log_function: Callable[..., None], + logger_method: str, + level: str, + color_code: str, + colors_message: bool, +): + messages: list[str] = [] + monkeypatch.setattr(logger_module.logger, logger_method, messages.append) + + log_function("Test message") + + message = ( + f"{color_code}Test message{_RESET_COLOR}" if colors_message else "Test message" + ) + assert messages == [ + f"{color_code}{level:<7}{_RESET_COLOR} │ EmbodiChain │ {message}" + ] + + +@pytest.mark.parametrize( + ("log_function", "logger_method", "level", "colors_message"), + tuple((*case[:3], case[4]) for case in _LEVEL_CASES), +) +def test_log_methods_allow_custom_level_color( + monkeypatch: pytest.MonkeyPatch, + log_function: Callable[..., None], + logger_method: str, + level: str, + colors_message: bool, +): + messages: list[str] = [] + monkeypatch.setattr(logger_module.logger, logger_method, messages.append) + + log_function("Test message", color="purple") + + message = ( + f"\033[95mTest message{_RESET_COLOR}" if colors_message else "Test message" + ) + assert messages == [f"\033[95m{level:<7}{_RESET_COLOR} │ EmbodiChain │ {message}"] + + +def test_log_color_can_be_disabled(monkeypatch: pytest.MonkeyPatch): + messages: list[str] = [] + monkeypatch.setattr(logger_module.logger, "info", messages.append) + + logger_module.log_info("Test message", color=None) + + assert messages == ["INFO │ EmbodiChain │ Test message"] + + +def test_log_info_can_omit_prefix(monkeypatch: pytest.MonkeyPatch): + calls: list[tuple[object, dict[str, object]]] = [] + + def capture(message: object, **kwargs: object) -> None: + calls.append((message, kwargs)) + + monkeypatch.setattr(logger_module.logger, "info", capture) + + logger_module.log_info("Environment initialized", prefix=False) + + assert calls == [ + ("Environment initialized", {"extra": {"embodichain_plain": True}}) + ] + + +def test_log_error_uses_default_color_and_preserves_error_type(): + with pytest.raises(ValueError) as error: + logger_module.log_error("Test message", ValueError) + + assert str(error.value) == ( + f"\033[91mERROR {_RESET_COLOR} │ EmbodiChain │ " + f"\033[91mTest message{_RESET_COLOR}" + ) + + +def test_log_error_allows_custom_level_color(): + with pytest.raises(RuntimeError) as error: + logger_module.log_error("Test message", color="purple") + + assert str(error.value) == ( + f"\033[95mERROR {_RESET_COLOR} │ EmbodiChain │ " + f"\033[95mTest message{_RESET_COLOR}" + ) diff --git a/tests/visualization/test_scene_exporter.py b/tests/visualization/test_scene_exporter.py index 8e295fffb..ef94f03d5 100644 --- a/tests/visualization/test_scene_exporter.py +++ b/tests/visualization/test_scene_exporter.py @@ -21,6 +21,7 @@ import numpy as np from embodichain.lab.visualization import ( + FrameOverlay, JointControlSpec, JointControlState, PointCloudOverlay, @@ -342,6 +343,30 @@ def get_gizmo_items(self) -> tuple[tuple[str, _Gizmo], ...]: return (("cube", _Gizmo()),) +class _AxisMarkerHandle: + def __init__(self, pose: np.ndarray, visible: bool = False) -> None: + self._pose = pose + self._visible = visible + + def get_world_pose(self) -> np.ndarray: + return self._pose + + def is_visible(self) -> bool: + return self._visible + + +class _AxisMarkerSimulation(_EmptySimulation): + def __init__(self) -> None: + pose = np.eye(4, dtype=np.float32) + pose[:3, 3] = [-0.4, 0.48, 0.1] + self.marker = _AxisMarkerHandle(pose) + + def get_axis_marker_items( + self, + ) -> tuple[tuple[str, tuple[_AxisMarkerHandle, ...], float, float], ...]: + return (("place_target_axis", (self.marker,), 0.2, 0.01),) + + class _Camera: def __init__(self, visualization_role: str = "sensor") -> None: self.cfg = SimpleNamespace( @@ -567,6 +592,54 @@ def test_gizmo_manifest_and_authoritative_pose_are_exported() -> None: np.testing.assert_allclose(result.frame.gizmos[0].position, [0.2, 0.3, 0.4]) +def test_axis_markers_ignore_headless_native_visibility() -> None: + exporter = SceneExporter( + _AxisMarkerSimulation(), + VisualizationCfg(backend="viser"), + run_id="axis-marker-run", + ) + + exporter.build_manifest() + result = exporter.capture(sim_step=1, sim_time=0.01) + + assert len(result.frame.overlays.frames) == 1 + overlay = result.frame.overlays.frames[0] + assert overlay.overlay_id == "marker:place_target_axis:0" + np.testing.assert_allclose(overlay.position, [-0.4, 0.48, 0.1]) + np.testing.assert_allclose(overlay.wxyz, [1.0, 0.0, 0.0, 0.0]) + assert overlay.axes_length == 0.2 + assert overlay.axes_radius == 0.01 + assert overlay.visible + + +def test_axis_marker_id_does_not_collide_with_caller_frame() -> None: + exporter = SceneExporter( + _AxisMarkerSimulation(), + VisualizationCfg(backend="viser"), + run_id="axis-marker-collision-run", + ) + caller_frame = FrameOverlay( + overlay_id="marker:place_target_axis:0", + position=np.array([0.1, 0.2, 0.3], dtype=np.float32), + wxyz=np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32), + ) + + exporter.build_manifest() + result = exporter.capture( + sim_step=1, + sim_time=0.01, + overlays=SceneOverlays(frames=(caller_frame,)), + ) + + frames = result.frame.overlays.frames + assert [frame.overlay_id for frame in frames] == [ + "marker:place_target_axis:0#1", + "marker:place_target_axis:0", + ] + np.testing.assert_allclose(frames[0].position, [-0.4, 0.48, 0.1]) + np.testing.assert_allclose(frames[1].position, caller_frame.position) + + def test_camera_frustum_pose_and_low_frequency_rgb_are_exported() -> None: simulation = _CameraSimulation() exporter = SceneExporter( diff --git a/tests/visualization/test_viser_backend.py b/tests/visualization/test_viser_backend.py index b8af652f3..aac24485c 100644 --- a/tests/visualization/test_viser_backend.py +++ b/tests/visualization/test_viser_backend.py @@ -25,6 +25,7 @@ CameraImageFrame, CameraSpec, DynamicMeshUpdate, + FrameOverlay, GizmoSpec, GizmoState, JointControlSpec, @@ -33,6 +34,7 @@ SceneFrame, SceneManifest, SceneNode, + SceneOverlays, ViserServerCfg, ) from embodichain.lab.visualization.backends.viser import ViserBackend @@ -222,6 +224,7 @@ def __init__(self) -> None: self.mesh_handles: list[_Handle] = [] self.dynamic_mesh_handles: list[_Handle] = [] self.camera_handles: list[_Handle] = [] + self.frame_handles: list[_Handle] = [] self.grid_handles: list[_Handle] = [] self.transform_controls: list[_TransformControls] = [] @@ -233,7 +236,9 @@ def set_up_direction(self, direction: str) -> None: def add_frame(self, name: str, **kwargs: object) -> _Handle: kwargs.setdefault("visible", True) - return _Handle(name=name, **kwargs) + handle = _Handle(name=name, removed=False, **kwargs) + self.frame_handles.append(handle) + return handle def add_batched_meshes_simple(self, name: str, **kwargs: object) -> _Handle: self.mesh_uploads += 1 @@ -321,6 +326,47 @@ def test_viser_backend_adds_one_meter_default_ground_grid() -> None: backend.stop() +def test_viser_backend_renders_axis_marker_frame_overlay() -> None: + server = _Server() + backend = ViserBackend(ViserServerCfg(port=8765), server_factory=lambda **_: server) + marker = FrameOverlay( + overlay_id="marker:place_target_axis:0", + position=np.array([-0.4, 0.48, 0.1], dtype=np.float32), + wxyz=np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32), + axes_length=0.2, + axes_radius=0.01, + ) + frame = SceneFrame( + run_id="run", + scene_revision=1, + sequence=1, + sim_step=1, + sim_time=0.01, + node_ids=(), + positions=np.empty((0, 3), dtype=np.float32), + wxyz=np.empty((0, 4), dtype=np.float32), + visible=np.empty((0,), dtype=np.bool_), + overlays=SceneOverlays(frames=(marker,)), + ) + + backend.start() + backend.publish_manifest(SceneManifest("run", 1, (), ())) + assert backend.publish_frame(frame) + + handle = next( + handle + for handle in server.scene.frame_handles + if handle.name.startswith("/overlays/frames/") + ) + assert handle.name == "/overlays/frames/marker%3Aplace_target_axis%3A0" + assert handle.axes_length == 0.2 + assert handle.axes_radius == 0.01 + np.testing.assert_allclose(handle.position, marker.position) + np.testing.assert_allclose(handle.wxyz, marker.wxyz) + assert handle.visible + backend.stop() + + def test_viser_backend_uploads_static_mesh_once_and_updates_only_poses() -> None: server = _Server() backend = ViserBackend(ViserServerCfg(port=8765), server_factory=lambda **_: server)