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 fe090cd78..562fc849c 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -365,6 +365,8 @@ topics: - physical YAML obstacle name - sphere derived obstacle names - empty collision mesh + - CollisionWorldInfo + - collision_world_info - dynamic_collision_entity_ids - collision_world_entity_ids - collision_world_batch_mode @@ -602,7 +604,9 @@ topics: - held_objects - HeldObjectState - ActionBinding - - ActionBindingRoute + - EndpointBinding + - RuntimeEndpointTarget + - JointPositionTarget - SkillBindingContract - SkillResourceSlot - SkillEndpointRequirement @@ -648,6 +652,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 @@ -662,6 +680,7 @@ topics: - build_pose_plan_states - build_joint_plan_states - engine.register + - engine.make_invocation - BUILTIN_ACTION_TYPES - load_builtins - engine.plan @@ -680,6 +699,8 @@ topics: - 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 diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index e32819404..1e77fb97e 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -14,34 +14,41 @@ There is no `ActionTarget`, `WorldState`, `ActionResult`, `execute()`, or `ActionInvocation` separates: - an action-owned typed goal, validated against the action's `GoalType`; -- `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` strategy, sampling, collision, and backend options; - bounded `RecoveryPolicy` thresholds and retry budgets; -- optional typed `skill_options` and role-scoped `control_overrides` for one - invocation revision. +- optional typed `skill_options` and endpoint-scoped `control_overrides` for + one invocation revision. `PlanningContext` separates measured `RobotObservation`, verified symbolic `TaskState`, versioned `SceneSnapshot`, environment IDs, and an optional explicit `control_dt` used only by action-owned interpolation. 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 -the legacy core's control-part command profiles. It does not own a timing -fallback. Planner results with positions require explicit `dt` and matching -`duration`, and actions must pass a complete `TimedTrajectory` to +its direct control-part command-profile snapshot. It also issues an opaque +binding-owner ID, so an `ActionBinding` cannot cross engine instances. It does +not own a timing fallback. Planner results with positions require explicit `dt`; +`duration` is derived from it. Actions must pass a complete `TimedTrajectory` to `build_plan()`. Environment-backed integrations put `BaseEnv.step_dt` on `PlanningContext.control_dt` when action-owned interpolation needs a cadence. `MotionGenerator.generate()` is the only stateful motion-planning entry point. @@ -51,9 +58,9 @@ 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. +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 @@ -95,14 +102,14 @@ generic DAG, not a fixed arm/tool schema: 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`: lowering values, an optional generic - command-profile key, joint IDs, adapter-defined claim tokens, and exclusivity. - `ControlPartEndpointAdapter` is installed by default for - `ControlPartEndpoint`; 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. + 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. @@ -120,23 +127,42 @@ Skills own the robot-independent side of the contract. A concrete `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 an optional `ActionBindingRoute`. Selecting one resource -per slot keeps related endpoints together, so a manipulation participant cannot -silently combine one arm with an unrelated tool. Endpoint views within that -resource may overlap by default, which permits an arm, mobile base, and +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. -`ActionBindingRoute` is only a transition adapter into the current core's -`manipulators` and `end_effectors` maps. Contract routes must cover the action's -declared core roles exactly. `BoundRobotSkillProfile.resolve()` returns a -`ResolvedSkillBinding` that retains the selected logical resources, the lowered -concrete `ActionBinding`, each resource's resolved endpoint data, and one -combined `ResourceClaim`. Direct-core callers may still construct -`ActionBinding` themselves, but that path does not perform profile capability -matching. +Profile binding lowers every selected endpoint directly into an +`EndpointBinding`. Its `target` supplies immutable runtime addressing +(`transport_id`, `target_id`); its semantic commands, capabilities, and claim +tokens remain attached to the same endpoint. `BoundRobotSkillProfile.resolve()` +returns a `ResolvedSkillBinding` that retains the selected logical resources, +the engine-owned `ActionBinding`, each resource's resolved endpoint data, and +one combined `ResourceClaim`. + +Advanced callers without a profile use +`engine.bind_control_parts(skill, endpoints)` with an exact nested +`slot -> endpoint -> control_part` mapping. The engine accepts an installed +skill ID or an explicit action instance later passed to `plan_action()`, checks +contract coverage, control-part existence, required commands, ownership, and +disjointness, then emits the same generic `ActionBinding` with +`JointPositionTarget` endpoints. Callers do not construct bindings manually, +and this path deliberately does not perform profile resource discovery or +capability matching. + +`engine.make_invocation(skill_id, goal, ...)` is the convenience construction +boundary when callers do not need to retain a binding separately. Pass +`control_parts` for the direct path, or rely on a bound `RobotSkillProfile` and +optionally pass `resources` as `slot -> resource_id` selections. The two binding +sources are mutually exclusive. Without a profile, `control_parts` is required; +with a profile, omitting `resources` uses unique or configured-default profile +resolution. The method returns an ordinary `ActionInvocation` and does not plan +or execute it. It resolves bindings only; profile policy presets and runner +configuration remain semantic-runtime concerns. Discovery boundaries are distinct: @@ -161,9 +187,12 @@ Binding and policy authority is split deliberately: - 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 - lowering metadata; -- the engine owns installed actions, one planner backend, and the legacy - control-part command profiles used by the current action core. + 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 @@ -172,14 +201,15 @@ source; passing `control_profiles` at the same time is rejected. immutable command container, but their mapping keys are generic profile IDs rather than necessarily being control-part names. `ControlPartEndpointAdapter` plus `RobotSkillProfile.action_control_profiles()` -is only the bridge that lowers applicable endpoint commands into the current -core's control-part-keyed profiles. Binding a profile to an already constructed -engine instead requires equivalent bridge commands to have been installed -already. 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 replacements. Resolving a -custom endpoint's commands does not by itself add their controller transport to -the current action core. +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 @@ -193,13 +223,13 @@ 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, -joint-mask command merger, or concurrency guarantee yet. `ExecutionSession` -and `ExecutionRunner` still emit, cancel, and hold full-robot joint commands. A -custom mobile/base endpoint can bind and participate in capability matching -once its adapter resolves it, including a controller claim token, but that does -not create a reusable navigation skill, planner/controller path, or command -transport. Do not treat successful binding or a non-conflicting claim as proof -of safe parallel or mobile execution. +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 @@ -336,8 +366,12 @@ 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. @@ -365,13 +399,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; @@ -395,42 +438,67 @@ 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` @@ -451,21 +519,20 @@ For lightweight sources that do not need environment correlation IDs, The public `AtomicAction.plan()` copies `MotionPolicy` and binds collision entity poses through `MotionGenerator.bind_collision_world()`. The motion generator owns option copying and the backend capability boundary, then forwards the -update through `BasePlanner.with_collision_world()`. Backends opt in via -`supports_collision_world_updates`; cuRobo implements this bridge using -`CuroboPlanOptions.dynamic_obstacle_poses`. Replanning therefore consumes the -same scene snapshot that triggered invalidation without adding obstacle -parameters to each skill. Add/remove/geometry mutations are not yet supported -by this pose-update path; providers should revision only pose-updatable -registered obstacles. - -`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()`. +update through `BasePlanner.with_collision_world()`. Backends opt in through +`BasePlanner.collision_world_info.supports_updates`; cuRobo implements this +bridge using `CuroboPlanOptions.dynamic_obstacle_poses`. Replanning therefore +consumes the same scene snapshot that triggered invalidation without adding +obstacle parameters to each skill. Add/remove/geometry mutations are not yet +supported by this pose-update path; providers should revision only +pose-updatable registered obstacles. + +`BasePlanner.collision_world_info` exposes the backend's complete world, +dynamic subset, batching mode, and update capability as one immutable contract. +`MotionGenerator` validates and forwards it for +`SceneRegistry.make_planning_scene_provider()`. External providers call +`validate_collision_integration(..., scene_provider=...)`. These construction +checks are separate from per-plan `bind_collision_world()`. Runnable closed-loop examples live under `scripts/tutorials/atomic_action/`: `tracking_error_recovery.py`, `moving_target_recovery.py`, and @@ -486,10 +553,10 @@ names, planner configuration, retry policy, or runtime state. typed planner options. Optional planner-backend compatibility belongs to `SkillPolicyPreset.required_planner`; velocity and acceleration constraints belong to the selected backend's typed `PlanOptions`. Timing belongs to the -trajectory producer: planners return explicit `dt`/`duration`, while custom or -composite interpolation constructs a `TimedTrajectory` using an explicit -cadence such as `PlanningContext.require_control_dt()`. Missing timing is an -error rather than an engine-owned default. +trajectory producer: planners return explicit `dt` with derived `duration`, +while custom or composite interpolation constructs a `TimedTrajectory` using +an explicit cadence such as `PlanningContext.require_control_dt()`. Missing +timing is an error rather than an engine-owned default. `DynamicCollisionMode.AUTO` consumes a live collision world when available, `OFF` ignores snapshot collision entities and their revisions, and `REQUIRED` fails unless the motion strategy, scene, and planner support that path. These @@ -504,7 +571,7 @@ There is no `ActionCfg` or built-in `*Cfg` layer. built-in can be replaced only with explicit `replace=True`. Registration means an implementation is installed; it does not prove that the current embodiment has compatible control parts, profiles, bindings, or task state. Capability -has compatible control parts, profiles, bindings, or task state. `engine.skills` +discovery is separate: `engine.skills` contains only agent-visible installed actions whose concrete classes explicitly declare a `binding_contract`; when a robot profile is bound, `engine.skill_profile.skills` further filters that catalog to valid resource @@ -518,21 +585,29 @@ 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. -`TaskState.held_objects` is the sole attachment map. A multi-manipulator grasp -stores one `HeldObjectState` per manipulator with the same `ObjectSemantics` -instance. `TaskState.held_object_mask()` exposes active rows, while -`exclusive_held_object_mask()` excludes rows where another manipulator holds -the same semantic object or live entity. Single-arm transport, release, and -handover operations only succeed on exclusive rows; coordinated placement +`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. + +Attachment state and `StateDelta` keys use the bound target's concrete control +part. `TaskState.held_objects` is the sole attachment map. A multi-manipulator +grasp stores one `HeldObjectState` per manipulator with the same +`ObjectSemantics` instance. `TaskState.held_object_mask()` exposes active rows, +while `exclusive_held_object_mask()` excludes rows where another manipulator +holds the same semantic object or live entity. Single-arm transport, release, +and handover operations only succeed on exclusive rows; coordinated placement likewise requires two distinct, exclusively held objects. -Embodiment-specific joint commands do not belong to Action options. A caller -using the legacy direct-core path without a `RobotSkillProfile` registers them +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 @@ -548,31 +623,33 @@ 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. Profile-based -integrations instead own commands under generic `command_profiles` IDs and let -endpoint declarations/adapters resolve those IDs; only -`action_control_profiles()` converts applicable control-part endpoints back to -the legacy core mapping. +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` | -| `slide` | `SlideGoal` | manipulator/end effector `primary` | -| `twist` | `TwistGoal` | manipulator/end effector `primary` | -| `coordinated_pickment` | `CoordinatedPickGoal` | `left`, `right` | -| `coordinated_placement` | `CoordinatedPlacementGoal` | `placing`, `support` | -| `hand_over` | `GraspGoal` | `source`, `destination` | +| `move_end_effector` | `EndEffectorPoseGoal` | `primary.motion` | +| `move_joints` | `JointPositionGoal` (`target` is explicit qpos or a profile command name) | `primary.motion` | +| `pick_up` | `GraspGoal` | `primary.motion`, `primary.grasp` | +| `move_held_object` | `HeldObjectPoseGoal` | `primary.motion`, `primary.grasp` | +| `place` | `PlaceGoal`, `AssembleGoal` | `primary.motion`, `primary.grasp` | +| `press` | `PressGoal` | `primary.motion`, `primary.grasp` | +| `slide` | `SlideGoal` | `primary.motion`, `primary.grasp` | +| `twist` | `TwistGoal` | `primary.motion`, `primary.grasp` | +| `coordinated_pickment` | `CoordinatedPickGoal` | `left.motion`, `left.grasp`, `right.motion`, `right.grasp` | +| `coordinated_placement` | `CoordinatedPlacementGoal` | `placing.motion`, `placing.grasp`, `support.motion`, `support.grasp` | +| `hand_over` | `GraspGoal` | `source.motion`, `source.grasp`, `destination.motion`, `destination.grasp` | `PressAffordance`, `SlideAffordance`, and `TwistAffordance` contain only target-local geometry and interaction semantics. Their goals own an explicit @@ -610,24 +687,33 @@ snapshot-grounded object example. 1. Define a frozen action-owned goal dataclass. 2. Define a frozen `ActionOptions` subclass only when runtime behavior exists. -3. Declare `skill_id`, `GoalType`, `OptionsType`, and required core roles. Also - declare a class-local `SkillBindingContract` when the skill should appear in - `engine.skills`; route every current core role exactly once. +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. If planning consumes a semantic object's snapshot pose, override `_scene_dependencies()`, preserve `super()` dependencies, and add exactly that semantic ID. -8. Return a full-robot `TimedTrajectory` through `build_plan()`; raw position - tensors are rejected. Preserve planner `dt`, or use - `TimedTrajectory.from_uniform_step()` with an explicitly selected cadence for - action-owned interpolation. - Build batched `list[PlanState]`, translate the policy with +8. For planner-backed joint motion, return a full-robot `TimedTrajectory` + through `build_plan()`; raw position tensors are rejected. Preserve planner + `dt`, or use `TimedTrajectory.from_uniform_step()` with an explicitly + selected cadence for action-owned interpolation. Build batched + `list[PlanState]`, translate the policy with `request.motion_policy.to_motion_gen_options()` (including - `interpolation_dt=context.control_dt` when applicable), and call - `self.motion_generator.generate()`. Import pure operations directly from - `trajectory_ops.py`. + `interpolation_dt=context.control_dt` when applicable), call + `self.motion_generator.generate()`, and import pure operations directly from + `trajectory_ops.py`. For mobile, whole-body, or other controller-native + motion, build `EndpointCommand` frames and a `TimedCommandSequence`, then use + `build_command_plan()`. A new transport family must define matching + `RuntimeEndpointTarget` and `RuntimeCommandPayload` types with the same + `transport_id`, plus an `EndpointCommandTransport` registered in the runner's + router. 9. Declare symbolic changes with `StateDelta`; do not mutate context or commit physical effects during planning. For partial attachment updates, retain previous scalar semantics while any previous row remains; merge only batched diff --git a/agent_context/topics/motion-planning/motion-planning.md b/agent_context/topics/motion-planning/motion-planning.md index 3502ce686..41483d2cf 100644 --- a/agent_context/topics/motion-planning/motion-planning.md +++ b/agent_context/topics/motion-planning/motion-planning.md @@ -5,7 +5,7 @@ | What | Path | |---|---| | Planner registry | `embodichain/lab/sim/planners/__init__.py` | -| Base planner class & config | `embodichain/lab/sim/planners/base_planner.py` → `BasePlanner`, `BasePlannerCfg`, `PlanOptions`, `validate_plan_options` | +| Base planner class & config | `embodichain/lab/sim/planners/base_planner.py` → `BasePlanner`, `BasePlannerCfg`, `CollisionWorldInfo`, `PlanOptions`, `validate_plan_options` | | TOPPRA planner | `embodichain/lab/sim/planners/toppra_planner.py` → `ToppraPlanner`, `ToppraPlannerCfg`, `ToppraPlanOptions` | | Neural planner | `embodichain/lab/sim/planners/neural_planner.py` → `NeuralPlanner`, `NeuralPlannerCfg`, `NeuralPlanOptions` | | cuRobo planner | `embodichain/lab/sim/planners/curobo/curobo_planner.py` → `CuroboPlanner`, `CuroboPlannerCfg`, `CuroboWorldCfg`, `CuroboPlanOptions` | @@ -129,23 +129,25 @@ differences require `"cuboid"` or `"mesh"` representation, registration in data and collision caches, so retain the shared default for identical rebased layouts. -`BasePlanner.supports_collision_world_updates` and +`BasePlanner.collision_world_info` and `with_collision_world(options, obstacle_poses=...)` form the generic per-plan -dynamic-world bridge. The base implementation opts out and leaves options -unchanged. `CuroboPlanner` opts in, clones the supplied pose tensors, and merges -them into `CuroboPlanOptions.dynamic_obstacle_poses`. +dynamic-world bridge. The base property returns `None` and the base hook leaves +options unchanged. `CuroboPlanner` returns an immutable `CollisionWorldInfo` +with updates enabled, clones the supplied pose tensors, and merges them into +`CuroboPlanOptions.dynamic_obstacle_poses`. `MotionGenerator.supports_dynamic_collision_world` exposes the capability and `MotionGenerator.bind_collision_world()` owns option copying before forwarding to the backend hook. Atomic actions use that facade from their framework-owned `plan()` template when a `SceneSnapshot` declares collision entities; individual skills must not construct backend obstacle options themselves. -`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. +`CollisionWorldInfo` carries the complete canonical world, its dynamic subset, +the `"shared"` / `"per_env"` mode, and update capability as one validated +contract. It requires unique canonical IDs and requires the dynamic subset to +belong to the complete world. `MotionGenerator.collision_world_info` forwards +that contract and retains derived ID/mode properties for callers. For cuRobo, +the complete set is every mapping key (or inferred sequence name), while the +dynamic set is exactly `CuroboWorldCfg.dynamic_obstacle_names`. Sphere-expanded +physical YAML names are not part of either logical ID declaration. `CuroboWorldCfg` rejects duplicate obstacle names and requires every `dynamic_obstacle_name` to match an object registered in `rigid_objects`, so a planner-local mismatch fails before backend construction. @@ -182,7 +184,7 @@ Unified interface for trajectory planning with optional pre-interpolation. - `generate()` and `interpolate_trajectory()` are env-batched (`B, N, DOF`). - `generate()` always returns a normalized `PlanResult`; failed rows hold the supplied `start_qpos`, and every returned trajectory has explicit `dt` and - matching `duration`. + a `duration` derived from it. `MotionGenOptions` fields: @@ -234,7 +236,7 @@ Convenience constructors: | `velocities` | `torch.Tensor \| None` | Joint velocities `(B, N, DOF)` | | `accelerations` | `torch.Tensor \| None` | Joint accelerations `(B, N, DOF)` | | `dt` | `torch.Tensor \| None` | Per-step arrival intervals `(B, N)`; required whenever `positions` is present | -| `duration` | `torch.Tensor \| None` | Total trajectory time `(B,)`; required with `positions` and equal to `dt.sum(dim=1)` | +| `duration` | `torch.Tensor \| None` | Read-only total trajectory time `(B,)`, derived as `dt.sum(dim=1)` | Helper: `PlanResult.is_all_success() -> bool` returns `True` only when every env succeeded. `PlanResult` rejects positions with missing, malformed, or inconsistent timing. @@ -266,11 +268,11 @@ total duration and emits new explicit arrival intervals. ### Registering a new planner -1. Create a `BasePlanner` subclass with a `plan()` method decorated with `@validate_plan_options`; every result containing positions must include `dt` and matching `duration`. +1. Create a `BasePlanner` subclass with a `plan()` method decorated with `@validate_plan_options`; every result containing positions must include `dt`, from which `duration` is derived. 2. Create a `BasePlannerCfg` subclass with a unique `planner_type` string. 3. Optionally create a `PlanOptions` subclass for planner-specific options. -4. For a planner that accepts live obstacles, set - `supports_collision_world_updates = True` and implement +4. For a planner that accepts live obstacles, override `collision_world_info` + with a `CollisionWorldInfo` whose `supports_updates=True`, and implement `with_collision_world()` without mutating caller-owned reusable options. 5. Register in `MotionGenerator._support_planner_dict`: ```python @@ -308,14 +310,14 @@ The decorator checks that every `PlanState` in `target_states` shares the same l `start_qpos`, `sample_count`, and `interpolation_dt`; it never reads live robot state or guesses a command period implicitly. - **Missing planner timing** — constructing a `PlanResult` with positions but - without matching `dt` and `duration` raises immediately. + without `dt` raises immediately; `duration` is derived from `dt`. - **CUDA requested on a CPU-only runtime** — planner success-mask normalization raises a direct `ValueError` before querying the active CUDA device. It never silently falls back to CPU. - **Constraint tolerance** — `is_satisfied_constraint` allows 10% velocity / 25% acceleration overshoot. Dense waypoint trajectories may appear to violate constraints but pass validation. - **Fork safety with GPU sim** — `ToppraPlannerCfg.mp_context=None` defaults to `spawn` on GPU to avoid fork-after-CUDA-init hazards. Force `fork` only when the sim device is CPU or you have verified it is safe. - **cuRobo shared-world mismatch** — World-frame poses may differ solely because replicated arenas are offset. Compare poses after robot-base rebasing: keep `multi_env=False` if they match, and enable it only when robot-relative layouts differ. -- **Dynamic obstacles silently stale** — A planner participates in atomic-action collision revision recovery only when it declares `supports_collision_world_updates=True`; its hook must bind every `collision_entity_id` pose into the current planning attempt. +- **Dynamic obstacles silently stale** — A planner participates in atomic-action collision revision recovery only when `collision_world_info.supports_updates=True`; its hook must bind every `collision_entity_id` pose into the current planning attempt. - **Registry/planner identity drift** — Registry-backed cuRobo worlds must use a canonical-ID mapping, not a list whose names are inferred from UIDs. Validate exact full registry/planner collision-world agreement, dynamic diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index 6f2701125..4c22c8899 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -1,9 +1,9 @@ # Declarative Expert Programs and Unified Semantic Skill Runtime -- Status: implementation in progress; Phase 0 and Phase 1 are complete on - `main`, and Phase 2 is next -- Baseline: `main@f4ffb6608f41ea1eaee3020714412e31549dbda7` -- Last updated: 2026-08-18 +- Status: implementation in progress; Phase 0, PR1, PR2A, and PR2B are + complete on `main`, and PR2C is implemented on this feature branch +- Baseline: `main@dbc6553f11d23a5ab738282fbcde1a7214fca783` +- Last updated: 2026-08-19 - Related issues: [#471](https://github.com/DexForce/EmbodiChain/issues/471), [#474](https://github.com/DexForce/EmbodiChain/issues/474) - Related implementation: @@ -228,11 +228,13 @@ callers. #### Core/advanced layer -The current action-owned goal dataclasses, `ActionInvocation`, `ActionBinding`, -policies, `PlanningContext`, `ActionPlan`, `ExecutionSession`, -`ExecutionRunner`, and provider protocols remain available for framework -authors and unusual integrations. They are no longer prerequisites for -ordinary task authoring. +The current action-owned goal dataclasses, `ActionInvocation`, generic endpoint +`ActionBinding`, policies, `PlanningContext`, `ActionPlan`, +`ExecutionSession`, `ExecutionRunner`, and provider protocols remain available +for framework authors and unusual integrations. `ActionPlan.commands` is the +runtime authority; a joint-backed plan may additionally retain a +`TimedTrajectory` for joint feedback and offline compilation. These contracts +are no longer prerequisites for ordinary task authoring. ### 6.2 Proposed package ownership @@ -249,7 +251,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 @@ -393,11 +397,11 @@ an `arm + tool` schema. It contains a generic resource DAG: through a `ResourceEndpoint` implementation; `ControlPartEndpoint` is the current joint/control-part declaration, while registered `ResourceEndpointAdapter`s resolve any endpoint kind into generic - `EndpointResolution` metadata (binding values, commands, 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; + `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; @@ -427,10 +431,10 @@ 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. The current `ActionBindingRoute` is a -transition adapter from generic endpoints to the core's existing -`manipulators`/`end_effectors` maps; those maps are not part of the Profile -resource model. +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: @@ -453,11 +457,11 @@ 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. Current runners emit and hold -full-robot commands, so claims do not imply safe parallel execution. Parallel -scheduling still requires one coordinator, joint-mask command merge, planner -serialization or isolation, cancellation semantics, and inter-trajectory -collision checks. +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 @@ -696,8 +700,8 @@ 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, action trajectory segments, effects, recovery, scene revisions, and post-policy results. @@ -715,16 +719,16 @@ fallback `control_dt`. Environment integrations copy `BaseEnv.step_dt` into `PlanningContext.control_dt` when an action performs deterministic interpolation. Timing is a strict producer contract. A planner result with positions includes -per-waypoint `dt` and a matching per-environment `duration`; an atomic action +per-waypoint `dt` and derives its per-environment `duration`; an atomic action passes a complete `TimedTrajectory` to `build_plan()`. Missing or inconsistent timing is rejected at construction. No layer repairs an untimed planner result or raw action position tensor with a default period. -Version 1 should require every emitted `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. +Version 1 should require every emitted +`RuntimeCommandFrame.hold_duration` to be representable by an integer number +of environment steps, preferably one step per yielded frame. An incompatible +frame is rejected with a clear timing error; it is not silently resampled. +Explicit timed-command resampling can be a later, separately tested feature. Recovery timeout and retry budgets are scoped to the enclosing action attempt. A `TrajectorySegment` does not start an independent timer or own a recovery @@ -876,6 +880,10 @@ PR1 snapshot/identity bridge (complete) v v PR2A SceneRegistry PR2B RobotSkillProfile (implemented) (implemented) + | | + | v + | PR2C Runtime Endpoints + | (in progress) +-----------+-----------+ v Semantic calls/compiler --> SkillRuntime/effect monitors @@ -966,8 +974,9 @@ documented deprecated fallbacks. ### Phase 1: unified integration data -Phase 1 is implemented as two focused follow-up PRs that join before the -semantic facade/compiler work. +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. #### PR2A: SceneRegistry (landed in PR #487) @@ -1017,7 +1026,7 @@ Deliverables: `EndpointResolution` protocol; `ControlPartEndpointAdapter` is the first implementation; - action-owned `SkillBindingContract`s with participant-local endpoint, - capability, typed-command, lowering-route, and disjoint-claim requirements; + 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/ @@ -1026,8 +1035,8 @@ Deliverables: 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 - that the current full-robot command runner supports safe parallel execution. + 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 @@ -1038,11 +1047,61 @@ PR2A and PR2B landed together through PR #487. That foundation does not migrate official tasks; the repeated-cube vertical slice opts in only after the compiler, runtime, and demo bridge are available. +#### PR2C: generic runtime endpoints (implemented on the feature branch) + +PR2C removes the temporary arm/tool lowering seam and makes the profile's +generic endpoint model executable end to end: + +- `ActionBinding` is an engine-owned collection keyed only by + `(slot_id, endpoint_id)`; `ActionBindingRoute`, arm/tool role maps, and the + intermediate resolved-control-part binding types are removed as an + intentional clean break; +- every resolved profile endpoint owns a typed immutable + `RuntimeEndpointTarget`, while `EndpointCommand` combines that destination + with a transport-specific `RuntimeCommandPayload`; +- `RuntimeCommandFrame` synchronizes per-environment endpoint commands and + timing, and `TimedCommandSequence` becomes the authoritative runtime content + of `ActionPlan`; +- `EndpointCommandTransport` and `EndpointCommandRouter` perform exact-ID + registration, preflight payload validation, transport grouping, + acknowledgement aggregation, cancellation, and transport-owned safe holds; +- the framework authorizes planned commands against binding-owned targets and + physical claims, requires stable destinations across frames and recovery + replans, and retains previously active targets when a failed plan is empty; +- transports actively neutralize inactive environment rows for every addressed + target instead of treating an omitted write as a safe state; +- `SimulationExecutionAdapter` implements the built-in joint-position + transport and writes or holds only the joints claimed by each addressed + endpoint; +- joint-backed planners retain an optional full-robot `TimedTrajectory` for + existing joint feedback and `engine.compile()`, while non-joint plans use + timed completion plus the existing semantic-effect verification boundary; +- full-body joint control and a custom planar-velocity endpoint are exercised + from binding/profile resolution through planning, session execution, routing, + completion, and safe hold without arm/tool-shaped fields. +- an explicit invocation revision declares the same non-empty runtime + destination set and preserves each target's address/safe-hold fingerprint. + The runner keeps the active frame deadline and replans from a fresh due-time + observation; a pending physical effect must be verified first. Changing a + base, arm, whole-body, controller destination, or hold footprint starts a new + invocation rather than hot-switching controller ownership in place. + +PR2C does not add parallel scheduling, claim merging, transport rollback, or a +generic endpoint-feedback evaluator. It also does not add cross-destination +hot revision. Those require separate contracts. + +PR2C exit criteria: an installed custom endpoint kind needs one reusable +endpoint declaration/adapter, payload, transport, and shared atomic skill, but +no core binding or runner changes; whole-body joint endpoints use the same +path; unknown transports and incompatible payloads fail before dispatch; and +cancel/hold behavior remains transport-owned and auditable. + Combined Phase 1 exit criteria: an object is registered once under an authoritative ID, aliases cannot introduce ambiguity, dynamic-object configuration mismatches fail before execution with an entity-centric -diagnostic, and robot capabilities resolve bindings/presets without task-owned -motion code. +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 @@ -1214,6 +1273,9 @@ The design is complete when all of the following hold: - [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. 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 8cf64587a..ae595999d 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 @@ -8,8 +8,9 @@ embodichain.lab.sim.atomic_actions .. autosummary:: ActionBinding - ResolvedActionBinding - ResolvedControlPart + EndpointBinding + RuntimeEndpointTarget + JointPositionTarget ControlCommand JointPositionCommand ControlPartCommandProfile @@ -26,8 +27,14 @@ embodichain.lab.sim.atomic_actions PlanningContext StateDelta TimedTrajectory + RuntimeCommandPayload + JointPositionPayload + EndpointCommand + RuntimeCommandFrame + TimedCommandSequence TrajectorySegment PlannerDiagnostics + ExecutionFeedbackMode ActionPlan CompiledTrajectory @@ -39,7 +46,6 @@ embodichain.lab.sim.atomic_actions SkillBindingContract SkillResourceSlot SkillEndpointRequirement - ActionBindingRoute DisjointSlotEndpoints DisjointResourceSlots @@ -56,6 +62,8 @@ embodichain.lab.sim.atomic_actions RunnerStatus ObservationProvider CommandSink + EndpointCommandTransport + EndpointCommandRouter CommandAcknowledgement CommandAckStatus CommandDispatch @@ -64,7 +72,6 @@ embodichain.lab.sim.atomic_actions SimulationExecutionAdapter ExecutionTick EffectVerificationRequest - JointCommand ExecutionEvent ExecutionEventKind ExecutionStatus @@ -125,9 +132,6 @@ Semantic resource contracts .. autoclass:: SkillEndpointRequirement :members: -.. autoclass:: ActionBindingRoute - :members: - .. autoclass:: DisjointSlotEndpoints :members: @@ -155,10 +159,13 @@ Planning and state .. autoclass:: ActionBinding :members: -.. autoclass:: ResolvedActionBinding +.. autoclass:: EndpointBinding + :members: + +.. autoclass:: RuntimeEndpointTarget :members: -.. autoclass:: ResolvedControlPart +.. autoclass:: JointPositionTarget :members: .. autoclass:: ControlCommand @@ -211,6 +218,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: @@ -239,6 +264,12 @@ Engine and execution .. autoclass:: CommandSink :members: +.. autoclass:: EndpointCommandTransport + :members: + +.. autoclass:: EndpointCommandRouter + :members: + .. autoclass:: ExecutionClock :members: @@ -269,9 +300,6 @@ Engine and execution .. autoclass:: ExecutionTick :members: -.. autoclass:: JointCommand - :members: - .. autoclass:: ExecutionEvent :members: diff --git a/docs/source/overview/sim/atomic_actions/builtin_actions.md b/docs/source/overview/sim/atomic_actions/builtin_actions.md index e484707a9..2a26f15c7 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` @@ -154,29 +155,29 @@ 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` | `PressAffordance` + target pose | open-loop motion; application verifies contact/actuation | -| `slide` | `SlideGoal` | manipulator + end effector `primary` | primary: `open`, `grasp` | `SlideAffordance` + link pose | open-loop motion; application verifies joint travel/grasp | -| `twist` | `TwistGoal` | manipulator + end effector `primary` | primary: `open`, `grasp` | `TwistAffordance` + target pose | open-loop motion; application verifies joint travel/grasp | -| `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 exclusively by the `primary.motion` target | preserve attachment | +| `place` | `PlaceGoal`, `AssembleGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `open`, `grasp` | any active attachment must be exclusive to `primary.motion`; `AssembleGoal` requires one | detach object | +| `press` | `PressGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `grasp` | `PressAffordance` + target pose | open-loop motion; application verifies contact/actuation | +| `slide` | `SlideGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `open`, `grasp` | `SlideAffordance` + link pose | open-loop motion; application verifies joint travel/grasp | +| `twist` | `TwistGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `open`, `grasp` | `TwistAffordance` + target pose | open-loop motion; application verifies joint travel/grasp | +| `coordinated_pickment` | `CoordinatedPickGoal` | `left.motion`, `left.grasp`, `right.motion`, `right.grasp` | both grasp endpoints: `open`, `grasp` | semantic object/entity | attach the shared object to both motion targets | +| `coordinated_placement` | `CoordinatedPlacementGoal` | `placing.motion`, `placing.grasp`, `support.motion`, `support.grasp` | `placing.grasp`: `open`, `grasp`; `support.grasp`: `grasp` | two distinct objects, each held exclusively by its 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 exclusively 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 | @@ -185,10 +186,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 @@ -261,9 +264,10 @@ do not participate in identity. 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 @@ -276,32 +280,35 @@ Use this rule when configuring a built-in or adding a new one: control period; - `RecoveryPolicy` carries all replan/retry thresholds and budgets. -All built-ins resolve participating arm and hand names exclusively from -`ActionBinding`. The engine then resolves the selected control part's profile +All built-ins resolve their `motion` and `grasp` endpoints exclusively from the +generic `ActionBinding`. The built-in control-part adapter resolves joint IDs and checks each joint-position command against its DoF. Invocation-level -`ActionControlOverrides` may replace a command by binding role for one explicit -revision. +`ActionControlOverrides` may replace a command by `(slot, endpoint)` for one +explicit revision. ### Planning and effect semantics -Every action returns a per-environment `plan_success` mask and one or more -full-robot trajectories. `plan_success=True` means motion planning succeeded; -it does not prove contact or object transfer. Actions that change attachment -state declare a `StateDelta`. Offline `compile()` projects it hypothetically; -closed-loop execution commits it only after external effect verification. +Every action returns a per-environment `plan_success` mask and an +`ActionPlan.commands` sequence of `RuntimeCommandFrame` values. Current +joint-planned built-ins also retain `ActionPlan.joint_trajectory` for joint +feedback, inspection, and static projection. `plan_success=True` means planning +succeeded; it does not prove contact or object transfer. Actions that change +attachment state declare a `StateDelta`. Offline `compile()` projects it +hypothetically; closed-loop execution commits it only after external effect +verification. (builtin-move-end-effector)= ## `MoveEndEffector` -Plans a free-space motion for a bound manipulator to reach one EEF pose or an -ordered set of pose waypoints. +Plans a free-space motion for the bound `primary.motion` endpoint to reach one +EEF pose or an ordered set of pose waypoints. | Contract | Value | |---|---| | Skill ID | `move_end_effector` | | Goal | `EndEffectorPoseGoal(xpos=...)` | -| Binding | manipulator role `primary` | +| Binding contract | `primary.motion` with Cartesian-pose capability | | Motion | EEF planning from observed arm qpos; output expanded to full robot DoF | | Completion | `EEF_GOAL_REACHED` | | Effect | none | @@ -325,7 +332,7 @@ than an EEF pose. |---|---| | Skill ID | `move_joints` | | Goal | `JointPositionGoal(target=...)` | -| Binding | manipulator role `primary` | +| Binding contract | `primary.motion` with joint-position capability | | Motion | joint planning/interpolation from observed qpos; supports joint waypoints | | Completion | `JOINT_GOAL_REACHED` | | Effect | none | @@ -333,7 +340,7 @@ than an EEF pose. `target` accepts an explicit qpos tensor with shape `(control_dof,)`, `(B, control_dof)`, or `(B, N, control_dof)`, or a non-empty string resolved -from the bound manipulator's `ControlPartCommandProfile`. Named poses remain +from the bound `primary.motion` endpoint's command profile. Named poses remain embodiment knowledge without becoming separate goal types: ```python @@ -355,15 +362,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_id` resolves in the planning snapshot, or the deprecated live `entity` fallback is available; an `AntipodalAffordance` is required when no explicit grasp pose is supplied | -| Effect | write `HeldObjectState` for the bound manipulator | +| Binding contract | `primary.motion` plus disjoint `primary.grasp` | +| Precondition | `ObjectSemantics.entity_id` resolves in the planning snapshot; the deprecated live `entity` fallback remains temporarily; an `AntipodalAffordance` is required when no explicit grasp pose is supplied | +| Effect | write `HeldObjectState` for the bound motion target | | Verification | the attachment effect must be verified during closed-loop execution | `grasp_xpos` may be `(4, 4)`, `(B, 4, 4)`, or a `SceneEntityPose`. A scene @@ -379,7 +386,7 @@ 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 `open` and `grasp` commands on the bound end-effector profile. +`PickUp` requires typed `open` and `grasp` commands on `primary.grasp`. Important `PickUpOptions` fields: | Field | Purpose | @@ -414,18 +421,19 @@ a live scene entity. |---|---| | Skill ID | `move_held_object` | | Goal | `HeldObjectPoseGoal(object_target_pose=...)` | -| Binding | manipulator + end effector role `primary` | -| Precondition | a `HeldObjectState` exists exclusively for the bound manipulator, normally from `PickUp` | +| Binding contract | `primary.motion` plus disjoint `primary.grasp` | +| Precondition | a `HeldObjectState` exists exclusively for the bound motion target, normally from `PickUp` | | Motion | single object-centric transport segment with closed-hand qpos | | Effect | none; the existing attachment is preserved | | Dynamic target | explicit pose or `SceneEntityPose` | -The bound end-effector profile must provide `grasp`; optional upright-transport -settings belong to `MoveHeldObjectOptions`. The arm and hand are selected by -`ActionBinding`; trajectory sampling remains in `MotionPolicy`, while timing is -explicit on the planner result or planning context. In a vectorized batch, rows -where another manipulator holds the -same semantic object or live entity are marked unsuccessful and held in place. +The bound `primary.grasp` endpoint must provide `grasp`; optional +upright-transport settings belong to `MoveHeldObjectOptions`. The participant's +motion and grasp endpoints are selected through `ActionBinding`; generic timing +is explicit on the planner result or planning context, while trajectory +sampling remains in `MotionPolicy`. In a vectorized batch, rows +where another manipulator holds the same semantic object or live entity are +marked unsuccessful and held in place. **Example:** `scripts/tutorials/atomic_action/move_held_object.py` @@ -441,9 +449,9 @@ 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 and exclusive | -| Effect | detach the object from the bound manipulator | +| Binding contract | `primary.motion` plus disjoint `primary.grasp` | +| State | consumes the bound motion target's attachment when present and exclusive | +| Effect | detach the object from the bound motion target | | Verification | release must be verified during closed-loop execution | | Dynamic target | explicit pose/waypoints or `SceneEntityPose` | @@ -453,7 +461,7 @@ orientation variant from the observed starting state and uses it consistently across all waypoints. An ordinary `PlaceGoal` may still open an unattached gripper, but it will not release one side of a shared multi-manipulator object. -The bound end-effector profile must provide `open` and `grasp`. Important +The bound `primary.grasp` endpoint must provide `open` and `grasp`. Important `PlaceOptions` fields: | Field | Purpose | @@ -511,15 +519,18 @@ right-handed orthonormal rotation even for vertical or oblique press axes. |---|---| | Skill ID | `press` | | Goal | `PressGoal(semantics=..., target_pose=...)` | -| Binding | manipulator + end effector role `primary` | +| Binding contract | `primary.motion` plus disjoint `primary.grasp` | | Motion | close, approach, contact, axis-constrained press, axis-constrained retract | | Effect | explicitly open-loop; no physical button/contact effect is claimed | +| Dynamic target | explicit pose or `SceneEntityPose` | `PressOptions` controls hand-close interpolation, approach distance, press distance, and an optional target-local `press_position`. An options-level position overrides the affordance's explicit surface point. The bound -end-effector profile must provide `grasp`; the action keeps the gripper closed -for all arm-motion segments. +`primary.grasp` endpoint must provide `grasp`; both endpoints come from the +generic `ActionBinding`, and the action keeps the gripper closed for all arm +motion segments. Applications that require force/contact confirmation must +verify it externally. **Example:** `scripts/tutorials/atomic_action/press.py` @@ -545,14 +556,14 @@ pose. |---|---| | Skill ID | `slide` | | Goal | `SlideGoal(semantics=..., target_pose=...)` | -| Binding | manipulator + end effector role `primary` | +| Binding contract | `primary.motion` plus disjoint `primary.grasp` | | Motion | pull: approach, reach, close, pull, open; push adds return to approach | | Effect | explicitly open-loop; no articulation travel or grasp success is claimed | `SlideOptions` controls `direction`, hand close/open interpolation, approach distance, and translation distance. The link-frame -translation axis belongs to `SlideAffordance`; the bound end-effector profile -must provide `open` and `grasp`. Reach, pull/push, and push-return use +translation axis belongs to `SlideAffordance`; the bound `primary.grasp` +endpoint must provide `open` and `grasp`. Reach, pull/push, and push-return use axis-aligned Cartesian samples rather than sparse joint-space endpoints. **Example:** `scripts/tutorials/atomic_action/slide.py` @@ -577,14 +588,15 @@ around the target link origin. |---|---| | Skill ID | `twist` | | Goal | `TwistGoal(semantics=..., target_pose=...)` | -| Binding | manipulator + end effector role `primary` | +| Binding contract | `primary.motion` plus disjoint `primary.grasp` | | Motion | approach, reach, close, rotate about the target-local axis, open, retract | | Effect | explicitly open-loop; no articulation travel or grasp success is claimed | `TwistOptions` controls the pre-grasp distance, close/open interpolation, Cartesian twist keyframes, and twist angle. The pre-grasp pose is offset along the grasp pose's negative z-axis; the target-local twist axis belongs to -`TwistAffordance`. +`TwistAffordance`. The bound `primary.grasp` endpoint must provide `open` and +`grasp`. `Twist` is intentionally a pure-rotation primitive. Thread pitch, coupled axial translation, and regrasping are outside its contract; an `Unscrew` action should @@ -608,7 +620,7 @@ both hands -> lift -> move object -> hold**. |---|---| | Skill ID | `coordinated_pickment` | | Goal | `CoordinatedPickGoal` | -| Binding | manipulator + end effector roles `left` and `right` | +| 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 | write one `HeldObjectState` per bound manipulator; both entries share the same object semantics | @@ -633,7 +645,7 @@ 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`; @@ -641,8 +653,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. @@ -659,8 +672,8 @@ hold -> optionally release the placing hand -> retreat the placing arm**. |---|---| | Skill ID | `coordinated_placement` | | Goal | `CoordinatedPlacementGoal` | -| Binding | manipulator + end effector roles `placing` and `support` | -| Precondition | each bound arm exclusively holds a different object | +| Binding contract | disjoint `placing` and `support` slots, each with disjoint `motion` and `grasp` endpoints | +| Precondition | each bound motion target exclusively holds a different object | | Goal geometry | placing/support object target poses, optional height offsets, optional release override | | Effect | preserve support attachment; remove or preserve placing attachment according to `release` | @@ -670,15 +683,15 @@ dynamic-goal invalidation. Goal-level height/release values override the same semantic object or live entity are a shared grasp, not a placing and support pair, and their environment rows are rejected. -The placing profile must provide `open` and `grasp`; the support profile must -provide `grasp`. Important `CoordinatedPlacementOptions` fields group into: +The `placing.grasp` endpoint must provide `open` and `grasp`; `support.grasp` +must provide `grasp`. Important `CoordinatedPlacementOptions` fields group into: - default `release`, placing/support height offsets, and `lift_height`; - `hand_interp_steps`, `hold_steps`, and `retreat_steps`. -The placing/support arms and hands come exclusively from the corresponding -binding roles. The same cuRobo restriction as coordinated pickment applies to dual-arm -`strategy="motion_gen"` planning. +The placing/support motion and grasp endpoints come exclusively from the +corresponding participant slots. The same cuRobo restriction as coordinated +pickment applies to dual-arm `strategy="motion_gen"` planning. **Example:** `scripts/tutorials/atomic_action/coordinated_placement.py` @@ -694,17 +707,17 @@ retreats -> destination delivers**. |---|---| | Skill ID | `hand_over` | | Goal | `GraspGoal(semantics=...)` | -| Binding | manipulator + end effector roles `source` and `destination` | -| Precondition | source arm exclusively has a verified `HeldObjectState`; goal semantics identify that object and support destination grasp selection | +| Binding contract | disjoint `source` and `destination` slots, each with disjoint `motion` and `grasp` endpoints | +| Precondition | source motion target exclusively has a verified `HeldObjectState`; goal semantics identify that object and support destination grasp selection | | Effect | remove source attachment and create destination `HeldObjectState` | | Verification | attachment transfer must be externally verified | -Both source and destination end-effector profiles must provide `open` and -`grasp`. `HandOverOptions` owns the destination grasp region and approach +Both source and destination grasp endpoints must provide `open` and `grasp`. +`HandOverOptions` owns the destination grasp region and approach direction, middle/final object poses, and segment distances/counts. The -source/destination arm and hand control parts come exclusively from the -corresponding `ActionBinding` roles. The destination attachment reuses the -source relation's canonical `ObjectSemantics` instance. +source/destination motion and grasp endpoints come exclusively from the +corresponding generic `ActionBinding` slots. The destination attachment reuses +the source relation's canonical `ObjectSemantics` instance. The middle and final poses are currently option tensors rather than `SceneEntityPose` goal fields. Consequently, handover supports tracking-error diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index 753ff80f8..ebe26490e 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -13,16 +13,18 @@ robot_skill_profiles ``` 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 @@ -35,7 +37,7 @@ and whole-body control are not implemented by this module yet. | | v | semantic adapter: schema validation, | - SceneRegistry grounding, capability binding | + SceneRegistry grounding, endpoint binding | | | +------------------+------------------+ | @@ -60,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: @@ -79,19 +84,21 @@ The boundary is deliberate: | 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 | +| 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 @@ -101,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), recovery_policy=RecoveryPolicy(max_replans=2), ) @@ -116,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 @@ -134,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 ``` @@ -158,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 @@ -167,21 +183,24 @@ from leaking into an Action Agent schema. | Contract | Contains | Does not contain | |---|---|---| | Action-owned goal dataclass | 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, dynamic-collision mode, typed planner options | Execution cadence, skill semantics, or robot-resource names | | `RecoveryPolicy` | Action replan/retry budgets, tracking and dynamic-goal thresholds, action-attempt timeout | Controller state or mutable counters | | `ExecutionRunnerCfg` | Runner-level acknowledgement deadlines, minimum feedback cadence, and completion hold policy | Skill behavior, planning resources, or invocation revision data | | `PlanningContext` | Measured `RobotObservation`, verified `TaskState`, versioned `SceneSnapshot`, stable environment IDs, and optional explicit control cadence for action-owned interpolation | Hypothetical simulator mutation or a planner timing fallback | -| `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. Every planner result that contains positions must also contain per-waypoint -`dt` and a matching per-environment `duration`. Every action passes a -`TimedTrajectory` to `build_plan()`; raw position tensors are rejected. For +`dt`; its per-environment `duration` is derived from those intervals. Every +action passes a `TimedTrajectory` to `build_plan()`; raw position tensors are +rejected. For action-owned deterministic interpolation, the integration supplies its authoritative cadence as `PlanningContext.control_dt` (normally `BaseEnv.step_dt`). The engine never supplies or guesses missing timing. @@ -194,69 +213,78 @@ engine resolves an invocation. There is no marker protocol, shared `ActionTarget` base class, or 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 lowering the selected endpoints to the current core -binding. The `ActionBinding` description below is the resulting direct-core -contract and remains available for advanced manual callers. - -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. - -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: +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: + +```python +resolved = engine.skill_profile.resolve( + "pick_up", + selections={"primary": "left_participant"}, +) +binding = resolved.action_binding +``` + +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. - -For a manually constructed `ActionBinding`, the validation boundary remains -intentionally narrow: the engine verifies required roles, `control_parts` -membership, resolvable joint indices, command type, and command dimensions. A -bound `RobotSkillProfile` adds capability matching, participant endpoint -pairing, command requirements, joint-claim checks, and deterministic -disambiguation before it produces that same core value. - -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 @@ -285,12 +313,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( @@ -298,9 +327,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), + } } } ), @@ -308,9 +339,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. @@ -405,8 +436,10 @@ an older custom action by renaming its implementation to `_plan()`. |---|---|---| | `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` | -| `session.revise_current(invocation)` | Runtime orchestrator or Action Agent | Replaces the active logical call with a newer revision and replans from the latest observed context | -| `runner.step(effect_success=...)` | Non-blocking controller integration | Observes and dispatches only when the next timed command is due | +| `engine.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)` | 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 | @@ -419,23 +452,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), ) 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. @@ -455,14 +496,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, @@ -470,7 +510,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) approach = ActionInvocation( @@ -524,7 +567,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, @@ -540,7 +586,7 @@ 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() ``` @@ -551,11 +597,12 @@ 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() ``` @@ -564,17 +611,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. @@ -649,14 +711,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: @@ -673,13 +733,22 @@ 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 @@ -696,7 +765,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. @@ -718,7 +787,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. @@ -734,7 +803,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 @@ -758,15 +827,17 @@ A new primitive should: 1. define a frozen, action-owned goal dataclass; 2. define a frozen `ActionOptions` subclass only for behavior that can vary per invocation; -3. declare `skill_id`, `GoalType`, `OptionsType`, required semantic roles, and - agent visibility; +3. declare `skill_id`, `GoalType`, `OptionsType`, an explicit + `SkillBindingContract`, and agent visibility; 4. put reusable embodiment commands on control-part profiles and generic motion/recovery choices in invocation policies; 5. implement side-effect-free `_plan(request, context)` using the engine-owned planning services; do not override the framework-owned public `plan()`—the class definition is rejected if it does; -6. return full-robot timed motion, per-environment planning success, optional - named segment metadata, diagnostics, and uncommitted effects; +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. diff --git a/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md b/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md index 4ca68249e..18444e768 100644 --- a/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md +++ b/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md @@ -27,9 +27,6 @@ An atomic action owns a - a {class}`~embodichain.lab.sim.atomic_actions.SkillEndpointRequirement` declares the all-of capabilities and typed semantic commands needed from that participant; -- an optional - {class}`~embodichain.lab.sim.atomic_actions.ActionBindingRoute` lowers a - generic endpoint into the current atomic-action core; and - {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 @@ -189,7 +186,8 @@ 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 the current-core `ActionBinding`. A +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. @@ -253,13 +251,15 @@ 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 command-profile key, -supported binding values, joint IDs when applicable, and adapter-defined claim +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 -`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. +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 @@ -267,19 +267,25 @@ 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. -{class}`ActionBindingRoute` remains a transition into the current core's -`manipulator` and `end_effector` maps. A new non-core controller therefore also -needs one reusable atomic skill/runtime integration for its route and command -transport. Once that shared capability exists, new tasks and robot variants -reuse it through profile and task configuration rather than task-specific -motion code. +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 -current action plans and commands still contain full-robot joint positions, and -the runtime does not merge concurrent command streams. +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 diff --git a/docs/source/overview/sim/planners/motion_generator.md b/docs/source/overview/sim/planners/motion_generator.md index cf8ba02ef..08f71b6fb 100644 --- a/docs/source/overview/sim/planners/motion_generator.md +++ b/docs/source/overview/sim/planners/motion_generator.md @@ -13,7 +13,7 @@ explicit cuRobo world. * **Explicit strategy**: Accepts only `"motion_gen"` or `"ik_interp"`; no planner bypass is inferred from a missing backend-options object. * **Strict timed results**: A planner result with positions must include - per-waypoint `dt` and matching `duration`. The generator validates that + per-waypoint `dt`; `duration` is derived from it. The generator validates that contract, preserves total duration when resampling, and holds failed rows at `start_qpos`. * **Flexible planner selection**: Supports TOPPRA, NeuralPlanner (experimental), and the optional CuroboPlanner backend, which plans on CUDA with either CPU or CUDA physics simulation. @@ -156,7 +156,8 @@ motion_opts = MotionGenOptions( Missing interpolation timing is an error; it is never inferred from an engine or global default. Custom planners likewise must return `PlanResult.dt` with -shape `(B, N)` and `duration == dt.sum(dim=1)` whenever they return positions. +shape `(B, N)` whenever they return positions; `duration` is exposed as the +derived value `dt.sum(dim=1)`. #### Cartesian Space Planning diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index bff87b5d6..f8c756cec 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -14,11 +14,13 @@ demonstrations of every built-in skill, see :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; @@ -27,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; @@ -49,9 +64,9 @@ step. Put invocation-varying behavior in ``ActionInvocation.skill_options``. ``load_builtins=False`` creates an isolated or fully custom engine. Trajectory timing is strict. A planner that returns positions must also return -per-waypoint ``dt`` and matching ``duration``; a custom action must pass a -complete ``TimedTrajectory`` to ``build_plan``. The engine does not repair -missing timing. Action-owned interpolation reads an explicit +per-waypoint ``dt``; ``PlanResult.duration`` is derived from it. A custom action +must pass a complete ``TimedTrajectory`` to ``build_plan``. The engine does not +repair missing timing. Action-owned interpolation reads an explicit ``PlanningContext.control_dt`` supplied by the integration, normally ``BaseEnv.step_dt``. @@ -73,7 +88,7 @@ Application code normally uses one of three engine entry points: - ``ActionPlan`` - Reads one context and does not project a next context * - ``engine.compile()`` - - Planning a fixed sequence whose goals are already known + - Planning a fixed sequence whose goals are known and whose plans retain joint trajectories - ``CompiledTrajectory`` - Propagates hypothetical qpos and expected effects, without observing execution * - ``engine.start()`` @@ -81,10 +96,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 @@ -168,9 +184,10 @@ engine is built: ) ``PickUp``, ``Place``, and the other manipulation skills resolve ``open`` and -``grasp`` from their bound end effector. ``MoveJoints`` resolves a string target -from its bound manipulator. Joint limits validate possible commands, but do not -define their semantic meaning; supply calibrated robot commands in production. +``grasp`` from their bound grasp endpoints. ``MoveJoints`` resolves a string +target from ``primary.motion``. Joint limits validate possible commands, but do +not define their semantic meaning; supply calibrated robot commands in +production. Planning one action ------------------- @@ -182,22 +199,27 @@ application-owned orchestration: .. code-block:: python from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, EndEffectorPoseGoal, MotionPolicy, ) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "left_arm"}}, + ) invocation = ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(xpos=target_pose), - binding=ActionBinding(manipulators={"primary": "left_arm"}), + binding=binding, motion_policy=MotionPolicy(sample_count=80), ) plan = engine.plan(invocation, latest_context) if plan.plan_success.all(): - trajectory = plan.trajectory.positions + command_frames = plan.commands.frames + if plan.joint_trajectory is not None: + trajectory = plan.joint_trajectory.positions diagnostics = plan.diagnostics segments = plan.segments @@ -206,22 +228,24 @@ sequence, call ``compiled.segment(action_index, name)`` to get the corresponding range in concatenated-trajectory coordinates. This is preferable to repeating a primitive's private sample-split formula in application or tutorial code. -The returned :class:`~embodichain.lab.sim.atomic_actions.ActionPlan` describes -only that invocation. Its expected effects are not committed, and ``plan`` does -not produce a projected context for a following action. Use ``compile`` when -the engine should propagate hypothetical state through a sequence. +The returned :class:`~embodichain.lab.sim.atomic_actions.ActionPlan` always owns +a transport-neutral ``commands`` sequence. Joint-planned actions may also retain +``joint_trajectory`` for feedback, inspection, and static qpos projection. The +plan describes only that invocation: expected effects are not committed, and +``plan`` does not produce a projected context for a following action. Use +``compile`` when the engine should propagate hypothetical state through a +sequence. Static compilation ------------------ Use :meth:`~embodichain.lab.sim.atomic_actions.AtomicActionEngine.compile` when -the scene is treated as fixed and all goals in a sequence are known during -planning: +the scene is treated as fixed, all goals in a sequence are known during +planning, and every action retains ``joint_trajectory``: .. code-block:: python from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, @@ -229,7 +253,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) approach = ActionInvocation( @@ -258,6 +285,10 @@ planning: state. Calling it with one invocation is valid, but ``plan`` is simpler when a projected context and sequence-shaped result are unnecessary. +This is intentionally an offline joint-trajectory projection API. It rejects a +generic command plan without ``joint_trajectory``; such plans remain valid for +``plan`` and closed-loop ``start``/``tick`` execution. + Do not compile across a point where later targets depend on physical execution. The coordinated-placement tutorial, for example, compiles both pick-ups, executes them, rebuilds held-object state from measured poses, and then compiles @@ -283,7 +314,10 @@ must be resolved from the latest scene snapshot: goal=EndEffectorPoseGoal( xpos=SceneEntityPose("moving_tray", relative_pose=tray_to_tcp) ), - binding=ActionBinding(manipulators={"primary": "left_arm"}), + binding=engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "left_arm"}}, + ), recovery_policy=RecoveryPolicy( max_replans=3, tracking_error_threshold=0.05, @@ -292,6 +326,7 @@ must be resolved from the latest scene snapshot: ) from embodichain.lab.sim.atomic_actions import ( + EndpointCommandRouter, ExecutionRunner, SimulationExecutionAdapter, TaskState, @@ -314,7 +349,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, @@ -322,10 +358,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. @@ -395,11 +436,24 @@ 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. +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 @@ -454,6 +508,22 @@ A minimal implementation looks like: from dataclasses import dataclass from typing import ClassVar + import torch + + from embodichain.lab.sim.atomic_actions import ( + CARTESIAN_POSE_CAPABILITY, + ActionOptions, + ActionPlan, + AtomicAction, + JointPositionTarget, + PlanningContext, + ResolvedActionRequest, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, + TimedTrajectory, + ) + @dataclass(frozen=True, slots=True) class PushGoal: contact_pose: torch.Tensor @@ -466,7 +536,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) @@ -478,7 +562,12 @@ A minimal implementation looks like: ) -> ActionPlan: goal = self.require_goal(request) options = request.skill_options - # Resolve the bound resource and plan from context.robot.qpos. + motion = request.binding.endpoint("primary", "motion") + motion_target = motion.require_target(JointPositionTarget) + # Plan from context.robot.qpos using motion_target.joint_ids and + # produce full_robot_positions. + # The joint helper lowers the result into RuntimeCommandFrame values + # and retains the trajectory for joint-position feedback. trajectory = TimedTrajectory.from_uniform_step( full_robot_positions, env_ids=context.env_ids, @@ -491,6 +580,13 @@ A minimal implementation looks like: trajectory=trajectory, ) +For a non-joint endpoint, define a typed ``RuntimeEndpointTarget`` and matching +``RuntimeCommandPayload``, have the profile endpoint adapter produce that +target, and call ``build_command_plan(commands=TimedCommandSequence(...))``. +Register the matching ``EndpointCommandTransport`` with the runner's router. +The skill contract, resource graph, binding, runner, and recovery model do not +gain controller-specific fields. + Do not step simulation, mutate ``PlanningContext``, commit ``StateDelta``, or expose planner-specific configuration through the goal. See the in-repository ``add-atomic-action`` skill for the complete checklist. diff --git a/docs/source/tutorial/motion_gen.rst b/docs/source/tutorial/motion_gen.rst index 094e3ebcf..9d550c454 100644 --- a/docs/source/tutorial/motion_gen.rst +++ b/docs/source/tutorial/motion_gen.rst @@ -128,7 +128,9 @@ API Reference ) -> PlanResult - ``strategy="motion_gen"`` delegates to the configured backend; ``strategy="ik_interp"`` performs deterministic waypoint IK and joint interpolation and requires ``interpolation_dt``. -- Returns a normalized, environment-batched ``PlanResult`` with explicit ``dt`` and matching ``duration`` whenever positions are present. Missing timing raises immediately. +- Returns a normalized, environment-batched ``PlanResult`` with explicit ``dt`` + and derived ``duration`` whenever positions are present. Missing timing raises + immediately. - Uses ``target_states`` (list of PlanState) and ``options`` (MotionGenOptions) instead of individual parameters. **interpolate_trajectory** diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index 58cf9fe13..ab6a5f760 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -37,7 +37,12 @@ SlideAffordance, TwistAffordance, ) -from .bindings import ActionBinding, ResolvedActionBinding, ResolvedControlPart +from .bindings import ( + ActionBinding, + EndpointBinding, + JointPositionTarget, + RuntimeEndpointTarget, +) from .control import ( ActionControlOverrides, ControlCommand, @@ -56,20 +61,19 @@ ExecutionSession, ExecutionStatus, ExecutionTick, - JointCommand, ) from .goals import 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 ( - ActionBindingRoute, BATCH_INVERSE_KINEMATICS_CAPABILITY, CARTESIAN_POSE_CAPABILITY, DisjointResourceSlots, @@ -83,6 +87,14 @@ 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, @@ -153,7 +165,6 @@ __all__ = [ "ActionBinding", - "ActionBindingRoute", "ActionControlOverrides", "ActionInvocation", "ActionOptions", @@ -186,10 +197,15 @@ "DisjointResourceSlots", "DisjointSlotEndpoints", "EndEffectorPoseGoal", + "EndpointBinding", + "EndpointCommand", + "EndpointCommandRouter", + "EndpointCommandTransport", "EntityState", "EffectVerificationRequest", "EffectVerifier", "ExecutionClock", + "ExecutionFeedbackMode", "ExecutionEvent", "ExecutionEventKind", "ExecutionRunner", @@ -208,8 +224,9 @@ "INVERSE_KINEMATICS_CAPABILITY", "InteractionPoints", "JointPositionGoal", - "JointCommand", "JointPositionCommand", + "JointPositionPayload", + "JointPositionTarget", "JOINT_POSITION_CAPABILITY", "MotionPolicy", "MonotonicExecutionClock", @@ -246,9 +263,10 @@ "RigidObjectSceneProvider", "RigidObjectSceneProviderCfg", "ResolvedActionRequest", - "ResolvedActionBinding", - "ResolvedControlPart", "RobotObservation", + "RuntimeCommandFrame", + "RuntimeCommandPayload", + "RuntimeEndpointTarget", "RunnerStatus", "RunnerStep", "RunnerStepCallback", @@ -263,6 +281,7 @@ "StateDelta", "SimulationExecutionAdapter", "TaskState", + "TimedCommandSequence", "TimedTrajectory", "TwistAffordance", "TrajectorySegment", diff --git a/embodichain/lab/sim/atomic_actions/bindings.py b/embodichain/lab/sim/atomic_actions/bindings.py index 7457f63ad..badfb26b5 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( num_envs=num_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 614061969..3af145d9c 100644 --- a/embodichain/lab/sim/atomic_actions/control.py +++ b/embodichain/lab/sim/atomic_actions/control.py @@ -149,9 +149,10 @@ def _snapshot_commands( if not isinstance(command, ControlCommand): raise TypeError(f"{field_name} values must be ControlCommand instances.") snapshot = command.snapshot() - if not isinstance(snapshot, ControlCommand): + if type(snapshot) is not type(command) or snapshot is command: raise TypeError( - f"{field_name}[{name!r}].snapshot() must return a ControlCommand." + f"{field_name}[{name!r}].snapshot() must return an independently " + "owned value of the same ControlCommand type." ) snapshots[name] = snapshot return MappingProxyType(snapshots) @@ -194,68 +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 or role != role.strip(): + 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} roles must be non-empty strings without outer " + f"{field_name} slot IDs must be non-empty strings without outer " "whitespace." ) - snapshots[role] = _snapshot_commands( - commands, - field_name=f"{field_name}[{role!r}]", - ) - return MappingProxyType(snapshots) + 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 acb44e32e..fd13e617a 100644 --- a/embodichain/lab/sim/atomic_actions/core.py +++ b/embodichain/lab/sim/atomic_actions/core.py @@ -30,6 +30,7 @@ from embodichain.lab.sim.common import BatchEntity from .affordance import Affordance +from .bindings import EndpointBinding, JointPositionTarget from .effects import StateDelta from .goals import collect_scene_dependencies from .invocation import ( @@ -41,6 +42,7 @@ ) from .plans import ( ActionPlan, + ExecutionFeedbackMode, PlannerDiagnostics, TimedTrajectory, TrajectorySegment, @@ -48,6 +50,12 @@ ) 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 @@ -149,8 +157,6 @@ class SkillDescriptor: skill_id: str goal_type: type[Any] | tuple[type[Any], ...] options_type: type[ActionOptions] - manipulator_roles: tuple[str, ...] = () - end_effector_roles: tuple[str, ...] = () agent_visible: bool = True open_loop: bool = False """Whether completion reports motion execution without physical-effect proof.""" @@ -173,23 +179,12 @@ def __post_init__(self) -> None: ) if not isinstance(self.open_loop, bool): raise TypeError("SkillDescriptor.open_loop must be a bool.") - 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." ) - self.binding_contract.validate_action_roles( - manipulator_roles=self.manipulator_roles, - end_effector_roles=self.end_effector_roles, - ) class AtomicAction(Generic[GoalT, OptionsT], ABC): @@ -209,12 +204,6 @@ 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.""" @@ -323,8 +312,6 @@ def descriptor(cls) -> SkillDescriptor: skill_id=cls.skill_id, goal_type=cls.GoalType, options_type=cls.OptionsType, - manipulator_roles=cls.manipulator_roles, - end_effector_roles=cls.end_effector_roles, agent_visible=cls.agent_visible, open_loop=cls.open_loop, binding_contract=cls.__dict__.get("binding_contract"), @@ -362,10 +349,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 @@ -379,7 +368,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, ), @@ -410,10 +399,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( @@ -432,7 +423,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, @@ -548,32 +545,67 @@ 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. + """ + if not isinstance(commands, TimedCommandSequence): + raise TypeError("commands must be a TimedCommandSequence.") + if commands.batch_size != context.batch_size: + raise ValueError( + "Command sequence and planning context batch sizes must match." + ) + if not torch.equal(commands.env_ids, context.env_ids): + raise ValueError("Command sequence env_ids must match the context.") + success_mask = normalize_success_mask( + success, + num_envs=context.batch_size, + device=self.device, + name="Planning success", + ) + commands = self._authorize_command_targets( + request, + commands, + active_mask=success_mask, + ) + segments = self._build_segments( + segment_lengths, + frame_count=commands.frame_count, + ) if diagnostics is None: diagnostics = PlannerDiagnostics( backend=self.planning_services.planner_name @@ -581,25 +613,218 @@ def build_plan( return ActionPlan( skill_id=self.skill_id, plan_success=success_mask, - trajectory=timed, + commands=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), + 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, - ), + 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, + *, + active_mask: torch.Tensor | None = None, + ) -> TimedCommandSequence: + """Bind every emitted command to an endpoint authorized by the request. + + Actions may choose a subset of their bound endpoints for any frame, but + they cannot synthesize a destination outside the resolved resource + binding. The returned sequence replaces caller-provided target metadata + with the engine-owned binding snapshot, so transports never receive + altered joint claims or other target fields. When ``active_mask`` is + provided, authorization and failed-row masking share the same rebuild. + """ + authorized: dict[tuple[str, str], list[EndpointBinding]] = {} + for endpoint in request.binding.endpoints: + authorized.setdefault(endpoint.destination_key, []).append(endpoint) + unknown = sorted( + { + command.destination_key + for frame in commands.frames + for command in frame.commands + if command.destination_key not in authorized + } + ) + if unknown: + raise ValueError( + "Runtime commands reference destinations not authorized by the " + f"action binding: {unknown}." + ) + + frames: list[RuntimeCommandFrame] = [] + for frame in commands.frames: + endpoint_commands: list[EndpointCommand] = [] + joint_owners: dict[int, tuple[str, str]] = {} + token_owners: dict[str, tuple[str, str]] = {} + for command in frame.commands: + bound_endpoints = authorized[command.destination_key] + target = bound_endpoints[0].target + if any( + type(endpoint.target) is not type(target) + for endpoint in bound_endpoints[1:] + ): + raise ValueError( + f"Action binding destination {command.destination_key} has " + "incompatible target declarations." + ) + if type(command.target) is not type(target): + raise TypeError( + f"Runtime command destination {command.destination_key} uses " + f"target type {type(command.target).__name__}, but its bound " + f"endpoint uses {type(target).__name__}." + ) + if isinstance(target, JointPositionTarget) and command.target != target: + raise ValueError( + f"Runtime command destination {command.destination_key} " + "does not preserve its bound joint-position target." + ) + joint_ids = { + joint_id + for endpoint in bound_endpoints + for joint_id in endpoint.joint_ids + } + claim_tokens = { + token + for endpoint in bound_endpoints + for token in endpoint.claim_tokens + } + overlapping_joints = sorted(joint_ids & joint_owners.keys()) + overlapping_tokens = sorted(claim_tokens & token_owners.keys()) + if overlapping_joints or overlapping_tokens: + conflicting_destinations = sorted( + {joint_owners[joint_id] for joint_id in overlapping_joints} + | {token_owners[token] for token in overlapping_tokens} + ) + raise ValueError( + f"Runtime command destination {command.destination_key} " + f"conflicts with {conflicting_destinations} on bound joint " + f"IDs {overlapping_joints} or claim tokens " + f"{overlapping_tokens}." + ) + for joint_id in joint_ids: + joint_owners[joint_id] = command.destination_key + for token in claim_tokens: + token_owners[token] = command.destination_key + endpoint_commands.append( + EndpointCommand(target=target, payload=command.payload) + ) + frames.append( + RuntimeCommandFrame( + commands=tuple(endpoint_commands), + active_mask=( + frame.active_mask + if active_mask is None + else frame.active_mask & active_mask + ), + env_ids=frame.env_ids, + hold_duration=frame.hold_duration, + ) + ) + return TimedCommandSequence(frames=tuple(frames), env_ids=commands.env_ids) + + def _joint_command_sequence( + self, + request: ResolvedActionRequest[GoalT, OptionsT], + trajectory: TimedTrajectory, + *, + active_mask: torch.Tensor, + ) -> TimedCommandSequence: + """Lower one full-robot planner trajectory to endpoint commands.""" + targets = tuple( + ( + endpoint, + endpoint.require_target(JointPositionTarget), + ) + for endpoint in request.binding.endpoints + ) + if not targets: + raise ValueError( + "Joint trajectory plans require at least one bound " + "JointPositionTarget endpoint." + ) + frames: list[RuntimeCommandFrame] = [] + for waypoint_index in range(trajectory.waypoint_count): + endpoint_commands: list[EndpointCommand] = [] + for _, target in targets: + joint_ids = list(target.joint_ids) + velocities = ( + None + if trajectory.velocities is None + else trajectory.velocities[:, waypoint_index, joint_ids] + ) + endpoint_commands.append( + EndpointCommand( + target=target, + payload=JointPositionPayload( + positions=trajectory.positions[ + :, waypoint_index, joint_ids + ], + velocities=velocities, + ), + ) + ) + next_waypoint_index = min( + waypoint_index + 1, + trajectory.waypoint_count - 1, + ) + # ``dt[:, i]`` is the arrival interval for waypoint ``i``. After + # dispatching it, wait for the next arrival interval; the terminal + # frame deliberately reuses its own interval as a settling window, + # preserving the closed-loop runner's pre-PR2C timing contract. + frames.append( + RuntimeCommandFrame( + commands=tuple(endpoint_commands), + active_mask=active_mask, + env_ids=trajectory.env_ids, + hold_duration=trajectory.dt[:, next_waypoint_index], + ) + ) + return TimedCommandSequence(frames=tuple(frames), env_ids=trajectory.env_ids) + + @staticmethod + def _build_segments( + segment_lengths: Mapping[str, int] | None, + *, + frame_count: int, + ) -> tuple[TrajectorySegment, ...]: + """Validate optional named ranges for one command sequence.""" + if segment_lengths is None: + return () + segments: list[TrajectorySegment] = [] + offset = 0 + for name, length in segment_lengths.items(): + if not isinstance(name, str) or not name: + raise ValueError("Trajectory segment names must be non-empty.") + if isinstance(length, bool) or not isinstance(length, int): + raise TypeError("Trajectory segment lengths must be integers.") + if length < 0: + raise ValueError("Trajectory segment lengths must be non-negative.") + if length == 0: + continue + segments.append( + TrajectorySegment(name=name, start=offset, stop=offset + length) + ) + offset += length + if offset != frame_count: + raise ValueError( + "Trajectory segment lengths must sum to the command frame count " + f"({frame_count}), got {offset}." + ) + return tuple(segments) + def failed_plan( self, request: ResolvedActionRequest[GoalT, OptionsT], @@ -617,23 +842,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/engine.py b/embodichain/lab/sim/atomic_actions/engine.py index 334653823..a7083fcb4 100644 --- a/embodichain/lab/sim/atomic_actions/engine.py +++ b/embodichain/lab/sim/atomic_actions/engine.py @@ -23,10 +23,12 @@ import torch +from .bindings import ActionBinding from .core import AtomicAction, SkillDescriptor -from .control import ControlPartCommandProfile -from .invocation import ActionInvocation, ResolvedActionRequest +from .control import ActionControlOverrides, ControlPartCommandProfile +from .invocation import ActionInvocation, GoalT, OptionsT, ResolvedActionRequest from .plans import ActionPlan, CompiledTrajectory, TimedTrajectory +from .policies import MotionPolicy, RecoveryPolicy from .runtime import ActionPlanningServices from .state import PlanningContext, RobotObservation, SceneSnapshot, TaskState @@ -118,6 +120,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.""" @@ -184,6 +191,121 @@ def bind_skill_profile( 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 make_invocation( + self, + skill_id: str, + goal: GoalT, + *, + control_parts: Mapping[str, Mapping[str, str]] | None = None, + resources: Mapping[str, str] | None = None, + motion_policy: MotionPolicy | None = None, + recovery_policy: RecoveryPolicy | None = None, + skill_options: OptionsT | None = None, + control_overrides: ActionControlOverrides | None = None, + invocation_id: str | None = None, + revision: int = 0, + ) -> ActionInvocation[GoalT, OptionsT]: + """Construct a grounded invocation while naming the skill only once. + + ``control_parts`` uses the advanced direct-core binding path. When it is + omitted, the engine must own a bound robot skill profile; ``resources`` + then optionally selects logical resource IDs by skill-local slot. An + omitted resource selection uses the profile's unique or default binding. + This method resolves bindings only; profile policy presets and runner + configuration remain responsibilities of the semantic runtime layer. + + Args: + skill_id: Stable identifier of an installed atomic skill. + goal: Action-specific typed goal. + control_parts: Optional direct ``slot -> endpoint -> control_part`` + mapping. + resources: Optional profile ``slot -> resource_id`` selections. + motion_policy: Optional invocation motion policy. + recovery_policy: Optional invocation recovery policy. + skill_options: Optional action-specific invocation options. + control_overrides: Optional endpoint-scoped command overrides. + invocation_id: Optional correlation identifier. + revision: Monotonic invocation revision. + + Returns: + A standard :class:`ActionInvocation` accepted by ``plan``, + ``compile``, and ``start``. + + Raises: + ValueError: If binding sources conflict or no binding source is + available. + KeyError: If the skill or an explicitly selected resource is unknown. + TypeError: If an invocation field or binding input has an invalid type. + """ + if control_parts is not None and resources is not None: + raise ValueError("control_parts and resources are mutually exclusive.") + if control_parts is not None: + binding = self.bind_control_parts(skill_id, control_parts) + else: + profile = self.skill_profile + if profile is None: + if resources is not None: + raise ValueError("resources requires a bound RobotSkillProfile.") + raise ValueError( + "control_parts is required when no RobotSkillProfile is bound." + ) + binding = profile.resolve(skill_id, resources).action_binding + + return ActionInvocation( + skill_id=skill_id, + goal=goal, + binding=binding, + motion_policy=MotionPolicy() if motion_policy is None else motion_policy, + recovery_policy=( + RecoveryPolicy() if recovery_policy is None else recovery_policy + ), + skill_options=skill_options, + control_overrides=( + ActionControlOverrides() + if control_overrides is None + else control_overrides + ), + invocation_id=invocation_id, + revision=revision, + ) + def register(self, action: AtomicAction, *, replace: bool = False) -> None: """Register one action instance using its descriptor. @@ -219,6 +341,50 @@ def _load_builtin_actions(self) -> None: for action_type in BUILTIN_ACTION_TYPES: self.register(action_type()) + def plan_action( + self, + action: AtomicAction, + invocation: ActionInvocation, + context: PlanningContext, + ) -> ActionPlan: + """Plan with an unregistered action using this engine's resources. + + This is an advanced extension and testing escape hatch. Built-in + parameter variants should use invocation ``skill_options`` with the + engine's registered implementation. + + Args: + action: Configured action implementation to invoke. + invocation: Grounded request matching the action skill identifier. + context: Latest measured planning state. + + Returns: + Validated side-effect-free action plan. + """ + if not isinstance(action, AtomicAction): + raise TypeError("action must be an AtomicAction instance.") + self._validate_context(context) + action._bind(self._planning_services) + request = action.resolve_request(invocation) + plan = action.plan(request, context) + self._validate_plan(plan, context, request) + return plan + + def resolve( + self, + invocation: ActionInvocation, + ) -> ResolvedActionRequest: + """Resolve a registered invocation into an engine-owned snapshot.""" + return self._resolve(invocation) + + def plan_request( + self, + request: ResolvedActionRequest, + context: PlanningContext | None = None, + ) -> ActionPlan: + """Plan an already-resolved request without rebuilding its snapshot.""" + return self._plan_request(request, context) + def _resolve( self, invocation: ActionInvocation, @@ -375,7 +541,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) @@ -455,15 +629,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 fc38ea97a..8511519a1 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 invocation.revision <= current.revision: - raise ValueError( - f"Revision must advance beyond {current.revision}, got " - f"{invocation.revision}." + 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=invocation.skill_id, + invocation_id=invocation.invocation_id, + revision=invocation.revision, + ) + return self._engine.resolve(invocation) - replacement = self._engine._resolve(invocation) - replacement_plan = self._engine._plan_request(replacement, self._context) + 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, + ) requests = list(self._requests) requests[self._invocation_index] = replacement self._requests = tuple(requests) + self._context = replacement_context self._waypoint_index = 0 self._action_retries.zero_() self._replans.zero_() self._install_plan( replacement_plan, - self._context, + replacement_context, ExecutionEventKind.INVOCATION_REVISED, + destination_continuity_validated=True, ) + def _validate_revision_identity( + self, + *, + skill_id: str, + invocation_id: str | None, + revision: int, + ) -> None: + """Validate identity and ordering shared by staged and direct revisions.""" + current = self._requests[self._invocation_index] + if skill_id != current.skill_id: + raise ValueError( + f"Revision skill_id {skill_id!r} does not match " + f"the active skill {current.skill_id!r}." + ) + if invocation_id != current.invocation_id: + raise ValueError( + "Revision invocation_id must match the active invocation_id." + ) + if revision <= current.revision: + raise ValueError( + f"Revision must advance beyond {current.revision}, got " f"{revision}." + ) + @property def latest_context(self) -> PlanningContext: """Latest validated context with the session's verified task state.""" return self._context @property - def active_trajectory(self) -> TimedTrajectory: - """Return an owned snapshot of the active action trajectory. + def active_commands(self) -> TimedCommandSequence: + """Return an owned snapshot of the active action command sequence. This inspection surface is intended for diagnostics and visualization. Mutating the returned tensors cannot affect execution state. """ assert self._plan is not None - return self._plan.trajectory.snapshot() + return self._plan.commands.snapshot() def trajectory_segment(self, name: str) -> TrajectorySegment: """Return named segment metadata for the active action plan. @@ -360,34 +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, - control_dt=context.control_dt, - ) + 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) @@ -397,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 @@ -422,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) @@ -445,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( @@ -457,12 +513,47 @@ 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, + control_dt=context.control_dt, + ) def _plan_current( self, @@ -479,13 +570,32 @@ def _install_plan( plan: ActionPlan, context: PlanningContext, event_kind: ExecutionEventKind, + *, + destination_continuity_validated: bool = False, ) -> None: - """Install an already validated plan as the current execution plan.""" + """Install a plan, checking target continuity unless already checked.""" + replacement_targets = { + (target.transport_id, target.target_id): target + for target in plan.commands.targets + } + replacement_destinations = frozenset(replacement_targets) + if not destination_continuity_validated: + 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 @@ -493,6 +603,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, @@ -518,9 +689,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 @@ -599,7 +779,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, @@ -616,9 +796,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(): @@ -630,8 +821,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 @@ -645,7 +836,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 @@ -674,11 +865,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, @@ -700,7 +891,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 @@ -708,64 +899,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.""" @@ -903,14 +1114,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, @@ -924,5 +1137,4 @@ def _tick_result( "ExecutionSession", "ExecutionStatus", "ExecutionTick", - "JointCommand", ] diff --git a/embodichain/lab/sim/atomic_actions/invocation.py b/embodichain/lab/sim/atomic_actions/invocation.py index 443a7f44a..f5fde60f9 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 .policies import MotionPolicy, RecoveryPolicy @@ -81,7 +81,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`` @@ -95,7 +95,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.""" @@ -152,7 +152,7 @@ class ResolvedActionRequest(Generic[GoalT, OptionsT]): skill_id: str goal: GoalT - binding: ResolvedActionBinding + binding: ActionBinding motion_policy: MotionPolicy recovery_policy: RecoveryPolicy skill_options: OptionsT @@ -162,8 +162,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): @@ -181,6 +181,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 79259dff1..25cf83c79 100644 --- a/embodichain/lab/sim/atomic_actions/plans.py +++ b/embodichain/lab/sim/atomic_actions/plans.py @@ -20,6 +20,7 @@ import math from dataclasses import dataclass, field +from enum import Enum from types import MappingProxyType from typing import Any, Mapping, Sequence @@ -27,8 +28,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 @@ -94,7 +97,25 @@ 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, "env_ids", self.env_ids.detach().clone()) @property def batch_size(self) -> int: @@ -347,6 +368,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. @@ -386,18 +414,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 @@ -418,21 +448,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 @@ -457,7 +645,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.") @@ -468,7 +656,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 @@ -479,10 +667,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) @@ -511,9 +709,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): @@ -549,7 +748,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: @@ -566,6 +770,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/_binding_contracts.py b/embodichain/lab/sim/atomic_actions/primitives/_binding_contracts.py index d92255e9a..4824d4e80 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/_binding_contracts.py +++ b/embodichain/lab/sim/atomic_actions/primitives/_binding_contracts.py @@ -22,7 +22,6 @@ from ..control import ControlCommand from ..requirements import ( - ActionBindingRoute, DisjointSlotEndpoints, GRASP_CAPABILITY, SkillEndpointRequirement, @@ -35,14 +34,13 @@ def make_motion_slot( *, capabilities: frozenset[str], ) -> SkillResourceSlot: - """Build one current-core manipulator slot.""" + """Build one motion-endpoint resource slot.""" return SkillResourceSlot( slot_id=role, endpoints=( SkillEndpointRequirement( endpoint_id="motion", capabilities=capabilities, - route=ActionBindingRoute("manipulator", role), ), ), ) @@ -61,13 +59,11 @@ def make_manipulation_slot( SkillEndpointRequirement( endpoint_id="motion", capabilities=motion_capabilities, - route=ActionBindingRoute("manipulator", role), ), SkillEndpointRequirement( endpoint_id="grasp", capabilities=frozenset({GRASP_CAPABILITY}), required_commands=grasp_commands, - route=ActionBindingRoute("end_effector", role), ), ), constraints=(DisjointSlotEndpoints(("motion", "grasp")),), diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py index 87fb2004d..935452aaf 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py @@ -27,7 +27,7 @@ from embodichain.utils.math import matrix_from_quat, pose_inv, quat_from_matrix from embodichain.lab.sim.atomic_actions.affordance import AntipodalAffordance -from embodichain.lab.sim.atomic_actions.bindings import ResolvedControlPart +from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget from embodichain.lab.sim.atomic_actions.control import ( GRASP_COMMAND, OPEN_COMMAND, @@ -56,18 +56,18 @@ INVERSE_KINEMATICS_CAPABILITY, SkillBindingContract, ) -from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( - make_manipulation_slot, +from embodichain.lab.sim.atomic_actions.state import HeldObjectState, PlanningContext +from embodichain.lab.sim.atomic_actions.trajectory_ops import ( + interpolate_joint_trajectory, + translate_pose_world, ) from embodichain.lab.sim.atomic_actions.primitives._helpers import ( assemble_full_robot_trajectory, repeat_qpos, resolve_batched_pose, ) -from embodichain.lab.sim.atomic_actions.state import HeldObjectState, PlanningContext -from embodichain.lab.sim.atomic_actions.trajectory_ops import ( - interpolate_joint_trajectory, - translate_pose_world, +from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( + make_manipulation_slot, ) @@ -174,10 +174,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 @@ -352,8 +352,6 @@ class CoordinatedPickment( skill_id: ClassVar[str] = "coordinated_pickment" GoalType: ClassVar[type] = CoordinatedPickGoal OptionsType: ClassVar[type] = CoordinatedPickmentOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("left", "right") - end_effector_roles: ClassVar[tuple[str, ...]] = ("left", "right") binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=tuple( make_manipulation_slot( @@ -401,16 +399,20 @@ def _resolve_resources( ) -> _CoordinatedPickResources: """Resolve left/right roles from robot control parts.""" binding = request.binding - left_arm = binding.manipulator("left") - right_arm = binding.manipulator("right") - left_hand = binding.end_effector("left") - right_hand = binding.end_effector("right") - if left_arm.name == right_arm.name: + left_motion = binding.endpoint("left", "motion") + right_motion = binding.endpoint("right", "motion") + left_grasp = binding.endpoint("left", "grasp") + right_grasp = binding.endpoint("right", "grasp") + left_arm = left_motion.require_target(JointPositionTarget) + right_arm = right_motion.require_target(JointPositionTarget) + left_hand = left_grasp.require_target(JointPositionTarget) + right_hand = right_grasp.require_target(JointPositionTarget) + 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." @@ -420,25 +422,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, num_envs=self.num_envs, device=self.device, dtype=torch.float32, ), - left_hand_close_qpos=left_hand.joint_positions( + left_hand_close_qpos=left_grasp.joint_positions( GRASP_COMMAND, num_envs=self.num_envs, device=self.device, dtype=torch.float32, ), - right_hand_open_qpos=right_hand.joint_positions( + right_hand_open_qpos=right_grasp.joint_positions( OPEN_COMMAND, num_envs=self.num_envs, device=self.device, dtype=torch.float32, ), - right_hand_close_qpos=right_hand.joint_positions( + right_hand_close_qpos=right_grasp.joint_positions( GRASP_COMMAND, num_envs=self.num_envs, device=self.device, @@ -757,12 +759,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( @@ -770,7 +772,7 @@ def _plan_synchronized_object_motion( num_envs=self.num_envs, device=self.device, name=( - f"IK success for {resources.left_arm.name} object waypoint " + f"IK success for {resources.left_arm.control_part} object waypoint " f"{waypoint_idx}" ), ) @@ -779,17 +781,17 @@ def _plan_synchronized_object_motion( num_envs=self.num_envs, device=self.device, name=( - f"IK success for {resources.right_arm.name} object waypoint " + f"IK success for {resources.right_arm.control_part} object waypoint " f"{waypoint_idx}" ), ) self._log_ik_failures( - resources.left_arm.name, + resources.left_arm.control_part, f"object waypoint {waypoint_idx}", success_mask & ~left_success, ) self._log_ik_failures( - resources.right_arm.name, + resources.right_arm.control_part, f"object waypoint {waypoint_idx}", success_mask & ~right_success, ) @@ -866,14 +868,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"], @@ -1018,8 +1020,8 @@ def _plan( ), expected_effects=StateDelta( held_object_updates={ - resources.left_arm.name: left_held_object, - resources.right_arm.name: right_held_object, + resources.left_arm.control_part: left_held_object, + resources.right_arm.control_part: right_held_object, }, ), segment_lengths={ diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py index e86598a0a..e6e2cc4c0 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py @@ -25,7 +25,7 @@ from embodichain.utils import logger -from embodichain.lab.sim.atomic_actions.bindings import ResolvedControlPart +from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget from embodichain.lab.sim.atomic_actions.control import ( GRASP_COMMAND, OPEN_COMMAND, @@ -52,8 +52,10 @@ DisjointResourceSlots, SkillBindingContract, ) -from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( - make_manipulation_slot, +from embodichain.lab.sim.atomic_actions.state import HeldObjectState, PlanningContext +from embodichain.lab.sim.atomic_actions.trajectory_ops import ( + interpolate_hand_qpos, + translate_pose_world, ) from embodichain.lab.sim.atomic_actions.primitives._helpers import ( assemble_full_robot_trajectory, @@ -62,10 +64,8 @@ resolve_batched_pose, resolve_object_target, ) -from embodichain.lab.sim.atomic_actions.state import HeldObjectState, PlanningContext -from embodichain.lab.sim.atomic_actions.trajectory_ops import ( - interpolate_hand_qpos, - translate_pose_world, +from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( + make_manipulation_slot, ) @@ -138,10 +138,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 @@ -155,8 +155,6 @@ 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=( make_manipulation_slot( @@ -185,16 +183,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." @@ -204,19 +206,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, num_envs=self.num_envs, device=self.device, dtype=torch.float32, ), - placing_hand_close_qpos=placing_hand.joint_positions( + placing_hand_close_qpos=placing_grasp.joint_positions( GRASP_COMMAND, num_envs=self.num_envs, device=self.device, dtype=torch.float32, ), - support_hand_close_qpos=support_hand.joint_positions( + support_hand_close_qpos=support_grasp.joint_positions( GRASP_COMMAND, num_envs=self.num_envs, device=self.device, @@ -251,8 +253,8 @@ def _plan( support_held_object, ) = self._resolve_target(target, state, resources, options) eligible = context.task.exclusive_held_object_mask( - resources.placing_arm.name - ) & context.task.exclusive_held_object_mask(resources.support_arm.name) + resources.placing_arm.control_part + ) & context.task.exclusive_held_object_mask(resources.support_arm.control_part) if not eligible.any(): logger.log_warning( "CoordinatedPlacement requires two exclusively held objects." @@ -281,7 +283,7 @@ def _plan( success_mask = eligible.clone() segment_success, placing_approach_traj = plan_named_arm_trajectory( self.motion_generator, - resources.placing_arm.name, + resources.placing_arm.control_part, placing_start_qpos, torch.stack([placing_lift_xpos, placing_xpos], dim=1), segments["approach"], @@ -302,7 +304,7 @@ def _plan( segment_success, support_approach_traj = plan_named_arm_trajectory( self.motion_generator, - resources.support_arm.name, + resources.support_arm.control_part, support_start_qpos, support_xpos.unsqueeze(1), segments["approach"], @@ -362,7 +364,7 @@ def _plan( segment_success, placing_retreat_traj = plan_named_arm_trajectory( self.motion_generator, - resources.placing_arm.name, + resources.placing_arm.control_part, placing_place_qpos, placing_lift_xpos.unsqueeze(1), segments["retreat"], @@ -413,10 +415,10 @@ def _plan( ), 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, }, ), segment_lengths={ @@ -495,8 +497,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: raise ValueError( diff --git a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py index 03266a30e..6e7fa9269 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py +++ b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py @@ -26,7 +26,7 @@ from embodichain.utils import logger from embodichain.utils.math import pose_inv -from embodichain.lab.sim.atomic_actions.bindings import ResolvedControlPart +from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget from embodichain.lab.sim.atomic_actions.control import ( GRASP_COMMAND, OPEN_COMMAND, @@ -53,8 +53,10 @@ FORWARD_KINEMATICS_CAPABILITY, SkillBindingContract, ) -from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( - make_manipulation_slot, +from embodichain.lab.sim.atomic_actions.state import HeldObjectState, PlanningContext +from embodichain.lab.sim.atomic_actions.trajectory_ops import ( + interpolate_hand_qpos, + translate_pose_world, ) from embodichain.lab.sim.atomic_actions.primitives._helpers import ( assemble_full_robot_trajectory, @@ -62,12 +64,10 @@ repeat_qpos, resolve_batched_pose, ) -from embodichain.lab.sim.atomic_actions.primitives.pick_up import GraspGoal -from embodichain.lab.sim.atomic_actions.state import HeldObjectState, PlanningContext -from embodichain.lab.sim.atomic_actions.trajectory_ops import ( - interpolate_hand_qpos, - translate_pose_world, +from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( + make_manipulation_slot, ) +from embodichain.lab.sim.atomic_actions.primitives.pick_up import GraspGoal @dataclass(frozen=True, slots=True, eq=False) @@ -141,10 +141,10 @@ def __post_init__(self) -> None: 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 @@ -163,8 +163,6 @@ class HandOver(AtomicAction[GraspGoal, HandOverOptions]): skill_id: ClassVar[str] = "hand_over" GoalType: ClassVar[type] = GraspGoal OptionsType: ClassVar[type] = HandOverOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("source", "destination") - end_effector_roles: ClassVar[tuple[str, ...]] = ("source", "destination") binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( make_manipulation_slot( @@ -207,16 +205,20 @@ def _resolve_resources( ) -> _HandOverResources: """Resolve source/destination roles from robot control parts.""" binding = request.binding - transfer_arm = binding.manipulator("source") - receive_arm = binding.manipulator("destination") - transfer_hand = binding.end_effector("source") - receive_hand = binding.end_effector("destination") - if transfer_arm.name == receive_arm.name: + transfer_motion = binding.endpoint("source", "motion") + receive_motion = binding.endpoint("destination", "motion") + transfer_grasp = binding.endpoint("source", "grasp") + receive_grasp = binding.endpoint("destination", "grasp") + transfer_arm = transfer_motion.require_target(JointPositionTarget) + receive_arm = receive_motion.require_target(JointPositionTarget) + transfer_hand = transfer_grasp.require_target(JointPositionTarget) + receive_hand = receive_grasp.require_target(JointPositionTarget) + 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." @@ -226,25 +228,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, num_envs=self.num_envs, device=self.device, dtype=torch.float32, ), - transfer_hand_close_qpos=transfer_hand.joint_positions( + transfer_hand_close_qpos=transfer_grasp.joint_positions( GRASP_COMMAND, num_envs=self.num_envs, device=self.device, dtype=torch.float32, ), - receive_hand_open_qpos=receive_hand.joint_positions( + receive_hand_open_qpos=receive_grasp.joint_positions( OPEN_COMMAND, num_envs=self.num_envs, device=self.device, dtype=torch.float32, ), - receive_hand_close_qpos=receive_hand.joint_positions( + receive_hand_close_qpos=receive_grasp.joint_positions( GRASP_COMMAND, num_envs=self.num_envs, device=self.device, @@ -274,14 +276,15 @@ def _plan( "Coordinated dual-arm planning is not supported by the cuRobo backend." ) state = context + transfer_control_part = resources.transfer_arm.control_part transfer_held_object = self._resolve_transfer_held_object( - state, resources.transfer_arm.name + state, transfer_control_part ) self._validate_requested_object( target.semantics, transfer_held_object.semantics ) semantics = transfer_held_object.semantics - eligible = context.task.exclusive_held_object_mask(resources.transfer_arm.name) + eligible = context.task.exclusive_held_object_mask(transfer_control_part) if not eligible.any(): logger.log_warning("HandOver requires an exclusively held source object.") return self.failed_plan( @@ -316,7 +319,7 @@ def _plan( # attachment and the transferring arm's current measured pose. transfer_current_eef = self.robot.compute_fk( qpos=transfer_start_qpos, - name=resources.transfer_arm.name, + name=resources.transfer_arm.control_part, to_matrix=True, ) current_object_pose = torch.bmm( @@ -372,7 +375,7 @@ def _plan( segment_success, transfer_move_traj = plan_named_arm_trajectory( self.motion_generator, - resources.transfer_arm.name, + resources.transfer_arm.control_part, transfer_start_qpos, transfer_middle_eef.unsqueeze(1), segments["transfer"], @@ -391,7 +394,7 @@ def _plan( segment_success, receive_approach_traj = plan_named_arm_trajectory( self.motion_generator, - 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"], @@ -415,7 +418,7 @@ def _plan( segment_success, transfer_retreat_traj = plan_named_arm_trajectory( self.motion_generator, - resources.transfer_arm.name, + resources.transfer_arm.control_part, transfer_hold_qpos, transfer_retreat_eef.unsqueeze(1), segments["deliver"], @@ -436,7 +439,7 @@ def _plan( segment_success, receive_deliver_traj = plan_named_arm_trajectory( self.motion_generator, - resources.receive_arm.name, + resources.receive_arm.control_part, receive_grasp_qpos, receive_final_eef.unsqueeze(1), segments["deliver"], @@ -577,8 +580,8 @@ def _plan( ), 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, 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 b773a5abb..c0a787241 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py @@ -23,6 +23,7 @@ import torch +from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget from embodichain.lab.sim.atomic_actions.core import AtomicAction from embodichain.lab.sim.atomic_actions.goals import ( PoseGoalValue, @@ -38,15 +39,15 @@ CARTESIAN_POSE_CAPABILITY, SkillBindingContract, ) -from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( - make_motion_slot, -) from embodichain.lab.sim.atomic_actions.state import PlanningContext from embodichain.lab.sim.atomic_actions.trajectory_ops import ( build_pose_plan_states, resolve_pose_target, to_full_robot_trajectory, ) +from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( + make_motion_slot, +) @dataclass(frozen=True, slots=True, eq=False) @@ -79,7 +80,6 @@ class MoveEndEffector(AtomicAction[EndEffectorPoseGoal, MoveEndEffectorOptions]) ), ) OptionsType: ClassVar[type] = MoveEndEffectorOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) def _plan( self, @@ -88,9 +88,11 @@ def _plan( ) -> ActionPlan: """Plan an end-effector pose goal from the observed joint state.""" goal = request.goal - 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"), num_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 e1634b1d0..b087951e5 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py @@ -29,6 +29,11 @@ pose_inv, ) +from embodichain.lab.sim.atomic_actions.primitives._helpers import ( + arm_qpos_from_state, + resolve_object_target, +) +from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget from embodichain.lab.sim.atomic_actions.control import ( GRASP_COMMAND, JointPositionCommand, @@ -49,15 +54,11 @@ FORWARD_KINEMATICS_CAPABILITY, SkillBindingContract, ) +from embodichain.lab.sim.atomic_actions.state import PlanningContext +from embodichain.lab.sim.atomic_actions.trajectory_ops import build_pose_plan_states from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( make_manipulation_slot, ) -from embodichain.lab.sim.atomic_actions.primitives._helpers import ( - arm_qpos_from_state, - resolve_object_target, -) -from embodichain.lab.sim.atomic_actions.state import PlanningContext -from embodichain.lab.sim.atomic_actions.trajectory_ops import build_pose_plan_states @dataclass(frozen=True, slots=True, eq=False) @@ -105,8 +106,6 @@ 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=( make_manipulation_slot( @@ -131,12 +130,14 @@ def _plan( target = request.goal options = request.skill_options binding = request.binding - manipulator = binding.manipulator() - end_effector = binding.end_effector() - control_part = manipulator.name - arm_joint_ids = list(manipulator.joint_ids) - hand_joint_ids = list(end_effector.joint_ids) - hand_grasp_qpos = end_effector.joint_positions( + motion = binding.endpoint("primary", "motion") + grasp = binding.endpoint("primary", "grasp") + motion_target = motion.require_target(JointPositionTarget) + grasp_target = grasp.require_target(JointPositionTarget) + 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, num_envs=context.batch_size, device=self.device, diff --git a/embodichain/lab/sim/atomic_actions/primitives/move_joints.py b/embodichain/lab/sim/atomic_actions/primitives/move_joints.py index ec8e5ebbc..fe2d0e4ea 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_joints.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_joints.py @@ -23,6 +23,7 @@ import torch +from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget from embodichain.lab.sim.atomic_actions.core import AtomicAction from embodichain.lab.sim.atomic_actions.invocation import ( ActionOptions, @@ -33,15 +34,15 @@ JOINT_POSITION_CAPABILITY, SkillBindingContract, ) -from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( - make_motion_slot, -) from embodichain.lab.sim.atomic_actions.state import PlanningContext from embodichain.lab.sim.atomic_actions.trajectory_ops import ( build_joint_plan_states, resolve_joint_target, to_full_robot_trajectory, ) +from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( + make_motion_slot, +) @dataclass(frozen=True, slots=True, eq=False) @@ -81,7 +82,6 @@ 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=( @@ -99,10 +99,11 @@ def _plan( ) -> ActionPlan: """Plan a joint-space goal without mutating the robot or task state.""" goal = request.goal - 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, @@ -146,7 +147,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, num_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 b1c4d0fc1..13437db44 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py @@ -32,8 +32,9 @@ quat_from_matrix, ) +from embodichain.lab.sim.atomic_actions.primitives._helpers import arm_qpos_from_state from embodichain.lab.sim.atomic_actions.affordance import AntipodalAffordance -from embodichain.lab.sim.atomic_actions.bindings import ResolvedControlPart +from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget from embodichain.lab.sim.atomic_actions.control import ( GRASP_COMMAND, OPEN_COMMAND, @@ -58,10 +59,6 @@ normalize_success_mask, ) from embodichain.lab.sim.atomic_actions.policies import MotionPolicy -from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( - make_manipulation_slot, -) -from embodichain.lab.sim.atomic_actions.primitives._helpers import arm_qpos_from_state from embodichain.lab.sim.atomic_actions.requirements import ( BATCH_INVERSE_KINEMATICS_CAPABILITY, CARTESIAN_POSE_CAPABILITY, @@ -76,6 +73,9 @@ split_three_segments, translate_pose_world, ) +from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( + make_manipulation_slot, +) @dataclass(frozen=True, slots=True, eq=False) @@ -171,8 +171,6 @@ class PickUp(AtomicAction[GraspGoal, PickUpOptions]): skill_id: ClassVar[str] = "pick_up" GoalType: ClassVar[type] = GraspGoal OptionsType: ClassVar[type] = PickUpOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) - end_effector_roles: ClassVar[tuple[str, ...]] = ("primary",) binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( make_manipulation_slot( @@ -211,8 +209,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, interpolation_dt: float, @@ -232,7 +230,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, interpolation_dt=interpolation_dt, ), @@ -251,7 +249,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, interpolation_dt=interpolation_dt, ), @@ -316,21 +314,23 @@ 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, num_envs=context.batch_size, device=self.device, dtype=context.robot.qpos.dtype, ) - hand_grasp_qpos = end_effector.joint_positions( + hand_grasp_qpos = grasp.joint_positions( GRASP_COMMAND, num_envs=context.batch_size, device=self.device, dtype=context.robot.qpos.dtype, ) - control_part = manipulator.name + control_part = manipulator.control_part state = context sem = target.semantics object_pose = _resolve_object_pose( @@ -424,7 +424,7 @@ def _resolve_grasp_pose( 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]: @@ -477,7 +477,7 @@ def _select_feasible_grasp_variants( 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]: @@ -559,7 +559,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]) @@ -604,22 +604,23 @@ 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.""" num_envs, n_pose, n_variant = poses.shape[:3] flat_poses = poses.reshape(num_envs, n_pose * n_variant, 4, 4) if joint_seed.dim() == 2: joint_seed = joint_seed[:, None, None, :].expand(-1, n_pose, n_variant, -1) - flat_seed = joint_seed.reshape(num_envs, n_pose * n_variant, manipulator.dof) + manipulator_dof = len(manipulator.joint_ids) + flat_seed = joint_seed.reshape(num_envs, n_pose * n_variant, manipulator_dof) is_success, qpos = self.robot.compute_batch_ik( pose=flat_poses, - name=manipulator.name, + name=manipulator.control_part, joint_seed=flat_seed, ) return ( is_success.reshape(num_envs, n_pose, n_variant), - qpos.reshape(num_envs, n_pose, n_variant, manipulator.dof), + qpos.reshape(num_envs, n_pose, n_variant, manipulator_dof), ) def _upright_adjusted_grasp_poses( diff --git a/embodichain/lab/sim/atomic_actions/primitives/place.py b/embodichain/lab/sim/atomic_actions/primitives/place.py index 0204f76fc..8d744afe0 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/place.py +++ b/embodichain/lab/sim/atomic_actions/primitives/place.py @@ -26,7 +26,12 @@ from embodichain.utils.math import quat_error_magnitude, quat_from_matrix +from embodichain.lab.sim.atomic_actions.primitives._helpers import ( + arm_qpos_from_state, + resolve_object_target, +) from embodichain.lab.sim.atomic_actions.affordance import AssembleAffordance +from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget from embodichain.lab.sim.atomic_actions.control import ( GRASP_COMMAND, OPEN_COMMAND, @@ -45,18 +50,11 @@ ResolvedActionRequest, ) from embodichain.lab.sim.atomic_actions.plans import ActionPlan, TimedTrajectory -from embodichain.lab.sim.atomic_actions.primitives._helpers import ( - arm_qpos_from_state, - resolve_object_target, -) from embodichain.lab.sim.atomic_actions.requirements import ( CARTESIAN_POSE_CAPABILITY, FORWARD_KINEMATICS_CAPABILITY, SkillBindingContract, ) -from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( - make_manipulation_slot, -) from embodichain.lab.sim.atomic_actions.state import PlanningContext from embodichain.lab.sim.atomic_actions.trajectory_ops import ( build_pose_plan_states, @@ -64,6 +62,9 @@ resolve_pose_target, split_three_segments, ) +from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( + make_manipulation_slot, +) TcpSymmetry = Literal["none", "z_roll_180"] @@ -173,8 +174,6 @@ class Place(AtomicAction[PlaceGoal | AssembleGoal, PlaceOptions]): AssembleGoal, ) OptionsType: ClassVar[type] = PlaceOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) - end_effector_roles: ClassVar[tuple[str, ...]] = ("primary",) binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( make_manipulation_slot( @@ -213,18 +212,21 @@ def _plan( target = request.goal options = request.skill_options binding = request.binding - manipulator = binding.manipulator() - end_effector = binding.end_effector() - control_part = manipulator.name - arm_joint_ids = list(manipulator.joint_ids) - hand_joint_ids = list(end_effector.joint_ids) - hand_open_qpos = end_effector.joint_positions( + motion_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, num_envs=context.batch_size, device=self.device, dtype=context.robot.qpos.dtype, ) - hand_grasp_qpos = end_effector.joint_positions( + hand_grasp_qpos = grasp.joint_positions( GRASP_COMMAND, num_envs=context.batch_size, device=self.device, diff --git a/embodichain/lab/sim/atomic_actions/primitives/press.py b/embodichain/lab/sim/atomic_actions/primitives/press.py index 6f119235c..e03e4e537 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/press.py +++ b/embodichain/lab/sim/atomic_actions/primitives/press.py @@ -24,7 +24,9 @@ import torch +from embodichain.lab.sim.atomic_actions.primitives._helpers import arm_qpos_from_state from embodichain.lab.sim.atomic_actions.affordance import PressAffordance +from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget from embodichain.lab.sim.atomic_actions.control import ( GRASP_COMMAND, JointPositionCommand, @@ -42,10 +44,6 @@ ResolvedActionRequest, ) from embodichain.lab.sim.atomic_actions.plans import ActionPlan, TimedTrajectory -from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( - make_manipulation_slot, -) -from embodichain.lab.sim.atomic_actions.primitives._helpers import arm_qpos_from_state from embodichain.lab.sim.atomic_actions.requirements import ( CARTESIAN_POSE_CAPABILITY, FORWARD_KINEMATICS_CAPABILITY, @@ -60,6 +58,9 @@ resolve_pose_target, translate_pose_world, ) +from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( + make_manipulation_slot, +) @dataclass(frozen=True, slots=True, eq=False) @@ -120,8 +121,6 @@ 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",) open_loop: ClassVar[bool] = True binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( @@ -173,13 +172,18 @@ def _plan( affordance = self._require_press_affordance(target.semantics) options = request.skill_options interpolation_dt = context.require_control_dt() - manipulator = request.binding.manipulator() - end_effector = request.binding.end_effector() - arm_joint_ids = list(manipulator.joint_ids) - hand_joint_ids = list(end_effector.joint_ids) + binding = request.binding + motion_target = binding.endpoint("primary", "motion").require_target( + JointPositionTarget + ) + grasp = binding.endpoint("primary", "grasp") + grasp_target = grasp.require_target(JointPositionTarget) + control_part = motion_target.control_part + arm_joint_ids = list(motion_target.joint_ids) + hand_joint_ids = list(grasp_target.joint_ids) start_arm_qpos = arm_qpos_from_state(context, arm_joint_ids) start_hand_qpos = context.last_qpos[:, hand_joint_ids] - hand_grasp_qpos = end_effector.joint_positions( + hand_grasp_qpos = grasp.joint_positions( GRASP_COMMAND, num_envs=context.batch_size, device=self.device, @@ -198,7 +202,7 @@ def _plan( contact_xpos = self._find_symmetric_nearest_xpos( contact_xpos, reference_xpos=self.robot.compute_fk( - qpos=start_arm_qpos, name=manipulator.name, to_matrix=True + qpos=start_arm_qpos, name=control_part, to_matrix=True ), ) approach_xpos = translate_pose_world( @@ -221,7 +225,7 @@ def _plan( approach_success, approach_arm = self._plan_pose_segment( approach_xpos, start_arm_qpos, - manipulator.name, + control_part, request, n_approach, interpolation_dt=interpolation_dt, @@ -235,7 +239,7 @@ def _plan( contact_success, contact_arm = self._plan_pose_segment( contact_keyframes, approach_arm[:, -1], - manipulator.name, + control_part, request, n_contact, interpolation_dt=interpolation_dt, @@ -250,7 +254,7 @@ def _plan( press_success, press_arm = self._plan_pose_segment( press_keyframes, contact_arm[:, -1], - manipulator.name, + control_part, request, n_press, interpolation_dt=interpolation_dt, @@ -265,7 +269,7 @@ def _plan( retract_success, retract_arm = self._plan_pose_segment( retract_keyframes, press_arm[:, -1], - manipulator.name, + control_part, request, n_retract, interpolation_dt=interpolation_dt, diff --git a/embodichain/lab/sim/atomic_actions/primitives/slide.py b/embodichain/lab/sim/atomic_actions/primitives/slide.py index 91e61fadc..574372f9f 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/slide.py +++ b/embodichain/lab/sim/atomic_actions/primitives/slide.py @@ -25,6 +25,7 @@ import torch from embodichain.lab.sim.atomic_actions.affordance import SlideAffordance +from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget from embodichain.lab.sim.atomic_actions.control import ( GRASP_COMMAND, OPEN_COMMAND, @@ -47,15 +48,13 @@ TimedTrajectory, normalize_success_mask, ) +from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( + make_manipulation_slot, +) from embodichain.lab.sim.atomic_actions.primitives._helpers import arm_qpos_from_state from embodichain.lab.sim.atomic_actions.requirements import ( - ActionBindingRoute, CARTESIAN_POSE_CAPABILITY, - DisjointSlotEndpoints, - GRASP_CAPABILITY, SkillBindingContract, - SkillEndpointRequirement, - SkillResourceSlot, ) from embodichain.lab.sim.atomic_actions.state import PlanningContext from embodichain.lab.sim.atomic_actions.trajectory_ops import ( @@ -118,30 +117,16 @@ class Slide(AtomicAction[SlideGoal, SlideOptions]): skill_id: ClassVar[str] = "slide" GoalType: ClassVar[type] = SlideGoal OptionsType: ClassVar[type] = SlideOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) - end_effector_roles: ClassVar[tuple[str, ...]] = ("primary",) open_loop: ClassVar[bool] = True binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( - SkillResourceSlot( - slot_id="primary", - endpoints=( - SkillEndpointRequirement( - endpoint_id="motion", - capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), - route=ActionBindingRoute("manipulator", "primary"), - ), - SkillEndpointRequirement( - endpoint_id="grasp", - capabilities=frozenset({GRASP_CAPABILITY}), - required_commands={ - OPEN_COMMAND: JointPositionCommand, - GRASP_COMMAND: JointPositionCommand, - }, - route=ActionBindingRoute("end_effector", "primary"), - ), - ), - constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + make_manipulation_slot( + "primary", + motion_capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + grasp_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, ), ), ) @@ -170,18 +155,23 @@ def _plan( affordance = self._require_slide_affordance(target.semantics) options = request.skill_options interpolation_dt = context.require_control_dt() - manipulator = request.binding.manipulator() - end_effector = request.binding.end_effector() - arm_joint_ids = list(manipulator.joint_ids) - hand_joint_ids = list(end_effector.joint_ids) + binding = request.binding + motion_target = binding.endpoint("primary", "motion").require_target( + JointPositionTarget + ) + grasp = binding.endpoint("primary", "grasp") + grasp_target = grasp.require_target(JointPositionTarget) + control_part = motion_target.control_part + arm_joint_ids = list(motion_target.joint_ids) + hand_joint_ids = list(grasp_target.joint_ids) start_arm_qpos = arm_qpos_from_state(context, arm_joint_ids) - hand_open_qpos = end_effector.joint_positions( + hand_open_qpos = grasp.joint_positions( OPEN_COMMAND, num_envs=context.batch_size, device=self.device, dtype=context.robot.qpos.dtype, ) - hand_grasp_qpos = end_effector.joint_positions( + hand_grasp_qpos = grasp.joint_positions( GRASP_COMMAND, num_envs=context.batch_size, device=self.device, @@ -233,7 +223,7 @@ def _plan( approach_success, approach_arm = self._plan_pose_segment( approach_xpos, start_arm_qpos, - manipulator.name, + control_part, request, motion_lengths[0], interpolation_dt=interpolation_dt, @@ -247,7 +237,7 @@ def _plan( reach_success, reach_arm = self._plan_pose_segment( reach_keyframes, approach_arm[:, -1], - manipulator.name, + control_part, request, motion_lengths[1], interpolation_dt=interpolation_dt, @@ -262,7 +252,7 @@ def _plan( translate_success, translate_arm = self._plan_pose_segment( translate_keyframes, reach_arm[:, -1], - manipulator.name, + control_part, request, motion_lengths[2], interpolation_dt=interpolation_dt, @@ -281,7 +271,7 @@ def _plan( return_success, return_arm = self._plan_pose_segment( return_keyframes, translate_arm[:, -1], - manipulator.name, + control_part, request, motion_lengths[3], interpolation_dt=interpolation_dt, diff --git a/embodichain/lab/sim/atomic_actions/primitives/twist.py b/embodichain/lab/sim/atomic_actions/primitives/twist.py index 5ac73f196..fed1b2f98 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/twist.py +++ b/embodichain/lab/sim/atomic_actions/primitives/twist.py @@ -31,6 +31,7 @@ ) from embodichain.lab.sim.atomic_actions.affordance import TwistAffordance +from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget from embodichain.lab.sim.atomic_actions.control import ( GRASP_COMMAND, OPEN_COMMAND, @@ -49,16 +50,14 @@ ResolvedActionRequest, ) from embodichain.lab.sim.atomic_actions.plans import ActionPlan, TimedTrajectory +from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( + make_manipulation_slot, +) from embodichain.lab.sim.atomic_actions.primitives._helpers import arm_qpos_from_state from embodichain.lab.sim.atomic_actions.requirements import ( - ActionBindingRoute, CARTESIAN_POSE_CAPABILITY, - DisjointSlotEndpoints, FORWARD_KINEMATICS_CAPABILITY, - GRASP_CAPABILITY, SkillBindingContract, - SkillEndpointRequirement, - SkillResourceSlot, ) from embodichain.lab.sim.atomic_actions.state import PlanningContext from embodichain.lab.sim.atomic_actions.trajectory_ops import ( @@ -118,35 +117,21 @@ class Twist(AtomicAction[TwistGoal, TwistOptions]): skill_id: ClassVar[str] = "twist" GoalType: ClassVar[type] = TwistGoal OptionsType: ClassVar[type] = TwistOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) - end_effector_roles: ClassVar[tuple[str, ...]] = ("primary",) open_loop: ClassVar[bool] = True binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( - SkillResourceSlot( - slot_id="primary", - endpoints=( - SkillEndpointRequirement( - endpoint_id="motion", - capabilities=frozenset( - { - CARTESIAN_POSE_CAPABILITY, - FORWARD_KINEMATICS_CAPABILITY, - } - ), - route=ActionBindingRoute("manipulator", "primary"), - ), - SkillEndpointRequirement( - endpoint_id="grasp", - capabilities=frozenset({GRASP_CAPABILITY}), - required_commands={ - OPEN_COMMAND: JointPositionCommand, - GRASP_COMMAND: JointPositionCommand, - }, - route=ActionBindingRoute("end_effector", "primary"), - ), + make_manipulation_slot( + "primary", + motion_capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } ), - constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + grasp_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, ), ), ) @@ -186,18 +171,23 @@ def _plan( affordance = self._require_twist_affordance(target.semantics) options = request.skill_options interpolation_dt = context.require_control_dt() - manipulator = request.binding.manipulator() - end_effector = request.binding.end_effector() - arm_joint_ids = list(manipulator.joint_ids) - hand_joint_ids = list(end_effector.joint_ids) + binding = request.binding + motion_target = binding.endpoint("primary", "motion").require_target( + JointPositionTarget + ) + grasp = binding.endpoint("primary", "grasp") + grasp_target = grasp.require_target(JointPositionTarget) + control_part = motion_target.control_part + arm_joint_ids = list(motion_target.joint_ids) + hand_joint_ids = list(grasp_target.joint_ids) start_arm_qpos = arm_qpos_from_state(context, arm_joint_ids) - hand_open_qpos = end_effector.joint_positions( + hand_open_qpos = grasp.joint_positions( OPEN_COMMAND, num_envs=context.batch_size, device=self.device, dtype=context.robot.qpos.dtype, ) - hand_grasp_qpos = end_effector.joint_positions( + hand_grasp_qpos = grasp.joint_positions( GRASP_COMMAND, num_envs=context.batch_size, device=self.device, @@ -215,7 +205,7 @@ def _plan( grasp_xpos = self._find_symmetric_nearest_xpos( grasp_xpos, reference_xpos=self.robot.compute_fk( - qpos=start_arm_qpos, name=manipulator.name, to_matrix=True + qpos=start_arm_qpos, name=control_part, to_matrix=True ), ) pre_grasp_xpos = translate_pose_world( @@ -239,7 +229,7 @@ def _plan( approach_success, approach_arm = self._plan_pose_segment( pre_grasp_xpos, start_arm_qpos, - manipulator.name, + control_part, request, n_approach, interpolation_dt=interpolation_dt, @@ -247,7 +237,7 @@ def _plan( reach_success, reach_arm = self._plan_pose_segment( grasp_xpos, approach_arm[:, -1], - manipulator.name, + control_part, request, n_reach, interpolation_dt=interpolation_dt, @@ -255,7 +245,7 @@ def _plan( twist_success, twist_arm = self._plan_pose_segment( twist_xpos, reach_arm[:, -1], - manipulator.name, + control_part, request, n_twist, interpolation_dt=interpolation_dt, @@ -263,7 +253,7 @@ def _plan( retract_success, retract_arm = self._plan_pose_segment( pre_grasp_xpos, twist_arm[:, -1], - manipulator.name, + control_part, request, n_retract, interpolation_dt=interpolation_dt, diff --git a/embodichain/lab/sim/atomic_actions/requirements.py b/embodichain/lab/sim/atomic_actions/requirements.py index 1e12aa610..7b62233d2 100644 --- a/embodichain/lab/sim/atomic_actions/requirements.py +++ b/embodichain/lab/sim/atomic_actions/requirements.py @@ -20,7 +20,7 @@ from dataclasses import dataclass, field from types import MappingProxyType -from typing import Literal, Mapping +from typing import Mapping from .control import ControlCommand @@ -69,34 +69,6 @@ def _normalize_identifiers( return normalized -@dataclass(frozen=True, slots=True) -class ActionBindingRoute: - """Lower one generic resource endpoint into the current action core. - - This is deliberately a transition adapter. Robot resources and skill-local - slots remain generic; only this route names the two maps currently exposed - by :class:`~embodichain.lab.sim.atomic_actions.ActionBinding`. - """ - - target: Literal["manipulator", "end_effector"] - """Current core binding namespace.""" - - role: str - """Action-local role within the selected namespace.""" - - def __post_init__(self) -> None: - if self.target not in ("manipulator", "end_effector"): - raise ValueError( - "ActionBindingRoute.target must be 'manipulator' or 'end_effector'." - ) - _validate_identifier(self.role, field_name="ActionBindingRoute.role") - - @property - def key(self) -> tuple[str, str]: - """Return the normalized core target key.""" - return self.target, self.role - - def _normalize_required_commands( values: Mapping[str, type[ControlCommand]], ) -> Mapping[str, type[ControlCommand]]: @@ -129,9 +101,6 @@ class SkillEndpointRequirement: required_commands: Mapping[str, type[ControlCommand]] = field(default_factory=dict) """Semantic command names and their required typed command contracts.""" - route: ActionBindingRoute | None = None - """Optional lowering route into the current atomic-action core.""" - def __post_init__(self) -> None: _validate_identifier( self.endpoint_id, @@ -150,8 +119,6 @@ def __post_init__(self) -> None: "required_commands", _normalize_required_commands(self.required_commands), ) - if self.route is not None and not isinstance(self.route, ActionBindingRoute): - raise TypeError("route must be an ActionBindingRoute or None.") @dataclass(frozen=True, slots=True) @@ -324,14 +291,6 @@ def __post_init__(self) -> None: f"Resource constraint references unknown slots {unknown}; " f"known slots are {sorted(known_slots)}." ) - routes = [ - endpoint.route.key - for slot in slots - for endpoint in slot.endpoints - if endpoint.route is not None - ] - if len(set(routes)) != len(routes): - raise ValueError("Action binding routes must target unique core roles.") object.__setattr__(self, "slots", slots) object.__setattr__(self, "constraints", constraints) @@ -340,32 +299,8 @@ def slot_ids(self) -> tuple[str, ...]: """Return required slot identifiers in declaration order.""" return tuple(slot.slot_id for slot in self.slots) - def validate_action_roles( - self, - *, - manipulator_roles: tuple[str, ...], - end_effector_roles: tuple[str, ...], - ) -> None: - """Require lowering routes to cover the current core roles exactly.""" - expected = {("manipulator", role) for role in manipulator_roles} - expected.update(("end_effector", role) for role in end_effector_roles) - actual = { - endpoint.route.key - for slot in self.slots - for endpoint in slot.endpoints - if endpoint.route is not None - } - if actual != expected: - missing = sorted(expected - actual) - extra = sorted(actual - expected) - raise ValueError( - "Skill binding routes do not exactly cover the action roles: " - f"missing={missing}, extra={extra}." - ) - __all__ = [ - "ActionBindingRoute", "BATCH_INVERSE_KINEMATICS_CAPABILITY", "CARTESIAN_POSE_CAPABILITY", "DisjointResourceSlots", 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 5cdb269ad..7500ed2ef 100644 --- a/embodichain/lab/sim/atomic_actions/sim_adapter.py +++ b/embodichain/lab/sim/atomic_actions/sim_adapter.py @@ -26,11 +26,12 @@ from embodichain.utils import configclass -from .execution import JointCommand +from .bindings import JointPositionTarget, RuntimeEndpointTarget from .runner import ( CommandAcknowledgement, CommandAckStatus, ) +from .runtime_commands import JointPositionPayload, RuntimeCommandFrame from .scene import SceneProvider from .state import ( EntityState, @@ -265,6 +266,9 @@ class SimulationExecutionAdapter: initial_time: Initial elapsed simulation time in seconds. """ + transport_id = JointPositionTarget.TRANSPORT_ID + payload_type = JointPositionPayload + def __init__( self, simulation: SimulationManager, @@ -406,59 +410,105 @@ 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. Returns: Accepted acknowledgement or a rejected diagnostic. """ - return self._write_command(command, timeout=timeout) + self._validate_timeout(timeout) + try: + 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( + CommandAckStatus.REJECTED, + f"{type(exc).__name__}: {exc}", + ) 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: Accepted acknowledgement or a rejected diagnostic. """ - return self._write_command(command, timeout=timeout) - - def _write_command( - self, - command: JointCommand, - *, - timeout: float, - ) -> CommandAcknowledgement: - """Validate and synchronously write a full-robot joint command.""" 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() @@ -468,10 +518,16 @@ def _write_command( 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: @@ -479,6 +535,13 @@ def cancel(self, *, timeout: float) -> CommandAcknowledgement: actual safe target. """ self._validate_timeout(timeout) + try: + self._validate_targets(targets) + except Exception as exc: + return CommandAcknowledgement( + CommandAckStatus.REJECTED, + f"{type(exc).__name__}: {exc}", + ) return CommandAcknowledgement.accepted_ack( "Simulation commands are synchronous; no queued command remained." ) @@ -508,18 +571,43 @@ def _read_optional_proprioception_tensor( return None return value if isinstance(value, torch.Tensor) else None - def _validate_command(self, command: JointCommand) -> None: - """Validate command identity and shape against the attached robot.""" - if not isinstance(command, JointCommand): - raise TypeError("command must be a JointCommand.") - qpos = self.robot.get_qpos() - if command.positions.shape != qpos.shape: - raise ValueError( - "Command shape must match full robot qpos, " - f"got {tuple(command.positions.shape)} and {tuple(qpos.shape)}." - ) + def _validate_command_frame(self, command: RuntimeCommandFrame) -> None: + """Validate one joint-position frame against the attached robot.""" + if not isinstance(command, RuntimeCommandFrame): + raise TypeError("command must be a RuntimeCommandFrame.") if not torch.equal(command.env_ids, self.env_ids): raise ValueError("Command env_ids must match the simulation adapter.") + self._validate_targets(command.targets) + for endpoint_command in command.commands: + if not isinstance(endpoint_command.payload, JointPositionPayload): + raise TypeError( + "SimulationExecutionAdapter accepts JointPositionPayload only." + ) + + def _validate_targets( + self, + targets: tuple[RuntimeEndpointTarget, ...], + ) -> None: + """Validate joint target ownership and robot dimensions.""" + if isinstance(targets, (str, bytes)): + raise TypeError("targets must be an iterable of runtime targets.") + qpos = self.robot.get_qpos() + seen_joints: set[int] = set() + for target in targets: + if not isinstance(target, JointPositionTarget): + raise TypeError( + "SimulationExecutionAdapter accepts JointPositionTarget only." + ) + if target.transport_id != self.transport_id: + raise ValueError("Target transport does not match this adapter.") + if max(target.joint_ids) >= qpos.shape[1]: + raise ValueError( + f"Target {target.target_id!r} references a joint outside robot DOF." + ) + overlaps = seen_joints.intersection(target.joint_ids) + if overlaps: + raise ValueError(f"Joint targets overlap on IDs {sorted(overlaps)}.") + seen_joints.update(target.joint_ids) @staticmethod def _validate_timeout(timeout: float) -> None: diff --git a/embodichain/lab/sim/atomic_actions/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 cf447b27d..3533c6c3c 100644 --- a/embodichain/lab/sim/planners/base_planner.py +++ b/embodichain/lab/sim/planners/base_planner.py @@ -21,7 +21,7 @@ import functools from abc import ABC, abstractmethod from collections.abc import Mapping -from dataclasses import MISSING +from dataclasses import MISSING, dataclass from typing import Literal from embodichain.utils import logger @@ -29,7 +29,13 @@ from embodichain.lab.sim.sim_manager import SimulationManager from .utils import MoveType, PlanState, PlanResult -__all__ = ["BasePlannerCfg", "PlanOptions", "BasePlanner", "validate_plan_options"] +__all__ = [ + "BasePlannerCfg", + "CollisionWorldInfo", + "PlanOptions", + "BasePlanner", + "validate_plan_options", +] @configclass @@ -46,6 +52,57 @@ class PlanOptions: pass +@dataclass(frozen=True, slots=True) +class CollisionWorldInfo: + """Describe one planner's collision-world integration contract. + + Args: + entity_ids: Every canonical entity ID represented in the planner world. + dynamic_entity_ids: Canonical IDs accepted for per-plan pose updates. + batch_mode: Whether the collision world is shared across environments or + instantiated per environment. ``None`` means the mode is irrelevant + or unspecified. + supports_updates: Whether the planner accepts per-plan dynamic poses via + :meth:`BasePlanner.with_collision_world`. + """ + + entity_ids: tuple[str, ...] = () + dynamic_entity_ids: tuple[str, ...] = () + batch_mode: Literal["shared", "per_env"] | None = None + supports_updates: bool = False + + def __post_init__(self) -> None: + for field_name, entity_ids in ( + ("entity_ids", self.entity_ids), + ("dynamic_entity_ids", self.dynamic_entity_ids), + ): + if not isinstance(entity_ids, tuple) or not all( + isinstance(entity_id, str) + and entity_id + and entity_id == entity_id.strip() + for entity_id in entity_ids + ): + raise TypeError( + f"{field_name} must be a tuple of non-empty strings without " + "outer whitespace." + ) + if len(set(entity_ids)) != len(entity_ids): + raise ValueError(f"{field_name} must contain unique IDs.") + + unknown_dynamic_ids = sorted( + set(self.dynamic_entity_ids).difference(self.entity_ids) + ) + if unknown_dynamic_ids: + raise ValueError( + "dynamic_entity_ids must be a subset of entity_ids; unknown=" + f"{unknown_dynamic_ids}." + ) + if self.batch_mode not in (None, "shared", "per_env"): + raise ValueError("batch_mode must be 'shared', 'per_env', or None.") + if not isinstance(self.supports_updates, bool): + raise TypeError("supports_updates must be a bool.") + + def _infer_batch_size(target_states: list[PlanState]) -> int | None: """Return the leading batch dim B of the first tensor found in target_states, or None if none.""" for s in target_states: @@ -176,22 +233,9 @@ def __init__(self, cfg: BasePlannerCfg): waypoint count. """ - 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.""" + def collision_world_info(self) -> CollisionWorldInfo | None: + """Return the planner's collision-world contract, if it has one.""" return None def supports_move_type(self, move_type: MoveType) -> bool: @@ -241,8 +285,8 @@ def with_collision_world( ) -> PlanOptions: """Attach dynamic obstacle poses to backend planning options. - The base planner does not consume a collision world. Backends declaring - :attr:`supports_collision_world_updates` override this method. + The base planner does not consume a collision world. Backends whose + :attr:`collision_world_info` enables updates override this method. Args: options: Backend-specific options to enrich. @@ -280,11 +324,12 @@ def plan( accelerations. Populated by planners that compute dynamics; may be ``None`` for planners that do not. - dt: torch.Tensor ``(B, N)``, per-point time deltas - - duration: torch.Tensor ``(B,)``, total trajectory duration per env + - duration: derived torch.Tensor ``(B,)``, total trajectory + duration per env - Returning ``positions`` without both timing tensors, or with a - duration that differs from ``dt.sum(dim=1)``, raises at - :class:`PlanResult` construction. + Returning ``positions`` without ``dt`` raises at + :class:`PlanResult` construction. ``duration`` is always derived + from ``dt.sum(dim=1)``. """ logger.log_error("Subclasses must implement plan() method", NotImplementedError) diff --git a/embodichain/lab/sim/planners/curobo/curobo_planner.py b/embodichain/lab/sim/planners/curobo/curobo_planner.py index e914fee6e..f350f5038 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, Literal +from typing import TYPE_CHECKING import torch import yaml @@ -51,6 +51,7 @@ from embodichain.lab.sim.planners.base_planner import ( BasePlanner, BasePlannerCfg, + CollisionWorldInfo, PlanOptions, validate_plan_options, ) @@ -815,7 +816,6 @@ class CuroboPlanner(BasePlanner): """ supported_move_types = frozenset({MoveType.EEF_MOVE, MoveType.JOINT_MOVE}) - supports_collision_world_updates = True @property def preserve_plan_samples(self) -> bool: @@ -828,22 +828,17 @@ 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) + def collision_world_info(self) -> CollisionWorldInfo: + """Return the configured collision-world integration contract.""" + return CollisionWorldInfo( + entity_ids=tuple( + name for name, _ in _named_rigid_objects(self.cfg.world.rigid_objects) + ), + dynamic_entity_ids=tuple(self.cfg.world.dynamic_obstacle_names), + batch_mode="per_env" if self.cfg.world.multi_env else "shared", + supports_updates=True, ) - @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 @@ -2084,12 +2079,10 @@ def _assemble_result( else: positions[b, :1] = start[b] positions[b, 1:] = start[b] - duration = dt.sum(dim=1) return PlanResult( success=alive.to(self.device), positions=positions.to(self.device), dt=dt.to(self.device), - duration=duration.to(self.device), ) # ------------------------------------------------------------------ diff --git a/embodichain/lab/sim/planners/motion_generator.py b/embodichain/lab/sim/planners/motion_generator.py index acccf960f..a2ee41596 100644 --- a/embodichain/lab/sim/planners/motion_generator.py +++ b/embodichain/lab/sim/planners/motion_generator.py @@ -28,6 +28,7 @@ from embodichain.lab.sim.planners import ( BasePlannerCfg, + CollisionWorldInfo, PlanOptions, BasePlanner, ToppraPlanner, @@ -179,6 +180,16 @@ def __init__(self, cfg: MotionGenCfg) -> None: self.robot = self.planner.robot self.device = self.robot.device + @property + def collision_world_info(self) -> CollisionWorldInfo | None: + """Return the selected planner's collision-world contract.""" + info = self.planner.collision_world_info + if info is not None and not isinstance(info, CollisionWorldInfo): + raise TypeError( + "Planner.collision_world_info must be a CollisionWorldInfo or None." + ) + return info + @property def supports_dynamic_collision_world(self) -> bool: """Whether the planner accepts per-plan dynamic obstacle poses. @@ -186,44 +197,20 @@ def supports_dynamic_collision_world(self) -> bool: Returns: ``True`` when the selected planner supports collision-world updates. """ - return getattr(self.planner, "supports_collision_world_updates", False) is True + info = self.collision_world_info + return info is not None and info.supports_updates @property def 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", - ) + info = self.collision_world_info + return () if info is None else info.dynamic_entity_ids @property def collision_world_entity_ids(self) -> tuple[str, ...]: """Return every canonical entity ID in the planner collision world.""" - 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 + info = self.collision_world_info + return () if info is None else info.entity_ids @staticmethod def _validate_collision_pose_keys( @@ -246,13 +233,8 @@ def _validate_collision_pose_keys( @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 + info = self.collision_world_info + return None if info is None else info.batch_mode def bind_collision_world( self, @@ -272,13 +254,15 @@ def bind_collision_world( Raises: ValueError: If the selected planner cannot consume dynamic obstacles. """ - if not self.supports_dynamic_collision_world: + info = self.collision_world_info + if info is None or not info.supports_updates: logger.log_error( f"{type(self.planner).__name__} does not support dynamic " "collision-world updates.", ValueError, ) - configured_ids = self.dynamic_collision_entity_ids + assert info is not None + configured_ids = info.dynamic_entity_ids received_ids = tuple(obstacle_poses) if not all( isinstance(entity_id, str) and entity_id and entity_id == entity_id.strip() @@ -606,7 +590,6 @@ def _generate_ik_interpolation( success=torch.ones(batch_size, dtype=torch.bool, device=device), positions=positions, dt=dt, - duration=dt.sum(dim=1), ) if move_type is not MoveType.EEF_MOVE: @@ -694,7 +677,6 @@ def _generate_ik_interpolation( success=success, positions=positions, dt=dt, - duration=dt.sum(dim=1), ) @staticmethod @@ -824,8 +806,6 @@ def normalize_derivative( velocities = normalize_derivative(result.velocities, "velocities") accelerations = normalize_derivative(result.accelerations, "accelerations") - duration = dt.sum(dim=1) - if start_qpos is not None and not success.all(): held = ( start_qpos.to(dtype=positions.dtype).unsqueeze(1).expand_as(positions) @@ -851,7 +831,6 @@ def normalize_derivative( velocities=velocities, accelerations=accelerations, dt=dt, - duration=duration, ) def _runtime_device(self) -> torch.device: diff --git a/embodichain/lab/sim/planners/neural_planner.py b/embodichain/lab/sim/planners/neural_planner.py index e5a9ffbe0..f5c3d8c90 100644 --- a/embodichain/lab/sim/planners/neural_planner.py +++ b/embodichain/lab/sim/planners/neural_planner.py @@ -483,7 +483,6 @@ def plan( accelerations=accelerations_t, xpos_list=xpos_t, dt=dt, - duration=dt.sum(dim=1), ) def _parse_waypoints( diff --git a/embodichain/lab/sim/planners/toppra_planner.py b/embodichain/lab/sim/planners/toppra_planner.py index 3cc866738..5d612348c 100644 --- a/embodichain/lab/sim/planners/toppra_planner.py +++ b/embodichain/lab/sim/planners/toppra_planner.py @@ -74,7 +74,7 @@ def _toppra_solve_one_env( Returns: dict with ``positions`` ``(N_b, DOF)``, ``velocities``, ``accelerations``, - ``dt`` ``(N_b,)``, ``success`` bool, ``n`` int, ``duration`` float. + ``dt`` ``(N_b,)``, ``success`` bool, and ``n`` int. """ dofs = waypoints.shape[1] vlims, alims = _build_constraint_arrays(vel_constraint, acc_constraint, dofs) @@ -107,7 +107,6 @@ def _toppra_solve_one_env( "dt": np.array([0.0, 0.0], dtype=np.float32), "success": True, "n": 2, - "duration": 0.0, } ss = np.linspace(0.0, 1.0, len(waypoints)) @@ -149,7 +148,6 @@ def _toppra_solve_one_env( "dt": dt, "success": True, "n": len(ts), - "duration": duration, } @@ -162,7 +160,6 @@ def _empty_failure(dofs: int) -> dict: "dt": np.array([0.0, 0.0], dtype=np.float32), "success": False, "n": 2, - "duration": 0.0, } @@ -518,7 +515,6 @@ def _assemble_batched_result(self, results: list[dict], dofs: int) -> PlanResult velocities = np.zeros((b, max_n, dofs), dtype=np.float32) accelerations = np.zeros((b, max_n, dofs), dtype=np.float32) dt = np.zeros((b, max_n), dtype=np.float32) - duration = np.zeros((b,), dtype=np.float32) success = np.zeros((b,), dtype=bool) for i, r in enumerate(results): n = r["n"] @@ -526,7 +522,6 @@ def _assemble_batched_result(self, results: list[dict], dofs: int) -> PlanResult velocities[i, :n] = r["velocities"] accelerations[i, :n] = r["accelerations"] dt[i, :n] = r["dt"] - duration[i] = r["duration"] success[i] = r["success"] # tail-pad: repeat final waypoint for held-pose rows if n < max_n: @@ -539,5 +534,4 @@ def _assemble_batched_result(self, results: list[dict], dofs: int) -> PlanResult velocities=torch.as_tensor(velocities, device=self.device), accelerations=torch.as_tensor(accelerations, device=self.device), dt=torch.as_tensor(dt, device=self.device), - duration=torch.as_tensor(duration, device=self.device), ) diff --git a/embodichain/lab/sim/planners/utils.py b/embodichain/lab/sim/planners/utils.py index 72a8c7ff3..63732bf76 100644 --- a/embodichain/lab/sim/planners/utils.py +++ b/embodichain/lab/sim/planners/utils.py @@ -186,9 +186,9 @@ class MoveType(Enum): class PlanResult: r"""Data class representing the result of a motion plan (env-batched). - A result that contains joint positions must also contain complete timing: - per-sample ``dt`` and a matching per-environment ``duration``. Failed plans - may omit all trajectory fields by leaving ``positions`` as ``None``. + A result that contains joint positions must also contain per-sample ``dt``. + Per-environment :attr:`duration` is derived from those intervals. Failed + plans may omit all trajectory fields by leaving ``positions`` as ``None``. """ success: bool | torch.Tensor = False @@ -209,13 +209,10 @@ class PlanResult: dt: torch.Tensor | None = None """Per-env time deltas, shape ``(B, N)``.""" - duration: torch.Tensor | None = None - """Per-env total duration, shape ``(B,)``.""" - def __post_init__(self) -> None: """Validate the explicit trajectory-timing contract.""" if self.positions is None: - if self.dt is not None or self.duration is not None: + if self.dt is not None: raise ValueError("PlanResult timing requires positions.") return if not isinstance(self.positions, torch.Tensor) or self.positions.dim() != 3: @@ -233,25 +230,11 @@ def __post_init__(self) -> None: raise ValueError("PlanResult.dt and positions must share a device.") if not torch.isfinite(self.dt).all() or (self.dt < 0).any(): raise ValueError("PlanResult.dt must contain finite non-negative values.") - if not isinstance(self.duration, torch.Tensor): - raise ValueError( - "PlanResult with positions requires explicit duration with shape (B,)." - ) - if self.duration.shape != (batch_size,): - raise ValueError(f"PlanResult.duration must have shape ({batch_size},).") - if self.duration.device != self.positions.device: - raise ValueError("PlanResult.duration and positions must share a device.") - if not torch.isfinite(self.duration).all() or (self.duration < 0).any(): - raise ValueError( - "PlanResult.duration must contain finite non-negative values." - ) - if not torch.allclose( - self.duration, - self.dt.sum(dim=1).to(dtype=self.duration.dtype), - rtol=1.0e-4, - atol=1.0e-6, - ): - raise ValueError("PlanResult.duration must equal dt.sum(dim=1).") + + @property + def duration(self) -> torch.Tensor | None: + """Return per-environment duration derived from :attr:`dt`.""" + return None if self.dt is None else self.dt.sum(dim=1) def is_all_success(self) -> bool: """Return True only when every env succeeded.""" diff --git a/embodichain/lab/sim/skills/profiles.py b/embodichain/lab/sim/skills/profiles.py index aa8d042c7..71fa2c4e0 100644 --- a/embodichain/lab/sim/skills/profiles.py +++ b/embodichain/lab/sim/skills/profiles.py @@ -25,7 +25,12 @@ from types import MappingProxyType from typing import ClassVar, Mapping, TYPE_CHECKING -from embodichain.lab.sim.atomic_actions.bindings import ActionBinding +from embodichain.lab.sim.atomic_actions.bindings import ( + ActionBinding, + EndpointBinding, + JointPositionTarget, + RuntimeEndpointTarget, +) from embodichain.lab.sim.atomic_actions.control import ( ControlCommand, ControlPartCommandProfile, @@ -109,10 +114,10 @@ def _snapshot_endpoint_commands( if not isinstance(command, ControlCommand): raise TypeError(f"{field_name} values must be ControlCommand instances.") snapshot = command.snapshot() - if not isinstance(snapshot, ControlCommand): + if type(snapshot) is not type(command) or snapshot is command: raise TypeError( - f"{field_name}[{command_name!r}].snapshot() must return a " - "ControlCommand." + 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) @@ -178,10 +183,10 @@ def __post_init__(self) -> None: @dataclass(frozen=True, slots=True) class EndpointResolution: - """Adapter-produced physical and lowering metadata for one endpoint.""" + """Adapter-produced runtime destination and claim metadata for one endpoint.""" - binding_values: Mapping[str, str] = field(default_factory=dict) - """Values supported for each current or future binding namespace.""" + 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.""" @@ -199,14 +204,28 @@ class EndpointResolution: """Whether this execution endpoint must declare a physical claim.""" def __post_init__(self) -> None: - object.__setattr__( - self, - "binding_values", - _normalize_named_mapping( - self.binding_values, - field_name="EndpointResolution.binding_values", - ), + 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, @@ -238,6 +257,11 @@ def __post_init__(self) -> None: ) 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.") @@ -333,10 +357,10 @@ def resolve( f"capabilities {sorted(declared)}, but has no configured solver." ) return EndpointResolution( - binding_values={ - "manipulator": endpoint.control_part, - "end_effector": endpoint.control_part, - }, + runtime_target=JointPositionTarget( + control_part=endpoint.control_part, + joint_ids=joint_ids, + ), command_profile_key=( endpoint.control_part if endpoint.command_profile is None @@ -354,7 +378,7 @@ class ResolvedResourceEndpoint: endpoint: ResourceEndpoint adapter_id: str - binding_values: Mapping[str, str] = field(default_factory=dict) + runtime_target: RuntimeEndpointTarget command_profile_key: str | None = None requires_command_profile: bool = False commands: Mapping[str, ControlCommand] = field(default_factory=dict) @@ -380,14 +404,14 @@ def __post_init__(self) -> None: field_name="ResolvedResourceEndpoint.adapter_id", ) resolution = EndpointResolution( - binding_values=self.binding_values, + 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, "binding_values", resolution.binding_values) + object.__setattr__(self, "runtime_target", resolution.runtime_target) object.__setattr__( self, "command_profile_key", @@ -420,7 +444,15 @@ def conflicts_with(self, other: ResolvedResourceEndpoint) -> bool: if not isinstance(other, ResolvedResourceEndpoint): raise TypeError("other must be a ResolvedResourceEndpoint.") return bool( - self.claim_tokens & other.claim_tokens + ( + 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) ) @@ -1335,35 +1367,33 @@ def _validate_engine_control_profiles(self) -> None: ) for resource in self._resources.values(): for endpoint in resource.endpoints.values(): - if not endpoint.commands: + if not endpoint.commands or not isinstance( + endpoint.runtime_target, + JointPositionTarget, + ): continue - control_parts = { - value - for target, value in endpoint.binding_values.items() - if target in {"manipulator", "end_effector"} - } - for control_part in control_parts: - installed = engine_profiles.get(control_part) - if installed is None: + 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"Endpoint command profile " - f"{endpoint.command_profile_key!r} for control part " - f"{control_part!r} is not installed on the " - "AtomicActionEngine." + 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." ) - 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.""" @@ -1392,6 +1422,18 @@ def _resolve_resources(self) -> Mapping[str, ResolvedRobotResource]: 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 @@ -1409,7 +1451,7 @@ def _resolve_resources(self) -> Mapping[str, ResolvedRobotResource]: resource_endpoints[endpoint_id] = ResolvedResourceEndpoint( endpoint=endpoint, adapter_id=adapter.adapter_id, - binding_values=resolution.binding_values, + runtime_target=resolution.runtime_target, command_profile_key=resolution.command_profile_key, requires_command_profile=resolution.requires_command_profile, commands=( @@ -1523,7 +1565,7 @@ def _validate_command_shapes( ) def _validate_leaf_ownership(self) -> None: - """Require physical leaf resources to own disjoint adapter claims.""" + """Require physical leaves to own disjoint claims and runtime targets.""" leaves = [ resource for resource in self._resources.values() if not resource.members ] @@ -1543,6 +1585,28 @@ def _validate_leaf_ownership(self) -> None: 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.""" @@ -1684,15 +1748,6 @@ def _rejection_reasons( f"endpoint {requirement.endpoint_id!r} missing capabilities " f"{missing_capabilities}" ) - if ( - requirement.route is not None - and requirement.route.target not in endpoint.binding_values - ): - reasons.append( - f"endpoint {requirement.endpoint_id!r} adapter " - f"{endpoint.adapter_id!r} cannot lower to binding target " - f"{requirement.route.target!r}" - ) for command_name, command_type in requirement.required_commands.items(): command = endpoint.commands.get(command_name) if command is None: @@ -1722,43 +1777,61 @@ def _rejection_reasons( 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}" + f"{overlapping_tokens} or share runtime target " + f"{shared_target}" ) return tuple(reasons) - @staticmethod def _lower_binding( + self, skill_id: str, contract: SkillBindingContract | None, assignment: Mapping[str, ResolvedRobotResource], ) -> ResolvedSkillBinding: - """Lower generic endpoints through the temporary current-core routes.""" + """Lower every required endpoint to one engine-owned action binding.""" assert contract is not None - manipulators: dict[str, str] = {} - end_effectors: dict[str, str] = {} + endpoints: list[EndpointBinding] = [] for slot in contract.slots: resource = assignment[slot.slot_id] for requirement in slot.endpoints: - if requirement.route is None: - continue endpoint = resource.endpoints[requirement.endpoint_id] - target = ( - manipulators - if requirement.route.target == "manipulator" - else end_effectors + 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, + ) ) - target[requirement.route.role] = endpoint.binding_values[ - requirement.route.target - ] return ResolvedSkillBinding( skill_id=skill_id, resources=assignment, action_binding=ActionBinding( - manipulators=manipulators, - end_effectors=end_effectors, + owner_id=self._engine.binding_owner_id, + endpoints=tuple(endpoints), ), claim=ResourceClaim.combine( tuple(resource.claim for resource in assignment.values()) diff --git a/embodichain/lab/sim/skills/scene.py b/embodichain/lab/sim/skills/scene.py index 62d71ec61..93076a03a 100644 --- a/embodichain/lab/sim/skills/scene.py +++ b/embodichain/lab/sim/skills/scene.py @@ -763,13 +763,20 @@ def validate_collision_integration( """ 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 + planner_info = motion_generator.collision_world_info + if planner_info is None: + planner_dynamic_ids = () + planner_world_ids = () + supports_updates = False + planner_mode = None + else: + planner_dynamic_ids = planner_info.dynamic_entity_ids + planner_world_ids = planner_info.entity_ids + supports_updates = planner_info.supports_updates + planner_mode = planner_info.batch_mode except AttributeError as exc: raise TypeError( - "motion_generator must expose collision-world integration properties." + "motion_generator must expose collision_world_info." ) from exc planner_dynamic_ids = self._validate_integration_ids( planner_dynamic_ids, 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 f6dbb4306..0638ffbdc 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, @@ -459,7 +468,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 8a7fb182b..152de4380 100644 --- a/embodichain_tasks/embodichain_tasks/tableware/blocks_ranking_rgb.py +++ b/embodichain_tasks/embodichain_tasks/tableware/blocks_ranking_rgb.py @@ -211,7 +211,6 @@ def _plan_block_segment( ) -> tuple[torch.Tensor, Iterable[torch.Tensor], torch.Tensor]: """Plan an atomic PickUp followed by Place for one block.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, GraspGoal, MotionPolicy, @@ -233,9 +232,19 @@ def _plan_block_segment( source_pose[:, :3, :3], local_grasp_offset.unsqueeze(-1) ).squeeze(-1) grasp_pose[:, :3, 3] = source_pose[:, :3, 3] + world_grasp_offset - binding = ActionBinding( - manipulators={"primary": arm}, - end_effectors={"primary": hand}, + endpoints = { + "primary": { + "motion": arm, + "grasp": hand, + } + } + pick_binding = self._action_engine.bind_control_parts( + "pick_up", + endpoints, + ) + place_binding = self._action_engine.bind_control_parts( + "place", + endpoints, ) pick_compiled = self._action_engine.compile( ( @@ -245,7 +254,7 @@ def _plan_block_segment( self._object_semantics[uid], grasp_xpos=grasp_pose, ), - binding=binding, + binding=pick_binding, motion_policy=MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( pre_grasp_distance=0.12, @@ -278,7 +287,7 @@ def _plan_block_segment( ActionInvocation( skill_id="place", goal=PlaceGoal(place_eef_pose), - binding=binding, + binding=place_binding, motion_policy=MotionPolicy(sample_count=PLACE_SAMPLE_INTERVAL), skill_options=PlaceOptions( lift_height=0.15, diff --git a/embodichain_tasks/embodichain_tasks/tableware/stack_blocks_two.py b/embodichain_tasks/embodichain_tasks/tableware/stack_blocks_two.py index d04794014..7e6021d31 100644 --- a/embodichain_tasks/embodichain_tasks/tableware/stack_blocks_two.py +++ b/embodichain_tasks/embodichain_tasks/tableware/stack_blocks_two.py @@ -133,7 +133,6 @@ def _plan_stack( ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor]: """Plan PickUp then Place while threading the held-object state.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, GraspGoal, MotionPolicy, @@ -156,9 +155,19 @@ def _plan_stack( grasp_pose[:, :3, 3] = source_pose[:, :3, 3] + torch.tensor( GRASP_OFFSET, dtype=torch.float32, device=self.device ) - binding = ActionBinding( - manipulators={"primary": CONTROL_PART}, - end_effectors={"primary": HAND_CONTROL_PART}, + endpoints = { + "primary": { + "motion": CONTROL_PART, + "grasp": HAND_CONTROL_PART, + } + } + pick_binding = self._action_engine.bind_control_parts( + "pick_up", + endpoints, + ) + place_binding = self._action_engine.bind_control_parts( + "place", + endpoints, ) pick_compiled = self._action_engine.compile( ( @@ -168,7 +177,7 @@ def _plan_stack( self._stack_block_semantics, grasp_xpos=grasp_pose, ), - binding=binding, + binding=pick_binding, motion_policy=MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( pre_grasp_distance=0.12, @@ -202,7 +211,7 @@ def _plan_stack( ActionInvocation( skill_id="place", goal=PlaceGoal(place_eef_pose), - binding=binding, + binding=place_binding, motion_policy=MotionPolicy(sample_count=PLACE_SAMPLE_INTERVAL), skill_options=PlaceOptions( lift_height=0.10, diff --git a/examples/sim/planners/curobo_planner.py b/examples/sim/planners/curobo_planner.py index a95e078a9..908f8619a 100644 --- a/examples/sim/planners/curobo_planner.py +++ b/examples/sim/planners/curobo_planner.py @@ -59,7 +59,6 @@ visualization_cfg_from_args, ) from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, @@ -749,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( strategy="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 f92e0b0de..8d88560b0 100644 --- a/scripts/benchmark/atomic_action/move_held_object_benchmark.py +++ b/scripts/benchmark/atomic_action/move_held_object_benchmark.py @@ -175,7 +175,6 @@ def _prepare_held_state( ): """Run PickUp precondition outside the timed MoveHeldObject block.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -212,22 +211,26 @@ def _prepare_held_state( move_position = obj_pose[0, :3, 3].clone() move_position[2] = 0.36 move_target = make_pre_pick_eef_pose(robot, move_position) - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, + move_binding = atomic_engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "arm"}}, + ) + pick_binding = atomic_engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "arm", "grasp": "hand"}}, ) result = atomic_engine.compile( ( ActionInvocation( "move_end_effector", EndEffectorPoseGoal(xpos=move_target), - binding, + move_binding, MotionPolicy(sample_count=MOVE_SAMPLE_INTERVAL), ), ActionInvocation( "pick_up", GraspGoal(semantics=semantics), - binding, + pick_binding, MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( approach_direction=resolve_pickup_approach_direction( @@ -270,7 +273,6 @@ def _run_case( ): """Run one MoveHeldObject benchmark case.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -320,16 +322,17 @@ def _run_case( }, ) target_pose = _make_object_target_pose(sim.device, case.xyz) + binding = atomic_engine.bind_control_parts( + "move_held_object", + {"primary": {"motion": "arm", "grasp": "hand"}}, + ) elapsed, mem_delta, peak_gpu, result = timed_call( lambda: atomic_engine.compile( ( ActionInvocation( skill_id="move_held_object", goal=HeldObjectPoseGoal(object_target_pose=target_pose), - binding=ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ), + binding=binding, motion_policy=MotionPolicy( sample_count=MOVE_HELD_OBJECT_SAMPLE_INTERVAL ), diff --git a/scripts/benchmark/atomic_action/move_joints_benchmark.py b/scripts/benchmark/atomic_action/move_joints_benchmark.py index 82d43c164..abdfcc6bd 100644 --- a/scripts/benchmark/atomic_action/move_joints_benchmark.py +++ b/scripts/benchmark/atomic_action/move_joints_benchmark.py @@ -106,17 +106,19 @@ def _qpos(values, device): return torch.tensor(values, dtype=torch.float32, device=device) -def _targets_for_sequence(sequence_case: JointSequenceCase, device): +def _targets_for_sequence(atomic_engine, sequence_case: JointSequenceCase, device): """Build typed MoveJoints targets for a sequence case.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, JointPositionGoal, MotionPolicy, ) targets = [] - binding = ActionBinding(manipulators={"primary": "arm"}) + binding = atomic_engine.bind_control_parts( + "move_joints", + {"primary": {"motion": "arm"}}, + ) policy = MotionPolicy(sample_count=MOVE_JOINTS_SAMPLE_INTERVAL) for index, name in enumerate(sequence_case.sequence): if index == 0 and name == "ready": @@ -147,7 +149,7 @@ def _run_case( """Run one MoveJoints case.""" torch = ensure_torch() reset_robot(robot, initial_qpos) - steps = _targets_for_sequence(case, sim.device) + steps = _targets_for_sequence(atomic_engine, case, sim.device) elapsed, mem_delta, peak_gpu, result = timed_call( lambda: atomic_engine.compile(steps) ) diff --git a/scripts/benchmark/atomic_action/pickup_benchmark.py b/scripts/benchmark/atomic_action/pickup_benchmark.py index 39c87e67c..245efaaad 100644 --- a/scripts/benchmark/atomic_action/pickup_benchmark.py +++ b/scripts/benchmark/atomic_action/pickup_benchmark.py @@ -123,7 +123,6 @@ def _run_case( ): """Run one PickUp benchmark case.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -175,16 +174,17 @@ def _run_case( build_gripper_collision_cfg=build_gripper_collision_cfg, build_grasp_generator_cfg=build_grasp_generator_cfg, ) + binding = atomic_engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "arm", "grasp": "hand"}}, + ) elapsed, mem_delta, peak_gpu, result = timed_call( lambda: atomic_engine.compile( ( ActionInvocation( skill_id="pick_up", goal=GraspGoal(semantics=semantics), - binding=ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ), + binding=binding, motion_policy=MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( approach_direction=approach_direction, diff --git a/scripts/benchmark/atomic_action/place_benchmark.py b/scripts/benchmark/atomic_action/place_benchmark.py index 5a7311b79..aa73713a9 100644 --- a/scripts/benchmark/atomic_action/place_benchmark.py +++ b/scripts/benchmark/atomic_action/place_benchmark.py @@ -174,7 +174,6 @@ def _prepare_held_state( ): """Run PickUp precondition outside the timed Place block.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -212,9 +211,9 @@ def _prepare_held_state( ActionInvocation( skill_id="pick_up", goal=GraspGoal(semantics=semantics), - binding=ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, + binding=atomic_engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "arm", "grasp": "hand"}}, ), motion_policy=MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( @@ -256,7 +255,6 @@ def _run_case( ): """Run one Place benchmark case.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -309,16 +307,17 @@ def _run_case( }, ) place_pose = _make_place_pose(sim.device, case.xyz) + binding = atomic_engine.bind_control_parts( + "place", + {"primary": {"motion": "arm", "grasp": "hand"}}, + ) elapsed, mem_delta, peak_gpu, result = timed_call( lambda: atomic_engine.compile( ( ActionInvocation( skill_id="place", goal=PlaceGoal(xpos=place_pose), - binding=ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ), + binding=binding, motion_policy=MotionPolicy(sample_count=PLACE_SAMPLE_INTERVAL), skill_options=PlaceOptions( lift_height=PLACE_LIFT_HEIGHT, diff --git a/scripts/benchmark/curobo_extraction/run_benchmark.py b/scripts/benchmark/curobo_extraction/run_benchmark.py index f8d07603b..1cd557ea5 100644 --- a/scripts/benchmark/curobo_extraction/run_benchmark.py +++ b/scripts/benchmark/curobo_extraction/run_benchmark.py @@ -175,8 +175,7 @@ def old_assemble_result( else: positions[b, :1] = start[b] positions[b, 1:] = start[b] - duration = dt.sum(dim=1) - return PlanResult(success=alive, positions=positions, dt=dt, duration=duration) + return PlanResult(success=alive, positions=positions, dt=dt) # ============================================================================= diff --git a/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md b/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md index 8e561933c..ae1a1bddc 100644 --- a/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md +++ b/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md @@ -431,7 +431,7 @@ For Atomic Action tracks, separate `action_planning_ms`, Distinguish three evaluation views: 1. **path-only**: resample by arc length and compare geometry; -2. **native-timing**: use each planner's own `dt/duration`; +2. **native-timing**: use each planner's own `dt` and derived duration; 3. **common-execution**: use the same controller, control dt, and simulator. Do not directly compare NMG's fixed nominal `dt=0.01` against IK interpolation diff --git a/scripts/benchmark/motion_generation/planners/ik_interpolate.py b/scripts/benchmark/motion_generation/planners/ik_interpolate.py index 75972d884..f9ef082b1 100644 --- a/scripts/benchmark/motion_generation/planners/ik_interpolate.py +++ b/scripts/benchmark/motion_generation/planners/ik_interpolate.py @@ -85,7 +85,6 @@ def plan(self, case: BenchmarkCase) -> PlanResult: success=alive, positions=positions, dt=dt, - duration=dt.sum(dim=1), ) diff --git a/scripts/tutorials/atomic_action/assemble.py b/scripts/tutorials/atomic_action/assemble.py index b0cbf56b6..c7429e62a 100644 --- a/scripts/tutorials/atomic_action/assemble.py +++ b/scripts/tutorials/atomic_action/assemble.py @@ -38,8 +38,6 @@ from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, AssembleAffordance, AssembleGoal, AtomicActionEngine, @@ -318,27 +316,24 @@ def run_assemble_demo( assemble_object_entity=can, assemble_to_base_pose=assemble_to_base, ) - binding = ActionBinding( - manipulators={"primary": "left_arm"}, - end_effectors={"primary": "left_hand"}, - ) + endpoint_mapping = {"primary": {"motion": "left_arm", "grasp": "left_hand"}} compiled = engine.compile( ( - ActionInvocation( + engine.make_invocation( "pick_up", GraspGoal(can_semantics), - binding, - MotionPolicy( + control_parts=endpoint_mapping, + motion_policy=MotionPolicy( strategy="motion_gen", sample_count=PICKUP_SAMPLE_INTERVAL, ), skill_options=pick_up_options, ), - ActionInvocation( + engine.make_invocation( "place", AssembleGoal(affordance=assemble_affordance), - binding, - MotionPolicy( + control_parts=endpoint_mapping, + motion_policy=MotionPolicy( strategy="motion_gen", sample_count=PLACE_SAMPLE_INTERVAL, ), diff --git a/scripts/tutorials/atomic_action/control_dt.py b/scripts/tutorials/atomic_action/control_dt.py index 2e1615fbc..617567cdc 100644 --- a/scripts/tutorials/atomic_action/control_dt.py +++ b/scripts/tutorials/atomic_action/control_dt.py @@ -29,8 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, AtomicActionEngine, JointPositionGoal, MotionPolicy, @@ -82,10 +80,10 @@ def main() -> None: arm_limits[:, 1], ) - invocation = ActionInvocation( - skill_id="move_joints", - goal=JointPositionGoal(target_arm_qpos), - binding=ActionBinding(manipulators={"primary": "arm"}), + invocation = engine.make_invocation( + "move_joints", + JointPositionGoal(target_arm_qpos), + control_parts={"primary": {"motion": "arm"}}, motion_policy=MotionPolicy( strategy="ik_interp", sample_count=SAMPLE_COUNT, diff --git a/scripts/tutorials/atomic_action/coordinated_pickment.py b/scripts/tutorials/atomic_action/coordinated_pickment.py index 8c47a81c5..2b52be831 100644 --- a/scripts/tutorials/atomic_action/coordinated_pickment.py +++ b/scripts/tutorials/atomic_action/coordinated_pickment.py @@ -37,8 +37,6 @@ from embodichain.lab.sim import SimulationManager from embodichain.data import get_data_path from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, CoordinatedPickGoal, @@ -444,14 +442,14 @@ def run_coordinated_pickment_demo( start_time = time.time() compiled = engine.compile( ( - ActionInvocation( + engine.make_invocation( "coordinated_pickment", pickment_target, - ActionBinding( - manipulators={"left": "left_arm", "right": "right_arm"}, - end_effectors={"left": "left_hand", "right": "right_hand"}, - ), - MotionPolicy( + control_parts={ + "left": {"motion": "left_arm", "grasp": "left_hand"}, + "right": {"motion": "right_arm", "grasp": "right_hand"}, + }, + motion_policy=MotionPolicy( strategy="motion_gen", sample_count=PICKMENT_SAMPLE_INTERVAL, ), diff --git a/scripts/tutorials/atomic_action/coordinated_placement.py b/scripts/tutorials/atomic_action/coordinated_placement.py index b3775cff9..f24f8aba9 100644 --- a/scripts/tutorials/atomic_action/coordinated_placement.py +++ b/scripts/tutorials/atomic_action/coordinated_placement.py @@ -38,8 +38,6 @@ from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, CoordinatedPlacementOptions, @@ -618,32 +616,26 @@ def run_coordinated_placement_demo( z_clearance=PAN_GRASP_Z_CLEARANCE, ) pick_invocations = ( - ActionInvocation( - skill_id="pick_up", - goal=GraspGoal( + engine.make_invocation( + "pick_up", + GraspGoal( semantics=bread_semantics, grasp_xpos=broadcast_pose_batch(bread_grasp_pose, num_envs=num_envs), ), - binding=ActionBinding( - manipulators={"primary": "left_arm"}, - end_effectors={"primary": "left_hand"}, - ), + control_parts={"primary": {"motion": "left_arm", "grasp": "left_hand"}}, motion_policy=MotionPolicy( strategy="motion_gen", sample_count=PICK_SAMPLE_INTERVAL, ), skill_options=left_pick_options, ), - ActionInvocation( - skill_id="pick_up", - goal=GraspGoal( + engine.make_invocation( + "pick_up", + GraspGoal( semantics=pan_semantics, grasp_xpos=broadcast_pose_batch(pan_grasp_pose, num_envs=num_envs), ), - binding=ActionBinding( - manipulators={"primary": "right_arm"}, - end_effectors={"primary": "right_hand"}, - ), + control_parts={"primary": {"motion": "right_arm", "grasp": "right_hand"}}, motion_policy=MotionPolicy( strategy="motion_gen", sample_count=PAN_PICK_SAMPLE_INTERVAL, @@ -666,8 +658,12 @@ def run_coordinated_placement_demo( if not pick_compiled.plan_success.all(): logger.log_warning("Failed to plan right pan pick-up trajectory.") return - left_pick_traj = left_pick_result.trajectory.positions - right_pick_traj = right_pick_result.trajectory.positions + left_pick_trajectory = left_pick_result.joint_trajectory + right_pick_trajectory = right_pick_result.joint_trajectory + if left_pick_trajectory is None or right_pick_trajectory is None: + raise RuntimeError("PickUp did not produce joint trajectories.") + left_pick_traj = left_pick_trajectory.positions + right_pick_traj = right_pick_trajectory.positions state = pick_compiled.projected_context bread_held_state = state.get_held_object("left_arm") if bread_held_state is None: @@ -693,7 +689,7 @@ def log_trajectory_execution(step_idx: int, total_steps: int) -> None: replay_trajectory( sim, robot, - left_pick_result.trajectory, + left_pick_trajectory, args, video_prefix="", hold_steps=0, @@ -706,7 +702,7 @@ def log_trajectory_execution(step_idx: int, total_steps: int) -> None: replay_trajectory( sim, robot, - right_pick_result.trajectory, + right_pick_trajectory, args, video_prefix="", hold_steps=0, @@ -789,19 +785,13 @@ def log_trajectory_execution(step_idx: int, total_steps: int) -> None: start_time = time.time() placement_compiled = engine.compile( ( - ActionInvocation( - skill_id="coordinated_placement", - goal=coordinated_target, - binding=ActionBinding( - manipulators={ - "placing": "left_arm", - "support": "right_arm", - }, - end_effectors={ - "placing": "left_hand", - "support": "right_hand", - }, - ), + engine.make_invocation( + "coordinated_placement", + coordinated_target, + control_parts={ + "placing": {"motion": "left_arm", "grasp": "left_hand"}, + "support": {"motion": "right_arm", "grasp": "right_hand"}, + }, motion_policy=MotionPolicy( strategy="motion_gen", sample_count=COORDINATED_SAMPLE_INTERVAL, diff --git a/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py b/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py index 293a3ed5a..dd8beedcb 100644 --- a/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py +++ b/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py @@ -32,13 +32,13 @@ 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, @@ -46,6 +46,7 @@ RunnerStep, SimulationExecutionAdapter, TaskState, + TimedCommandSequence, ) from embodichain.lab.sim.cfg import RigidBodyAttributesCfg from embodichain.lab.sim.objects import RigidObject, RigidObjectCfg, Robot @@ -293,21 +294,35 @@ def _minimum_cuboid_clearance( return (outside_distance + inside_distance).amin(dim=1) -def _trajectory_eef_positions( +def _command_eef_positions( robot: Robot, - trajectory_positions: torch.Tensor, + commands: TimedCommandSequence, *, control_part: str, ) -> torch.Tensor: - """Convert a full-robot joint trajectory to batched EEF positions.""" - if trajectory_positions.dim() != 3: - raise ValueError("trajectory_positions must have shape (B, N, robot_dof).") - joint_ids = robot.get_joint_ids(name=control_part) - arm_trajectory = trajectory_positions[:, :, joint_ids] + """Convert one endpoint command sequence to batched EEF positions.""" + if not commands.frames: + raise ValueError("commands must contain at least one frame.") positions = [] - for waypoint_index in range(arm_trajectory.shape[1]): + for frame in commands.frames: + matching_commands = tuple( + command + for command in frame.commands + if isinstance(command.target, JointPositionTarget) + and command.target.control_part == control_part + ) + if len(matching_commands) != 1: + raise ValueError( + f"Expected one joint command for control part {control_part!r}, " + f"got {len(matching_commands)}." + ) + payload = matching_commands[0].payload + if not isinstance(payload, JointPositionPayload): + raise TypeError( + f"Control part {control_part!r} did not receive joint positions." + ) pose = robot.compute_fk( - qpos=arm_trajectory[:, waypoint_index], + qpos=payload.positions, name=control_part, to_matrix=True, ) @@ -452,13 +467,11 @@ def main() -> None: dtype=target_pose.dtype, device=target_pose.device, ) - engine = AtomicActionEngine( - motion_generator=motion_gen, - ) - invocation = ActionInvocation( - skill_id="move_end_effector", - goal=EndEffectorPoseGoal(target_pose), - binding=ActionBinding(manipulators={"primary": CONTROL_PART}), + engine = AtomicActionEngine(motion_generator=motion_gen) + invocation = engine.make_invocation( + "move_end_effector", + EndEffectorPoseGoal(target_pose), + control_parts={"primary": {"motion": CONTROL_PART}}, motion_policy=MotionPolicy( strategy="motion_gen", sample_count=SAMPLE_COUNT, @@ -472,9 +485,9 @@ def main() -> None: ) task_state = TaskState.empty(robot.get_qpos().shape[0], robot.device) session = engine.start((invocation,), adapter.observe(task_state)) - initial_eef_path = _trajectory_eef_positions( + initial_eef_path = _command_eef_positions( robot, - session.active_trajectory.positions, + session.active_commands, control_part=CONTROL_PART, ) blocking_obstacle_pose, blocking_waypoint_index = _blocking_obstacle_pose( @@ -583,9 +596,9 @@ def on_step(step: RunnerStep) -> None: and replanned_eef_path is None and ExecutionEventKind.COLLISION_WORLD_CHANGED in observed_events ): - replanned_eef_path = _trajectory_eef_positions( + replanned_eef_path = _command_eef_positions( robot, - session.active_trajectory.positions, + session.active_commands, control_part=CONTROL_PART, ) replan_detour = _maximum_path_deviation( diff --git a/scripts/tutorials/atomic_action/hand_over.py b/scripts/tutorials/atomic_action/hand_over.py index d15ecb451..446596c86 100644 --- a/scripts/tutorials/atomic_action/hand_over.py +++ b/scripts/tutorials/atomic_action/hand_over.py @@ -35,8 +35,6 @@ from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, GraspGoal, AtomicActionEngine, ControlPartCommandProfile, @@ -260,33 +258,27 @@ def run_handover_demo( sim.update(step=10) compiled = engine.compile( ( - ActionInvocation( + engine.make_invocation( "pick_up", GraspGoal(object_semantics), - ActionBinding( - manipulators={"primary": "left_arm"}, - end_effectors={"primary": "left_hand"}, - ), - MotionPolicy( + control_parts={"primary": {"motion": "left_arm", "grasp": "left_hand"}}, + motion_policy=MotionPolicy( strategy="motion_gen", sample_count=PICKUP_SAMPLE_INTERVAL, ), skill_options=pick_up_options, ), - ActionInvocation( + engine.make_invocation( "hand_over", GraspGoal(object_semantics), - ActionBinding( - manipulators={ - "source": "left_arm", - "destination": "right_arm", - }, - end_effectors={ - "source": "left_hand", - "destination": "right_hand", + control_parts={ + "source": {"motion": "left_arm", "grasp": "left_hand"}, + "destination": { + "motion": "right_arm", + "grasp": "right_hand", }, - ), - MotionPolicy( + }, + motion_policy=MotionPolicy( strategy="motion_gen", sample_count=HANDOVER_SAMPLE_INTERVAL, ), diff --git a/scripts/tutorials/atomic_action/move_end_effector.py b/scripts/tutorials/atomic_action/move_end_effector.py index 0b80a6b91..9993916e1 100644 --- a/scripts/tutorials/atomic_action/move_end_effector.py +++ b/scripts/tutorials/atomic_action/move_end_effector.py @@ -29,8 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, MotionPolicy, @@ -94,12 +92,10 @@ def main() -> None: compiled = engine.compile( ( - ActionInvocation( - skill_id="move_end_effector", - goal=EndEffectorPoseGoal( - broadcast_waypoint_pose_batch(poses, num_envs) - ), - binding=ActionBinding(manipulators={"primary": "arm"}), + engine.make_invocation( + "move_end_effector", + EndEffectorPoseGoal(broadcast_waypoint_pose_batch(poses, num_envs)), + control_parts={"primary": {"motion": "arm"}}, motion_policy=MotionPolicy( strategy="motion_gen", sample_count=MOVE_SAMPLE_INTERVAL, diff --git a/scripts/tutorials/atomic_action/move_held_object.py b/scripts/tutorials/atomic_action/move_held_object.py index 38b8d0b59..797e157c4 100644 --- a/scripts/tutorials/atomic_action/move_held_object.py +++ b/scripts/tutorials/atomic_action/move_held_object.py @@ -30,8 +30,6 @@ from embodichain.data import get_data_path from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, EndEffectorPoseGoal, @@ -148,26 +146,24 @@ 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"}} compiled = engine.compile( ( - ActionInvocation( + engine.make_invocation( "move_end_effector", EndEffectorPoseGoal(move_target), - binding, - MotionPolicy( + control_parts=motion_mapping, + motion_policy=MotionPolicy( strategy="motion_gen", sample_count=MOVE_SAMPLE_INTERVAL, ), ), - ActionInvocation( + engine.make_invocation( "pick_up", GraspGoal(semantics), - binding, - MotionPolicy( + control_parts=manipulation_mapping, + motion_policy=MotionPolicy( strategy="motion_gen", sample_count=PICK_SAMPLE_INTERVAL, ), @@ -177,11 +173,11 @@ def main() -> None: hand_interp_steps=HAND_INTERP_STEPS, ), ), - ActionInvocation( + engine.make_invocation( "move_held_object", HeldObjectPoseGoal(object_target), - binding, - MotionPolicy( + control_parts=manipulation_mapping, + motion_policy=MotionPolicy( strategy="motion_gen", 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 9814819ab..0a35a5b9f 100644 --- a/scripts/tutorials/atomic_action/move_joints.py +++ b/scripts/tutorials/atomic_action/move_joints.py @@ -29,8 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, JointPositionGoal, @@ -100,18 +98,24 @@ def offset_from_home(offsets: tuple[float, ...]) -> torch.Tensor: waypoints = ( torch.stack([mid, home]).unsqueeze(0).repeat(robot.get_qpos().shape[0], 1, 1) ) - binding = ActionBinding(manipulators={"primary": "arm"}) + control_parts = {"primary": {"motion": "arm"}} policy = MotionPolicy( strategy="motion_gen", sample_count=MOVE_JOINTS_SAMPLE_INTERVAL, ) compiled = engine.compile( ( - ActionInvocation( - "move_joints", JointPositionGoal("ready"), binding, policy + engine.make_invocation( + "move_joints", + JointPositionGoal("ready"), + control_parts=control_parts, + motion_policy=policy, ), - ActionInvocation( - "move_joints", JointPositionGoal(waypoints), binding, policy + engine.make_invocation( + "move_joints", + JointPositionGoal(waypoints), + control_parts=control_parts, + motion_policy=policy, ), ), engine.initial_context(control_dt=sim.sim_config.physics_dt), diff --git a/scripts/tutorials/atomic_action/moving_target_recovery.py b/scripts/tutorials/atomic_action/moving_target_recovery.py index 753e2eb76..a31ee0c66 100644 --- a/scripts/tutorials/atomic_action/moving_target_recovery.py +++ b/scripts/tutorials/atomic_action/moving_target_recovery.py @@ -31,8 +31,6 @@ from embodichain.lab.sim import SimulationManager, VisualMaterialCfg from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, Affordance, AtomicActionEngine, ControlPartCommandProfile, @@ -278,10 +276,6 @@ def main() -> None: entity=target, entity_id=TARGET_ENTITY_ID, ) - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ) engine = AtomicActionEngine( motion_generator=motion_gen, control_profiles={ @@ -291,16 +285,16 @@ def main() -> None: ) }, ) - pick_invocation = ActionInvocation( - skill_id="pick_up", - goal=GraspGoal( + pick_invocation = engine.make_invocation( + "pick_up", + GraspGoal( semantics, grasp_xpos=SceneEntityPose( TARGET_ENTITY_ID, relative_pose=target_to_grasp, ), ), - binding=binding, + control_parts={"primary": {"motion": "arm", "grasp": "hand"}}, motion_policy=MotionPolicy( strategy="motion_gen", sample_count=PICK_SAMPLE_COUNT, diff --git a/scripts/tutorials/atomic_action/pickup.py b/scripts/tutorials/atomic_action/pickup.py index a490cb76b..b5450f6bc 100644 --- a/scripts/tutorials/atomic_action/pickup.py +++ b/scripts/tutorials/atomic_action/pickup.py @@ -29,8 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, GraspGoal, @@ -155,13 +153,10 @@ def main() -> None: compiled = engine.compile( ( - ActionInvocation( - skill_id="pick_up", - goal=GraspGoal(semantics), - binding=ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ), + engine.make_invocation( + "pick_up", + GraspGoal(semantics), + control_parts={"primary": {"motion": "arm", "grasp": "hand"}}, motion_policy=MotionPolicy( strategy="motion_gen", sample_count=PICK_SAMPLE_INTERVAL, diff --git a/scripts/tutorials/atomic_action/place.py b/scripts/tutorials/atomic_action/place.py index 5bd8c5662..17a1bad8d 100644 --- a/scripts/tutorials/atomic_action/place.py +++ b/scripts/tutorials/atomic_action/place.py @@ -29,8 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, GraspGoal, @@ -156,17 +154,14 @@ def main() -> None: sim, args, "Inspect the cube, then press Enter to plan PickUp -> Place..." ) - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ) + endpoint_mapping = {"primary": {"motion": "arm", "grasp": "hand"}} compiled = engine.compile( ( - ActionInvocation( + engine.make_invocation( "pick_up", GraspGoal(semantics), - binding, - MotionPolicy( + control_parts=endpoint_mapping, + motion_policy=MotionPolicy( strategy="motion_gen", sample_count=PICK_SAMPLE_INTERVAL, ), @@ -176,15 +171,15 @@ def main() -> None: hand_interp_steps=HAND_INTERP_STEPS, ), ), - ActionInvocation( + engine.make_invocation( "place", PlaceGoal( broadcast_waypoint_pose_batch( place_poses, robot.get_qpos().shape[0] ) ), - binding, - MotionPolicy( + control_parts=endpoint_mapping, + motion_policy=MotionPolicy( strategy="motion_gen", sample_count=PLACE_SAMPLE_INTERVAL, ), diff --git a/scripts/tutorials/atomic_action/press.py b/scripts/tutorials/atomic_action/press.py index 4e5682ed8..384b836ad 100644 --- a/scripts/tutorials/atomic_action/press.py +++ b/scripts/tutorials/atomic_action/press.py @@ -30,8 +30,6 @@ from embodichain.data import get_data_path from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, EntityState, @@ -210,16 +208,13 @@ def main() -> None: compiled = engine.compile( ( - ActionInvocation( - skill_id="press", - goal=PressGoal( + engine.make_invocation( + "press", + PressGoal( semantics, SceneEntityPose(BUTTON_SCENE_ENTITY_ID), ), - binding=ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ), + control_parts={"primary": {"motion": "arm", "grasp": "hand"}}, motion_policy=MotionPolicy(sample_count=PRESS_SAMPLE_INTERVAL), skill_options=PressOptions( hand_interp_steps=HAND_INTERP_STEPS, diff --git a/scripts/tutorials/atomic_action/slide.py b/scripts/tutorials/atomic_action/slide.py index 5fe8f224d..aa5a5a8d1 100644 --- a/scripts/tutorials/atomic_action/slide.py +++ b/scripts/tutorials/atomic_action/slide.py @@ -32,7 +32,6 @@ from embodichain.data import get_data_path from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -168,6 +167,7 @@ def create_drawer_semantics( def create_invocation( + engine: AtomicActionEngine, semantics: ObjectSemantics, *, direction: Literal["pull", "push"], @@ -177,6 +177,7 @@ def create_invocation( """Create one pull or push invocation for the shared drawer target. Args: + engine: Engine used to resolve the slide control-part binding. semantics: Drawer-handle semantics shared by both operations. direction: Whether this invocation pulls open or pushes closed. approach_distance: Pre-grasp offset opposite the approach axis. @@ -185,16 +186,13 @@ def create_invocation( Returns: A grounded pull/push invocation for the tutorial UR5. """ - return ActionInvocation( - skill_id="slide", - goal=SlideGoal( + return engine.make_invocation( + "slide", + SlideGoal( semantics, SceneEntityPose(HANDLE_SCENE_ENTITY_ID), ), - binding=ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ), + control_parts={"primary": {"motion": "arm", "grasp": "hand"}}, motion_policy=MotionPolicy(sample_count=TRAJECTORY_SAMPLE_COUNT), skill_options=SlideOptions( direction=direction, @@ -262,6 +260,7 @@ def main() -> None: compiled = engine.compile( ( create_invocation( + engine, semantics, direction=direction, approach_distance=args.approach_distance, diff --git a/scripts/tutorials/atomic_action/twist.py b/scripts/tutorials/atomic_action/twist.py index aed8b7cb4..44f6ebec6 100644 --- a/scripts/tutorials/atomic_action/twist.py +++ b/scripts/tutorials/atomic_action/twist.py @@ -30,8 +30,6 @@ from embodichain.data import get_data_path from embodichain.lab.sim.atomic_actions import ( - ActionBinding, - ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, EntityState, @@ -191,16 +189,13 @@ def main() -> None: compiled = engine.compile( ( - ActionInvocation( - skill_id="twist", - goal=TwistGoal( + engine.make_invocation( + "twist", + TwistGoal( semantics, SceneEntityPose(KNOB_SCENE_ENTITY_ID), ), - binding=ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ), + control_parts={"primary": {"motion": "arm", "grasp": "hand"}}, motion_policy=MotionPolicy(sample_count=TWIST_SAMPLE_INTERVAL), skill_options=TwistOptions( hand_interp_steps=HAND_INTERP_STEPS, diff --git a/tests/benchmark/motion_generation/test_motion_generation_benchmark.py b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py index 5d5dace7a..d29a50af1 100644 --- a/tests/benchmark/motion_generation/test_motion_generation_benchmark.py +++ b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py @@ -185,7 +185,6 @@ def _timed_plan_result( success=success, positions=positions, dt=dt, - duration=dt.sum(dim=1), ) diff --git a/tests/sim/atomic_actions/test_actions.py b/tests/sim/atomic_actions/test_actions.py index 4385b1314..89118dda4 100644 --- a/tests/sim/atomic_actions/test_actions.py +++ b/tests/sim/atomic_actions/test_actions.py @@ -28,6 +28,7 @@ from embodichain.lab.sim.atomic_actions import ( ActionBinding, ActionInvocation, + ActionPlan, Affordance, AntipodalAffordance, AssembleAffordance, @@ -50,6 +51,8 @@ HeldObjectPoseGoal, HeldObjectState, JointPositionGoal, + JointPositionPayload, + JointPositionTarget, MotionPolicy, MoveEndEffector, MoveEndEffectorOptions, @@ -76,6 +79,7 @@ SceneEntityPose, SceneSnapshot, TaskState, + TimedTrajectory, TwistAffordance, Twist, TwistGoal, @@ -100,6 +104,7 @@ DUAL_ROBOT_DOF = DUAL_ARM_DOF + 2 * HAND_DOF ActionT = TypeVar("ActionT", bound=AtomicAction) +_ACTION_ENGINES: dict[int, AtomicActionEngine] = {} @pytest.fixture(autouse=True) @@ -187,6 +192,7 @@ def _motion_generator() -> MotionGenerator: generator.device = torch.device("cpu") generator.planner = Mock() generator.planner.cfg.planner_type = "stub" + generator.planner.collision_world_info = None generator.planner.preserve_plan_samples = False generator.planner.supports_move_type.return_value = False generator.planner.default_plan_options.return_value = PlanOptions() @@ -217,6 +223,7 @@ def _bind_action( load_builtins=False, ) engine.register(action) + _ACTION_ENGINES[id(action)] = engine return action @@ -263,27 +270,79 @@ 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 _joint_trajectory(plan: ActionPlan) -> TimedTrajectory: + """Return the owned planner trajectory for a joint-feedback plan.""" + assert plan.joint_trajectory is not None + return plan.joint_trajectory + + +def _joint_command_positions( + plan: ActionPlan, + control_part: str, +) -> torch.Tensor: + """Stack runtime joint commands sent to one concrete control part.""" + return torch.stack( + [payload.positions for payload in _joint_command_payloads(plan, control_part)], + dim=1, + ) + + +def _joint_command_payloads( + plan: ActionPlan, + control_part: str, +) -> tuple[JointPositionPayload, ...]: + """Return runtime joint payloads sent to one concrete control part.""" + payloads: list[JointPositionPayload] = [] + for frame in plan.commands.frames: + matching = [ + command + for command in frame.commands + if isinstance(command.target, JointPositionTarget) + and command.target.control_part == control_part + ] + assert len(matching) == 1 + payload = matching[0].payload + assert isinstance(payload, JointPositionPayload) + payloads.append(payload) + return tuple(payloads) + + def _semantics(*, entity_id: str | None = None) -> ObjectSemantics: entity = Mock(spec=BatchEntity) entity.get_local_pose.return_value = torch.eye(4).repeat(NUM_ENVS, 1, 1) @@ -374,6 +433,7 @@ def compute_fk( generator.device = torch.device("cpu") generator.planner = Mock() generator.planner.cfg.planner_type = "stub" + generator.planner.collision_world_info = None generator.planner.preserve_plan_samples = False generator.planner.supports_move_type.return_value = False generator.planner.default_plan_options.return_value = PlanOptions() @@ -399,17 +459,21 @@ def _dual_context( 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", + }, }, ) @@ -506,7 +570,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, ), @@ -514,8 +578,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 @@ -541,12 +606,13 @@ 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: @@ -559,7 +625,7 @@ def test_pick_and_place_declare_effects_without_mutating_context() -> None: 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) @@ -577,7 +643,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( @@ -617,15 +683,16 @@ def move_ik( plan = _plan_action( action, - _invocation("place", PlaceGoal(torch.eye(4))), + _invocation(action, PlaceGoal(torch.eye(4))), context, ) projected = plan.expected_effects.apply(task, plan.plan_success) assert plan.plan_success.tolist() == [False, True] + trajectory = _joint_trajectory(plan) assert torch.allclose( - plan.trajectory.positions[0], - context.robot.qpos[0].unsqueeze(0).expand(plan.trajectory.waypoint_count, -1), + trajectory.positions[0], + context.robot.qpos[0].unsqueeze(0).expand(trajectory.waypoint_count, -1), ) primary = projected.get_held_object("arm") alternate = projected.get_held_object("alternate_arm") @@ -637,7 +704,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, ) @@ -667,7 +734,7 @@ def test_move_held_object_requires_projected_attachment() -> None: configured_invocation = ActionInvocation( skill_id="move_held_object", goal=HeldObjectPoseGoal(torch.eye(4)), - binding=_binding(), + binding=_binding(action), motion_policy=MotionPolicy(sample_count=10), skill_options=MoveHeldObjectOptions(pick_rotate_upright=0.25), ) @@ -684,6 +751,48 @@ def test_move_held_object_requires_projected_attachment() -> None: semantics.entity.get_local_pose.assert_not_called() +def test_move_held_object_moves_only_exclusively_held_rows() -> None: + generator = _motion_generator() + + def move_ik( + pose: torch.Tensor, + name: str, + joint_seed: torch.Tensor, + **_: object, + ) -> tuple[torch.Tensor, torch.Tensor]: + return torch.ones(NUM_ENVS, dtype=torch.bool), joint_seed + 0.1 + + generator.robot.compute_ik.side_effect = move_ik + action = _bind_action(generator, MoveHeldObject()) + semantics = _semantics() + task = TaskState( + batch_size=NUM_ENVS, + device="cpu", + held_objects={ + "arm": _held(semantics), + "alternate_arm": _held( + semantics, + env_mask=torch.tensor([True, False]), + ), + }, + ) + context = _context(task) + + plan = _plan_action( + action, + _invocation(action, HeldObjectPoseGoal(torch.eye(4))), + context, + ) + + assert plan.plan_success.tolist() == [False, True] + trajectory = _joint_trajectory(plan) + assert torch.allclose( + trajectory.positions[0], + context.robot.qpos[0].unsqueeze(0).expand(trajectory.waypoint_count, -1), + ) + assert not torch.allclose(trajectory.positions[1], context.robot.qpos[1]) + + def test_strategy_and_sample_count_are_not_action_config_fields() -> None: with pytest.raises(TypeError): MoveEndEffectorOptions(strategy="motion_gen") # type: ignore[call-arg] @@ -697,7 +806,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) @@ -708,7 +817,7 @@ def test_move_joints_rejects_incompatible_goal_at_action_boundary() -> None: invocation = ActionInvocation( skill_id="move_joints", goal=object(), # type: ignore[arg-type] - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=_binding(action), ) with pytest.raises(TypeError, match="expects goal JointPositionGoal"): @@ -724,7 +833,7 @@ def test_builtin_action_validates_resolved_request_once() -> None: _plan_action( action, _invocation( - "move_end_effector", + action, EndEffectorPoseGoal(torch.eye(4)), ), _context(), @@ -745,22 +854,24 @@ def test_planner_timing_is_preserved_in_simple_action() -> None: velocities=torch.full((NUM_ENVS, 3, ARM_DOF), 0.5), accelerations=torch.zeros(NUM_ENVS, 3, ARM_DOF), dt=torch.tensor([[0.0, 0.1, 0.2]]).repeat(NUM_ENVS, 1), - duration=torch.full((NUM_ENVS,), 0.3), ) action = _bind_action(generator, MoveJoints()) invocation = ActionInvocation( skill_id="move_joints", goal=JointPositionGoal(torch.ones(ARM_DOF)), - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=_binding(action), motion_policy=MotionPolicy(strategy="motion_gen", sample_count=3), ) plan = _plan_action(action, invocation, _context()) - assert plan.trajectory.duration.tolist() == pytest.approx([0.3, 0.3]) - assert plan.trajectory.velocities is not None - assert torch.all(plan.trajectory.velocities[:, :, :ARM_DOF] == 0.5) - assert torch.all(plan.trajectory.velocities[:, :, ARM_DOF:] == 0.0) + trajectory = _joint_trajectory(plan) + payloads = _joint_command_payloads(plan, "arm") + assert trajectory.duration.tolist() == pytest.approx([0.3, 0.3]) + assert all(payload.velocities is not None for payload in payloads) + assert torch.all( + torch.stack([payload.velocities for payload in payloads], dim=1) == 0.5 + ) def test_move_end_effector_visits_batched_waypoints_in_order() -> None: @@ -785,7 +896,7 @@ def compute_ik( plan = _plan_action( action, _invocation( - "move_end_effector", + action, EndEffectorPoseGoal(waypoints), sample_count=9, ), @@ -818,19 +929,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(), ) @@ -860,7 +972,7 @@ def test_pick_explicit_grasp_bypasses_sampling_and_records_grasp() -> None: request = action.resolve_request( _invocation( - "pick_up", + action, GraspGoal(semantics=semantics, grasp_xpos=grasp), sample_count=20, ) @@ -911,17 +1023,19 @@ def test_pick_holds_only_environment_without_a_feasible_grasp() -> None: 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] @@ -948,7 +1062,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( @@ -996,9 +1110,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"), @@ -1033,9 +1149,10 @@ def test_pick_uses_binding_control_part_as_effect_resource() -> None: semantics=_semantics(entity_id="target"), grasp_xpos=torch.eye(4), ), - binding=ActionBinding( - manipulators={"primary": "alternate_arm"}, - end_effectors={"primary": "alternate_hand"}, + binding=_binding( + action, + motion="alternate_arm", + grasp="alternate_hand", ), motion_policy=MotionPolicy(sample_count=20), ) @@ -1055,6 +1172,44 @@ def test_pick_uses_binding_control_part_as_effect_resource() -> None: assert projected.get_held_object("arm") is None +def test_press_closes_hand_without_changing_projected_attachment() -> None: + semantics = ObjectSemantics( + affordance=PressAffordance( + press_axis=torch.tensor([1.0, 0.0, 0.0]), + press_position=(0.0, 0.0, 0.0), + ), + geometry={}, + label="button", + ) + held = _held(semantics) + task = TaskState( + batch_size=NUM_ENVS, + device="cpu", + held_objects={"arm": held}, + ) + action = _bind_action( + _motion_generator(), + Press(default_options=PressOptions(hand_interp_steps=4)), + ) + + plan = _plan_action( + action, + _invocation( + action, + PressGoal(semantics, torch.eye(4)), + sample_count=12, + ), + _context(task), + ) + projected = plan.expected_effects.apply(task, plan.plan_success) + + assert torch.all(_joint_command_positions(plan, "hand")[:, -1] == 1.0) + projected_held = projected.get_held_object("arm") + assert projected_held is not None + assert projected_held.semantics is held.semantics + assert torch.equal(projected_held.object_to_eef, held.object_to_eef) + + def test_twist_plans_six_segments_from_articulation_link() -> None: affordance = TwistAffordance( grasp_position=(0.0, 0.0, 0.0), @@ -1074,7 +1229,7 @@ def test_twist_plans_six_segments_from_articulation_link() -> None: ActionInvocation( skill_id="twist", goal=TwistGoal(semantics, torch.eye(4)), - binding=_binding(), + binding=_binding(action), motion_policy=MotionPolicy(sample_count=24), skill_options=TwistOptions(hand_interp_steps=3), ), @@ -1082,7 +1237,8 @@ def test_twist_plans_six_segments_from_articulation_link() -> None: ) assert plan.plan_success.tolist() == [True, True] - assert plan.trajectory.positions.shape == (NUM_ENVS, 24, ROBOT_DOF) + trajectory = _joint_trajectory(plan) + assert trajectory.positions.shape == (NUM_ENVS, 24, ROBOT_DOF) assert [segment.name for segment in plan.segments] == [ "approach", "reach", @@ -1092,10 +1248,10 @@ def test_twist_plans_six_segments_from_articulation_link() -> None: "retract", ] assert torch.all( - plan.trajectory.positions[:, plan.segment("close").stop - 1, ARM_DOF:] == 1.0 + trajectory.positions[:, plan.segment("close").stop - 1, ARM_DOF:] == 1.0 ) assert torch.all( - plan.trajectory.positions[:, plan.segment("open").stop - 1, ARM_DOF:] == 0.0 + trajectory.positions[:, plan.segment("open").stop - 1, ARM_DOF:] == 0.0 ) first_target = generator.robot.compute_ik.call_args_list[0].kwargs["pose"] grasp_pose = affordance.get_grasp_pose(torch.eye(4).repeat(NUM_ENVS, 1, 1)) @@ -1116,12 +1272,13 @@ def test_twist_plans_from_explicit_rigid_object_pose_snapshot() -> None: label="rigid-knob", ) + action = _bind_action(_motion_generator(), Twist()) plan = _plan_action( - _bind_action(_motion_generator(), Twist()), + action, ActionInvocation( skill_id="twist", goal=TwistGoal(semantics, torch.eye(4)), - binding=_binding(), + binding=_binding(action), motion_policy=MotionPolicy(sample_count=24), skill_options=TwistOptions(hand_interp_steps=3), ), @@ -1207,7 +1364,8 @@ def test_twist_session_replans_when_scene_target_moves() -> None: }, load_builtins=False, ) - engine.register(Twist()) + action = Twist() + engine.register(action) semantics = ObjectSemantics( affordance=TwistAffordance( grasp_position=(0.0, 0.0, 0.0), @@ -1219,7 +1377,10 @@ def test_twist_session_replans_when_scene_target_moves() -> None: invocation = ActionInvocation( skill_id="twist", goal=TwistGoal(semantics, SceneEntityPose("target")), - binding=_binding(), + binding=engine.bind_control_parts( + "twist", + {"primary": {"motion": "arm", "grasp": "hand"}}, + ), motion_policy=MotionPolicy(sample_count=24), skill_options=TwistOptions(hand_interp_steps=3), ) @@ -1312,21 +1473,22 @@ def sample_grasp( ActionInvocation( skill_id="slide", goal=SlideGoal(semantics, link_pose), - binding=_binding(), + binding=_binding(action), motion_policy=MotionPolicy(sample_count=24), skill_options=options, ), _context(), ) + trajectory = _joint_trajectory(plan) assert plan.plan_success.tolist() == [True, True] - assert plan.trajectory.positions.shape == (NUM_ENVS, 24, ROBOT_DOF) + assert trajectory.positions.shape == (NUM_ENVS, 24, ROBOT_DOF) assert [segment.name for segment in plan.segments] == expected_segments assert torch.all( - plan.trajectory.positions[:, plan.segment("close").stop - 1, ARM_DOF:] == 1.0 + trajectory.positions[:, plan.segment("close").stop - 1, ARM_DOF:] == 1.0 ) assert torch.all( - plan.trajectory.positions[:, plan.segment("open").stop - 1, ARM_DOF:] == 0.0 + trajectory.positions[:, plan.segment("open").stop - 1, ARM_DOF:] == 0.0 ) assert len(grasp_calls) == 1 assert torch.equal(grasp_calls[0][0], link_pose) @@ -1412,7 +1574,7 @@ def successful_ik( ActionInvocation( skill_id="slide", goal=SlideGoal(semantics, torch.eye(4)), - binding=_binding(), + binding=_binding(action), motion_policy=MotionPolicy(sample_count=18), skill_options=SlideOptions(hand_interp_steps=3), ), @@ -1420,9 +1582,10 @@ def successful_ik( ) 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(18, -1), ) @@ -1475,7 +1638,7 @@ def position_fk( ActionInvocation( skill_id="slide", goal=SlideGoal(semantics, torch.eye(4)), - binding=_binding(), + binding=_binding(action), motion_policy=MotionPolicy(sample_count=24), skill_options=SlideOptions(direction="pull", hand_interp_steps=3), ), @@ -1483,7 +1646,7 @@ def position_fk( ) pull_segment = plan.segment("pull") - arm_path = plan.trajectory.positions[ + arm_path = _joint_trajectory(plan).positions[ :, pull_segment.start : pull_segment.stop, :ARM_DOF ] fk_path = position_fk(arm_path.reshape(-1, ARM_DOF), "arm", True).reshape( @@ -1518,7 +1681,7 @@ def test_press_plans_close_approach_press_and_retract() -> None: ActionInvocation( skill_id="press", goal=PressGoal(semantics, torch.eye(4)), - binding=_binding(), + binding=_binding(action), motion_policy=MotionPolicy(sample_count=24), skill_options=options, ), @@ -1526,7 +1689,8 @@ def test_press_plans_close_approach_press_and_retract() -> None: ) assert plan.plan_success.tolist() == [True, True] - assert plan.trajectory.positions.shape == (NUM_ENVS, 24, ROBOT_DOF) + trajectory = _joint_trajectory(plan) + assert trajectory.positions.shape == (NUM_ENVS, 24, ROBOT_DOF) assert [segment.name for segment in plan.segments] == [ "close", "approach", @@ -1535,7 +1699,7 @@ def test_press_plans_close_approach_press_and_retract() -> None: "retract", ] assert torch.all( - plan.trajectory.positions[:, plan.segment("close").stop - 1, ARM_DOF:] == 1.0 + trajectory.positions[:, plan.segment("close").stop - 1, ARM_DOF:] == 1.0 ) contact_pose = affordance.get_press_pose(torch.eye(4).repeat(NUM_ENVS, 1, 1)) expected_approach = ( @@ -1569,13 +1733,14 @@ def test_press_plans_from_rigid_object_pose_snapshot_with_option_position() -> N label="rigid-button", ) generator = _motion_generator() + action = _bind_action(generator, Press()) plan = _plan_action( - _bind_action(generator, Press()), + action, ActionInvocation( skill_id="press", goal=PressGoal(semantics, torch.eye(4)), - binding=_binding(), + binding=_binding(action), motion_policy=MotionPolicy(sample_count=24), skill_options=PressOptions( hand_interp_steps=3, @@ -1630,20 +1795,19 @@ def position_fk( ActionInvocation( skill_id="press", goal=PressGoal(semantics, torch.eye(4)), - binding=_binding(), + binding=_binding(action), motion_policy=MotionPolicy(sample_count=24), skill_options=PressOptions(hand_interp_steps=3, press_distance=0.04), ), _context(), ) - contact_arm = plan.trajectory.positions[ - :, plan.segment("contact").stop - 1, :ARM_DOF - ] + trajectory = _joint_trajectory(plan) + contact_arm = trajectory.positions[:, plan.segment("contact").stop - 1, :ARM_DOF] contact_fk = position_fk(contact_arm, "arm", True) assert torch.allclose(contact_fk[:, :3, 3], torch.zeros(NUM_ENVS, 3)) press_segment = plan.segment("press") - press_arm = plan.trajectory.positions[ + press_arm = trajectory.positions[ :, press_segment.start : press_segment.stop, :ARM_DOF ] press_fk = position_fk(press_arm.reshape(-1, ARM_DOF), "arm", True).reshape( @@ -1685,7 +1849,7 @@ def partial_ik( ActionInvocation( skill_id="press", goal=PressGoal(semantics, torch.eye(4)), - binding=_binding(), + binding=_binding(action), motion_policy=MotionPolicy(sample_count=18), skill_options=PressOptions(hand_interp_steps=3), ), @@ -1693,9 +1857,10 @@ def partial_ik( ) 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(18, -1), ) @@ -1714,7 +1879,7 @@ def test_press_rejects_non_press_affordance() -> None: with pytest.raises(ValueError, match="PressAffordance"): _plan_action( action, - _invocation("press", PressGoal(semantics, torch.eye(4))), + _invocation(action, PressGoal(semantics, torch.eye(4))), _context(), ) @@ -1729,10 +1894,12 @@ def test_press_requires_primary_arm_and_end_effector_bindings() -> None: invocation = ActionInvocation( skill_id="press", goal=PressGoal(semantics, torch.eye(4)), - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=ActionBinding( + owner_id=_ACTION_ENGINES[id(action)].binding_owner_id, + ), ) - with pytest.raises(KeyError, match="No end effector is bound to role 'primary'"): + with pytest.raises(ValueError, match="missing=.*grasp"): action.resolve_request(invocation) @@ -1765,7 +1932,7 @@ def test_twist_rejects_non_twist_affordance() -> None: with pytest.raises(ValueError, match="TwistAffordance"): _plan_action( action, - _invocation("twist", TwistGoal(semantics, torch.eye(4))), + _invocation(action, TwistGoal(semantics, torch.eye(4))), _context(), ) @@ -1795,7 +1962,7 @@ def test_slide_rejects_non_slide_affordance() -> None: _plan_action( action, _invocation( - "slide", + action, SlideGoal(semantics, torch.eye(4)), ), _context(), @@ -1812,10 +1979,12 @@ def test_slide_requires_primary_end_effector() -> None: invocation = ActionInvocation( skill_id="slide", goal=SlideGoal(semantics, torch.eye(4)), - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=ActionBinding( + owner_id=_ACTION_ENGINES[id(action)].binding_owner_id, + ), ) - with pytest.raises(KeyError, match="No end effector is bound to role 'primary'"): + with pytest.raises(ValueError, match="missing=.*grasp"): action.resolve_request(invocation) @@ -1891,7 +2060,7 @@ def plan_from_start( semantics=semantics, grasp_xpos=SceneEntityPose("unused_grasp_pose"), ), - binding=_dual_binding("source", "destination"), + binding=_dual_binding(action, "source", "destination"), motion_policy=MotionPolicy(sample_count=30), ) @@ -1970,7 +2139,7 @@ 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), ) @@ -1979,11 +2148,13 @@ def fail_second_receiving_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) received = projected.get_held_object("right_arm") assert received is not None assert received.env_mask.tolist() == [True, False] @@ -2011,7 +2182,7 @@ def test_handover_rejects_goal_for_a_different_held_object() -> None: invocation = ActionInvocation( skill_id="hand_over", goal=GraspGoal(semantics=goal_semantics), - binding=_dual_binding("source", "destination"), + binding=_dual_binding(action, "source", "destination"), ) with pytest.raises(ValueError, match="must identify the object held"): @@ -2057,7 +2228,7 @@ def test_handover_transfers_only_exclusively_held_rows() -> None: 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), ) @@ -2065,8 +2236,9 @@ def test_handover_transfers_only_exclusively_held_rows() -> None: projected = plan.expected_effects.apply(context.task, plan.plan_success) assert plan.plan_success.tolist() == [False, True] + trajectory = _joint_trajectory(plan) assert torch.allclose( - plan.trajectory.positions[0], + trajectory.positions[0], context.robot.qpos[0].unsqueeze(0).expand(30, -1), ) transferred = projected.get_held_object("left_arm") @@ -2106,7 +2278,7 @@ 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() @@ -2116,13 +2288,24 @@ def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> 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 _joint_trajectory(plan).positions.shape == ( + NUM_ENVS, + 30, + DUAL_ROBOT_DOF, + ) left_held = projected.get_held_object("left_arm") right_held = projected.get_held_object("right_arm") assert isinstance(left_held, HeldObjectState) assert isinstance(right_held, HeldObjectState) assert left_held.semantics is right_held.semantics assert left_held.semantics is not semantics + 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 [segment.name for segment in plan.segments] == [ @@ -2168,7 +2351,7 @@ def test_coordinated_pick_implicit_initial_pose_uses_scene_snapshot() -> None: semantics=semantics, object_target_pose=object_pose, ), - binding=_dual_binding("left", "right"), + 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)) @@ -2221,7 +2404,7 @@ def test_assemble_place_uses_explicit_base_snapshot() -> None: request = action.resolve_request( _invocation( - "place", + action, AssembleGoal( affordance=affordance, base_pose=SceneEntityPose("base"), @@ -2248,7 +2431,7 @@ def test_assemble_place_legacy_base_entity_warns() -> None: ) request = action.resolve_request( - _invocation("place", AssembleGoal(affordance=affordance)) + _invocation(action, AssembleGoal(affordance=affordance)) ) with pytest.warns(DeprecationWarning, match="base_pose"): plan = action.plan(request, _context(task)) @@ -2302,7 +2485,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() @@ -2311,11 +2494,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) left_held = projected.get_held_object("left_arm") right_held = projected.get_held_object("right_arm") assert left_held is not None and right_held is not None @@ -2364,14 +2549,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: @@ -2403,7 +2593,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) @@ -2412,7 +2602,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 @@ -2442,14 +2638,14 @@ def test_coordinated_placement_rejects_one_object_held_by_both_arms() -> 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), ) plan = _plan_action(action, invocation, _dual_context(task)) assert plan.plan_success.tolist() == [False, False] - assert plan.trajectory.waypoint_count == 0 + assert _joint_trajectory(plan).waypoint_count == 0 generator.robot.compute_ik.assert_not_called() @@ -2503,7 +2699,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), ) @@ -2511,11 +2707,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] @@ -2541,7 +2739,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, ) @@ -2555,7 +2753,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_control.py b/tests/sim/atomic_actions/test_control.py index e1d20f55b..0bae379fe 100644 --- a/tests/sim/atomic_actions/test_control.py +++ b/tests/sim/atomic_actions/test_control.py @@ -24,12 +24,15 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionControlOverrides, ActionPlanningServices, ControlCommand, ControlPartCommandProfile, + DisjointSlotEndpoints, JointPositionCommand, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, ) @@ -45,6 +48,18 @@ def equivalent_to(self, other: ControlCommand) -> bool: 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") @@ -67,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) @@ -91,6 +133,11 @@ def test_control_profile_rejects_invalid_command_snapshot_type() -> None: 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( @@ -98,65 +145,76 @@ def test_control_profile_rejects_command_name_outer_whitespace() -> None: ) +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 = _services().resolve_binding( - ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ) - ) + resolved = _binding(_services()) - grasp = resolved.end_effector().joint_positions( + grasp = resolved.endpoint("primary", "grasp").joint_positions( "grasp", num_envs=2, device="cpu", ) assert grasp.tolist() == [[1.0, 1.0], [1.0, 1.0]] - with pytest.raises(KeyError, match="Available commands"): - resolved.end_effector().joint_positions( + with pytest.raises(KeyError, match="available commands"): + resolved.endpoint("primary", "grasp").joint_positions( "pinch", num_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", num_envs=1, device="cpu"), + overridden.endpoint("primary", "grasp").joint_positions( + "grasp", num_envs=1, device="cpu" + ), torch.full((1, 2), 0.4), ) assert torch.equal( - base.end_effector().joint_positions("grasp", num_envs=1, device="cpu"), + base.endpoint("primary", "grasp").joint_positions( + "grasp", num_envs=1, device="cpu" + ), torch.ones(1, 2), ) -def test_override_rejects_role_not_present_in_binding() -> None: +def test_override_rejects_endpoint_not_present_in_binding() -> None: services = _services() - binding = ActionBinding(end_effectors={"primary": "hand"}) + binding = _binding(services) overrides = ActionControlOverrides( - end_effectors={ - "destination": {"open": JointPositionCommand(torch.zeros(2))}, + endpoints={ + "destination": { + "grasp": {"open": JointPositionCommand(torch.zeros(2))}, + }, } ) - with pytest.raises(KeyError, match="unbound end effector roles"): - services.resolve_binding(binding, overrides) + with pytest.raises(KeyError, match="unbound endpoints"): + services.apply_command_overrides(binding, overrides) diff --git a/tests/sim/atomic_actions/test_core.py b/tests/sim/atomic_actions/test_core.py index 65e7206b0..512f0295a 100644 --- a/tests/sim/atomic_actions/test_core.py +++ b/tests/sim/atomic_actions/test_core.py @@ -18,7 +18,7 @@ from __future__ import annotations -from dataclasses import FrozenInstanceError +from dataclasses import dataclass, FrozenInstanceError from unittest.mock import Mock import pytest @@ -31,22 +31,31 @@ ActionPlan, Affordance, AtomicAction, + AtomicActionEngine, DynamicCollisionMode, + EndpointBinding, + EndpointCommand, EndEffectorPoseGoal, EntityState, + ExecutionFeedbackMode, HeldObjectState, + JointPositionPayload, + JointPositionTarget, MotionPolicy, ObjectSemantics, PlannerDiagnostics, PlanningContext, RecoveryPolicy, - ResolvedActionBinding, ResolvedActionRequest, RobotObservation, + RuntimeCommandFrame, + RuntimeEndpointTarget, SceneEntityPose, SceneSnapshot, + SkillBindingContract, StateDelta, TaskState, + TimedCommandSequence, TimedTrajectory, ) from embodichain.lab.sim.atomic_actions.goals import ( @@ -103,13 +112,112 @@ def _context( ) +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 - manipulator_roles = () + binding_contract = SkillBindingContract() @property def device(self) -> torch.device: @@ -139,18 +247,82 @@ def _plan( raise NotImplementedError -def test_action_binding_is_role_based_and_immutable() -> None: +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), + ) @pytest.mark.parametrize("entity_id", ["", " ", 7]) @@ -526,21 +698,21 @@ def test_dependency_collection_does_not_descend_object_semantics() -> None: 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=ResolvedActionBinding(), + binding=ActionBinding(owner_id=engine.binding_owner_id), motion_policy=MotionPolicy(), recovery_policy=RecoveryPolicy(), skill_options=ActionOptions(), ) - action = _DependencyAction() - action._bind(Mock()) - trajectory = TimedTrajectory.from_uniform_step( - context.robot.qpos.unsqueeze(1), - env_ids=context.env_ids, - step_dt=0.02, - ) with pytest.raises(TypeError, match="TimedTrajectory with explicit dt"): action.build_plan( @@ -550,17 +722,431 @@ def test_build_plan_uses_action_scene_dependency_hook() -> None: trajectory=context.robot.qpos.unsqueeze(1), # type: ignore[arg-type] ) - plan = action.build_plan( + plan = action.build_command_plan( request, context, success=True, - trajectory=trajectory, + 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_uniform_step( + trajectory_positions, + env_ids=env_ids, + step_dt=0.1, + ) + plan_success = torch.tensor([True, False]) + + plan = _action_plan( + commands, + plan_success=plan_success, + joint_trajectory=trajectory, + 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_uniform_step( + torch.ones(1, trajectory_frame_count, 2), + env_ids=trajectory_env_ids, + step_dt=0.1, + ) + + with pytest.raises(ValueError, match=message): + _action_plan( + commands, + joint_trajectory=trajectory, + 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_uniform_step( + torch.tensor([[[1.0, 1.0], [2.0, 2.0]]]), + env_ids=env_ids, + step_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_uniform_step( + torch.ones(1, 1, 2), + env_ids=env_ids, + step_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_uniform_step( + torch.zeros(1, 1, 2), + env_ids=env_ids, + step_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_uniform_step( + torch.ones(1, 1, 2), + env_ids=env_ids, + step_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_uniform_step( + torch.ones(1, 1, 2), + velocities=torch.ones(1, 1, 2), + env_ids=env_ids, + step_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( @@ -654,6 +1240,42 @@ def test_timed_trajectory_uses_explicit_uniform_timing_and_holds_rows() -> None: assert torch.all(held.positions[1] == -1.0) +def test_timed_trajectory_constructor_detaches_and_owns_all_tensor_fields() -> None: + positions = torch.tensor([[[1.0, 2.0], [3.0, 4.0]]], requires_grad=True) + velocities = torch.full_like(positions, 0.5, requires_grad=True) + accelerations = torch.full_like(positions, 0.25, requires_grad=True) + dt = torch.tensor([[0.0, 0.1]], requires_grad=True) + env_ids = torch.tensor([4], dtype=torch.long) + inputs = { + "positions": positions, + "velocities": velocities, + "accelerations": accelerations, + "dt": dt, + "env_ids": env_ids, + } + expected = {name: value.detach().clone() for name, value in inputs.items()} + + trajectory = TimedTrajectory(**inputs) + + with torch.no_grad(): + for value in inputs.values(): + value.zero_() + for name, value in expected.items(): + owned = getattr(trajectory, name) + assert torch.equal(owned, value) + assert owned.grad_fn is None + assert not owned.requires_grad + + +def test_timed_trajectory_rejects_duplicate_environment_ids() -> None: + with pytest.raises(ValueError, match="unique"): + TimedTrajectory.from_positions( + torch.zeros(2, 1, 2), + env_ids=torch.tensor([4, 4], dtype=torch.long), + dt=torch.zeros(2, 1), + ) + + def test_planning_context_requires_explicit_interpolation_period() -> None: with pytest.raises(ValueError, match="explicit PlanningContext.control_dt"): _context().require_control_dt() 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..6200b3596 --- /dev/null +++ b/tests/sim/atomic_actions/test_endpoint_runtime_e2e.py @@ -0,0 +1,534 @@ +# ---------------------------------------------------------------------------- +# 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, + ) + + 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 3c80963b4..da8b75408 100644 --- a/tests/sim/atomic_actions/test_engine.py +++ b/tests/sim/atomic_actions/test_engine.py @@ -37,11 +37,26 @@ ControlPartCommandProfile, JointPositionCommand, JointPositionGoal, + JointPositionTarget, + JOINT_POSITION_CAPABILITY, MotionPolicy, + ObjectSemantics, PlanningContext, + PressAffordance, + PressGoal, + PressOptions, ResolvedActionRequest, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, TimedTrajectory, ) +from embodichain.lab.sim.skills import ( + ControlPartEndpoint, + ResourceBinding, + RobotResource, + RobotSkillProfile, +) ACTION_DT = 0.02 @@ -51,7 +66,19 @@ class StubAction(AtomicAction[JointPositionGoal, ActionOptions]): skill_id: ClassVar[str] = "stub" GoalType: ClassVar[type] = JointPositionGoal - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset({JOINT_POSITION_CAPABILITY}), + ), + ), + ), + ), + ) def _plan( self, @@ -121,12 +148,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), ) @@ -170,17 +201,53 @@ def test_engine_can_disable_builtin_loading() -> None: assert _engine(load_builtins=False).actions == {} +def test_auto_registered_builtin_accepts_per_invocation_options() -> None: + generator = _motion_generator(robot_dof=3) + generator.robot.control_parts = {"arm": object(), "hand": object()} + generator.robot.get_joint_ids.side_effect = lambda name: ( + [0, 1] if name == "arm" else [2] + ) + engine = AtomicActionEngine( + generator, + control_profiles={ + "hand": ControlPartCommandProfile.joint_positions(grasp=torch.ones(1)) + }, + ) + options = PressOptions(hand_interp_steps=7) + semantics = ObjectSemantics( + affordance=PressAffordance(press_position=(0.0, 0.0, 0.0)), + geometry={}, + ) + invocation = ActionInvocation( + skill_id="press", + goal=PressGoal(semantics, torch.eye(4)), + binding=engine.bind_control_parts( + "press", + {"primary": {"motion": "arm", "grasp": "hand"}}, + ), + motion_policy=MotionPolicy(sample_count=20), + skill_options=options, + ) + + request = engine.actions["press"].resolve_request(invocation) + + assert request.skill_options.hand_interp_steps == 7 + assert request.skill_options is not options + + def test_engine_compile_projects_terminal_state_between_actions() -> None: engine = _engine() engine.register(StubAction()) 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 @@ -194,12 +261,14 @@ def test_engine_compile_holds_failed_rows_for_remaining_actions() -> None: first = torch.tensor([[1.0, 1.0, 1.0], [float("nan"), 2.0, 2.0]]) second = torch.full((2, 3), 4.0) - compiled = engine.compile((_invocation(first), _invocation(second))) + compiled = engine.compile((_invocation(engine, first), _invocation(engine, second))) assert compiled.plan_success.tolist() == [True, False] assert torch.all(compiled.projected_context.robot.qpos[0] == 4.0) assert torch.all(compiled.projected_context.robot.qpos[1] == 0.0) - assert torch.all(compiled.action_plans[0].trajectory.positions[1] == 0.0) + first_trajectory = compiled.action_plans[0].joint_trajectory + assert first_trajectory is not None + assert torch.all(first_trajectory.positions[1] == 0.0) assert torch.all(compiled.trajectory.positions[1] == 0.0) @@ -216,8 +285,14 @@ def test_engine_compile_empty_sequence_is_successful_noop() -> None: def test_engine_rejects_unknown_skill() -> None: engine = _engine() + invocation = ActionInvocation( + skill_id="stub", + goal=JointPositionGoal(torch.zeros(2, 3)), + binding=ActionBinding(owner_id=engine.binding_owner_id), + motion_policy=MotionPolicy(sample_count=2), + ) with pytest.raises(KeyError, match="stub"): - engine.compile((_invocation(torch.zeros(2, 3)),)) + engine.compile((invocation,)) def test_engine_rejects_duplicate_instance_registration() -> None: @@ -259,24 +334,125 @@ def test_engine_preserves_custom_action_timing() -> None: engine = _engine() engine.register(StubAction()) - plan = engine.plan(_invocation(torch.ones(2, 3))) + plan = engine.plan(_invocation(engine, torch.ones(2, 3))) + assert plan.joint_trajectory is not None assert torch.allclose( - plan.trajectory.dt, + plan.joint_trajectory.dt, torch.tensor([[0.0, ACTION_DT], [0.0, ACTION_DT]]), ) def test_engine_resolves_action_binding_from_robot_control_parts() -> None: engine = _engine(robot_dof=3) + engine.register(StubAction()) + + resolved = engine.bind_control_parts( + "stub", + {"primary": {"motion": "all"}}, + ) + target = resolved.endpoint("primary", "motion").require_target(JointPositionTarget) + + assert target.control_part == "all" + assert target.joint_ids == (0, 1, 2) + + +def test_engine_make_invocation_binds_direct_control_parts() -> None: + engine = _engine(robot_dof=3) + engine.register(StubAction()) + goal = JointPositionGoal(torch.ones(2, 3)) + motion_policy = MotionPolicy(sample_count=2) + + invocation = engine.make_invocation( + "stub", + goal, + control_parts={"primary": {"motion": "all"}}, + motion_policy=motion_policy, + invocation_id="direct-call", + revision=1, + ) + target = invocation.binding.endpoint("primary", "motion").require_target( + JointPositionTarget + ) + + assert invocation.skill_id == "stub" + assert invocation.goal is goal + assert invocation.motion_policy is motion_policy + assert invocation.invocation_id == "direct-call" + assert invocation.revision == 1 + assert target.control_part == "all" + assert engine.plan(invocation).plan_success.tolist() == [True, True] + + +def test_engine_make_invocation_uses_profile_default_binding() -> None: + engine = _engine(robot_dof=3) + engine.register(StubAction()) + engine.bind_skill_profile( + RobotSkillProfile( + profile_id="stub-profile", + resources={ + "whole_robot": RobotResource( + "whole_robot", + endpoints={ + "motion": ControlPartEndpoint( + "all", + capabilities=frozenset({JOINT_POSITION_CAPABILITY}), + ) + }, + ) + }, + defaults={ + "stub": ResourceBinding({"primary": "whole_robot"}), + }, + ) + ) - resolved = engine.planning_services.resolve_binding( - ActionBinding(manipulators={"primary": "all"}) + invocation = engine.make_invocation( + "stub", + JointPositionGoal(torch.ones(2, 3)), + motion_policy=MotionPolicy(sample_count=2), ) + endpoint = invocation.binding.endpoint("primary", "motion") + + assert endpoint.resource_id == "whole_robot" + assert endpoint.require_target(JointPositionTarget).control_part == "all" + assert engine.plan(invocation).plan_success.tolist() == [True, True] - assert resolved.manipulator().name == "all" - assert resolved.manipulator().joint_ids == (0, 1, 2) - assert resolved.manipulator().dof == 3 + +def test_engine_make_invocation_requires_direct_binding_without_profile() -> None: + engine = _engine() + engine.register(StubAction()) + + with pytest.raises(ValueError, match="control_parts is required"): + engine.make_invocation( + "stub", + JointPositionGoal(torch.ones(2, 3)), + ) + + +def test_engine_make_invocation_rejects_resources_without_profile() -> None: + engine = _engine() + engine.register(StubAction()) + + with pytest.raises(ValueError, match="requires a bound RobotSkillProfile"): + engine.make_invocation( + "stub", + JointPositionGoal(torch.ones(2, 3)), + resources={"primary": "whole_robot"}, + ) + + +def test_engine_make_invocation_rejects_conflicting_binding_sources() -> None: + engine = _engine() + engine.register(StubAction()) + + with pytest.raises(ValueError, match="mutually exclusive"): + engine.make_invocation( + "stub", + JointPositionGoal(torch.ones(2, 3)), + control_parts={"primary": {"motion": "all"}}, + resources={"primary": "whole_robot"}, + ) def test_engine_resolves_invocation_control_override_into_request() -> None: @@ -288,10 +464,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, @@ -301,7 +479,7 @@ def test_engine_resolves_invocation_control_override_into_request() -> None: assert request.revision == 2 assert torch.allclose( - request.binding.manipulator().joint_positions( + request.binding.endpoint("primary", "motion").joint_positions( "ready", num_envs=2, device="cpu" ), torch.full((2, 3), 0.4), @@ -311,15 +489,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: @@ -329,6 +504,43 @@ def test_engine_motion_generator_is_read_only() -> None: engine.motion_generator = Mock() # type: ignore[misc] +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, + engine.initial_context(), + ) + + assert plan.plan_success.tolist() == [True, True] + assert action.is_bound + 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) @@ -347,9 +559,11 @@ def test_bound_action_exposes_num_envs_property() -> 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: @@ -367,4 +581,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 33893da7c..7929a98ee 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__() @@ -98,6 +113,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, @@ -133,6 +149,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, @@ -147,6 +164,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, @@ -176,6 +194,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.""" @@ -218,6 +312,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, ...], @@ -282,6 +400,7 @@ def _collision_context( def _invocation( + engine: AtomicActionEngine, *, skill_id: str = "dynamic", max_replans: int = 2, @@ -293,7 +412,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, strategy=strategy, @@ -310,17 +432,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] @@ -329,7 +484,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), ) @@ -367,7 +522,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(), @@ -387,7 +542,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)) @@ -400,6 +555,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 @@ -415,7 +616,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) @@ -454,6 +655,7 @@ def test_collision_world_exhaustion_only_disables_changed_environment() -> None: session = engine.start( ( _invocation( + engine, max_replans=0, strategy="motion_gen", ), @@ -502,6 +704,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, ), @@ -532,6 +735,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( @@ -550,6 +754,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, ), @@ -563,6 +768,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, ), @@ -585,6 +791,7 @@ def test_required_dynamic_collision_binds_supported_scene() -> None: plan = engine.plan( _invocation( + engine, strategy="motion_gen", dynamic_collision_mode=DynamicCollisionMode.REQUIRED, ), @@ -604,7 +811,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), @@ -629,7 +836,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)) @@ -650,17 +857,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)) @@ -680,9 +888,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 @@ -708,13 +958,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"): @@ -734,10 +983,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)) @@ -756,6 +1079,7 @@ def test_action_timeout_retry_budget_is_bounded() -> None: session = engine.start( ( _invocation( + engine, max_action_retries=1, action_timeout=0.05, ), @@ -782,7 +1106,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, @@ -796,7 +1120,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)) @@ -807,7 +1131,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,)) @@ -820,7 +1144,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, @@ -864,10 +1188,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, @@ -894,7 +1247,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 7e6856b43..38bff128c 100644 --- a/tests/sim/atomic_actions/test_motion_strategy_e2e.py +++ b/tests/sim/atomic_actions/test_motion_strategy_e2e.py @@ -25,7 +25,6 @@ from embodichain.lab.sim.robots import CobotMagicCfg from embodichain.lab.sim.planners import MotionGenerator, MotionGenCfg, ToppraPlannerCfg from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, @@ -79,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, @@ -96,7 +97,10 @@ def _run_reach_test(self, strategy: str): engine.initial_context(control_dt=sim.sim_config.physics_dt), ) assert result.plan_success.all().item(), f"{strategy} reported failure" - final_q = result.trajectory.positions[0, -1, arm_ids] + plan = result.action_plans[0] + assert plan.joint_trajectory is not None + assert plan.commands.frame_count == plan.joint_trajectory.waypoint_count + final_q = plan.joint_trajectory.positions[0, -1, arm_ids] fk = robot.compute_fk( qpos=final_q[None], name=self.CONTROL_PART, to_matrix=True )[0] diff --git a/tests/sim/atomic_actions/test_runner.py b/tests/sim/atomic_actions/test_runner.py index 6ea096545..296623a88 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__() @@ -212,10 +251,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, @@ -231,7 +279,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") @@ -246,7 +296,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), recovery_policy=RecoveryPolicy( max_replans=2, @@ -265,6 +315,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() @@ -288,13 +362,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: @@ -323,6 +418,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( @@ -344,6 +444,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 @@ -362,6 +464,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: @@ -382,14 +486,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), recovery_policy=RecoveryPolicy( max_replans=2, @@ -399,11 +506,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 @@ -411,6 +531,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), + 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 @@ -424,6 +579,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_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_base_planner.py b/tests/sim/planners/test_base_planner.py new file mode 100644 index 000000000..0bf1d09c2 --- /dev/null +++ b/tests/sim/planners/test_base_planner.py @@ -0,0 +1,76 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from dataclasses import FrozenInstanceError + +import pytest + +from embodichain.lab.sim.planners.base_planner import CollisionWorldInfo + + +def test_collision_world_info_represents_one_contract() -> None: + info = CollisionWorldInfo( + entity_ids=("cube", "table"), + dynamic_entity_ids=("cube",), + batch_mode="per_env", + supports_updates=True, + ) + + assert info.entity_ids == ("cube", "table") + assert info.dynamic_entity_ids == ("cube",) + + +def test_collision_world_info_is_immutable() -> None: + info = CollisionWorldInfo() + + with pytest.raises(FrozenInstanceError): + info.supports_updates = False # type: ignore[misc] + + +@pytest.mark.parametrize( + ("entity_ids", "error_type", "match"), + [ + (("cube", "cube"), ValueError, "unique"), + ((" cube",), TypeError, "outer whitespace"), + ], +) +def test_collision_world_info_rejects_invalid_entity_ids( + entity_ids: tuple[str, ...], + error_type: type[Exception], + match: str, +) -> None: + with pytest.raises(error_type, match=match): + CollisionWorldInfo(entity_ids=entity_ids) + + +def test_collision_world_info_requires_dynamic_ids_in_complete_world() -> None: + with pytest.raises(ValueError, match="subset"): + CollisionWorldInfo( + entity_ids=("table",), + dynamic_entity_ids=("cube",), + ) + + +def test_collision_world_info_rejects_invalid_batch_mode() -> None: + with pytest.raises(ValueError, match="batch_mode"): + CollisionWorldInfo(batch_mode="batched") # type: ignore[arg-type] + + +def test_collision_world_info_requires_boolean_update_capability() -> None: + with pytest.raises(TypeError, match="supports_updates"): + CollisionWorldInfo(supports_updates=1) # type: ignore[arg-type] diff --git a/tests/sim/planners/test_curobo_planner.py b/tests/sim/planners/test_curobo_planner.py index b1f5ec92e..d4feb6d38 100644 --- a/tests/sim/planners/test_curobo_planner.py +++ b/tests/sim/planners/test_curobo_planner.py @@ -316,9 +316,12 @@ def test_curobo_planner_exposes_collision_world_contract(multi_env, expected_mod ), ) - assert planner.dynamic_collision_entity_ids == ("registry_cube",) - assert planner.collision_world_entity_ids == ("registry_cube",) - assert planner.collision_world_batch_mode == expected_mode + info = planner.collision_world_info + + assert info.dynamic_entity_ids == ("registry_cube",) + assert info.entity_ids == ("registry_cube",) + assert info.batch_mode == expected_mode + assert info.supports_updates is True def test_curobo_collision_world_binding_merges_owned_obstacle_poses(): @@ -928,7 +931,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, @@ -939,13 +941,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), ), ) @@ -964,7 +970,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), ), ) @@ -982,7 +988,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, @@ -993,13 +998,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 10dcddcae..4b84350d9 100644 --- a/tests/sim/planners/test_motion_generator_batched.py +++ b/tests/sim/planners/test_motion_generator_batched.py @@ -16,15 +16,17 @@ from __future__ import annotations -import torch -import pytest +from typing import Literal from unittest.mock import Mock, patch +import pytest +import torch + +from embodichain.lab.sim.planners.base_planner import CollisionWorldInfo, PlanOptions from embodichain.lab.sim.planners.motion_generator import ( MotionGenerator, MotionGenOptions, ) -from embodichain.lab.sim.planners.base_planner import PlanOptions from embodichain.lab.sim.planners.utils import PlanState, PlanResult, MoveType BATCH_SIZE = 2 @@ -33,6 +35,22 @@ STEP_DT = 0.05 +def _collision_world_info( + dynamic_entity_ids: tuple[str, ...] = (), + *, + entity_ids: tuple[str, ...] | None = None, + batch_mode: Literal["shared", "per_env"] | None = "shared", + supports_updates: bool = True, +) -> CollisionWorldInfo: + """Build a valid collision-world contract for planner test doubles.""" + return CollisionWorldInfo( + entity_ids=dynamic_entity_ids if entity_ids is None else entity_ids, + dynamic_entity_ids=dynamic_entity_ids, + batch_mode=batch_mode, + supports_updates=supports_updates, + ) + + def _timed_result( positions: torch.Tensor, *, @@ -47,7 +65,6 @@ def _timed_result( success=success, positions=positions, dt=dt, - duration=dt.sum(dim=1), ) @@ -147,8 +164,7 @@ def test_direct_cartesian_planner_requires_joint_fallback_inputs(): def test_bind_collision_world_copies_caller_options() -> None: planner = Mock() - planner.supports_collision_world_updates = True - planner.dynamic_collision_entity_ids = ("obstacle",) + planner.collision_world_info = _collision_world_info(("obstacle",)) original = PlanOptions() obstacle_pose = torch.eye(4).unsqueeze(0) @@ -190,8 +206,7 @@ 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 + planner.collision_world_info = _collision_world_info(configured_ids) generator = object.__new__(MotionGenerator) generator.planner = planner @@ -203,8 +218,7 @@ def test_bind_collision_world_requires_exact_planner_entity_ids( 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",) + planner.collision_world_info = _collision_world_info(("cube",)) generator = object.__new__(MotionGenerator) generator.planner = planner options = PlanOptions() @@ -221,8 +235,7 @@ def test_bind_collision_world_rejects_extra_ids_in_caller_options() -> None: 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",) + planner.collision_world_info = _collision_world_info(("cube",)) def bind(options, *, obstacle_poses): options.dynamic_obstacle_poses = { @@ -244,8 +257,7 @@ def bind(options, *, obstacle_poses): 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.collision_world_info = _collision_world_info() planner.default_plan_options.return_value = PlanOptions() def bind(options, *, obstacle_poses): @@ -264,8 +276,7 @@ def bind(options, *, obstacle_poses): def test_bind_collision_world_rejects_non_string_option_keys() -> None: planner = Mock() - planner.supports_collision_world_updates = True - planner.dynamic_collision_entity_ids = () + planner.collision_world_info = _collision_world_info() generator = object.__new__(MotionGenerator) generator.planner = planner options = PlanOptions() @@ -279,39 +290,34 @@ def test_bind_collision_world_rejects_non_string_option_keys() -> None: 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" + info = _collision_world_info( + ("cube", "tray"), + entity_ids=("cube", "tray", "table"), + batch_mode="per_env", + ) + planner.collision_world_info = info generator = object.__new__(MotionGenerator) generator.planner = planner + assert generator.collision_world_info is info assert generator.dynamic_collision_entity_ids == ("cube", "tray") assert generator.collision_world_entity_ids == ("cube", "tray", "table") assert generator.collision_world_batch_mode == "per_env" -@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: +def test_motion_generator_rejects_invalid_collision_world_contract() -> None: planner = Mock() - planner.dynamic_collision_entity_ids = entity_ids + planner.collision_world_info = object() generator = object.__new__(MotionGenerator) generator.planner = planner - with pytest.raises(error_type, match=match): - _ = generator.dynamic_collision_entity_ids + with pytest.raises(TypeError, match="CollisionWorldInfo"): + _ = generator.collision_world_info def test_bind_collision_world_rejects_unsupported_planner() -> None: planner = Mock() - planner.supports_collision_world_updates = False + planner.collision_world_info = None generator = object.__new__(MotionGenerator) generator.planner = planner @@ -327,8 +333,7 @@ def test_bind_collision_world_rejects_unsupported_planner() -> None: def test_bind_collision_world_uses_backend_default_options() -> None: planner = Mock() - planner.supports_collision_world_updates = True - planner.dynamic_collision_entity_ids = ("obstacle",) + planner.collision_world_info = _collision_world_info(("obstacle",)) defaults = PlanOptions() planner.default_plan_options.return_value = defaults planner.with_collision_world.return_value = defaults diff --git a/tests/sim/planners/test_plan_state_batched.py b/tests/sim/planners/test_plan_state_batched.py index 5b5c8e6b1..fc195655b 100644 --- a/tests/sim/planners/test_plan_state_batched.py +++ b/tests/sim/planners/test_plan_state_batched.py @@ -59,29 +59,17 @@ def test_batched_shapes(self): velocities=torch.zeros(2, 10, 7), accelerations=torch.zeros(2, 10, 7), dt=dt, - duration=dt.sum(dim=1), ) assert r.positions.shape == (2, 10, 7) assert r.dt.shape == (2, 10) assert r.duration.shape == (2,) + assert torch.equal(r.duration, dt.sum(dim=1)) - def test_positions_require_complete_matching_timing(self): + def test_positions_require_explicit_timing(self): positions = torch.zeros(2, 3, 7) with pytest.raises(ValueError, match="explicit dt"): PlanResult(success=True, positions=positions) - dt = torch.zeros(2, 3) - with pytest.raises(ValueError, match="explicit duration"): - PlanResult(success=True, positions=positions, dt=dt) - - with pytest.raises(ValueError, match="equal"): - PlanResult( - success=True, - positions=positions, - dt=dt, - duration=torch.ones(2), - ) - class TestValidateBatchConsistency: def test_rejects_inconsistent_B(self): diff --git a/tests/sim/planners/test_toppra_batched.py b/tests/sim/planners/test_toppra_batched.py index f9d331770..a769327ce 100644 --- a/tests/sim/planners/test_toppra_batched.py +++ b/tests/sim/planners/test_toppra_batched.py @@ -85,7 +85,7 @@ def test_solve_one_env_same_waypoint_shortcut(self): ) assert out["success"] is True assert out["n"] == 2 - assert out["duration"] == 0.0 + assert out["dt"].sum() == 0.0 def test_solve_one_env_duplicate_plateau(self): # Long plateaus of identical waypoints (e.g. from interpolating a diff --git a/tests/sim/skills/test_profiles.py b/tests/sim/skills/test_profiles.py index d14d036a1..5ca64cdbe 100644 --- a/tests/sim/skills/test_profiles.py +++ b/tests/sim/skills/test_profiles.py @@ -26,7 +26,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBindingRoute, ActionOptions, ActionPlan, AtomicAction, @@ -51,6 +50,10 @@ 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, @@ -228,7 +231,6 @@ class _WholeBodyAction(AtomicAction[JointPositionGoal, ActionOptions]): SkillEndpointRequirement( "motion", capabilities=frozenset({"motion.whole_body"}), - route=ActionBindingRoute("manipulator", "primary"), ), ), ), @@ -254,7 +256,6 @@ class _NavigateAction(AtomicAction[JointPositionGoal, ActionOptions]): SkillEndpointRequirement( "motion", capabilities=frozenset({"motion.base.se2"}), - route=ActionBindingRoute("manipulator", "primary"), ), ), ), @@ -274,6 +275,7 @@ 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) @@ -284,6 +286,41 @@ class _MutableMetadataEndpoint(ResourceEndpoint): 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.""" @@ -314,9 +351,13 @@ def resolve( """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:{endpoint.controller_id}"}), + claim_tokens=frozenset({f"controller:{claim_id}"}), ) @@ -325,8 +366,6 @@ class _VelocityNavigateAction(AtomicAction[JointPositionGoal, ActionOptions]): skill_id: ClassVar[str] = "navigate_velocity" GoalType: ClassVar[type] = JointPositionGoal - manipulator_roles: ClassVar[tuple[str, ...]] = () - end_effector_roles: ClassVar[tuple[str, ...]] = () binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( SkillResourceSlot( @@ -350,36 +389,6 @@ def _plan( raise NotImplementedError -class _RoutedVelocityAction(AtomicAction[JointPositionGoal, ActionOptions]): - """Test skill requiring a current-core route from a custom endpoint.""" - - skill_id: ClassVar[str] = "navigate_velocity_routed" - GoalType: ClassVar[type] = JointPositionGoal - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) - end_effector_roles: ClassVar[tuple[str, ...]] = () - binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( - slots=( - SkillResourceSlot( - "body", - endpoints=( - SkillEndpointRequirement( - "motion", - capabilities=frozenset({"motion.base.velocity"}), - route=ActionBindingRoute("manipulator", "primary"), - ), - ), - ), - ) - ) - - 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 = { @@ -403,23 +412,6 @@ class Derived(BUILTIN_ACTION_TYPES[0]): assert Derived.descriptor().binding_contract is None -def test_descriptor_contract_must_exactly_cover_current_core_roles() -> None: - class InvalidRouteAction(AtomicAction[JointPositionGoal, ActionOptions]): - skill_id: ClassVar[str] = "invalid_route" - GoalType: ClassVar[type] = JointPositionGoal - binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract() - - def _plan( - self, - request: ResolvedActionRequest[JointPositionGoal, ActionOptions], - context: PlanningContext, - ) -> ActionPlan: - raise NotImplementedError - - with pytest.raises(ValueError, match="do not exactly cover"): - InvalidRouteAction.descriptor() - - def test_profile_owns_input_mappings_and_command_tensors() -> None: resources = _resources() open_positions = torch.tensor([0.0]) @@ -464,6 +456,55 @@ def test_profile_owns_custom_endpoint_nested_payloads() -> None: 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( @@ -568,12 +609,112 @@ def test_custom_endpoint_adapter_resolves_commands_and_physical_claim() -> None: ) 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.manipulators == {} - assert resolved.action_binding.end_effectors == {} + 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: @@ -626,31 +767,67 @@ def test_custom_endpoint_claim_tokens_protect_distinct_leaf_aliases() -> None: ) -def test_missing_adapter_binding_target_filters_skill_with_diagnostic() -> None: +def test_distinct_physical_leaves_cannot_share_one_runtime_target() -> None: profile = RobotSkillProfile( - "mobile", + "duplicate_runtime_target", resources={ - "mobile_base": RobotResource( - "mobile_base", + "base_a": RobotResource( + "base_a", endpoints={ - "motion": _BaseVelocityEndpoint( - "base_velocity", - capabilities=frozenset({"motion.base.velocity"}), - ) + "motion": _BaseVelocityEndpoint("shared", claim_id="base_a") }, - ) + ), + "base_b": RobotResource( + "base_b", + endpoints={ + "motion": _BaseVelocityEndpoint("shared", claim_id="base_b") + }, + ), }, ) - engine = _engine(control_profiles={}, load_builtins=False) - engine.register(_RoutedVelocityAction()) - bound = profile.bind( - engine, - endpoint_adapters={_BaseVelocityEndpoint: _BaseVelocityEndpointAdapter()}, + + 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")}, + ) + }, ) - assert "navigate_velocity_routed" not in bound.skills - with pytest.raises(UnsupportedSkillError, match="cannot lower.*manipulator"): - bound.resolve("navigate_velocity_routed") + 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: @@ -665,7 +842,9 @@ def resolve( engine: AtomicActionEngine, ) -> EndpointResolution: del endpoint, engine - return EndpointResolution() + return EndpointResolution( + runtime_target=_BaseVelocityTarget("base_velocity") + ) profile = RobotSkillProfile( "mobile", @@ -699,7 +878,10 @@ def resolve( engine: AtomicActionEngine, ) -> EndpointResolution: del endpoint, engine - return EndpointResolution(exclusive=False) + return EndpointResolution( + runtime_target=_BaseVelocityTarget("virtual"), + exclusive=False, + ) profile = RobotSkillProfile( "virtual", @@ -728,7 +910,10 @@ def resolve( engine: AtomicActionEngine, ) -> EndpointResolution: del endpoint, engine - return EndpointResolution(exclusive=False) + return EndpointResolution( + runtime_target=_BaseVelocityTarget("base_velocity"), + exclusive=False, + ) profile = RobotSkillProfile( "mobile", @@ -912,13 +1097,21 @@ def test_bind_rejects_unverified_standard_solver_capability() -> None: def test_unique_capability_binding_lowers_to_exact_action_binding() -> None: profile = _profile(resources=_resources(include_right=False)) - bound = profile.bind(_engine(control_profiles=_command_profiles())) + 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.manipulators == {"primary": "left_arm"} - assert resolved.action_binding.end_effectors == {"primary": "left_hand"} + 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) @@ -1011,7 +1204,6 @@ def test_coupled_endpoint_views_are_allowed_without_disjoint_constraint() -> Non class CoupledWholeBodyAction(AtomicAction[JointPositionGoal, ActionOptions]): skill_id: ClassVar[str] = "coupled_whole_body" GoalType: ClassVar[type] = JointPositionGoal - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( SkillResourceSlot( @@ -1020,7 +1212,6 @@ class CoupledWholeBodyAction(AtomicAction[JointPositionGoal, ActionOptions]): SkillEndpointRequirement( "motion", capabilities=frozenset({JOINT_POSITION_CAPABILITY}), - route=ActionBindingRoute("manipulator", "primary"), ), SkillEndpointRequirement( "posture", @@ -1091,12 +1282,22 @@ def test_generic_profile_supports_base_and_whole_body_without_arm_tool_fields() assert set(bound.skills) == {"navigate", "whole_body_reach"} assert whole_body.resource_ids == {"body": "whole_body"} - assert whole_body.action_binding.manipulators == {"primary": "full_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.manipulators == {"primary": "base"} + assert ( + navigation.action_binding.endpoint("body", "motion") + .require_target(JointPositionTarget) + .control_part + == "base" + ) def test_presets_are_versioned_snapshots_and_validate_planner() -> None: @@ -1194,7 +1395,6 @@ class Replacement(action_type): SkillEndpointRequirement( "motion", capabilities=frozenset({JOINT_POSITION_CAPABILITY}), - route=ActionBindingRoute("manipulator", "primary"), ), ), ), diff --git a/tests/sim/skills/test_scene.py b/tests/sim/skills/test_scene.py index 89c7b34fc..904c749a8 100644 --- a/tests/sim/skills/test_scene.py +++ b/tests/sim/skills/test_scene.py @@ -19,11 +19,13 @@ from __future__ import annotations from dataclasses import FrozenInstanceError +from typing import Literal import pytest import torch from embodichain.lab.sim.atomic_actions import Affordance, EntityState, SceneSnapshot +from embodichain.lab.sim.planners.base_planner import CollisionWorldInfo from embodichain.lab.sim.skills import ( SceneAffordanceRef, SceneArticulationRef, @@ -90,14 +92,14 @@ def __init__( entity_ids: tuple[str, ...], world_entity_ids: tuple[str, ...] | None = None, supports_updates: bool = True, - batch_mode: str | None = "per_env", + batch_mode: Literal["shared", "per_env"] | 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.collision_world_info = CollisionWorldInfo( + entity_ids=entity_ids if world_entity_ids is None else world_entity_ids, + dynamic_entity_ids=entity_ids, + supports_updates=supports_updates, + batch_mode=batch_mode, ) - self.supports_dynamic_collision_world = supports_updates - self.collision_world_batch_mode = batch_mode class _ExternalSceneProvider: