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/agent_context/MAP.yaml b/agent_context/MAP.yaml index 60a5145e0..d8648c4b6 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -280,6 +280,8 @@ topics: - toppra - motion generator - trajectory planning + - curobo collision world + - dynamic collision integration - 运动生成 - 运动规划 - 轨迹规划 @@ -296,6 +298,16 @@ topics: - waypoint - velocity - acceleration + - canonical obstacle ID + - logical collision source ID + - physical YAML obstacle name + - sphere derived obstacle names + - empty collision mesh + - 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 +316,11 @@ 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/skills/scene.py related_topics: - robot-system - ik-solvers + - atomic-actions status: active - id: rl-learning @@ -427,6 +441,15 @@ 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 + - capability binding - AtomicAction - ActionInvocation - AtomicActionEngine @@ -451,11 +474,77 @@ 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 - ActionBinding + - EndpointBinding + - RuntimeEndpointTarget + - JointPositionTarget + - SkillBindingContract + - SkillResourceSlot + - SkillEndpointRequirement + - DisjointSlotEndpoints + - DisjointResourceSlots + - RobotSkillProfile + - BoundRobotSkillProfile + - RobotResource + - ResourceEndpoint + - ResourceEndpointAdapter + - ControlPartEndpoint + - ControlPartEndpointAdapter + - EndpointResolution + - ResolvedResourceEndpoint + - ResourceBinding + - ResourceClaim + - ResolvedRobotResource + - ResolvedSkillBinding + - SkillPolicyPreset + - 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 - dynamic goal - error recovery @@ -464,6 +553,20 @@ 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 @@ -485,11 +588,16 @@ topics: 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,6 +608,9 @@ 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/profiles.py + - embodichain/lab/sim/skills/__init__.py related_topics: - motion-planning - robot-system diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 112d2596b..4c56e299c 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -14,40 +14,48 @@ 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; +- 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` planner/timing choices; - 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. +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. +`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 @@ -71,6 +79,266 @@ 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. +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. + +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; 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: there is no resource lease manager, parallel scheduler, +or concurrency guarantee yet. Dynamic execution can dispatch multiple +endpoint commands in one synchronized frame, but that 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. + +`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`. | +| `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` | A `SceneEntityPose` in `xpos`. | +| `CoordinatedPlacement` | `SceneEntityPose` values in the placing or support object target pose. | +| `HandOver` | No semantic-object scene dependency. It verifies stable attachment identity and derives current pose from held state; its middle/final option poses are tensors, and 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. + ## Static compilation Built-ins are already registered by their class-level stable `skill_id`; call: @@ -81,12 +349,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 @@ -105,13 +382,22 @@ result = runner.step(effect_success=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: - -- joint tracking error against the previous command; +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 in joint-position mode; - translation/rotation drift of referenced scene entities; - per-environment collision-world revision changes for collision-sensitive actions; @@ -135,52 +421,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. @@ -196,6 +510,14 @@ 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_entity_ids`, `dynamic_collision_entity_ids`, and +`collision_world_batch_mode` expose the backend's complete world, dynamic +subset, and batching contract. `MotionGenerator` validates and forwards those +properties 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 `dynamic_obstacle_recovery.py`. Each injects one disturbance, reports the @@ -224,27 +546,34 @@ offsets, and grasp selection behavior. An action constructor may accept There is no `ActionCfg` or built-in `*Cfg` layer. `engine.register(action)` is reserved for custom skill implementations. A -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. +built-in can be replaced only with explicit `replace=True`. Registration puts +the implementation in `engine.actions`; it does not prove that the current +embodiment supports it. Semantic exposure additionally requires a concrete +class-local `binding_contract` for `engine.skills` and a valid profile assignment +for `engine.skill_profile.skills`. 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. `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. - -Embodiment-specific joint commands do not belong to Action options. Register -them once by actual control-part name: +`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 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 +588,78 @@ 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` | +| `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` | `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`. 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 full-robot positions or a + `TimedTrajectory` through `build_plan()`: build batched `list[PlanState]`, + translate the policy with `request.motion_policy.to_motion_gen_options()`, + 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/motion-planning/motion-planning.md b/agent_context/topics/motion-planning/motion-planning.md index 4f5644cc2..b5edd4a8b 100644 --- a/agent_context/topics/motion-planning/motion-planning.md +++ b/agent_context/topics/motion-planning/motion-planning.md @@ -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: @@ -126,6 +139,32 @@ them into `CuroboPlanOptions.dynamic_obstacle_poses`. 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. +`BasePlanner.collision_world_entity_ids`, `dynamic_collision_entity_ids`, and +`collision_world_batch_mode` expose the complete canonical world, its dynamic +subset, and the `"shared"` / `"per_env"` mode. `MotionGenerator` validates and +forwards those properties to the integration layer. 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 @@ -261,7 +300,15 @@ The decorator checks that every `PlanState` in `target_states` shares the same l 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. +- **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. +- **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/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index 66c78b4e4..bc70bd30d 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -1,10 +1,13 @@ # Declarative Expert Programs and Unified Semantic Skill Runtime -- Status: design plan -- Baseline: `main@26b69c22d7efbf96cb35f5487f6922c8645f91d7` -- Last updated: 2026-08-10 +- Status: implementation in progress; Phase 0 and PR1 complete, and PR2A, + PR2B, and PR2C implemented on stacked feature branches +- Baseline: `main@bcccb787e8f9165e9c8acf6f39f165ba6ac752a4` +- 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) ## 1. Executive summary @@ -63,7 +66,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 +91,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@e445133c` after PR #475. 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 +104,42 @@ 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. | +| 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 | Expert configuration does not expose a separate control period. | | Adaptive dynamic-object settling (#470) | Reset/event implementation exists | Extract a reusable monitor; demo post-policies must advance through `env.step()`. | | 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. + +The remaining #474 prerequisites on `main` are: -- `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. +- robot capability declarations, resource selection, semantic commands, and + stable policies do not yet have an embodiment-owned source of truth; +- dynamic-obstacle validation is planner-local; provider collision entity IDs + and planner-declared names are not yet fully cross-validated at integration + construction time; +- `MotionPolicy` still exposes implementation-level tuning that should be + hidden behind semantic presets for ordinary users. + +PR1 closes the snapshot-grounding and stable-identity bridge. PR2A closes the +first and third gaps for registry-backed integrations by introducing one +authoritative registration boundary, a registry-derived scene provider, and +construction-time collision-world validation. PR2B closes the robot-profile +gap on its stacked branch with generic resources, deterministic binding, +profile-owned commands, and named policy presets. The semantic facade remains +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 +155,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 +173,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 +235,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 `ActionGoal`, `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 +258,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 +299,216 @@ 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, 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, receiver?, 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. + `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 +521,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 +532,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 +553,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 @@ -462,13 +688,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 +706,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. @@ -495,29 +722,45 @@ normally, then resume with a fresh observation. `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. +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. -### 9.3 Named phases +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. -Plans and execution events need stable semantic phase names. Initial built-ins -should expose at least: +### 9.3 Named atomic trajectory segments -- 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`. +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: -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. +| 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 +784,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 +808,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 +823,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 +853,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 +869,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 + | (in progress) + +-----------+-----------+ + v Semantic calls/compiler --> SkillRuntime/effect monitors | | +---------------+--------------+ @@ -638,36 +901,205 @@ 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 are met on `main@e445133c`; this remains the foundation for the +completed PR1 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 `refactor/atomic-actions-phase0`: 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 (implemented on the feature branch) 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 (implemented on the feature branch) + +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 API can represent mobile-base and whole-body resources today. A +new endpoint kind still needs one shared adapter and a compatible shared atomic +skill before the current core can execute it; adding tasks that reuse that +capability then remains configuration-only. + +PR2B may proceed in parallel with PR2A after the PR1 bridge. Neither follow-up +requires official task migration; the repeated-cube vertical slice opts in only +after the registry, profile, 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. ### Phase 2: semantic facade and compiler @@ -690,9 +1122,12 @@ effect verifier. 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 @@ -702,16 +1137,17 @@ compiler/runtime code and produce equivalent results. 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 @@ -725,12 +1161,13 @@ Deliverables: 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 @@ -739,7 +1176,7 @@ 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. @@ -781,27 +1218,39 @@ 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; @@ -819,8 +1268,15 @@ The design is complete when all of the following hold: typed atomic-action core, and runtime. - [ ] 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. +- [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. +- [ ] 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. - [ ] 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 @@ -828,15 +1284,21 @@ The design is complete when all of the following hold: - [ ] `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] Repeated sub-threshold motion eventually publishes the correct scene revision. -- [ ] Custom actions have a documented and tested compatibility path. +- [x] Custom actions have a documented and tested intentional hard-break + migration from overriding `plan()` to implementing `_plan()`; no + compatibility adapter is required. +- [ ] Version 1 creates exactly one one-invocation `ExecutionSession` for each + semantic call and re-observes before lowering the next call. - [ ] Demonstration timing is derived from `BaseEnv.step_dt` and commands pass through `env.step()`. -- [ ] Phase hooks use stable names rather than trajectory indices. +- [ ] No program post-policy, effect, or tracing integration depends on + hard-coded waypoint 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 + observed program/demo segments with settle/effect/validation metadata. +- [ ] 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, sessions, and planners as escape hatches. @@ -854,7 +1316,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/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.rst index c2a3d4dde..b2b0236d9 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 @@ -9,8 +9,9 @@ embodichain.lab.sim.atomic_actions ActionGoal ActionBinding - ResolvedActionBinding - ResolvedControlPart + EndpointBinding + RuntimeEndpointTarget + JointPositionTarget ControlCommand JointPositionCommand ControlPartCommandProfile @@ -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,6 @@ embodichain.lab.sim.atomic_actions SimulationExecutionAdapter ExecutionTick EffectVerificationRequest - JointCommand ExecutionEvent ExecutionEventKind ExecutionStatus @@ -89,16 +108,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 +209,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 +255,12 @@ Engine and execution .. autoclass:: CommandSink :members: +.. autoclass:: EndpointCommandTransport + :members: + +.. autoclass:: EndpointCommandRouter + :members: + .. autoclass:: ExecutionClock :members: @@ -209,9 +291,6 @@ Engine and execution .. autoclass:: ExecutionTick :members: -.. autoclass:: JointCommand - :members: - .. autoclass:: ExecutionEvent :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..1b3022fe8 --- /dev/null +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.skills.rst @@ -0,0 +1,148 @@ +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 + +.. 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 + +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/overview/sim/atomic_actions/builtin_actions.md b/docs/source/overview/sim/atomic_actions/builtin_actions.md index 7e7aeef73..9783df20f 100644 --- a/docs/source/overview/sim/atomic_actions/builtin_actions.md +++ b/docs/source/overview/sim/atomic_actions/builtin_actions.md @@ -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` @@ -135,27 +136,27 @@ 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 | + +### 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 +165,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,23 +199,55 @@ 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` `ObjectSemantics.entity_id` grounding | implicit snapshot reference | yes; always consumed for the object pose | +| 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 | + +### 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 @@ -221,32 +256,35 @@ Use this rule when configuring a built-in or adding a new one: collision choice, and planner options; - `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 +308,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 +316,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,15 +338,15 @@ 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 and clear overlapping coordinated attachment state | | 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 @@ -318,7 +356,13 @@ dependency, so material target motion invalidates and replans an executing 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. + +`PickUp` requires typed `open` and `grasp` commands on `primary.grasp`. Important `PickUpOptions` fields: | Field | Purpose | @@ -330,11 +374,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 +389,24 @@ 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 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 +and trajectory sampling remain in `MotionPolicy`. **Example:** `scripts/tutorials/atomic_action/move_held_object.py` @@ -373,8 +422,8 @@ 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 | +| Binding contract | `primary.motion` plus disjoint `primary.grasp` | +| State | consumes the bound motion target's attachment when present | | Effect | detach the object and clear overlapping coordinated attachment state | | Verification | release must be verified during closed-loop execution | | Dynamic target | explicit pose/waypoints or `SceneEntityPose` | @@ -384,7 +433,7 @@ translation remain physically equivalent. The action selects the closer orientation variant from the observed starting state and uses it consistently across all waypoints. -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,26 +449,27 @@ 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 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. +The `AssembleAffordance` stores the relative assembly pose. A prior verified +`PickUp` must have populated the held object's `object_to_eef` transform. +`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. -**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)= @@ -433,16 +483,16 @@ arm should retreat along its planned path after reaching the target. |---|---| | Skill ID | `press` | | Goal | `PressGoal(xpos=...)` | -| Binding | manipulator + end effector role `primary` | +| Binding contract | `primary.motion` plus disjoint `primary.grasp` | | Motion | close, press, joint-space return | | Effect | none; existing attachment state is unchanged | | 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. +The bound `primary.grasp` endpoint must provide `grasp`, while +`PressOptions.hand_interp_steps` controls the close interpolation. Both +endpoints come from the generic `ActionBinding`. Contact detection is not +itself a symbolic effect in the current action; applications that require +force/contact confirmation should verify it externally. **Example:** `scripts/tutorials/atomic_action/press.py` @@ -457,8 +507,8 @@ 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)]` | | Verification | coordinated attachment must be externally verified | @@ -471,11 +521,15 @@ 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. +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 end-effector profiles must provide `open` and `grasp`. Important +Both bound grasp endpoints must provide `open` and `grasp`. Important `CoordinatedPickmentOptions` fields group into: - `pre_grasp_distance` and `lift_height`; @@ -483,8 +537,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,7 +556,7 @@ hold -> optionally release the placing hand -> retreat the placing arm**. |---|---| | Skill ID | `coordinated_placement` | | Goal | `CoordinatedPlacementGoal` | -| Binding | manipulator + end effector roles `placing` and `support` | +| Binding contract | disjoint `placing` and `support` slots, each with disjoint `motion` and `grasp` endpoints | | Precondition | separate `HeldObjectState` entries exist for both bound arms | | 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 | @@ -510,15 +565,15 @@ Both object targets may use `SceneEntityPose`, so either can participate in dynamic-goal invalidation. Goal-level height/release values override `CoordinatedPlacementOptions` for that invocation. -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,24 +589,26 @@ retreats -> destination delivers**. |---|---| | Skill ID | `hand_over` | | Goal | `GraspGoal(semantics=...)` | -| Binding | manipulator + end effector roles `source` and `destination` | +| Binding contract | disjoint `source` and `destination` slots, each with disjoint `motion` and `grasp` endpoints | | Precondition | source arm has a verified `HeldObjectState`; semantic object supports 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. +source/destination motion and grasp endpoints come exclusively from the +corresponding generic `ActionBinding` slots. 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. +`HandOverOptions`. 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. As with the other coordinated primitive, cuRobo does not currently support its dual-arm `strategy="motion_gen"` path. diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index 6b38c9164..0ae3d06b3 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -6,22 +6,25 @@ :hidden: builtin_actions +robot_skill_profiles ``` ```{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 +36,8 @@ and whole-body control are not implemented by this module yet. +---------------+----------------+ +---------------+----------------+ | | v | - agent adapter: schema validation, | - scene grounding, capability binding | + semantic adapter: schema validation, | + SceneRegistry grounding, endpoint binding | | | +------------------+------------------+ | @@ -59,14 +62,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,22 +81,24 @@ 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 | +| 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 verification | Application observer | Verifies grasp, release, handover, and 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 @@ -100,10 +108,14 @@ semantic skill call that an adapter validates, grounds, and converts into an 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"}), + binding=binding, motion_policy=MotionPolicy(sample_count=80, control_dt=1.0 / 60.0), recovery_policy=RecoveryPolicy(max_replans=2), ) @@ -115,10 +127,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 +145,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 +169,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 @@ -166,15 +183,17 @@ 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 | +| `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 | +| `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, timing, limits, dynamic-collision mode, typed planner options | 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 | +| `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. @@ -183,65 +202,89 @@ 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 +### 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: -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 +302,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 +316,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 +328,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. @@ -317,11 +363,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: @@ -366,13 +418,18 @@ 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 | +| `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.step(effect_success=...)` | Non-blocking controller integration | Observes and routes a `RuntimeCommandFrame` only when it is due | | `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,23 +442,31 @@ 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"}), + binding=binding, motion_policy=MotionPolicy(sample_count=80, control_dt=1.0 / 60.0), ) 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. @@ -418,14 +483,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,7 +497,10 @@ from embodichain.lab.sim.atomic_actions import ( ) engine = AtomicActionEngine(motion_generator) -binding = ActionBinding(manipulators={"primary": "left_arm"}) +binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "left_arm"}}, +) motion_policy = MotionPolicy(sample_count=80, control_dt=1.0 / 60.0) approach = ActionInvocation( @@ -487,7 +554,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 +573,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 +598,32 @@ 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. + +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 +651,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 +698,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 +720,30 @@ 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. Dynamic collision invalidation is provider-driven. Only registered, pose-updatable collision entities are supported; adding/removing obstacles or @@ -636,7 +752,7 @@ 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. @@ -651,7 +767,7 @@ if tick.pending_effect is not None: tick = session.tick(latest_context, effect_success=effect_success) ``` -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. @@ -667,7 +783,7 @@ and embodiment capabilities, then produce the typed invocation: MLLM SkillCallSpec -> schema validation -> object / scene grounding - -> capability and role binding + -> participant and endpoint capability binding -> safe skill-option selection -> semantic command selection (never raw qpos) -> ActionInvocation @@ -691,14 +807,17 @@ A new primitive should: 1. define a frozen, action-owned goal dataclass with a stable `goal_kind`; 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 +826,7 @@ 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}`../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..f03d453e2 --- /dev/null +++ b/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md @@ -0,0 +1,287 @@ +(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, + MotionPolicy, +) +from embodichain.lab.sim.skills import ( + ControlPartEndpoint, + 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", + motion_policy=MotionPolicy(strategy="ik_interp"), + ), + }, + default_preset="default", +) +``` + +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. + +## 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. + +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 and +future scheduling work. They do not enable parallel action execution. The +runtime does not merge concurrent endpoint-command streams. Joint-backed plans +may retain a full-robot trajectory for feedback and offline compilation, but +runtime dispatch is 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..20d25c7a5 100644 --- a/docs/source/overview/sim/index.rst +++ b/docs/source/overview/sim/index.rst @@ -43,6 +43,10 @@ The simulation stack can be read from the bottom up: |-- planners | |-- joint-space and Cartesian trajectory generation | `-- time parameterization and sampling utilities + |-- 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 @@ -84,6 +88,16 @@ 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 @@ -120,6 +134,11 @@ 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 ` when building scripted manipulation from reusable motion primitives. @@ -145,4 +164,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..72a512290 100644 --- a/docs/source/overview/sim/planners/curobo_planner.md +++ b/docs/source/overview/sim/planners/curobo_planner.md @@ -59,6 +59,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 +69,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 +152,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 +234,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 +263,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 diff --git a/docs/source/overview/sim/scene_registry.md b/docs/source/overview/sim/scene_registry.md new file mode 100644 index 000000000..74f693c15 --- /dev/null +++ b/docs/source/overview/sim/scene_registry.md @@ -0,0 +1,244 @@ +(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. + +## 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/index` for snapshot grounding and recovery semantics, +and {doc}`planners/curobo_planner` for cuRobo world representation and frame +details. diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index f9d57c009..6f7de6cb2 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; @@ -65,7 +81,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,10 +89,11 @@ 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 @@ -85,6 +102,11 @@ the protected ``_plan()`` hook instead. Similarly, ``engine.plan_action()`` is reserved for extensions and isolated tests that need to plan an unregistered instance. +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 ----------------- @@ -146,9 +168,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 +183,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"}), + binding=binding, motion_policy=MotionPolicy(sample_count=80, control_dt=1.0 / 60.0), ) 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 +212,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,7 +237,10 @@ planning: ) engine = AtomicActionEngine(motion_generator) - binding = ActionBinding(manipulators={"primary": "left_arm"}) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "left_arm"}}, + ) motion_policy = MotionPolicy(sample_count=80, control_dt=1.0 / 60.0) approach = ActionInvocation( @@ -236,6 +269,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 +290,6 @@ must be resolved from the latest scene snapshot: from embodichain.lab.sim.atomic_actions import ( EndEffectorPoseGoal, RecoveryPolicy, - RigidObjectSceneProvider, SceneEntityPose, ) @@ -262,7 +298,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 +310,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, @@ -285,7 +333,8 @@ 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) + 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 +342,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. @@ -319,13 +373,24 @@ for comparison: 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,16 +420,31 @@ 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. Task-state effects ------------------ @@ -397,7 +477,8 @@ Define an action-owned frozen goal dataclass with a stable ``goal_kind``. 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 @@ -410,6 +491,21 @@ 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, + ) + @dataclass(frozen=True, slots=True) class PushGoal: goal_kind: ClassVar[str] = "push" @@ -423,7 +519,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,8 +545,11 @@ 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. + # The joint helper lowers the result into RuntimeCommandFrame values + # and retains the trajectory for joint-position feedback. return self.build_plan( request, context, @@ -444,6 +557,13 @@ A minimal implementation looks like: trajectory=full_robot_positions, ) +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/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index 86e00f80e..607470da6 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -34,7 +34,12 @@ AssembleAffordance, InteractionPoints, ) -from .bindings import ActionBinding, ResolvedActionBinding, ResolvedControlPart +from .bindings import ( + ActionBinding, + EndpointBinding, + JointPositionTarget, + RuntimeEndpointTarget, +) from .control import ( ActionControlOverrides, ControlCommand, @@ -58,19 +63,40 @@ ExecutionSession, ExecutionStatus, ExecutionTick, - JointCommand, ) from .goals import ActionGoal, ObjectActionGoal, PoseGoalValue, SceneEntityPose from .invocation import ActionInvocation, ActionOptions, ResolvedActionRequest from .plans import ( ActionPlan, CompiledTrajectory, + ExecutionFeedbackMode, 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 .primitives import ( AssembleGoal, BUILTIN_ACTION_TYPES, @@ -149,6 +175,8 @@ "AtomicAction", "AtomicActionEngine", "BUILTIN_ACTION_TYPES", + "BATCH_INVERSE_KINEMATICS_CAPABILITY", + "CARTESIAN_POSE_CAPABILITY", "CompiledTrajectory", "CommandAcknowledgement", "CommandAckStatus", @@ -165,11 +193,18 @@ "CoordinatedPlacementGoal", "CoordinatedPlacementOptions", "DynamicCollisionMode", + "DisjointResourceSlots", + "DisjointSlotEndpoints", "EndEffectorPoseGoal", + "EndpointBinding", + "EndpointCommand", + "EndpointCommandRouter", + "EndpointCommandTransport", "EntityState", "EffectVerificationRequest", "EffectVerifier", "ExecutionClock", + "ExecutionFeedbackMode", "ExecutionEvent", "ExecutionEventKind", "ExecutionRunner", @@ -178,15 +213,20 @@ "ExecutionStatus", "ExecutionTick", "GRASP_COMMAND", + "GRASP_CAPABILITY", "GraspGoal", "HandOver", "HandOverOptions", "HeldObjectPoseGoal", "HeldObjectState", + "FORWARD_KINEMATICS_CAPABILITY", + "INVERSE_KINEMATICS_CAPABILITY", "InteractionPoints", "JointPositionGoal", - "JointCommand", "JointPositionCommand", + "JointPositionPayload", + "JointPositionTarget", + "JOINT_POSITION_CAPABILITY", "MotionPolicy", "MonotonicExecutionClock", "MoveEndEffector", @@ -214,9 +254,10 @@ "RigidObjectSceneProvider", "RigidObjectSceneProviderCfg", "ResolvedActionRequest", - "ResolvedActionBinding", - "ResolvedControlPart", "RobotObservation", + "RuntimeCommandFrame", + "RuntimeCommandPayload", + "RuntimeEndpointTarget", "RunnerStatus", "RunnerStep", "RunnerStepCallback", @@ -225,9 +266,13 @@ "SceneSnapshotSupplier", "SceneEntityPose", "SkillDescriptor", + "SkillBindingContract", + "SkillEndpointRequirement", + "SkillResourceSlot", "StateDelta", "SimulationExecutionAdapter", "TaskState", + "TimedCommandSequence", "TimedTrajectory", "TrajectorySegment", "get_registered_actions", diff --git a/embodichain/lab/sim/atomic_actions/affordance.py b/embodichain/lab/sim/atomic_actions/affordance.py index fdab5e91f..dbe1ffea7 100644 --- a/embodichain/lab/sim/atomic_actions/affordance.py +++ b/embodichain/lab/sim/atomic_actions/affordance.py @@ -240,17 +240,18 @@ def get_approach_direction(self, point_idx: int) -> torch.Tensor: 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.""" @@ -274,18 +275,41 @@ def get_assemble_object_pose(self, base_pose: torch.Tensor) -> torch.Tensor: Returns: Assemble-object target pose with shape ``(n_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) + 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 (n_envs, 4, 4).") n_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: + if rel.shape == (4, 4): rel = rel.unsqueeze(0).repeat(n_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 (n_envs, 4, 4)." + ) elif rel.shape[0] == 1: rel = rel.repeat(n_envs, 1, 1) + elif rel.shape[0] != n_envs: + raise ValueError( + "assemble_to_base_pose batch size must match base_pose batch size." + ) return torch.bmm(base_pose, rel) diff --git a/embodichain/lab/sim/atomic_actions/bindings.py b/embodichain/lab/sim/atomic_actions/bindings.py index 5257c5035..d56713580 100644 --- a/embodichain/lab/sim/atomic_actions/bindings.py +++ b/embodichain/lab/sim/atomic_actions/bindings.py @@ -14,191 +14,295 @@ # 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 -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 _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. """ - 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.""" + @property + @abstractmethod + def transport_id(self) -> str: + """Return the registered transport kind used by this target.""" - 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 target_id(self) -> str: + """Return the destination identifier within its transport.""" - def manipulator(self, role: str = "primary") -> str: - """Return the manipulator control-part name bound to ``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 - Args: - role: Semantic manipulator role. + def snapshot(self) -> RuntimeEndpointTarget: + """Return an independently owned target snapshot.""" + return deepcopy(self) - Returns: - Key from the current robot's ``control_parts`` mapping. - 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 +@dataclass(frozen=True, slots=True) +class JointPositionTarget(RuntimeEndpointTarget): + """Joint-position destination backed by one robot control part.""" - def end_effector(self, role: str = "primary") -> str: - """Return the tool/hand control-part name bound to ``role``. + TRANSPORT_ID = "robot.joint_position" - Args: - role: Semantic end-effector role. + control_part: str + joint_ids: tuple[int, ...] - Returns: - Key from the current robot's ``control_parts`` mapping. + 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) - 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 + @property + def transport_id(self) -> str: + """Return the built-in joint-position transport identifier.""" + return self.TRANSPORT_ID + @property + def target_id(self) -> str: + """Return the robot control-part destination.""" + return self.control_part -@dataclass(frozen=True, slots=True) -class ResolvedControlPart: - """One engine-validated robot control part. + @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, + ) - Instances are produced by engine-owned planning services. They keep - robot-specific indices out of :class:`ActionBinding` and agent-facing - invocation schemas. - """ - name: str - """Key from ``Robot.control_parts``.""" +TargetT = TypeVar("TargetT", bound=RuntimeEndpointTarget) - joint_ids: tuple[int, ...] - """Full-robot joint indices belonging to this control part.""" +@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 + 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) + 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() @@ -211,82 +315,141 @@ def joint_positions( 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, + 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, + 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, + 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..03648c79a 100644 --- a/embodichain/lab/sim/atomic_actions/control.py +++ b/embodichain/lab/sim/atomic_actions/control.py @@ -44,6 +44,10 @@ 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): @@ -78,6 +82,12 @@ 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, *, @@ -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..707ed9233 100644 --- a/embodichain/lab/sim/atomic_actions/core.py +++ b/embodichain/lab/sim/atomic_actions/core.py @@ -29,6 +29,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 +41,20 @@ ) from .plans import ( ActionPlan, + ExecutionFeedbackMode, PlannerDiagnostics, TimedTrajectory, TrajectorySegment, normalize_success_mask, ) from .policies import DynamicCollisionMode +from .requirements import SkillBindingContract +from .runtime_commands import ( + EndpointCommand, + JointPositionPayload, + RuntimeCommandFrame, + TimedCommandSequence, +) if TYPE_CHECKING: from embodichain.lab.sim.objects import Robot @@ -70,9 +79,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 +104,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 +116,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 +156,9 @@ 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 + 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 +174,12 @@ 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 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 +199,17 @@ 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.""" + 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) @@ -253,9 +302,8 @@ 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, + binding_contract=cls.__dict__.get("binding_contract"), ) def resolve_request( @@ -290,10 +338,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 @@ -314,7 +364,7 @@ def resolve_request( 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, ), @@ -345,10 +395,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 +419,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,6 +487,13 @@ 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], @@ -481,32 +546,71 @@ def build_plan( 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, + replannable=replannable, + diagnostics=diagnostics, + segment_lengths=segment_lengths, + feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, + 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, + replannable: bool = True, + diagnostics: PlannerDiagnostics | None = None, + segment_lengths: Mapping[str, int] | None = None, + feedback_mode: ExecutionFeedbackMode = ExecutionFeedbackMode.TIMED, + joint_trajectory: TimedTrajectory | None = None, + ) -> ActionPlan: + """Build a plan from transport-neutral runtime command frames. + Non-joint command sequences use timed completion unless a future + endpoint-specific feedback evaluator is installed. Semantic effects + remain externally verified through the execution session. + """ + self.require_goal(request) + 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.") + commands = self._authorize_command_targets(request, commands) + success_mask = normalize_success_mask( + success, + n_envs=context.batch_size, + device=self.device, + name="Planning success", + ) + masked_commands = TimedCommandSequence( + frames=tuple( + frame.with_active_mask(frame.active_mask & success_mask) + for frame in commands.frames + ), + env_ids=commands.env_ids, + ) + segments = self._build_segments( + segment_lengths, + frame_count=masked_commands.frame_count, + ) if diagnostics is None: diagnostics = PlannerDiagnostics( backend=self.planning_services.planner_name @@ -514,25 +618,211 @@ def build_plan( return ActionPlan( skill_id=self.skill_id, plan_success=success_mask, - trajectory=timed, + commands=masked_commands, recovery_policy=request.recovery_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, - ), + feedback_mode=feedback_mode, + joint_trajectory=joint_trajectory, + segments=segments, + scene_dependencies=self._scene_dependencies(request), + collision_world_sensitive=self._uses_collision_world(request, context), replannable=replannable, expected_effects=expected_effects or StateDelta(), invocation_id=request.invocation_id, invocation_revision=request.revision, ) + @staticmethod + def _authorize_command_targets( + request: ResolvedActionRequest[GoalT, OptionsT], + commands: TimedCommandSequence, + ) -> 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. + """ + 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, + 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 +840,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..f9c1f537b 100644 --- a/embodichain/lab/sim/atomic_actions/effects.py +++ b/embodichain/lab/sim/atomic_actions/effects.py @@ -68,6 +68,8 @@ def _merge_held( 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 +87,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 +98,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 +113,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 +132,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 +143,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 ), diff --git a/embodichain/lab/sim/atomic_actions/engine.py b/embodichain/lab/sim/atomic_actions/engine.py index 080b3ef37..0baab8ab7 100644 --- a/embodichain/lab/sim/atomic_actions/engine.py +++ b/embodichain/lab/sim/atomic_actions/engine.py @@ -18,11 +18,13 @@ from __future__ import annotations +from types import MappingProxyType from typing import Iterable, Mapping, TYPE_CHECKING import torch -from .core import AtomicAction +from .bindings import ActionBinding +from .core import AtomicAction, SkillDescriptor from .control import ControlPartCommandProfile from .invocation import ActionInvocation, ResolvedActionRequest from .plans import ActionPlan, CompiledTrajectory, TimedTrajectory @@ -32,6 +34,12 @@ 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 @@ -88,6 +96,10 @@ 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, ) -> None: """Initialize one engine and bind its built-in action implementations. @@ -96,14 +108,40 @@ 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. """ + 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, ) 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 +163,11 @@ def planning_services(self) -> ActionPlanningServices: """Engine-owned resources shared by every bound atomic action.""" return self._planning_services + @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 +178,109 @@ 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]], + ) -> 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. + + 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) + def register(self, action: AtomicAction, *, replace: bool = False) -> None: """Register one action instance using its descriptor. @@ -159,6 +305,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.""" @@ -356,7 +510,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) @@ -436,15 +598,23 @@ def _validate_plan( raise ValueError( "ActionPlan.invocation_revision must preserve the request revision." ) - trajectory = plan.trajectory - if trajectory.batch_size != context.batch_size: + 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) diff --git a/embodichain/lab/sim/atomic_actions/execution.py b/embodichain/lab/sim/atomic_actions/execution.py index 1b95e64f9..b425b9934 100644 --- a/embodichain/lab/sim/atomic_actions/execution.py +++ b/embodichain/lab/sim/atomic_actions/execution.py @@ -26,7 +26,17 @@ from .effects import StateDelta from .invocation import ActionInvocation, ResolvedActionRequest -from .plans import ActionPlan, TimedTrajectory, TrajectorySegment +from .bindings import JointPositionTarget, RuntimeEndpointTarget +from .plans import ( + ActionPlan, + ExecutionFeedbackMode, + TrajectorySegment, +) +from .runtime_commands import ( + JointPositionPayload, + RuntimeCommandFrame, + TimedCommandSequence, +) from .state import EntityState, PlanningContext, SceneSnapshot, TaskState if TYPE_CHECKING: @@ -120,65 +130,14 @@ def __post_init__(self) -> None: object.__setattr__(self, "env_mask", self.env_mask.clone()) -@dataclass(frozen=True, slots=True, eq=False) -class JointCommand: - """Full-robot command produced by one session tick.""" - - 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.""" - - 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,).") - if ( - not torch.isfinite(self.hold_duration).all() - or (self.hold_duration < 0.0).any() - ): - 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()) - - @dataclass(frozen=True, slots=True, eq=False) class ExecutionTick: """Result returned after one closed-loop execution update.""" 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 @@ -192,16 +151,37 @@ def __post_init__(self) -> None: raise TypeError( "pending_effect must be an EffectVerificationRequest or None." ) + 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.") + 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 + latest observation and scene snapshot and emits at most one synchronized + endpoint-command frame. Expected symbolic effects are committed only after the caller supplies ``effect_success`` for a non-empty :class:`StateDelta`. Environment eligibility and recovery budgets are tracked per row. The @@ -227,9 +207,14 @@ def __init__( self._invocation_index = 0 self._waypoint_index = 0 self._plan: ActionPlan | None = None + self._active_targets: dict[ + tuple[str, str], + RuntimeEndpointTarget, + ] = {} self._planned_scene = context.scene self._action_started_at = context.robot.timestamp - self._last_command: torch.Tensor | None = None + self._last_joint_command: torch.Tensor | None = None + self._last_joint_ids: tuple[int, ...] = () self._last_command_mask = torch.zeros( context.batch_size, dtype=torch.bool, device=context.robot.qpos.device ) @@ -264,75 +249,151 @@ 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 + + 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: + raise RuntimeError( + "Cannot revise while a physical effect is awaiting verification; " + "verify 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: + raise RuntimeError( + "Cannot revise while a physical effect is awaiting verification; " + "verify 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, + ) - 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, ) + 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() def trajectory_segment(self, name: str) -> TrajectorySegment: """Return named segment metadata for the active action plan. @@ -360,33 +421,7 @@ def tick( 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 self._status is not ExecutionStatus.RUNNING: return self._tick_result(command=None, events=events) @@ -396,12 +431,16 @@ def tick( execution_mask = ( self._pending_effect.env_mask & self._pending & self._plan.plan_success ) - command, completion_events = self._finish_action( + command, hold_targets, completion_events = self._finish_action( execution_mask, effect_success, ) events.extend(completion_events) - return self._tick_result(command=command, events=events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) plan = self._plan execution_mask = self._pending & plan.plan_success @@ -421,8 +460,8 @@ def tick( plan = self._plan execution_mask = self._pending & self._plan.plan_success - trajectory = plan.trajectory - if self._waypoint_index < trajectory.waypoint_count: + 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) @@ -444,9 +483,27 @@ def tick( 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) + if plan.commands.frame_count > 0: + command = self._command_at(plan, 0, execution_mask) + self._waypoint_index = 1 + return self._tick_result(command=command, events=events) + events.append( + self._event( + ExecutionEventKind.TRAJECTORY_COMPLETED, + execution_mask, + "Replanned action has no executable command frame.", + ) + ) + command, hold_targets, completion_events = self._finish_action( + execution_mask, + effect_success, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) events.append( self._event( @@ -456,12 +513,46 @@ def tick( ) ) - command, completion_events = self._finish_action( + command, hold_targets, completion_events = self._finish_action( execution_mask, effect_success, ) 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, + ) def _plan_current( self, @@ -480,11 +571,27 @@ def _install_plan( event_kind: ExecutionEventKind, ) -> None: """Install an already validated plan as the current execution plan.""" + replacement_targets = { + (target.transport_id, target.target_id): target.snapshot() + for target in plan.commands.targets + } + replacement_destinations = frozenset(replacement_targets) + self._validate_destination_continuity(plan, event_kind) + if ( + event_kind + not in ( + ExecutionEventKind.REPLANNED, + ExecutionEventKind.INVOCATION_REVISED, + ) + or replacement_destinations + ): + self._active_targets = replacement_targets self._plan = plan self._waypoint_index = 0 self._planned_scene = context.scene self._action_started_at = context.robot.timestamp - self._last_command = None + self._last_joint_command = None + self._last_joint_ids = () self._last_command_mask.zero_() self._pending_effect = None planned_mask = self._pending & plan.plan_success @@ -492,6 +599,67 @@ def _install_plan( self._event(event_kind, planned_mask, "Planned from the latest context.") ) + 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 _recover_if_needed( self, plan: ActionPlan, @@ -517,9 +685,18 @@ def _recover_if_needed( ExecutionEventKind.COLLISION_WORLD_CHANGED, "The collision world changed after this trajectory was planned.", ) - if self._last_command is not None: + if ( + plan.feedback_mode is ExecutionFeedbackMode.JOINT_POSITION + and self._last_joint_command is not None + and self._last_joint_ids + ): + joint_ids = list(self._last_joint_ids) tracking_error = torch.amax( - torch.abs(self._context.robot.qpos - self._last_command), dim=1 + torch.abs( + self._context.robot.qpos[:, joint_ids] + - self._last_joint_command[:, joint_ids] + ), + dim=1, ) tracking_mask = ( execution_mask @@ -598,7 +775,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, @@ -615,9 +792,20 @@ def _finish_action( self, execution_mask: torch.Tensor, effect_success: torch.Tensor | None, - ) -> tuple[JointCommand | None, list[ExecutionEvent]]: + ) -> 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] = [] planning_failed = self._pending & ~self._plan.plan_success if not execution_mask.any() and planning_failed.any(): @@ -629,8 +817,8 @@ def _finish_action( ) ) if self._status is not ExecutionStatus.RUNNING: - return None, events - return self._hold_command(), events + return None, active_targets, events + return None, active_targets, events if self._plan.expected_effects.is_empty: verified = execution_mask @@ -644,7 +832,7 @@ def _finish_action( "Expected symbolic effects require external 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 @@ -672,11 +860,11 @@ def _finish_action( ) ) if self._status is not ExecutionStatus.RUNNING: - return None, events - return self._hold_command(), events + return None, active_targets, events + return None, active_targets, events if self._pending.any(): - return self._hold_command(), events + return None, active_targets, events events.append( self._event( ExecutionEventKind.ACTION_COMPLETED, @@ -698,7 +886,7 @@ def _finish_action( "Invocation sequence completed.", ) ) - return None, events + return None, (active_targets if orphaned_targets else ()), events self._pending = self._eligible.clone() self._pending_effect = None @@ -706,64 +894,84 @@ def _finish_action( self._replans.zero_() self._plan_current(self._context, ExecutionEventKind.ACTION_PLANNED) events.extend(self._drain_events()) - return self._hold_command(), events + return None, 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, - ) + ) -> RuntimeCommandFrame: + """Return one frame and retain joint targets when feedback requires it.""" + frame = plan.commands.frames[waypoint_index] + frame = frame.with_active_mask(frame.active_mask & active_mask) + if plan.feedback_mode is ExecutionFeedbackMode.JOINT_POSITION: + positions = self._context.robot.qpos.clone() + commanded_joint_ids: list[int] = [] + for command in frame.commands: + if not isinstance( + command.target, JointPositionTarget + ) or not isinstance( + command.payload, + JointPositionPayload, + ): + raise TypeError( + "joint_position feedback requires only joint-position " + "targets and payloads." + ) + joint_ids = list(command.target.joint_ids) + commanded_joint_ids.extend(joint_ids) + positions[:, joint_ids] = torch.where( + frame.active_mask[:, None], + command.payload.positions, + positions[:, joint_ids], + ) + self._last_joint_command = positions + self._last_joint_ids = tuple(commanded_joint_ids) + self._last_command_mask = frame.active_mask.clone() + else: + self._last_joint_command = None + self._last_joint_ids = () + self._last_command_mask.zero_() + return frame - 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( + def _terminal_error(self, plan: ActionPlan) -> torch.Tensor: + """Return terminal error for the plan's explicit feedback contract.""" + if plan.feedback_mode is ExecutionFeedbackMode.TIMED: + return torch.zeros( self._context.batch_size, - dtype=torch.float32, + dtype=self._context.robot.qpos.dtype, device=self._context.robot.qpos.device, - ), - ) - - 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, - ) + ) + if plan.commands.frame_count == 0: + return torch.full_like( + self._eligible, + float("inf"), + dtype=self._context.robot.qpos.dtype, + ) + errors: list[torch.Tensor] = [] + for command in plan.commands.frames[-1].commands: + if not isinstance(command.target, JointPositionTarget) or not isinstance( + command.payload, + JointPositionPayload, + ): + raise TypeError( + "joint_position feedback requires only joint-position targets " + "and payloads." + ) + joint_ids = list(command.target.joint_ids) + errors.append( + torch.abs( + self._context.robot.qpos[:, joint_ids] - command.payload.positions + ) + ) + if not errors: + return torch.full_like( + self._eligible, + float("inf"), + dtype=self._context.robot.qpos.dtype, + ) + return torch.amax(torch.cat(errors, dim=1), dim=1) def _dynamic_scene_change_mask(self, plan: ActionPlan) -> torch.Tensor: """Detect material motion of entities referenced by the action goal.""" @@ -901,14 +1109,16 @@ def _update_terminal_status(self) -> None: def _tick_result( self, *, - command: JointCommand | None, + command: RuntimeCommandFrame | None, events: list[ExecutionEvent], + hold_targets: tuple[RuntimeEndpointTarget, ...] = (), ) -> ExecutionTick: """Build an immutable tick result.""" 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, @@ -922,5 +1132,4 @@ def _tick_result( "ExecutionSession", "ExecutionStatus", "ExecutionTick", - "JointCommand", ] diff --git a/embodichain/lab/sim/atomic_actions/goals.py b/embodichain/lab/sim/atomic_actions/goals.py index bdf38811a..755d3ec24 100644 --- a/embodichain/lab/sim/atomic_actions/goals.py +++ b/embodichain/lab/sim/atomic_actions/goals.py @@ -18,6 +18,7 @@ from __future__ import annotations +import warnings from collections.abc import Mapping, Sequence from dataclasses import dataclass, fields, is_dataclass from typing import Any, ClassVar, Protocol, TYPE_CHECKING @@ -68,9 +69,22 @@ 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, + ) + PoseGoalValue = torch.Tensor | SceneEntityPose """Explicit pose tensor or a pose resolved from the latest scene snapshot.""" @@ -163,13 +177,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)) diff --git a/embodichain/lab/sim/atomic_actions/invocation.py b/embodichain/lab/sim/atomic_actions/invocation.py index b47795612..652cbf78a 100644 --- a/embodichain/lab/sim/atomic_actions/invocation.py +++ b/embodichain/lab/sim/atomic_actions/invocation.py @@ -25,7 +25,7 @@ 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 @@ -82,7 +82,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,7 +96,7 @@ 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.""" @@ -159,7 +159,7 @@ class ResolvedActionRequest(Generic[GoalT, OptionsT]): skill_id: str goal: GoalT - binding: ResolvedActionBinding + binding: ActionBinding motion_policy: MotionPolicy recovery_policy: RecoveryPolicy skill_options: OptionsT @@ -169,8 +169,8 @@ class ResolvedActionRequest(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.") - 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.recovery_policy, RecoveryPolicy): @@ -188,6 +188,14 @@ 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, "recovery_policy", deepcopy(self.recovery_policy)) object.__setattr__(self, "skill_options", deepcopy(self.skill_options)) diff --git a/embodichain/lab/sim/atomic_actions/plans.py b/embodichain/lab/sim/atomic_actions/plans.py index 356a0d6f6..b423cf19e 100644 --- a/embodichain/lab/sim/atomic_actions/plans.py +++ b/embodichain/lab/sim/atomic_actions/plans.py @@ -19,6 +19,7 @@ from __future__ import annotations from dataclasses import dataclass, field +from enum import Enum from types import MappingProxyType from typing import Any, Mapping, Sequence @@ -26,8 +27,10 @@ from embodichain.lab.sim.planners.utils import normalize_success_mask +from .bindings import JointPositionTarget from .effects import StateDelta from .policies import RecoveryPolicy +from .runtime_commands import JointPositionPayload, TimedCommandSequence from .state import PlanningContext @@ -109,7 +112,26 @@ def __post_init__(self) -> None: 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, "duration", self.duration.detach().clone()) + object.__setattr__(self, "env_ids", self.env_ids.detach().clone()) @property def batch_size(self) -> int: @@ -352,6 +374,13 @@ def __post_init__(self) -> None: object.__setattr__(self, "metadata", MappingProxyType(dict(self.metadata))) +class ExecutionFeedbackMode(str, Enum): + """Feedback contract used to decide whether an action reached its target.""" + + JOINT_POSITION = "joint_position" + TIMED = "timed" + + @dataclass(frozen=True, slots=True) class TrajectorySegment: """Named half-open waypoint range inside an action trajectory. @@ -391,18 +420,20 @@ 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. """ skill_id: str plan_success: torch.Tensor - trajectory: TimedTrajectory + commands: TimedCommandSequence recovery_policy: RecoveryPolicy planned_scene_version: int planned_collision_world_revision: tuple[int, ...] diagnostics: PlannerDiagnostics + feedback_mode: ExecutionFeedbackMode = ExecutionFeedbackMode.TIMED + joint_trajectory: TimedTrajectory | None = None segments: tuple[TrajectorySegment, ...] = () scene_dependencies: tuple[str, ...] = () collision_world_sensitive: bool = False @@ -423,21 +454,179 @@ 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.feedback_mode, ExecutionFeedbackMode): + raise TypeError("feedback_mode must be an ExecutionFeedbackMode.") + 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.") + if ( + self.feedback_mode is ExecutionFeedbackMode.JOINT_POSITION + and self.joint_trajectory is None + ): + raise ValueError( + "joint_position feedback requires an owned joint_trajectory." + ) + if self.feedback_mode is ExecutionFeedbackMode.JOINT_POSITION: + if bool(self.plan_success.any().item()) and self.commands.frame_count == 0: + raise ValueError( + "joint_position feedback requires command frames when any " + "environment planned successfully." + ) + assert self.joint_trajectory is not None + expected_destinations: dict[tuple[str, str], tuple[int, ...]] | None = None + for frame_index, frame in enumerate(self.commands.frames): + if not frame.commands: + raise ValueError( + "joint_position feedback requires at least one endpoint " + f"command in frame {frame_index}." + ) + if any( + not isinstance(command.target, JointPositionTarget) + or not isinstance(command.payload, JointPositionPayload) + for command in frame.commands + ): + raise ValueError( + "joint_position feedback accepts only JointPositionTarget " + "and JointPositionPayload commands." + ) + for command in frame.commands: + target = command.target + payload = command.payload + assert isinstance(target, JointPositionTarget) + assert isinstance(payload, JointPositionPayload) + if any( + joint_id >= self.joint_trajectory.robot_dof + for joint_id in target.joint_ids + ): + raise ValueError( + f"Joint target {command.destination_key} contains joint " + "IDs outside joint_trajectory robot_dof " + f"{self.joint_trajectory.robot_dof}." + ) + joint_ids = list(target.joint_ids) + expected_positions = self.joint_trajectory.positions[ + :, frame_index, joint_ids + ] + if ( + payload.positions.dtype != expected_positions.dtype + or not torch.equal(payload.positions, expected_positions) + ): + raise ValueError( + f"Joint payload positions for {command.destination_key} " + "must exactly match the corresponding joint_trajectory " + f"slice at frame {frame_index}." + ) + trajectory_velocities = self.joint_trajectory.velocities + if (payload.velocities is None) != (trajectory_velocities is None): + raise ValueError( + f"Joint payload velocities for {command.destination_key} " + "must have the same presence as joint_trajectory " + "velocities." + ) + if ( + payload.velocities is not None + and trajectory_velocities is not None + ): + expected_velocities = trajectory_velocities[ + :, frame_index, joint_ids + ] + if ( + payload.velocities.dtype != expected_velocities.dtype + or not torch.equal( + payload.velocities, + expected_velocities, + ) + ): + raise ValueError( + "Joint payload velocities for " + f"{command.destination_key} must exactly match the " + "corresponding joint_trajectory slice at frame " + f"{frame_index}." + ) + destinations = { + command.destination_key: command.target.joint_ids + for command in frame.commands + if isinstance(command.target, JointPositionTarget) + } + if expected_destinations is None: + expected_destinations = destinations + elif destinations != expected_destinations: + raise ValueError( + "joint_position feedback requires a stable joint endpoint " + "set across every command frame." + ) 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 @@ -462,7 +651,7 @@ def __post_init__(self) -> None: 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 + waypoint_count = self.commands.frame_count segments = tuple(self.segments) if not all(isinstance(segment, TrajectorySegment) for segment in segments): raise TypeError("segments must contain only TrajectorySegment values.") @@ -473,7 +662,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,10 +673,20 @@ 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, + "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, "scene_dependencies", dependencies) object.__setattr__(self, "segments", segments) @@ -516,9 +715,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 +754,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 +776,7 @@ def segment(self, action_index: int, name: str) -> TrajectorySegment: __all__ = [ "ActionPlan", "CompiledTrajectory", + "ExecutionFeedbackMode", "PlannerDiagnostics", "TimedTrajectory", "TrajectorySegment", diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py index f59d5286c..b8cbb0b42 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py @@ -27,18 +27,28 @@ 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 ..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, resolve_pose_goal, validate_pose_goal, ) from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan, normalize_success_mask +from ..requirements import ( + DisjointResourceSlots, + DisjointSlotEndpoints, + GRASP_CAPABILITY, + INVERSE_KINEMATICS_CAPABILITY, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) from ..state import CoordinatedHeldObjectState, PlanningContext from ..trajectory_ops import interpolate_joint_trajectory, translate_pose_world @@ -59,7 +69,11 @@ class CoordinatedPickGoal(ObjectActionGoal): """Target pose for the shared object, shape ``(4, 4)`` or ``(n_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 +158,10 @@ 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_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 @@ -335,8 +349,30 @@ 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( + SkillResourceSlot( + slot_id=role, + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset({INVERSE_KINEMATICS_CAPABILITY}), + ), + SkillEndpointRequirement( + endpoint_id="grasp", + capabilities=frozenset({GRASP_CAPABILITY}), + required_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + ) + for role in ("left", "right") + ), + constraints=(DisjointResourceSlots(("left", "right")),), + ) _assemble_segment = _DualArmHelpers._assemble_segment _expand_qpos = _DualArmHelpers._expand_qpos @@ -359,22 +395,42 @@ def _on_bind(self) -> None: self.n_envs = self.robot.get_qpos().shape[0] self.robot_dof = self.robot.dof + def _scene_dependencies( + self, + 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, request: ResolvedActionRequest[CoordinatedPickGoal, CoordinatedPickmentOptions], ) -> _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) + 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." @@ -384,25 +440,25 @@ def _resolve_resources( 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, 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, 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, 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, device=self.device, @@ -424,14 +480,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", ) @@ -720,12 +774,12 @@ 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( @@ -733,7 +787,7 @@ def _plan_synchronized_object_motion( n_envs=self.n_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}" ), ) @@ -742,17 +796,17 @@ def _plan_synchronized_object_motion( n_envs=self.n_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, ) @@ -828,14 +882,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"], @@ -971,13 +1025,13 @@ def _plan( trajectory=full, expected_effects=StateDelta( held_object_updates={ - resources.left_arm.name: None, - resources.right_arm.name: None, + resources.left_arm.control_part: None, + resources.right_arm.control_part: None, }, coordinated_held_object_updates={ ( - resources.left_arm.name, - resources.right_arm.name, + resources.left_arm.control_part, + resources.right_arm.control_part, ): 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..c00771e02 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py @@ -26,14 +26,23 @@ 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 ..policies import MotionPolicy +from ..requirements import ( + CARTESIAN_POSE_CAPABILITY, + DisjointResourceSlots, + DisjointSlotEndpoints, + GRASP_CAPABILITY, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) from ..state import HeldObjectState, PlanningContext from ..trajectory_ops import ( build_pose_plan_states, @@ -113,10 +122,10 @@ 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_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,8 +139,44 @@ 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") + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="placing", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + ), + SkillEndpointRequirement( + endpoint_id="grasp", + capabilities=frozenset({GRASP_CAPABILITY}), + required_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + ), + SkillResourceSlot( + slot_id="support", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + ), + SkillEndpointRequirement( + endpoint_id="grasp", + capabilities=frozenset({GRASP_CAPABILITY}), + required_commands={GRASP_COMMAND: JointPositionCommand}, + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + ), + ), + constraints=(DisjointResourceSlots(("placing", "support")),), + ) def __init__( self, @@ -152,16 +197,20 @@ 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) + 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." @@ -171,19 +220,19 @@ def _resolve_resources( 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, 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, 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, device=self.device, @@ -239,7 +288,7 @@ def _plan( device=self.device, ) segment_success, placing_approach_traj = self._plan_named_arm_trajectory( - resources.placing_arm.name, + resources.placing_arm.control_part, placing_start_qpos, torch.stack([placing_lift_xpos, placing_xpos], dim=1), segments["approach"], @@ -258,7 +307,7 @@ def _plan( ) segment_success, support_approach_traj = self._plan_named_arm_trajectory( - resources.support_arm.name, + resources.support_arm.control_part, support_start_qpos, support_xpos.unsqueeze(1), segments["approach"], @@ -316,7 +365,7 @@ def _plan( ) segment_success, placing_retreat_traj = self._plan_named_arm_trajectory( - resources.placing_arm.name, + resources.placing_arm.control_part, placing_place_qpos, placing_lift_xpos.unsqueeze(1), segments["retreat"], @@ -356,8 +405,8 @@ def _plan( dim=1, ) involved_control_parts = { - resources.placing_arm.name, - resources.support_arm.name, + resources.placing_arm.control_part, + resources.support_arm.control_part, } coordinated_removals = { key: None @@ -371,10 +420,10 @@ def _plan( trajectory=full, expected_effects=StateDelta( held_object_updates={ - resources.placing_arm.name: ( + resources.placing_arm.control_part: ( None if release else placing_held_object ), - resources.support_arm.name: support_held_object, + resources.support_arm.control_part: support_held_object, }, coordinated_held_object_updates=coordinated_removals, ), @@ -458,8 +507,8 @@ def _resolve_target( HeldObjectState, HeldObjectState, ]: - placing_control_part = resources.placing_arm.name - support_control_part = resources.support_arm.name + placing_control_part = resources.placing_arm.control_part + support_control_part = resources.support_arm.control_part placing_held_object = state.get_held_object(placing_control_part) if placing_held_object is None: logger.log_error( diff --git a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py index 4c0878773..72c4bb0f4 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py +++ b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py @@ -26,13 +26,29 @@ 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 ..bindings import JointPositionTarget +from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand +from ..core import AtomicAction, ObjectSemantics, _same_object_identity from ..effects import StateDelta +from ..goals import ( + PoseGoalValue, + 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 ..requirements import ( + CARTESIAN_POSE_CAPABILITY, + DisjointResourceSlots, + DisjointSlotEndpoints, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) from ..state import HeldObjectState, PlanningContext from ..trajectory_ops import ( build_pose_plan_states, @@ -50,13 +66,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 +124,26 @@ 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_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,8 +162,52 @@ 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=( + SkillResourceSlot( + slot_id="source", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } + ), + ), + SkillEndpointRequirement( + endpoint_id="grasp", + capabilities=frozenset({GRASP_CAPABILITY}), + required_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + ), + SkillResourceSlot( + slot_id="destination", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + ), + SkillEndpointRequirement( + endpoint_id="grasp", + capabilities=frozenset({GRASP_CAPABILITY}), + required_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + ), + ), + constraints=(DisjointResourceSlots(("source", "destination")),), + ) def __init__( self, @@ -149,22 +220,42 @@ def _on_bind(self) -> None: self.n_envs = self.robot.get_qpos().shape[0] self.robot_dof = self.robot.dof + def _scene_dependencies( + self, + 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, request: ResolvedActionRequest[GraspGoal, HandOverOptions], ) -> _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) + 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." @@ -174,25 +265,25 @@ def _resolve_resources( 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, 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, 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, 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, device=self.device, @@ -224,15 +315,31 @@ 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_arm.control_part, + semantics, + ) + 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 +348,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] @@ -285,15 +401,12 @@ 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, + resources.transfer_arm.control_part, transfer_start_qpos, transfer_middle_eef.unsqueeze(1), segments["transfer"], @@ -310,7 +423,7 @@ def _plan( return self.failed_plan(request, context, message="Transfer move failed.") segment_success, receive_approach_traj = self._plan_named_arm_trajectory( - resources.receive_arm.name, + resources.receive_arm.control_part, receive_start_qpos, torch.stack([receive_pre_grasp_eef, receive_grasp_xpos], dim=1), segments["approach"], @@ -332,7 +445,7 @@ def _plan( receive_grasp_qpos = receive_approach_traj[:, -1] segment_success, transfer_retreat_traj = self._plan_named_arm_trajectory( - resources.transfer_arm.name, + resources.transfer_arm.control_part, transfer_hold_qpos, transfer_retreat_eef.unsqueeze(1), segments["deliver"], @@ -351,7 +464,7 @@ def _plan( ) segment_success, receive_deliver_traj = self._plan_named_arm_trajectory( - resources.receive_arm.name, + resources.receive_arm.control_part, receive_grasp_qpos, receive_final_eef.unsqueeze(1), segments["deliver"], @@ -487,8 +600,8 @@ def _plan( trajectory=full, expected_effects=StateDelta( held_object_updates={ - resources.transfer_arm.name: None, - resources.receive_arm.name: held_object, + resources.transfer_arm.control_part: None, + resources.receive_arm.control_part: held_object, } ), segment_lengths=segment_lengths, @@ -507,7 +620,7 @@ def _validate_pose_options(options: HandOverOptions) -> None: ) def _resolve_matrix(self, matrix: torch.Tensor, name: str) -> torch.Tensor: - matrix = matrix.to(device=self.device, dtype=torch.float32) + matrix = matrix.to(device=self.device, dtype=torch.float32).clone() if matrix.shape == (4, 4): matrix = matrix.unsqueeze(0).repeat(self.n_envs, 1, 1) if matrix.shape != (self.n_envs, 4, 4): @@ -522,6 +635,7 @@ def _resolve_transfer_object_to_eef( self, state: PlanningContext, transfer_control_part: str, + target_semantics: ObjectSemantics, ) -> torch.Tensor: held = state.get_held_object(transfer_control_part) if held is None: @@ -530,6 +644,11 @@ def _resolve_transfer_object_to_eef( f"{transfer_control_part!r} (run PickUp first).", ValueError, ) + if not _same_object_identity(target_semantics, held.semantics): + raise ValueError( + "HandOver target semantics must identify the object held by " + f"transfer control part {transfer_control_part!r}." + ) return self._resolve_matrix(held.object_to_eef, "held_object.object_to_eef") def _resolve_receive_grasp( 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..382607b06 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py @@ -23,10 +23,17 @@ import torch +from ..bindings import JointPositionTarget from ..core import AtomicAction from ..goals import PoseGoalValue, resolve_pose_goal, validate_pose_goal from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan +from ..requirements import ( + CARTESIAN_POSE_CAPABILITY, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) from ..state import PlanningContext from ..trajectory_ops import ( build_pose_plan_states, @@ -58,8 +65,20 @@ class MoveEndEffector(AtomicAction[EndEffectorPoseGoal, MoveEndEffectorOptions]) skill_id: ClassVar[str] = "move_end_effector" GoalType: ClassVar[type] = EndEffectorPoseGoal + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + ), + ), + ), + ), + ) OptionsType: ClassVar[type] = MoveEndEffectorOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) def __init__( self, @@ -74,9 +93,11 @@ def _plan( ) -> 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) + 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, 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..7cb43860e 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py @@ -24,14 +24,28 @@ 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 ..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 ..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 @@ -83,8 +97,30 @@ 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",) + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } + ), + ), + SkillEndpointRequirement( + endpoint_id="grasp", + capabilities=frozenset({GRASP_CAPABILITY}), + required_commands={GRASP_COMMAND: JointPositionCommand}, + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + ), + ), + ) def __init__( self, @@ -106,12 +142,14 @@ def _plan( target = self.require_goal(request) 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) + 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, device=self.device, @@ -138,18 +176,19 @@ def _plan( 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.n_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: diff --git a/embodichain/lab/sim/atomic_actions/primitives/move_joints.py b/embodichain/lab/sim/atomic_actions/primitives/move_joints.py index c2693d091..ffa7876eb 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_joints.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_joints.py @@ -23,9 +23,16 @@ import torch +from ..bindings import JointPositionTarget from ..core import AtomicAction from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan +from ..requirements import ( + JOINT_POSITION_CAPABILITY, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) from ..state import PlanningContext from ..trajectory_ops import ( build_joint_plan_states, @@ -73,8 +80,20 @@ 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 + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset({JOINT_POSITION_CAPABILITY}), + ), + ), + ), + ), + ) def __init__( self, @@ -89,10 +108,11 @@ def _plan( ) -> 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 + 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, @@ -136,7 +156,7 @@ 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, device=self.device, diff --git a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py index ed81535c6..a5037d661 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 @@ -34,19 +34,31 @@ from ._helpers import arm_qpos_from_state 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 ..requirements import ( + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + DisjointSlotEndpoints, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) from ..state import HeldObjectState, PlanningContext from ..trajectory_ops import ( build_pose_plan_states, @@ -101,7 +113,7 @@ 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.""" obj_upright_direction: torch.Tensor | None = None @@ -135,10 +147,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,8 +174,34 @@ 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=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } + ), + ), + SkillEndpointRequirement( + endpoint_id="grasp", + capabilities=frozenset({GRASP_CAPABILITY}), + required_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + ), + ), + ) def __init__( self, @@ -166,6 +214,22 @@ def _on_bind(self) -> None: self.n_envs = self.robot.get_qpos().shape[0] self.robot_dof = self.robot.dof + def _scene_dependencies( + self, + 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, grasp_xpos: torch.Tensor, @@ -174,8 +238,8 @@ 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, ) -> tuple[torch.Tensor, torch.Tensor, dict[str, int]]: @@ -194,7 +258,7 @@ 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, ), ) @@ -212,7 +276,7 @@ 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, ), ) @@ -264,7 +328,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( + target, + context, + name=f"downstream_object_target_poses[{index}]", + ) + for index, target in enumerate( + request.skill_options.downstream_object_target_poses + ) + ), + ) approach_direction = options.approach_direction.to( device=self.device, dtype=torch.float32 ) @@ -272,23 +348,30 @@ 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) + hand_open_qpos = grasp.joint_positions( OPEN_COMMAND, n_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, 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 ): @@ -296,17 +379,18 @@ def _plan( "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 - ) start_arm_qpos = arm_qpos_from_state( state, list(manipulator.joint_ids), ) 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( @@ -316,7 +400,9 @@ def _plan( ) 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) grasp_success = normalize_success_mask( @@ -350,8 +436,7 @@ def _plan( 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 ) @@ -373,18 +458,18 @@ def _plan( 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_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, ) - n_envs = obj_poses.shape[0] + n_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 @@ -408,10 +493,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, @@ -426,11 +510,10 @@ def _resolve_grasp_pose( 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]: @@ -441,7 +524,9 @@ def _select_feasible_grasp_variants( 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() @@ -511,7 +596,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]) @@ -556,28 +641,29 @@ 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) 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(n_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), + qpos.reshape(n_envs, n_pose, n_variant, manipulator_dof), ) def _upright_adjusted_grasp_poses( self, - semantics: ObjectSemantics, grasp_xpos: torch.Tensor, + object_pose: torch.Tensor, options: PickUpOptions, ) -> torch.Tensor: """Return grasp poses after the optional upright-in-place roll adjustment.""" @@ -592,8 +678,7 @@ def _upright_adjusted_grasp_poses( 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) + 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..b4678dd02 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/place.py +++ b/embodichain/lab/sim/atomic_actions/primitives/place.py @@ -18,6 +18,7 @@ from __future__ import annotations +import warnings from dataclasses import dataclass from typing import ClassVar, Literal @@ -28,12 +29,27 @@ from ._helpers import arm_qpos_from_state, 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 ..goals import ( + PoseGoalValue, + SceneEntityPose, + resolve_pose_goal, + validate_pose_goal, +) from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan +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, @@ -79,11 +95,12 @@ 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" @@ -91,6 +108,18 @@ class AssembleGoal: 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): @@ -129,9 +158,10 @@ class Place(AtomicAction[PlaceGoal | AssembleGoal, PlaceOptions]): 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,8 +170,33 @@ 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=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } + ), + ), + SkillEndpointRequirement( + endpoint_id="grasp", + capabilities=frozenset({GRASP_CAPABILITY}), + required_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + ), + ), + ) def __init__( self, @@ -154,6 +209,17 @@ def _on_bind(self) -> None: self.n_envs = self.robot.get_qpos().shape[0] self.robot_dof = self.robot.dof + def _scene_dependencies( + self, + 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, request: ResolvedActionRequest[PlaceGoal | AssembleGoal, PlaceOptions], @@ -163,18 +229,21 @@ def _plan( target = self.require_goal(request) 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_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) + hand_open_qpos = grasp.joint_positions( OPEN_COMMAND, n_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, device=self.device, @@ -325,7 +394,7 @@ def _resolve_assemble_place_xpos( Place EEF poses with shape ``(n_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) if held is None: @@ -335,15 +404,37 @@ def _resolve_assemble_place_xpos( ValueError, ) 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", + ), + n_envs=self.n_envs, + device=self.device, + name="base_pose", + ) + else: + if affordance.base_object_entity is None: + logger.log_error( + "AssembleGoal requires base_pose or " + "AssembleAffordance.base_object_entity.", + ValueError, + ) + 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), + n_envs=self.n_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, diff --git a/embodichain/lab/sim/atomic_actions/primitives/press.py b/embodichain/lab/sim/atomic_actions/primitives/press.py index a57e44bb8..46fa15369 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/press.py +++ b/embodichain/lab/sim/atomic_actions/primitives/press.py @@ -26,11 +26,21 @@ from embodichain.utils import logger from ._helpers import arm_qpos_from_state -from ..control import GRASP_COMMAND +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 ..requirements import ( + CARTESIAN_POSE_CAPABILITY, + DisjointSlotEndpoints, + GRASP_CAPABILITY, + JOINT_POSITION_CAPABILITY, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) from ..state import PlanningContext from ..trajectory_ops import ( build_joint_plan_states, @@ -71,8 +81,30 @@ class Press(AtomicAction[PressGoal, PressOptions]): 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",) + 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="grasp", + capabilities=frozenset({GRASP_CAPABILITY}), + required_commands={GRASP_COMMAND: JointPositionCommand}, + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + ), + ), + ) def __init__( self, @@ -94,12 +126,15 @@ def _plan( target = self.require_goal(request) 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_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) + hand_close_qpos = grasp.joint_positions( GRASP_COMMAND, n_envs=self.n_envs, device=self.device, 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..063bd1fa5 100644 --- a/embodichain/lab/sim/atomic_actions/runner.py +++ b/embodichain/lab/sim/atomic_actions/runner.py @@ -29,12 +29,14 @@ from embodichain.utils import configclass +from .bindings import RuntimeEndpointTarget from .execution import ( ExecutionSession, ExecutionStatus, ExecutionTick, - JointCommand, ) +from .invocation import ActionInvocation, ResolvedActionRequest +from .runtime_commands import RuntimeCommandFrame from .state import PlanningContext, TaskState @@ -123,15 +125,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 +144,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: @@ -290,7 +302,8 @@ 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. @@ -334,10 +347,16 @@ 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` on the runner, rather than mutating the + session directly, while this runner owns scheduling. + """ return self._session @property @@ -358,6 +377,39 @@ 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 step( self, *, @@ -398,6 +450,12 @@ def step( self._last_context = context try: + if self._pending_revision is not None: + self._session._install_prepared_revision( + self._pending_revision, + context, + ) + self._pending_revision = None tick = self._session.tick(context, effect_success=effect_success) except Exception as exc: return self._fail( @@ -408,12 +466,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 +497,29 @@ def step( self._command_count += 1 interval = self._command_interval(tick.command) self._next_step_at = self._clock_now() + interval + elif tick.hold_targets: + 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 else: self._next_step_at = self._clock_now() @@ -440,7 +527,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: @@ -499,6 +587,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(), @@ -630,7 +719,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 +738,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 +775,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 +804,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 +830,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(), diff --git a/embodichain/lab/sim/atomic_actions/runtime.py b/embodichain/lab/sim/atomic_actions/runtime.py index b8a62d9d0..c0530db40 100644 --- a/embodichain/lab/sim/atomic_actions/runtime.py +++ b/embodichain/lab/sim/atomic_actions/runtime.py @@ -21,16 +21,18 @@ 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, +) if TYPE_CHECKING: from embodichain.lab.sim.objects import Robot @@ -38,18 +40,7 @@ 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, @@ -59,13 +50,10 @@ def __init__( 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 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 +70,14 @@ 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 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 +94,210 @@ 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]], + ) -> 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. - - Args: - binding: Semantic-role mapping to validate and resolve. - control_overrides: Optional per-role command replacements for this - invocation revision. - - 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. + This is the advanced direct-core construction path. Profile-backed + callers obtain the same :class:`ActionBinding` from + ``BoundRobotSkillProfile.resolve()``. """ - 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", - ), - ) + 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: 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__}." + ) + resolved.append( + EndpointBinding( + slot_id=slot_id, + endpoint_id=endpoint_id, + resource_id=f"direct.{slot_id}", + adapter_id="control_part", + target=JointPositionTarget(control_part, joint_ids), + 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 +333,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..d53b61b69 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, @@ -262,6 +263,9 @@ class SimulationExecutionAdapter: initial_time: Initial elapsed simulation time in seconds. """ + transport_id = JointPositionTarget.TRANSPORT_ID + payload_type = JointPositionPayload + def __init__( self, simulation: SimulationManager, @@ -395,16 +399,15 @@ def observe(self, task_state: TaskState) -> PlanningContext: 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 +416,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 +462,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 +479,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 +507,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 +524,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 +560,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..69cf8d044 100644 --- a/embodichain/lab/sim/atomic_actions/state.py +++ b/embodichain/lab/sim/atomic_actions/state.py @@ -18,9 +18,10 @@ from __future__ import annotations +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 @@ -423,6 +424,30 @@ def __post_init__(self) -> None: object.__setattr__(self, "pose", self.pose.clone()) +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) + + @dataclass(frozen=True, slots=True, eq=False) class SceneSnapshot: """Versioned scene state used to ground dynamic goals and obstacles.""" @@ -487,7 +512,7 @@ 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, "collision_entity_ids", collision_entity_ids) def collision_world_revisions(self, batch_size: int) -> tuple[int, ...]: diff --git a/embodichain/lab/sim/atomic_actions/trajectory_ops.py b/embodichain/lab/sim/atomic_actions/trajectory_ops.py index 0154e8418..ca4d4c61a 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 @@ -182,7 +181,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. " 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/planners/base_planner.py b/embodichain/lab/sim/planners/base_planner.py index c6f0eecae..d58f7b019 100644 --- a/embodichain/lab/sim/planners/base_planner.py +++ b/embodichain/lab/sim/planners/base_planner.py @@ -22,6 +22,7 @@ from abc import ABC, abstractmethod from collections.abc import Mapping from dataclasses import MISSING +from typing import Literal from embodichain.utils import logger from embodichain.utils import configclass @@ -178,6 +179,21 @@ def __init__(self, cfg: BasePlannerCfg): supports_collision_world_updates: bool = False """Whether per-plan dynamic obstacle poses can update the collision world.""" + @property + def dynamic_collision_entity_ids(self) -> tuple[str, ...]: + """Return canonical entity IDs accepted for dynamic pose updates.""" + return () + + @property + def collision_world_entity_ids(self) -> tuple[str, ...]: + """Return every entity ID represented in the planner collision world.""" + return () + + @property + def collision_world_batch_mode(self) -> Literal["shared", "per_env"] | None: + """Return the planner collision world's batch-sharing mode, if any.""" + return None + def supports_move_type(self, move_type: MoveType) -> bool: """Return whether the planner accepts a movement target type directly. diff --git a/embodichain/lab/sim/planners/curobo/curobo_planner.py b/embodichain/lab/sim/planners/curobo/curobo_planner.py index f9cf5fce5..6259134de 100644 --- a/embodichain/lab/sim/planners/curobo/curobo_planner.py +++ b/embodichain/lab/sim/planners/curobo/curobo_planner.py @@ -40,7 +40,7 @@ from dataclasses import dataclass from pathlib import Path from types import SimpleNamespace -from typing import TYPE_CHECKING +from typing import TYPE_CHECKING, Literal import torch import yaml @@ -136,6 +136,25 @@ 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) + + +def _named_rigid_objects( + rigid_objects: list[RigidObject] | Mapping[str, RigidObject] | None, +) -> list[tuple[str, RigidObject]]: + """Return canonical cuRobo 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 ()) + ] + + @configclass class CuroboWorldCfg: """Static collision-world configuration for the cuRobo backend. @@ -144,16 +163,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 +200,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 +233,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 +494,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 +541,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)``. @@ -761,6 +838,23 @@ def preserve_plan_samples(self) -> bool: """ return self.cfg.preserve_plan_samples + @property + def dynamic_collision_entity_ids(self) -> tuple[str, ...]: + """Return canonical registry IDs accepted for dynamic pose updates.""" + return tuple(self.cfg.world.dynamic_obstacle_names) + + @property + def collision_world_entity_ids(self) -> tuple[str, ...]: + """Return every obstacle ID represented in the generated world.""" + return tuple( + name for name, _ in _named_rigid_objects(self.cfg.world.rigid_objects) + ) + + @property + def collision_world_batch_mode(self) -> Literal["shared", "per_env"]: + """Return the configured collision-world batching policy.""" + return "per_env" if self.cfg.world.multi_env else "shared" + def __init__(self, cfg: CuroboPlannerCfg) -> None: super().__init__(cfg) self.cfg: CuroboPlannerCfg = cfg @@ -1638,8 +1732,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] @@ -2219,8 +2312,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 diff --git a/embodichain/lab/sim/planners/curobo/curobo_yaml.py b/embodichain/lab/sim/planners/curobo/curobo_yaml.py index 1b24eec68..f470f8e4b 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 @@ -539,7 +540,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 +553,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 +569,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 +597,48 @@ def generate_curobo_world_yaml( import yaml - rigid_objects = list(rigid_objects) - if not rigid_objects: + registry_backed = isinstance(rigid_objects, Mapping) + if registry_backed: + named_rigid_objects = list(rigid_objects.items()) + else: + named_rigid_objects = [ + (getattr(obj, "uid", None) or f"obstacle_{idx}", obj) + for idx, obj in enumerate(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..0c3e9a4a4 100644 --- a/embodichain/lab/sim/planners/motion_generator.py +++ b/embodichain/lab/sim/planners/motion_generator.py @@ -168,6 +168,72 @@ def supports_dynamic_collision_world(self) -> bool: """ return getattr(self.planner, "supports_collision_world_updates", False) is True + @property + def dynamic_collision_entity_ids(self) -> tuple[str, ...]: + """Return canonical dynamic-obstacle IDs declared by the planner.""" + entity_ids = getattr(self.planner, "dynamic_collision_entity_ids", ()) + return self._validate_collision_entity_ids( + entity_ids, + field_name="dynamic_collision_entity_ids", + ) + + @property + def collision_world_entity_ids(self) -> tuple[str, ...]: + """Return every canonical entity ID in the planner collision world.""" + entity_ids = getattr(self.planner, "collision_world_entity_ids", ()) + return self._validate_collision_entity_ids( + entity_ids, + field_name="collision_world_entity_ids", + ) + + @staticmethod + def _validate_collision_entity_ids( + entity_ids: object, + *, + field_name: str, + ) -> tuple[str, ...]: + """Validate one planner-owned canonical collision-ID declaration.""" + 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"Planner.{field_name} must be a tuple of " + "non-empty strings without outer whitespace." + ) + if len(set(entity_ids)) != len(entity_ids): + raise ValueError(f"Planner.{field_name} must contain unique IDs.") + return 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.""" + mode = getattr(self.planner, "collision_world_batch_mode", None) + if mode not in (None, "shared", "per_env"): + raise ValueError( + "Planner.collision_world_batch_mode must be 'shared', 'per_env', " + "or None." + ) + return mode + def bind_collision_world( self, plan_opts: PlanOptions | None, @@ -192,15 +258,68 @@ def bind_collision_world( "collision-world updates.", ValueError, ) + configured_ids = self.dynamic_collision_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 resolve_plan_options( self, diff --git a/embodichain/lab/sim/planners/utils.py b/embodichain/lab/sim/planners/utils.py index 1913449eb..76a4e1beb 100644 --- a/embodichain/lab/sim/planners/utils.py +++ b/embodichain/lab/sim/planners/utils.py @@ -58,11 +58,18 @@ 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) if not isinstance(success, torch.Tensor): diff --git a/embodichain/lab/sim/skills/__init__.py b/embodichain/lab/sim/skills/__init__.py new file mode 100644 index 000000000..d3b7c2ea2 --- /dev/null +++ b/embodichain/lab/sim/skills/__init__.py @@ -0,0 +1,173 @@ +# ---------------------------------------------------------------------------- +# 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, + Pick, + Place, + PlaceRelationTarget, + RegisteredSemanticCall, + SemanticCallCatalog, + SemanticCallDescriptor, + SemanticCallSpec, + SemanticPose, + builtin_semantic_call_catalog, +) +from .compiler import ( + AnalyzedSemanticCall, + GroundedSemanticCall, + HandOverPoseProvider, + HandOverPoseTargets, + RegisteredSemanticLowerer, + RelationTargetGrounder, + SemanticEffectDependency, + SemanticEffectKind, + SemanticHandOverTarget, + SemanticLowering, + SemanticObjectTarget, + SemanticRelationTarget, + SemanticSkillCompiler, + SemanticWorkflow, +) +from .integration import ( + BoundSemanticCall, + BoundSemanticIntegration, + LinkedSemanticCall, + PathPart, + SceneEntityManifest, + SceneManifest, + SemanticDiagnostic, + SemanticIntegrationManifest, + SemanticValidationError, +) +from .profiles import ( + AmbiguousSkillBindingError, + BoundRobotSkillProfile, + ControlPartEndpoint, + ControlPartEndpointAdapter, + EndpointResolution, + ProfileValidationError, + ResolvedResourceEndpoint, + ResolvedRobotResource, + ResolvedSkillBinding, + ResourceBinding, + ResourceClaim, + ResourceEndpoint, + ResourceEndpointAdapter, + RobotResource, + RobotSkillProfile, + SkillPolicyPreset, + UnsupportedSkillError, +) +from .scene import ( + AmbiguousSceneAffordanceError, + GRASP_AFFORDANCE_CAPABILITY, + PLACE_IN_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, + RegistrySceneProvider, + SceneAffordanceRef, + SceneArticulationRef, + SceneCollisionRole, + SceneCollisionWorldMode, + SceneDynamics, + SceneEntityRef, + SceneEntityMetadata, + SceneEntityRegistration, + SceneEntityStateProvider, + SceneGeometryProvider, + SceneLinkRef, + SceneObjectRef, + SceneRegistry, + UnsupportedSceneAffordanceError, +) + +__all__ = [ + "AmbiguousSceneAffordanceError", + "AmbiguousSkillBindingError", + "AnalyzedSemanticCall", + "BoundSemanticCall", + "BoundSemanticIntegration", + "BoundRobotSkillProfile", + "ControlPartEndpoint", + "ControlPartEndpointAdapter", + "DeclarativeValue", + "EndpointResolution", + "GRASP_AFFORDANCE_CAPABILITY", + "GroundedSemanticCall", + "HandOver", + "HandOverPoseProvider", + "HandOverPoseTargets", + "LinkedSemanticCall", + "PLACE_IN_AFFORDANCE_CAPABILITY", + "PLACE_ON_AFFORDANCE_CAPABILITY", + "PathPart", + "Pick", + "Place", + "PlaceRelationTarget", + "ProfileValidationError", + "RegistrySceneProvider", + "ResolvedRobotResource", + "ResolvedResourceEndpoint", + "ResolvedSkillBinding", + "ResourceBinding", + "ResourceClaim", + "ResourceEndpoint", + "ResourceEndpointAdapter", + "RegisteredSemanticCall", + "RegisteredSemanticLowerer", + "RelationTargetGrounder", + "RobotResource", + "RobotSkillProfile", + "SceneAffordanceRef", + "SceneArticulationRef", + "SceneCollisionRole", + "SceneCollisionWorldMode", + "SceneDynamics", + "SceneEntityRef", + "SceneEntityMetadata", + "SceneEntityRegistration", + "SceneEntityManifest", + "SceneEntityStateProvider", + "SceneGeometryProvider", + "SceneLinkRef", + "SceneObjectRef", + "SceneRegistry", + "SceneManifest", + "SemanticCallCatalog", + "SemanticCallDescriptor", + "SemanticCallSpec", + "SemanticDiagnostic", + "SemanticEffectDependency", + "SemanticEffectKind", + "SemanticHandOverTarget", + "SemanticIntegrationManifest", + "SemanticLowering", + "SemanticObjectTarget", + "SemanticPose", + "SemanticRelationTarget", + "SemanticSkillCompiler", + "SemanticValidationError", + "SemanticWorkflow", + "SkillPolicyPreset", + "UnsupportedSkillError", + "UnsupportedSceneAffordanceError", + "builtin_semantic_call_catalog", +] diff --git a/embodichain/lab/sim/skills/calls.py b/embodichain/lab/sim/skills/calls.py new file mode 100644 index 000000000..71301fe71 --- /dev/null +++ b/embodichain/lab/sim/skills/calls.py @@ -0,0 +1,822 @@ +# ---------------------------------------------------------------------------- +# 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 +import math +import re +from types import MappingProxyType +from typing import ClassVar, TypeAlias + +import torch + +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 + w, x, y, z = quaternion.unbind(dim=-1) + output = torch.zeros( + quaternion.shape[0], + 4, + 4, + dtype=quaternion.dtype, + device=quaternion.device, + ) + output[:, 0, 0] = 1.0 - 2.0 * (y * y + z * z) + output[:, 0, 1] = 2.0 * (x * y - z * w) + output[:, 0, 2] = 2.0 * (x * z + y * w) + output[:, 1, 0] = 2.0 * (x * y + z * w) + output[:, 1, 1] = 1.0 - 2.0 * (x * x + z * z) + output[:, 1, 2] = 2.0 * (y * z - x * w) + output[:, 2, 0] = 2.0 * (x * z - y * w) + output[:, 2, 1] = 2.0 * (y * z + x * w) + output[:, 2, 2] = 1.0 - 2.0 * (x * x + y * y) + output[:, :3, 3] = position + output[:, 3, 3] = 1.0 + return output[0] if was_unbatched else output + + +@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 + + +@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. + receiver: Optional destination resource ID. It is equivalent to the + ``destination`` resource slot and must agree with an explicit map. + final_target: Optional final object-space delivery pose. + resources: Optional skill-local resource overrides. + """ + + call_kind: ClassVar[str] = "hand_over" + + object: SceneObjectRef + receiver: str | None = None + 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.") + resources = dict(self.resources) + if self.receiver is not None: + _validate_identifier(self.receiver, field_name="HandOver.receiver") + selected = resources.get("destination") + if selected is not None and selected != self.receiver: + raise ValueError( + "HandOver.receiver conflicts with resources['destination']." + ) + resources["destination"] = self.receiver + object.__setattr__(self, "resources", _snapshot_resources(resources)) + 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(), + ) + + +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. + skill_id: Atomic skill identifier installed separately on an engine. + binding_contract: Robot-independent resource requirements. + 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] + skill_id: str + binding_contract: SkillBindingContract + schema_version: int = 1 + target_descriptor: SkillDescriptor | None = None + + def __post_init__(self) -> None: + _validate_identifier(self.call_id, field_name="SemanticCallDescriptor.call_id") + _validate_identifier( + self.skill_id, field_name="SemanticCallDescriptor.skill_id" + ) + if self.spec_type not in (Pick, Place, HandOver, RegisteredSemanticCall): + raise TypeError( + "spec_type must be exactly Pick, Place, HandOver, or " + "RegisteredSemanticCall; extensions use the registered payload " + "contract rather than executable call subclasses." + ) + _validate_static_binding_contract( + self.binding_contract, + field_name="SemanticCallDescriptor.binding_contract", + ) + 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.skill_id != expected.skill_id + or (self.binding_contract != expected.binding_contract) + or ( + 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 ( + self.target_descriptor.skill_id != self.skill_id + or self.target_descriptor.binding_contract != self.binding_contract + or not self.target_descriptor.agent_visible + or self.target_descriptor.binding_contract is None + ): + raise ValueError( + "Registered target_descriptor must be agent-visible and match " + "skill_id plus binding_contract exactly." + ) + if self.spec_type is RegisteredSemanticCall and self.call_id in { + Pick.call_kind, + Place.call_kind, + HandOver.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) + + +@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, 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.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(), + } + 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, + skill_id=_builtin_call_target(spec_type).skill_id, + binding_contract=_builtin_call_target(spec_type).binding_contract, + ) + for spec_type in (Pick, Place, HandOver) + ) + return SemanticCallCatalog(descriptors) + + +__all__ = [ + "DeclarativeValue", + "HandOver", + "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..9d3914736 --- /dev/null +++ b/embodichain/lab/sim/skills/compiler.py @@ -0,0 +1,1446 @@ +# ---------------------------------------------------------------------------- +# 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 dataclasses import dataclass, field +from enum import Enum +from types import MappingProxyType +from typing import ClassVar +from uuid import uuid4 + +import torch + +from embodichain.lab.sim.atomic_actions import ( + ActionControlOverrides, + ActionInvocation, + ActionOptions, + Affordance, + GraspGoal, + HandOverOptions, + JointPositionTarget, + HeldObjectState, + PickUpOptions, + PlaceGoal, + PlaceOptions, + PlanningContext, + PoseGoalValue, + SceneEntityPose, + SkillDescriptor, +) +from .calls import ( + HandOver, + Pick, + Place, + RegisteredSemanticCall, + SemanticCallSpec, + SemanticPose, +) +from .integration import ( + BoundSemanticCall, + BoundSemanticIntegration, + PathPart, + SemanticDiagnostic, + SemanticValidationError, +) +from .scene import ( + PLACE_IN_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, + SceneAffordanceRef, + SceneObjectRef, +) + + +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)) + + +class SemanticEffectKind(str, Enum): + """Symbolic effect boundary inferred for a semantic call.""" + + ATTACH = "attach" + RELEASE = "release" + TRANSFER = "transfer" + REGISTERED = "registered" + + +@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. + """ + + +@dataclass(frozen=True, slots=True) +class SemanticObjectTarget: + """One object-space look-ahead target. + + Exactly one source is set. 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. + """ + + pose: SemanticPose | SceneEntityPose | None = None + relation: SemanticRelationTarget | None = None + handover: SemanticHandOverTarget | None = None + + def __post_init__(self) -> None: + selected = sum( + value is not None for value in (self.pose, self.relation, self.handover) + ) + if selected != 1: + raise ValueError( + "SemanticObjectTarget requires exactly one of pose, relation, " + "or handover." + ) + if self.pose is not None: + if type(self.pose) is SemanticPose: + object.__setattr__(self, "pose", self.pose.snapshot()) + elif type(self.pose) is SceneEntityPose: + object.__setattr__(self, "pose", self.pose.snapshot()) + else: + raise TypeError( + "pose must be exactly SemanticPose, SceneEntityPose, or None." + ) + if self.relation is not None and ( + type(self.relation) is not SemanticRelationTarget + ): + raise TypeError("relation must be exactly SemanticRelationTarget or None.") + if self.handover is not None and ( + type(self.handover) is not SemanticHandOverTarget + ): + raise TypeError("handover must be exactly SemanticHandOverTarget or 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 + 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.") + 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, init=False) +class SemanticWorkflow: + """Factory-owned immutable result of static workflow analysis.""" + + workflow_id: str + calls: tuple[AnalyzedSemanticCall, ...] + effect_dependencies: tuple[SemanticEffectDependency, ...] = () + engine_owner_id: str = field(repr=False, compare=False, default="") + skill_catalog_revision: int = field(repr=False, compare=False, default=0) + compiler_id: str = field(repr=False, compare=False, default="") + + def __init__(self, *args: object, **kwargs: object) -> None: + """Reject construction outside :class:`SemanticSkillCompiler`.""" + del args, kwargs + raise TypeError( + "SemanticWorkflow values are created by " "SemanticSkillCompiler.analyze()." + ) + + @classmethod + def _create( + cls, + *, + workflow_id: str, + calls: tuple[AnalyzedSemanticCall, ...], + effect_dependencies: tuple[SemanticEffectDependency, ...], + engine_owner_id: str, + skill_catalog_revision: int, + compiler_id: str, + ) -> SemanticWorkflow: + """Create one owned workflow after compiler analysis.""" + instance = object.__new__(cls) + object.__setattr__(instance, "workflow_id", workflow_id) + object.__setattr__(instance, "calls", calls) + object.__setattr__(instance, "effect_dependencies", effect_dependencies) + object.__setattr__(instance, "engine_owner_id", engine_owner_id) + object.__setattr__( + instance, + "skill_catalog_revision", + skill_catalog_revision, + ) + object.__setattr__(instance, "compiler_id", compiler_id) + instance.__post_init__() + return instance + + 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.engine_owner_id, field_name="engine_owner_id") + _validate_identifier(self.compiler_id, field_name="compiler_id") + if type(self.skill_catalog_revision) is not int or ( + self.skill_catalog_revision < 0 + ): + raise ValueError("skill_catalog_revision must be non-negative.") + 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: + goal_kind = getattr(type(self.goal), "goal_kind", None) + if type(goal_kind) is not str or not goal_kind: + raise TypeError("goal must implement the typed ActionGoal protocol.") + 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] + target_descriptor: ClassVar[SkillDescriptor] + + @abstractmethod + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> SemanticLowering: + """Lower one registered value to goal/options without changing policy.""" + + +@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. + """ + + +@dataclass(frozen=True, slots=True, init=False) +class GroundedSemanticCall: + """Factory-owned call lowered from the latest observed context.""" + + analyzed: AnalyzedSemanticCall + invocation: ActionInvocation + _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, + 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, "_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 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.") + + @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] = (), + ) -> 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. + """ + 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}." + ) + target_descriptor = getattr(type(lowerer), "target_descriptor", None) + if type(target_descriptor) is not SkillDescriptor or ( + target_descriptor != descriptor.target_descriptor + ): + raise ValueError( + f"Lowerer {call_id!r} target_descriptor must exactly match " + "the registered catalog target." + ) + 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) + + @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 + + 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, 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] = [] + 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), + ) + bound_calls.append( + self._integration.link_call( + call, + path=(*path, index, "call"), + ) + ) + for index, bound in enumerate(bound_calls): + 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_registration = self._integration.scene_registry.lookup( + target.relation.affordance, + expected_type=SceneAffordanceRef, + ) + if destination_registration.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"), + ) + + dependencies: list[SemanticEffectDependency] = [] + latest_holder: dict[str, tuple[int, str]] = {} + analyzed: list[AnalyzedSemanticCall] = [] + for index, bound in enumerate(bound_calls): + call = bound.linked.call + requires_held = type(call) in (Place, HandOver) + 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"], + ) + 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() + downstream_targets = ( + self._downstream_targets(index, bound_calls) + if type(call) is Pick + else () + ) + analyzed.append( + AnalyzedSemanticCall( + index=index, + bound=bound, + effect_kind=effect_kind, + downstream_object_targets=downstream_targets, + requires_verified_held_object=requires_held, + ) + ) + return SemanticWorkflow._create( + workflow_id=workflow_id, + calls=tuple(analyzed), + effect_dependencies=tuple(dependencies), + engine_owner_id=self._integration.engine.binding_owner_id, + skill_catalog_revision=self._integration.engine.skill_catalog_revision, + compiler_id=self._compiler_id, + ) + + 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: Factory-owned 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: + Compiler-owned 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._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 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 + invocation = ActionInvocation( + skill_id=bound.linked.descriptor.skill_id, + goal=lowering.goal, + binding=bound.binding.action_binding, + motion_policy=bound.preset.motion_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, + ) + return GroundedSemanticCall._create( + analyzed=analyzed, + invocation=invocation, + 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")) + engine = self._integration.engine + if workflow.engine_owner_id != engine.binding_owner_id: + raise _diagnostic( + "semantic_workflow_owner_mismatch", + path, + "The workflow belongs to a different action engine.", + ) + if workflow.compiler_id != self._compiler_id: + raise _diagnostic( + "semantic_program_stale", + path, + "The workflow belongs to a different compiler/grounder registry.", + ) + if workflow.skill_catalog_revision != engine.skill_catalog_revision: + raise _diagnostic( + "semantic_catalog_stale", + path, + "The installed semantic skill catalog changed after workflow " + "analysis.", + ) + + @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 _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( + handover=SemanticHandOverTarget( + provider_id=provider_id, + bound=bound, + ) + ) + ) + break + if type(call) is Place: + if call.at is not None: + targets.append(SemanticObjectTarget(pose=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, + ) + return SemanticLowering( + goal=GraspGoal(semantics=semantics), + skill_options=PickUpOptions( + 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 + control_part, held = self._require_held_object( + analyzed, + context, + eligible, + slot_id="primary", + path=(*path, analyzed.index, "call", "object"), + ) + del control_part + 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=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", "object"), + ) + _, 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(pose=call.final_target) + if call.final_target is not None + else targets.final + ) + final = self._ground_object_target(final_target, context) + return SemanticLowering( + goal=GraspGoal(semantics=semantics), + skill_options=HandOverOptions( + middle_object_pose=middle, + final_object_pose=final, + ), + ) + + 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), + ) + lowering = lowerer.lower( + call, + context=context, + bound=analyzed.bound, + ) + if type(lowering) is not SemanticLowering: + raise TypeError( + "RegisteredSemanticLowerer.lower() must return exactly " + "SemanticLowering." + ) + descriptor = analyzed.bound.linked.descriptor + target = descriptor.target_descriptor + assert target is not None + 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 and ( + type(lowering.skill_options) is not target.options_type + ): + raise TypeError( + f"Lowerer {call.call_id!r} produced incompatible skill options." + ) + return lowering + + 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.") + registration = self._integration.scene_registry.lookup( + affordance_ref, + expected_type=SceneAffordanceRef, + ) + if registration.affordance is None or registration.affordance_revision is None: + raise AssertionError( + "Capability-bearing relation affordance lacks payload metadata." + ) + return SemanticObjectTarget( + relation=SemanticRelationTarget( + capability=capability, + affordance=affordance_ref, + payload_type=type(registration.affordance), + payload_revision=registration.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 motion control part and verify its held-object identity.""" + endpoint = analyzed.bound.binding.action_binding.endpoint(slot_id, "motion") + try: + target = endpoint.require_target(JointPositionTarget) + except TypeError as exc: + raise _diagnostic( + "unsupported_builtin_endpoint", + (*path, "resources", slot_id, "motion"), + "The current built-in semantic lowerer requires a joint-position " + "motion endpoint.", + ) from exc + held = context.task.get_held_object(target.control_part) + 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, + f"Call requires verified object {call_object.entity_id!r} held by " + f"{target.control_part!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, + f"Object {call_object.entity_id!r} is not verified as held in " + "every eligible environment.", + missing_env_ids, + ) + return target.control_part, 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() + + +__all__ = [ + "AnalyzedSemanticCall", + "GroundedSemanticCall", + "HandOverPoseProvider", + "HandOverPoseTargets", + "RelationTargetGrounder", + "RegisteredSemanticLowerer", + "SemanticEffectDependency", + "SemanticEffectKind", + "SemanticHandOverTarget", + "SemanticLowering", + "SemanticObjectTarget", + "SemanticRelationTarget", + "SemanticSkillCompiler", + "SemanticWorkflow", +] diff --git a/embodichain/lab/sim/skills/integration.py b/embodichain/lab/sim/skills/integration.py new file mode 100644 index 000000000..8efc8e046 --- /dev/null +++ b/embodichain/lab/sim/skills/integration.py @@ -0,0 +1,1261 @@ +# ---------------------------------------------------------------------------- +# 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 ( + Affordance, + AtomicActionEngine, + DisjointResourceSlots, + DisjointSlotEndpoints, + SkillResourceSlot, +) + +from .calls import ( + HandOver, + Pick, + Place, + RegisteredSemanticCall, + SemanticCallCatalog, + SemanticCallDescriptor, + SemanticCallSpec, +) +from .profiles import ( + BoundRobotSkillProfile, + ControlPartEndpoint, + ResolvedSkillBinding, + ResourceEndpoint, + ResourceEndpointAdapter, + RobotResource, + RobotSkillProfile, + SkillPolicyPreset, +) +from .scene import ( + GRASP_AFFORDANCE_CAPABILITY, + PLACE_IN_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, + SceneAffordanceRef, + SceneArticulationRef, + SceneCollisionRole, + SceneDynamics, + SceneEntityMetadata, + SceneEntityRef, + SceneEntityRegistration, + SceneLinkRef, + SceneObjectRef, + SceneRegistry, +) + +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: + """Provider-free static scene-entity declaration. + + Args: + ref: Canonical typed entity reference. + aliases: Boundary aliases accepted during static linking. + 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 classification. + affordance_capabilities: Semantic operations supplied by an affordance. + default_affordances: Capability-scoped direct-child defaults. + affordance_payload_type: Exact registered affordance payload type. + affordance_revision: Stable payload revision or fingerprint. + relative_pose: Flattened parent-relative homogeneous transform. + """ + + 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: + if not isinstance(self.ref, SceneEntityRef): + raise TypeError("SceneEntityManifest.ref must be a SceneEntityRef.") + if isinstance(self.aliases, (str, bytes)): + raise TypeError("aliases must be an iterable of identifiers.") + aliases = tuple(self.aliases) + for alias in aliases: + _validate_identifier(alias, field_name="scene aliases") + aliases = tuple(alias for alias in aliases if alias != self.ref.entity_id) + if len(set(aliases)) != len(aliases): + raise ValueError("SceneEntityManifest.aliases must be unique.") + object.__setattr__(self, "aliases", aliases) + if self.parent is not None and not isinstance(self.parent, SceneEntityRef): + raise TypeError("parent must be a SceneEntityRef or None.") + if self.semantic_type is not None: + _validate_identifier(self.semantic_type, field_name="semantic_type") + if isinstance(self.affordance_capabilities, (str, bytes)): + raise TypeError("affordance_capabilities must be an iterable.") + capabilities = frozenset(self.affordance_capabilities) + for capability in capabilities: + _validate_identifier(capability, field_name="affordance capabilities") + object.__setattr__(self, "affordance_capabilities", capabilities) + if not isinstance(self.default_affordances, Mapping): + raise TypeError("default_affordances must be a mapping.") + defaults: dict[str, SceneAffordanceRef] = {} + for capability, affordance in self.default_affordances.items(): + _validate_identifier(capability, field_name="default capabilities") + if type(affordance) is not SceneAffordanceRef: + raise TypeError( + "default_affordances values must be SceneAffordanceRef values." + ) + defaults[capability] = affordance + object.__setattr__( + self, + "default_affordances", + MappingProxyType(defaults), + ) + metadata = SceneEntityMetadata( + ref=self.ref, + aliases=self.aliases, + parent=self.parent, + native_name=self.native_name, + dynamics=self.dynamics, + collision_role=self.collision_role, + semantic_type=self.semantic_type, + affordance_capabilities=self.affordance_capabilities, + default_affordances=self.default_affordances, + affordance_payload_type=self.affordance_payload_type, + affordance_revision=self.affordance_revision, + relative_pose=self.relative_pose, + ) + object.__setattr__(self, "aliases", metadata.aliases) + object.__setattr__(self, "default_affordances", metadata.default_affordances) + object.__setattr__(self, "relative_pose", metadata.relative_pose) + + @classmethod + def from_registration( + cls, + registration: SceneEntityRegistration, + ) -> SceneEntityManifest: + """Project live registration metadata without reading providers.""" + if not isinstance(registration, SceneEntityRegistration): + raise TypeError("registration must be a SceneEntityRegistration.") + return cls.from_metadata(SceneEntityMetadata.from_registration(registration)) + + @classmethod + def from_metadata(cls, metadata: SceneEntityMetadata) -> SceneEntityManifest: + """Copy one provider-free registry 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, ...] + _by_id: Mapping[str, SceneEntityManifest] + _aliases: Mapping[str, str] + _affordances: Mapping[tuple[str, str], tuple[SceneAffordanceRef, ...]] + + def __init__(self, entries: Iterable[SceneEntityManifest] = ()) -> 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.") + by_id: dict[str, SceneEntityManifest] = {} + for entry in supplied: + if entry.ref.entity_id in by_id: + raise ValueError( + f"Duplicate scene manifest ID {entry.ref.entity_id!r}." + ) + by_id[entry.ref.entity_id] = entry + aliases: dict[str, str] = {} + for entry in supplied: + for alias in entry.aliases: + if alias in by_id: + raise ValueError( + f"Scene manifest alias {alias!r} collides with a canonical ID." + ) + previous = aliases.get(alias) + if previous is not None: + raise ValueError( + f"Scene manifest alias {alias!r} is ambiguous between " + f"{previous!r} and {entry.ref.entity_id!r}." + ) + aliases[alias] = entry.ref.entity_id + affordances: dict[tuple[str, str], list[SceneAffordanceRef]] = {} + native_members: dict[tuple[type[SceneEntityRef], str, str], str] = {} + for entry in supplied: + if entry.parent is not None: + parent_entry = by_id.get(entry.parent.entity_id) + if parent_entry is None: + raise ValueError( + f"Scene manifest entity {entry.ref.entity_id!r} references " + f"unknown parent {entry.parent.entity_id!r}." + ) + if type(parent_entry.ref) is not type(entry.parent): + raise TypeError( + f"Scene manifest parent {entry.parent.entity_id!r} has " + "the wrong reference type." + ) + if entry.native_name is not None and isinstance( + entry.ref, (SceneLinkRef, SceneAffordanceRef) + ): + native_key = ( + type(entry.ref), + entry.parent.entity_id, + entry.native_name, + ) + previous = native_members.get(native_key) + if previous is not None: + raise ValueError( + f"Scene manifest parent {entry.parent.entity_id!r} " + f"and native_name {entry.native_name!r} are already " + f"registered as {previous!r}." + ) + native_members[native_key] = entry.ref.entity_id + if isinstance(entry.ref, SceneAffordanceRef): + if entry.parent is None: + raise ValueError( + f"Affordance {entry.ref.entity_id!r} requires a parent." + ) + for capability in entry.affordance_capabilities: + affordances.setdefault( + (entry.parent.entity_id, capability), [] + ).append(entry.ref) + elif entry.affordance_capabilities: + raise ValueError( + "Only SceneAffordanceRef entries may declare " + "affordance_capabilities." + ) + for entry in supplied: + if isinstance(entry.ref, SceneAffordanceRef) and entry.default_affordances: + raise ValueError( + "Scene affordance entries cannot declare default_affordances." + ) + for capability, default in entry.default_affordances.items(): + default_entry = by_id.get(default.entity_id) + if default_entry is None or not isinstance( + default_entry.ref, SceneAffordanceRef + ): + raise ValueError( + f"Default affordance {default.entity_id!r} is not a " + "registered affordance entry." + ) + if default_entry.parent != entry.ref: + raise ValueError( + f"Default affordance {default.entity_id!r} is not a direct " + f"child of {entry.ref.entity_id!r}." + ) + if capability not in default_entry.affordance_capabilities: + raise ValueError( + f"Default affordance {default.entity_id!r} does not support " + f"capability {capability!r}." + ) + object.__setattr__(self, "_entries", supplied) + object.__setattr__(self, "_by_id", MappingProxyType(by_id)) + object.__setattr__(self, "_aliases", MappingProxyType(aliases)) + object.__setattr__( + self, + "_affordances", + MappingProxyType( + { + key: tuple(sorted(refs, key=lambda ref: ref.entity_id)) + for key, refs in affordances.items() + } + ), + ) + + @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 + ) + + 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._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._by_id.get(candidate_id) + if entry is None: + raise SemanticValidationError( + SemanticDiagnostic( + "unknown_entity", + path, + f"Unknown scene entity {candidate_id!r}.", + tuple(self._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._by_id[ref.entity_id] + + 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._affordances.get((parent_ref.entity_id, capability), ()) + if explicit is not None: + selected = self.resolve( + explicit, + expected_type=SceneAffordanceRef, + path=path, + ) + entry = self._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._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._by_id) + live_ids = set(live._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._by_id[entity_id] != live._by_id[entity_id]: + raise SemanticValidationError( + SemanticDiagnostic( + "scene_manifest_mismatch", + (*path, entity_id), + "Live scene metadata 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, 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.") + 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, 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"), + ) + return LinkedSemanticCall( + call=normalized_call, + descriptor=descriptor, + preset_id=preset_id, + affordances=affordances, + ) + + 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) + 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, + ) + + +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) + 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 + return BoundSemanticCall._create( + linked=linked, + binding=binding, + preset=preset, + robot_profile=self._robot_profile, + ) + + +__all__ = [ + "BoundSemanticCall", + "BoundSemanticIntegration", + "LinkedSemanticCall", + "PathPart", + "SceneEntityManifest", + "SceneManifest", + "SemanticDiagnostic", + "SemanticIntegrationManifest", + "SemanticValidationError", +] diff --git a/embodichain/lab/sim/skills/profiles.py b/embodichain/lab/sim/skills/profiles.py new file mode 100644 index 000000000..a9fe70f14 --- /dev/null +++ b/embodichain/lab/sim/skills/profiles.py @@ -0,0 +1,1893 @@ +# ---------------------------------------------------------------------------- +# 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 +from itertools import product +from types import MappingProxyType +from typing import ClassVar, Mapping, TYPE_CHECKING + +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.policies import MotionPolicy, RecoveryPolicy +from embodichain.lab.sim.atomic_actions.requirements import ( + BATCH_INVERSE_KINEMATICS_CAPABILITY, + DisjointResourceSlots, + DisjointSlotEndpoints, + FORWARD_KINEMATICS_CAPABILITY, + INVERSE_KINEMATICS_CAPABILITY, + SkillBindingContract, + SkillResourceSlot, +) +from embodichain.lab.sim.atomic_actions.runner import ExecutionRunnerCfg + +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 _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) + + +@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.""" + + 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.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.""" + + @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 + + 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." + ) + return EndpointResolution( + runtime_target=JointPositionTarget( + control_part=endpoint.control_part, + joint_ids=joint_ids, + ), + 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, + 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 + 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, + 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) + 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, init=False) +class SkillPolicyPreset: + """Versioned planning, recovery, and runner policy bundle.""" + + preset_id: str + schema_version: int + _motion_policy: MotionPolicy + _recovery_policy: RecoveryPolicy + _runner_cfg: ExecutionRunnerCfg + + def __init__( + self, + preset_id: str, + schema_version: int = 1, + motion_policy: MotionPolicy | None = None, + recovery_policy: RecoveryPolicy | None = None, + runner_cfg: ExecutionRunnerCfg | 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 != 1: + raise ValueError( + "Unsupported SkillPolicyPreset.schema_version " + f"{schema_version}; supported versions are [1]." + ) + selected_motion = MotionPolicy() if motion_policy is None else motion_policy + selected_recovery = ( + RecoveryPolicy() if recovery_policy is None else 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_recovery, RecoveryPolicy): + raise TypeError("recovery_policy must be a RecoveryPolicy.") + if not isinstance(selected_runner, ExecutionRunnerCfg): + raise TypeError("runner_cfg must be an ExecutionRunnerCfg.") + object.__setattr__(self, "preset_id", preset_id) + object.__setattr__(self, "schema_version", schema_version) + object.__setattr__(self, "_motion_policy", deepcopy(selected_motion)) + object.__setattr__(self, "_recovery_policy", deepcopy(selected_recovery)) + object.__setattr__(self, "_runner_cfg", deepcopy(selected_runner)) + + @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 runner_cfg(self) -> ExecutionRunnerCfg: + """Return an independently owned runner configuration.""" + return deepcopy(self._runner_cfg) + + 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, + recovery_policy=self.recovery_policy, + runner_cfg=self.runner_cfg, + ) + + +@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.motion_policy.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, + 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 self._resource_matches(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) + + def _resource_matches( + self, + resource: ResolvedRobotResource, + slot: SkillResourceSlot, + ) -> bool: + """Return whether one resource satisfies all slot-local endpoints.""" + matched_endpoints: dict[str, ResolvedResourceEndpoint] = {} + for requirement in slot.endpoints: + endpoint = resource.endpoints.get(requirement.endpoint_id) + if endpoint is None: + return False + if not requirement.capabilities.issubset(endpoint.capabilities): + return False + for command_name, command_type in requirement.required_commands.items(): + command = endpoint.commands.get(command_name) + if not isinstance(command, command_type): + return False + matched_endpoints[requirement.endpoint_id] = endpoint + for constraint in slot.constraints: + if isinstance(constraint, DisjointSlotEndpoints): + endpoints = [ + matched_endpoints[endpoint_id] + for endpoint_id in constraint.endpoint_ids + ] + for index, left in enumerate(endpoints): + if any( + left.conflicts_with(right) for right in endpoints[index + 1 :] + ): + return False + return True + + @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, + 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", +] diff --git a/embodichain/lab/sim/skills/scene.py b/embodichain/lab/sim/skills/scene.py new file mode 100644 index 000000000..7d613da06 --- /dev/null +++ b/embodichain/lab/sim/skills/scene.py @@ -0,0 +1,1875 @@ +# ---------------------------------------------------------------------------- +# 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 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, + EntityState, + ObjectSemantics, + SceneProvider, + SceneSnapshot, +) + +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.""" + +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.""" + + +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.""" + + +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." + ) + 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()) + ), + ) + + +@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 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. + 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.""" + + 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 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." + ) + return + + if isinstance(self.ref, SceneLinkRef): + 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 ( + 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.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) + _aliases: Mapping[str, str] = 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) + _entity_metadata: tuple[SceneEntityMetadata, ...] = field(repr=False) + _affordances_by_parent_capability: Mapping[ + tuple[str, str], tuple[SceneAffordanceRef, ...] + ] = 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) + by_id: dict[str, SceneEntityRegistration] = {} + for registration in owned: + entity_id = registration.ref.entity_id + if entity_id in by_id: + raise ValueError(f"Duplicate canonical scene entity ID {entity_id!r}.") + by_id[entity_id] = registration + + aliases: dict[str, str] = {} + canonical_ids = set(by_id) + for registration in owned: + canonical_id = registration.ref.entity_id + for alias in registration.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(owned, by_id) + affordances_by_parent_capability = self._index_affordances(owned, by_id) + entity_metadata = tuple( + SceneEntityMetadata.from_registration(item) for item in owned + ) + object.__setattr__(self, "_registrations", owned) + object.__setattr__( + self, + "_registrations_by_id", + MappingProxyType(by_id), + ) + object.__setattr__(self, "_aliases", MappingProxyType(aliases)) + 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, + "_affordances_by_parent_capability", + MappingProxyType(affordances_by_parent_capability), + ) + object.__setattr__(self, "_entity_metadata", entity_metadata) + object.__setattr__(self, "collision_world_mode", collision_world_mode) + + @staticmethod + def _validate_relationships( + registrations: tuple[SceneEntityRegistration, ...], + by_id: Mapping[str, SceneEntityRegistration], + ) -> None: + """Require every parent to be a canonical, correctly typed ref.""" + native_members: dict[tuple[type[SceneEntityRef], str, str], str] = {} + for registration in registrations: + parent = registration.parent + if parent is None: + continue + if parent.entity_id == registration.ref.entity_id: + raise ValueError( + f"Scene entity {registration.ref.entity_id!r} cannot parent itself." + ) + parent_registration = by_id.get(parent.entity_id) + if parent_registration is None: + raise ValueError( + f"Scene entity {registration.ref.entity_id!r} references " + f"unregistered parent {parent.entity_id!r}." + ) + if type(parent_registration.ref) is not type(parent): + raise TypeError( + f"Parent {parent.entity_id!r} is registered as " + f"{type(parent_registration.ref).__name__}, not " + f"{type(parent).__name__}." + ) + if isinstance(registration.ref, (SceneLinkRef, SceneAffordanceRef)): + assert registration.native_name is not None + member_key = ( + type(registration.ref), + parent.entity_id, + registration.native_name, + ) + previous = native_members.get(member_key) + if previous is not None: + raise ValueError( + f"{type(registration.ref).__name__} parent " + f"{parent.entity_id!r} and native_name " + f"{registration.native_name!r} are already registered as " + f"canonical ID {previous!r}." + ) + native_members[member_key] = registration.ref.entity_id + + for registration in registrations: + for capability, default_ref in registration.default_affordances.items(): + default_registration = by_id.get(default_ref.entity_id) + if default_registration is None: + raise ValueError( + f"Scene entity {registration.ref.entity_id!r} declares " + f"unknown default affordance {default_ref.entity_id!r} " + f"for capability {capability!r}." + ) + if not isinstance(default_registration.ref, SceneAffordanceRef): + raise TypeError( + f"Default affordance {default_ref.entity_id!r} is registered " + f"as {type(default_registration.ref).__name__}, not " + "SceneAffordanceRef." + ) + if default_registration.parent != registration.ref: + actual_parent = default_registration.parent + raise ValueError( + f"Default affordance {default_ref.entity_id!r} is not a " + f"direct child of {registration.ref.entity_id!r}; its parent " + f"is {None if actual_parent is None else actual_parent.entity_id!r}." + ) + if capability not in default_registration.affordance_capabilities: + raise ValueError( + f"Default affordance {default_ref.entity_id!r} does not " + f"declare capability {capability!r}." + ) + + @staticmethod + def _index_affordances( + registrations: tuple[SceneEntityRegistration, ...], + by_id: Mapping[str, SceneEntityRegistration], + ) -> dict[tuple[str, str], tuple[SceneAffordanceRef, ...]]: + """Build deterministic parent/capability reverse lookup entries.""" + del by_id + mutable: dict[tuple[str, str], list[SceneAffordanceRef]] = {} + for registration in registrations: + if not isinstance(registration.ref, SceneAffordanceRef): + continue + assert registration.parent is not None + for capability in registration.affordance_capabilities: + mutable.setdefault( + (registration.parent.entity_id, capability), [] + ).append(registration.ref) + return { + key: tuple(sorted(refs, key=lambda ref: ref.entity_id)) + for key, refs in mutable.items() + } + + @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._entity_metadata + + @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._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. + """ + if not isinstance(expected_type, type) or not issubclass( + expected_type, + SceneEntityRef, + ): + raise TypeError("expected_type must be a SceneEntityRef subclass.") + supplied_ref: SceneEntityRef | None + if isinstance(identifier, SceneEntityRef): + canonical_id = identifier.entity_id + supplied_ref = 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.") + + registration = self._registrations_by_id.get(canonical_id) + if registration is None: + raise KeyError(f"Unknown scene entity {identifier!r}.") + canonical_ref = registration.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 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. + """ + 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 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. + """ + parent_ref = self.resolve(parent) + _validate_identifier(capability, "affordance capability") + 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 + registration = self._registrations_by_id[selected.entity_id] + if registration.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 registration.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] + parent_registration = self._registrations_by_id[parent_ref.entity_id] + default = parent_registration.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." + ) + + 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_dynamic_ids = motion_generator.dynamic_collision_entity_ids + planner_world_ids = motion_generator.collision_world_entity_ids + supports_updates = motion_generator.supports_dynamic_collision_world + planner_mode = motion_generator.collision_world_batch_mode + except AttributeError as exc: + raise TypeError( + "motion_generator must expose collision-world integration properties." + ) 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=(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), + aliases=(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 _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._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, + ) + 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 + ) + if confidence_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._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, + ) + + 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__ = [ + "AmbiguousSceneAffordanceError", + "GRASP_AFFORDANCE_CAPABILITY", + "PLACE_IN_AFFORDANCE_CAPABILITY", + "PLACE_ON_AFFORDANCE_CAPABILITY", + "RegistrySceneProvider", + "SceneAffordanceRef", + "SceneArticulationRef", + "SceneCollisionRole", + "SceneCollisionWorldMode", + "SceneDynamics", + "SceneEntityRef", + "SceneEntityMetadata", + "SceneEntityRegistration", + "SceneEntityStateProvider", + "SceneGeometryProvider", + "SceneLinkRef", + "SceneObjectRef", + "SceneRegistry", + "UnsupportedSceneAffordanceError", +] 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..1965563b0 100644 --- a/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py +++ b/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py @@ -404,7 +404,6 @@ def _plan_pick_place_cycle( ) -> 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, @@ -416,16 +415,26 @@ def _plan_pick_place_cycle( 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"}, + 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( ( ActionInvocation( skill_id="pick_up", goal=GraspGoal(self._cube_semantics), - binding=binding, + binding=pick_binding, motion_policy=MotionPolicy(sample_count=self.PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( pre_grasp_distance=0.15, @@ -458,7 +467,7 @@ def _plan_pick_place_cycle( ActionInvocation( skill_id="place", goal=PlaceGoal(place_eef_pose), - binding=binding, + binding=place_binding, motion_policy=MotionPolicy(sample_count=self.PLACE_SAMPLE_INTERVAL), skill_options=PlaceOptions( lift_height=0.14, diff --git a/embodichain_tasks/embodichain_tasks/tableware/blocks_ranking_rgb.py b/embodichain_tasks/embodichain_tasks/tableware/blocks_ranking_rgb.py index 5e09bac89..0a5d19dc7 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, @@ -277,7 +286,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/stack_blocks_two.py b/embodichain_tasks/embodichain_tasks/tableware/stack_blocks_two.py index 9001f0c73..9279e3037 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, @@ -201,7 +210,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/planners/curobo_planner.py b/examples/sim/planners/curobo_planner.py index b3105f248..e391852b0 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,7 +748,10 @@ 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", plan_opts=CuroboPlanOptions( 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..35dbb0386 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( @@ -269,7 +272,6 @@ def _run_case( ): """Run one MoveHeldObject benchmark case.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -319,16 +321,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..f50885890 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, diff --git a/scripts/benchmark/atomic_action/place_benchmark.py b/scripts/benchmark/atomic_action/place_benchmark.py index 4c8242719..f63a7e73c 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( @@ -255,7 +254,6 @@ def _run_case( ): """Run one Place benchmark case.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -308,16 +306,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 index a5687af7f..fa534703f 100644 --- a/scripts/benchmark/atomic_action/press_benchmark.py +++ b/scripts/benchmark/atomic_action/press_benchmark.py @@ -81,7 +81,6 @@ def _ensure_runtime_imports() -> None: 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, @@ -125,7 +124,6 @@ def _ensure_runtime_imports() -> None: "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, @@ -562,27 +560,31 @@ def _timed_atomic_run( press_target: torch.Tensor, ) -> tuple[float, dict[str, float], float, bool, torch.Tensor]: """Run a timed atomic-action sequence and return timing/memory/results.""" + move_binding = atomic_engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "arm"}}, + ) + press_binding = atomic_engine.bind_control_parts( + "press", + {"primary": {"motion": "arm", "grasp": "hand"}}, + ) _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, + move_binding, MotionPolicy(sample_count=MOVE_SAMPLE_INTERVAL), ), ActionInvocation( "press", PressGoal(xpos=press_target), - binding, + press_binding, MotionPolicy(sample_count=PRESS_SAMPLE_INTERVAL), skill_options=PressOptions( hand_interp_steps=HAND_INTERP_STEPS, diff --git a/scripts/tutorials/atomic_action/assemble.py b/scripts/tutorials/atomic_action/assemble.py index ac0594ef3..00ce492da 100644 --- a/scripts/tutorials/atomic_action/assemble.py +++ b/scripts/tutorials/atomic_action/assemble.py @@ -38,7 +38,6 @@ from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AssembleAffordance, AssembleGoal, @@ -316,23 +315,28 @@ 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"}} + pick_binding = engine.bind_control_parts( + "pick_up", + endpoint_mapping, + ) + place_binding = engine.bind_control_parts( + "place", + endpoint_mapping, ) compiled = engine.compile( ( ActionInvocation( "pick_up", GraspGoal(can_semantics), - binding, + pick_binding, MotionPolicy(sample_count=PICKUP_SAMPLE_INTERVAL), skill_options=pick_up_options, ), ActionInvocation( "place", AssembleGoal(affordance=assemble_affordance), - binding, + place_binding, MotionPolicy(sample_count=PLACE_SAMPLE_INTERVAL), skill_options=place_options, ), diff --git a/scripts/tutorials/atomic_action/coordinated_pickment.py b/scripts/tutorials/atomic_action/coordinated_pickment.py index b3eeb15b7..a81fc81c0 100644 --- a/scripts/tutorials/atomic_action/coordinated_pickment.py +++ b/scripts/tutorials/atomic_action/coordinated_pickment.py @@ -36,7 +36,6 @@ from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -412,15 +411,19 @@ def run_coordinated_pickment_demo( ) start_time = time.time() + binding = engine.bind_control_parts( + "coordinated_pickment", + { + "left": {"motion": "left_arm", "grasp": "left_hand"}, + "right": {"motion": "right_arm", "grasp": "right_hand"}, + }, + ) compiled = engine.compile( ( ActionInvocation( "coordinated_pickment", pickment_target, - ActionBinding( - manipulators={"left": "left_arm", "right": "right_arm"}, - end_effectors={"left": "left_hand", "right": "right_hand"}, - ), + binding, MotionPolicy(sample_count=PICKMENT_SAMPLE_INTERVAL), skill_options=pickment_options, ), diff --git a/scripts/tutorials/atomic_action/coordinated_placement.py b/scripts/tutorials/atomic_action/coordinated_placement.py index b715cefda..7f3d69f07 100644 --- a/scripts/tutorials/atomic_action/coordinated_placement.py +++ b/scripts/tutorials/atomic_action/coordinated_placement.py @@ -38,7 +38,6 @@ from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -620,6 +619,14 @@ def run_coordinated_placement_demo( sim.device, z_clearance=PAN_GRASP_Z_CLEARANCE, ) + left_pick_binding = engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "left_arm", "grasp": "left_hand"}}, + ) + right_pick_binding = engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "right_arm", "grasp": "right_hand"}}, + ) pick_invocations = ( ActionInvocation( skill_id="pick_up", @@ -627,10 +634,7 @@ def run_coordinated_placement_demo( semantics=bread_semantics, grasp_xpos=broadcast_pose_batch(bread_grasp_pose, num_envs=n_envs), ), - binding=ActionBinding( - manipulators={"primary": "left_arm"}, - end_effectors={"primary": "left_hand"}, - ), + binding=left_pick_binding, motion_policy=MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=left_pick_options, ), @@ -640,10 +644,7 @@ def run_coordinated_placement_demo( semantics=pan_semantics, grasp_xpos=broadcast_pose_batch(pan_grasp_pose, num_envs=n_envs), ), - binding=ActionBinding( - manipulators={"primary": "right_arm"}, - end_effectors={"primary": "right_hand"}, - ), + binding=right_pick_binding, motion_policy=MotionPolicy(sample_count=PAN_PICK_SAMPLE_INTERVAL), skill_options=right_pick_options, ), @@ -663,8 +664,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 +695,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 +708,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, @@ -785,21 +790,19 @@ def log_trajectory_execution(step_idx: int, total_steps: int) -> None: release=True, ) start_time = time.time() + placement_binding = engine.bind_control_parts( + "coordinated_placement", + { + "placing": {"motion": "left_arm", "grasp": "left_hand"}, + "support": {"motion": "right_arm", "grasp": "right_hand"}, + }, + ) 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", - }, - ), + binding=placement_binding, motion_policy=MotionPolicy(sample_count=COORDINATED_SAMPLE_INTERVAL), skill_options=coordinated_options, ), diff --git a/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py b/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py index fc679530d..03ab89f29 100644 --- a/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py +++ b/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py @@ -33,13 +33,14 @@ 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 +48,7 @@ RunnerStep, SimulationExecutionAdapter, TaskState, + TimedCommandSequence, ) from embodichain.lab.sim.cfg import RigidBodyAttributesCfg from embodichain.lab.sim.objects import RigidObject, RigidObjectCfg, Robot @@ -294,21 +296,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, ) @@ -457,10 +473,14 @@ def main() -> None: device=target_pose.device, ) engine = AtomicActionEngine(motion_generator=motion_gen) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": CONTROL_PART}}, + ) invocation = ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(target_pose), - binding=ActionBinding(manipulators={"primary": CONTROL_PART}), + binding=binding, motion_policy=MotionPolicy( strategy="motion_gen", sample_count=SAMPLE_COUNT, @@ -475,9 +495,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( @@ -586,9 +606,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..042de17be 100644 --- a/scripts/tutorials/atomic_action/hand_over.py +++ b/scripts/tutorials/atomic_action/hand_over.py @@ -35,7 +35,6 @@ from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, GraspGoal, AtomicActionEngine, @@ -258,31 +257,30 @@ def run_handover_demo( # wait for object to drop for _ in range(20): sim.update(step=10) + pick_binding = engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "left_arm", "grasp": "left_hand"}}, + ) + handover_binding = engine.bind_control_parts( + "hand_over", + { + "source": {"motion": "left_arm", "grasp": "left_hand"}, + "destination": {"motion": "right_arm", "grasp": "right_hand"}, + }, + ) compiled = engine.compile( ( ActionInvocation( "pick_up", GraspGoal(object_semantics), - ActionBinding( - manipulators={"primary": "left_arm"}, - end_effectors={"primary": "left_hand"}, - ), + pick_binding, MotionPolicy(sample_count=PICKUP_SAMPLE_INTERVAL), skill_options=pick_up_options, ), ActionInvocation( "hand_over", GraspGoal(object_semantics), - ActionBinding( - manipulators={ - "source": "left_arm", - "destination": "right_arm", - }, - end_effectors={ - "source": "left_hand", - "destination": "right_hand", - }, - ), + handover_binding, MotionPolicy(sample_count=HANDOVER_SAMPLE_INTERVAL), skill_options=handover_options, ), diff --git a/scripts/tutorials/atomic_action/move_end_effector.py b/scripts/tutorials/atomic_action/move_end_effector.py index f46dbe250..b6d203ebc 100644 --- a/scripts/tutorials/atomic_action/move_end_effector.py +++ b/scripts/tutorials/atomic_action/move_end_effector.py @@ -29,7 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, @@ -97,7 +96,10 @@ def main() -> None: ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(broadcast_waypoint_pose_batch(poses, n_envs)), - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "arm"}}, + ), motion_policy=MotionPolicy(sample_count=MOVE_SAMPLE_INTERVAL), ), ) diff --git a/scripts/tutorials/atomic_action/move_held_object.py b/scripts/tutorials/atomic_action/move_held_object.py index f320b6118..bff059972 100644 --- a/scripts/tutorials/atomic_action/move_held_object.py +++ b/scripts/tutorials/atomic_action/move_held_object.py @@ -30,7 +30,6 @@ from embodichain.data import get_data_path from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -148,22 +147,32 @@ def main() -> None: 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"}} + move_binding = engine.bind_control_parts( + "move_end_effector", + motion_mapping, + ) + pick_binding = engine.bind_control_parts( + "pick_up", + manipulation_mapping, + ) + held_object_binding = engine.bind_control_parts( + "move_held_object", + manipulation_mapping, ) compiled = engine.compile( ( ActionInvocation( "move_end_effector", EndEffectorPoseGoal(move_target), - binding, + move_binding, MotionPolicy(sample_count=MOVE_SAMPLE_INTERVAL), ), ActionInvocation( "pick_up", GraspGoal(semantics), - binding, + pick_binding, MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( pre_grasp_distance=0.15, @@ -174,7 +183,7 @@ def main() -> None: ActionInvocation( "move_held_object", HeldObjectPoseGoal(object_target), - binding, + held_object_binding, MotionPolicy(sample_count=MOVE_HELD_OBJECT_SAMPLE_INTERVAL), ), ) diff --git a/scripts/tutorials/atomic_action/move_joints.py b/scripts/tutorials/atomic_action/move_joints.py index 7fbee6948..4bc32ea7e 100644 --- a/scripts/tutorials/atomic_action/move_joints.py +++ b/scripts/tutorials/atomic_action/move_joints.py @@ -29,7 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -95,7 +94,10 @@ def main() -> None: waypoints = ( torch.stack([mid, home]).unsqueeze(0).repeat(robot.get_qpos().shape[0], 1, 1) ) - binding = ActionBinding(manipulators={"primary": "arm"}) + binding = engine.bind_control_parts( + "move_joints", + {"primary": {"motion": "arm"}}, + ) policy = MotionPolicy(sample_count=MOVE_JOINTS_SAMPLE_INTERVAL) compiled = engine.compile( ( diff --git a/scripts/tutorials/atomic_action/moving_target_recovery.py b/scripts/tutorials/atomic_action/moving_target_recovery.py index a9053453d..db9da7b13 100644 --- a/scripts/tutorials/atomic_action/moving_target_recovery.py +++ b/scripts/tutorials/atomic_action/moving_target_recovery.py @@ -31,7 +31,6 @@ from embodichain.lab.sim import SimulationManager, VisualMaterialCfg from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, Affordance, AtomicActionEngine, @@ -275,10 +274,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,6 +285,10 @@ def main() -> None: ) }, ) + binding = engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "arm", "grasp": "hand"}}, + ) pick_invocation = ActionInvocation( skill_id="pick_up", goal=GraspGoal( diff --git a/scripts/tutorials/atomic_action/pickup.py b/scripts/tutorials/atomic_action/pickup.py index a9c361671..325d36b74 100644 --- a/scripts/tutorials/atomic_action/pickup.py +++ b/scripts/tutorials/atomic_action/pickup.py @@ -29,7 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -158,9 +157,9 @@ def main() -> None: ActionInvocation( skill_id="pick_up", goal=GraspGoal(semantics), - binding=ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, + binding=engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "arm", "grasp": "hand"}}, ), motion_policy=MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( diff --git a/scripts/tutorials/atomic_action/place.py b/scripts/tutorials/atomic_action/place.py index ae30526ad..0b7fc74c1 100644 --- a/scripts/tutorials/atomic_action/place.py +++ b/scripts/tutorials/atomic_action/place.py @@ -29,7 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -156,16 +155,21 @@ 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"}} + pick_binding = engine.bind_control_parts( + "pick_up", + endpoint_mapping, + ) + place_binding = engine.bind_control_parts( + "place", + endpoint_mapping, ) compiled = engine.compile( ( ActionInvocation( "pick_up", GraspGoal(semantics), - binding, + pick_binding, MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( pre_grasp_distance=0.15, @@ -180,7 +184,7 @@ def main() -> None: place_poses, robot.get_qpos().shape[0] ) ), - binding, + place_binding, MotionPolicy(sample_count=PLACE_SAMPLE_INTERVAL), skill_options=PlaceOptions( lift_height=PLACE_LIFT_HEIGHT, diff --git a/scripts/tutorials/atomic_action/press.py b/scripts/tutorials/atomic_action/press.py index 59a4e5b6e..b21ef10d7 100644 --- a/scripts/tutorials/atomic_action/press.py +++ b/scripts/tutorials/atomic_action/press.py @@ -29,7 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -183,22 +182,26 @@ def main() -> None: sim, args, "Inspect the wooden block, then press Enter to plan..." ) - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, + move_binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "arm"}}, + ) + press_binding = engine.bind_control_parts( + "press", + {"primary": {"motion": "arm", "grasp": "hand"}}, ) compiled = engine.compile( ( ActionInvocation( "move_end_effector", EndEffectorPoseGoal(move_target), - binding, + move_binding, MotionPolicy(sample_count=MOVE_SAMPLE_INTERVAL), ), ActionInvocation( "press", PressGoal(press_target), - binding, + press_binding, MotionPolicy(sample_count=PRESS_SAMPLE_INTERVAL), skill_options=PressOptions( hand_interp_steps=HAND_INTERP_STEPS, diff --git a/tests/sim/atomic_actions/test_actions.py b/tests/sim/atomic_actions/test_actions.py index 06703fae5..3cfa32029 100644 --- a/tests/sim/atomic_actions/test_actions.py +++ b/tests/sim/atomic_actions/test_actions.py @@ -27,8 +27,10 @@ from embodichain.lab.sim.atomic_actions import ( ActionBinding, ActionInvocation, + ActionPlan, Affordance, AntipodalAffordance, + AssembleAffordance, AssembleGoal, AtomicAction, AtomicActionEngine, @@ -49,6 +51,8 @@ HeldObjectPoseGoal, HeldObjectState, JointPositionGoal, + JointPositionPayload, + JointPositionTarget, MotionPolicy, MoveEndEffector, MoveEndEffectorOptions, @@ -70,6 +74,7 @@ SceneEntityPose, SceneSnapshot, TaskState, + TimedTrajectory, ) from embodichain.lab.sim.planners import ( MotionGenerator, @@ -77,6 +82,7 @@ PlanOptions, PlanResult, ) +from embodichain.utils.math import pose_inv NUM_ENVS = 2 ARM_DOF = 6 @@ -86,6 +92,7 @@ DUAL_ROBOT_DOF = DUAL_ARM_DOF + 2 * HAND_DOF ActionT = TypeVar("ActionT", bound=AtomicAction) +_ACTION_ENGINES: dict[int, AtomicActionEngine] = {} @pytest.fixture(autouse=True) @@ -203,6 +210,7 @@ def _bind_action( load_builtins=False, ) engine.register(action) + _ACTION_ENGINES[id(action)] = engine return action @@ -248,28 +256,80 @@ def _target_scene( ) -def _binding() -> ActionBinding: - return ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, +def _binding( + action: AtomicAction, + *, + motion: str = "arm", + grasp: str = "hand", +) -> 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} + 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 + }, ) 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: +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() entity.get_local_pose.return_value = torch.eye(4).repeat(NUM_ENVS, 1, 1) return ObjectSemantics( @@ -277,6 +337,7 @@ def _semantics() -> ObjectSemantics: geometry={}, label="test_object", entity=entity, + entity_id=entity_id, ) @@ -362,28 +423,36 @@ 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), ) def _dual_binding( - first_role: str, - second_role: str, + action: AtomicAction, + first_slot: str, + second_slot: str, ) -> 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", + }, }, ) @@ -435,6 +504,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 +558,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 +566,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 @@ -502,24 +593,26 @@ def test_move_joints_uses_binding_and_preserves_uncontrolled_joints() -> None: 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)), + _invocation(pick, GraspGoal(semantics=semantics, grasp_xpos=grasp)), initial, ) picked_task = pick_plan.expected_effects.apply(initial.task, pick_plan.plan_success) @@ -536,7 +629,7 @@ def test_pick_and_place_declare_effects_without_mutating_context() -> None: ) place_plan = _plan_action( place, - _invocation("place", PlaceGoal(torch.eye(4))), + _invocation(place, PlaceGoal(torch.eye(4))), picked_context, ) placed_task = place_plan.expected_effects.apply( @@ -551,7 +644,7 @@ 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, ) @@ -559,15 +652,43 @@ def test_move_held_object_requires_projected_attachment() -> None: 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}, ) - 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), + 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 + 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: @@ -576,12 +697,12 @@ def test_press_uses_invocation_sample_budget() -> None: plan = _plan_action( action, - _invocation("press", PressGoal(torch.eye(4)), sample_count=12), + _invocation(action, PressGoal(torch.eye(4)), sample_count=12), _context(), ) assert plan.plan_success.tolist() == [True, True] - assert plan.trajectory.waypoint_count == 12 + assert plan.commands.frame_count == 12 assert plan.expected_effects.is_empty @@ -598,7 +719,7 @@ 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) @@ -622,16 +743,19 @@ def test_planner_timing_is_preserved_in_simple_action() -> None: 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 +780,7 @@ def compute_ik( plan = _plan_action( action, _invocation( - "move_end_effector", + action, EndEffectorPoseGoal(waypoints), sample_count=9, ), @@ -689,19 +813,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,32 +836,41 @@ 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", @@ -754,6 +888,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 +897,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 +938,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 +946,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( @@ -838,6 +982,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 +994,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"), @@ -882,16 +1029,27 @@ def test_pick_uses_binding_control_part_as_effect_resource() -> None: 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", ), 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 @@ -913,12 +1071,12 @@ def test_press_closes_hand_without_changing_projected_attachment() -> None: plan = _plan_action( action, - _invocation("press", PressGoal(torch.eye(4)), sample_count=12), + _invocation(action, PressGoal(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 @@ -940,15 +1098,24 @@ 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)) @@ -966,15 +1133,29 @@ def plan_from_start( action._plan_named_arm_trajectory = Mock(side_effect=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) + semantics.entity.get_local_pose.assert_not_called() assert [segment.name for segment in plan.segments] == [ "transfer", "approach", @@ -985,6 +1166,65 @@ def plan_from_start( ] +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: generator = _dual_motion_generator() original_compute_ik = generator.robot.compute_ik.side_effect @@ -1007,7 +1247,7 @@ 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", @@ -1035,22 +1275,57 @@ 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"), 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), ) + assert all(not frame.active_mask[1].item() for frame in plan.commands.frames) received = projected.get_held_object("right_arm") assert received is not None assert received.env_mask.tolist() == [True, False] + semantics.entity.get_local_pose.assert_not_called() + + +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() def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None: @@ -1067,8 +1342,14 @@ def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None ) 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,16 +1358,25 @@ 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"), 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 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("left_arm") is None assert projected.get_held_object("right_arm") is None assert isinstance( @@ -1102,6 +1392,129 @@ def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None ] +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() + held = projected.get_coordinated_held_object("left_arm", "right_arm") + assert held is not None + assert torch.allclose(held.left_object_to_eef, pose_inv(object_pose)) + assert torch.allclose(held.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={"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( + _invocation( + action, + AssembleGoal( + affordance=affordance, + base_pose=SceneEntityPose("base"), + ), + ) + ) + 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: generator = _dual_motion_generator() original_compute_ik = generator.robot.compute_ik.side_effect @@ -1145,7 +1558,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,11 +1567,13 @@ 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), ) + assert all(not frame.active_mask[1].item() for frame in plan.commands.frames) held = projected.get_coordinated_held_object("left_arm", "right_arm") assert held is not None assert held.env_mask.tolist() == [True, False] @@ -1204,14 +1619,19 @@ 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: @@ -1243,7 +1663,7 @@ 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"), motion_policy=MotionPolicy(sample_count=30), ) context = _dual_context(task) @@ -1252,7 +1672,13 @@ 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 plan.commands.frame_count == 30 + assert {target.target_id for target in plan.commands.targets} == { + "left_arm", + "left_hand", + "right_arm", + "right_hand", + } 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 @@ -1314,7 +1740,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 +1748,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 +1780,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 +1794,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..d85dbfe3e 100644 --- a/tests/sim/atomic_actions/test_affordance.py +++ b/tests/sim/atomic_actions/test_affordance.py @@ -18,9 +18,11 @@ 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, @@ -193,3 +195,16 @@ def test_get_assemble_object_pose_broadcasts_batched_relative_pose(self): result = aff.get_assemble_object_pose(base_pose) assert result.shape == (n_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_control.py b/tests/sim/atomic_actions/test_control.py index ee056aeaa..996944c09 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,6 +82,33 @@ 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) @@ -73,65 +128,93 @@ def test_joint_position_command_rejects_incompatible_control_part() -> None: command.resolve(n_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, 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, device="cpu", ) -def test_invocation_override_replaces_only_resolved_role_snapshot() -> None: +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", n_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", n_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..eabb6359a 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 +from unittest.mock import Mock import pytest import torch @@ -26,41 +27,86 @@ from embodichain.lab.sim.atomic_actions import ( ActionBinding, ActionInvocation, + ActionOptions, + ActionPlan, Affordance, + AtomicAction, + AtomicActionEngine, + CoordinatedHeldObjectState, DynamicCollisionMode, + EndpointBinding, + EndpointCommand, EndEffectorPoseGoal, EntityState, + ExecutionFeedbackMode, HeldObjectState, + JointPositionPayload, + JointPositionTarget, MotionPolicy, ObjectSemantics, + PlannerDiagnostics, PlanningContext, RecoveryPolicy, + ResolvedActionRequest, RobotObservation, + RuntimeCommandFrame, + RuntimeEndpointTarget, SceneEntityPose, SceneSnapshot, + SkillBindingContract, StateDelta, TaskState, + TimedCommandSequence, TimedTrajectory, ) from embodichain.lab.sim.atomic_actions.goals import ( + _resolve_object_pose, collect_scene_dependencies, resolve_pose_goal, ) -def _semantics(label: str = "object") -> ObjectSemantics: - return ObjectSemantics(affordance=Affordance(), geometry={}, label=label) +def _semantics( + label: str = "object", + *, + entity_id: str | None = None, +) -> ObjectSemantics: + return ObjectSemantics( + affordance=Affordance(), + geometry={}, + label=label, + entity_id=entity_id, + ) -def _held(batch_size: int = 2) -> HeldObjectState: +def _held( + batch_size: int = 2, + *, + semantics: ObjectSemantics | 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, ) +def _coordinated_held( + batch_size: int = 2, + *, + semantics: ObjectSemantics | None = None, +) -> CoordinatedHeldObjectState: + pose = torch.eye(4).repeat(batch_size, 1, 1) + return CoordinatedHeldObjectState( + semantics=semantics or _semantics(), + left_object_to_eef=pose, + right_object_to_eef=pose, + left_grasp_xpos=pose, + right_grasp_xpos=pose, + ) + + def _context(scene: SceneSnapshot | None = None) -> PlanningContext: qpos = torch.zeros(2, 4) return PlanningContext( @@ -71,18 +117,217 @@ def _context(scene: SceneSnapshot | None = None) -> PlanningContext: ) -def test_action_binding_is_role_based_and_immutable() -> None: +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, + feedback_mode: ExecutionFeedbackMode = ExecutionFeedbackMode.TIMED, +) -> 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(), + planned_scene_version=0, + planned_collision_world_revision=(0,) * commands.batch_size, + diagnostics=PlannerDiagnostics(backend="test"), + feedback_mode=feedback_mode, + joint_trajectory=joint_trajectory, + ) + + +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(), + 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: @@ -90,10 +335,27 @@ def test_invocation_rejects_values_without_goal_contract() -> None: ActionInvocation( skill_id="move_end_effector", goal=object(), # type: ignore[arg-type] - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=ActionBinding(owner_id="test-engine"), ) +@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) assert policy.sample_count == 24 @@ -164,6 +426,137 @@ def test_task_state_normalizes_held_relations_and_masks_updates() -> None: assert state.get_held_object("right_arm") is None +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_state_delta_merges_coordinated_semantics_with_same_entity_id() -> None: + previous_semantics = _semantics(entity_id="tray") + candidate_semantics = _semantics(entity_id="tray") + key = ("left_arm", "right_arm") + state = TaskState( + batch_size=2, + device="cpu", + coordinated_held_objects={ + key: _coordinated_held(semantics=previous_semantics), + }, + ) + + updated = StateDelta( + coordinated_held_object_updates={ + key: _coordinated_held(semantics=candidate_semantics), + } + ).apply(state, torch.tensor([True, False])) + + held = updated.get_coordinated_held_object(*key) + assert previous_semantics is not candidate_semantics + 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 +576,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 +608,516 @@ 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(), + recovery_policy=RecoveryPolicy(), + skill_options=ActionOptions(), + ) + + 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_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(), + 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(), + 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(), + 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(), + 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_positions( + trajectory_positions, + env_ids=env_ids, + control_dt=0.1, + ) + plan_success = torch.tensor([True, False]) + + plan = _action_plan( + commands, + plan_success=plan_success, + joint_trajectory=trajectory, + feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, + ) + 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_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 plan.feedback_mode is ExecutionFeedbackMode.TIMED + + +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_positions( + torch.ones(1, trajectory_frame_count, 2), + env_ids=trajectory_env_ids, + control_dt=0.1, + ) + + with pytest.raises(ValueError, match=message): + _action_plan( + commands, + joint_trajectory=trajectory, + feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, + ) + + +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, + feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, + ) + + +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, + feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, + ) + + assert plan.commands.frame_count == 0 + + +@pytest.mark.parametrize( + "feedback_mode", + [ExecutionFeedbackMode.TIMED, ExecutionFeedbackMode.JOINT_POSITION], +) +def test_action_plan_requires_stable_destination_set( + feedback_mode: ExecutionFeedbackMode, +) -> 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)), + ), + ) + trajectory = ( + TimedTrajectory.from_positions( + torch.tensor([[[1.0, 1.0], [2.0, 2.0]]]), + env_ids=env_ids, + control_dt=0.1, + ) + if feedback_mode is ExecutionFeedbackMode.JOINT_POSITION + else None + ) + + with pytest.raises(ValueError, match="same destination set"): + _action_plan( + commands, + joint_trajectory=trajectory, + feedback_mode=feedback_mode, + ) + + +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_joint_position_plan_rejects_joint_ids_outside_trajectory() -> None: + env_ids = torch.tensor([4], dtype=torch.long) + commands = _command_sequence( + env_ids=env_ids, + frame_count=1, + targets=(JointPositionTarget("arm", (0, 2)),), + ) + trajectory = TimedTrajectory.from_positions( + torch.ones(1, 1, 2), + env_ids=env_ids, + control_dt=0.1, + ) + + with pytest.raises(ValueError, match="outside joint_trajectory robot_dof"): + _action_plan( + commands, + joint_trajectory=trajectory, + feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, + ) + + +def test_joint_position_plan_rejects_payload_position_mismatch() -> None: + env_ids = torch.tensor([4], dtype=torch.long) + commands = _command_sequence(env_ids=env_ids, frame_count=1) + trajectory = TimedTrajectory.from_positions( + torch.zeros(1, 1, 2), + env_ids=env_ids, + control_dt=0.1, + ) + + with pytest.raises(ValueError, match="positions.*exactly match"): + _action_plan( + commands, + joint_trajectory=trajectory, + feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, + ) + + +def test_joint_position_plan_rejects_payload_velocity_presence_mismatch() -> None: + env_ids = torch.tensor([4], dtype=torch.long) + commands = _command_sequence( + env_ids=env_ids, + frame_count=1, + velocities=(torch.zeros(1, 2),), + ) + trajectory = TimedTrajectory.from_positions( + torch.ones(1, 1, 2), + env_ids=env_ids, + control_dt=0.1, + ) + + with pytest.raises(ValueError, match="same presence"): + _action_plan( + commands, + joint_trajectory=trajectory, + feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, + ) + + +def test_joint_position_plan_rejects_payload_velocity_value_mismatch() -> None: + env_ids = torch.tensor([4], dtype=torch.long) + commands = _command_sequence( + env_ids=env_ids, + frame_count=1, + velocities=(torch.zeros(1, 2),), + ) + trajectory = TimedTrajectory.from_positions( + torch.ones(1, 1, 2), + velocities=torch.ones(1, 1, 2), + env_ids=env_ids, + control_dt=0.1, + ) + + with pytest.raises(ValueError, match="velocities.*exactly match"): + _action_plan( + commands, + joint_trajectory=trajectory, + feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, + ) + + def test_scene_snapshot_expands_global_collision_world_revision() -> None: pose = torch.eye(4).repeat(2, 1, 1) snapshot = SceneSnapshot( @@ -233,6 +1137,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( @@ -259,6 +1211,44 @@ 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) + duration = torch.tensor([0.1], requires_grad=True) + env_ids = torch.tensor([4], dtype=torch.long) + inputs = { + "positions": positions, + "velocities": velocities, + "accelerations": accelerations, + "dt": dt, + "duration": duration, + "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), + control_dt=0.1, + ) + + def test_timed_trajectory_snapshot_owns_its_tensor_storage() -> None: trajectory = TimedTrajectory.from_positions( torch.arange(12, dtype=torch.float32).reshape(1, 3, 4), 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..7c6dd3688 --- /dev/null +++ b/tests/sim/atomic_actions/test_endpoint_runtime_e2e.py @@ -0,0 +1,535 @@ +# ---------------------------------------------------------------------------- +# 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, +) +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, + duration=dt.sum(dim=1), + ) + + 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, + ) + 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..eed03fad8 100644 --- a/tests/sim/atomic_actions/test_engine.py +++ b/tests/sim/atomic_actions/test_engine.py @@ -37,11 +37,16 @@ ControlPartCommandProfile, JointPositionCommand, JointPositionGoal, + JointPositionTarget, + JOINT_POSITION_CAPABILITY, MotionPolicy, PlanningContext, PressGoal, PressOptions, ResolvedActionRequest, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, register_action, get_registered_actions, unregister_action, @@ -53,7 +58,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, @@ -119,12 +136,16 @@ 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), ) @@ -179,14 +200,24 @@ 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) invocation = ActionInvocation( skill_id="press", goal=PressGoal(torch.eye(4)), - binding=ActionBinding( - manipulators={"primary": "all"}, - end_effectors={"primary": "all"}, + binding=engine.bind_control_parts( + "press", + {"primary": {"motion": "arm", "grasp": "hand"}}, ), motion_policy=MotionPolicy(sample_count=20), skill_options=options, @@ -204,11 +235,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 +255,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 +279,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: @@ -285,14 +326,16 @@ def test_engine_binds_one_planning_service_to_every_action() -> None: 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"}}, ) + target = resolved.endpoint("primary", "motion").require_target(JointPositionTarget) - assert resolved.manipulator().name == "all" - assert resolved.manipulator().joint_ids == (0, 1, 2) - assert resolved.manipulator().dof == 3 + assert target.control_part == "all" + assert target.joint_ids == (0, 1, 2) def test_engine_resolves_invocation_control_override_into_request() -> None: @@ -304,10 +347,12 @@ 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, @@ -317,7 +362,9 @@ def test_engine_resolves_invocation_control_override_into_request() -> None: 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", n_envs=2, device="cpu" + ), torch.full((2, 3), 0.4), ) @@ -325,15 +372,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 +390,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 +412,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) @@ -368,9 +434,11 @@ def test_action_cannot_be_rebound_to_another_engine() -> None: 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 +456,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..7243013fd 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -36,22 +36,30 @@ AtomicActionEngine, DynamicCollisionMode, EndEffectorPoseGoal, + EndpointBinding, + EndpointCommand, EntityState, ExecutionEventKind, ExecutionStatus, GraspGoal, HeldObjectState, + JointPositionPayload, + JointPositionTarget, MotionPolicy, ObjectSemantics, PlanningContext, RecoveryPolicy, - ResolvedActionBinding, ResolvedActionRequest, RobotObservation, + RuntimeCommandFrame, SceneEntityPose, SceneSnapshot, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, StateDelta, TaskState, + TimedCommandSequence, TimedTrajectory, ) from embodichain.lab.sim.common import BatchEntity @@ -64,7 +72,14 @@ class DynamicAction(AtomicAction[EndEffectorPoseGoal, ActionOptions]): 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__() @@ -93,6 +108,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, @@ -123,6 +139,7 @@ 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, @@ -137,6 +154,7 @@ 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, @@ -168,6 +186,82 @@ 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, ...]) -> None: + super().__init__() + self.destinations = destinations + 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, + ), + ) + return self.build_command_plan( + request, + context, + success=True, + commands=TimedCommandSequence( + frames=(frame,), + env_ids=context.env_ids, + ), + ) + + class UncopyableEntity(BatchEntity): """Minimal live entity whose simulator identity must not be copied.""" @@ -210,6 +304,30 @@ def _engine(batch_size: int = 1) -> tuple[AtomicActionEngine, DynamicAction]: return engine, action +def _destination_engine( + destinations: tuple[str | 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) + engine.register(action) + return engine, action + + def _context( timestamp: float, qpos: float | tuple[float, ...], @@ -274,6 +392,7 @@ def _collision_context( def _invocation( + engine: AtomicActionEngine, *, skill_id: str = "dynamic", max_replans: int = 2, @@ -286,7 +405,10 @@ def _invocation( 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, @@ -304,17 +426,50 @@ 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", + } + }, + ), + recovery_policy=RecoveryPolicy( + max_replans=2, + max_action_retries=1, + goal_translation_threshold=0.02, + ), + invocation_id="destination-call", + ) + + +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] @@ -323,7 +478,7 @@ def test_session_commands_schedule_arrivals_and_final_settling() -> None: engine, _ = _engine() engine.register(NonuniformTimingAction()) session = engine.start( - (_invocation(skill_id="nonuniform_timing"),), + (_invocation(engine, skill_id="nonuniform_timing"),), _context(0.0, 0.0, 0.2, 0), ) @@ -361,7 +516,7 @@ def test_request_snapshot_preserves_live_entity_identity() -> None: request = ResolvedActionRequest( skill_id="pick_up", goal=goal, - binding=ResolvedActionBinding(), + binding=ActionBinding(owner_id="snapshot-test"), motion_policy=MotionPolicy(), recovery_policy=RecoveryPolicy(), skill_options=ActionOptions(), @@ -381,7 +536,7 @@ 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)) @@ -394,6 +549,52 @@ def test_scene_motion_replans_late_bound_goal() -> None: assert tick.command is not None +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_collision_world_change_replans_with_latest_obstacle_pose() -> None: engine, action = _engine() generator = engine.motion_generator @@ -409,7 +610,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) @@ -448,6 +649,7 @@ def test_collision_world_exhaustion_only_disables_changed_environment() -> None: session = engine.start( ( _invocation( + engine, max_replans=0, strategy="motion_gen", ), @@ -496,6 +698,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 +729,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 +748,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 +762,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 +785,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 +805,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 +830,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 +851,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 +882,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 @@ -702,13 +952,12 @@ def test_session_revision_replans_from_latest_context() -> None: 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 +977,84 @@ 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,), + ), + 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_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)) @@ -750,6 +1073,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 +1100,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, @@ -790,7 +1114,7 @@ def test_session_rejects_changed_environment_identity() -> None: def test_session_rejects_regressing_scene_snapshot() -> None: engine, _ = _engine() - session = engine.start((_invocation(),), _context(1.0, 0.0, 0.2, 2)) + 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)) @@ -801,7 +1125,7 @@ def test_session_rejects_regressing_collision_world_revision() -> None: qpos = torch.zeros(1, 2) initial = _collision_context(0.0, qpos, torch.tensor([0.4]), (2,)) session = engine.start( - (_invocation(strategy="motion_gen"),), + (_invocation(engine, strategy="motion_gen"),), initial, ) regressed = _collision_context(0.1, qpos, torch.tensor([0.4]), (1,)) @@ -814,7 +1138,7 @@ 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, @@ -858,10 +1182,39 @@ def test_nonempty_effect_is_committed_only_after_external_verification() -> None assert completed.task_state.get_held_object("arm") is not None +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="awaiting verification"): + 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_success=torch.tensor([True]), + ) + 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, @@ -888,7 +1241,7 @@ def test_effect_failure_does_not_commit_and_exhausts_retry_budget() -> None: def test_failed_effect_plan_retries_without_requesting_effect_verification() -> None: engine, _ = _engine() engine.register(FailedEffectAction()) - base = _invocation(max_action_retries=0) + base = _invocation(engine, max_action_retries=0) invocation = ActionInvocation( skill_id="failed_effect", goal=base.goal, diff --git a/tests/sim/atomic_actions/test_motion_strategy_e2e.py b/tests/sim/atomic_actions/test_motion_strategy_e2e.py index dd3e7e058..eaf99f44d 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,14 +78,16 @@ 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, @@ -95,7 +96,10 @@ def _run_reach_test(self, strategy: str): ) ) 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_runner.py b/tests/sim/atomic_actions/test_runner.py index bfeccc04b..7fe66b73c 100644 --- a/tests/sim/atomic_actions/test_runner.py +++ b/tests/sim/atomic_actions/test_runner.py @@ -41,15 +41,22 @@ ExecutionRunner, ExecutionRunnerCfg, HeldObjectState, - JointCommand, + JOINT_POSITION_CAPABILITY, + JointPositionPayload, + JointPositionTarget, MotionPolicy, ObjectSemantics, PlanningContext, RecoveryPolicy, ResolvedActionRequest, RobotObservation, + RuntimeCommandFrame, + RuntimeEndpointTarget, RunnerStatus, SceneSnapshot, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, StateDelta, TaskState, TimedTrajectory, @@ -115,14 +122,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,22 +144,41 @@ 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() @@ -161,7 +188,19 @@ class TimedAction(AtomicAction[EndEffectorPoseGoal, ActionOptions]): 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: super().__init__() @@ -213,10 +252,19 @@ def _plan( ) +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, batch_size: int = BATCH_SIZE, + control_joint_ids: tuple[int, ...] | None = None, ) -> tuple[ ExecutionRunner, FakeClock, @@ -232,7 +280,9 @@ 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") @@ -247,7 +297,7 @@ def _make_runner( invocation = ActionInvocation( skill_id="timed", goal=EndEffectorPoseGoal(goal_pose), - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=_timed_action_binding(action), motion_policy=MotionPolicy(sample_count=3, control_dt=FIRST_INTERVAL), recovery_policy=RecoveryPolicy( max_replans=2, @@ -266,6 +316,30 @@ def _make_runner( return runner, clock, provider, sink, action +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_ERROR + 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 +363,34 @@ 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_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 +419,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 +445,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 +465,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: @@ -383,14 +487,17 @@ def test_runner_replans_from_observation_after_tracking_error() -> None: assert recovered.status is RunnerStatus.RUNNING -def test_runner_surfaces_explicit_invocation_revision() -> None: - runner, _, _, _, action = _make_runner() +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"}), + binding=_timed_action_binding(action), motion_policy=MotionPolicy(sample_count=3, control_dt=FIRST_INTERVAL), recovery_policy=RecoveryPolicy( max_replans=2, @@ -400,11 +507,24 @@ def test_runner_surfaces_explicit_invocation_revision() -> None: 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 +532,41 @@ 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, control_dt=FIRST_INTERVAL), + recovery_policy=RecoveryPolicy( + max_replans=2, + tracking_error_threshold=0.05, + action_timeout=10.0, + ), + revision=1, + ) + + with pytest.raises(RuntimeError, match="awaiting verification"): + runner.revise_current(revised) + + 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, + ) + ) + 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 +580,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 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_trajectory_ops.py b/tests/sim/atomic_actions/test_trajectory_ops.py index e3dae34a6..41c9c4ed5 100644 --- a/tests/sim/atomic_actions/test_trajectory_ops.py +++ b/tests/sim/atomic_actions/test_trajectory_ops.py @@ -83,6 +83,21 @@ def test_non_binary_integer_success_is_rejected(self): 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, + n_envs=2, + device="cuda", + name="IK success", + ) + class TestResolvePoseTarget: def test_unbatched_pose_broadcasts(self): @@ -279,6 +294,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): 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/planners/test_curobo_planner.py b/tests/sim/planners/test_curobo_planner.py index f62563d2c..8cfd6e2cd 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,108 @@ 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, + ), + ) + + assert planner.dynamic_collision_entity_ids == ("registry_cube",) + assert planner.collision_world_entity_ids == ("registry_cube",) + assert planner.collision_world_batch_mode == expected_mode + + def test_curobo_collision_world_binding_merges_owned_obstacle_poses(): planner = object.__new__(CuroboPlanner) configured_pose = torch.eye(4).unsqueeze(0) @@ -513,6 +616,88 @@ 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_generate_mesh_world_yaml_assembles_schema(tmp_path): rigid_object = _FakeRigidObject( "demo_block", @@ -566,6 +751,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 +788,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 +928,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 +938,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 +967,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 +985,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 +995,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..32d18fd9e 100644 --- a/tests/sim/planners/test_motion_generator_batched.py +++ b/tests/sim/planners/test_motion_generator_batched.py @@ -129,6 +129,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.dynamic_collision_entity_ids = ("obstacle",) original = PlanOptions() obstacle_pose = torch.eye(4).unsqueeze(0) @@ -152,6 +153,143 @@ 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.supports_collision_world_updates = True + planner.dynamic_collision_entity_ids = 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.supports_collision_world_updates = True + planner.dynamic_collision_entity_ids = ("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.supports_collision_world_updates = True + planner.dynamic_collision_entity_ids = ("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.supports_collision_world_updates = True + planner.dynamic_collision_entity_ids = () + 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.supports_collision_world_updates = True + planner.dynamic_collision_entity_ids = () + 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() + planner.dynamic_collision_entity_ids = ("cube", "tray") + planner.collision_world_entity_ids = ("cube", "tray", "table") + planner.collision_world_batch_mode = "per_env" + generator = object.__new__(MotionGenerator) + generator.planner = planner + + 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" + + +@pytest.mark.parametrize( + ("entity_ids", "error_type", "match"), + [ + (("cube", "cube"), ValueError, "unique"), + ((" cube",), TypeError, "outer whitespace"), + ], +) +def test_motion_generator_rejects_invalid_collision_entity_metadata( + entity_ids, error_type, match +) -> None: + planner = Mock() + planner.dynamic_collision_entity_ids = entity_ids + generator = object.__new__(MotionGenerator) + generator.planner = planner + + with pytest.raises(error_type, match=match): + _ = generator.dynamic_collision_entity_ids + + def test_bind_collision_world_rejects_unsupported_planner() -> None: planner = Mock() planner.supports_collision_world_updates = False @@ -171,6 +309,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.dynamic_collision_entity_ids = ("obstacle",) defaults = PlanOptions() planner.default_plan_options.return_value = defaults planner.with_collision_world.return_value = defaults 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_calls.py b/tests/sim/skills/test_calls.py new file mode 100644 index 000000000..742d0058c --- /dev/null +++ b/tests/sim/skills/test_calls.py @@ -0,0 +1,434 @@ +# ---------------------------------------------------------------------------- +# 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 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, + skill_id=target.skill_id, + binding_contract=target.binding_contract, + 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) + + +@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_normalizes_receiver_as_destination_resource() -> None: + call = HandOver(object=SceneObjectRef("cube"), receiver="right_actor") + + assert call.receiver == "right_actor" + assert call.resources == {"destination": "right_actor"} + + +def test_handover_rejects_conflicting_receiver_resource() -> None: + with pytest.raises(ValueError, match="conflicts"): + HandOver( + object=SceneObjectRef("cube"), + receiver="right_actor", + resources={"destination": "left_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, + skill_id="unsafe", + binding_contract=SkillBindingContract(), + ) + + +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: + with pytest.raises(ValueError, match="exact curated"): + SemanticCallDescriptor( + call_id=Pick.call_kind, + spec_type=Pick, + skill_id="move_joints", + binding_contract=SkillBindingContract(), + ) + + +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, + skill_id=source.skill_id, + binding_contract=source.binding_contract, + 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, + skill_id=target.skill_id, + binding_contract=contract, + 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, + skill_id=target.skill_id, + binding_contract=target.binding_contract, + 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..f9a206e5f --- /dev/null +++ b/tests/sim/skills/test_compiler.py @@ -0,0 +1,892 @@ +# ---------------------------------------------------------------------------- +# 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 ( + Affordance, + AntipodalAffordance, + AtomicActionEngine, + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + ControlPartCommandProfile, + EntityState, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + GraspGoal, + HandOverOptions, + HeldObjectState, + ObjectSemantics, + PickUp, + PickUpOptions, + PlaceGoal, + PlanningContext, + RobotObservation, + SceneEntityPose, + SkillDescriptor, + TaskState, +) +from embodichain.lab.sim.skills.calls import ( + HandOver, + Pick, + Place, + RegisteredSemanticCall, + SemanticCallDescriptor, + SemanticPose, + builtin_semantic_call_catalog, +) +from embodichain.lab.sim.skills.compiler import ( + GroundedSemanticCall, + HandOverPoseProvider, + HandOverPoseTargets, + RegisteredSemanticLowerer, + RelationTargetGrounder, + SemanticLowering, + SemanticObjectTarget, + SemanticRelationTarget, + SemanticSkillCompiler, + SemanticWorkflow, +) +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 ( + GRASP_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, + SceneAffordanceRef, + SceneEntityRegistration, + SceneObjectRef, + SceneRegistry, +) + +_MOTION_CAPABILITIES = frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } +) +_PICK_TARGET = PickUp.descriptor() + + +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 _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 + target_descriptor: ClassVar[SkillDescriptor] = _PICK_TARGET + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: object, + ) -> SemanticLowering: + del call, context, bound + return SemanticLowering( + goal=GraspGoal( + semantics=ObjectSemantics( + affordance=AntipodalAffordance(), + geometry={}, + entity_id="cube", + ) + ), + skill_options=PickUpOptions(), + ) + + +class _DerivedGraspGoal(GraspGoal): + """Executable subclass that an extension must not smuggle into the core.""" + + +class _DerivedPickUpOptions(PickUpOptions): + """Options subclass that must fail the registered target contract.""" + + +class _SubclassOutputLowerer(RegisteredSemanticLowerer): + """Try to bypass exact target contracts with executable subclasses.""" + + call_id: ClassVar[str] = "vendor.inspect" + schema_version: ClassVar[int] = 1 + target_descriptor: ClassVar[SkillDescriptor] = _PICK_TARGET + + def __init__(self, output: str) -> None: + self.output = output + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> SemanticLowering: + del call, context, bound + semantics = ObjectSemantics( + affordance=AntipodalAffordance(), + geometry={}, + entity_id="cube", + ) + if self.output == "goal": + return SemanticLowering( + goal=_DerivedGraspGoal(semantics=semantics), + skill_options=PickUpOptions(), + ) + return SemanticLowering( + goal=GraspGoal(semantics=semantics), + skill_options=_DerivedPickUpOptions(), + ) + + +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(pose=SceneEntityPose("table_top")), + final=SemanticObjectTarget( + pose=SemanticPose( + (0.5, 0.0, 0.4), + (1.0, 0.0, 0.0, 0.0), + ) + ), + ) + + +def _scene_registry() -> 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}, + ), + 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), + ), + ) + ) + return registry, (cube_provider, table_provider) + + +def _profile() -> 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": SkillPolicyPreset("safe")}, + default_preset="safe", + ) + + +def _dual_profile(*, provider_id: str | None = "dual_center") -> 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": SkillPolicyPreset("safe")}, + default_preset="safe", + grounding_providers=({} if provider_id is None else {"hand_over": provider_id}), + ) + + +def _engine(profile: RobotSkillProfile) -> 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" + return AtomicActionEngine(generator, skill_profile=profile) + + +def _integration( + registry: SceneRegistry, + *, + registered: bool = False, +) -> tuple[SemanticIntegrationManifest, AtomicActionEngine]: + profile = _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, + skill_id=_PICK_TARGET.skill_id, + binding_contract=_PICK_TARGET.binding_contract, + target_descriptor=_PICK_TARGET, + ) + ) + manifest = SemanticIntegrationManifest( + scene=SceneManifest.from_registry(registry), + robot_profile=profile, + call_catalog=catalog, + ) + return manifest, _engine(profile) + + +def _compiler( + registry: SceneRegistry, + *, + registered: bool = False, + relation_grounders: tuple[RelationTargetGrounder, ...] = ( + _FrameRelationGrounder(), + ), + registered_lowerers: tuple[RegisteredSemanticLowerer, ...] = (), +) -> tuple[SemanticSkillCompiler, AtomicActionEngine]: + manifest, engine = _integration(registry, registered=registered) + bound = manifest.bind(registry, engine) + return ( + SemanticSkillCompiler( + bound, + relation_grounders=relation_grounders, + registered_lowerers=registered_lowerers, + ), + engine, + ) + + +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, + control_part: str = "arm", + 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={control_part: held}, + ), + robot_dof=robot_dof, + ) + + +def test_analysis_is_provider_free_and_propagates_object_target() -> None: + registry, providers = _scene_registry() + compiler, engine = _compiler(registry) + 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 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 + torch.testing.assert_close( + options.downstream_object_target_poses[0], + drop.to_matrix(), + ) + engine.resolve(grounded.invocation) + + +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() + profile = _dual_profile() + 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 len(workflow.calls[0].downstream_object_targets) == 1 + 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), + control_part="left_arm", + 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 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), + control_part="left_arm", + 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() + compiler, engine = _compiler(registry) + 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 + 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" + + 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" + + grounded = compiler.ground( + workflow, + 0, + partial_context, + eligible_mask=torch.tensor([True, False]), + ) + assert grounded.eligible_mask.tolist() == [True, False] + with pytest.raises(TypeError, match="created by"): + GroundedSemanticCall( + analyzed=grounded.analyzed, + invocation=grounded.invocation, + eligible_mask=torch.tensor([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", ["goal", "options"]) +def test_registered_lowerer_cannot_return_target_subclasses(output: 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="produced|incompatible"): + compiler.ground(workflow, 0, _context(registry)) + + +def test_workflow_is_factory_owned_and_cannot_cross_compilers() -> None: + registry, _ = _scene_registry() + first, _ = _compiler(registry) + second, _ = _compiler(registry) + workflow = first.analyze((Pick(object=SceneObjectRef("cube")),)) + + with pytest.raises(TypeError, match="created by"): + SemanticWorkflow() + with pytest.raises(SemanticValidationError) as error: + second.ground(workflow, 0, _context(registry)) + assert error.value.diagnostic.code in { + "semantic_program_stale", + "semantic_workflow_owner_mismatch", + } diff --git a/tests/sim/skills/test_integration.py b/tests/sim/skills/test_integration.py new file mode 100644 index 000000000..ffa790bce --- /dev/null +++ b/tests/sim/skills/test_integration.py @@ -0,0 +1,544 @@ +# ---------------------------------------------------------------------------- +# 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, + EntityState, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, +) +from embodichain.lab.sim.skills.calls import ( + Pick, + RegisteredSemanticCall, + SemanticCallCatalog, + SemanticCallDescriptor, + builtin_semantic_call_catalog, +) +from embodichain.lab.sim.skills.integration import ( + BoundSemanticCall, + SceneEntityManifest, + SceneManifest, + SemanticIntegrationManifest, + SemanticValidationError, +) +from embodichain.lab.sim.skills.profiles import ( + ControlPartEndpoint, + RobotResource, + RobotSkillProfile, + SkillPolicyPreset, +) +from embodichain.lab.sim.skills.scene import ( + AmbiguousSceneAffordanceError, + GRASP_AFFORDANCE_CAPABILITY, + PLACE_ON_AFFORDANCE_CAPABILITY, + SceneAffordanceRef, + SceneEntityRegistration, + SceneObjectRef, + SceneRegistry, + UnsupportedSceneAffordanceError, +) + +_MOTION_CAPABILITIES = frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } +) + + +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() + + +def _scene_registry( + *, + with_default: bool, +) -> 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, + ), + 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), + ), + ) + ) + return registry, provider + + +def _semantic_integration( + registry: SceneRegistry, +) -> SemanticIntegrationManifest: + 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={"safe": SkillPolicyPreset("safe")}, + default_preset="safe", + ) + return SemanticIntegrationManifest( + scene=SceneManifest.from_registry(registry), + robot_profile=robot_profile, + call_catalog=builtin_semantic_call_catalog(), + ) + + +def _engine_for_integration( + integration: SemanticIntegrationManifest, +) -> 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" + 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_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_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, + skill_id=pick.skill_id, + binding_contract=pick.binding_contract, + 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_is_factory_owned_by_installed_profile() -> 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 + with pytest.raises(TypeError, match="created by"): + BoundSemanticCall() + + +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_profiles.py b/tests/sim/skills/test_profiles.py new file mode 100644 index 000000000..ac31f550e --- /dev/null +++ b/tests/sim/skills/test_profiles.py @@ -0,0 +1,1445 @@ +# ---------------------------------------------------------------------------- +# 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, + 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.skills import ( + AmbiguousSkillBindingError, + ControlPartEndpoint, + ControlPartEndpointAdapter, + EndpointResolution, + ProfileValidationError, + ResourceBinding, + ResourceEndpoint, + ResourceEndpointAdapter, + RobotResource, + RobotSkillProfile, + SkillPolicyPreset, + UnsupportedSkillError, +) + +_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"] + + +@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 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", + motion_policy=MotionPolicy(planner="stub_planner", sample_count=80), + ) + 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 == 1 + assert first.motion_policy.sample_count == 80 + 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 \[1\]"): + SkillPolicyPreset("future", schema_version=2) + + incompatible = RobotSkillProfile( + "bad_preset", + resources=_resources(), + command_profiles=_command_profiles(), + presets={ + "other": SkillPolicyPreset( + "other", + motion_policy=MotionPolicy(planner="other_planner"), + ) + }, + ) + with pytest.raises(ProfileValidationError, match="requires planner"): + incompatible.bind(_engine(control_profiles=_command_profiles())) + + +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_scene.py b/tests/sim/skills/test_scene.py new file mode 100644 index 000000000..bf364f56f --- /dev/null +++ b/tests/sim/skills/test_scene.py @@ -0,0 +1,1061 @@ +# ---------------------------------------------------------------------------- +# 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 + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + Affordance, + AntipodalAffordance, + EntityState, + SceneSnapshot, +) +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 _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: str | None = "per_env", + ) -> None: + self.dynamic_collision_entity_ids = entity_ids + self.collision_world_entity_ids = ( + entity_ids if world_entity_ids is None else world_entity_ids + ) + self.supports_dynamic_collision_world = supports_updates + self.collision_world_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) -> None: + self.pose = pose + + def get_local_pose(self, *, to_matrix: bool) -> torch.Tensor: + assert to_matrix is True + return self.pose + + +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)), + } + + 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_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_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