From 389e702abdf8042a4da9bf384f82b42af5bf2366 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 01:46:16 +0800 Subject: [PATCH 1/5] refactor(atomic-actions): generalize runtime endpoints Make endpoint bindings, timed command frames, transports, routing, safe holds, and profile lowering controller-agnostic. Preserve joint trajectories as optional feedback artifacts and add staged, same-address invocation revision semantics for mobile and whole-body safety. --- .agents/skills/add-atomic-action/SKILL.md | 149 +++- agent_context/MAP.yaml | 20 +- .../topics/atomic-actions/atomic-actions.md | 332 +++++---- .../design/declarative_expert_program_plan.md | 138 +++- .../embodichain.lab.sim.atomic_actions.rst | 52 +- .../sim/atomic_actions/builtin_actions.md | 177 ++--- .../overview/sim/atomic_actions/index.md | 334 +++++---- .../atomic_actions/robot_skill_profiles.md | 42 +- docs/source/tutorial/atomic_actions.rst | 183 +++-- .../lab/sim/atomic_actions/__init__.py | 33 +- .../lab/sim/atomic_actions/bindings.py | 549 ++++++++++----- embodichain/lab/sim/atomic_actions/control.py | 95 +-- embodichain/lab/sim/atomic_actions/core.py | 385 ++++++++-- embodichain/lab/sim/atomic_actions/engine.py | 117 +++- .../lab/sim/atomic_actions/execution.py | 574 ++++++++++----- .../lab/sim/atomic_actions/invocation.py | 20 +- embodichain/lab/sim/atomic_actions/plans.py | 241 ++++++- .../primitives/coordinated_pickment.py | 59 +- .../primitives/coordinated_placement.py | 57 +- .../atomic_actions/primitives/hand_over.py | 60 +- .../primitives/move_end_effector.py | 12 +- .../primitives/move_held_object.py | 20 +- .../atomic_actions/primitives/move_joints.py | 15 +- .../sim/atomic_actions/primitives/pick_up.py | 42 +- .../sim/atomic_actions/primitives/place.py | 23 +- .../sim/atomic_actions/primitives/press.py | 21 +- .../lab/sim/atomic_actions/requirements.py | 67 +- embodichain/lab/sim/atomic_actions/runner.py | 175 +++-- embodichain/lab/sim/atomic_actions/runtime.py | 340 +++++---- .../sim/atomic_actions/runtime_commands.py | 481 +++++++++++++ .../lab/sim/atomic_actions/sim_adapter.py | 164 ++++- .../lab/sim/atomic_actions/transports.py | 489 +++++++++++++ embodichain/lab/sim/skills/profiles.py | 228 +++--- .../multi_segments/cube_pick_place.py | 21 +- .../tableware/blocks_ranking_rgb.py | 21 +- .../tableware/stack_blocks_two.py | 21 +- examples/sim/planners/curobo_planner.py | 6 +- .../move_end_effector_benchmark.py | 7 +- .../move_held_object_benchmark.py | 25 +- .../atomic_action/move_joints_benchmark.py | 10 +- .../atomic_action/pickup_benchmark.py | 10 +- .../atomic_action/place_benchmark.py | 17 +- .../atomic_action/press_benchmark.py | 18 +- scripts/tutorials/atomic_action/assemble.py | 16 +- .../atomic_action/coordinated_pickment.py | 13 +- .../atomic_action/coordinated_placement.py | 49 +- .../dynamic_obstacle_recovery.py | 50 +- scripts/tutorials/atomic_action/hand_over.py | 28 +- .../atomic_action/move_end_effector.py | 6 +- .../atomic_action/move_held_object.py | 23 +- .../tutorials/atomic_action/move_joints.py | 6 +- .../atomic_action/moving_target_recovery.py | 9 +- scripts/tutorials/atomic_action/pickup.py | 7 +- scripts/tutorials/atomic_action/place.py | 16 +- scripts/tutorials/atomic_action/press.py | 15 +- tests/sim/atomic_actions/test_actions.py | 278 +++++--- tests/sim/atomic_actions/test_control.py | 114 ++- tests/sim/atomic_actions/test_core.py | 660 +++++++++++++++++- .../test_curobo_motion_strategy_e2e.py | 12 +- .../test_endpoint_runtime_e2e.py | 535 ++++++++++++++ tests/sim/atomic_actions/test_engine.py | 139 +++- .../sim/atomic_actions/test_engine_per_env.py | 409 ++++++++++- .../test_motion_strategy_e2e.py | 14 +- tests/sim/atomic_actions/test_runner.py | 199 +++++- .../atomic_actions/test_runtime_commands.py | 379 ++++++++++ tests/sim/atomic_actions/test_sim_adapter.py | 189 ++++- .../sim/atomic_actions/test_trajectory_ops.py | 2 +- tests/sim/atomic_actions/test_transports.py | 522 ++++++++++++++ tests/sim/planners/test_curobo_planner.py | 16 +- tests/sim/skills/test_profiles.py | 366 +++++++--- 70 files changed, 7920 insertions(+), 2002 deletions(-) create mode 100644 embodichain/lab/sim/atomic_actions/runtime_commands.py create mode 100644 embodichain/lab/sim/atomic_actions/transports.py create mode 100644 tests/sim/atomic_actions/test_endpoint_runtime_e2e.py create mode 100644 tests/sim/atomic_actions/test_runtime_commands.py create mode 100644 tests/sim/atomic_actions/test_transports.py 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 df7ac14aa..e069029cc 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -512,7 +512,9 @@ topics: - held_objects - HeldObjectState - ActionBinding - - ActionBindingRoute + - EndpointBinding + - RuntimeEndpointTarget + - JointPositionTarget - SkillBindingContract - SkillResourceSlot - SkillEndpointRequirement @@ -552,6 +554,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 @@ -581,6 +597,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 06bcf00c1..059e4d7b0 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -14,40 +14,48 @@ There is no `ActionTarget`, `WorldState`, `ActionResult`, `execute()`, or `ActionInvocation` separates: - an action-owned typed goal, 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` planner/timing choices; - bounded `RecoveryPolicy` thresholds and retry budgets; -- optional typed `skill_options` and role-scoped `control_overrides` for one - invocation revision. +- optional typed `skill_options` and endpoint-scoped `control_overrides` for + one invocation revision. `PlanningContext` separates measured `RobotObservation`, verified symbolic `TaskState`, versioned `SceneSnapshot`, and environment IDs. An `ActionPlan` -contains per-environment planning success, one full-robot `TimedTrajectory`, -action-level recovery and scene-invalidation metadata, planner diagnostics, -named `TrajectorySegment` ranges, and an uncommitted `StateDelta`. Segments are -inspection/tracing metadata inside one trajectory; they are not independently -replannable execution boundaries. - -`AtomicAction.build_plan()` normalizes the success mask and freezes unsuccessful -trajectory rows at the context's observed qpos; skill implementations should -return row-local success instead of duplicating failure-row masking. +contains per-environment planning success, an authoritative +`TimedCommandSequence` in `commands`, an optional full-robot `TimedTrajectory` +in `joint_trajectory`, action-level recovery and scene-invalidation metadata, +planner diagnostics, named `TrajectorySegment` frame ranges, and an uncommitted +`StateDelta`. Segments are inspection/tracing metadata inside one command +sequence; they are not independently replannable execution boundaries. + +`AtomicAction.build_plan()` is the planner-backed joint convenience path: it +normalizes the success mask, freezes unsuccessful trajectory rows at the +context's observed qpos, and lowers the trajectory through bound +`JointPositionTarget` values. `AtomicAction.build_command_plan()` is the generic +extension boundary for transport-neutral command sequences. Both mask failed +rows; skill implementations should return row-local success instead of +duplicating that work. Use `plan.segment(name)` for action-local half-open ranges and `compiled.segment(action_index, name)` for concatenated coordinates; do not recompute private sample splits in callers. Each `AtomicActionEngine` exclusively owns one `ActionPlanningServices` instance, which contains its robot, one `MotionGenerator`/planner backend, and -the legacy core's control-part command profiles. `MotionGenerator.generate()` is the only -stateful motion-planning entry point. `MotionPolicy.to_motion_gen_options()` -passes the invocation's `strategy` directly into `MotionGenOptions`; it is either -`"motion_gen"` or `"ik_interp"`. Target shaping, world-frame pose translation, -hand/joint interpolation used by composite actions, and full-robot trajectory -embedding are pure functions in `trajectory_ops.py`. Actions retain only an -owned copy of typed default options and borrow engine services. Engine -construction creates and binds a fresh instance of every type in -`BUILTIN_ACTION_TYPES`; use `load_builtins=False` only for isolated tests or a -fully custom action set. A bound action cannot be reused by another engine. +its direct control-part command-profile snapshot. It also issues an opaque +binding-owner ID, so an `ActionBinding` cannot cross engine instances. +`MotionGenerator.generate()` is the only stateful motion-planning entry point. +`MotionPolicy.to_motion_gen_options()` passes the invocation's `strategy` +directly into `MotionGenOptions`; it is either `"motion_gen"` or `"ik_interp"`. +Target shaping, world-frame pose translation, hand/joint interpolation used by +composite actions, and full-robot trajectory embedding are pure functions in +`trajectory_ops.py`. Actions retain only an owned copy of typed default options +and borrow engine services. Engine construction creates and binds a fresh +instance of every type in `BUILTIN_ACTION_TYPES`; use `load_builtins=False` only +for isolated tests or a fully custom action set. A bound action cannot be +reused by another engine. ## Engine entry points @@ -89,14 +97,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. @@ -114,23 +122,32 @@ 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. Discovery boundaries are distinct: @@ -155,9 +172,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 @@ -166,14 +186,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 @@ -187,13 +208,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 @@ -328,8 +349,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. @@ -357,13 +382,22 @@ result = runner.step(effect_success=None) ``` `ExecutionSession` owns deterministic planning progress and recovery state. It -emits at most one `JointCommand` per tick. The command's per-environment -`hold_duration` schedules the next feedback cycle from `TimedTrajectory.dt`: -command `i` carries the arrival interval `dt[:, i + 1]` leading to the next -waypoint. The final command reuses its own interval as a settling window. The -session monitors: - -- joint tracking error against the previous command; +emits at most one synchronized `RuntimeCommandFrame` per tick from the plan's +authoritative `TimedCommandSequence`. A frame contains one or more +`EndpointCommand` values, a shared environment batch and active mask, and a +per-environment `hold_duration`. Every command pairs a +`RuntimeEndpointTarget` with a `RuntimeCommandPayload`; their `transport_id` +values must match, destinations must be unique within the frame, and joint +targets may not overlap. `ExecutionFeedbackMode.JOINT_POSITION` requires an +owned `joint_trajectory` and joint-position targets/payloads; generic command +plans default to timed completion and retain external semantic-effect +verification. Framework authorization replaces every emitted target with its +binding-owned snapshot and rejects unbound destinations, target substitution, +and endpoint claim conflicts. A plan's non-empty frames and its recovery +replans retain a stable destination set. Empty failed plans retain previously +active targets so the caller can still hold them. The session monitors: + +- joint tracking error against the previous command in joint-position mode; - translation/rotation drift of referenced scene entities; - per-environment collision-world revision changes for collision-sensitive actions; @@ -387,42 +421,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` @@ -504,21 +563,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 @@ -534,29 +601,31 @@ 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` | -| `coordinated_pickment` | `CoordinatedPickGoal` | `left`, `right` | -| `coordinated_placement` | `CoordinatedPlacementGoal` | `placing`, `support` | -| `hand_over` | `GraspGoal` | `source`, `destination` | +| `move_end_effector` | `EndEffectorPoseGoal` | `primary.motion` | +| `move_joints` | `JointPositionGoal` (`target` is explicit qpos or a profile command name) | `primary.motion` | +| `pick_up` | `GraspGoal` | `primary.motion`, `primary.grasp` | +| `move_held_object` | `HeldObjectPoseGoal` | `primary.motion`, `primary.grasp` | +| `place` | `PlaceGoal`, `AssembleGoal` | `primary.motion`, `primary.grasp` | +| `press` | `PressGoal` | `primary.motion`, `primary.grasp` | +| `coordinated_pickment` | `CoordinatedPickGoal` | `left.motion`, `left.grasp`, `right.motion`, `right.grasp` | +| `coordinated_placement` | `CoordinatedPlacementGoal` | `placing.motion`, `placing.grasp`, `support.motion`, `support.grasp` | +| `hand_over` | `GraspGoal` | `source.motion`, `source.grasp`, `destination.motion`, `destination.grasp` | `GraspGoal.grasp_xpos` accepts an explicit pose tensor, a late-bound `SceneEntityPose`, or `None` for affordance sampling. A `SceneEntityPose` @@ -577,20 +646,29 @@ 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 full-robot positions or a `TimedTrajectory` through `build_plan()`. - Build batched `list[PlanState]`, translate the policy with - `request.motion_policy.to_motion_gen_options()`, and call - `self.motion_generator.generate()`. Import pure operations directly from - `trajectory_ops.py`. +8. For planner-backed joint motion, return full-robot positions or a + `TimedTrajectory` through `build_plan()`: build batched `list[PlanState]`, + translate the policy with `request.motion_policy.to_motion_gen_options()`, + call `self.motion_generator.generate()`, and import pure operations directly + from `trajectory_ops.py`. For mobile, whole-body, or other controller-native + motion, build `EndpointCommand` frames and a `TimedCommandSequence`, then use + `build_command_plan()`. A new transport family must define matching + `RuntimeEndpointTarget` and `RuntimeCommandPayload` types with the same + `transport_id`, plus an `EndpointCommandTransport` registered in the runner's + router. 9. Declare symbolic changes with `StateDelta`; do not mutate context or commit physical effects during planning. For partial attachment updates, retain previous scalar semantics while any previous row remains; merge only batched diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index e8226f62b..b47b7fc46 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 PR1 complete, PR2A and PR2B - implemented on stacked feature branches -- Baseline: `main@e445133c79c8b32019dab1c844b799b43a1658d6` -- Last updated: 2026-08-10 +- Status: implementation in progress; Phase 0 and PR1 complete, and PR2A, + PR2B, and PR2C implemented on stacked feature branches +- Baseline: `main@bcccb787e8f9165e9c8acf6f39f165ba6ac752a4` +- Last updated: 2026-08-11 - Related issues: [#471](https://github.com/DexForce/EmbodiChain/issues/471), [#474](https://github.com/DexForce/EmbodiChain/issues/474) - Related implementation: @@ -235,11 +235,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 @@ -256,7 +258,9 @@ embodichain/lab/sim/skills/ effects.py # built-in EffectMonitor contracts/implementations embodichain/lab/sim/atomic_actions/ - ... # existing typed core and built-in atomic planners + runtime_commands.py # transport-neutral endpoint payloads and timed frames + transports.py # endpoint transport protocol and exact-ID router + ... # typed core and built-in atomic planners embodichain/lab/gym/envs/expert_program/ cfg.py # strict @configclass schema @@ -400,11 +404,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; @@ -433,10 +437,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: @@ -459,11 +463,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 @@ -702,8 +706,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. @@ -718,11 +722,11 @@ normally, then resume with a fresh observation. `BaseEnv.step_dt` is the authoritative control cadence. Semantic task configuration does not expose `control_dt`. -Version 1 should require every emitted `JointCommand.hold_duration` to be -representable by an integer number of environment steps, preferably one step -per yielded command. An incompatible command is rejected with a clear timing -error; it is not silently resampled. Explicit timed-command resampling can be a -later, separately tested feature. +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 @@ -874,6 +878,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 @@ -964,8 +972,9 @@ the 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 (implemented on the feature branch) @@ -1015,7 +1024,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/ @@ -1024,8 +1033,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 @@ -1036,11 +1045,61 @@ PR2B may proceed in parallel with PR2A after the PR1 bridge. Neither follow-up requires official task migration; the repeated-cube vertical slice opts in only after the registry, profile, compiler, runtime, and demo bridge are available. +#### PR2C: generic runtime endpoints (implemented on the feature branch) + +PR2C removes the temporary arm/tool lowering seam and makes the profile's +generic endpoint model executable end to end: + +- `ActionBinding` is an engine-owned collection keyed only by + `(slot_id, endpoint_id)`; `ActionBindingRoute`, arm/tool role maps, and the + intermediate resolved-control-part binding types are removed as an + intentional clean break; +- every resolved profile endpoint owns a typed immutable + `RuntimeEndpointTarget`, while `EndpointCommand` combines that destination + with a transport-specific `RuntimeCommandPayload`; +- `RuntimeCommandFrame` synchronizes per-environment endpoint commands and + timing, and `TimedCommandSequence` becomes the authoritative runtime content + of `ActionPlan`; +- `EndpointCommandTransport` and `EndpointCommandRouter` perform exact-ID + registration, preflight payload validation, transport grouping, + acknowledgement aggregation, cancellation, and transport-owned safe holds; +- the framework authorizes planned commands against binding-owned targets and + physical claims, requires stable destinations across frames and recovery + replans, and retains previously active targets when a failed plan is empty; +- transports actively neutralize inactive environment rows for every addressed + target instead of treating an omitted write as a safe state; +- `SimulationExecutionAdapter` implements the built-in joint-position + transport and writes or holds only the joints claimed by each addressed + endpoint; +- joint-backed planners retain an optional full-robot `TimedTrajectory` for + existing joint feedback and `engine.compile()`, while non-joint plans use + timed completion plus the existing semantic-effect verification boundary; +- full-body joint control and a custom planar-velocity endpoint are exercised + from binding/profile resolution through planning, session execution, routing, + completion, and safe hold without arm/tool-shaped fields. +- an explicit invocation revision declares the same non-empty runtime + destination set and preserves each target's address/safe-hold fingerprint. + The runner keeps the active frame deadline and replans from a fresh due-time + observation; a pending physical effect must be verified first. Changing a + base, arm, whole-body, controller destination, or hold footprint starts a new + invocation rather than hot-switching controller ownership in place. + +PR2C does not add parallel scheduling, claim merging, transport rollback, or a +generic endpoint-feedback evaluator. It also does not add cross-destination +hot revision. Those require separate contracts. + +PR2C exit criteria: an installed custom endpoint kind needs one reusable +endpoint declaration/adapter, payload, transport, and shared atomic skill, but +no core binding or runner changes; whole-body joint endpoints use the same +path; unknown transports and incompatible payloads fail before dispatch; and +cancel/hold behavior remains transport-owned and auditable. + Combined Phase 1 exit criteria: an object is registered once under an authoritative ID, aliases cannot introduce ambiguity, dynamic-object configuration mismatches fail before execution with an entity-centric -diagnostic, 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 @@ -1212,6 +1271,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 3c9703039..9b86b8668 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 @@ -115,9 +122,6 @@ Semantic resource contracts .. autoclass:: SkillEndpointRequirement :members: -.. autoclass:: ActionBindingRoute - :members: - .. autoclass:: DisjointSlotEndpoints :members: @@ -145,10 +149,13 @@ Planning and state .. autoclass:: ActionBinding :members: -.. autoclass:: ResolvedActionBinding +.. autoclass:: EndpointBinding + :members: + +.. autoclass:: RuntimeEndpointTarget :members: -.. autoclass:: ResolvedControlPart +.. autoclass:: JointPositionTarget :members: .. autoclass:: ControlCommand @@ -201,6 +208,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: @@ -229,6 +254,12 @@ Engine and execution .. autoclass:: CommandSink :members: +.. autoclass:: EndpointCommandTransport + :members: + +.. autoclass:: EndpointCommandRouter + :members: + .. autoclass:: ExecutionClock :members: @@ -259,9 +290,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 0205dcc8e..1a5c9144e 100644 --- a/docs/source/overview/sim/atomic_actions/builtin_actions.md +++ b/docs/source/overview/sim/atomic_actions/builtin_actions.md @@ -21,9 +21,10 @@ the built-in catalog. Generic motion and recovery choices belong to the invocation, and per-call primitive behavior belongs to `skill_options`. Registration only installs an implementation. Whether a built-in is executable -for a particular call still depends on its binding roles, the robot's control -parts, semantic command profiles, and task-state preconditions. Action Agent -adapters must also honor `agent_visible` and filter by embodiment capability. +for a particular call still depends on its `SkillBindingContract`, the selected +resource endpoints, semantic command profiles, and task-state preconditions. +Action Agent adapters must also honor `agent_visible` and filter by embodiment +capability. ```{note} The current manipulation primitives consume semantic `open` and `grasp` @@ -135,27 +136,27 @@ The animations below are the focused simulator demos under ## Capability matrix -| Skill ID | Accepted goal | Required binding roles | Required profile commands | Required task state | Expected task effect | +| Skill ID | Accepted goal | Required endpoints | Required profile commands | Required task state | Expected task effect | |---|---|---|---|---|---| -| `move_end_effector` | `EndEffectorPoseGoal` | manipulator `primary` | none | none | none | -| `move_joints` | `JointPositionGoal` | manipulator `primary` | named target only: command matching `target` | none | none | -| `pick_up` | `GraspGoal` | manipulator + end effector `primary` | primary: `open`, `grasp` | semantic object/entity | attach object to `primary` manipulator | -| `move_held_object` | `HeldObjectPoseGoal` | manipulator + end effector `primary` | primary: `grasp` | object held exclusively by `primary` | preserve attachment | -| `place` | `PlaceGoal`, `AssembleGoal` | manipulator + end effector `primary` | primary: `open`, `grasp` | an active attachment must be exclusive to `primary`; `AssembleGoal` requires one | detach object | -| `press` | `PressGoal` | manipulator + end effector `primary` | primary: `grasp` | none | none | -| `coordinated_pickment` | `CoordinatedPickGoal` | manipulator + end effector `left`, `right` | both: `open`, `grasp` | semantic object/entity | attach the shared object to both manipulators | -| `coordinated_placement` | `CoordinatedPlacementGoal` | manipulator + end effector `placing`, `support` | placing: `open`, `grasp`; support: `grasp` | two distinct objects, each held exclusively by its arm | optionally detach placing object; preserve support attachment | -| `hand_over` | `GraspGoal` | manipulator + end effector `source`, `destination` | both: `open`, `grasp` | object held exclusively 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` | none | none | +| `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 | @@ -164,10 +165,12 @@ it does not make the two maps interchangeable. | `placing` | `coordinated_placement` | Participant that aligns and optionally releases the placing object | | `support` | `coordinated_placement` | Participant that keeps holding and positioning the support object | -The action's `manipulator_roles` and `end_effector_roles` declarations determine -which entries are required. The engine checks that those entries exist and that -every value resolves through `Robot.control_parts`; the caller or capability -binder must select a physically compatible arm and hand/tool combination. +Each endpoint requirement declares an open capability set and optional typed +semantic commands. Intra-slot and inter-slot disjointness constraints express +physical compatibility without global arm/tool categories. The built-in +control-part adapter resolves current joint-backed endpoints through +`Robot.control_parts`; custom adapters may instead return mobile, whole-body, or +other runtime targets. `MoveJoints` is intentionally `agent_visible=False`: it is useful for home, recovery, calibration, and scripted postures, but is not exposed to an Action @@ -241,9 +244,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 @@ -252,32 +256,35 @@ Use this rule when configuring a built-in or adding a new one: collision choice, and planner options; - `RecoveryPolicy` carries all replan/retry thresholds and budgets. -All built-ins resolve participating arm and hand names exclusively from -`ActionBinding`. The engine then resolves the selected control part's profile +All built-ins resolve their `motion` and `grasp` endpoints exclusively from the +generic `ActionBinding`. The built-in control-part adapter resolves joint IDs and checks each joint-position command against its DoF. Invocation-level -`ActionControlOverrides` may replace a command by binding role for one explicit -revision. +`ActionControlOverrides` may replace a command by `(slot, endpoint)` for one +explicit revision. ### Planning and effect semantics -Every action returns a per-environment `plan_success` mask and one or more -full-robot trajectories. `plan_success=True` means motion planning succeeded; -it does not prove contact or object transfer. Actions that change attachment -state declare a `StateDelta`. Offline `compile()` projects it hypothetically; -closed-loop execution commits it only after external effect verification. +Every action returns a per-environment `plan_success` mask and an +`ActionPlan.commands` sequence of `RuntimeCommandFrame` values. Current +joint-planned built-ins also retain `ActionPlan.joint_trajectory` for joint +feedback, inspection, and static projection. `plan_success=True` means planning +succeeded; it does not prove contact or object transfer. Actions that change +attachment state declare a `StateDelta`. Offline `compile()` projects it +hypothetically; closed-loop execution commits it only after external effect +verification. (builtin-move-end-effector)= ## `MoveEndEffector` -Plans a free-space motion for a bound manipulator to reach one EEF pose or an -ordered set of pose waypoints. +Plans a free-space motion for the bound `primary.motion` endpoint to reach one +EEF pose or an ordered set of pose waypoints. | Contract | Value | |---|---| | Skill ID | `move_end_effector` | | Goal | `EndEffectorPoseGoal(xpos=...)` | -| Binding | manipulator role `primary` | +| Binding contract | `primary.motion` with Cartesian-pose capability | | Motion | EEF planning from observed arm qpos; output expanded to full robot DoF | | Completion | `EEF_GOAL_REACHED` | | Effect | none | @@ -301,7 +308,7 @@ than an EEF pose. |---|---| | Skill ID | `move_joints` | | Goal | `JointPositionGoal(target=...)` | -| Binding | manipulator role `primary` | +| Binding contract | `primary.motion` with joint-position capability | | Motion | joint planning/interpolation from observed qpos; supports joint waypoints | | Completion | `JOINT_GOAL_REACHED` | | Effect | none | @@ -309,7 +316,7 @@ than an EEF pose. `target` accepts an explicit qpos tensor with shape `(control_dof,)`, `(B, control_dof)`, or `(B, N, control_dof)`, or a non-empty string resolved -from the bound manipulator's `ControlPartCommandProfile`. Named poses remain +from the bound `primary.motion` endpoint's command profile. Named poses remain embodiment knowledge without becoming separate goal types: ```python @@ -331,15 +338,15 @@ named_goal = JointPositionGoal(target="home") ## `PickUp` Plans **approach -> close hand -> lift** and declares the object attached to the -bound manipulator. +bound motion target. | Contract | Value | |---|---| | Skill ID | `pick_up` | | Goal | `GraspGoal(semantics=..., grasp_xpos=None)` | -| Binding | manipulator + end effector role `primary` | -| Precondition | `ObjectSemantics.entity_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 @@ -355,7 +362,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 | @@ -390,17 +397,18 @@ 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`; generic timing and trajectory sampling remain 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. +The bound `primary.grasp` endpoint must provide `grasp`; optional +upright-transport settings belong to `MoveHeldObjectOptions`. The participant's +motion and grasp endpoints are selected through `ActionBinding`; generic timing +and trajectory sampling remain in `MotionPolicy`. 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` @@ -416,9 +424,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` | @@ -428,7 +436,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 | @@ -479,16 +487,16 @@ arm should retreat along its planned path after reaching the target. |---|---| | Skill ID | `press` | | Goal | `PressGoal(xpos=...)` | -| Binding | manipulator + end effector role `primary` | +| Binding contract | `primary.motion` plus disjoint `primary.grasp` | | Motion | close, press, joint-space return | | Effect | none; existing attachment state is unchanged | | Dynamic target | explicit pose or `SceneEntityPose` | -The bound end-effector profile must provide `grasp`, while -`PressOptions.hand_interp_steps` controls the close interpolation. The arm and -hand control parts come from `ActionBinding`. Contact detection is not itself a -symbolic effect in the current action; applications that require force/contact -confirmation should verify it externally. +The bound `primary.grasp` endpoint must provide `grasp`, while +`PressOptions.hand_interp_steps` controls the close interpolation. Both +endpoints come from the generic `ActionBinding`. Contact detection is not +itself a symbolic effect in the current action; applications that require +force/contact confirmation should verify it externally. **Example:** `scripts/tutorials/atomic_action/press.py` @@ -503,7 +511,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 | @@ -528,7 +536,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`; @@ -536,8 +544,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. @@ -554,8 +563,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` | @@ -565,15 +574,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` @@ -589,17 +598,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 20c451cf2..3ecf9ef54 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, control_dt=1.0 / 60.0), 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,15 +183,17 @@ 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, timing, limits, dynamic-collision mode, typed planner options | Skill semantics or robot-resource names | | `RecoveryPolicy` | Action replan/retry budgets, tracking and dynamic-goal thresholds, action-attempt timeout | Controller state or mutable counters | | `ExecutionRunnerCfg` | Runner-level acknowledgement deadlines, minimum feedback cadence, and completion hold policy | Skill behavior, planning resources, or invocation revision data | | `PlanningContext` | Measured `RobotObservation`, verified `TaskState`, versioned `SceneSnapshot`, stable environment IDs | Hypothetical simulator mutation | -| `ActionPlan` | Per-environment result, one scene-bound timed trajectory, named segments, action-level recovery metadata, diagnostics, expected `StateDelta` | Proof that a grasp/release/contact physically succeeded; independently recoverable segment boundaries | +| `ActionPlan` | Per-environment result, `TimedCommandSequence`, optional joint trajectory, named segments, action-level recovery metadata, diagnostics, expected `StateDelta` | Proof that a grasp/release/contact physically succeeded; independently recoverable segment boundaries | +| `RuntimeCommandFrame` | Synchronized endpoint commands, active rows, stable environment IDs, and per-row hold duration | Live transport or controller objects | `MotionPolicy.strategy` accepts exactly `"motion_gen"` or `"ik_interp"`; the same value is forwarded to `MotionGenOptions.strategy` without an adapter layer. @@ -186,69 +204,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 @@ -277,12 +304,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( @@ -290,9 +318,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), + } } } ), @@ -300,9 +330,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. @@ -397,8 +427,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 | @@ -411,23 +443,31 @@ Use `engine.plan()` when one registered action needs to be inspected, tested, or integrated into application-owned orchestration: ```python +binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "left_arm"}}, +) invocation = ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(xpos=target_pose), - binding=ActionBinding(manipulators={"primary": "left_arm"}), + binding=binding, motion_policy=MotionPolicy(sample_count=80, control_dt=1.0 / 60.0), ) plan = engine.plan(invocation, latest_context) if plan.plan_success.all(): - positions = plan.trajectory.positions + command_frames = plan.commands.frames + if plan.joint_trajectory is not None: + positions = plan.joint_trajectory.positions ``` -The result contains that action's trajectory, named segment ranges, -diagnostics, action-level recovery metadata, and uncommitted expected effects. -`plan()` does not automatically create a next context. If another action must -be planned against this action's hypothetical result, use `compile()` instead -of manually reproducing its state projection rules. +The result always contains that action's transport-neutral command sequence. A +joint-planned action may additionally retain `joint_trajectory` for feedback, +inspection, and static qpos projection. The plan also contains named segment +ranges, diagnostics, action-level recovery metadata, and uncommitted expected +effects. `plan()` does not automatically create a next context. If another +action must be planned against this action's hypothetical result, use +`compile()` instead of manually reproducing its state projection rules. `AtomicAction.build_plan()` normalizes scalar or per-environment planner success and replaces unsuccessful rows with the context's observed joint position. @@ -444,14 +484,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, @@ -459,7 +498,10 @@ from embodichain.lab.sim.atomic_actions import ( ) engine = AtomicActionEngine(motion_generator) -binding = ActionBinding(manipulators={"primary": "left_arm"}) +binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "left_arm"}}, +) motion_policy = MotionPolicy(sample_count=80, control_dt=1.0 / 60.0) approach = ActionInvocation( @@ -513,7 +555,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, @@ -529,7 +574,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() ``` @@ -540,11 +585,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() ``` @@ -553,17 +599,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. @@ -638,14 +699,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: @@ -662,13 +721,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 @@ -685,7 +753,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. @@ -707,7 +775,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. @@ -723,7 +791,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 @@ -747,15 +815,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 b5af6a58b..f03d453e2 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 @@ -184,7 +181,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. @@ -248,13 +246,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 @@ -262,19 +262,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/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index e26d7d5e5..4a223ebc7 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; @@ -66,7 +81,7 @@ Application code normally uses one of three engine entry points: - ``ActionPlan`` - Reads one context and does not project a next context * - ``engine.compile()`` - - Planning a fixed sequence whose goals are already known + - Planning a fixed sequence whose goals are known and whose plans retain joint trajectories - ``CompiledTrajectory`` - Propagates hypothetical qpos and expected effects, without observing execution * - ``engine.start()`` @@ -74,10 +89,11 @@ Application code normally uses one of three engine entry points: - ``ExecutionSession`` - ``tick()`` consumes measured context, emits commands, requests effect verification, and can replan -As a short rule: use ``plan`` for one action, ``compile`` for a static action -sequence, and ``start`` followed by ``tick`` for observed execution and error -recovery. None of these APIs steps the simulator directly. The application -sends commands returned by an execution session and supplies new observations. +As a short rule: use ``plan`` for one action, ``compile`` for a static +joint-trajectory sequence, and ``start`` followed by ``tick`` for observed +execution and error recovery. None of these APIs steps the simulator directly. +The application sends commands returned by an execution session and supplies +new observations. ``AtomicAction.plan(request, context)`` is different from ``engine.plan()``. It is the framework-owned template method called by the engine, not an @@ -151,9 +167,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 ------------------- @@ -165,22 +182,27 @@ application-owned orchestration: .. code-block:: python from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, EndEffectorPoseGoal, MotionPolicy, ) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "left_arm"}}, + ) invocation = ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(xpos=target_pose), - binding=ActionBinding(manipulators={"primary": "left_arm"}), + binding=binding, motion_policy=MotionPolicy(sample_count=80, control_dt=1.0 / 60.0), ) plan = engine.plan(invocation, latest_context) if plan.plan_success.all(): - trajectory = plan.trajectory.positions + command_frames = plan.commands.frames + if plan.joint_trajectory is not None: + trajectory = plan.joint_trajectory.positions diagnostics = plan.diagnostics segments = plan.segments @@ -189,22 +211,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, @@ -212,7 +236,10 @@ planning: ) engine = AtomicActionEngine(motion_generator) - binding = ActionBinding(manipulators={"primary": "left_arm"}) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "left_arm"}}, + ) motion_policy = MotionPolicy(sample_count=80, control_dt=1.0 / 60.0) approach = ActionInvocation( @@ -241,6 +268,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 @@ -266,7 +297,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, @@ -275,6 +309,7 @@ must be resolved from the latest scene snapshot: ) from embodichain.lab.sim.atomic_actions import ( + EndpointCommandRouter, ExecutionRunner, SimulationExecutionAdapter, TaskState, @@ -297,7 +332,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, @@ -305,10 +341,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. @@ -378,11 +419,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 @@ -436,6 +490,21 @@ A minimal implementation looks like: from dataclasses import dataclass from typing import ClassVar + import torch + + from embodichain.lab.sim.atomic_actions import ( + CARTESIAN_POSE_CAPABILITY, + ActionOptions, + ActionPlan, + AtomicAction, + JointPositionTarget, + PlanningContext, + ResolvedActionRequest, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, + ) + @dataclass(frozen=True, slots=True) class PushGoal: contact_pose: torch.Tensor @@ -448,7 +517,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) @@ -460,8 +543,11 @@ A minimal implementation looks like: ) -> ActionPlan: goal = self.require_goal(request) options = request.skill_options - # Resolve the bound resource, plan from context.robot.qpos, and - # return a full-robot TimedTrajectory or position tensor. + motion = request.binding.endpoint("primary", "motion") + motion_target = motion.require_target(JointPositionTarget) + # Plan from context.robot.qpos using motion_target.joint_ids. + # The joint helper lowers the result into RuntimeCommandFrame values + # and retains the trajectory for joint-position feedback. return self.build_plan( request, context, @@ -469,6 +555,13 @@ A minimal implementation looks like: trajectory=full_robot_positions, ) +For a non-joint endpoint, define a typed ``RuntimeEndpointTarget`` and matching +``RuntimeCommandPayload``, have the profile endpoint adapter produce that +target, and call ``build_command_plan(commands=TimedCommandSequence(...))``. +Register the matching ``EndpointCommandTransport`` with the runner's router. +The skill contract, resource graph, binding, runner, and recovery model do not +gain controller-specific fields. + Do not step simulation, mutate ``PlanningContext``, commit ``StateDelta``, or expose planner-specific configuration through the goal. See the in-repository ``add-atomic-action`` skill for the complete checklist. diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index 99cf83e04..caabafd44 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -34,7 +34,12 @@ AssembleAffordance, InteractionPoints, ) -from .bindings import ActionBinding, ResolvedActionBinding, ResolvedControlPart +from .bindings import ( + ActionBinding, + EndpointBinding, + JointPositionTarget, + RuntimeEndpointTarget, +) from .control import ( ActionControlOverrides, ControlCommand, @@ -53,20 +58,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, @@ -80,6 +84,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, @@ -144,7 +156,6 @@ __all__ = [ "ActionBinding", - "ActionBindingRoute", "ActionControlOverrides", "ActionInvocation", "ActionOptions", @@ -177,10 +188,15 @@ "DisjointResourceSlots", "DisjointSlotEndpoints", "EndEffectorPoseGoal", + "EndpointBinding", + "EndpointCommand", + "EndpointCommandRouter", + "EndpointCommandTransport", "EntityState", "EffectVerificationRequest", "EffectVerifier", "ExecutionClock", + "ExecutionFeedbackMode", "ExecutionEvent", "ExecutionEventKind", "ExecutionRunner", @@ -199,8 +215,9 @@ "INVERSE_KINEMATICS_CAPABILITY", "InteractionPoints", "JointPositionGoal", - "JointCommand", "JointPositionCommand", + "JointPositionPayload", + "JointPositionTarget", "JOINT_POSITION_CAPABILITY", "MotionPolicy", "MonotonicExecutionClock", @@ -229,9 +246,10 @@ "RigidObjectSceneProvider", "RigidObjectSceneProviderCfg", "ResolvedActionRequest", - "ResolvedActionBinding", - "ResolvedControlPart", "RobotObservation", + "RuntimeCommandFrame", + "RuntimeCommandPayload", + "RuntimeEndpointTarget", "RunnerStatus", "RunnerStep", "RunnerStepCallback", @@ -246,6 +264,7 @@ "StateDelta", "SimulationExecutionAdapter", "TaskState", + "TimedCommandSequence", "TimedTrajectory", "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 d4b7906bb..4b5841c27 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 binding_contract: SkillBindingContract | None = None """Explicit generic resource contract used by the semantic skill layer.""" @@ -169,23 +175,12 @@ def __post_init__(self) -> None: raise TypeError( "SkillDescriptor.options_type must be an ActionOptions subclass." ) - for field_name in ("manipulator_roles", "end_effector_roles"): - roles = tuple(getattr(self, field_name)) - if len(set(roles)) != len(roles) or not all( - isinstance(role, str) and role for role in roles - ): - raise ValueError(f"{field_name} must contain unique non-empty roles.") - object.__setattr__(self, field_name, roles) if self.binding_contract is not None: if not isinstance(self.binding_contract, SkillBindingContract): raise TypeError( "SkillDescriptor.binding_contract must be a " "SkillBindingContract or None." ) - self.binding_contract.validate_action_roles( - manipulator_roles=self.manipulator_roles, - end_effector_roles=self.end_effector_roles, - ) class AtomicAction(Generic[GoalT, OptionsT], ABC): @@ -205,12 +200,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.""" @@ -316,8 +305,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, binding_contract=cls.__dict__.get("binding_contract"), ) @@ -354,10 +341,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 @@ -378,7 +367,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, ), @@ -409,10 +398,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( @@ -431,7 +422,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, @@ -551,32 +548,70 @@ 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.") + commands = self._authorize_command_targets(request, commands) + success_mask = normalize_success_mask( + success, + num_envs=context.batch_size, + device=self.device, + name="Planning success", + ) + masked_commands = TimedCommandSequence( + frames=tuple( + frame.with_active_mask(frame.active_mask & success_mask) + for frame in commands.frames + ), + env_ids=commands.env_ids, + ) + segments = self._build_segments( + segment_lengths, + frame_count=masked_commands.frame_count, + ) if diagnostics is None: diagnostics = PlannerDiagnostics( backend=self.planning_services.planner_name @@ -584,25 +619,211 @@ def build_plan( return ActionPlan( skill_id=self.skill_id, plan_success=success_mask, - trajectory=timed, + commands=masked_commands, recovery_policy=request.recovery_policy, planned_scene_version=context.scene.version, planned_collision_world_revision=( context.scene.collision_world_revisions(context.batch_size) ), diagnostics=diagnostics, - segments=tuple(segments), + 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, + ) -> TimedCommandSequence: + """Bind every emitted command to an endpoint authorized by the request. + + Actions may choose a subset of their bound endpoints for any frame, but + they cannot synthesize a destination outside the resolved resource + binding. The returned sequence replaces caller-provided target metadata + with the engine-owned binding snapshot, so transports never receive + altered joint claims or other target fields. + """ + authorized: dict[tuple[str, str], list[EndpointBinding]] = {} + for endpoint in request.binding.endpoints: + authorized.setdefault(endpoint.destination_key, []).append(endpoint) + unknown = sorted( + { + command.destination_key + for frame in commands.frames + for command in frame.commands + if command.destination_key not in authorized + } + ) + if unknown: + raise ValueError( + "Runtime commands reference destinations not authorized by the " + f"action binding: {unknown}." + ) + + frames: list[RuntimeCommandFrame] = [] + for frame in commands.frames: + endpoint_commands: list[EndpointCommand] = [] + joint_owners: dict[int, tuple[str, str]] = {} + token_owners: dict[str, tuple[str, str]] = {} + for command in frame.commands: + bound_endpoints = authorized[command.destination_key] + target = bound_endpoints[0].target + if any( + type(endpoint.target) is not type(target) + for endpoint in bound_endpoints[1:] + ): + raise ValueError( + f"Action binding destination {command.destination_key} has " + "incompatible target declarations." + ) + if type(command.target) is not type(target): + raise TypeError( + f"Runtime command destination {command.destination_key} uses " + f"target type {type(command.target).__name__}, but its bound " + f"endpoint uses {type(target).__name__}." + ) + if isinstance(target, JointPositionTarget) and command.target != target: + raise ValueError( + f"Runtime command destination {command.destination_key} " + "does not preserve its bound joint-position target." + ) + joint_ids = { + joint_id + for endpoint in bound_endpoints + for joint_id in endpoint.joint_ids + } + claim_tokens = { + token + for endpoint in bound_endpoints + for token in endpoint.claim_tokens + } + overlapping_joints = sorted(joint_ids & joint_owners.keys()) + overlapping_tokens = sorted(claim_tokens & token_owners.keys()) + if overlapping_joints or overlapping_tokens: + conflicting_destinations = sorted( + {joint_owners[joint_id] for joint_id in overlapping_joints} + | {token_owners[token] for token in overlapping_tokens} + ) + raise ValueError( + f"Runtime command destination {command.destination_key} " + f"conflicts with {conflicting_destinations} on bound joint " + f"IDs {overlapping_joints} or claim tokens " + f"{overlapping_tokens}." + ) + for joint_id in joint_ids: + joint_owners[joint_id] = command.destination_key + for token in claim_tokens: + token_owners[token] = command.destination_key + endpoint_commands.append( + EndpointCommand(target=target, payload=command.payload) + ) + frames.append( + RuntimeCommandFrame( + commands=tuple(endpoint_commands), + active_mask=frame.active_mask, + env_ids=frame.env_ids, + hold_duration=frame.hold_duration, + ) + ) + return TimedCommandSequence(frames=tuple(frames), env_ids=commands.env_ids) + + def _joint_command_sequence( + self, + request: ResolvedActionRequest[GoalT, OptionsT], + trajectory: TimedTrajectory, + *, + active_mask: torch.Tensor, + ) -> TimedCommandSequence: + """Lower one full-robot planner trajectory to endpoint commands.""" + targets = tuple( + ( + endpoint, + endpoint.require_target(JointPositionTarget), + ) + for endpoint in request.binding.endpoints + ) + if not targets: + raise ValueError( + "Joint trajectory plans require at least one bound " + "JointPositionTarget endpoint." + ) + frames: list[RuntimeCommandFrame] = [] + for waypoint_index in range(trajectory.waypoint_count): + endpoint_commands: list[EndpointCommand] = [] + for _, target in targets: + joint_ids = list(target.joint_ids) + velocities = ( + None + if trajectory.velocities is None + else trajectory.velocities[:, waypoint_index, joint_ids] + ) + endpoint_commands.append( + EndpointCommand( + target=target, + payload=JointPositionPayload( + positions=trajectory.positions[ + :, waypoint_index, joint_ids + ], + velocities=velocities, + ), + ) + ) + next_waypoint_index = min( + waypoint_index + 1, + trajectory.waypoint_count - 1, + ) + # ``dt[:, i]`` is the arrival interval for waypoint ``i``. After + # dispatching it, wait for the next arrival interval; the terminal + # frame deliberately reuses its own interval as a settling window, + # preserving the closed-loop runner's pre-PR2C timing contract. + frames.append( + RuntimeCommandFrame( + commands=tuple(endpoint_commands), + active_mask=active_mask, + env_ids=trajectory.env_ids, + hold_duration=trajectory.dt[:, next_waypoint_index], + ) + ) + return TimedCommandSequence(frames=tuple(frames), env_ids=trajectory.env_ids) + + @staticmethod + def _build_segments( + segment_lengths: Mapping[str, int] | None, + *, + frame_count: int, + ) -> tuple[TrajectorySegment, ...]: + """Validate optional named ranges for one command sequence.""" + if segment_lengths is None: + return () + segments: list[TrajectorySegment] = [] + offset = 0 + for name, length in segment_lengths.items(): + if not isinstance(name, str) or not name: + raise ValueError("Trajectory segment names must be non-empty.") + if isinstance(length, bool) or not isinstance(length, int): + raise TypeError("Trajectory segment lengths must be integers.") + if length < 0: + raise ValueError("Trajectory segment lengths must be non-negative.") + if length == 0: + continue + segments.append( + TrajectorySegment(name=name, start=offset, stop=offset + length) + ) + offset += length + if offset != frame_count: + raise ValueError( + "Trajectory segment lengths must sum to the command frame count " + f"({frame_count}), got {offset}." + ) + return tuple(segments) + def failed_plan( self, request: ResolvedActionRequest[GoalT, OptionsT], @@ -620,23 +841,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 c52d724ea..a60499326 100644 --- a/embodichain/lab/sim/atomic_actions/engine.py +++ b/embodichain/lab/sim/atomic_actions/engine.py @@ -23,6 +23,7 @@ import torch +from .bindings import ActionBinding from .core import AtomicAction, SkillDescriptor from .control import ControlPartCommandProfile from .invocation import ActionInvocation, ResolvedActionRequest @@ -118,6 +119,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 +190,43 @@ 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 register(self, action: AtomicAction, *, replace: bool = False) -> None: """Register one action instance using its descriptor. @@ -219,6 +262,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, @@ -372,7 +459,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) @@ -452,15 +547,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 8e14b608c..f49bf8ae4 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,150 @@ 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, ) + def _validate_revision_identity( + self, + *, + skill_id: str, + invocation_id: str | None, + revision: int, + ) -> None: + """Validate identity and ordering shared by staged and direct revisions.""" + current = self._requests[self._invocation_index] + if skill_id != current.skill_id: + raise ValueError( + f"Revision skill_id {skill_id!r} does not match " + f"the active skill {current.skill_id!r}." + ) + if invocation_id != current.invocation_id: + raise ValueError( + "Revision invocation_id must match the active invocation_id." + ) + if revision <= current.revision: + raise ValueError( + f"Revision must advance beyond {current.revision}, got " f"{revision}." + ) + @property def latest_context(self) -> PlanningContext: """Latest validated context with the session's verified task state.""" return self._context @property - def active_trajectory(self) -> TimedTrajectory: - """Return an owned snapshot of the active action trajectory. + def active_commands(self) -> TimedCommandSequence: + """Return an owned snapshot of the active action command sequence. This inspection surface is intended for diagnostics and visualization. Mutating the returned tensors cannot affect execution state. """ assert self._plan is not None - return self._plan.trajectory.snapshot() + return self._plan.commands.snapshot() def trajectory_segment(self, name: str) -> TrajectorySegment: """Return named segment metadata for the active action plan. @@ -360,33 +420,7 @@ def tick( Returns: Status, optional command, events, and current verified task state. """ - self._engine._validate_context(context) - if context.robot.timestamp < self._context.robot.timestamp: - raise ValueError("Execution tick timestamps must be monotonic.") - if context.scene.timestamp < self._context.scene.timestamp: - raise ValueError("Scene snapshot timestamps must be monotonic.") - if context.scene.version < self._context.scene.version: - raise ValueError("Scene snapshot versions must be monotonic.") - previous_collision_revision = torch.tensor( - self._context.scene.collision_world_revisions(context.batch_size), - dtype=torch.long, - device=context.robot.qpos.device, - ) - current_collision_revision = torch.tensor( - context.scene.collision_world_revisions(context.batch_size), - dtype=torch.long, - device=context.robot.qpos.device, - ) - if (current_collision_revision < previous_collision_revision).any(): - raise ValueError("Collision-world revisions must be monotonic.") - if not torch.equal(context.env_ids, self._context.env_ids): - raise ValueError("Execution tick env_ids must remain stable and ordered.") - self._context = PlanningContext( - robot=context.robot, - task=self._task_state, - scene=context.scene, - env_ids=context.env_ids, - ) + self._context = self._validated_context(context) events = self._drain_events() if self._status is not ExecutionStatus.RUNNING: return self._tick_result(command=None, events=events) @@ -396,12 +430,16 @@ def tick( execution_mask = ( self._pending_effect.env_mask & self._pending & self._plan.plan_success ) - command, completion_events = self._finish_action( + command, hold_targets, completion_events = self._finish_action( execution_mask, effect_success, ) events.extend(completion_events) - return self._tick_result(command=command, events=events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) plan = self._plan execution_mask = self._pending & plan.plan_success @@ -421,8 +459,8 @@ def tick( plan = self._plan execution_mask = self._pending & self._plan.plan_success - trajectory = plan.trajectory - if self._waypoint_index < trajectory.waypoint_count: + commands = plan.commands + if self._waypoint_index < commands.frame_count: command = self._command_at(plan, self._waypoint_index, execution_mask) self._waypoint_index += 1 return self._tick_result(command=command, events=events) @@ -444,9 +482,27 @@ def tick( assert self._plan is not None plan = self._plan execution_mask = self._pending & self._plan.plan_success - command = self._command_at(plan, 0, execution_mask) - self._waypoint_index = 1 - return self._tick_result(command=command, events=events) + if plan.commands.frame_count > 0: + command = self._command_at(plan, 0, execution_mask) + self._waypoint_index = 1 + return self._tick_result(command=command, events=events) + events.append( + self._event( + ExecutionEventKind.TRAJECTORY_COMPLETED, + execution_mask, + "Replanned action has no executable command frame.", + ) + ) + command, hold_targets, completion_events = self._finish_action( + execution_mask, + effect_success, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) events.append( self._event( @@ -456,12 +512,46 @@ def tick( ) ) - command, completion_events = self._finish_action( + command, hold_targets, completion_events = self._finish_action( execution_mask, effect_success, ) events.extend(completion_events) - return self._tick_result(command=command, events=events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) + + def _validated_context(self, context: PlanningContext) -> PlanningContext: + """Validate one monotonic observation and attach verified task state.""" + self._engine._validate_context(context) + if context.robot.timestamp < self._context.robot.timestamp: + raise ValueError("Execution tick timestamps must be monotonic.") + if context.scene.timestamp < self._context.scene.timestamp: + raise ValueError("Scene snapshot timestamps must be monotonic.") + if context.scene.version < self._context.scene.version: + raise ValueError("Scene snapshot versions must be monotonic.") + previous_collision_revision = torch.tensor( + self._context.scene.collision_world_revisions(context.batch_size), + dtype=torch.long, + device=context.robot.qpos.device, + ) + current_collision_revision = torch.tensor( + context.scene.collision_world_revisions(context.batch_size), + dtype=torch.long, + device=context.robot.qpos.device, + ) + if (current_collision_revision < previous_collision_revision).any(): + raise ValueError("Collision-world revisions must be monotonic.") + if not torch.equal(context.env_ids, self._context.env_ids): + raise ValueError("Execution tick env_ids must remain stable and ordered.") + return PlanningContext( + robot=context.robot, + task=self._task_state, + scene=context.scene, + env_ids=context.env_ids, + ) def _plan_current( self, @@ -480,11 +570,27 @@ def _install_plan( event_kind: ExecutionEventKind, ) -> None: """Install an already validated plan as the current execution plan.""" + replacement_targets = { + (target.transport_id, target.target_id): target.snapshot() + for target in plan.commands.targets + } + replacement_destinations = frozenset(replacement_targets) + self._validate_destination_continuity(plan, event_kind) + if ( + event_kind + not in ( + ExecutionEventKind.REPLANNED, + ExecutionEventKind.INVOCATION_REVISED, + ) + or replacement_destinations + ): + self._active_targets = replacement_targets self._plan = plan self._waypoint_index = 0 self._planned_scene = context.scene self._action_started_at = context.robot.timestamp - self._last_command = None + self._last_joint_command = None + self._last_joint_ids = () self._last_command_mask.zero_() self._pending_effect = None planned_mask = self._pending & plan.plan_success @@ -492,6 +598,67 @@ def _install_plan( self._event(event_kind, planned_mask, "Planned from the latest context.") ) + def _validate_destination_continuity( + self, + plan: ActionPlan, + event_kind: ExecutionEventKind, + ) -> None: + """Reject in-place plans that change controller or safe-hold ownership.""" + if event_kind not in ( + ExecutionEventKind.REPLANNED, + ExecutionEventKind.INVOCATION_REVISED, + ): + return + replacement_targets = { + (target.transport_id, target.target_id): target + for target in plan.commands.targets + } + active_destinations = frozenset(self._active_targets) + replacement_destinations = frozenset(replacement_targets) + if not active_destinations: + return + if not replacement_destinations: + if event_kind is ExecutionEventKind.REPLANNED: + return + raise ValueError( + "Invocation revisions must declare the active runtime destination " + "set; an empty replacement plan cannot prove target continuity." + ) + if replacement_destinations == active_destinations: + mismatched_fingerprints = sorted( + destination + for destination in active_destinations + if replacement_targets[destination].address_fingerprint + != self._active_targets[destination].address_fingerprint + ) + if not mismatched_fingerprints: + return + prefix = ( + "Recovery replans" + if event_kind is ExecutionEventKind.REPLANNED + else "Invocation revisions" + ) + guidance = ( + "" + if event_kind is ExecutionEventKind.REPLANNED + else " Start a new invocation to change runtime target addresses." + ) + raise ValueError( + f"{prefix} must preserve each runtime target address fingerprint; " + f"changed={mismatched_fingerprints}.{guidance}" + ) + if event_kind is ExecutionEventKind.REPLANNED: + prefix = "Recovery replans" + guidance = "" + else: + prefix = "Invocation revisions" + guidance = " Start a new invocation to change runtime destinations." + raise ValueError( + f"{prefix} must preserve the active runtime destination set; " + f"previous={sorted(active_destinations)}, " + f"replacement={sorted(replacement_destinations)}.{guidance}" + ) + def _recover_if_needed( self, plan: ActionPlan, @@ -517,9 +684,18 @@ def _recover_if_needed( ExecutionEventKind.COLLISION_WORLD_CHANGED, "The collision world changed after this trajectory was planned.", ) - if self._last_command is not None: + if ( + plan.feedback_mode is ExecutionFeedbackMode.JOINT_POSITION + and self._last_joint_command is not None + and self._last_joint_ids + ): + joint_ids = list(self._last_joint_ids) tracking_error = torch.amax( - torch.abs(self._context.robot.qpos - self._last_command), dim=1 + torch.abs( + self._context.robot.qpos[:, joint_ids] + - self._last_joint_command[:, joint_ids] + ), + dim=1, ) tracking_mask = ( execution_mask @@ -598,7 +774,7 @@ def _attempt_action_retry( ) if allowed.any(): self._action_retries[allowed] += 1 - self._replans.zero_() + self._replans[allowed] = 0 events.append( self._event( ExecutionEventKind.ACTION_RETRY, @@ -615,9 +791,20 @@ def _finish_action( self, execution_mask: torch.Tensor, effect_success: torch.Tensor | None, - ) -> tuple[JointCommand | None, list[ExecutionEvent]]: + ) -> tuple[ + RuntimeCommandFrame | None, + tuple[RuntimeEndpointTarget, ...], + list[ExecutionEvent], + ]: """Verify effects, update symbolic state, and advance the action barrier.""" assert self._plan is not None + plan_targets = self._plan.commands.targets + active_targets = ( + plan_targets + if plan_targets + else tuple(target.snapshot() for target in self._active_targets.values()) + ) + orphaned_targets = bool(active_targets) and not plan_targets events: list[ExecutionEvent] = [] planning_failed = self._pending & ~self._plan.plan_success if not execution_mask.any() and planning_failed.any(): @@ -629,8 +816,8 @@ def _finish_action( ) ) if self._status is not ExecutionStatus.RUNNING: - return None, events - return self._hold_command(), events + return None, active_targets, events + return None, active_targets, events if self._plan.expected_effects.is_empty: verified = execution_mask @@ -644,7 +831,7 @@ def _finish_action( "Expected symbolic effects require external verification.", ) ) - return self._hold_command(), events + return None, active_targets, events else: verified_input = self._normalize_mask(effect_success, "effect_success") verified = execution_mask & verified_input @@ -672,11 +859,11 @@ def _finish_action( ) ) if self._status is not ExecutionStatus.RUNNING: - return None, events - return self._hold_command(), events + return None, active_targets, events + return None, active_targets, events if self._pending.any(): - return self._hold_command(), events + return None, active_targets, events events.append( self._event( ExecutionEventKind.ACTION_COMPLETED, @@ -698,7 +885,7 @@ def _finish_action( "Invocation sequence completed.", ) ) - return None, events + return None, (active_targets if orphaned_targets else ()), events self._pending = self._eligible.clone() self._pending_effect = None @@ -706,64 +893,84 @@ def _finish_action( self._replans.zero_() self._plan_current(self._context, ExecutionEventKind.ACTION_PLANNED) events.extend(self._drain_events()) - return self._hold_command(), events + return None, active_targets, events def _command_at( self, plan: ActionPlan, waypoint_index: int, active_mask: torch.Tensor, - ) -> JointCommand: - """Build one command and retain it for tracking-error monitoring.""" - positions = plan.trajectory.positions[:, waypoint_index] - hold = self._context.robot.qpos - positions = torch.where(active_mask[:, None], positions, hold) - velocities = None - if plan.trajectory.velocities is not None: - values = plan.trajectory.velocities[:, waypoint_index] - velocities = torch.where( - active_mask[:, None], values, torch.zeros_like(values) - ) - self._last_command = positions.clone() - self._last_command_mask = active_mask.clone() - # ``dt[:, i]`` leads to waypoint ``i``. After dispatching waypoint - # ``i``, wait for ``dt[:, i + 1]`` before the next dispatch. Reuse the - # final arrival interval as its terminal settling window. - next_waypoint_index = min( - waypoint_index + 1, - plan.trajectory.waypoint_count - 1, - ) - hold_duration = plan.trajectory.dt[:, next_waypoint_index] - return JointCommand( - positions=positions, - velocities=velocities, - active_mask=active_mask, - env_ids=plan.trajectory.env_ids, - hold_duration=hold_duration, - ) + ) -> RuntimeCommandFrame: + """Return one frame and retain joint targets when feedback requires it.""" + frame = plan.commands.frames[waypoint_index] + frame = frame.with_active_mask(frame.active_mask & active_mask) + if plan.feedback_mode is ExecutionFeedbackMode.JOINT_POSITION: + positions = self._context.robot.qpos.clone() + commanded_joint_ids: list[int] = [] + for command in frame.commands: + if not isinstance( + command.target, JointPositionTarget + ) or not isinstance( + command.payload, + JointPositionPayload, + ): + raise TypeError( + "joint_position feedback requires only joint-position " + "targets and payloads." + ) + joint_ids = list(command.target.joint_ids) + commanded_joint_ids.extend(joint_ids) + positions[:, joint_ids] = torch.where( + frame.active_mask[:, None], + command.payload.positions, + positions[:, joint_ids], + ) + self._last_joint_command = positions + self._last_joint_ids = tuple(commanded_joint_ids) + self._last_command_mask = frame.active_mask.clone() + else: + self._last_joint_command = None + self._last_joint_ids = () + self._last_command_mask.zero_() + return frame - def _hold_command(self) -> JointCommand: - """Build a passive hold command from the latest observation.""" - return JointCommand( - positions=self._context.robot.qpos, - velocities=torch.zeros_like(self._context.robot.qpos), - active_mask=torch.zeros_like(self._eligible), - env_ids=self._context.env_ids, - hold_duration=torch.zeros( + def _terminal_error(self, plan: ActionPlan) -> torch.Tensor: + """Return terminal error for the plan's explicit feedback contract.""" + if plan.feedback_mode is ExecutionFeedbackMode.TIMED: + return torch.zeros( self._context.batch_size, - dtype=torch.float32, + dtype=self._context.robot.qpos.dtype, device=self._context.robot.qpos.device, - ), - ) - - def _terminal_error(self, plan: ActionPlan) -> torch.Tensor: - """Return per-row max joint error to the action terminal command.""" - if plan.trajectory.waypoint_count == 0: - return torch.full_like(self._eligible, float("inf"), dtype=torch.float32) - return torch.amax( - torch.abs(self._context.robot.qpos - plan.trajectory.positions[:, -1]), - dim=1, - ) + ) + if plan.commands.frame_count == 0: + return torch.full_like( + self._eligible, + float("inf"), + dtype=self._context.robot.qpos.dtype, + ) + errors: list[torch.Tensor] = [] + for command in plan.commands.frames[-1].commands: + if not isinstance(command.target, JointPositionTarget) or not isinstance( + command.payload, + JointPositionPayload, + ): + raise TypeError( + "joint_position feedback requires only joint-position targets " + "and payloads." + ) + joint_ids = list(command.target.joint_ids) + errors.append( + torch.abs( + self._context.robot.qpos[:, joint_ids] - command.payload.positions + ) + ) + if not errors: + return torch.full_like( + self._eligible, + float("inf"), + dtype=self._context.robot.qpos.dtype, + ) + return torch.amax(torch.cat(errors, dim=1), dim=1) def _dynamic_scene_change_mask(self, plan: ActionPlan) -> torch.Tensor: """Detect material motion of entities referenced by the action goal.""" @@ -901,14 +1108,16 @@ def _update_terminal_status(self) -> None: def _tick_result( self, *, - command: JointCommand | None, + command: RuntimeCommandFrame | None, events: list[ExecutionEvent], + hold_targets: tuple[RuntimeEndpointTarget, ...] = (), ) -> ExecutionTick: """Build an immutable tick result.""" return ExecutionTick( status=self._status, eligible_mask=self._eligible, command=command, + hold_targets=hold_targets, events=tuple(events), task_state=self._task_state, pending_effect=self._pending_effect, @@ -922,5 +1131,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 eeb1d2dcc..f919931ba 100644 --- a/embodichain/lab/sim/atomic_actions/plans.py +++ b/embodichain/lab/sim/atomic_actions/plans.py @@ -19,6 +19,7 @@ from __future__ import annotations from dataclasses import dataclass, field +from enum import Enum from types import MappingProxyType from typing import Any, Mapping, Sequence @@ -26,8 +27,10 @@ from embodichain.lab.sim.planners.utils import normalize_success_mask +from .bindings import JointPositionTarget from .effects import StateDelta from .policies import RecoveryPolicy +from .runtime_commands import JointPositionPayload, TimedCommandSequence from .state import PlanningContext @@ -93,7 +96,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: @@ -336,6 +357,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. @@ -375,18 +403,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 @@ -407,21 +437,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 @@ -446,7 +634,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.") @@ -457,7 +645,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 @@ -468,10 +656,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) @@ -500,9 +698,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): @@ -538,7 +737,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: @@ -555,6 +759,7 @@ def segment(self, action_index: int, name: str) -> TrajectorySegment: __all__ = [ "ActionPlan", "CompiledTrajectory", + "ExecutionFeedbackMode", "PlannerDiagnostics", "TimedTrajectory", "TrajectorySegment", diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py index 84d1d7f5b..b704502bf 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 ..affordance import AntipodalAffordance -from ..bindings import ResolvedControlPart +from ..bindings import JointPositionTarget from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand from ..core import AtomicAction, ObjectSemantics from ..effects import StateDelta @@ -41,7 +41,6 @@ from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan, normalize_success_mask from ..requirements import ( - ActionBindingRoute, DisjointResourceSlots, DisjointSlotEndpoints, GRASP_CAPABILITY, @@ -162,10 +161,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 @@ -340,8 +339,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( SkillResourceSlot( @@ -350,7 +347,6 @@ class CoordinatedPickment( SkillEndpointRequirement( endpoint_id="motion", capabilities=frozenset({INVERSE_KINEMATICS_CAPABILITY}), - route=ActionBindingRoute("manipulator", role), ), SkillEndpointRequirement( endpoint_id="grasp", @@ -359,7 +355,6 @@ class CoordinatedPickment( OPEN_COMMAND: JointPositionCommand, GRASP_COMMAND: JointPositionCommand, }, - route=ActionBindingRoute("end_effector", role), ), ), constraints=(DisjointSlotEndpoints(("motion", "grasp")),), @@ -401,16 +396,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 +419,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 +756,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 +769,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 +778,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 +865,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"], @@ -1014,8 +1013,8 @@ def _plan( trajectory=full, 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 49e606b4d..c45b173b9 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 ..bindings import ResolvedControlPart +from ..bindings import JointPositionTarget from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand from ..core import AtomicAction from ..effects import StateDelta @@ -33,7 +33,6 @@ from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan, normalize_success_mask from ..requirements import ( - ActionBindingRoute, CARTESIAN_POSE_CAPABILITY, DisjointResourceSlots, DisjointSlotEndpoints, @@ -125,10 +124,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 @@ -142,8 +141,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=( SkillResourceSlot( @@ -152,7 +149,6 @@ class CoordinatedPlacement( SkillEndpointRequirement( endpoint_id="motion", capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), - route=ActionBindingRoute("manipulator", "placing"), ), SkillEndpointRequirement( endpoint_id="grasp", @@ -161,7 +157,6 @@ class CoordinatedPlacement( OPEN_COMMAND: JointPositionCommand, GRASP_COMMAND: JointPositionCommand, }, - route=ActionBindingRoute("end_effector", "placing"), ), ), constraints=(DisjointSlotEndpoints(("motion", "grasp")),), @@ -172,13 +167,11 @@ class CoordinatedPlacement( SkillEndpointRequirement( endpoint_id="motion", capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), - route=ActionBindingRoute("manipulator", "support"), ), SkillEndpointRequirement( endpoint_id="grasp", capabilities=frozenset({GRASP_CAPABILITY}), required_commands={GRASP_COMMAND: JointPositionCommand}, - route=ActionBindingRoute("end_effector", "support"), ), ), constraints=(DisjointSlotEndpoints(("motion", "grasp")),), @@ -196,16 +189,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." @@ -215,19 +212,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, @@ -262,8 +259,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." @@ -292,7 +289,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"], @@ -312,7 +309,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"], @@ -371,7 +368,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"], @@ -417,10 +414,10 @@ def _plan( trajectory=full, expected_effects=StateDelta( held_object_updates={ - resources.placing_arm.name: ( + resources.placing_arm.control_part: ( None if release else placing_held_object ), - resources.support_arm.name: support_held_object, + resources.support_arm.control_part: support_held_object, }, ), segment_lengths={ @@ -499,8 +496,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 de1379076..c0d9815f9 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py +++ b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py @@ -26,14 +26,13 @@ from embodichain.utils import logger from embodichain.utils.math import pose_inv -from ..bindings import ResolvedControlPart +from ..bindings import JointPositionTarget from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand from ..core import AtomicAction, ObjectSemantics, _same_object_identity from ..effects import StateDelta from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan, normalize_success_mask from ..requirements import ( - ActionBindingRoute, CARTESIAN_POSE_CAPABILITY, DisjointResourceSlots, DisjointSlotEndpoints, @@ -128,10 +127,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 @@ -150,8 +149,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=( SkillResourceSlot( @@ -165,7 +162,6 @@ class HandOver(AtomicAction[GraspGoal, HandOverOptions]): FORWARD_KINEMATICS_CAPABILITY, } ), - route=ActionBindingRoute("manipulator", "source"), ), SkillEndpointRequirement( endpoint_id="grasp", @@ -174,7 +170,6 @@ class HandOver(AtomicAction[GraspGoal, HandOverOptions]): OPEN_COMMAND: JointPositionCommand, GRASP_COMMAND: JointPositionCommand, }, - route=ActionBindingRoute("end_effector", "source"), ), ), constraints=(DisjointSlotEndpoints(("motion", "grasp")),), @@ -185,7 +180,6 @@ class HandOver(AtomicAction[GraspGoal, HandOverOptions]): SkillEndpointRequirement( endpoint_id="motion", capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), - route=ActionBindingRoute("manipulator", "destination"), ), SkillEndpointRequirement( endpoint_id="grasp", @@ -194,7 +188,6 @@ class HandOver(AtomicAction[GraspGoal, HandOverOptions]): OPEN_COMMAND: JointPositionCommand, GRASP_COMMAND: JointPositionCommand, }, - route=ActionBindingRoute("end_effector", "destination"), ), ), constraints=(DisjointSlotEndpoints(("motion", "grasp")),), @@ -218,16 +211,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." @@ -237,25 +234,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, @@ -285,14 +282,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( @@ -327,7 +325,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( @@ -383,7 +381,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"], @@ -401,7 +399,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"], @@ -424,7 +422,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"], @@ -444,7 +442,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"], @@ -580,8 +578,8 @@ def _plan( trajectory=full, expected_effects=StateDelta( held_object_updates={ - resources.transfer_arm.name: None, - resources.receive_arm.name: held_object, + resources.transfer_arm.control_part: None, + resources.receive_arm.control_part: held_object, } ), segment_lengths=segment_lengths, 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 2c28d8041..06523454e 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py @@ -23,12 +23,12 @@ import torch +from ..bindings import JointPositionTarget from ..core import AtomicAction from ..goals import PoseGoalValue, resolve_pose_goal, validate_pose_goal from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan from ..requirements import ( - ActionBindingRoute, CARTESIAN_POSE_CAPABILITY, SkillBindingContract, SkillEndpointRequirement, @@ -71,14 +71,12 @@ class MoveEndEffector(AtomicAction[EndEffectorPoseGoal, MoveEndEffectorOptions]) SkillEndpointRequirement( endpoint_id="motion", capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), - route=ActionBindingRoute("manipulator", "primary"), ), ), ), ), ) OptionsType: ClassVar[type] = MoveEndEffectorOptions - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) def _plan( self, @@ -87,9 +85,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 3b28b6fdf..d662ee004 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py @@ -30,13 +30,13 @@ ) from ._helpers import arm_qpos_from_state, resolve_object_target +from ..bindings import JointPositionTarget from ..control import GRASP_COMMAND, JointPositionCommand from ..core import AtomicAction from ..goals import PoseGoalValue, resolve_pose_goal, validate_pose_goal from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan from ..requirements import ( - ActionBindingRoute, CARTESIAN_POSE_CAPABILITY, DisjointSlotEndpoints, FORWARD_KINEMATICS_CAPABILITY, @@ -94,8 +94,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=( SkillResourceSlot( @@ -109,13 +107,11 @@ class MoveHeldObject(AtomicAction[HeldObjectPoseGoal, MoveHeldObjectOptions]): FORWARD_KINEMATICS_CAPABILITY, } ), - route=ActionBindingRoute("manipulator", "primary"), ), SkillEndpointRequirement( endpoint_id="grasp", capabilities=frozenset({GRASP_CAPABILITY}), required_commands={GRASP_COMMAND: JointPositionCommand}, - route=ActionBindingRoute("end_effector", "primary"), ), ), constraints=(DisjointSlotEndpoints(("motion", "grasp")),), @@ -132,12 +128,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 4cea2ab34..97712ce34 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_joints.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_joints.py @@ -23,11 +23,11 @@ import torch +from ..bindings import JointPositionTarget from ..core import AtomicAction from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan from ..requirements import ( - ActionBindingRoute, JOINT_POSITION_CAPABILITY, SkillBindingContract, SkillEndpointRequirement, @@ -78,7 +78,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=( @@ -88,7 +87,6 @@ class MoveJoints(AtomicAction[JointPositionGoal, MoveJointsOptions]): SkillEndpointRequirement( endpoint_id="motion", capabilities=frozenset({JOINT_POSITION_CAPABILITY}), - route=ActionBindingRoute("manipulator", "primary"), ), ), ), @@ -102,10 +100,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, @@ -149,7 +148,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 d2d71bf43..443d6f26b 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py @@ -34,7 +34,7 @@ from ._helpers import arm_qpos_from_state from ..affordance import AntipodalAffordance -from ..bindings import ResolvedControlPart +from ..bindings import JointPositionTarget from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand from ..core import AtomicAction, ObjectSemantics from ..effects import StateDelta @@ -49,7 +49,6 @@ from ..plans import ActionPlan, normalize_success_mask from ..policies import MotionPolicy from ..requirements import ( - ActionBindingRoute, BATCH_INVERSE_KINEMATICS_CAPABILITY, CARTESIAN_POSE_CAPABILITY, DisjointSlotEndpoints, @@ -162,8 +161,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=( SkillResourceSlot( @@ -178,7 +175,6 @@ class PickUp(AtomicAction[GraspGoal, PickUpOptions]): FORWARD_KINEMATICS_CAPABILITY, } ), - route=ActionBindingRoute("manipulator", "primary"), ), SkillEndpointRequirement( endpoint_id="grasp", @@ -187,7 +183,6 @@ class PickUp(AtomicAction[GraspGoal, PickUpOptions]): OPEN_COMMAND: JointPositionCommand, GRASP_COMMAND: JointPositionCommand, }, - route=ActionBindingRoute("end_effector", "primary"), ), ), constraints=(DisjointSlotEndpoints(("motion", "grasp")),), @@ -214,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, ) -> tuple[torch.Tensor, torch.Tensor, dict[str, int]]: @@ -234,7 +229,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, ), ) @@ -252,7 +247,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, ), ) @@ -316,21 +311,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( @@ -419,7 +416,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]: @@ -472,7 +469,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]: @@ -554,7 +551,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]) @@ -599,22 +596,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 86ae1f599..10decd7b3 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/place.py +++ b/embodichain/lab/sim/atomic_actions/primitives/place.py @@ -28,6 +28,7 @@ from ._helpers import arm_qpos_from_state, resolve_object_target from ..affordance import AssembleAffordance +from ..bindings import JointPositionTarget from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand from ..core import AtomicAction from ..effects import StateDelta @@ -40,7 +41,6 @@ from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan from ..requirements import ( - ActionBindingRoute, CARTESIAN_POSE_CAPABILITY, DisjointSlotEndpoints, FORWARD_KINEMATICS_CAPABILITY, @@ -165,8 +165,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=( SkillResourceSlot( @@ -180,7 +178,6 @@ class Place(AtomicAction[PlaceGoal | AssembleGoal, PlaceOptions]): FORWARD_KINEMATICS_CAPABILITY, } ), - route=ActionBindingRoute("manipulator", "primary"), ), SkillEndpointRequirement( endpoint_id="grasp", @@ -189,7 +186,6 @@ class Place(AtomicAction[PlaceGoal | AssembleGoal, PlaceOptions]): OPEN_COMMAND: JointPositionCommand, GRASP_COMMAND: JointPositionCommand, }, - route=ActionBindingRoute("end_effector", "primary"), ), ), constraints=(DisjointSlotEndpoints(("motion", "grasp")),), @@ -217,18 +213,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 d9d3ff920..0e8d6cf32 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/press.py +++ b/embodichain/lab/sim/atomic_actions/primitives/press.py @@ -24,13 +24,13 @@ import torch from ._helpers import arm_qpos_from_state +from ..bindings import JointPositionTarget from ..control import GRASP_COMMAND, JointPositionCommand from ..core import AtomicAction from ..goals import PoseGoalValue, resolve_pose_goal, validate_pose_goal from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan from ..requirements import ( - ActionBindingRoute, CARTESIAN_POSE_CAPABILITY, DisjointSlotEndpoints, GRASP_CAPABILITY, @@ -77,8 +77,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",) binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( SkillResourceSlot( @@ -92,13 +90,11 @@ class Press(AtomicAction[PressGoal, PressOptions]): JOINT_POSITION_CAPABILITY, } ), - route=ActionBindingRoute("manipulator", "primary"), ), SkillEndpointRequirement( endpoint_id="grasp", capabilities=frozenset({GRASP_CAPABILITY}), required_commands={GRASP_COMMAND: JointPositionCommand}, - route=ActionBindingRoute("end_effector", "primary"), ), ), constraints=(DisjointSlotEndpoints(("motion", "grasp")),), @@ -115,12 +111,15 @@ 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_close_qpos = end_effector.joint_positions( + motion_target = binding.endpoint("primary", "motion").require_target( + JointPositionTarget + ) + grasp = binding.endpoint("primary", "grasp") + grasp_target = grasp.require_target(JointPositionTarget) + control_part = motion_target.control_part + arm_joint_ids = list(motion_target.joint_ids) + hand_joint_ids = list(grasp_target.joint_ids) + hand_close_qpos = grasp.joint_positions( GRASP_COMMAND, num_envs=self.num_envs, device=self.device, 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 af1ebfc23..d53b61b69 100644 --- a/embodichain/lab/sim/atomic_actions/sim_adapter.py +++ b/embodichain/lab/sim/atomic_actions/sim_adapter.py @@ -26,11 +26,12 @@ from embodichain.utils import configclass -from .execution import JointCommand +from .bindings import JointPositionTarget, RuntimeEndpointTarget from .runner import ( CommandAcknowledgement, CommandAckStatus, ) +from .runtime_commands import JointPositionPayload, RuntimeCommandFrame from .scene import SceneProvider from .state import ( EntityState, @@ -262,6 +263,9 @@ class SimulationExecutionAdapter: initial_time: Initial elapsed simulation time in seconds. """ + transport_id = JointPositionTarget.TRANSPORT_ID + payload_type = JointPositionPayload + def __init__( self, simulation: SimulationManager, @@ -395,59 +399,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() @@ -457,10 +507,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: @@ -468,6 +524,13 @@ def cancel(self, *, timeout: float) -> CommandAcknowledgement: actual safe target. """ self._validate_timeout(timeout) + try: + self._validate_targets(targets) + except Exception as exc: + return CommandAcknowledgement( + CommandAckStatus.REJECTED, + f"{type(exc).__name__}: {exc}", + ) return CommandAcknowledgement.accepted_ack( "Simulation commands are synchronous; no queued command remained." ) @@ -497,18 +560,43 @@ def _read_optional_proprioception_tensor( return None return value if isinstance(value, torch.Tensor) else None - def _validate_command(self, command: JointCommand) -> None: - """Validate command identity and shape against the attached robot.""" - if not isinstance(command, JointCommand): - raise TypeError("command must be a JointCommand.") - qpos = self.robot.get_qpos() - if command.positions.shape != qpos.shape: - raise ValueError( - "Command shape must match full robot qpos, " - f"got {tuple(command.positions.shape)} and {tuple(qpos.shape)}." - ) + def _validate_command_frame(self, command: RuntimeCommandFrame) -> None: + """Validate one joint-position frame against the attached robot.""" + if not isinstance(command, RuntimeCommandFrame): + raise TypeError("command must be a RuntimeCommandFrame.") if not torch.equal(command.env_ids, self.env_ids): raise ValueError("Command env_ids must match the simulation adapter.") + self._validate_targets(command.targets) + for endpoint_command in command.commands: + if not isinstance(endpoint_command.payload, JointPositionPayload): + raise TypeError( + "SimulationExecutionAdapter accepts JointPositionPayload only." + ) + + def _validate_targets( + self, + targets: tuple[RuntimeEndpointTarget, ...], + ) -> None: + """Validate joint target ownership and robot dimensions.""" + if isinstance(targets, (str, bytes)): + raise TypeError("targets must be an iterable of runtime targets.") + qpos = self.robot.get_qpos() + seen_joints: set[int] = set() + for target in targets: + if not isinstance(target, JointPositionTarget): + raise TypeError( + "SimulationExecutionAdapter accepts JointPositionTarget only." + ) + if target.transport_id != self.transport_id: + raise ValueError("Target transport does not match this adapter.") + if max(target.joint_ids) >= qpos.shape[1]: + raise ValueError( + f"Target {target.target_id!r} references a joint outside robot DOF." + ) + overlaps = seen_joints.intersection(target.joint_ids) + if overlaps: + raise ValueError(f"Joint targets overlap on IDs {sorted(overlaps)}.") + seen_joints.update(target.joint_ids) @staticmethod def _validate_timeout(timeout: float) -> None: diff --git a/embodichain/lab/sim/atomic_actions/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/skills/profiles.py b/embodichain/lab/sim/skills/profiles.py index 0e1d1a8c9..f5f79dbf4 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) ) @@ -1316,35 +1348,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"Engine control profile {control_part!r} is missing " + f"profile command {command_name!r}." + ) + if not command.equivalent_to(installed_command): 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 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.""" @@ -1373,6 +1403,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 @@ -1390,7 +1432,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=( @@ -1504,7 +1546,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 ] @@ -1524,6 +1566,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.""" @@ -1611,11 +1675,6 @@ def _resource_matches( return False if not requirement.capabilities.issubset(endpoint.capabilities): return False - if ( - requirement.route is not None - and requirement.route.target not in endpoint.binding_values - ): - return False for command_name, command_type in requirement.required_commands.items(): command = endpoint.commands.get(command_name) if not isinstance(command, command_type): @@ -1701,15 +1760,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: @@ -1739,43 +1789,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_tasks/embodichain_tasks/multi_segments/cube_pick_place.py b/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py index 7d076b322..1965563b0 100644 --- a/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py +++ b/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py @@ -404,7 +404,6 @@ def _plan_pick_place_cycle( ) -> tuple[torch.Tensor, Iterable[torch.Tensor], torch.Tensor]: """Plan one pickup/place cycle from the cube's current measured pose.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, GraspGoal, MotionPolicy, @@ -416,16 +415,26 @@ def _plan_pick_place_cycle( source_pose = self._cube.get_local_pose(to_matrix=True).to( device=self.device, dtype=torch.float32 ) - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, + endpoints = { + "primary": { + "motion": "arm", + "grasp": "hand", + } + } + pick_binding = self._action_engine.bind_control_parts( + "pick_up", + endpoints, + ) + place_binding = self._action_engine.bind_control_parts( + "place", + endpoints, ) pick_compiled = self._action_engine.compile( ( ActionInvocation( skill_id="pick_up", goal=GraspGoal(self._cube_semantics), - binding=binding, + binding=pick_binding, motion_policy=MotionPolicy(sample_count=self.PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( pre_grasp_distance=0.15, @@ -458,7 +467,7 @@ def _plan_pick_place_cycle( ActionInvocation( skill_id="place", goal=PlaceGoal(place_eef_pose), - binding=binding, + binding=place_binding, motion_policy=MotionPolicy(sample_count=self.PLACE_SAMPLE_INTERVAL), skill_options=PlaceOptions( lift_height=0.14, diff --git a/embodichain_tasks/embodichain_tasks/tableware/blocks_ranking_rgb.py b/embodichain_tasks/embodichain_tasks/tableware/blocks_ranking_rgb.py index 5e09bac89..0a5d19dc7 100644 --- a/embodichain_tasks/embodichain_tasks/tableware/blocks_ranking_rgb.py +++ b/embodichain_tasks/embodichain_tasks/tableware/blocks_ranking_rgb.py @@ -211,7 +211,6 @@ def _plan_block_segment( ) -> tuple[torch.Tensor, Iterable[torch.Tensor], torch.Tensor]: """Plan an atomic PickUp followed by Place for one block.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, GraspGoal, MotionPolicy, @@ -233,9 +232,19 @@ def _plan_block_segment( source_pose[:, :3, :3], local_grasp_offset.unsqueeze(-1) ).squeeze(-1) grasp_pose[:, :3, 3] = source_pose[:, :3, 3] + world_grasp_offset - binding = ActionBinding( - manipulators={"primary": arm}, - end_effectors={"primary": hand}, + endpoints = { + "primary": { + "motion": arm, + "grasp": hand, + } + } + pick_binding = self._action_engine.bind_control_parts( + "pick_up", + endpoints, + ) + place_binding = self._action_engine.bind_control_parts( + "place", + endpoints, ) pick_compiled = self._action_engine.compile( ( @@ -245,7 +254,7 @@ def _plan_block_segment( self._object_semantics[uid], grasp_xpos=grasp_pose, ), - binding=binding, + binding=pick_binding, motion_policy=MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( pre_grasp_distance=0.12, @@ -277,7 +286,7 @@ def _plan_block_segment( ActionInvocation( skill_id="place", goal=PlaceGoal(place_eef_pose), - binding=binding, + binding=place_binding, motion_policy=MotionPolicy(sample_count=PLACE_SAMPLE_INTERVAL), skill_options=PlaceOptions( lift_height=0.15, diff --git a/embodichain_tasks/embodichain_tasks/tableware/stack_blocks_two.py b/embodichain_tasks/embodichain_tasks/tableware/stack_blocks_two.py index 9001f0c73..9279e3037 100644 --- a/embodichain_tasks/embodichain_tasks/tableware/stack_blocks_two.py +++ b/embodichain_tasks/embodichain_tasks/tableware/stack_blocks_two.py @@ -133,7 +133,6 @@ def _plan_stack( ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor]: """Plan PickUp then Place while threading the held-object state.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, GraspGoal, MotionPolicy, @@ -156,9 +155,19 @@ def _plan_stack( grasp_pose[:, :3, 3] = source_pose[:, :3, 3] + torch.tensor( GRASP_OFFSET, dtype=torch.float32, device=self.device ) - binding = ActionBinding( - manipulators={"primary": CONTROL_PART}, - end_effectors={"primary": HAND_CONTROL_PART}, + endpoints = { + "primary": { + "motion": CONTROL_PART, + "grasp": HAND_CONTROL_PART, + } + } + pick_binding = self._action_engine.bind_control_parts( + "pick_up", + endpoints, + ) + place_binding = self._action_engine.bind_control_parts( + "place", + endpoints, ) pick_compiled = self._action_engine.compile( ( @@ -168,7 +177,7 @@ def _plan_stack( self._stack_block_semantics, grasp_xpos=grasp_pose, ), - binding=binding, + binding=pick_binding, motion_policy=MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( pre_grasp_distance=0.12, @@ -201,7 +210,7 @@ def _plan_stack( ActionInvocation( skill_id="place", goal=PlaceGoal(place_eef_pose), - binding=binding, + binding=place_binding, motion_policy=MotionPolicy(sample_count=PLACE_SAMPLE_INTERVAL), skill_options=PlaceOptions( lift_height=0.10, diff --git a/examples/sim/planners/curobo_planner.py b/examples/sim/planners/curobo_planner.py index b3105f248..e391852b0 100644 --- a/examples/sim/planners/curobo_planner.py +++ b/examples/sim/planners/curobo_planner.py @@ -59,7 +59,6 @@ visualization_cfg_from_args, ) from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, @@ -749,7 +748,10 @@ def main() -> None: ) ) engine = AtomicActionEngine(motion_generator) - binding = ActionBinding(manipulators={"primary": control_part}) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": control_part}}, + ) motion_policy = MotionPolicy( motion_source="motion_gen", plan_opts=CuroboPlanOptions( diff --git a/scripts/benchmark/atomic_action/move_end_effector_benchmark.py b/scripts/benchmark/atomic_action/move_end_effector_benchmark.py index 0f2f1de48..7630f9641 100644 --- a/scripts/benchmark/atomic_action/move_end_effector_benchmark.py +++ b/scripts/benchmark/atomic_action/move_end_effector_benchmark.py @@ -121,7 +121,6 @@ def _run_case( """Run one MoveEndEffector case.""" torch = ensure_torch() from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, EndEffectorPoseGoal, MotionPolicy, @@ -129,6 +128,10 @@ def _run_case( reset_robot(robot, initial_qpos) target_pose = _make_pose(sim.device, pose_case.xyz) + binding = atomic_engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "arm"}}, + ) elapsed, mem_delta, peak_gpu, result = timed_call( lambda: atomic_engine.compile( @@ -136,7 +139,7 @@ def _run_case( ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(xpos=target_pose), - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=binding, motion_policy=MotionPolicy(sample_count=MOVE_SAMPLE_INTERVAL), ), ) diff --git a/scripts/benchmark/atomic_action/move_held_object_benchmark.py b/scripts/benchmark/atomic_action/move_held_object_benchmark.py index 0f66c5b9d..35dbb0386 100644 --- a/scripts/benchmark/atomic_action/move_held_object_benchmark.py +++ b/scripts/benchmark/atomic_action/move_held_object_benchmark.py @@ -175,7 +175,6 @@ def _prepare_held_state( ): """Run PickUp precondition outside the timed MoveHeldObject block.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -212,22 +211,26 @@ def _prepare_held_state( move_position = obj_pose[0, :3, 3].clone() move_position[2] = 0.36 move_target = make_pre_pick_eef_pose(robot, move_position) - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, + move_binding = atomic_engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "arm"}}, + ) + pick_binding = atomic_engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "arm", "grasp": "hand"}}, ) result = atomic_engine.compile( ( ActionInvocation( "move_end_effector", EndEffectorPoseGoal(xpos=move_target), - binding, + move_binding, MotionPolicy(sample_count=MOVE_SAMPLE_INTERVAL), ), ActionInvocation( "pick_up", GraspGoal(semantics=semantics), - binding, + pick_binding, MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( approach_direction=resolve_pickup_approach_direction( @@ -269,7 +272,6 @@ def _run_case( ): """Run one MoveHeldObject benchmark case.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -319,16 +321,17 @@ def _run_case( }, ) target_pose = _make_object_target_pose(sim.device, case.xyz) + binding = atomic_engine.bind_control_parts( + "move_held_object", + {"primary": {"motion": "arm", "grasp": "hand"}}, + ) elapsed, mem_delta, peak_gpu, result = timed_call( lambda: atomic_engine.compile( ( ActionInvocation( skill_id="move_held_object", goal=HeldObjectPoseGoal(object_target_pose=target_pose), - binding=ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ), + binding=binding, motion_policy=MotionPolicy( sample_count=MOVE_HELD_OBJECT_SAMPLE_INTERVAL ), diff --git a/scripts/benchmark/atomic_action/move_joints_benchmark.py b/scripts/benchmark/atomic_action/move_joints_benchmark.py index 82d43c164..abdfcc6bd 100644 --- a/scripts/benchmark/atomic_action/move_joints_benchmark.py +++ b/scripts/benchmark/atomic_action/move_joints_benchmark.py @@ -106,17 +106,19 @@ def _qpos(values, device): return torch.tensor(values, dtype=torch.float32, device=device) -def _targets_for_sequence(sequence_case: JointSequenceCase, device): +def _targets_for_sequence(atomic_engine, sequence_case: JointSequenceCase, device): """Build typed MoveJoints targets for a sequence case.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, JointPositionGoal, MotionPolicy, ) targets = [] - binding = ActionBinding(manipulators={"primary": "arm"}) + binding = atomic_engine.bind_control_parts( + "move_joints", + {"primary": {"motion": "arm"}}, + ) policy = MotionPolicy(sample_count=MOVE_JOINTS_SAMPLE_INTERVAL) for index, name in enumerate(sequence_case.sequence): if index == 0 and name == "ready": @@ -147,7 +149,7 @@ def _run_case( """Run one MoveJoints case.""" torch = ensure_torch() reset_robot(robot, initial_qpos) - steps = _targets_for_sequence(case, sim.device) + steps = _targets_for_sequence(atomic_engine, case, sim.device) elapsed, mem_delta, peak_gpu, result = timed_call( lambda: atomic_engine.compile(steps) ) diff --git a/scripts/benchmark/atomic_action/pickup_benchmark.py b/scripts/benchmark/atomic_action/pickup_benchmark.py index 4559d3e6e..f50885890 100644 --- a/scripts/benchmark/atomic_action/pickup_benchmark.py +++ b/scripts/benchmark/atomic_action/pickup_benchmark.py @@ -123,7 +123,6 @@ def _run_case( ): """Run one PickUp benchmark case.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -175,16 +174,17 @@ def _run_case( build_gripper_collision_cfg=build_gripper_collision_cfg, build_grasp_generator_cfg=build_grasp_generator_cfg, ) + binding = atomic_engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "arm", "grasp": "hand"}}, + ) elapsed, mem_delta, peak_gpu, result = timed_call( lambda: atomic_engine.compile( ( ActionInvocation( skill_id="pick_up", goal=GraspGoal(semantics=semantics), - binding=ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ), + binding=binding, motion_policy=MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( approach_direction=approach_direction, diff --git a/scripts/benchmark/atomic_action/place_benchmark.py b/scripts/benchmark/atomic_action/place_benchmark.py index 4c8242719..f63a7e73c 100644 --- a/scripts/benchmark/atomic_action/place_benchmark.py +++ b/scripts/benchmark/atomic_action/place_benchmark.py @@ -174,7 +174,6 @@ def _prepare_held_state( ): """Run PickUp precondition outside the timed Place block.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -212,9 +211,9 @@ def _prepare_held_state( ActionInvocation( skill_id="pick_up", goal=GraspGoal(semantics=semantics), - binding=ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, + binding=atomic_engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "arm", "grasp": "hand"}}, ), motion_policy=MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( @@ -255,7 +254,6 @@ def _run_case( ): """Run one Place benchmark case.""" from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -308,16 +306,17 @@ def _run_case( }, ) place_pose = _make_place_pose(sim.device, case.xyz) + binding = atomic_engine.bind_control_parts( + "place", + {"primary": {"motion": "arm", "grasp": "hand"}}, + ) elapsed, mem_delta, peak_gpu, result = timed_call( lambda: atomic_engine.compile( ( ActionInvocation( skill_id="place", goal=PlaceGoal(xpos=place_pose), - binding=ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ), + binding=binding, motion_policy=MotionPolicy(sample_count=PLACE_SAMPLE_INTERVAL), skill_options=PlaceOptions( lift_height=PLACE_LIFT_HEIGHT, diff --git a/scripts/benchmark/atomic_action/press_benchmark.py b/scripts/benchmark/atomic_action/press_benchmark.py index a5687af7f..fa534703f 100644 --- a/scripts/benchmark/atomic_action/press_benchmark.py +++ b/scripts/benchmark/atomic_action/press_benchmark.py @@ -81,7 +81,6 @@ def _ensure_runtime_imports() -> None: import torch as torch_module from embodichain.lab.sim import SimulationManager as simulation_manager_cls from embodichain.lab.sim.atomic_actions import ( - ActionBinding as action_binding_cls, ActionInvocation as action_invocation_cls, AtomicActionEngine as atomic_action_engine_cls, ControlPartCommandProfile as control_part_command_profile_cls, @@ -125,7 +124,6 @@ def _ensure_runtime_imports() -> None: "SimulationManager": simulation_manager_cls, "AtomicActionEngine": atomic_action_engine_cls, "ControlPartCommandProfile": control_part_command_profile_cls, - "ActionBinding": action_binding_cls, "ActionInvocation": action_invocation_cls, "EndEffectorPoseGoal": end_effector_pose_target_cls, "MotionPolicy": motion_policy_cls, @@ -562,27 +560,31 @@ def _timed_atomic_run( press_target: torch.Tensor, ) -> tuple[float, dict[str, float], float, bool, torch.Tensor]: """Run a timed atomic-action sequence and return timing/memory/results.""" + move_binding = atomic_engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "arm"}}, + ) + press_binding = atomic_engine.bind_control_parts( + "press", + {"primary": {"motion": "arm", "grasp": "hand"}}, + ) _reset_peak_gpu_memory() mem_before = _memory_snapshot() _sync_cuda() start = time.perf_counter() - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, - ) result = atomic_engine.compile( ( ActionInvocation( "move_end_effector", EndEffectorPoseGoal(xpos=move_target), - binding, + move_binding, MotionPolicy(sample_count=MOVE_SAMPLE_INTERVAL), ), ActionInvocation( "press", PressGoal(xpos=press_target), - binding, + press_binding, MotionPolicy(sample_count=PRESS_SAMPLE_INTERVAL), skill_options=PressOptions( hand_interp_steps=HAND_INTERP_STEPS, diff --git a/scripts/tutorials/atomic_action/assemble.py b/scripts/tutorials/atomic_action/assemble.py index 99151be3d..1d3127feb 100644 --- a/scripts/tutorials/atomic_action/assemble.py +++ b/scripts/tutorials/atomic_action/assemble.py @@ -38,7 +38,6 @@ from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AssembleAffordance, AssembleGoal, @@ -316,23 +315,28 @@ def run_assemble_demo( assemble_object_entity=can, assemble_to_base_pose=assemble_to_base, ) - binding = ActionBinding( - manipulators={"primary": "left_arm"}, - end_effectors={"primary": "left_hand"}, + endpoint_mapping = {"primary": {"motion": "left_arm", "grasp": "left_hand"}} + pick_binding = engine.bind_control_parts( + "pick_up", + endpoint_mapping, + ) + place_binding = engine.bind_control_parts( + "place", + endpoint_mapping, ) compiled = engine.compile( ( ActionInvocation( "pick_up", GraspGoal(can_semantics), - binding, + pick_binding, MotionPolicy(sample_count=PICKUP_SAMPLE_INTERVAL), skill_options=pick_up_options, ), ActionInvocation( "place", AssembleGoal(affordance=assemble_affordance), - binding, + place_binding, MotionPolicy(sample_count=PLACE_SAMPLE_INTERVAL), skill_options=place_options, ), diff --git a/scripts/tutorials/atomic_action/coordinated_pickment.py b/scripts/tutorials/atomic_action/coordinated_pickment.py index b953ee466..5d71fceb6 100644 --- a/scripts/tutorials/atomic_action/coordinated_pickment.py +++ b/scripts/tutorials/atomic_action/coordinated_pickment.py @@ -37,7 +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, @@ -437,15 +436,19 @@ def run_coordinated_pickment_demo( ) start_time = time.time() + binding = engine.bind_control_parts( + "coordinated_pickment", + { + "left": {"motion": "left_arm", "grasp": "left_hand"}, + "right": {"motion": "right_arm", "grasp": "right_hand"}, + }, + ) compiled = engine.compile( ( ActionInvocation( "coordinated_pickment", pickment_target, - ActionBinding( - manipulators={"left": "left_arm", "right": "right_arm"}, - end_effectors={"left": "left_hand", "right": "right_hand"}, - ), + binding, MotionPolicy(sample_count=PICKMENT_SAMPLE_INTERVAL), skill_options=pickment_options, ), diff --git a/scripts/tutorials/atomic_action/coordinated_placement.py b/scripts/tutorials/atomic_action/coordinated_placement.py index 0fe36f519..c1ab4c862 100644 --- a/scripts/tutorials/atomic_action/coordinated_placement.py +++ b/scripts/tutorials/atomic_action/coordinated_placement.py @@ -38,7 +38,6 @@ from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -620,6 +619,14 @@ def run_coordinated_placement_demo( sim.device, z_clearance=PAN_GRASP_Z_CLEARANCE, ) + left_pick_binding = engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "left_arm", "grasp": "left_hand"}}, + ) + right_pick_binding = engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "right_arm", "grasp": "right_hand"}}, + ) pick_invocations = ( ActionInvocation( skill_id="pick_up", @@ -627,10 +634,7 @@ def run_coordinated_placement_demo( semantics=bread_semantics, grasp_xpos=broadcast_pose_batch(bread_grasp_pose, num_envs=num_envs), ), - binding=ActionBinding( - manipulators={"primary": "left_arm"}, - end_effectors={"primary": "left_hand"}, - ), + binding=left_pick_binding, motion_policy=MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=left_pick_options, ), @@ -640,10 +644,7 @@ def run_coordinated_placement_demo( semantics=pan_semantics, grasp_xpos=broadcast_pose_batch(pan_grasp_pose, num_envs=num_envs), ), - binding=ActionBinding( - manipulators={"primary": "right_arm"}, - end_effectors={"primary": "right_hand"}, - ), + binding=right_pick_binding, motion_policy=MotionPolicy(sample_count=PAN_PICK_SAMPLE_INTERVAL), skill_options=right_pick_options, ), @@ -663,8 +664,12 @@ def run_coordinated_placement_demo( if not pick_compiled.plan_success.all(): logger.log_warning("Failed to plan right pan pick-up trajectory.") return - left_pick_traj = left_pick_result.trajectory.positions - right_pick_traj = right_pick_result.trajectory.positions + left_pick_trajectory = left_pick_result.joint_trajectory + right_pick_trajectory = right_pick_result.joint_trajectory + if left_pick_trajectory is None or right_pick_trajectory is None: + raise RuntimeError("PickUp did not produce joint trajectories.") + left_pick_traj = left_pick_trajectory.positions + right_pick_traj = right_pick_trajectory.positions state = pick_compiled.projected_context bread_held_state = state.get_held_object("left_arm") if bread_held_state is None: @@ -690,7 +695,7 @@ def log_trajectory_execution(step_idx: int, total_steps: int) -> None: replay_trajectory( sim, robot, - left_pick_result.trajectory, + left_pick_trajectory, args, video_prefix="", hold_steps=0, @@ -703,7 +708,7 @@ def log_trajectory_execution(step_idx: int, total_steps: int) -> None: replay_trajectory( sim, robot, - right_pick_result.trajectory, + right_pick_trajectory, args, video_prefix="", hold_steps=0, @@ -784,21 +789,19 @@ def log_trajectory_execution(step_idx: int, total_steps: int) -> None: release=True, ) start_time = time.time() + placement_binding = engine.bind_control_parts( + "coordinated_placement", + { + "placing": {"motion": "left_arm", "grasp": "left_hand"}, + "support": {"motion": "right_arm", "grasp": "right_hand"}, + }, + ) placement_compiled = engine.compile( ( ActionInvocation( skill_id="coordinated_placement", goal=coordinated_target, - binding=ActionBinding( - manipulators={ - "placing": "left_arm", - "support": "right_arm", - }, - end_effectors={ - "placing": "left_hand", - "support": "right_hand", - }, - ), + binding=placement_binding, motion_policy=MotionPolicy(sample_count=COORDINATED_SAMPLE_INTERVAL), skill_options=coordinated_options, ), diff --git a/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py b/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py index fc679530d..03ab89f29 100644 --- a/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py +++ b/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py @@ -33,13 +33,14 @@ from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser from embodichain.lab.sim import SimulationManager, VisualMaterialCfg from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, ExecutionEventKind, ExecutionRunner, ExecutionRunnerCfg, + JointPositionPayload, + JointPositionTarget, MotionPolicy, RecoveryPolicy, RigidObjectSceneProvider, @@ -47,6 +48,7 @@ RunnerStep, SimulationExecutionAdapter, TaskState, + TimedCommandSequence, ) from embodichain.lab.sim.cfg import RigidBodyAttributesCfg from embodichain.lab.sim.objects import RigidObject, RigidObjectCfg, Robot @@ -294,21 +296,35 @@ def _minimum_cuboid_clearance( return (outside_distance + inside_distance).amin(dim=1) -def _trajectory_eef_positions( +def _command_eef_positions( robot: Robot, - trajectory_positions: torch.Tensor, + commands: TimedCommandSequence, *, control_part: str, ) -> torch.Tensor: - """Convert a full-robot joint trajectory to batched EEF positions.""" - if trajectory_positions.dim() != 3: - raise ValueError("trajectory_positions must have shape (B, N, robot_dof).") - joint_ids = robot.get_joint_ids(name=control_part) - arm_trajectory = trajectory_positions[:, :, joint_ids] + """Convert one endpoint command sequence to batched EEF positions.""" + if not commands.frames: + raise ValueError("commands must contain at least one frame.") positions = [] - for waypoint_index in range(arm_trajectory.shape[1]): + for frame in commands.frames: + matching_commands = tuple( + command + for command in frame.commands + if isinstance(command.target, JointPositionTarget) + and command.target.control_part == control_part + ) + if len(matching_commands) != 1: + raise ValueError( + f"Expected one joint command for control part {control_part!r}, " + f"got {len(matching_commands)}." + ) + payload = matching_commands[0].payload + if not isinstance(payload, JointPositionPayload): + raise TypeError( + f"Control part {control_part!r} did not receive joint positions." + ) pose = robot.compute_fk( - qpos=arm_trajectory[:, waypoint_index], + qpos=payload.positions, name=control_part, to_matrix=True, ) @@ -457,10 +473,14 @@ def main() -> None: device=target_pose.device, ) engine = AtomicActionEngine(motion_generator=motion_gen) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": CONTROL_PART}}, + ) invocation = ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(target_pose), - binding=ActionBinding(manipulators={"primary": CONTROL_PART}), + binding=binding, motion_policy=MotionPolicy( strategy="motion_gen", sample_count=SAMPLE_COUNT, @@ -475,9 +495,9 @@ def main() -> None: ) task_state = TaskState.empty(robot.get_qpos().shape[0], robot.device) session = engine.start((invocation,), adapter.observe(task_state)) - initial_eef_path = _trajectory_eef_positions( + initial_eef_path = _command_eef_positions( robot, - session.active_trajectory.positions, + session.active_commands, control_part=CONTROL_PART, ) blocking_obstacle_pose, blocking_waypoint_index = _blocking_obstacle_pose( @@ -586,9 +606,9 @@ def on_step(step: RunnerStep) -> None: and replanned_eef_path is None and ExecutionEventKind.COLLISION_WORLD_CHANGED in observed_events ): - replanned_eef_path = _trajectory_eef_positions( + replanned_eef_path = _command_eef_positions( robot, - session.active_trajectory.positions, + session.active_commands, control_part=CONTROL_PART, ) replan_detour = _maximum_path_deviation( diff --git a/scripts/tutorials/atomic_action/hand_over.py b/scripts/tutorials/atomic_action/hand_over.py index bec85110e..042de17be 100644 --- a/scripts/tutorials/atomic_action/hand_over.py +++ b/scripts/tutorials/atomic_action/hand_over.py @@ -35,7 +35,6 @@ from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, GraspGoal, AtomicActionEngine, @@ -258,31 +257,30 @@ def run_handover_demo( # wait for object to drop for _ in range(20): sim.update(step=10) + pick_binding = engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "left_arm", "grasp": "left_hand"}}, + ) + handover_binding = engine.bind_control_parts( + "hand_over", + { + "source": {"motion": "left_arm", "grasp": "left_hand"}, + "destination": {"motion": "right_arm", "grasp": "right_hand"}, + }, + ) compiled = engine.compile( ( ActionInvocation( "pick_up", GraspGoal(object_semantics), - ActionBinding( - manipulators={"primary": "left_arm"}, - end_effectors={"primary": "left_hand"}, - ), + pick_binding, MotionPolicy(sample_count=PICKUP_SAMPLE_INTERVAL), skill_options=pick_up_options, ), ActionInvocation( "hand_over", GraspGoal(object_semantics), - ActionBinding( - manipulators={ - "source": "left_arm", - "destination": "right_arm", - }, - end_effectors={ - "source": "left_hand", - "destination": "right_hand", - }, - ), + handover_binding, MotionPolicy(sample_count=HANDOVER_SAMPLE_INTERVAL), skill_options=handover_options, ), diff --git a/scripts/tutorials/atomic_action/move_end_effector.py b/scripts/tutorials/atomic_action/move_end_effector.py index 6e777a599..ac640fc34 100644 --- a/scripts/tutorials/atomic_action/move_end_effector.py +++ b/scripts/tutorials/atomic_action/move_end_effector.py @@ -29,7 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, @@ -99,7 +98,10 @@ def main() -> None: goal=EndEffectorPoseGoal( broadcast_waypoint_pose_batch(poses, num_envs) ), - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "arm"}}, + ), motion_policy=MotionPolicy(sample_count=MOVE_SAMPLE_INTERVAL), ), ) diff --git a/scripts/tutorials/atomic_action/move_held_object.py b/scripts/tutorials/atomic_action/move_held_object.py index 6b0e1bf24..872fe97a7 100644 --- a/scripts/tutorials/atomic_action/move_held_object.py +++ b/scripts/tutorials/atomic_action/move_held_object.py @@ -30,7 +30,6 @@ from embodichain.data import get_data_path from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -148,22 +147,32 @@ def main() -> None: sim, args, "Inspect the paper cup, then press Enter to plan..." ) - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, + motion_mapping = {"primary": {"motion": "arm"}} + manipulation_mapping = {"primary": {"motion": "arm", "grasp": "hand"}} + move_binding = engine.bind_control_parts( + "move_end_effector", + motion_mapping, + ) + pick_binding = engine.bind_control_parts( + "pick_up", + manipulation_mapping, + ) + held_object_binding = engine.bind_control_parts( + "move_held_object", + manipulation_mapping, ) compiled = engine.compile( ( ActionInvocation( "move_end_effector", EndEffectorPoseGoal(move_target), - binding, + move_binding, MotionPolicy(sample_count=MOVE_SAMPLE_INTERVAL), ), ActionInvocation( "pick_up", GraspGoal(semantics), - binding, + pick_binding, MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( pre_grasp_distance=0.15, @@ -174,7 +183,7 @@ def main() -> None: ActionInvocation( "move_held_object", HeldObjectPoseGoal(object_target), - binding, + held_object_binding, MotionPolicy(sample_count=MOVE_HELD_OBJECT_SAMPLE_INTERVAL), ), ) diff --git a/scripts/tutorials/atomic_action/move_joints.py b/scripts/tutorials/atomic_action/move_joints.py index 7fbee6948..4bc32ea7e 100644 --- a/scripts/tutorials/atomic_action/move_joints.py +++ b/scripts/tutorials/atomic_action/move_joints.py @@ -29,7 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -95,7 +94,10 @@ def main() -> None: waypoints = ( torch.stack([mid, home]).unsqueeze(0).repeat(robot.get_qpos().shape[0], 1, 1) ) - binding = ActionBinding(manipulators={"primary": "arm"}) + binding = engine.bind_control_parts( + "move_joints", + {"primary": {"motion": "arm"}}, + ) policy = MotionPolicy(sample_count=MOVE_JOINTS_SAMPLE_INTERVAL) compiled = engine.compile( ( diff --git a/scripts/tutorials/atomic_action/moving_target_recovery.py b/scripts/tutorials/atomic_action/moving_target_recovery.py index a34efec07..db9da7b13 100644 --- a/scripts/tutorials/atomic_action/moving_target_recovery.py +++ b/scripts/tutorials/atomic_action/moving_target_recovery.py @@ -31,7 +31,6 @@ from embodichain.lab.sim import SimulationManager, VisualMaterialCfg from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, Affordance, AtomicActionEngine, @@ -277,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={ @@ -290,6 +285,10 @@ def main() -> None: ) }, ) + binding = engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "arm", "grasp": "hand"}}, + ) pick_invocation = ActionInvocation( skill_id="pick_up", goal=GraspGoal( diff --git a/scripts/tutorials/atomic_action/pickup.py b/scripts/tutorials/atomic_action/pickup.py index a9c361671..325d36b74 100644 --- a/scripts/tutorials/atomic_action/pickup.py +++ b/scripts/tutorials/atomic_action/pickup.py @@ -29,7 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -158,9 +157,9 @@ def main() -> None: ActionInvocation( skill_id="pick_up", goal=GraspGoal(semantics), - binding=ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, + binding=engine.bind_control_parts( + "pick_up", + {"primary": {"motion": "arm", "grasp": "hand"}}, ), motion_policy=MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( diff --git a/scripts/tutorials/atomic_action/place.py b/scripts/tutorials/atomic_action/place.py index ae30526ad..0b7fc74c1 100644 --- a/scripts/tutorials/atomic_action/place.py +++ b/scripts/tutorials/atomic_action/place.py @@ -29,7 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -156,16 +155,21 @@ def main() -> None: sim, args, "Inspect the cube, then press Enter to plan PickUp -> Place..." ) - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, + endpoint_mapping = {"primary": {"motion": "arm", "grasp": "hand"}} + pick_binding = engine.bind_control_parts( + "pick_up", + endpoint_mapping, + ) + place_binding = engine.bind_control_parts( + "place", + endpoint_mapping, ) compiled = engine.compile( ( ActionInvocation( "pick_up", GraspGoal(semantics), - binding, + pick_binding, MotionPolicy(sample_count=PICK_SAMPLE_INTERVAL), skill_options=PickUpOptions( pre_grasp_distance=0.15, @@ -180,7 +184,7 @@ def main() -> None: place_poses, robot.get_qpos().shape[0] ) ), - binding, + place_binding, MotionPolicy(sample_count=PLACE_SAMPLE_INTERVAL), skill_options=PlaceOptions( lift_height=PLACE_LIFT_HEIGHT, diff --git a/scripts/tutorials/atomic_action/press.py b/scripts/tutorials/atomic_action/press.py index a2e066a9b..63c3d7b6c 100644 --- a/scripts/tutorials/atomic_action/press.py +++ b/scripts/tutorials/atomic_action/press.py @@ -29,7 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, @@ -185,22 +184,26 @@ def main() -> None: sim, args, "Inspect the wooden block, then press Enter to plan..." ) - binding = ActionBinding( - manipulators={"primary": "arm"}, - end_effectors={"primary": "hand"}, + move_binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": "arm"}}, + ) + press_binding = engine.bind_control_parts( + "press", + {"primary": {"motion": "arm", "grasp": "hand"}}, ) compiled = engine.compile( ( ActionInvocation( "move_end_effector", EndEffectorPoseGoal(move_target), - binding, + move_binding, MotionPolicy(sample_count=MOVE_SAMPLE_INTERVAL), ), ActionInvocation( "press", PressGoal(press_target), - binding, + press_binding, MotionPolicy(sample_count=PRESS_SAMPLE_INTERVAL), skill_options=PressOptions( hand_interp_steps=HAND_INTERP_STEPS, diff --git a/tests/sim/atomic_actions/test_actions.py b/tests/sim/atomic_actions/test_actions.py index 06bafe4da..4ad5f22db 100644 --- a/tests/sim/atomic_actions/test_actions.py +++ b/tests/sim/atomic_actions/test_actions.py @@ -27,6 +27,7 @@ from embodichain.lab.sim.atomic_actions import ( ActionBinding, ActionInvocation, + ActionPlan, Affordance, AntipodalAffordance, AssembleAffordance, @@ -49,6 +50,8 @@ HeldObjectPoseGoal, HeldObjectState, JointPositionGoal, + JointPositionPayload, + JointPositionTarget, MotionPolicy, MoveEndEffector, MoveEndEffectorOptions, @@ -70,6 +73,7 @@ SceneEntityPose, SceneSnapshot, TaskState, + TimedTrajectory, ) from embodichain.lab.sim.common import BatchEntity from embodichain.lab.sim.planners import ( @@ -88,6 +92,7 @@ DUAL_ROBOT_DOF = DUAL_ARM_DOF + 2 * HAND_DOF ActionT = TypeVar("ActionT", bound=AtomicAction) +_ACTION_ENGINES: dict[int, AtomicActionEngine] = {} @pytest.fixture(autouse=True) @@ -205,6 +210,7 @@ def _bind_action( load_builtins=False, ) engine.register(action) + _ACTION_ENGINES[id(action)] = engine return action @@ -250,27 +256,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) @@ -385,17 +443,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", + }, }, ) @@ -480,7 +542,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, ), @@ -488,8 +550,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 @@ -514,12 +577,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: @@ -532,7 +596,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) @@ -549,7 +613,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( @@ -589,15 +653,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") @@ -609,7 +674,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, ) @@ -639,7 +704,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), ) @@ -685,16 +750,17 @@ def move_ik( plan = _plan_action( action, - _invocation("move_held_object", HeldObjectPoseGoal(torch.eye(4))), + _invocation(action, HeldObjectPoseGoal(torch.eye(4))), context, ) 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), ) - assert not torch.allclose(plan.trajectory.positions[1], context.robot.qpos[1]) + assert not torch.allclose(trajectory.positions[1], context.robot.qpos[1]) def test_press_uses_invocation_sample_budget() -> None: @@ -703,12 +769,12 @@ def test_press_uses_invocation_sample_budget() -> None: plan = _plan_action( action, - _invocation("press", PressGoal(torch.eye(4)), sample_count=12), + _invocation(action, PressGoal(torch.eye(4)), sample_count=12), _context(), ) assert plan.plan_success.tolist() == [True, True] - assert plan.trajectory.waypoint_count == 12 + assert plan.commands.frame_count == 12 assert plan.expected_effects.is_empty @@ -725,7 +791,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) @@ -736,7 +802,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"): @@ -752,7 +818,7 @@ def test_builtin_action_validates_resolved_request_once() -> None: _plan_action( action, _invocation( - "move_end_effector", + action, EndEffectorPoseGoal(torch.eye(4)), ), _context(), @@ -779,16 +845,19 @@ def test_planner_timing_is_preserved_in_simple_action() -> None: invocation = ActionInvocation( skill_id="move_joints", goal=JointPositionGoal(torch.ones(ARM_DOF)), - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=_binding(action), motion_policy=MotionPolicy(strategy="motion_gen", sample_count=3), ) plan = _plan_action(action, invocation, _context()) - assert plan.trajectory.duration.tolist() == pytest.approx([0.3, 0.3]) - assert plan.trajectory.velocities is not None - assert torch.all(plan.trajectory.velocities[:, :, :ARM_DOF] == 0.5) - assert torch.all(plan.trajectory.velocities[:, :, ARM_DOF:] == 0.0) + trajectory = _joint_trajectory(plan) + payloads = _joint_command_payloads(plan, "arm") + assert trajectory.duration.tolist() == pytest.approx([0.3, 0.3]) + assert all(payload.velocities is not None for payload in payloads) + assert torch.all( + torch.stack([payload.velocities for payload in payloads], dim=1) == 0.5 + ) def test_move_end_effector_visits_batched_waypoints_in_order() -> None: @@ -813,7 +882,7 @@ def compute_ik( plan = _plan_action( action, _invocation( - "move_end_effector", + action, EndEffectorPoseGoal(waypoints), sample_count=9, ), @@ -846,19 +915,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(), ) @@ -888,7 +958,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, ) @@ -939,17 +1009,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] @@ -976,7 +1048,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( @@ -1024,9 +1096,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"), @@ -1061,9 +1135,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), ) @@ -1098,12 +1173,12 @@ def test_press_closes_hand_without_changing_projected_attachment() -> None: plan = _plan_action( action, - _invocation("press", PressGoal(torch.eye(4)), sample_count=12), + _invocation(action, PressGoal(torch.eye(4)), sample_count=12), _context(task), ) projected = plan.expected_effects.apply(task, plan.plan_success) - assert torch.all(plan.trajectory.positions[:, -1, ARM_DOF:] == 1.0) + assert torch.all(_joint_command_positions(plan, "hand")[:, -1] == 1.0) projected_held = projected.get_held_object("arm") assert projected_held is not None assert projected_held.semantics is held.semantics @@ -1171,7 +1246,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), ) @@ -1250,7 +1325,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), ) @@ -1259,11 +1334,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] @@ -1291,7 +1368,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"): @@ -1337,7 +1414,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), ) @@ -1345,8 +1422,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") @@ -1386,7 +1464,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() @@ -1396,13 +1474,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] == [ @@ -1448,7 +1537,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)) @@ -1501,7 +1590,7 @@ def test_assemble_place_uses_explicit_base_snapshot() -> None: request = action.resolve_request( _invocation( - "place", + action, AssembleGoal( affordance=affordance, base_pose=SceneEntityPose("base"), @@ -1528,7 +1617,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)) @@ -1582,7 +1671,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() @@ -1591,11 +1680,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 @@ -1644,14 +1735,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: @@ -1683,7 +1779,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) @@ -1692,7 +1788,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 @@ -1722,14 +1824,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() @@ -1783,7 +1885,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), ) @@ -1791,11 +1893,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] @@ -1821,7 +1925,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, ) @@ -1835,7 +1939,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 5d4a133dd..58722e800 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 ( @@ -97,13 +106,112 @@ def _context(scene: SceneSnapshot | None = None) -> PlanningContext: ) +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: @@ -133,18 +241,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]) @@ -514,27 +686,447 @@ 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() - plan = action.build_plan( + plan = action.build_command_plan( request, context, success=True, - trajectory=context.robot.qpos.unsqueeze(1), + commands=TimedCommandSequence(frames=(), env_ids=context.env_ids), diagnostics=PlannerDiagnostics(backend="test"), ) assert plan.scene_dependencies == ("extra", "tracked") +def test_build_command_plan_rejects_unbound_runtime_destination() -> None: + context = _context() + generator = Mock() + generator.robot = Mock() + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "test" + engine = AtomicActionEngine(generator, load_builtins=False) + action = _DependencyAction() + engine.register(action) + request = ResolvedActionRequest( + skill_id="dependency_test", + goal=EndEffectorPoseGoal(SceneEntityPose("tracked")), + binding=ActionBinding(owner_id=engine.binding_owner_id), + motion_policy=MotionPolicy(), + recovery_policy=RecoveryPolicy(), + skill_options=ActionOptions(), + ) + + with pytest.raises(ValueError, match="not authorized"): + action.build_command_plan( + request, + context, + success=True, + commands=_command_sequence(env_ids=context.env_ids, frame_count=1), + diagnostics=PlannerDiagnostics(backend="test"), + ) + + +def test_public_plan_authorizes_raw_action_plan_destinations() -> None: + context = _context() + generator = Mock() + generator.robot = Mock() + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "test" + engine = AtomicActionEngine(generator, load_builtins=False) + action = _RawCommandAction() + engine.register(action) + request = ResolvedActionRequest( + skill_id=action.skill_id, + goal=EndEffectorPoseGoal(SceneEntityPose("tracked")), + binding=ActionBinding(owner_id=engine.binding_owner_id), + motion_policy=MotionPolicy(), + recovery_policy=RecoveryPolicy(), + skill_options=ActionOptions(), + ) + + with pytest.raises(ValueError, match="not authorized"): + action.plan(request, context) + + +def test_command_target_authorization_rejects_altered_joint_claims() -> None: + context = _context() + request = ResolvedActionRequest( + skill_id="dependency_test", + goal=EndEffectorPoseGoal(SceneEntityPose("tracked")), + binding=ActionBinding( + owner_id="test-engine", + endpoints=( + EndpointBinding( + slot_id="primary", + endpoint_id="motion", + resource_id="arm", + adapter_id="control_part", + target=JointPositionTarget("arm", (0, 1)), + ), + ), + ), + motion_policy=MotionPolicy(), + recovery_policy=RecoveryPolicy(), + skill_options=ActionOptions(), + ) + frame = RuntimeCommandFrame( + commands=( + EndpointCommand( + target=JointPositionTarget("arm", (2, 3)), + payload=JointPositionPayload(torch.ones(2, 2)), + ), + ), + active_mask=torch.ones(2, dtype=torch.bool), + env_ids=context.env_ids, + hold_duration=torch.full((2,), 0.1), + ) + + with pytest.raises(ValueError, match="bound joint-position target"): + _DependencyAction._authorize_command_targets( + request, + TimedCommandSequence(frames=(frame,), env_ids=context.env_ids), + ) + + +def test_command_target_authorization_rejects_custom_claim_conflicts() -> None: + context = _context() + endpoints = tuple( + EndpointBinding( + slot_id="primary", + endpoint_id=name, + resource_id=name, + adapter_id="test.claimed", + target=_ClaimedTarget(name), + claim_tokens=frozenset({"controller:shared"}), + ) + for name in ("first", "second") + ) + request = ResolvedActionRequest( + skill_id="dependency_test", + goal=EndEffectorPoseGoal(SceneEntityPose("tracked")), + binding=ActionBinding(owner_id="test-engine", endpoints=endpoints), + motion_policy=MotionPolicy(), + recovery_policy=RecoveryPolicy(), + skill_options=ActionOptions(), + ) + frame = RuntimeCommandFrame( + commands=tuple( + EndpointCommand( + target=endpoint.target, + payload=JointPositionPayload(torch.ones(2, 1)), + ) + for endpoint in endpoints + ), + active_mask=torch.ones(2, dtype=torch.bool), + env_ids=context.env_ids, + hold_duration=torch.full((2,), 0.1), + ) + + with pytest.raises(ValueError, match="claim tokens.*controller:shared"): + _DependencyAction._authorize_command_targets( + request, + TimedCommandSequence(frames=(frame,), env_ids=context.env_ids), + ) + + +def test_action_plan_owns_commands_and_optional_joint_trajectory() -> None: + env_ids = torch.tensor([4, 7], dtype=torch.long) + commands = _command_sequence(env_ids=env_ids, frame_count=2) + trajectory_positions = torch.stack( + ( + torch.full((2, 2), 1.0), + torch.full((2, 2), 2.0), + ), + dim=1, + ) + trajectory = TimedTrajectory.from_positions( + trajectory_positions, + env_ids=env_ids, + control_dt=0.1, + ) + plan_success = torch.tensor([True, False]) + + plan = _action_plan( + commands, + plan_success=plan_success, + joint_trajectory=trajectory, + feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, + ) + payload = commands.frames[0].commands[0].payload + assert isinstance(payload, JointPositionPayload) + plan_success.zero_() + payload.positions.zero_() + commands.frames[0].active_mask.zero_() + commands.frames[0].hold_duration.zero_() + commands.env_ids.zero_() + trajectory.positions.zero_() + + owned_payload = plan.commands.frames[0].commands[0].payload + assert isinstance(owned_payload, JointPositionPayload) + assert plan.plan_success.tolist() == [True, False] + assert torch.all(owned_payload.positions == 1.0) + assert plan.commands.frames[0].active_mask.tolist() == [True, True] + assert torch.all(plan.commands.frames[0].hold_duration == 0.1) + assert plan.commands.env_ids.tolist() == [4, 7] + assert plan.joint_trajectory is not None + assert torch.equal(plan.joint_trajectory.positions, trajectory_positions) + + +def test_action_plan_allows_timed_commands_without_joint_trajectory() -> None: + commands = _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, + ) + + plan = _action_plan(commands) + + assert plan.commands.frame_count == 1 + assert plan.joint_trajectory is None + assert plan.feedback_mode is ExecutionFeedbackMode.TIMED + + +def test_action_plan_rejects_command_device_mismatch() -> None: + commands = _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, + ) + + with pytest.raises(ValueError, match="share a device"): + _action_plan( + commands, + plan_success=torch.ones(1, dtype=torch.bool, device="meta"), + ) + + +@pytest.mark.parametrize( + ("trajectory_env_ids", "trajectory_frame_count", "message"), + [ + (torch.tensor([7], dtype=torch.long), 1, "env_ids must match"), + (torch.tensor([4], dtype=torch.long), 2, "waypoints must match"), + ], +) +def test_action_plan_validates_joint_trajectory_against_commands( + trajectory_env_ids: torch.Tensor, + trajectory_frame_count: int, + message: str, +) -> None: + commands = _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, + ) + trajectory = TimedTrajectory.from_positions( + torch.ones(1, trajectory_frame_count, 2), + env_ids=trajectory_env_ids, + control_dt=0.1, + ) + + with pytest.raises(ValueError, match=message): + _action_plan( + commands, + joint_trajectory=trajectory, + feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, + ) + + +def test_joint_position_plan_rejects_empty_commands_for_successful_rows() -> None: + env_ids = torch.tensor([4], dtype=torch.long) + commands = TimedCommandSequence(frames=(), env_ids=env_ids) + trajectory = TimedTrajectory.empty( + batch_size=1, + robot_dof=2, + device=env_ids.device, + env_ids=env_ids, + ) + + with pytest.raises(ValueError, match="requires command frames"): + _action_plan( + commands, + plan_success=torch.tensor([True]), + joint_trajectory=trajectory, + feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, + ) + + +def test_joint_position_plan_allows_empty_commands_when_all_rows_fail() -> None: + env_ids = torch.tensor([4], dtype=torch.long) + commands = TimedCommandSequence(frames=(), env_ids=env_ids) + trajectory = TimedTrajectory.empty( + batch_size=1, + robot_dof=2, + device=env_ids.device, + env_ids=env_ids, + ) + + plan = _action_plan( + commands, + plan_success=torch.tensor([False]), + joint_trajectory=trajectory, + feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, + ) + + assert plan.commands.frame_count == 0 + + +@pytest.mark.parametrize( + "feedback_mode", + [ExecutionFeedbackMode.TIMED, ExecutionFeedbackMode.JOINT_POSITION], +) +def test_action_plan_requires_stable_destination_set( + feedback_mode: ExecutionFeedbackMode, +) -> None: + env_ids = torch.tensor([4], dtype=torch.long) + commands = _command_sequence( + env_ids=env_ids, + frame_count=2, + targets=( + JointPositionTarget("arm", (0, 1)), + JointPositionTarget("other_arm", (0, 1)), + ), + ) + trajectory = ( + TimedTrajectory.from_positions( + torch.tensor([[[1.0, 1.0], [2.0, 2.0]]]), + env_ids=env_ids, + control_dt=0.1, + ) + if feedback_mode is ExecutionFeedbackMode.JOINT_POSITION + else None + ) + + with pytest.raises(ValueError, match="same destination set"): + _action_plan( + commands, + joint_trajectory=trajectory, + feedback_mode=feedback_mode, + ) + + +def test_action_plan_requires_stable_exact_target_type() -> None: + env_ids = torch.tensor([4], dtype=torch.long) + commands = _command_sequence( + env_ids=env_ids, + frame_count=2, + targets=( + JointPositionTarget("arm", (0, 1)), + _AlternateJointPositionTarget("arm", (0, 1)), + ), + ) + + with pytest.raises(ValueError, match="exact target type"): + _action_plan(commands) + + +def test_action_plan_requires_stable_target_address_fingerprint() -> None: + env_ids = torch.tensor([4], dtype=torch.long) + commands = _command_sequence( + env_ids=env_ids, + frame_count=2, + targets=( + JointPositionTarget("arm", (0, 1)), + JointPositionTarget("arm", (1, 0)), + ), + ) + + with pytest.raises(ValueError, match="target address fingerprint"): + _action_plan(commands) + + +def test_joint_position_plan_rejects_joint_ids_outside_trajectory() -> None: + env_ids = torch.tensor([4], dtype=torch.long) + commands = _command_sequence( + env_ids=env_ids, + frame_count=1, + targets=(JointPositionTarget("arm", (0, 2)),), + ) + trajectory = TimedTrajectory.from_positions( + torch.ones(1, 1, 2), + env_ids=env_ids, + control_dt=0.1, + ) + + with pytest.raises(ValueError, match="outside joint_trajectory robot_dof"): + _action_plan( + commands, + joint_trajectory=trajectory, + feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, + ) + + +def test_joint_position_plan_rejects_payload_position_mismatch() -> None: + env_ids = torch.tensor([4], dtype=torch.long) + commands = _command_sequence(env_ids=env_ids, frame_count=1) + trajectory = TimedTrajectory.from_positions( + torch.zeros(1, 1, 2), + env_ids=env_ids, + control_dt=0.1, + ) + + with pytest.raises(ValueError, match="positions.*exactly match"): + _action_plan( + commands, + joint_trajectory=trajectory, + feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, + ) + + +def test_joint_position_plan_rejects_payload_velocity_presence_mismatch() -> None: + env_ids = torch.tensor([4], dtype=torch.long) + commands = _command_sequence( + env_ids=env_ids, + frame_count=1, + velocities=(torch.zeros(1, 2),), + ) + trajectory = TimedTrajectory.from_positions( + torch.ones(1, 1, 2), + env_ids=env_ids, + control_dt=0.1, + ) + + with pytest.raises(ValueError, match="same presence"): + _action_plan( + commands, + joint_trajectory=trajectory, + feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, + ) + + +def test_joint_position_plan_rejects_payload_velocity_value_mismatch() -> None: + env_ids = torch.tensor([4], dtype=torch.long) + commands = _command_sequence( + env_ids=env_ids, + frame_count=1, + velocities=(torch.zeros(1, 2),), + ) + trajectory = TimedTrajectory.from_positions( + torch.ones(1, 1, 2), + velocities=torch.ones(1, 1, 2), + env_ids=env_ids, + control_dt=0.1, + ) + + with pytest.raises(ValueError, match="velocities.*exactly match"): + _action_plan( + commands, + joint_trajectory=trajectory, + feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, + ) + + def test_scene_snapshot_expands_global_collision_world_revision() -> None: pose = torch.eye(4).repeat(2, 1, 1) snapshot = SceneSnapshot( @@ -628,6 +1220,42 @@ def test_timed_trajectory_synthesizes_timing_and_holds_selected_rows() -> None: assert torch.all(held.positions[1] == -1.0) +def test_timed_trajectory_constructor_detaches_and_owns_all_tensor_fields() -> None: + positions = torch.tensor([[[1.0, 2.0], [3.0, 4.0]]], requires_grad=True) + velocities = torch.full_like(positions, 0.5, requires_grad=True) + accelerations = torch.full_like(positions, 0.25, requires_grad=True) + dt = torch.tensor([[0.0, 0.1]], requires_grad=True) + env_ids = torch.tensor([4], dtype=torch.long) + inputs = { + "positions": positions, + "velocities": velocities, + "accelerations": accelerations, + "dt": dt, + "env_ids": env_ids, + } + expected = {name: value.detach().clone() for name, value in inputs.items()} + + trajectory = TimedTrajectory(**inputs) + + with torch.no_grad(): + for value in inputs.values(): + value.zero_() + for name, value in expected.items(): + owned = getattr(trajectory, name) + assert torch.equal(owned, value) + assert owned.grad_fn is None + assert not owned.requires_grad + + +def test_timed_trajectory_rejects_duplicate_environment_ids() -> None: + with pytest.raises(ValueError, match="unique"): + TimedTrajectory.from_positions( + torch.zeros(2, 1, 2), + env_ids=torch.tensor([4, 4], dtype=torch.long), + control_dt=0.1, + ) + + def test_timed_trajectory_snapshot_owns_its_tensor_storage() -> None: trajectory = TimedTrajectory.from_positions( torch.arange(12, dtype=torch.float32).reshape(1, 3, 4), diff --git a/tests/sim/atomic_actions/test_curobo_motion_strategy_e2e.py b/tests/sim/atomic_actions/test_curobo_motion_strategy_e2e.py index da4e87a41..7ea946be9 100644 --- a/tests/sim/atomic_actions/test_curobo_motion_strategy_e2e.py +++ b/tests/sim/atomic_actions/test_curobo_motion_strategy_e2e.py @@ -46,7 +46,6 @@ CuroboWorldCfg, ) from embodichain.lab.sim.atomic_actions import ( # noqa: E402 - ActionBinding, ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, @@ -128,12 +127,16 @@ def test_atomic_move_end_effector_uses_curobo_v2(): sim, robot, engine = _make_franka_curobo_engine() try: target = _reachable_target_beyond_demo_block(robot) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": CONTROL_PART}}, + ) result = engine.compile( ( ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(xpos=target), - binding=ActionBinding(manipulators={"primary": CONTROL_PART}), + binding=binding, motion_policy=MotionPolicy( strategy="motion_gen", sample_count=SAMPLE_INTERVAL, @@ -141,7 +144,10 @@ def test_atomic_move_end_effector_uses_curobo_v2(): ), ) ) - trajectory = result.trajectory.positions + plan = result.action_plans[0] + assert plan.joint_trajectory is not None + assert plan.commands.frame_count == plan.joint_trajectory.waypoint_count + trajectory = plan.joint_trajectory.positions assert result.plan_success.shape == (1,) assert bool(result.plan_success.item()) assert trajectory.shape[2] == robot.dof diff --git a/tests/sim/atomic_actions/test_endpoint_runtime_e2e.py b/tests/sim/atomic_actions/test_endpoint_runtime_e2e.py new file mode 100644 index 000000000..7c6dd3688 --- /dev/null +++ b/tests/sim/atomic_actions/test_endpoint_runtime_e2e.py @@ -0,0 +1,535 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""End-to-end coverage for generic atomic-action runtime endpoints.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import ClassVar +from unittest.mock import Mock + +import torch + +from embodichain.lab.sim.atomic_actions import ( + ActionInvocation, + ActionOptions, + ActionPlan, + AtomicAction, + AtomicActionEngine, + CommandAcknowledgement, + EndpointCommand, + EndpointCommandRouter, + ExecutionRunner, + ExecutionStatus, + JOINT_POSITION_CAPABILITY, + JointPositionGoal, + JointPositionPayload, + JointPositionTarget, + MoveJoints, + PlanningContext, + RobotObservation, + RunnerStatus, + RuntimeCommandFrame, + RuntimeCommandPayload, + RuntimeEndpointTarget, + SceneSnapshot, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, + TaskState, + TimedCommandSequence, +) +from embodichain.lab.sim.atomic_actions.invocation import ResolvedActionRequest +from embodichain.lab.sim.planners import PlanResult +from embodichain.lab.sim.skills import ( + EndpointResolution, + ResourceBinding, + ResourceEndpoint, + ResourceEndpointAdapter, + RobotResource, + RobotSkillProfile, +) + + +class _Clock: + """Deterministic clock used by the runner.""" + + def __init__(self) -> None: + self.value = 0.0 + + def now(self) -> float: + """Return simulated time.""" + return self.value + + def sleep(self, duration: float) -> None: + """Advance simulated time.""" + self.value += duration + + +class _Robot: + """Small stateful robot with one whole-body control part.""" + + def __init__(self) -> None: + self.device = torch.device("cpu") + self.dof = 4 + self.control_parts = {"whole_body": object()} + self.qpos = torch.zeros(2, self.dof) + + def get_qpos(self, name: str | None = None) -> torch.Tensor: + """Return observed joint positions.""" + if name is not None and name != "whole_body": + raise KeyError(name) + return self.qpos.clone() + + def get_qvel(self, name: str | None = None) -> torch.Tensor: + """Return zero joint velocities.""" + return torch.zeros_like(self.get_qpos(name)) + + def get_joint_ids(self, name: str) -> list[int]: + """Resolve the whole-body control part.""" + if name != "whole_body": + raise KeyError(name) + return list(range(self.dof)) + + +class _Provider: + """Observe the stateful robot at the injected clock time.""" + + def __init__(self, robot: _Robot, clock: _Clock) -> None: + self.robot = robot + self.clock = clock + self.env_ids = torch.tensor([3, 7], dtype=torch.long) + + def observe(self, task_state: TaskState) -> PlanningContext: + """Return one fresh, correlated planning context.""" + qpos = self.robot.get_qpos() + timestamp = self.clock.now() + return PlanningContext( + robot=RobotObservation( + timestamp=timestamp, + qpos=qpos, + qvel=torch.zeros_like(qpos), + ), + task=task_state, + scene=SceneSnapshot(timestamp=timestamp, version=0), + env_ids=self.env_ids, + ) + + +def _engine(robot: _Robot) -> AtomicActionEngine: + """Build a core engine around a controllable planning stub.""" + generator = Mock() + generator.robot = robot + generator.device = robot.device + generator.planner.cfg.planner_type = "stub" + + def generate(states: list[object], *, options: object) -> PlanResult: + target = states[-1].qpos + assert isinstance(target, torch.Tensor) + start = options.start_qpos + assert isinstance(start, torch.Tensor) + positions = torch.stack((start, target), dim=1) + dt = torch.zeros(positions.shape[:2], dtype=torch.float32) + dt[:, 1] = 0.01 + return PlanResult( + success=torch.ones(positions.shape[0], dtype=torch.bool), + positions=positions, + dt=dt, + duration=dt.sum(dim=1), + ) + + generator.generate.side_effect = generate + return AtomicActionEngine(generator, load_builtins=False) + + +class _JointTransport: + """Apply joint endpoint payloads to the stateful test robot.""" + + transport_id = JointPositionTarget.TRANSPORT_ID + payload_type = JointPositionPayload + + def __init__(self, robot: _Robot) -> None: + self.robot = robot + self.sent: list[RuntimeCommandFrame] = [] + self.held: list[tuple[RuntimeEndpointTarget, ...]] = [] + + def send( + self, + frame: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Apply each addressed joint subset.""" + del timeout + self.sent.append(frame.snapshot()) + for command in frame.commands: + assert isinstance(command.target, JointPositionTarget) + assert isinstance(command.payload, JointPositionPayload) + joint_ids = list(command.target.joint_ids) + self.robot.qpos[:, joint_ids] = torch.where( + frame.active_mask[:, None], + command.payload.positions, + self.robot.qpos[:, joint_ids], + ) + return CommandAcknowledgement.accepted_ack() + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Hold only the joint subsets addressed by the runner.""" + del timeout + self.held.append(tuple(target.snapshot() for target in targets)) + for target in targets: + assert isinstance(target, JointPositionTarget) + joint_ids = list(target.joint_ids) + self.robot.qpos[:, joint_ids] = context.robot.qpos[:, joint_ids] + return CommandAcknowledgement.accepted_ack() + + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + """Acknowledge synchronous cancellation.""" + del targets, timeout + return CommandAcknowledgement.accepted_ack() + + +def test_whole_body_joint_endpoint_executes_without_arm_or_tool_roles() -> None: + robot = _Robot() + engine = _engine(robot) + engine.register(MoveJoints()) + binding = engine.bind_control_parts( + "move_joints", + {"primary": {"motion": "whole_body"}}, + ) + invocation = ActionInvocation( + skill_id="move_joints", + goal=JointPositionGoal(torch.full((2, robot.dof), 0.5)), + binding=binding, + ) + clock = _Clock() + provider = _Provider(robot, clock) + context = provider.observe(TaskState.empty(batch_size=2, device="cpu")) + + plan = engine.plan(invocation, context) + target = binding.endpoint("primary", "motion").require_target(JointPositionTarget) + assert target.control_part == "whole_body" + assert plan.joint_trajectory is not None + assert plan.commands.targets[0].target_id == "whole_body" + + transport = _JointTransport(robot) + runner = ExecutionRunner( + engine.start((invocation,), context), + provider, + EndpointCommandRouter((transport,)), + clock=clock, + ) + result = runner.run_until_blocked() + + assert result.status is RunnerStatus.COMPLETED + assert result.tick is not None + assert result.tick.status is ExecutionStatus.COMPLETED + assert len(transport.sent) == 2 + assert len(transport.held) == 1 + assert transport.held[0][0].target_id == "whole_body" + assert torch.allclose(robot.qpos, torch.full((2, robot.dof), 0.5)) + + +@dataclass(frozen=True, slots=True) +class _PlanarVelocityTarget(RuntimeEndpointTarget): + """Address one planar velocity controller.""" + + controller_id: str + + @property + def transport_id(self) -> str: + """Return the custom transport identifier.""" + return "test.planar_velocity" + + @property + def target_id(self) -> str: + """Return the controller-local target identifier.""" + return self.controller_id + + +@dataclass(frozen=True, slots=True, eq=False) +class _PlanarVelocityPayload(RuntimeCommandPayload): + """Batched ``(vx, vy, yaw_rate)`` commands.""" + + twist: torch.Tensor + + def __post_init__(self) -> None: + if not isinstance(self.twist, torch.Tensor) or self.twist.dim() != 2: + raise ValueError("twist must have shape (batch_size, 3).") + if self.twist.shape[0] < 1 or self.twist.shape[1] != 3: + raise ValueError("twist must have shape (batch_size, 3).") + object.__setattr__(self, "twist", self.twist.clone()) + + @property + def batch_size(self) -> int: + """Return the number of environment rows.""" + return int(self.twist.shape[0]) + + @property + def device(self) -> torch.device: + """Return the payload device.""" + return self.twist.device + + @property + def transport_id(self) -> str: + """Return the custom transport identifier.""" + return "test.planar_velocity" + + def snapshot(self) -> _PlanarVelocityPayload: + """Return an independently owned payload.""" + return _PlanarVelocityPayload(self.twist) + + +@dataclass(frozen=True, slots=True) +class _PlanarVelocityEndpoint(ResourceEndpoint): + """Profile declaration for a planar velocity controller.""" + + controller_id: str + + +class _PlanarVelocityAdapter(ResourceEndpointAdapter): + """Resolve the custom profile endpoint to a runtime target.""" + + adapter_id: ClassVar[str] = "test.planar_velocity" + endpoint_type: ClassVar[type[ResourceEndpoint]] = _PlanarVelocityEndpoint + + def resolve( + self, + endpoint: ResourceEndpoint, + *, + engine: AtomicActionEngine, + ) -> EndpointResolution: + """Resolve immutable addressing and an exclusive controller claim.""" + del engine + assert isinstance(endpoint, _PlanarVelocityEndpoint) + return EndpointResolution( + runtime_target=_PlanarVelocityTarget(endpoint.controller_id), + claim_tokens=frozenset({f"controller:{endpoint.controller_id}"}), + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class _DriveGoal: + """Planar velocity command used by the custom atomic action.""" + + goal_kind: ClassVar[str] = "planar_velocity" + twist: torch.Tensor + + +class _DriveVelocity(AtomicAction[_DriveGoal, ActionOptions]): + """Custom action proving non-joint commands cross the full runtime.""" + + skill_id: ClassVar[str] = "drive_velocity" + GoalType: ClassVar[type] = _DriveGoal + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + "body", + endpoints=( + SkillEndpointRequirement( + "motion", + capabilities=frozenset({"motion.base.planar_velocity"}), + ), + ), + ), + ) + ) + + def _plan( + self, + request: ResolvedActionRequest[_DriveGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + """Emit one drive frame followed by an explicit zero-velocity frame.""" + goal = self.require_goal(request) + target = request.binding.endpoint("body", "motion").require_target( + _PlanarVelocityTarget + ) + active = torch.ones(context.batch_size, dtype=torch.bool, device=self.device) + frames = tuple( + RuntimeCommandFrame( + commands=( + EndpointCommand( + target=target, + payload=_PlanarVelocityPayload(twist), + ), + ), + active_mask=active, + env_ids=context.env_ids, + hold_duration=torch.full( + (context.batch_size,), + duration, + dtype=torch.float32, + device=self.device, + ), + ) + for twist, duration in ( + (goal.twist.to(self.device), 0.02), + (torch.zeros_like(goal.twist, device=self.device), 0.0), + ) + ) + return self.build_command_plan( + request, + context, + success=True, + commands=TimedCommandSequence(frames, context.env_ids), + segment_lengths={"drive": 1, "stop": 1}, + ) + + +class _PlanarVelocityTransport: + """Record velocity frames and own the zero-velocity safe state.""" + + transport_id = "test.planar_velocity" + payload_type = _PlanarVelocityPayload + + def __init__(self) -> None: + self.sent: list[torch.Tensor] = [] + self.hold_targets: tuple[RuntimeEndpointTarget, ...] = () + self.last_twist: torch.Tensor | None = None + + def send( + self, + frame: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Record active twists and neutralize every inactive row.""" + del timeout + payload = frame.commands[0].payload + assert isinstance(payload, _PlanarVelocityPayload) + self.last_twist = torch.where( + frame.active_mask[:, None], + payload.twist, + torch.zeros_like(payload.twist), + ) + self.sent.append(self.last_twist) + return CommandAcknowledgement.accepted_ack() + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Apply the velocity transport's safe zero command.""" + del context, timeout + self.hold_targets = tuple(target.snapshot() for target in targets) + assert self.last_twist is not None + self.last_twist = torch.zeros_like(self.last_twist) + return CommandAcknowledgement.accepted_ack() + + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + """Acknowledge cancellation.""" + del targets, timeout + return CommandAcknowledgement.accepted_ack() + + +def test_custom_planar_velocity_endpoint_runs_from_profile_through_router() -> None: + robot = _Robot() + engine = _engine(robot) + engine.register(_DriveVelocity()) + profile = RobotSkillProfile( + profile_id="mobile", + resources={ + "mobile_base": RobotResource( + "mobile_base", + endpoints={ + "motion": _PlanarVelocityEndpoint( + "base_controller", + capabilities=frozenset({"motion.base.planar_velocity"}), + ) + }, + ) + }, + defaults={"drive_velocity": ResourceBinding({"body": "mobile_base"})}, + ) + bound = engine.bind_skill_profile( + profile, + endpoint_adapters={_PlanarVelocityEndpoint: _PlanarVelocityAdapter()}, + ) + binding = bound.resolve("drive_velocity").action_binding + goal_twist = torch.tensor([[0.5, 0.0, 0.1], [0.2, 0.0, -0.1]]) + invocation = ActionInvocation( + skill_id="drive_velocity", + goal=_DriveGoal(goal_twist), + binding=binding, + ) + clock = _Clock() + provider = _Provider(robot, clock) + context = provider.observe(TaskState.empty(batch_size=2, device="cpu")) + + plan = engine.plan(invocation, context) + assert plan.joint_trajectory is None + assert plan.segment("drive").waypoint_count == 1 + assert plan.commands.targets[0].transport_id == "test.planar_velocity" + + transport = _PlanarVelocityTransport() + runner = ExecutionRunner( + engine.start((invocation,), context), + provider, + EndpointCommandRouter((transport,)), + clock=clock, + ) + result = runner.run_until_blocked() + + assert result.status is RunnerStatus.COMPLETED + assert len(transport.sent) == 2 + assert torch.allclose(transport.sent[0], goal_twist) + assert torch.count_nonzero(transport.sent[1]) == 0 + assert transport.last_twist is not None + assert torch.count_nonzero(transport.last_twist) == 0 + assert transport.hold_targets[0].target_id == "base_controller" + + +def test_planar_velocity_transport_neutralizes_inactive_rows() -> None: + transport = _PlanarVelocityTransport() + frame = RuntimeCommandFrame( + commands=( + EndpointCommand( + target=_PlanarVelocityTarget("base_controller"), + payload=_PlanarVelocityPayload(torch.ones(2, 3)), + ), + ), + active_mask=torch.tensor([True, False]), + env_ids=torch.tensor([0, 1]), + hold_duration=torch.zeros(2), + ) + + acknowledgement = transport.send(frame, timeout=1.0) + + assert acknowledgement.accepted + assert transport.last_twist is not None + assert torch.equal(transport.last_twist[0], torch.ones(3)) + assert torch.count_nonzero(transport.last_twist[1]) == 0 diff --git a/tests/sim/atomic_actions/test_engine.py b/tests/sim/atomic_actions/test_engine.py index 8704c357a..2f577b142 100644 --- a/tests/sim/atomic_actions/test_engine.py +++ b/tests/sim/atomic_actions/test_engine.py @@ -37,11 +37,16 @@ ControlPartCommandProfile, JointPositionCommand, JointPositionGoal, + JointPositionTarget, + JOINT_POSITION_CAPABILITY, MotionPolicy, PlanningContext, PressGoal, PressOptions, ResolvedActionRequest, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, ) @@ -50,7 +55,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, @@ -116,12 +133,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), ) @@ -166,14 +187,24 @@ def test_engine_can_disable_builtin_loading() -> None: def test_auto_registered_builtin_accepts_per_invocation_options() -> None: - engine = _engine(load_builtins=True) + generator = _motion_generator(robot_dof=3) + generator.robot.control_parts = {"arm": object(), "hand": object()} + generator.robot.get_joint_ids.side_effect = lambda name: ( + [0, 1] if name == "arm" else [2] + ) + engine = AtomicActionEngine( + generator, + control_profiles={ + "hand": ControlPartCommandProfile.joint_positions(grasp=torch.ones(1)) + }, + ) options = PressOptions(hand_interp_steps=7) invocation = ActionInvocation( skill_id="press", goal=PressGoal(torch.eye(4)), - binding=ActionBinding( - manipulators={"primary": "all"}, - end_effectors={"primary": "all"}, + binding=engine.bind_control_parts( + "press", + {"primary": {"motion": "arm", "grasp": "hand"}}, ), motion_policy=MotionPolicy(sample_count=20), skill_options=options, @@ -191,11 +222,13 @@ def test_engine_compile_projects_terminal_state_between_actions() -> None: first = torch.ones(2, 3) second = torch.full((2, 3), 2.0) - compiled = engine.compile((_invocation(first), _invocation(second))) + compiled = engine.compile((_invocation(engine, first), _invocation(engine, second))) assert compiled.plan_success.tolist() == [True, True] assert compiled.trajectory.positions.shape == (2, 4, 3) - assert torch.equal(compiled.action_plans[1].trajectory.positions[:, 0], first) + second_trajectory = compiled.action_plans[1].joint_trajectory + assert second_trajectory is not None + assert torch.equal(second_trajectory.positions[:, 0], first) assert torch.equal(compiled.projected_context.robot.qpos, second) assert torch.count_nonzero(engine.robot.get_qpos()) == 0 assert compiled.action_waypoint_offset(1) == 2 @@ -209,12 +242,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) @@ -231,8 +266,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: @@ -272,14 +313,16 @@ def test_engine_binds_one_planning_service_to_every_action() -> None: def test_engine_resolves_action_binding_from_robot_control_parts() -> None: engine = _engine(robot_dof=3) + engine.register(StubAction()) - resolved = engine.planning_services.resolve_binding( - ActionBinding(manipulators={"primary": "all"}) + resolved = engine.bind_control_parts( + "stub", + {"primary": {"motion": "all"}}, ) + target = resolved.endpoint("primary", "motion").require_target(JointPositionTarget) - assert resolved.manipulator().name == "all" - assert resolved.manipulator().joint_ids == (0, 1, 2) - assert resolved.manipulator().dof == 3 + assert target.control_part == "all" + assert target.joint_ids == (0, 1, 2) def test_engine_resolves_invocation_control_override_into_request() -> None: @@ -291,10 +334,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, @@ -304,7 +349,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), @@ -314,15 +359,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: @@ -332,6 +374,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) @@ -350,9 +429,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: @@ -370,4 +451,4 @@ def wrong_skill_plan( engine.register(action) with pytest.raises(ValueError, match="must match its request"): - engine.compile((_invocation(torch.zeros(2, 3)),)) + engine.compile((_invocation(engine, torch.zeros(2, 3)),)) diff --git a/tests/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index 1458b7367..7243013fd 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -36,22 +36,30 @@ AtomicActionEngine, DynamicCollisionMode, EndEffectorPoseGoal, + EndpointBinding, + EndpointCommand, EntityState, ExecutionEventKind, ExecutionStatus, GraspGoal, HeldObjectState, + JointPositionPayload, + JointPositionTarget, MotionPolicy, ObjectSemantics, PlanningContext, RecoveryPolicy, - ResolvedActionBinding, ResolvedActionRequest, RobotObservation, + RuntimeCommandFrame, SceneEntityPose, SceneSnapshot, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, StateDelta, TaskState, + TimedCommandSequence, TimedTrajectory, ) from embodichain.lab.sim.common import BatchEntity @@ -64,7 +72,14 @@ class DynamicAction(AtomicAction[EndEffectorPoseGoal, ActionOptions]): skill_id: ClassVar[str] = "dynamic" GoalType: ClassVar[type] = EndEffectorPoseGoal - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=(SkillEndpointRequirement(endpoint_id="motion"),), + ), + ) + ) def __init__(self) -> None: super().__init__() @@ -93,6 +108,7 @@ class EffectAction(DynamicAction): """Dynamic test action that declares an attachment effect.""" skill_id: ClassVar[str] = "effect" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract def _plan( self, @@ -123,6 +139,7 @@ class FailedEffectAction(EffectAction): """Effect-declaring action whose planner fails for every environment.""" skill_id: ClassVar[str] = "failed_effect" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract def _plan( self, @@ -137,6 +154,7 @@ class NonuniformTimingAction(DynamicAction): """Test action with explicit nonuniform waypoint arrival intervals.""" skill_id: ClassVar[str] = "nonuniform_timing" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract def _plan( self, @@ -168,6 +186,82 @@ def _plan( ) +class DestinationSequenceAction(AtomicAction[EndEffectorPoseGoal, ActionOptions]): + """Emit a configured destination sequence across recovery plans.""" + + skill_id: ClassVar[str] = "destination_sequence" + GoalType: ClassVar[type] = EndEffectorPoseGoal + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement(endpoint_id="first"), + SkillEndpointRequirement(endpoint_id="second"), + ), + ), + ) + ) + + def __init__(self, destinations: tuple[str | None, ...]) -> None: + super().__init__() + self.destinations = destinations + self.plan_count = 0 + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + self.require_goal(request) + index = min(self.plan_count, len(self.destinations) - 1) + endpoint_id = self.destinations[index] + self.plan_count += 1 + if endpoint_id is None: + commands = TimedCommandSequence(frames=(), env_ids=context.env_ids) + return self.build_command_plan( + request, + context, + success=False, + commands=commands, + ) + + target = request.binding.endpoint("primary", endpoint_id).require_target( + JointPositionTarget + ) + joint_ids = list(target.joint_ids) + frame = RuntimeCommandFrame( + commands=( + EndpointCommand( + target=target, + payload=JointPositionPayload( + positions=context.robot.qpos[:, joint_ids] + ), + ), + ), + active_mask=torch.ones( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ), + env_ids=context.env_ids, + hold_duration=torch.zeros( + context.batch_size, + dtype=torch.float32, + device=context.robot.qpos.device, + ), + ) + return self.build_command_plan( + request, + context, + success=True, + commands=TimedCommandSequence( + frames=(frame,), + env_ids=context.env_ids, + ), + ) + + class UncopyableEntity(BatchEntity): """Minimal live entity whose simulator identity must not be copied.""" @@ -210,6 +304,30 @@ def _engine(batch_size: int = 1) -> tuple[AtomicActionEngine, DynamicAction]: return engine, action +def _destination_engine( + destinations: tuple[str | None, ...], +) -> tuple[AtomicActionEngine, DestinationSequenceAction]: + robot = Mock() + robot.device = torch.device("cpu") + robot.dof = 2 + robot.control_parts = {"arm_a": object(), "arm_b": object()} + robot.get_qpos.return_value = torch.zeros(1, 2) + robot.get_qvel.return_value = torch.zeros(1, 2) + robot.get_joint_ids.side_effect = lambda *, name: { + "arm_a": [0], + "arm_b": [1], + }[name] + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "stub" + generator.supports_dynamic_collision_world = False + engine = AtomicActionEngine(generator, load_builtins=False) + action = DestinationSequenceAction(destinations) + engine.register(action) + return engine, action + + def _context( timestamp: float, qpos: float | tuple[float, ...], @@ -274,6 +392,7 @@ def _collision_context( def _invocation( + engine: AtomicActionEngine, *, skill_id: str = "dynamic", max_replans: int = 2, @@ -286,7 +405,10 @@ def _invocation( return ActionInvocation( skill_id=skill_id, goal=EndEffectorPoseGoal(SceneEntityPose("target")), - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=engine.planning_services.bind_control_parts( + DynamicAction.binding_contract, + {"primary": {"motion": "arm"}}, + ), motion_policy=MotionPolicy( sample_count=2, control_dt=control_dt, @@ -304,17 +426,50 @@ def _invocation( ) +def _destination_invocation( + engine: AtomicActionEngine, +) -> ActionInvocation[EndEffectorPoseGoal]: + return ActionInvocation( + skill_id=DestinationSequenceAction.skill_id, + goal=EndEffectorPoseGoal(SceneEntityPose("target")), + binding=engine.bind_control_parts( + DestinationSequenceAction.skill_id, + { + "primary": { + "first": "arm_a", + "second": "arm_b", + } + }, + ), + recovery_policy=RecoveryPolicy( + max_replans=2, + max_action_retries=1, + goal_translation_threshold=0.02, + ), + invocation_id="destination-call", + ) + + +def _joint_positions(command: RuntimeCommandFrame | None) -> torch.Tensor: + """Return the only joint-position payload emitted by the test action.""" + assert command is not None + assert len(command.commands) == 1 + payload = command.commands[0].payload + assert isinstance(payload, JointPositionPayload) + return payload.positions + + def test_session_completes_incremental_command_sequence() -> None: engine, _ = _engine() - session = engine.start((_invocation(),), _context(0.0, 0.0, 0.2, 0)) + session = engine.start((_invocation(engine),), _context(0.0, 0.0, 0.2, 0)) first = session.tick(_context(0.0, 0.0, 0.2, 0)) second = session.tick(_context(0.1, 0.0, 0.2, 0)) final = session.tick(_context(0.2, 0.2, 0.2, 0)) - assert first.command is not None and torch.all(first.command.positions == 0.0) + assert torch.all(_joint_positions(first.command) == 0.0) assert all(event.invocation_id == "dynamic-call" for event in first.events) - assert second.command is not None and torch.all(second.command.positions == 0.2) + assert torch.all(_joint_positions(second.command) == 0.2) assert final.status is ExecutionStatus.COMPLETED assert final.eligible_mask.tolist() == [True] @@ -323,7 +478,7 @@ def test_session_commands_schedule_arrivals_and_final_settling() -> None: engine, _ = _engine() engine.register(NonuniformTimingAction()) session = engine.start( - (_invocation(skill_id="nonuniform_timing"),), + (_invocation(engine, skill_id="nonuniform_timing"),), _context(0.0, 0.0, 0.2, 0), ) @@ -361,7 +516,7 @@ def test_request_snapshot_preserves_live_entity_identity() -> None: request = ResolvedActionRequest( skill_id="pick_up", goal=goal, - binding=ResolvedActionBinding(), + binding=ActionBinding(owner_id="snapshot-test"), motion_policy=MotionPolicy(), recovery_policy=RecoveryPolicy(), skill_options=ActionOptions(), @@ -381,7 +536,7 @@ def test_request_snapshot_preserves_live_entity_identity() -> None: def test_scene_motion_replans_late_bound_goal() -> None: engine, action = _engine() - session = engine.start((_invocation(),), _context(0.0, 0.0, 0.1, 0)) + session = engine.start((_invocation(engine),), _context(0.0, 0.0, 0.1, 0)) session.tick(_context(0.0, 0.0, 0.1, 0)) tick = session.tick(_context(0.1, 0.0, 0.3, 1)) @@ -394,6 +549,52 @@ def test_scene_motion_replans_late_bound_goal() -> None: assert tick.command is not None +def test_recovery_replan_rejects_runtime_destination_change() -> None: + engine, action = _destination_engine(("first", "second")) + invocation = _destination_invocation(engine) + initial = _context(0.0, 0.0, 0.1, 0) + session = engine.start((invocation,), initial) + + activated = session.tick(initial) + assert activated.command is not None + assert activated.command.commands[0].target.target_id == "arm_a" + + with pytest.raises( + ValueError, + match="Recovery replans must preserve the active runtime destination set", + ) as exc_info: + session.tick(_context(0.1, 0.0, 0.3, 1)) + + assert "arm_a" in str(exc_info.value) + assert "arm_b" in str(exc_info.value) + assert action.plan_count == 2 + + +def test_empty_failed_replan_preserves_destination_for_same_target_retry() -> None: + engine, action = _destination_engine(("first", None, "first")) + invocation = _destination_invocation(engine) + initial = _context(0.0, 0.0, 0.1, 0) + session = engine.start((invocation,), initial) + + activated = session.tick(initial) + assert activated.command is not None + assert activated.command.commands[0].target.target_id == "arm_a" + + recovered = session.tick(_context(0.1, 0.0, 0.3, 1)) + + kinds = [event.kind for event in recovered.events] + assert ExecutionEventKind.DYNAMIC_GOAL_CHANGED in kinds + assert ExecutionEventKind.ACTION_RETRY in kinds + assert kinds.count(ExecutionEventKind.REPLANNED) == 2 + assert action.plan_count == 3 + assert recovered.command is None + assert [target.target_id for target in recovered.hold_targets] == ["arm_a"] + + resumed = session.tick(_context(0.2, 0.0, 0.3, 1)) + assert resumed.command is not None + assert resumed.command.commands[0].target.target_id == "arm_a" + + def test_collision_world_change_replans_with_latest_obstacle_pose() -> None: engine, action = _engine() generator = engine.motion_generator @@ -409,7 +610,7 @@ def test_collision_world_change_replans_with_latest_obstacle_pose() -> None: (0,), ) session = engine.start( - (_invocation(strategy="motion_gen"),), + (_invocation(engine, strategy="motion_gen"),), initial, ) session.tick(initial) @@ -448,6 +649,7 @@ def test_collision_world_exhaustion_only_disables_changed_environment() -> None: session = engine.start( ( _invocation( + engine, max_replans=0, strategy="motion_gen", ), @@ -496,6 +698,7 @@ def test_dynamic_collision_off_skips_binding_and_revision_recovery() -> None: session = engine.start( ( _invocation( + engine, strategy="motion_gen", dynamic_collision_mode=DynamicCollisionMode.OFF, ), @@ -526,6 +729,7 @@ def test_required_dynamic_collision_rejects_incompatible_strategy() -> None: with pytest.raises(ValueError, match="strategy='motion_gen'"): engine.plan( _invocation( + engine, dynamic_collision_mode=DynamicCollisionMode.REQUIRED, ), _collision_context( @@ -544,6 +748,7 @@ def test_required_dynamic_collision_rejects_missing_scene_entities() -> None: with pytest.raises(ValueError, match="scene collision entities"): engine.plan( _invocation( + engine, strategy="motion_gen", dynamic_collision_mode=DynamicCollisionMode.REQUIRED, ), @@ -557,6 +762,7 @@ def test_required_dynamic_collision_rejects_unsupported_planner() -> None: with pytest.raises(ValueError, match="dynamic collision-world support"): engine.plan( _invocation( + engine, strategy="motion_gen", dynamic_collision_mode=DynamicCollisionMode.REQUIRED, ), @@ -579,6 +785,7 @@ def test_required_dynamic_collision_binds_supported_scene() -> None: plan = engine.plan( _invocation( + engine, strategy="motion_gen", dynamic_collision_mode=DynamicCollisionMode.REQUIRED, ), @@ -598,7 +805,7 @@ def test_resolved_goal_snapshot_is_reused_during_recovery() -> None: engine, action = _engine() target = torch.eye(4).unsqueeze(0) target[:, 0, 3] = 0.2 - base = _invocation() + base = _invocation(engine) invocation = ActionInvocation( skill_id=base.skill_id, goal=EndEffectorPoseGoal(target), @@ -623,7 +830,7 @@ def test_resolved_goal_snapshot_is_reused_during_recovery() -> None: def test_subset_replan_restarts_synchronized_active_cohort() -> None: engine, action = _engine(batch_size=2) session = engine.start( - (_invocation(),), + (_invocation(engine),), _context(0.0, (0.0, 0.0), (0.1, 0.2), 0), ) session.tick(_context(0.0, (0.0, 0.0), (0.1, 0.2), 0)) @@ -644,17 +851,18 @@ def test_subset_replan_restarts_synchronized_active_cohort() -> None: assert changed.env_mask.tolist() == [True, False] assert cohort.env_mask.tolist() == [True, True] assert replanned.eligible_mask.tolist() == [True, True] - assert replanned.command is not None - assert torch.all(replanned.command.positions == 0.0) - assert next_command.command is not None - assert torch.equal(next_command.command.positions[:, 0], torch.tensor([0.4, 0.2])) + assert torch.all(_joint_positions(replanned.command) == 0.0) + assert torch.equal( + _joint_positions(next_command.command)[:, 0], + torch.tensor([0.4, 0.2]), + ) assert action.plan_count == 2 def test_replan_exhaustion_disables_only_triggering_row() -> None: engine, _ = _engine(batch_size=2) session = engine.start( - (_invocation(max_replans=1),), + (_invocation(engine, max_replans=1),), _context(0.0, (0.0, 0.0), (0.1, 0.2), 0), ) session.tick(_context(0.0, (0.0, 0.0), (0.1, 0.2), 0)) @@ -674,9 +882,51 @@ def test_replan_exhaustion_disables_only_triggering_row() -> None: assert exhausted.command.active_mask.tolist() == [False, True] +def test_action_retry_resets_replan_budget_only_for_allowed_rows() -> None: + engine, _ = _engine(batch_size=2) + session = engine.start( + ( + _invocation( + engine, + max_replans=1, + max_action_retries=1, + ), + ), + _context(0.0, (0.0, 0.0), (0.1, 0.2), 0), + ) + session.tick(_context(0.0, (0.0, 0.0), (0.1, 0.2), 0)) + + row_b_replan = session.tick(_context(0.1, (0.0, 0.0), (0.1, 0.4), 1)) + changed = next( + event + for event in row_b_replan.events + if event.kind is ExecutionEventKind.DYNAMIC_GOAL_CHANGED + ) + assert changed.env_mask.tolist() == [False, True] + + retry_events = session._attempt_action_retry( + torch.tensor([True, False]), + ExecutionEventKind.ACTION_TIMEOUT, + "Row A starts a new action attempt.", + ) + retried = next( + event for event in retry_events if event.kind is ExecutionEventKind.ACTION_RETRY + ) + assert retried.env_mask.tolist() == [True, False] + + row_b_exhausted = session.tick(_context(0.2, (0.0, 0.0), (0.1, 0.6), 2)) + exhausted = next( + event + for event in row_b_exhausted.events + if event.kind is ExecutionEventKind.RECOVERY_EXHAUSTED + ) + assert exhausted.env_mask.tolist() == [False, True] + assert row_b_exhausted.eligible_mask.tolist() == [True, False] + + def test_session_revision_replans_from_latest_context() -> None: engine, action = _engine() - original = _invocation() + original = _invocation(engine) session = engine.start((original,), _context(0.0, 0.0, 0.1, 0)) revised_pose = torch.eye(4).unsqueeze(0) revised_pose[:, 0, 3] = 0.8 @@ -702,13 +952,12 @@ def test_session_revision_replans_from_latest_context() -> None: and event.invocation_revision == 1 for event in first.events ) - assert second.command is not None - assert torch.all(second.command.positions == 0.8) + assert torch.all(_joint_positions(second.command) == 0.8) def test_session_revision_must_advance_same_invocation() -> None: engine, _ = _engine() - original = _invocation() + original = _invocation(engine) session = engine.start((original,), _context(0.0, 0.0, 0.1, 0)) with pytest.raises(ValueError, match="must advance"): @@ -728,10 +977,84 @@ def test_session_revision_must_advance_same_invocation() -> None: ) +def test_session_revision_rejects_runtime_destination_change() -> None: + engine, action = _destination_engine(("first", "second")) + invocation = _destination_invocation(engine) + initial = _context(0.0, 0.0, 0.1, 0) + session = engine.start((invocation,), initial) + + with pytest.raises( + ValueError, + match="Invocation revisions must preserve the active runtime destination set", + ) as exc_info: + session.revise_current(replace(invocation, revision=1)) + + assert "Start a new invocation" in str(exc_info.value) + assert "arm_a" in str(exc_info.value) + assert "arm_b" in str(exc_info.value) + assert action.plan_count == 2 + + active = session.tick(initial) + assert active.command is not None + assert active.command.commands[0].target.target_id == "arm_a" + + +def test_session_revision_rejects_empty_target_plan() -> None: + engine, action = _destination_engine(("first", None)) + invocation = _destination_invocation(engine) + initial = _context(0.0, 0.0, 0.1, 0) + session = engine.start((invocation,), initial) + + with pytest.raises(ValueError, match="empty replacement plan"): + session.revise_current(replace(invocation, revision=1)) + + assert action.plan_count == 2 + active = session.tick(initial) + assert active.command is not None + assert active.command.commands[0].target.target_id == "arm_a" + + +def test_session_revision_rejects_changed_target_address_fingerprint() -> None: + engine, action = _engine() + invocation = _invocation(engine) + initial = _context(0.0, 0.0, 0.1, 0) + session = engine.start((invocation,), initial) + endpoint = invocation.binding.endpoint("primary", "motion") + changed_endpoint = EndpointBinding( + slot_id=endpoint.slot_id, + endpoint_id=endpoint.endpoint_id, + resource_id=endpoint.resource_id, + adapter_id=endpoint.adapter_id, + target=JointPositionTarget(control_part="arm", joint_ids=(0,)), + capabilities=endpoint.capabilities, + commands=endpoint.commands, + claim_tokens=endpoint.claim_tokens, + joint_ids=(0,), + ) + revised = replace( + invocation, + binding=ActionBinding( + owner_id=invocation.binding.owner_id, + endpoints=(changed_endpoint,), + ), + revision=1, + ) + + with pytest.raises(ValueError, match="address fingerprint"): + session.revise_current(revised) + + assert action.plan_count == 2 + active = session.tick(initial) + assert active.command is not None + target = active.command.commands[0].target + assert isinstance(target, JointPositionTarget) + assert target.joint_ids == (0, 1) + + def test_tracking_error_fails_when_replan_budget_is_zero() -> None: engine, _ = _engine() session = engine.start( - (_invocation(max_replans=0),), + (_invocation(engine, max_replans=0),), _context(0.0, 0.0, 0.2, 0), ) session.tick(_context(0.0, 0.0, 0.2, 0)) @@ -750,6 +1073,7 @@ def test_action_timeout_retry_budget_is_bounded() -> None: session = engine.start( ( _invocation( + engine, max_action_retries=1, action_timeout=0.05, ), @@ -776,7 +1100,7 @@ def test_action_timeout_retry_budget_is_bounded() -> None: def test_session_rejects_changed_environment_identity() -> None: engine, _ = _engine() initial = _context(0.0, 0.0, 0.2, 0) - session = engine.start((_invocation(),), initial) + session = engine.start((_invocation(engine),), initial) changed = PlanningContext( robot=initial.robot, task=initial.task, @@ -790,7 +1114,7 @@ def test_session_rejects_changed_environment_identity() -> None: def test_session_rejects_regressing_scene_snapshot() -> None: engine, _ = _engine() - session = engine.start((_invocation(),), _context(1.0, 0.0, 0.2, 2)) + session = engine.start((_invocation(engine),), _context(1.0, 0.0, 0.2, 2)) with pytest.raises(ValueError, match="versions must be monotonic"): session.tick(_context(1.0, 0.0, 0.2, 1)) @@ -801,7 +1125,7 @@ def test_session_rejects_regressing_collision_world_revision() -> None: qpos = torch.zeros(1, 2) initial = _collision_context(0.0, qpos, torch.tensor([0.4]), (2,)) session = engine.start( - (_invocation(strategy="motion_gen"),), + (_invocation(engine, strategy="motion_gen"),), initial, ) regressed = _collision_context(0.1, qpos, torch.tensor([0.4]), (1,)) @@ -814,7 +1138,7 @@ def test_nonempty_effect_is_committed_only_after_external_verification() -> None engine, _ = _engine() effect = EffectAction() engine.register(effect) - invocation = _invocation() + invocation = _invocation(engine) invocation = ActionInvocation( skill_id="effect", goal=invocation.goal, @@ -858,10 +1182,39 @@ def test_nonempty_effect_is_committed_only_after_external_verification() -> None assert completed.task_state.get_held_object("arm") is not None +def test_session_revision_cannot_abandon_pending_effect_verification() -> None: + engine, _ = _engine() + engine.register(EffectAction()) + base = _invocation(engine) + invocation = ActionInvocation( + skill_id="effect", + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) + session.tick(_context(0.0, 0.0, 0.2, 0)) + session.tick(_context(0.1, 0.0, 0.2, 0)) + waiting = session.tick(_context(0.2, 0.2, 0.2, 0)) + assert waiting.pending_effect is not None + + with pytest.raises(RuntimeError, match="awaiting verification"): + session.revise_current(replace(invocation, revision=1)) + + assert session.effect_verification_pending is True + completed = session.tick( + _context(0.3, 0.2, 0.2, 0), + effect_success=torch.tensor([True]), + ) + assert completed.status is ExecutionStatus.COMPLETED + assert completed.task_state.get_held_object("arm") is not None + + def test_effect_failure_does_not_commit_and_exhausts_retry_budget() -> None: engine, _ = _engine() engine.register(EffectAction()) - base = _invocation(max_action_retries=0) + base = _invocation(engine, max_action_retries=0) invocation = ActionInvocation( skill_id="effect", goal=base.goal, @@ -888,7 +1241,7 @@ def test_effect_failure_does_not_commit_and_exhausts_retry_budget() -> None: def test_failed_effect_plan_retries_without_requesting_effect_verification() -> None: engine, _ = _engine() engine.register(FailedEffectAction()) - base = _invocation(max_action_retries=0) + base = _invocation(engine, max_action_retries=0) invocation = ActionInvocation( skill_id="failed_effect", goal=base.goal, diff --git a/tests/sim/atomic_actions/test_motion_strategy_e2e.py b/tests/sim/atomic_actions/test_motion_strategy_e2e.py index dd3e7e058..eaf99f44d 100644 --- a/tests/sim/atomic_actions/test_motion_strategy_e2e.py +++ b/tests/sim/atomic_actions/test_motion_strategy_e2e.py @@ -25,7 +25,6 @@ from embodichain.lab.sim.robots import CobotMagicCfg from embodichain.lab.sim.planners import MotionGenerator, MotionGenCfg, ToppraPlannerCfg from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, @@ -79,14 +78,16 @@ def _run_reach_test(self, strategy: str): sim, robot, engine = self._setup() try: target, arm_ids = self._reachable_target(robot) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": self.CONTROL_PART}}, + ) result = engine.compile( ( ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(xpos=target), - binding=ActionBinding( - manipulators={"primary": self.CONTROL_PART} - ), + binding=binding, motion_policy=MotionPolicy( strategy=strategy, sample_count=self.SAMPLE_INTERVAL, @@ -95,7 +96,10 @@ def _run_reach_test(self, strategy: str): ) ) assert result.plan_success.all().item(), f"{strategy} reported failure" - final_q = result.trajectory.positions[0, -1, arm_ids] + plan = result.action_plans[0] + assert plan.joint_trajectory is not None + assert plan.commands.frame_count == plan.joint_trajectory.waypoint_count + final_q = plan.joint_trajectory.positions[0, -1, arm_ids] fk = robot.compute_fk( qpos=final_q[None], name=self.CONTROL_PART, to_matrix=True )[0] diff --git a/tests/sim/atomic_actions/test_runner.py b/tests/sim/atomic_actions/test_runner.py index bfeccc04b..7fe66b73c 100644 --- a/tests/sim/atomic_actions/test_runner.py +++ b/tests/sim/atomic_actions/test_runner.py @@ -41,15 +41,22 @@ ExecutionRunner, ExecutionRunnerCfg, HeldObjectState, - JointCommand, + JOINT_POSITION_CAPABILITY, + JointPositionPayload, + JointPositionTarget, MotionPolicy, ObjectSemantics, PlanningContext, RecoveryPolicy, ResolvedActionRequest, RobotObservation, + RuntimeCommandFrame, + RuntimeEndpointTarget, RunnerStatus, SceneSnapshot, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, StateDelta, TaskState, TimedTrajectory, @@ -115,14 +122,15 @@ def __init__(self, provider: FakeObservationProvider) -> None: self.provider = provider self.send_statuses: deque[CommandAckStatus] = deque() self.follow_commands: deque[bool] = deque() - self.sent: list[JointCommand] = [] + self.sent: list[RuntimeCommandFrame] = [] self.send_times: list[float] = [] - self.held: list[JointCommand] = [] + self.held: list[tuple[tuple[RuntimeEndpointTarget, ...], PlanningContext]] = [] + self.cancelled: list[tuple[RuntimeEndpointTarget, ...]] = [] self.cancel_count = 0 def send( self, - command: JointCommand, + command: RuntimeCommandFrame, *, timeout: float, ) -> CommandAcknowledgement: @@ -136,22 +144,41 @@ def send( ) follows = self.follow_commands.popleft() if self.follow_commands else True if status is CommandAckStatus.ACCEPTED and follows: - self.provider.qpos = command.positions.clone() + positions = self.provider.qpos.clone() + for endpoint_command in command.commands: + target = endpoint_command.target + payload = endpoint_command.payload + assert isinstance(target, JointPositionTarget) + assert isinstance(payload, JointPositionPayload) + joint_ids = list(target.joint_ids) + positions[:, joint_ids] = torch.where( + command.active_mask[:, None], + payload.positions, + positions[:, joint_ids], + ) + self.provider.qpos = positions return CommandAcknowledgement(status) def hold( self, - command: JointCommand, + targets: tuple[RuntimeEndpointTarget, ...], + context: PlanningContext, *, timeout: float, ) -> CommandAcknowledgement: - """Record and apply a hold command.""" - self.held.append(command) - self.provider.qpos = command.positions.clone() + """Record targets and apply the supplied observed-state hold.""" + self.held.append((tuple(targets), context)) + self.provider.qpos = context.robot.qpos.clone() return CommandAcknowledgement.accepted_ack() - def cancel(self, *, timeout: float) -> CommandAcknowledgement: + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: """Record controller cancellation.""" + self.cancelled.append(tuple(targets)) self.cancel_count += 1 return CommandAcknowledgement.accepted_ack() @@ -161,7 +188,19 @@ class TimedAction(AtomicAction[EndEffectorPoseGoal, ActionOptions]): skill_id: ClassVar[str] = "timed" GoalType: ClassVar[type] = EndEffectorPoseGoal - manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset({JOINT_POSITION_CAPABILITY}), + ), + ), + ), + ) + ) def __init__(self, *, with_effect: bool = False) -> None: super().__init__() @@ -213,10 +252,19 @@ def _plan( ) +def _timed_action_binding(action: TimedAction) -> ActionBinding: + """Bind the timed action's generic motion endpoint to the fake arm.""" + return action.planning_services.bind_control_parts( + TimedAction.binding_contract, + {"primary": {"motion": "arm"}}, + ) + + def _make_runner( *, with_effect: bool = False, batch_size: int = BATCH_SIZE, + control_joint_ids: tuple[int, ...] | None = None, ) -> tuple[ ExecutionRunner, FakeClock, @@ -232,7 +280,9 @@ def _make_runner( robot.dof = ROBOT_DOF robot.control_parts = {"arm": object()} robot.get_qpos.return_value = torch.zeros(batch_size, ROBOT_DOF) - robot.get_joint_ids.return_value = list(range(ROBOT_DOF)) + robot.get_joint_ids.return_value = list( + range(ROBOT_DOF) if control_joint_ids is None else control_joint_ids + ) generator = Mock() generator.robot = robot generator.device = torch.device("cpu") @@ -247,7 +297,7 @@ def _make_runner( invocation = ActionInvocation( skill_id="timed", goal=EndEffectorPoseGoal(goal_pose), - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=_timed_action_binding(action), motion_policy=MotionPolicy(sample_count=3, control_dt=FIRST_INTERVAL), recovery_policy=RecoveryPolicy( max_replans=2, @@ -266,6 +316,30 @@ def _make_runner( return runner, clock, provider, sink, action +def test_joint_feedback_ignores_motion_outside_bound_endpoint() -> None: + runner, clock, provider, sink, action = _make_runner(control_joint_ids=(0,)) + + runner.step() + provider.qpos[:, 1] = 42.0 + clock.advance(FIRST_INTERVAL) + second = runner.step() + clock.advance(SECOND_INTERVAL) + runner.step() + clock.advance(SECOND_INTERVAL) + completed = runner.step() + + assert action.plan_count == 1 + assert len(sink.sent) == 3 + assert not any( + event.kind is ExecutionEventKind.TRACKING_ERROR + for step in (second, completed) + if step.tick is not None + for event in step.tick.events + ) + assert completed.status is RunnerStatus.COMPLETED + assert provider.qpos[0, 1].item() == 42.0 + + def test_runner_dispatches_only_when_timed_waypoint_is_due() -> None: runner, clock, _, sink, _ = _make_runner() @@ -289,13 +363,34 @@ def test_runner_dispatches_only_when_timed_waypoint_is_due() -> None: assert third.wait_duration == pytest.approx(SECOND_INTERVAL) -def test_session_active_trajectory_returns_an_owned_snapshot() -> None: +def test_runner_dispatches_transport_neutral_endpoint_frames() -> None: + runner, _, _, sink, _ = _make_runner() + + runner.step() + + frame = sink.sent[0] + assert isinstance(frame, RuntimeCommandFrame) + assert len(frame.commands) == 1 + endpoint_command = frame.commands[0] + assert isinstance(endpoint_command.target, JointPositionTarget) + assert endpoint_command.target.transport_id == "robot.joint_position" + assert endpoint_command.target.target_id == "arm" + assert endpoint_command.target.joint_ids == (0, 1) + assert isinstance(endpoint_command.payload, JointPositionPayload) + assert endpoint_command.payload.transport_id == endpoint_command.target.transport_id + + +def test_session_active_commands_return_an_owned_endpoint_snapshot() -> None: runner, _, _, _, _ = _make_runner() - trajectory = runner.session.active_trajectory - trajectory.positions.fill_(-1.0) + commands = runner.session.active_commands + payload = commands.frames[0].commands[0].payload + assert isinstance(payload, JointPositionPayload) + payload.positions.fill_(-1.0) - assert torch.all(runner.session.active_trajectory.positions >= 0.0) + current_payload = runner.session.active_commands.frames[0].commands[0].payload + assert isinstance(current_payload, JointPositionPayload) + assert torch.all(current_payload.positions >= 0.0) def test_runner_uses_the_longest_active_batch_interval_as_a_barrier() -> None: @@ -324,6 +419,11 @@ def test_runner_completes_and_holds_after_last_command_settles() -> None: assert completed.command_count == 3 assert [item.operation for item in completed.dispatches] == [CommandOperation.HOLD] assert len(sink.held) == 1 + held_targets, hold_context = sink.held[0] + assert [(target.transport_id, target.target_id) for target in held_targets] == [ + ("robot.joint_position", "arm") + ] + assert torch.equal(hold_context.robot.qpos, sink.provider.qpos) @pytest.mark.parametrize( @@ -345,6 +445,8 @@ def test_runner_safely_stops_when_command_is_not_accepted( CommandOperation.HOLD, ] assert sink.cancel_count == 1 + assert [target.target_id for target in sink.cancelled[0]] == ["arm"] + assert [target.target_id for target in sink.held[0][0]] == ["arm"] assert failed.message is not None and status.value in failed.message @@ -363,6 +465,8 @@ def test_runner_cancel_performs_cancel_then_hold() -> None: assert repeated.status is RunnerStatus.CANCELLED assert repeated.dispatches == () assert sink.cancel_count == 1 + assert sink.cancelled == [()] + assert sink.held[0][0] == () def test_runner_replans_from_observation_after_tracking_error() -> None: @@ -383,14 +487,17 @@ def test_runner_replans_from_observation_after_tracking_error() -> None: assert recovered.status is RunnerStatus.RUNNING -def test_runner_surfaces_explicit_invocation_revision() -> None: - runner, _, _, _, action = _make_runner() +def test_runner_revision_waits_for_deadline_and_plans_from_fresh_observation() -> None: + runner, clock, provider, sink, action = _make_runner() + first = runner.step() + assert first.wait_duration == pytest.approx(FIRST_INTERVAL) + revised_pose = torch.eye(4) revised_pose[0, 3] = 2.0 * TARGET_POSITION revised = ActionInvocation( skill_id="timed", goal=EndEffectorPoseGoal(revised_pose), - binding=ActionBinding(manipulators={"primary": "arm"}), + binding=_timed_action_binding(action), motion_policy=MotionPolicy(sample_count=3, control_dt=FIRST_INTERVAL), recovery_policy=RecoveryPolicy( max_replans=2, @@ -400,11 +507,24 @@ def test_runner_surfaces_explicit_invocation_revision() -> None: revision=1, ) - runner.session.revise_current(revised) + runner.revise_current(revised) + provider.qpos.fill_(0.4) + waiting = runner.step() + + assert waiting.is_waiting is True + assert action.plan_count == 1 + assert sink.send_times == [0.0] + + clock.advance(FIRST_INTERVAL) result = runner.step() assert action.plan_count == 2 + assert result.command_count == 2 + assert sink.send_times == pytest.approx([0.0, FIRST_INTERVAL]) assert result.tick is not None + revised_payload = result.tick.command.commands[0].payload + assert isinstance(revised_payload, JointPositionPayload) + assert torch.allclose(revised_payload.positions, torch.full((1, 2), 0.4)) assert any( event.kind is ExecutionEventKind.INVOCATION_REVISED and event.invocation_revision == 1 @@ -412,6 +532,41 @@ def test_runner_surfaces_explicit_invocation_revision() -> None: ) +def test_runner_revision_rejects_pending_effect_verification() -> None: + runner, _, _, _, action = _make_runner(with_effect=True) + blocked = runner.run_until_blocked() + assert blocked.tick is not None + assert blocked.tick.pending_effect is not None + assert runner.effect_verification_pending is True + + revised_pose = torch.eye(4) + revised_pose[0, 3] = 2.0 * TARGET_POSITION + revised = ActionInvocation( + skill_id="timed", + goal=EndEffectorPoseGoal(revised_pose), + binding=_timed_action_binding(action), + motion_policy=MotionPolicy(sample_count=3, control_dt=FIRST_INTERVAL), + recovery_policy=RecoveryPolicy( + max_replans=2, + tracking_error_threshold=0.05, + action_timeout=10.0, + ), + revision=1, + ) + + with pytest.raises(RuntimeError, match="awaiting verification"): + runner.revise_current(revised) + + assert runner.effect_verification_pending is True + completed = runner.run_until_blocked( + effect_verifier=lambda context, tick: torch.ones( + context.batch_size, + dtype=torch.bool, + ) + ) + assert completed.status is RunnerStatus.COMPLETED + + def test_runner_fails_safely_when_observation_provider_raises() -> None: runner, _, provider, sink, _ = _make_runner() provider.fail = True @@ -425,6 +580,8 @@ def test_runner_fails_safely_when_observation_provider_raises() -> None: ] assert len(sink.held) == 1 assert sink.cancel_count == 1 + assert sink.cancelled == [()] + assert sink.held[0][0] == () assert failed.message is not None and "observation unavailable" in failed.message diff --git a/tests/sim/atomic_actions/test_runtime_commands.py b/tests/sim/atomic_actions/test_runtime_commands.py new file mode 100644 index 000000000..fb0e6bd62 --- /dev/null +++ b/tests/sim/atomic_actions/test_runtime_commands.py @@ -0,0 +1,379 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Pure value-object tests for transport-neutral runtime commands.""" + +from __future__ import annotations + +from dataclasses import dataclass + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions.bindings import ( + JointPositionTarget, + RuntimeEndpointTarget, +) +from embodichain.lab.sim.atomic_actions.runtime_commands import ( + EndpointCommand, + JointPositionPayload, + RuntimeCommandFrame, + RuntimeCommandPayload, + TimedCommandSequence, +) + + +@dataclass(frozen=True, slots=True) +class _TestTarget(RuntimeEndpointTarget): + """Small target used to exercise custom transports.""" + + _transport_id: str + _target_id: str + + @property + def transport_id(self) -> str: + """Return the test transport identifier.""" + return self._transport_id + + @property + def target_id(self) -> str: + """Return the test destination identifier.""" + return self._target_id + + +@dataclass(frozen=True, slots=True) +class _OpaquePayload(RuntimeCommandPayload): + """Metadata-only payload used for transport and device validation.""" + + rows: int + payload_device: torch.device + payload_transport: str + + @property + def batch_size(self) -> int: + """Return the configured row count.""" + return self.rows + + @property + def device(self) -> torch.device: + """Return the configured device.""" + return self.payload_device + + @property + def transport_id(self) -> str: + """Return the configured transport identifier.""" + return self.payload_transport + + def snapshot(self) -> _OpaquePayload: + """Return an independently owned payload.""" + return _OpaquePayload( + rows=self.rows, + payload_device=self.payload_device, + payload_transport=self.payload_transport, + ) + + +class _SelfSnapshotPayload(RuntimeCommandPayload): + """Invalid payload whose snapshot aliases the source.""" + + @property + def batch_size(self) -> int: + """Return one row.""" + return 1 + + @property + def device(self) -> torch.device: + """Return the CPU device.""" + return torch.device("cpu") + + @property + def transport_id(self) -> str: + """Return the test transport.""" + return "test.transport" + + def snapshot(self) -> _SelfSnapshotPayload: + """Incorrectly return this same payload.""" + return self + + +def _joint_command( + control_part: str, + joint_ids: tuple[int, ...], + positions: torch.Tensor, +) -> EndpointCommand: + """Build one joint endpoint command for a test.""" + return EndpointCommand( + target=JointPositionTarget(control_part, joint_ids), + payload=JointPositionPayload(positions), + ) + + +def _frame( + commands: tuple[EndpointCommand, ...], + *, + active_mask: torch.Tensor | None = None, + env_ids: torch.Tensor | None = None, + hold_duration: torch.Tensor | None = None, +) -> RuntimeCommandFrame: + """Build a two-row CPU frame with optional field replacements.""" + return RuntimeCommandFrame( + commands=commands, + active_mask=( + torch.tensor([True, False]) if active_mask is None else active_mask + ), + env_ids=torch.tensor([4, 9]) if env_ids is None else env_ids, + hold_duration=( + torch.tensor([0.0, 0.1]) if hold_duration is None else hold_duration + ), + ) + + +def test_runtime_command_payload_is_abstract() -> None: + with pytest.raises(TypeError): + RuntimeCommandPayload() # type: ignore[abstract] + + +def test_joint_position_payload_owns_tensors_and_snapshots() -> None: + positions = torch.tensor([[1.0, 2.0], [3.0, 4.0]]) + velocities = torch.tensor([[0.1, 0.2], [0.3, 0.4]]) + payload = JointPositionPayload(positions, velocities) + + positions.fill_(9.0) + velocities.fill_(8.0) + snapshot = payload.snapshot() + snapshot.positions.fill_(7.0) + assert payload.positions.tolist() == [[1.0, 2.0], [3.0, 4.0]] + assert payload.velocities is not None + assert torch.allclose( + payload.velocities, + torch.tensor([[0.1, 0.2], [0.3, 0.4]]), + ) + assert payload.batch_size == 2 + assert payload.dof == 2 + assert payload.device == torch.device("cpu") + assert payload.transport_id == JointPositionTarget.TRANSPORT_ID + + +@pytest.mark.parametrize( + "positions, message", + [ + (torch.empty(0, 2), "non-zero"), + (torch.empty(2, 0), "non-zero"), + (torch.zeros(2), "shape"), + (torch.tensor([[float("nan")]]), "finite"), + ], +) +def test_joint_position_payload_rejects_invalid_positions( + positions: torch.Tensor, + message: str, +) -> None: + with pytest.raises(ValueError, match=message): + JointPositionPayload(positions) + + +def test_joint_position_payload_validates_velocities() -> None: + positions = torch.zeros(2, 2) + with pytest.raises(ValueError, match="match positions shape"): + JointPositionPayload(positions, torch.zeros(2, 3)) + with pytest.raises(ValueError, match="finite"): + JointPositionPayload( + positions, + torch.tensor([[0.0, float("inf")], [0.0, 0.0]]), + ) + + +def test_endpoint_command_requires_matching_transport() -> None: + with pytest.raises(ValueError, match="does not accept"): + EndpointCommand( + target=_TestTarget("test.target", "base"), + payload=_OpaquePayload(2, torch.device("cpu"), "test.payload"), + ) + + +def test_endpoint_command_owns_target_and_payload_snapshots() -> None: + target = _TestTarget("test.transport", "base") + payload = _OpaquePayload(2, torch.device("cpu"), "test.transport") + command = EndpointCommand(target=target, payload=payload) + + assert command.target is not target + assert command.payload is not payload + assert command.transport_id == "test.transport" + assert command.destination_key == ("test.transport", "base") + assert command.batch_size == 2 + assert command.device == torch.device("cpu") + assert command.snapshot().payload is not command.payload + + +def test_endpoint_command_rejects_aliased_payload_snapshot() -> None: + with pytest.raises(TypeError, match="independently owned"): + EndpointCommand( + target=_TestTarget("test.transport", "base"), + payload=_SelfSnapshotPayload(), + ) + + +def test_runtime_command_frame_accepts_disjoint_joint_destinations() -> None: + frame = _frame( + ( + _joint_command("left", (0, 2), torch.zeros(2, 2)), + _joint_command("right", (1, 3), torch.ones(2, 2)), + ) + ) + + assert frame.batch_size == 2 + assert frame.device == torch.device("cpu") + assert [target.target_id for target in frame.targets] == ["left", "right"] + assert frame.active_mask.tolist() == [True, False] + assert frame.env_ids.tolist() == [4, 9] + + +def test_runtime_command_frame_rejects_payload_batch_mismatch() -> None: + with pytest.raises(ValueError, match="batch size 1, expected 2"): + _frame((_joint_command("arm", (0,), torch.zeros(1, 1)),)) + + +def test_runtime_command_frame_rejects_payload_device_mismatch() -> None: + command = EndpointCommand( + target=_TestTarget("test.transport", "base"), + payload=_OpaquePayload(2, torch.device("meta"), "test.transport"), + ) + with pytest.raises(ValueError, match="share the frame device"): + _frame((command,)) + + +def test_runtime_command_frame_rejects_duplicate_destination() -> None: + target = _TestTarget("test.transport", "base") + command = EndpointCommand( + target=target, + payload=_OpaquePayload(2, torch.device("cpu"), "test.transport"), + ) + with pytest.raises(ValueError, match="duplicate destination"): + _frame((command, command)) + + +def test_runtime_command_frame_requires_joint_payload_for_joint_target() -> None: + command = EndpointCommand( + target=JointPositionTarget("arm", (0,)), + payload=_OpaquePayload( + 2, + torch.device("cpu"), + JointPositionTarget.TRANSPORT_ID, + ), + ) + with pytest.raises(TypeError, match="requires a JointPositionPayload"): + _frame((command,)) + + +def test_runtime_command_frame_rejects_joint_target_dof_mismatch() -> None: + with pytest.raises(ValueError, match="DOF 1, expected 2"): + _frame((_joint_command("arm", (0, 1), torch.zeros(2, 1)),)) + + +def test_runtime_command_frame_rejects_overlapping_joint_ids() -> None: + with pytest.raises(ValueError, match=r"overlaps joint IDs \[2\]"): + _frame( + ( + _joint_command("left", (0, 2), torch.zeros(2, 2)), + _joint_command("right", (2, 3), torch.zeros(2, 2)), + ) + ) + + +def test_runtime_command_frame_validates_batch_metadata() -> None: + command = _joint_command("arm", (0,), torch.zeros(2, 1)) + with pytest.raises(ValueError, match="active_mask"): + _frame((command,), active_mask=torch.tensor([1, 0])) + with pytest.raises(ValueError, match="env_ids"): + _frame((command,), env_ids=torch.tensor([4.0, 9.0])) + with pytest.raises(ValueError, match="hold_duration"): + _frame((command,), hold_duration=torch.tensor([0.0, float("nan")])) + with pytest.raises(ValueError, match="non-negative"): + _frame((command,), hold_duration=torch.tensor([0.0, -0.1])) + with pytest.raises(ValueError, match="unique"): + _frame((command,), env_ids=torch.tensor([4, 4])) + + +def test_runtime_command_frame_with_active_mask_returns_owned_frame() -> None: + frame = _frame((_joint_command("arm", (0,), torch.zeros(2, 1)),)) + replacement = torch.tensor([False, True]) + updated = frame.with_active_mask(replacement) + + replacement.fill_(False) + updated.commands[0].payload.positions.fill_(4.0) + assert updated.active_mask.tolist() == [False, True] + assert frame.active_mask.tolist() == [True, False] + assert isinstance(frame.commands[0].payload, JointPositionPayload) + assert frame.commands[0].payload.positions.tolist() == [[0.0], [0.0]] + + +def test_timed_command_sequence_preserves_empty_batch_and_device() -> None: + env_ids = torch.tensor([3, 7], dtype=torch.long) + sequence = TimedCommandSequence(frames=(), env_ids=env_ids) + + env_ids.fill_(0) + assert sequence.frame_count == 0 + assert sequence.batch_size == 2 + assert sequence.device == torch.device("cpu") + assert sequence.env_ids.tolist() == [3, 7] + assert sequence.targets == () + + +def test_timed_command_sequence_requires_matching_frame_env_ids() -> None: + frame = _frame((_joint_command("arm", (0,), torch.zeros(2, 1)),)) + with pytest.raises(ValueError, match="env_ids do not match"): + TimedCommandSequence( + frames=(frame,), + env_ids=torch.tensor([4, 8], dtype=torch.long), + ) + + +def test_timed_command_sequence_owns_frames_and_returns_unique_targets() -> None: + first = _frame( + ( + _joint_command("left", (0,), torch.zeros(2, 1)), + _joint_command("right", (1,), torch.ones(2, 1)), + ) + ) + second = _frame((_joint_command("left", (0,), torch.full((2, 1), 2.0)),)) + sequence = TimedCommandSequence( + frames=(first, second), + env_ids=torch.tensor([4, 9]), + ) + snapshot = sequence.snapshot() + + snapshot.frames[0].active_mask.fill_(False) + targets = sequence.targets + assert sequence.frame_count == 2 + assert sequence.frames[0].active_mask.tolist() == [True, False] + assert [target.target_id for target in targets] == ["left", "right"] + assert targets[0] is not sequence.frames[0].commands[0].target + + +def test_timed_command_sequence_rejects_invalid_frame_values() -> None: + with pytest.raises(TypeError, match="RuntimeCommandFrame"): + TimedCommandSequence( + frames=(object(),), # type: ignore[arg-type] + env_ids=torch.tensor([0], dtype=torch.long), + ) + + +def test_timed_command_sequence_requires_nonempty_int64_batch() -> None: + with pytest.raises(ValueError, match="int64"): + TimedCommandSequence(frames=(), env_ids=torch.empty(0, dtype=torch.long)) + with pytest.raises(ValueError, match="int64"): + TimedCommandSequence(frames=(), env_ids=torch.tensor([0.0])) + with pytest.raises(ValueError, match="unique"): + TimedCommandSequence(frames=(), env_ids=torch.tensor([2, 2])) diff --git a/tests/sim/atomic_actions/test_sim_adapter.py b/tests/sim/atomic_actions/test_sim_adapter.py index 01356abde..5b29109c0 100644 --- a/tests/sim/atomic_actions/test_sim_adapter.py +++ b/tests/sim/atomic_actions/test_sim_adapter.py @@ -25,9 +25,13 @@ from embodichain.lab.sim.atomic_actions import ( CommandAckStatus, - JointCommand, + EndpointCommand, + EndpointCommandTransport, + JointPositionPayload, + JointPositionTarget, RigidObjectSceneProvider, RigidObjectSceneProviderCfg, + RuntimeCommandFrame, SceneSnapshot, SimulationExecutionAdapter, TaskState, @@ -53,10 +57,20 @@ def _command( *, env_ids: torch.Tensor | None = None, active_mask: torch.Tensor | None = None, -) -> JointCommand: - return JointCommand( - positions=torch.ones(BATCH_SIZE, ROBOT_DOF), - velocities=torch.full((BATCH_SIZE, ROBOT_DOF), 0.5), +) -> RuntimeCommandFrame: + return RuntimeCommandFrame( + commands=( + EndpointCommand( + target=JointPositionTarget( + control_part="arm", + joint_ids=tuple(range(ROBOT_DOF)), + ), + payload=JointPositionPayload( + positions=torch.ones(BATCH_SIZE, ROBOT_DOF), + velocities=torch.full((BATCH_SIZE, ROBOT_DOF), 0.5), + ), + ), + ), active_mask=( torch.tensor([True, False]) if active_mask is None else active_mask ), @@ -80,6 +94,15 @@ def test_simulation_adapter_observes_full_robot_state() -> None: assert context.scene.version == 0 +def test_simulation_adapter_is_joint_position_transport() -> None: + simulation, robot = _simulation_and_robot() + adapter = SimulationExecutionAdapter(simulation, robot) + + assert isinstance(adapter, EndpointCommandTransport) + assert adapter.transport_id == JointPositionTarget.TRANSPORT_ID + assert adapter.payload_type is JointPositionPayload + + @pytest.mark.parametrize("error", [AttributeError, NotImplementedError]) def test_simulation_adapter_treats_unavailable_effort_as_optional( error: type[Exception], @@ -115,10 +138,86 @@ def test_simulation_adapter_sends_active_rows_and_inactive_holds_together() -> N assert acknowledgement.status is CommandAckStatus.ACCEPTED sent_qpos = robot.set_qpos.call_args.args[0] sent_qvel = robot.set_qvel.call_args.args[0] - assert torch.equal(sent_qpos, command.positions) - assert torch.equal(sent_qvel, command.velocities) + expected_qpos = torch.zeros(BATCH_SIZE, ROBOT_DOF) + expected_qpos[0] = 1.0 + expected_qvel = torch.zeros(BATCH_SIZE, ROBOT_DOF) + expected_qvel[0] = 0.5 + assert torch.equal(sent_qpos, expected_qpos) + assert torch.equal(sent_qvel, expected_qvel) + endpoint_command = command.commands[0] + assert isinstance(endpoint_command.target, JointPositionTarget) + assert endpoint_command.target.target_id == "arm" + assert endpoint_command.target.joint_ids == tuple(range(ROBOT_DOF)) + assert isinstance(endpoint_command.payload, JointPositionPayload) assert robot.set_qpos.call_args.kwargs["env_ids"] == [0, 1] + assert robot.set_qpos.call_args.kwargs["joint_ids"] == [0, 1, 2] assert robot.set_qvel.call_args.kwargs["env_ids"] == [0, 1] + assert robot.set_qvel.call_args.kwargs["joint_ids"] == [0, 1, 2] + + +def test_simulation_adapter_writes_disjoint_joint_endpoints_independently() -> None: + simulation, robot = _simulation_and_robot() + adapter = SimulationExecutionAdapter(simulation, robot) + command = RuntimeCommandFrame( + commands=( + EndpointCommand( + target=JointPositionTarget("arm", (0, 2)), + payload=JointPositionPayload(torch.tensor([[1.0, 3.0], [4.0, 6.0]])), + ), + EndpointCommand( + target=JointPositionTarget("tool", (1,)), + payload=JointPositionPayload(torch.tensor([[2.0], [5.0]])), + ), + ), + active_mask=torch.ones(BATCH_SIZE, dtype=torch.bool), + env_ids=torch.arange(BATCH_SIZE, dtype=torch.long), + hold_duration=torch.zeros(BATCH_SIZE), + ) + + acknowledgement = adapter.send(command, timeout=1.0) + + assert acknowledgement.status is CommandAckStatus.ACCEPTED + assert robot.set_qpos.call_count == 2 + arm_call, tool_call = robot.set_qpos.call_args_list + assert torch.equal( + arm_call.args[0], + torch.tensor([[1.0, 3.0], [4.0, 6.0]]), + ) + assert arm_call.kwargs == {"joint_ids": [0, 2], "env_ids": [0, 1]} + assert torch.equal(tool_call.args[0], torch.tensor([[2.0], [5.0]])) + assert tool_call.kwargs == {"joint_ids": [1], "env_ids": [0, 1]} + robot.set_qvel.assert_not_called() + + +def test_simulation_adapter_neutralizes_inactive_rows_without_velocity_payload() -> ( + None +): + simulation, robot = _simulation_and_robot() + robot.get_qvel.return_value = torch.tensor([[0.1, 0.2, 0.3], [0.4, 0.5, 0.6]]) + adapter = SimulationExecutionAdapter(simulation, robot) + command = RuntimeCommandFrame( + commands=( + EndpointCommand( + target=JointPositionTarget("arm", (0, 2)), + payload=JointPositionPayload(torch.ones(BATCH_SIZE, 2)), + ), + ), + active_mask=torch.tensor([True, False]), + env_ids=torch.arange(BATCH_SIZE, dtype=torch.long), + hold_duration=torch.zeros(BATCH_SIZE), + ) + + acknowledgement = adapter.send(command, timeout=1.0) + + assert acknowledgement.accepted + assert torch.equal( + robot.set_qvel.call_args.args[0], + torch.tensor([[0.1, 0.3], [0.0, 0.0]]), + ) + assert robot.set_qvel.call_args.kwargs == { + "joint_ids": [0, 2], + "env_ids": [0, 1], + } def test_simulation_adapter_send_writes_a_pure_hold_batch() -> None: @@ -129,8 +228,18 @@ def test_simulation_adapter_send_writes_a_pure_hold_batch() -> None: acknowledgement = adapter.send(command, timeout=1.0) assert acknowledgement.status is CommandAckStatus.ACCEPTED - robot.set_qpos.assert_called_once_with(command.positions, env_ids=[0, 1]) - robot.set_qvel.assert_called_once_with(command.velocities, env_ids=[0, 1]) + assert torch.equal( + robot.set_qpos.call_args.args[0], + torch.zeros(BATCH_SIZE, ROBOT_DOF), + ) + assert torch.equal( + robot.set_qvel.call_args.args[0], + torch.zeros(BATCH_SIZE, ROBOT_DOF), + ) + assert robot.set_qpos.call_args.kwargs["env_ids"] == [0, 1] + assert robot.set_qpos.call_args.kwargs["joint_ids"] == [0, 1, 2] + assert robot.set_qvel.call_args.kwargs["env_ids"] == [0, 1] + assert robot.set_qvel.call_args.kwargs["joint_ids"] == [0, 1, 2] def test_simulation_adapter_keeps_stable_ids_separate_from_robot_indices() -> None: @@ -147,14 +256,70 @@ def test_simulation_adapter_keeps_stable_ids_separate_from_robot_indices() -> No def test_simulation_adapter_hold_targets_every_environment() -> None: simulation, robot = _simulation_and_robot() + observed_positions = torch.full((BATCH_SIZE, ROBOT_DOF), 0.25) + robot.get_qpos.return_value = observed_positions adapter = SimulationExecutionAdapter(simulation, robot) command = _command() + context = adapter.observe(TaskState.empty(BATCH_SIZE, "cpu")) + + acknowledgement = adapter.hold(command.targets, context, timeout=1.0) + + assert acknowledgement.status is CommandAckStatus.ACCEPTED + assert torch.equal(robot.set_qpos.call_args.args[0], observed_positions) + assert torch.equal( + robot.set_qvel.call_args.args[0], + torch.zeros_like(observed_positions), + ) + assert robot.set_qpos.call_args.kwargs["env_ids"] == [0, 1] + assert robot.set_qpos.call_args.kwargs["joint_ids"] == [0, 1, 2] + assert robot.set_qvel.call_args.kwargs["env_ids"] == [0, 1] + assert robot.set_qvel.call_args.kwargs["joint_ids"] == [0, 1, 2] + + +def test_simulation_adapter_hold_scopes_write_to_target_joint_ids() -> None: + simulation, robot = _simulation_and_robot() + observed_positions = torch.tensor([[0.1, 0.2, 0.3], [0.4, 0.5, 0.6]]) + robot.get_qpos.return_value = observed_positions + adapter = SimulationExecutionAdapter(simulation, robot) + context = adapter.observe(TaskState.empty(BATCH_SIZE, "cpu")) + + acknowledgement = adapter.hold( + (JointPositionTarget("tool", (1,)),), + context, + timeout=1.0, + ) + + assert acknowledgement.status is CommandAckStatus.ACCEPTED + assert torch.equal( + robot.set_qpos.call_args.args[0], + torch.tensor([[0.2], [0.5]]), + ) + assert robot.set_qpos.call_args.kwargs == { + "joint_ids": [1], + "env_ids": [0, 1], + } + assert torch.equal(robot.set_qvel.call_args.args[0], torch.zeros(BATCH_SIZE, 1)) - acknowledgement = adapter.hold(command, timeout=1.0) + +def test_simulation_adapter_cancel_validates_transport_targets() -> None: + simulation, robot = _simulation_and_robot() + adapter = SimulationExecutionAdapter(simulation, robot) + targets = _command().targets + + acknowledgement = adapter.cancel(targets, timeout=1.0) assert acknowledgement.status is CommandAckStatus.ACCEPTED - robot.set_qpos.assert_called_once_with(command.positions, env_ids=[0, 1]) - robot.set_qvel.assert_called_once_with(command.velocities, env_ids=[0, 1]) + assert [(target.transport_id, target.target_id) for target in targets] == [ + (JointPositionTarget.TRANSPORT_ID, "arm") + ] + robot.set_qpos.assert_not_called() + + invalid = adapter.cancel( + (JointPositionTarget("invalid", (ROBOT_DOF,)),), + timeout=1.0, + ) + assert invalid.status is CommandAckStatus.REJECTED + assert "outside robot DOF" in invalid.message def test_simulation_adapter_sleep_advances_integral_physics_steps() -> None: diff --git a/tests/sim/atomic_actions/test_trajectory_ops.py b/tests/sim/atomic_actions/test_trajectory_ops.py index 47e1b3cf6..a23384834 100644 --- a/tests/sim/atomic_actions/test_trajectory_ops.py +++ b/tests/sim/atomic_actions/test_trajectory_ops.py @@ -93,7 +93,7 @@ def unexpected_current_device(): with pytest.raises(ValueError, match="CUDA device requested"): normalize_success_mask( True, - n_envs=2, + num_envs=2, device="cuda", name="IK success", ) diff --git a/tests/sim/atomic_actions/test_transports.py b/tests/sim/atomic_actions/test_transports.py new file mode 100644 index 000000000..32b74c3a2 --- /dev/null +++ b/tests/sim/atomic_actions/test_transports.py @@ -0,0 +1,522 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Pure routing tests for endpoint-command transports.""" + +from __future__ import annotations + +from dataclasses import dataclass + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions.bindings import RuntimeEndpointTarget +from embodichain.lab.sim.atomic_actions.runner import ( + CommandAcknowledgement, + CommandAckStatus, + CommandSink, +) +from embodichain.lab.sim.atomic_actions.runtime_commands import ( + EndpointCommand, + RuntimeCommandFrame, + RuntimeCommandPayload, +) +from embodichain.lab.sim.atomic_actions.transports import ( + EndpointCommandRouter, + EndpointCommandTransport, +) + + +@dataclass(frozen=True, slots=True) +class _Target(RuntimeEndpointTarget): + """Test-only runtime target.""" + + _transport_id: str + _target_id: str + + @property + def transport_id(self) -> str: + """Return the addressed transport.""" + return self._transport_id + + @property + def target_id(self) -> str: + """Return the local destination.""" + return self._target_id + + +@dataclass(frozen=True, slots=True) +class _Payload(RuntimeCommandPayload): + """Test-only payload with transport-neutral scalar data.""" + + _transport_id: str + values: torch.Tensor + + @property + def batch_size(self) -> int: + """Return the payload batch size.""" + return int(self.values.shape[0]) + + @property + def device(self) -> torch.device: + """Return the payload device.""" + return self.values.device + + @property + def transport_id(self) -> str: + """Return the addressed transport.""" + return self._transport_id + + def snapshot(self) -> _Payload: + """Return an independently owned payload.""" + return _Payload(self._transport_id, self.values.clone()) + + +@dataclass(frozen=True, slots=True) +class _OtherPayload(RuntimeCommandPayload): + """Different payload type used to exercise compatibility checks.""" + + _transport_id: str + values: torch.Tensor + + @property + def batch_size(self) -> int: + """Return the payload batch size.""" + return int(self.values.shape[0]) + + @property + def device(self) -> torch.device: + """Return the payload device.""" + return self.values.device + + @property + def transport_id(self) -> str: + """Return the addressed transport.""" + return self._transport_id + + def snapshot(self) -> _OtherPayload: + """Return an independently owned payload.""" + return _OtherPayload(self._transport_id, self.values.clone()) + + +class _FakeTransport: + """Recording transport with configurable acknowledgements.""" + + def __init__( + self, + transport_id: str, + *, + payload_type: type[RuntimeCommandPayload] = _Payload, + ) -> None: + self._transport_id = transport_id + self._payload_type = payload_type + self.send_ack: object = CommandAcknowledgement.accepted_ack() + self.hold_ack: object = CommandAcknowledgement.accepted_ack() + self.cancel_ack: object = CommandAcknowledgement.accepted_ack() + self.send_error: Exception | None = None + self.hold_error: Exception | None = None + self.cancel_error: Exception | None = None + self.send_calls: list[tuple[RuntimeCommandFrame, float]] = [] + self.hold_calls: list[ + tuple[tuple[RuntimeEndpointTarget, ...], object, float] + ] = [] + self.cancel_calls: list[tuple[tuple[RuntimeEndpointTarget, ...], float]] = [] + + @property + def transport_id(self) -> str: + """Return the fake registration identifier.""" + return self._transport_id + + @property + def payload_type(self) -> type[RuntimeCommandPayload]: + """Return the accepted fake payload type.""" + return self._payload_type + + def send( + self, + frame: RuntimeCommandFrame, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Record one transport-local frame.""" + self.send_calls.append((frame, timeout)) + if self.send_error is not None: + raise self.send_error + return self.send_ack # type: ignore[return-value] + + def hold( + self, + targets: tuple[RuntimeEndpointTarget, ...], + context: object, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Record one transport-local hold.""" + self.hold_calls.append((targets, context, timeout)) + if self.hold_error is not None: + raise self.hold_error + return self.hold_ack # type: ignore[return-value] + + def cancel( + self, + targets: tuple[RuntimeEndpointTarget, ...], + *, + timeout: float, + ) -> CommandAcknowledgement: + """Record one transport-local cancellation.""" + self.cancel_calls.append((targets, timeout)) + if self.cancel_error is not None: + raise self.cancel_error + return self.cancel_ack # type: ignore[return-value] + + +def _command( + transport_id: str, + target_id: str, + *, + payload_type: type[RuntimeCommandPayload] = _Payload, +) -> EndpointCommand: + """Build one two-row endpoint command.""" + return EndpointCommand( + target=_Target(transport_id, target_id), + payload=payload_type( # type: ignore[call-arg] + transport_id, + torch.tensor([[1.0], [2.0]]), + ), + ) + + +def _frame(*commands: EndpointCommand) -> RuntimeCommandFrame: + """Build one two-row command frame.""" + return RuntimeCommandFrame( + commands=commands, + active_mask=torch.tensor([True, False]), + env_ids=torch.tensor([3, 8]), + hold_duration=torch.tensor([0.1, 0.2]), + ) + + +def test_transport_protocol_is_runtime_checkable() -> None: + assert isinstance(_FakeTransport("alpha"), EndpointCommandTransport) + assert not isinstance(object(), EndpointCommandTransport) + + +def test_router_structurally_implements_command_sink() -> None: + assert isinstance(EndpointCommandRouter([]), CommandSink) + + +def test_router_builds_owned_exact_registry_from_mapping() -> None: + alpha = _FakeTransport("alpha") + registrations = {"alpha": alpha} + router = EndpointCommandRouter(registrations) + + registrations.clear() + assert dict(router.transports) == {"alpha": alpha} + with pytest.raises(TypeError): + router.transports["beta"] = _FakeTransport("beta") # type: ignore[index] + + +def test_router_rejects_non_exact_mapping_key() -> None: + with pytest.raises(ValueError, match="exactly match"): + EndpointCommandRouter({"alias": _FakeTransport("alpha")}) + + +def test_router_rejects_duplicate_transport_registration() -> None: + with pytest.raises(ValueError, match="more than once"): + EndpointCommandRouter([_FakeTransport("alpha"), _FakeTransport("alpha")]) + + +def test_router_rejects_invalid_transport_contract_and_payload_type() -> None: + with pytest.raises(TypeError, match="EndpointCommandTransport"): + EndpointCommandRouter([object()]) # type: ignore[list-item] + + invalid_payload = _FakeTransport("alpha") + invalid_payload._payload_type = str # type: ignore[assignment] + with pytest.raises(TypeError, match="payload_type"): + EndpointCommandRouter([invalid_payload]) + + +def test_send_groups_subframes_and_preserves_frame_metadata() -> None: + alpha = _FakeTransport("alpha") + beta = _FakeTransport("beta") + router = EndpointCommandRouter({"alpha": alpha, "beta": beta}) + frame = _frame( + _command("alpha", "a0"), + _command("beta", "b0"), + _command("alpha", "a1"), + ) + + acknowledgement = router.send(frame, timeout=0.75) + + assert acknowledgement.accepted + assert len(alpha.send_calls) == 1 + assert len(beta.send_calls) == 1 + alpha_frame, alpha_timeout = alpha.send_calls[0] + beta_frame, beta_timeout = beta.send_calls[0] + assert [command.target.target_id for command in alpha_frame.commands] == [ + "a0", + "a1", + ] + assert [command.target.target_id for command in beta_frame.commands] == ["b0"] + assert torch.equal(alpha_frame.active_mask, frame.active_mask) + assert torch.equal(alpha_frame.env_ids, frame.env_ids) + assert torch.equal(alpha_frame.hold_duration, frame.hold_duration) + assert alpha_frame.active_mask.data_ptr() != frame.active_mask.data_ptr() + assert alpha_timeout == beta_timeout == 0.75 + + +def test_send_unknown_transport_rejects_before_any_dispatch() -> None: + alpha = _FakeTransport("alpha") + router = EndpointCommandRouter([alpha]) + + acknowledgement = router.send( + _frame(_command("alpha", "a0"), _command("missing", "x0")), + timeout=1.0, + ) + + assert acknowledgement.status is CommandAckStatus.REJECTED + assert "missing" in acknowledgement.message + assert alpha.send_calls == [] + + +def test_send_incompatible_payload_rejects_before_dispatch() -> None: + alpha = _FakeTransport("alpha", payload_type=_Payload) + router = EndpointCommandRouter([alpha]) + + acknowledgement = router.send( + _frame(_command("alpha", "a0", payload_type=_OtherPayload)), + timeout=1.0, + ) + + assert acknowledgement.status is CommandAckStatus.REJECTED + assert "alpha" in acknowledgement.message + assert "_Payload" in acknowledgement.message + assert "_OtherPayload" in acknowledgement.message + assert alpha.send_calls == [] + + +def test_send_aggregates_partial_rejection_with_transport_id() -> None: + alpha = _FakeTransport("alpha") + beta = _FakeTransport("beta") + alpha.send_ack = CommandAcknowledgement.accepted_ack("queued") + beta.send_ack = CommandAcknowledgement( + CommandAckStatus.REJECTED, + "controller busy", + ) + router = EndpointCommandRouter([alpha, beta]) + + acknowledgement = router.send( + _frame(_command("alpha", "a0"), _command("beta", "b0")), + timeout=1.0, + ) + + assert acknowledgement.status is CommandAckStatus.REJECTED + assert "beta" in acknowledgement.message + assert "controller busy" in acknowledgement.message + assert len(alpha.send_calls) == len(beta.send_calls) == 1 + + +def test_send_timed_out_status_takes_failure_precedence() -> None: + alpha = _FakeTransport("alpha") + beta = _FakeTransport("beta") + alpha.send_ack = CommandAcknowledgement(CommandAckStatus.REJECTED, "rejected") + beta.send_ack = CommandAcknowledgement(CommandAckStatus.TIMED_OUT, "late") + router = EndpointCommandRouter([alpha, beta]) + + acknowledgement = router.send( + _frame(_command("alpha", "a0"), _command("beta", "b0")), + timeout=1.0, + ) + + assert acknowledgement.status is CommandAckStatus.TIMED_OUT + assert "alpha" in acknowledgement.message + assert "beta" in acknowledgement.message + + +def test_send_converts_transport_exception_and_continues_dispatch() -> None: + alpha = _FakeTransport("alpha") + beta = _FakeTransport("beta") + alpha.send_error = RuntimeError("send exploded") + router = EndpointCommandRouter([alpha, beta]) + + acknowledgement = router.send( + _frame(_command("alpha", "a0"), _command("beta", "b0")), + timeout=1.0, + ) + + assert acknowledgement.status is CommandAckStatus.REJECTED + assert "alpha" in acknowledgement.message + assert "RuntimeError" in acknowledgement.message + assert "send exploded" in acknowledgement.message + assert len(alpha.send_calls) == len(beta.send_calls) == 1 + + +def test_hold_groups_targets_and_forwards_observation_context() -> None: + alpha = _FakeTransport("alpha") + beta = _FakeTransport("beta") + router = EndpointCommandRouter([alpha, beta]) + context = object() + + acknowledgement = router.hold( + ( + _Target("alpha", "a0"), + _Target("beta", "b0"), + _Target("alpha", "a1"), + ), + context, # type: ignore[arg-type] + timeout=0.4, + ) + + assert acknowledgement.accepted + alpha_targets, alpha_context, alpha_timeout = alpha.hold_calls[0] + beta_targets, beta_context, beta_timeout = beta.hold_calls[0] + assert [target.target_id for target in alpha_targets] == ["a0", "a1"] + assert [target.target_id for target in beta_targets] == ["b0"] + assert alpha_context is beta_context is context + assert alpha_timeout == beta_timeout == 0.4 + + +def test_cancel_groups_targets_and_aggregates_partial_failure() -> None: + alpha = _FakeTransport("alpha") + beta = _FakeTransport("beta") + beta.cancel_ack = CommandAcknowledgement(CommandAckStatus.TIMED_OUT, "late") + router = EndpointCommandRouter([alpha, beta]) + + acknowledgement = router.cancel( + ( + _Target("beta", "b0"), + _Target("alpha", "a0"), + _Target("beta", "b1"), + ), + timeout=0.2, + ) + + assert acknowledgement.status is CommandAckStatus.TIMED_OUT + assert "beta" in acknowledgement.message + assert [target.target_id for target in beta.cancel_calls[0][0]] == ["b0", "b1"] + assert [target.target_id for target in alpha.cancel_calls[0][0]] == ["a0"] + + +@pytest.mark.parametrize("operation", ["hold", "cancel"]) +def test_safe_stop_transport_exception_does_not_block_later_transport( + operation: str, +) -> None: + alpha = _FakeTransport("alpha") + beta = _FakeTransport("beta") + setattr(alpha, f"{operation}_error", RuntimeError(f"{operation} exploded")) + router = EndpointCommandRouter([alpha, beta]) + targets = (_Target("alpha", "a0"), _Target("beta", "b0")) + + if operation == "hold": + acknowledgement = router.hold( + targets, + object(), # type: ignore[arg-type] + timeout=1.0, + ) + alpha_calls = alpha.hold_calls + beta_calls = beta.hold_calls + else: + acknowledgement = router.cancel(targets, timeout=1.0) + alpha_calls = alpha.cancel_calls + beta_calls = beta.cancel_calls + + assert acknowledgement.status is CommandAckStatus.REJECTED + assert "alpha" in acknowledgement.message + assert "RuntimeError" in acknowledgement.message + assert f"{operation} exploded" in acknowledgement.message + assert len(alpha_calls) == len(beta_calls) == 1 + + +@pytest.mark.parametrize("operation", ["hold", "cancel"]) +def test_target_operation_unknown_transport_rejects_before_dispatch( + operation: str, +) -> None: + alpha = _FakeTransport("alpha") + router = EndpointCommandRouter([alpha]) + if operation == "hold": + acknowledgement = router.hold( + (_Target("missing", "x0"),), + object(), # type: ignore[arg-type] + timeout=1.0, + ) + calls = alpha.hold_calls + else: + acknowledgement = router.cancel( + (_Target("missing", "x0"),), + timeout=1.0, + ) + calls = alpha.cancel_calls + + assert acknowledgement.status is CommandAckStatus.REJECTED + assert "missing" in acknowledgement.message + assert calls == [] + + +@pytest.mark.parametrize("operation", ["send", "hold", "cancel"]) +def test_router_converts_invalid_return_type_and_continues_dispatch( + operation: str, +) -> None: + alpha = _FakeTransport("alpha") + beta = _FakeTransport("beta") + setattr(alpha, f"{operation}_ack", object()) + router = EndpointCommandRouter([alpha, beta]) + + if operation == "send": + acknowledgement = router.send( + _frame(_command("alpha", "a0"), _command("beta", "b0")), + timeout=1.0, + ) + beta_calls = beta.send_calls + elif operation == "hold": + acknowledgement = router.hold( + (_Target("alpha", "a0"), _Target("beta", "b0")), + object(), # type: ignore[arg-type] + timeout=1.0, + ) + beta_calls = beta.hold_calls + else: + acknowledgement = router.cancel( + (_Target("alpha", "a0"), _Target("beta", "b0")), + timeout=1.0, + ) + beta_calls = beta.cancel_calls + + assert acknowledgement.status is CommandAckStatus.REJECTED + assert "alpha" in acknowledgement.message + assert "CommandAcknowledgement" in acknowledgement.message + assert len(beta_calls) == 1 + + +@pytest.mark.parametrize("timeout", [0.0, -1.0, float("inf"), float("nan")]) +@pytest.mark.parametrize("operation", ["send", "hold", "cancel"]) +def test_router_rejects_invalid_timeout(operation: str, timeout: float) -> None: + router = EndpointCommandRouter([]) + + with pytest.raises(ValueError, match="timeout"): + if operation == "send": + router.send(_frame(), timeout=timeout) + elif operation == "hold": + router.hold((), object(), timeout=timeout) # type: ignore[arg-type] + else: + router.cancel((), timeout=timeout) + + +def test_empty_operations_are_accepted() -> None: + router = EndpointCommandRouter([]) + + assert router.send(_frame(), timeout=1.0).accepted + assert router.hold((), object(), timeout=1.0).accepted # type: ignore[arg-type] + assert router.cancel((), timeout=1.0).accepted diff --git a/tests/sim/planners/test_curobo_planner.py b/tests/sim/planners/test_curobo_planner.py index b1f5ec92e..8cfd6e2cd 100644 --- a/tests/sim/planners/test_curobo_planner.py +++ b/tests/sim/planners/test_curobo_planner.py @@ -928,7 +928,6 @@ def _make_curobo_engine( def test_curobo_reuses_non_graph_backend(): from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, EndEffectorPoseGoal, MotionPolicy, @@ -939,13 +938,17 @@ def test_curobo_reuses_non_graph_backend(): try: engine = _make_curobo_engine(block) target = _target_beyond_block(robot) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": _SIM_CONTROL_PART}}, + ) result = engine.compile( ( ActionInvocation( "move_end_effector", EndEffectorPoseGoal(xpos=target), - ActionBinding(manipulators={"primary": _SIM_CONTROL_PART}), + binding, MotionPolicy(strategy="motion_gen", sample_count=80), ), ) @@ -964,7 +967,7 @@ def test_curobo_reuses_non_graph_backend(): ActionInvocation( "move_end_effector", EndEffectorPoseGoal(xpos=target), - ActionBinding(manipulators={"primary": _SIM_CONTROL_PART}), + binding, MotionPolicy(strategy="motion_gen", sample_count=80), ), ) @@ -982,7 +985,6 @@ def test_curobo_reuses_non_graph_backend(): def test_curobo_uses_accelerator_with_cpu_physics(): from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionBinding, ActionInvocation, EndEffectorPoseGoal, MotionPolicy, @@ -993,13 +995,17 @@ def test_curobo_uses_accelerator_with_cpu_physics(): try: engine = _make_curobo_engine(block, use_cuda_graph=True) target = _target_beyond_block(robot) + binding = engine.bind_control_parts( + "move_end_effector", + {"primary": {"motion": _SIM_CONTROL_PART}}, + ) result = engine.compile( ( ActionInvocation( "move_end_effector", EndEffectorPoseGoal(xpos=target), - ActionBinding(manipulators={"primary": _SIM_CONTROL_PART}), + binding, MotionPolicy(strategy="motion_gen", sample_count=80), ), ) diff --git a/tests/sim/skills/test_profiles.py b/tests/sim/skills/test_profiles.py index 0f7be1fb8..2082c35fa 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: @@ -1190,7 +1391,6 @@ class Replacement(action_type): SkillEndpointRequirement( "motion", capabilities=frozenset({JOINT_POSITION_CAPABILITY}), - route=ActionBindingRoute("manipulator", "primary"), ), ), ), From 623a064c7d70751fb50236fd6f81bc2ca4c26e0b Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 19 Aug 2026 09:13:06 +0000 Subject: [PATCH 2/5] refactor(atomic-actions)!: require explicit trajectory timing Remove fallback control periods from motion policies and the atomic action engine. Require planners and custom actions to provide validated timing, propagate explicit interpolation cadence through planning contexts, and update built-ins, tasks, benchmarks, tutorials, docs, and tests. BREAKING CHANGE: planner results with positions must include dt and duration, custom atomic actions must return TimedTrajectory, and fallback_control_dt is removed. --- agent_context/MAP.yaml | 9 + .../topics/atomic-actions/atomic-actions.md | 44 +++-- .../topics/motion-planning/motion-planning.md | 19 +- .../design/declarative_expert_program_plan.md | 28 ++- .../sim/atomic_actions/builtin_actions.md | 13 +- .../overview/sim/atomic_actions/index.md | 21 ++- .../atomic_actions/robot_skill_profiles.md | 5 + .../overview/sim/planners/motion_generator.md | 25 ++- docs/source/tutorial/atomic_actions.rst | 32 +++- docs/source/tutorial/motion_gen.rst | 5 +- embodichain/lab/sim/atomic_actions/core.py | 25 +-- embodichain/lab/sim/atomic_actions/engine.py | 3 + .../lab/sim/atomic_actions/execution.py | 2 + embodichain/lab/sim/atomic_actions/plans.py | 99 ++++++----- .../lab/sim/atomic_actions/policies.py | 24 +-- .../sim/atomic_actions/primitives/_helpers.py | 2 + .../primitives/coordinated_pickment.py | 8 +- .../primitives/coordinated_placement.py | 11 +- .../atomic_actions/primitives/hand_over.py | 12 +- .../primitives/move_end_effector.py | 2 +- .../primitives/move_held_object.py | 10 +- .../atomic_actions/primitives/move_joints.py | 2 +- .../sim/atomic_actions/primitives/pick_up.py | 12 +- .../sim/atomic_actions/primitives/place.py | 10 +- .../sim/atomic_actions/primitives/press.py | 10 +- .../lab/sim/atomic_actions/sim_adapter.py | 11 ++ embodichain/lab/sim/atomic_actions/state.py | 25 +++ .../lab/sim/atomic_actions/trajectory_ops.py | 12 +- embodichain/lab/sim/planners/base_planner.py | 4 + .../lab/sim/planners/motion_generator.py | 92 ++++++++-- .../lab/sim/planners/neural_planner.py | 9 +- embodichain/lab/sim/planners/utils.py | 50 +++++- embodichain/lab/sim/skills/profiles.py | 23 ++- .../multi_segments/cube_pick_place.py | 3 +- .../tableware/blocks_ranking_rgb.py | 3 +- .../tableware/stack_blocks_two.py | 3 +- examples/sim/planners/curobo_planner.py | 2 +- .../move_held_object_benchmark.py | 3 +- .../atomic_action/pickup_benchmark.py | 3 +- .../atomic_action/place_benchmark.py | 3 +- .../atomic_action/press_benchmark.py | 5 +- .../planners/ik_interpolate.py | 18 +- .../motion_generation/suites/coverage.yaml | 3 +- .../motion_generation/suites/smoke.yaml | 3 +- scripts/tutorials/atomic_action/assemble.py | 3 +- scripts/tutorials/atomic_action/control_dt.py | 168 ++++++++++++++++++ .../atomic_action/coordinated_pickment.py | 3 +- .../atomic_action/coordinated_placement.py | 2 +- .../dynamic_obstacle_recovery.py | 8 +- scripts/tutorials/atomic_action/hand_over.py | 3 +- .../atomic_action/move_end_effector.py | 3 +- .../atomic_action/move_held_object.py | 3 +- .../tutorials/atomic_action/move_joints.py | 3 +- .../atomic_action/moving_target_recovery.py | 2 +- scripts/tutorials/atomic_action/pickup.py | 3 +- scripts/tutorials/atomic_action/place.py | 3 +- scripts/tutorials/atomic_action/press.py | 3 +- .../test_motion_generation_benchmark.py | 25 ++- tests/sim/atomic_actions/test_actions.py | 7 + tests/sim/atomic_actions/test_core.py | 69 +++++-- tests/sim/atomic_actions/test_engine.py | 21 ++- .../sim/atomic_actions/test_engine_per_env.py | 18 +- tests/sim/atomic_actions/test_runner.py | 5 +- .../sim/atomic_actions/test_tutorial_utils.py | 1 - .../planners/test_motion_generator_batched.py | 72 ++++++-- tests/sim/planners/test_plan_state_batched.py | 23 ++- tests/sim/skills/test_profiles.py | 8 +- 67 files changed, 903 insertions(+), 261 deletions(-) create mode 100644 scripts/tutorials/atomic_action/control_dt.py diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index df7ac14aa..84562cb08 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -293,6 +293,9 @@ topics: - motion generator - resolve_plan_options - MotionGenOptions.strategy + - MotionGenOptions.interpolation_dt + - PlanResult.dt + - explicit trajectory timing - ik_interp - path - waypoint @@ -316,6 +319,8 @@ topics: - embodichain/lab/sim/planners/curobo/curobo_planner.py - embodichain/lab/sim/planners/curobo/curobo_yaml.py - embodichain/lab/sim/planners/motion_generator.py + - embodichain/lab/sim/planners/neural_planner.py + - embodichain/lab/sim/planners/utils.py - embodichain/lab/sim/skills/scene.py related_topics: - robot-system @@ -532,6 +537,7 @@ topics: - ResolvedRobotResource - ResolvedSkillBinding - SkillPolicyPreset + - SkillPolicyPreset.required_planner - binding_contract - engine.skills - skill_profile @@ -555,6 +561,9 @@ topics: - invocation revision - MotionPolicy - MotionPolicy.strategy + - PlanningContext.control_dt + - explicit trajectory timing + - TimedTrajectory.from_uniform_step - RecoveryPolicy - MotionGenOptions.strategy - motion_gen diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 06bcf00c1..af20e838d 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -16,13 +16,14 @@ There is no `ActionTarget`, `WorldState`, `ActionResult`, `execute()`, or - 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; -- reusable `MotionPolicy` planner/timing choices; +- 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. `PlanningContext` separates measured `RobotObservation`, verified symbolic -`TaskState`, versioned `SceneSnapshot`, and environment IDs. An `ActionPlan` +`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 @@ -38,14 +39,19 @@ 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. `MotionGenerator.generate()` is the only -stateful motion-planning entry point. `MotionPolicy.to_motion_gen_options()` -passes the invocation's `strategy` directly into `MotionGenOptions`; it is either -`"motion_gen"` or `"ik_interp"`. Target shaping, world-frame pose translation, -hand/joint interpolation used by composite actions, and full-robot trajectory -embedding are pure functions in `trajectory_ops.py`. Actions retain only an -owned copy of typed default options and borrow engine services. Engine -construction creates and binds a fresh instance of every type in +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 +`build_plan()`. Environment-backed integrations put `BaseEnv.step_dt` on +`PlanningContext.control_dt` when action-owned interpolation needs a cadence. +`MotionGenerator.generate()` is the only stateful motion-planning entry point. +`MotionPolicy.to_motion_gen_options()` passes the invocation's `strategy` +directly into `MotionGenOptions`; it is either `"motion_gen"` or `"ik_interp"`. +Target shaping, world-frame pose translation, hand/joint interpolation used by +composite actions, and full-robot trajectory embedding are pure functions in +`trajectory_ops.py`. Actions retain only an owned copy of typed default options +and borrow engine services. Engine construction creates and binds a fresh +instance of every type in `BUILTIN_ACTION_TYPES`; use `load_builtins=False` only for isolated tests or a fully custom action set. A bound action cannot be reused by another engine. @@ -474,8 +480,14 @@ Collision-world revisions must also remain monotonic per environment. Goal dataclasses carry only semantic task intent. They do not carry robot part names, planner configuration, retry policy, or runtime state. -`MotionPolicy` owns planner selection, motion strategy, sample count, fallback -control period, limits, dynamic-collision mode, and typed planner options. +`MotionPolicy` owns motion strategy, sample count, dynamic-collision mode, and +typed planner options. Optional planner-backend compatibility belongs to +`SkillPolicyPreset.required_planner`; velocity and acceleration constraints +belong to the selected backend's typed `PlanOptions`. Timing belongs to the +trajectory producer: planners return explicit `dt`/`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 @@ -586,9 +598,13 @@ snapshot-grounded object example. 7. If planning consumes a semantic object's snapshot pose, override `_scene_dependencies()`, preserve `super()` dependencies, and add exactly that semantic ID. -8. Return full-robot positions or a `TimedTrajectory` through `build_plan()`. +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 - `request.motion_policy.to_motion_gen_options()`, and call + `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`. 9. Declare symbolic changes with `StateDelta`; do not mutate context or commit diff --git a/agent_context/topics/motion-planning/motion-planning.md b/agent_context/topics/motion-planning/motion-planning.md index b5edd4a8b..3502ce686 100644 --- a/agent_context/topics/motion-planning/motion-planning.md +++ b/agent_context/topics/motion-planning/motion-planning.md @@ -181,7 +181,8 @@ Unified interface for trajectory planning with optional pre-interpolation. - `MotionGenCfg.planner_cfg` is **MISSING** — must be provided. - `generate()` and `interpolate_trajectory()` are env-batched (`B, N, DOF`). - `generate()` always returns a normalized `PlanResult`; failed rows hold the - supplied `start_qpos`. + supplied `start_qpos`, and every returned trajectory has explicit `dt` and + matching `duration`. `MotionGenOptions` fields: @@ -195,6 +196,7 @@ Unified interface for trajectory planning with optional pre-interpolation. | `control_part` | `str \| None` | `None` | Robot control part name (must match `RobotCfg.control_parts` key) | | `plan_opts` | `PlanOptions \| None` | `None` | Passed to the underlying planner | | `is_interpolate` | `bool` | `False` | Pre-interpolate waypoints before planning | +| `interpolation_dt` | `float \| None` | `None` | Required explicit waypoint interval for `strategy="ik_interp"` and automatic joint interpolation fallback | | `interpolate_nums` | `int \| list[int]` | `10` | Points per segment (scalar or per-segment list) | | `is_linear` | `bool` | `False` | `True` = Cartesian linear interpolation; `False` = joint-space | | `interpolate_position_step` | `float` | `0.002` | Cartesian step size (meters) or joint step size (radians) | @@ -231,10 +233,14 @@ Convenience constructors: | `positions` | `torch.Tensor \| None` | Joint positions `(B, N, DOF)` | | `velocities` | `torch.Tensor \| None` | Joint velocities `(B, N, DOF)` | | `accelerations` | `torch.Tensor \| None` | Joint accelerations `(B, N, DOF)` | -| `dt` | `torch.Tensor \| None` | Per-step time durations `(B, N)` | -| `duration` | `float \| torch.Tensor` | Total trajectory time per env `(B,)` | +| `dt` | `torch.Tensor \| None` | Per-step arrival intervals `(B, N)`; required whenever `positions` is present | +| `duration` | `torch.Tensor \| None` | Total trajectory time `(B,)`; required with `positions` and equal to `dt.sum(dim=1)` | Helper: `PlanResult.is_all_success() -> bool` returns `True` only when every env succeeded. +`PlanResult` rejects positions with missing, malformed, or inconsistent timing. +A failed result may omit the trajectory entirely by leaving `positions=None`. +When `MotionGenerator` resamples a fully timed result, it preserves each row's +total duration and emits new explicit arrival intervals. ### MoveType enum @@ -260,7 +266,7 @@ Helper: `PlanResult.is_all_success() -> bool` returns `True` only when every env ### Registering a new planner -1. Create a `BasePlanner` subclass with a `plan()` method decorated with `@validate_plan_options`. +1. Create a `BasePlanner` subclass with a `plan()` method decorated with `@validate_plan_options`; every result containing positions must include `dt` and matching `duration`. 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 @@ -299,7 +305,10 @@ The decorator checks that every `PlanState` in `target_states` shares the same l - **IK interpolation with unsupported MoveType** — `strategy="ik_interp"` accepts only `EEF_MOVE` and `JOINT_MOVE` and raises for other target types. - **Missing interpolation inputs** — `strategy="ik_interp"` requires explicit - `start_qpos` and `sample_count`; it never reads live robot state implicitly. + `start_qpos`, `sample_count`, and `interpolation_dt`; it never reads live robot + state or guesses a command period implicitly. +- **Missing planner timing** — constructing a `PlanResult` with positions but + without matching `dt` and `duration` raises immediately. - **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. diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index 419d67bd4..6f2701125 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -93,11 +93,12 @@ sessions, or verifiers. ## 4. Baseline on current `main` -This plan is updated against committed `main@f4ffb660`. PR #517 simplified the -atomic-action core, and PR #487 landed the complete Phase 1 foundation. The -scene registry and robot skill profile APIs are available, but official task -environments have not adopted them yet; that rollout starts only after the -semantic compiler and runtime exist. +This plan is updated against committed `main@dbc6553f`. PR #517 simplified the +atomic-action core, PR #487 landed the complete Phase 1 foundation, and PR #523 +simplified that foundation without changing its contracts. The scene registry +and robot skill profile APIs are available, but official task environments have +not adopted them yet; that rollout starts only after the semantic compiler and +runtime exist. | Capability | Current main | Design consequence | |---|---|---| @@ -106,10 +107,10 @@ semantic compiler and runtime exist. | Closed-loop `ExecutionRunner` and simulator ports (#449) | Available | `SkillRuntime` wraps/reuses the runner rather than scheduling commands itself. | | Dynamic scene recovery and `DynamicCollisionMode` (#450) | Available | Profiles select precise collision semantics and fail early when required capabilities are unavailable. | | Refined planning architecture (#475) | `MotionGenerator.generate()` is the single planning facade; each `ActionPlan` owns one trajectory and one recovery boundary; named `TrajectorySegment`s are metadata | Do not reintroduce `TrajectoryBuilder`, `MotionPlanningAdapter`, or trajectory-segment recovery. | -| Environment cadence through `BaseEnv.step_dt` (#472) | Available | Expert configuration does not expose a separate control period. | +| Environment cadence through `BaseEnv.step_dt` (#472) | Available; planner and action trajectories now require explicit timing | Expert configuration, `MotionPolicy`, and the engine do not own a fallback period. Environment integrations put `BaseEnv.step_dt` on `PlanningContext` only for action-owned interpolation. | | Adaptive dynamic-object settling (#470) | Reset/event implementation exists | Extract a reusable monitor; demo post-policies must advance through `env.step()`. | | Authoritative scene registry (#487) | Foundation available; official environments not migrated | Reuse it from the semantic compiler and opt in task scenes explicitly. | -| Declarative robot skill profiles (#487) | Foundation available; official profiles not yet installed | Bind reusable embodiment profiles through the semantic integration layer. | +| Declarative robot skill profiles (#487) | Foundation available; official profiles not yet installed | Bind reusable embodiment profiles through the semantic integration layer; put optional backend compatibility in `SkillPolicyPreset.required_planner`, not `MotionPolicy`. | | Repeated cube pick/place demo | Manually constructs invocations and transform math | First configuration-only vertical slice. | | Open Drawer task (#473) | Manually builds approach, grasp, pull, and command trajectories | Evidence that the semantic layer needs articulation/link/affordance references and a reusable articulation skill. | | Action Bank | Configuration plus task-specific Python node/edge functions | Keep only as a compatibility path while semantic coverage is built. | @@ -404,7 +405,8 @@ an `arm + tool` schema. It contains a generic resource DAG: embodiment data owned by generic profile IDs selected by each endpoint adapter; only the current core bridge lowers applicable profiles to robot control-part keys; -- versioned `SkillPolicyPreset` values own motion, recovery, and runner policy; +- versioned `SkillPolicyPreset` values own motion, recovery, and runner policy, + plus an optional required-planner compatibility constraint; - per-skill defaults map every skill-local slot to one resource ID. Resource and endpoint declarations are owned snapshots. A custom endpoint with @@ -708,7 +710,15 @@ normally, then resume with a fresh observation. ### 9.2 Timing `BaseEnv.step_dt` is the authoritative control cadence. Semantic task -configuration does not expose `control_dt`. +configuration, `MotionPolicy`, and `AtomicActionEngine` do not expose or own a +fallback `control_dt`. Environment integrations copy `BaseEnv.step_dt` into +`PlanningContext.control_dt` when an action performs deterministic interpolation. + +Timing is a strict producer contract. A planner result with positions includes +per-waypoint `dt` and a matching 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 diff --git a/docs/source/overview/sim/atomic_actions/builtin_actions.md b/docs/source/overview/sim/atomic_actions/builtin_actions.md index 0205dcc8e..856892136 100644 --- a/docs/source/overview/sim/atomic_actions/builtin_actions.md +++ b/docs/source/overview/sim/atomic_actions/builtin_actions.md @@ -248,8 +248,12 @@ Use this rule when configuring a built-in or adding a new one: invocation; an action may provide defaults; - the engine's **control-part profiles** carry embodiment-specific semantic commands such as `open`, `grasp`, and named postures; -- `MotionPolicy` carries sample count, timing, motion strategy, limits, - collision choice, and planner options; +- `MotionPolicy` carries sample count, motion strategy, collision choice, and + typed planner options; +- planner-backed segments preserve explicit planner timing, while action-owned + interpolation reads the environment cadence from `PlanningContext.control_dt`; +- missing planner or action timing is an error; the engine has no fallback + control period; - `RecoveryPolicy` carries all replan/retry thresholds and budgets. All built-ins resolve participating arm and hand names exclusively from @@ -398,8 +402,9 @@ a live scene entity. The bound end-effector profile must provide `grasp`; optional upright-transport settings belong to `MoveHeldObjectOptions`. The arm and hand are selected by -`ActionBinding`; generic timing and trajectory sampling remain in -`MotionPolicy`. In a vectorized batch, rows where another manipulator holds the +`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. **Example:** `scripts/tutorials/atomic_action/move_held_object.py` diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index 20c451cf2..56014f99a 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -105,7 +105,7 @@ manual_invocation = ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(xpos=target_pose), binding=ActionBinding(manipulators={"primary": "left_arm"}), - motion_policy=MotionPolicy(sample_count=80, control_dt=1.0 / 60.0), + motion_policy=MotionPolicy(sample_count=80), recovery_policy=RecoveryPolicy(max_replans=2), ) @@ -171,14 +171,22 @@ from leaking into an Action Agent schema. | `ActionOptions` / built-in `*Options` | Frozen invocation-varying skill behavior: segment counts, offsets, grasp-selection rules | Robot resource names, hand qpos, planner backend | | `ControlPartCommandProfile` | Embodiment-specific semantic commands such as `open`, `grasp`, and `ready`, keyed by actual control-part name | Action roles, task goals, recovery state | | `ActionControlOverrides` | Optional role-scoped command replacements for one invocation revision | Persistent robot configuration | -| `MotionPolicy` | Motion strategy, sample count, timing, limits, dynamic-collision mode, typed planner options | Skill semantics or robot-resource names | +| `MotionPolicy` | Motion strategy, sample count, dynamic-collision mode, typed planner options | Execution cadence, skill semantics, or robot-resource names | | `RecoveryPolicy` | Action replan/retry budgets, tracking and dynamic-goal thresholds, action-attempt timeout | Controller state or mutable counters | | `ExecutionRunnerCfg` | Runner-level acknowledgement deadlines, minimum feedback cadence, and completion hold policy | Skill behavior, planning resources, or invocation revision data | -| `PlanningContext` | Measured `RobotObservation`, verified `TaskState`, versioned `SceneSnapshot`, stable environment IDs | Hypothetical simulator mutation | +| `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 | `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 +action-owned deterministic interpolation, the integration supplies its +authoritative cadence as `PlanningContext.control_dt` (normally +`BaseEnv.step_dt`). The engine never supplies or guesses missing timing. +Planner-backend compatibility is a profile-level concern expressed by +`SkillPolicyPreset.required_planner`, not a per-invocation motion choice. Each action owns one or more frozen goal dataclasses and declares the accepted type through `AtomicAction.GoalType`. The action validates that type when the @@ -415,7 +423,7 @@ invocation = ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(xpos=target_pose), binding=ActionBinding(manipulators={"primary": "left_arm"}), - motion_policy=MotionPolicy(sample_count=80, control_dt=1.0 / 60.0), + motion_policy=MotionPolicy(sample_count=80), ) plan = engine.plan(invocation, latest_context) @@ -433,6 +441,9 @@ of manually reproducing its state projection rules. and replaces unsuccessful rows with the context's observed joint position. Primitive implementations therefore preserve row-local failures in `plan_success`; they do not need to duplicate failure-row hold logic. +It accepts only `TimedTrajectory`. Interpolation code can construct one with +`TimedTrajectory.from_uniform_step(..., step_dt=context.require_control_dt())`; +planner-backed code should preserve the planner's explicit `dt`. `TrajectorySegment.start` and `.stop` form an action-local half-open waypoint range. `plan.segment(name)` resolves that local metadata, while @@ -460,7 +471,7 @@ from embodichain.lab.sim.atomic_actions import ( engine = AtomicActionEngine(motion_generator) binding = ActionBinding(manipulators={"primary": "left_arm"}) -motion_policy = MotionPolicy(sample_count=80, control_dt=1.0 / 60.0) +motion_policy = MotionPolicy(sample_count=80) approach = ActionInvocation( skill_id="move_end_effector", 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 b5af6a58b..4ca68249e 100644 --- a/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md +++ b/docs/source/overview/sim/atomic_actions/robot_skill_profiles.md @@ -147,6 +147,11 @@ profile = RobotSkillProfile( ) ``` +Set `SkillPolicyPreset.required_planner` only when a preset depends on one +planner backend, typically because it carries backend-specific typed planning +options. Profile binding checks that requirement against the engine's configured +backend and fails early on a mismatch. Leave it as `None` for portable presets. + Every `ControlPartEndpoint.control_part` must be a key in `robot.control_parts`. A composite endpoint may reuse a member's control part, but all joints controlled directly by the composite must already be covered by diff --git a/docs/source/overview/sim/planners/motion_generator.md b/docs/source/overview/sim/planners/motion_generator.md index 6cde5ed93..cf8ba02ef 100644 --- a/docs/source/overview/sim/planners/motion_generator.md +++ b/docs/source/overview/sim/planners/motion_generator.md @@ -12,8 +12,10 @@ explicit cuRobo world. * **Unified planning interface**: Supports interpolation-oriented planners and collision-aware cuRobo V2 planning through one `generate()` API. * **Explicit strategy**: Accepts only `"motion_gen"` or `"ik_interp"`; no planner bypass is inferred from a missing backend-options object. -* **Normalized results**: Validates batched positions, success, derivatives and - timing, applies requested resampling, and holds failed rows at `start_qpos`. +* **Strict timed results**: A planner result with positions must include + per-waypoint `dt` and matching `duration`. The generator validates that + contract, preserves total duration when resampling, and holds failed rows at + `start_qpos`. * **Flexible planner selection**: Supports TOPPRA, NeuralPlanner (experimental), and the optional CuroboPlanner backend, which plans on CUDA with either CPU or CUDA physics simulation. * **Automatic constraint handling**: Retrieves velocity and acceleration limits from the robot or uses user-specified/default values. * **Backend-aware target handling**: Generates discrete trajectories using joint or Cartesian interpolation where appropriate; cuRobo receives original Cartesian goals so it can perform collision-aware IK itself. @@ -29,7 +31,8 @@ through `supported_move_types` and exposes them through * convert EEF targets into joint waypoints only for joint-only backends such as TOPPRA when `MotionGenOptions.is_interpolate=True`; * fall back to deterministic joint interpolation when a backend cannot consume - a `JOINT_MOVE` target and explicit `start_qpos`/`sample_count` are available; + a `JOINT_MOVE` target and explicit `start_qpos`/`sample_count`/ + `interpolation_dt` are available; * reject unsupported target types before entering the backend. The built-in declarations are: @@ -139,6 +142,22 @@ result = motion_gen.generate( ) ``` +For deterministic interpolation, select the timing explicitly: + +```python +motion_opts = MotionGenOptions( + strategy="ik_interp", + sample_count=50, + interpolation_dt=0.02, + start_qpos=start_qpos, + control_part="arm", +) +``` + +Missing interpolation timing is an error; it is never inferred from an engine +or global default. Custom planners likewise must return `PlanResult.dt` with +shape `(B, N)` and `duration == dt.sum(dim=1)` whenever they return positions. + #### Cartesian Space Planning ```python diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index e26d7d5e5..6cf9a58db 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -48,6 +48,13 @@ step. Put invocation-varying behavior in ``ActionInvocation.skill_options``. ``register()`` remains available for custom implementations, and ``load_builtins=False`` creates an isolated or fully custom engine. +Trajectory timing is strict. A planner that returns positions must also return +per-waypoint ``dt`` 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 +``PlanningContext.control_dt`` supplied by the integration, normally +``BaseEnv.step_dt``. + Choosing an engine entry point ------------------------------ @@ -97,6 +104,7 @@ Focused examples live under ``scripts/tutorials/atomic_action``: * ``move_end_effector.py`` * ``move_joints.py`` +* ``control_dt.py`` * ``pickup.py`` * ``move_held_object.py`` * ``place.py`` @@ -115,10 +123,17 @@ video under ``outputs/videos``: .. code-block:: bash python scripts/tutorials/atomic_action/move_end_effector.py --headless --auto_play --device cpu + python scripts/tutorials/atomic_action/control_dt.py --headless --auto_play --device cpu python scripts/tutorials/atomic_action/pickup.py --headless --auto_play --device cpu python scripts/tutorials/atomic_action/assemble.py --headless --auto_play --device cpu python scripts/tutorials/atomic_action/hand_over.py --headless --auto_play --device cpu +``control_dt.py`` compiles the same 40-waypoint ``ik_interp`` path twice. The +positions stay identical, while changing ``PlanningContext.control_dt`` from +``2 * physics_dt`` to ``8 * physics_dt`` makes every arrival interval and the +total trajectory duration four times longer. The script checks that relationship +before replaying the fast and slow trajectories in sequence. + The ``motion_generator`` variable in the snippets below is a configured :class:`~embodichain.lab.sim.planners.MotionGenerator`; its robot, planner, device, cache, and collision world become the resources owned by the engine. @@ -175,7 +190,7 @@ application-owned orchestration: skill_id="move_end_effector", goal=EndEffectorPoseGoal(xpos=target_pose), binding=ActionBinding(manipulators={"primary": "left_arm"}), - motion_policy=MotionPolicy(sample_count=80, control_dt=1.0 / 60.0), + motion_policy=MotionPolicy(sample_count=80), ) plan = engine.plan(invocation, latest_context) @@ -213,7 +228,7 @@ planning: engine = AtomicActionEngine(motion_generator) binding = ActionBinding(manipulators={"primary": "left_arm"}) - motion_policy = MotionPolicy(sample_count=80, control_dt=1.0 / 60.0) + motion_policy = MotionPolicy(sample_count=80) approach = ActionInvocation( skill_id="move_end_effector", @@ -427,7 +442,8 @@ rename it to ``_plan()``; defining ``plan()`` is rejected immediately. Return scalar or per-environment planner success through ``build_plan``. The framework normalizes the mask and holds failed rows at the observed qpos, so a -new action should not reproduce that masking itself. +new action should not reproduce that masking itself. ``build_plan`` accepts +only a ``TimedTrajectory``; an untimed position tensor raises immediately. A minimal implementation looks like: @@ -460,13 +476,17 @@ A minimal implementation looks like: ) -> ActionPlan: goal = self.require_goal(request) options = request.skill_options - # Resolve the bound resource, plan from context.robot.qpos, and - # return a full-robot TimedTrajectory or position tensor. + # Resolve the bound resource and plan from context.robot.qpos. + trajectory = TimedTrajectory.from_uniform_step( + full_robot_positions, + env_ids=context.env_ids, + step_dt=context.require_control_dt(), + ) return self.build_plan( request, context, success=success_mask, - trajectory=full_robot_positions, + trajectory=trajectory, ) Do not step simulation, mutate ``PlanningContext``, commit ``StateDelta``, or diff --git a/docs/source/tutorial/motion_gen.rst b/docs/source/tutorial/motion_gen.rst index ae9f913bd..094e3ebcf 100644 --- a/docs/source/tutorial/motion_gen.rst +++ b/docs/source/tutorial/motion_gen.rst @@ -107,6 +107,7 @@ API Reference motion_opts = MotionGenOptions( strategy="motion_gen", # "motion_gen" or "ik_interp" sample_count=None, # Optional normalized output length + interpolation_dt=None, # Required for deterministic interpolation plan_opts=ToppraPlanOptions(...), # Options for the underlying planner control_part=arm_name, # Robot part to control (e.g., 'left_arm') is_interpolate=False, # Whether to pre-interpolate trajectory @@ -126,8 +127,8 @@ API Reference options: MotionGenOptions | None = None, ) -> PlanResult -- ``strategy="motion_gen"`` delegates to the configured backend; ``strategy="ik_interp"`` performs deterministic waypoint IK and joint interpolation. -- Returns a normalized, environment-batched ``PlanResult``. +- ``strategy="motion_gen"`` delegates to the configured backend; ``strategy="ik_interp"`` performs deterministic waypoint IK and joint interpolation and requires ``interpolation_dt``. +- Returns a normalized, environment-batched ``PlanResult`` with explicit ``dt`` and matching ``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/core.py b/embodichain/lab/sim/atomic_actions/core.py index d4b7906bb..bace0a347 100644 --- a/embodichain/lab/sim/atomic_actions/core.py +++ b/embodichain/lab/sim/atomic_actions/core.py @@ -368,13 +368,6 @@ def resolve_request( f"Skill {self.skill_id!r} expects options " f"{self.OptionsType.__name__}, got {type(options).__name__}." ) - required_planner = invocation.motion_policy.planner - configured_planner_name = self.planning_services.planner_name - if required_planner is not None and required_planner != configured_planner_name: - raise ValueError( - f"Motion policy requires planner {required_planner!r}, but this " - f"action uses {configured_planner_name!r}." - ) return ResolvedActionRequest( skill_id=invocation.skill_id, goal=invocation.goal, @@ -506,7 +499,7 @@ def build_plan( context: PlanningContext, *, success: bool | torch.Tensor, - trajectory: TimedTrajectory | torch.Tensor, + trajectory: TimedTrajectory, expected_effects: StateDelta | None = None, replannable: bool = True, diagnostics: PlannerDiagnostics | None = None, @@ -518,7 +511,7 @@ def build_plan( request: Resolved invocation snapshot being planned. context: Planning input used for the plan. success: Per-environment planning success or scalar planner result. - trajectory: Full-robot timed trajectory or position tensor. + trajectory: Full-robot trajectory with explicit timing. expected_effects: Symbolic effects to verify after execution. replannable: Whether the execution runtime may replan this action. diagnostics: Optional retained planner diagnostics. @@ -535,16 +528,12 @@ def build_plan( name="Planning success", ) - if isinstance(trajectory, torch.Tensor): - timed = TimedTrajectory.from_positions( - trajectory, - env_ids=context.env_ids, - control_dt=request.motion_policy.control_dt, + if not isinstance(trajectory, TimedTrajectory): + raise TypeError( + "trajectory must be a TimedTrajectory with explicit dt; atomic " + "actions may not return untimed position tensors." ) - elif isinstance(trajectory, TimedTrajectory): - timed = trajectory - else: - raise TypeError("trajectory must be TimedTrajectory or torch.Tensor.") + timed = trajectory if timed.batch_size != context.batch_size: raise ValueError("Trajectory and planning context batch sizes must match.") if timed.robot_dof != context.robot.robot_dof: diff --git a/embodichain/lab/sim/atomic_actions/engine.py b/embodichain/lab/sim/atomic_actions/engine.py index c52d724ea..334653823 100644 --- a/embodichain/lab/sim/atomic_actions/engine.py +++ b/embodichain/lab/sim/atomic_actions/engine.py @@ -303,6 +303,7 @@ def initial_context( task: TaskState | None = None, scene: SceneSnapshot | None = None, timestamp: float = 0.0, + control_dt: float | None = None, ) -> PlanningContext: """Capture the robot state needed to start offline compilation. @@ -310,6 +311,7 @@ def initial_context( task: Optional symbolic task state; an empty state is used otherwise. scene: Optional scene snapshot; an empty snapshot is used otherwise. timestamp: Timestamp assigned to the captured robot observation. + control_dt: Explicit command period for action-owned interpolation. Returns: Planning context containing owned robot tensors. @@ -332,6 +334,7 @@ def initial_context( task=task, scene=scene, env_ids=torch.arange(batch_size, dtype=torch.long, device=self.device), + control_dt=control_dt, ) def compile( diff --git a/embodichain/lab/sim/atomic_actions/execution.py b/embodichain/lab/sim/atomic_actions/execution.py index 8e14b608c..fc38ea97a 100644 --- a/embodichain/lab/sim/atomic_actions/execution.py +++ b/embodichain/lab/sim/atomic_actions/execution.py @@ -386,6 +386,7 @@ def tick( task=self._task_state, scene=context.scene, env_ids=context.env_ids, + control_dt=context.control_dt, ) events = self._drain_events() if self._status is not ExecutionStatus.RUNNING: @@ -659,6 +660,7 @@ def _finish_action( task=self._task_state, scene=self._context.scene, env_ids=self._context.env_ids, + control_dt=self._context.control_dt, ) self._pending &= ~verified failed_effect = execution_mask & ~verified diff --git a/embodichain/lab/sim/atomic_actions/plans.py b/embodichain/lab/sim/atomic_actions/plans.py index eeb1d2dcc..79259dff1 100644 --- a/embodichain/lab/sim/atomic_actions/plans.py +++ b/embodichain/lab/sim/atomic_actions/plans.py @@ -18,6 +18,7 @@ from __future__ import annotations +import math from dataclasses import dataclass, field from types import MappingProxyType from typing import Any, Mapping, Sequence @@ -138,70 +139,80 @@ def from_positions( positions: torch.Tensor, *, env_ids: torch.Tensor, - control_dt: float, + dt: torch.Tensor, velocities: torch.Tensor | None = None, accelerations: torch.Tensor | None = None, - dt: torch.Tensor | None = None, - duration: torch.Tensor | float | None = None, ) -> TimedTrajectory: - """Build a timed trajectory and synthesize missing timing metadata. + """Build a trajectory from positions and explicit per-sample timing. Args: positions: Full-robot positions, shape ``(B, N, D)``. env_ids: Environment identifiers, shape ``(B,)``. - control_dt: Fallback interval used when ``dt`` is absent. + dt: Per-sample arrival intervals, shape ``(B, N)``. velocities: Optional joint velocities. accelerations: Optional joint accelerations. - dt: Optional per-sample time deltas. - duration: Optional duration used to synthesize or validate ``dt``. Returns: Validated timed trajectory. """ - if control_dt <= 0.0: - raise ValueError("control_dt must be greater than zero.") if not isinstance(positions, torch.Tensor) or positions.dim() != 3: raise ValueError("positions must have shape (B, N, D).") - batch_size, waypoint_count, _ = positions.shape - if dt is None: - dt = torch.zeros( - (batch_size, waypoint_count), - dtype=torch.float32, - device=positions.device, - ) - if waypoint_count > 1: - if duration is None: - dt[:, 1:] = control_dt - else: - duration_tensor = torch.as_tensor( - duration, dtype=torch.float32, device=positions.device - ) - if duration_tensor.dim() == 0: - duration_tensor = duration_tensor.expand(batch_size) - if duration_tensor.shape != (batch_size,): - raise ValueError(f"duration must have shape ({batch_size},).") - dt[:, 1:] = duration_tensor[:, None] / (waypoint_count - 1) - else: - dt = dt.to(device=positions.device, dtype=torch.float32) - computed_duration = dt.sum(dim=1) - if duration is not None: - duration_tensor = torch.as_tensor( - duration, dtype=torch.float32, device=positions.device - ) - if duration_tensor.dim() == 0: - duration_tensor = duration_tensor.expand(batch_size) - if duration_tensor.shape != (batch_size,): - raise ValueError(f"duration must have shape ({batch_size},).") - if not torch.allclose( - computed_duration, duration_tensor, rtol=1e-4, atol=1e-6 - ): - raise ValueError("duration does not match the supplied dt.") + if not isinstance(dt, torch.Tensor): + raise TypeError("dt must be a torch.Tensor.") return cls( positions=positions, velocities=velocities, accelerations=accelerations, - dt=dt, + dt=dt.to(device=positions.device, dtype=torch.float32), + env_ids=env_ids, + ) + + @classmethod + def from_uniform_step( + cls, + positions: torch.Tensor, + *, + env_ids: torch.Tensor, + step_dt: float, + velocities: torch.Tensor | None = None, + accelerations: torch.Tensor | None = None, + ) -> TimedTrajectory: + """Build an explicitly uniform-time trajectory. + + The first waypoint has zero arrival time; every following waypoint uses + ``step_dt``. This factory is intended for interpolation algorithms whose + cadence is selected by the caller, not for repairing untimed plans. + + Args: + positions: Full-robot positions, shape ``(B, N, D)``. + env_ids: Environment identifiers, shape ``(B,)``. + step_dt: Explicit interval between consecutive waypoints. + velocities: Optional joint velocities. + accelerations: Optional joint accelerations. + + Returns: + Validated uniformly timed trajectory. + """ + if isinstance(step_dt, bool) or not isinstance(step_dt, (int, float)): + raise TypeError("step_dt must be a real number.") + if not math.isfinite(step_dt) or step_dt <= 0.0: + raise ValueError("step_dt must be finite and greater than zero.") + if not isinstance(positions, torch.Tensor) or positions.dim() != 3: + raise ValueError("positions must have shape (B, N, D).") + batch_size, waypoint_count, _ = positions.shape + dt = torch.zeros( + (batch_size, waypoint_count), + dtype=torch.float32, + device=positions.device, + ) + if waypoint_count > 1: + dt[:, 1:] = float(step_dt) + return cls.from_positions( + positions, env_ids=env_ids, + dt=dt, + velocities=velocities, + accelerations=accelerations, ) @classmethod diff --git a/embodichain/lab/sim/atomic_actions/policies.py b/embodichain/lab/sim/atomic_actions/policies.py index 8f82b49a6..78b41de54 100644 --- a/embodichain/lab/sim/atomic_actions/policies.py +++ b/embodichain/lab/sim/atomic_actions/policies.py @@ -56,24 +56,12 @@ class MotionPolicy: cannot change an invocation after it has been created. """ - planner: str | None = None - """Optional required planner backend name; ``None`` accepts the configured one.""" - strategy: Literal["motion_gen", "ik_interp"] = "ik_interp" """Motion strategy: ``motion_gen`` or ``ik_interp``.""" sample_count: int = 50 """Requested trajectory sample count when the backend does not preserve samples.""" - control_dt: float = 1.0 / 60.0 - """Fallback command period in seconds when a planner supplies no timing.""" - - velocity_limit: float | None = None - """Optional planner velocity limit.""" - - acceleration_limit: float | None = None - """Optional planner acceleration limit.""" - dynamic_collision_mode: DynamicCollisionMode = DynamicCollisionMode.AUTO """How this invocation consumes live scene-snapshot collision entities.""" @@ -89,12 +77,6 @@ def __post_init__(self) -> None: ) if self.sample_count < 2: raise ValueError("sample_count must be at least 2.") - if self.control_dt <= 0.0: - raise ValueError("control_dt must be greater than zero.") - if self.velocity_limit is not None and self.velocity_limit <= 0.0: - raise ValueError("velocity_limit must be greater than zero when set.") - if self.acceleration_limit is not None and self.acceleration_limit <= 0.0: - raise ValueError("acceleration_limit must be greater than zero when set.") mode = self.dynamic_collision_mode if isinstance(mode, str): try: @@ -117,6 +99,7 @@ def to_motion_gen_options( start_qpos: "torch.Tensor", control_part: str, sample_count: int | None = None, + interpolation_dt: float | None = None, ) -> "MotionGenOptions": """Translate this atomic policy into motion-generator options. @@ -124,6 +107,8 @@ def to_motion_gen_options( start_qpos: Observed controlled-joint start positions. control_part: Bound robot control-part name. sample_count: Optional segment-local sample-count override. + interpolation_dt: Explicit waypoint interval used only by + deterministic interpolation. Returns: Independently owned options for :class:`MotionGenerator`. @@ -133,12 +118,11 @@ def to_motion_gen_options( return MotionGenOptions( strategy=self.strategy, sample_count=self.sample_count if sample_count is None else sample_count, - velocity_limit=self.velocity_limit, - acceleration_limit=self.acceleration_limit, start_qpos=start_qpos, control_part=control_part, plan_opts=self.plan_opts, is_interpolate=True, + interpolation_dt=interpolation_dt, ) diff --git a/embodichain/lab/sim/atomic_actions/primitives/_helpers.py b/embodichain/lab/sim/atomic_actions/primitives/_helpers.py index 0ed5a7783..cbb8ae7e0 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/_helpers.py +++ b/embodichain/lab/sim/atomic_actions/primitives/_helpers.py @@ -99,6 +99,7 @@ def plan_named_arm_trajectory( target_poses: torch.Tensor, n_waypoints: int, motion_policy: MotionPolicy, + interpolation_dt: float | None, ) -> tuple[torch.Tensor, torch.Tensor]: """Plan a fixed-size pose trajectory for one named manipulator.""" result = motion_generator.generate( @@ -107,6 +108,7 @@ def plan_named_arm_trajectory( start_qpos=start_qpos, control_part=control_part, sample_count=n_waypoints, + interpolation_dt=interpolation_dt, ), ) if not isinstance(result.success, torch.Tensor): diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py index 7829f72ec..9d5fdcad7 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py @@ -39,7 +39,7 @@ validate_pose_goal, ) from ..invocation import ActionOptions, ResolvedActionRequest -from ..plans import ActionPlan, normalize_success_mask +from ..plans import ActionPlan, TimedTrajectory, normalize_success_mask from ..requirements import ( DisjointResourceSlots, INVERSE_KINEMATICS_CAPABILITY, @@ -995,7 +995,11 @@ def _plan( request, context, success=success_mask, - trajectory=full, + trajectory=TimedTrajectory.from_uniform_step( + full, + env_ids=context.env_ids, + step_dt=context.require_control_dt(), + ), expected_effects=StateDelta( held_object_updates={ resources.left_arm.name: left_held_object, diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py index 9db4563f7..a4f494036 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py @@ -31,7 +31,7 @@ from ..effects import StateDelta from ..goals import PoseGoalValue, resolve_pose_goal, validate_pose_goal from ..invocation import ActionOptions, ResolvedActionRequest -from ..plans import ActionPlan, normalize_success_mask +from ..plans import ActionPlan, TimedTrajectory, normalize_success_mask from ..requirements import ( CARTESIAN_POSE_CAPABILITY, DisjointResourceSlots, @@ -269,6 +269,7 @@ def _plan( torch.stack([placing_lift_xpos, placing_xpos], dim=1), segments["approach"], request.motion_policy, + context.control_dt, ) success_mask &= normalize_success_mask( segment_success, @@ -289,6 +290,7 @@ def _plan( support_xpos.unsqueeze(1), segments["approach"], request.motion_policy, + context.control_dt, ) success_mask &= normalize_success_mask( segment_success, @@ -348,6 +350,7 @@ def _plan( placing_lift_xpos.unsqueeze(1), segments["retreat"], request.motion_policy, + context.control_dt, ) success_mask &= normalize_success_mask( segment_success, @@ -386,7 +389,11 @@ def _plan( request, context, success=success_mask, - trajectory=full, + trajectory=TimedTrajectory.from_uniform_step( + full, + env_ids=context.env_ids, + step_dt=context.require_control_dt(), + ), expected_effects=StateDelta( held_object_updates={ resources.placing_arm.name: ( diff --git a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py index 04b635c04..9e936c171 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py +++ b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py @@ -31,7 +31,7 @@ from ..core import AtomicAction, ObjectSemantics, _same_object_identity from ..effects import StateDelta from ..invocation import ActionOptions, ResolvedActionRequest -from ..plans import ActionPlan, normalize_success_mask +from ..plans import ActionPlan, TimedTrajectory, normalize_success_mask from ..requirements import ( CARTESIAN_POSE_CAPABILITY, DisjointResourceSlots, @@ -360,6 +360,7 @@ def _plan( transfer_middle_eef.unsqueeze(1), segments["transfer"], request.motion_policy, + context.control_dt, ) success_mask &= normalize_success_mask( segment_success, @@ -378,6 +379,7 @@ def _plan( torch.stack([receive_pre_grasp_eef, receive_grasp_xpos], dim=1), segments["approach"], request.motion_policy, + context.control_dt, ) success_mask &= normalize_success_mask( segment_success, @@ -401,6 +403,7 @@ def _plan( transfer_retreat_eef.unsqueeze(1), segments["deliver"], request.motion_policy, + context.control_dt, ) success_mask &= normalize_success_mask( segment_success, @@ -421,6 +424,7 @@ def _plan( receive_final_eef.unsqueeze(1), segments["deliver"], request.motion_policy, + context.control_dt, ) success_mask &= normalize_success_mask( segment_success, @@ -549,7 +553,11 @@ def _plan( request, context, success=success_mask, - trajectory=full, + trajectory=TimedTrajectory.from_uniform_step( + full, + env_ids=context.env_ids, + step_dt=context.require_control_dt(), + ), expected_effects=StateDelta( held_object_updates={ resources.transfer_arm.name: None, 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 78a35fcde..d3cccb468 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py @@ -90,6 +90,7 @@ def _plan( options=request.motion_policy.to_motion_gen_options( start_qpos=start_qpos, control_part=control_part, + interpolation_dt=context.control_dt, ), ) success, trajectory = to_full_robot_trajectory( @@ -97,7 +98,6 @@ def _plan( base_qpos=context.robot.qpos, joint_ids=joint_ids, env_ids=context.env_ids, - control_dt=request.motion_policy.control_dt, ) return self.build_plan( request, diff --git a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py index 0f8d014e0..2f928f51d 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py @@ -34,7 +34,7 @@ from ..core import AtomicAction from ..goals import PoseGoalValue, resolve_pose_goal, validate_pose_goal from ..invocation import ActionOptions, ResolvedActionRequest -from ..plans import ActionPlan +from ..plans import ActionPlan, TimedTrajectory from ..requirements import ( CARTESIAN_POSE_CAPABILITY, FORWARD_KINEMATICS_CAPABILITY, @@ -177,6 +177,7 @@ def _plan( options=request.motion_policy.to_motion_gen_options( start_qpos=start_arm_qpos, control_part=control_part, + interpolation_dt=context.control_dt, ), ) assert isinstance(result.success, torch.Tensor) @@ -192,12 +193,17 @@ def _plan( full[:, :, :] = state.last_qpos.unsqueeze(1) full[:, :, arm_joint_ids] = arm_traj full[:, :, hand_joint_ids] = hand_grasp_qpos.unsqueeze(1) + assert result.dt is not None return self.build_plan( request, context, success=success, - trajectory=full, + trajectory=TimedTrajectory.from_positions( + full, + env_ids=context.env_ids, + dt=result.dt, + ), segment_lengths={"transport": full.shape[1]}, ) diff --git a/embodichain/lab/sim/atomic_actions/primitives/move_joints.py b/embodichain/lab/sim/atomic_actions/primitives/move_joints.py index 60d9383da..8fbdaf6ef 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_joints.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_joints.py @@ -112,6 +112,7 @@ def _plan( options=request.motion_policy.to_motion_gen_options( start_qpos=start_qpos, control_part=control_part, + interpolation_dt=context.control_dt, ), ) success, trajectory = to_full_robot_trajectory( @@ -119,7 +120,6 @@ def _plan( base_qpos=context.robot.qpos, joint_ids=joint_ids, env_ids=context.env_ids, - control_dt=request.motion_policy.control_dt, ) return self.build_plan( request, diff --git a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py index dea984d1d..c4d1c730d 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py @@ -46,7 +46,7 @@ validate_pose_goal, ) from ..invocation import ActionOptions, ResolvedActionRequest -from ..plans import ActionPlan, normalize_success_mask +from ..plans import ActionPlan, TimedTrajectory, normalize_success_mask from ..policies import MotionPolicy from ..requirements import ( BATCH_INVERSE_KINEMATICS_CAPABILITY, @@ -202,6 +202,7 @@ def _get_full_pickup_trajectory( end_effector: ResolvedControlPart, hand_open_qpos: torch.Tensor, hand_grasp_qpos: torch.Tensor, + interpolation_dt: float, ) -> tuple[torch.Tensor, torch.Tensor, dict[str, int]]: pre_grasp_xpos = translate_pose_world( grasp_xpos, -approach_direction * options.pre_grasp_distance @@ -220,6 +221,7 @@ def _get_full_pickup_trajectory( start_qpos=start_arm_qpos, control_part=manipulator.name, sample_count=n_approach, + interpolation_dt=interpolation_dt, ), ) assert isinstance(approach_result.success, torch.Tensor) @@ -238,6 +240,7 @@ def _get_full_pickup_trajectory( start_qpos=grasp_arm_qpos, control_part=manipulator.name, sample_count=n_lift, + interpolation_dt=interpolation_dt, ), ) assert isinstance(lift_result.success, torch.Tensor) @@ -377,6 +380,7 @@ def _plan( end_effector, hand_open_qpos, hand_grasp_qpos, + context.require_control_dt(), ) success_mask = grasp_success & normalize_success_mask( trajectory_success, @@ -393,7 +397,11 @@ def _plan( request, context, success=success_mask, - trajectory=full, + trajectory=TimedTrajectory.from_uniform_step( + full, + env_ids=context.env_ids, + step_dt=context.require_control_dt(), + ), expected_effects=StateDelta(held_object_updates={control_part: held}), segment_lengths=segment_lengths, ) diff --git a/embodichain/lab/sim/atomic_actions/primitives/place.py b/embodichain/lab/sim/atomic_actions/primitives/place.py index 6bee6fd38..41a610d8a 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/place.py +++ b/embodichain/lab/sim/atomic_actions/primitives/place.py @@ -38,7 +38,7 @@ validate_pose_goal, ) from ..invocation import ActionOptions, ResolvedActionRequest -from ..plans import ActionPlan +from ..plans import ActionPlan, TimedTrajectory from ..requirements import ( CARTESIAN_POSE_CAPABILITY, FORWARD_KINEMATICS_CAPABILITY, @@ -265,6 +265,7 @@ def _plan( start_qpos=start_arm_qpos, control_part=control_part, sample_count=n_down, + interpolation_dt=context.control_dt, ), ) assert isinstance(down_result.success, torch.Tensor) @@ -282,6 +283,7 @@ def _plan( start_qpos=reach_arm_qpos, control_part=control_part, sample_count=n_back, + interpolation_dt=context.control_dt, ), ) assert isinstance(back_result.success, torch.Tensor) @@ -317,7 +319,11 @@ def _plan( request, context, success=success, - trajectory=full, + trajectory=TimedTrajectory.from_uniform_step( + full, + env_ids=context.env_ids, + step_dt=context.require_control_dt(), + ), expected_effects=StateDelta(held_object_updates={control_part: None}), segment_lengths={ "approach": n_down_actual, diff --git a/embodichain/lab/sim/atomic_actions/primitives/press.py b/embodichain/lab/sim/atomic_actions/primitives/press.py index 4310451f7..0fe082597 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/press.py +++ b/embodichain/lab/sim/atomic_actions/primitives/press.py @@ -28,7 +28,7 @@ from ..core import AtomicAction from ..goals import PoseGoalValue, resolve_pose_goal, validate_pose_goal from ..invocation import ActionOptions, ResolvedActionRequest -from ..plans import ActionPlan +from ..plans import ActionPlan, TimedTrajectory from ..requirements import ( CARTESIAN_POSE_CAPABILITY, JOINT_POSITION_CAPABILITY, @@ -135,6 +135,7 @@ def _plan( start_qpos=start_arm_qpos, control_part=control_part, sample_count=n_down, + interpolation_dt=context.control_dt, ), ) assert isinstance(down_result.success, torch.Tensor) @@ -149,6 +150,7 @@ def _plan( start_qpos=press_arm_qpos, control_part=control_part, sample_count=n_back, + interpolation_dt=context.control_dt, ), ) assert isinstance(back_result.success, torch.Tensor) @@ -182,7 +184,11 @@ def _plan( request, context, success=success, - trajectory=full, + trajectory=TimedTrajectory.from_uniform_step( + full, + env_ids=context.env_ids, + step_dt=context.require_control_dt(), + ), segment_lengths={ "close": n_close, "press": n_down_actual, diff --git a/embodichain/lab/sim/atomic_actions/sim_adapter.py b/embodichain/lab/sim/atomic_actions/sim_adapter.py index af1ebfc23..5cdb269ad 100644 --- a/embodichain/lab/sim/atomic_actions/sim_adapter.py +++ b/embodichain/lab/sim/atomic_actions/sim_adapter.py @@ -254,6 +254,9 @@ class SimulationExecutionAdapter: simulation: Simulation manager advanced by the execution clock. robot: Robot observed and commanded by the adapter. physics_dt: Optional physics period. Defaults to the simulation config. + control_dt: Optional command period exposed to action interpolation. + Defaults to ``physics_dt`` because that is the adapter's minimum + executable command cadence. env_ids: Optional stable correlation IDs matching every robot row. They are not used as simulator indices; row order maps to robot instances. scene_provider: Optional provider for versioned scene observations. @@ -268,6 +271,7 @@ def __init__( robot: Robot, *, physics_dt: float | None = None, + control_dt: float | None = None, env_ids: torch.Tensor | None = None, scene_provider: SceneProvider | None = None, scene_supplier: SceneSnapshotSupplier | None = None, @@ -282,6 +286,11 @@ def __init__( ) if not math.isfinite(resolved_physics_dt) or resolved_physics_dt <= 0.0: raise ValueError("physics_dt must be finite and greater than zero.") + resolved_control_dt = ( + resolved_physics_dt if control_dt is None else float(control_dt) + ) + if not math.isfinite(resolved_control_dt) or resolved_control_dt <= 0.0: + raise ValueError("control_dt must be finite and greater than zero.") qpos = robot.get_qpos() if not isinstance(qpos, torch.Tensor) or qpos.dim() != 2: raise ValueError("robot.get_qpos() must return shape (B, robot_dof).") @@ -301,6 +310,7 @@ def __init__( self.simulation = simulation self.robot = robot self.physics_dt = resolved_physics_dt + self.control_dt = resolved_control_dt self.env_ids = env_ids.clone() self._robot_env_indices = list(range(qpos.shape[0])) if scene_provider is not None and scene_supplier is not None: @@ -391,6 +401,7 @@ def observe(self, task_state: TaskState) -> PlanningContext: task=task_state, scene=scene, env_ids=self.env_ids, + control_dt=self.control_dt, ) def send( diff --git a/embodichain/lab/sim/atomic_actions/state.py b/embodichain/lab/sim/atomic_actions/state.py index a94c368ce..1cfd4af45 100644 --- a/embodichain/lab/sim/atomic_actions/state.py +++ b/embodichain/lab/sim/atomic_actions/state.py @@ -18,6 +18,7 @@ from __future__ import annotations +import math from collections.abc import Iterator, Mapping from dataclasses import dataclass, field from types import MappingProxyType @@ -520,6 +521,8 @@ class PlanningContext: task: TaskState scene: SceneSnapshot env_ids: torch.Tensor + control_dt: float | None = None + """Explicit command period used by action-owned interpolation.""" def __post_init__(self) -> None: if not isinstance(self.robot, RobotObservation): @@ -552,6 +555,14 @@ def __post_init__(self) -> None: raise ValueError("env_ids and robot tensors must share a device.") if torch.unique(self.env_ids).numel() != self.env_ids.numel(): raise ValueError("env_ids must be unique.") + if self.control_dt is not None: + if isinstance(self.control_dt, bool) or not isinstance( + self.control_dt, (int, float) + ): + raise TypeError("control_dt must be a real number or None.") + if not math.isfinite(self.control_dt) or self.control_dt <= 0.0: + raise ValueError("control_dt must be finite and greater than zero.") + object.__setattr__(self, "control_dt", float(self.control_dt)) object.__setattr__(self, "env_ids", self.env_ids.clone()) @property @@ -573,6 +584,19 @@ def get_held_object(self, resource: str) -> HeldObjectState | None: """Return the object held by ``resource``, if any.""" return self.task.get_held_object(resource) + def require_control_dt(self) -> float: + """Return the explicit command period required for interpolation. + + Raises: + ValueError: If the caller did not provide ``control_dt``. + """ + if self.control_dt is None: + raise ValueError( + "This action performs interpolation and requires an explicit " + "PlanningContext.control_dt." + ) + return self.control_dt + def project( self, *, @@ -593,6 +617,7 @@ def project( task=task, scene=self.scene, env_ids=self.env_ids, + control_dt=self.control_dt, ) diff --git a/embodichain/lab/sim/atomic_actions/trajectory_ops.py b/embodichain/lab/sim/atomic_actions/trajectory_ops.py index e64af32b7..ad4438c40 100644 --- a/embodichain/lab/sim/atomic_actions/trajectory_ops.py +++ b/embodichain/lab/sim/atomic_actions/trajectory_ops.py @@ -250,7 +250,6 @@ def to_full_robot_trajectory( base_qpos: torch.Tensor, joint_ids: list[int], env_ids: torch.Tensor, - control_dt: float, ) -> tuple[torch.Tensor, TimedTrajectory]: """Embed a controlled-joint plan into a timed full-robot trajectory.""" positions = result.positions @@ -270,23 +269,14 @@ def embed_derivative(value: torch.Tensor | None) -> torch.Tensor | None: full[:, :, joint_ids] = value return full - duration: float | torch.Tensor | None = result.duration if result.dt is None: - duration_tensor = torch.as_tensor( - result.duration, - dtype=torch.float32, - device=base_qpos.device, - ) - if not bool((duration_tensor > 0.0).any().item()): - duration = None + raise ValueError("PlanResult must include explicit dt.") timed = TimedTrajectory.from_positions( full_positions, env_ids=env_ids, - control_dt=control_dt, velocities=embed_derivative(result.velocities), accelerations=embed_derivative(result.accelerations), dt=result.dt, - duration=duration, ) success = normalize_success_mask( result.success, diff --git a/embodichain/lab/sim/planners/base_planner.py b/embodichain/lab/sim/planners/base_planner.py index d58f7b019..cf447b27d 100644 --- a/embodichain/lab/sim/planners/base_planner.py +++ b/embodichain/lab/sim/planners/base_planner.py @@ -281,6 +281,10 @@ def plan( 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 + + Returning ``positions`` without both timing tensors, or with a + duration that differs from ``dt.sum(dim=1)``, raises at + :class:`PlanResult` construction. """ logger.log_error("Subclasses must implement plan() method", NotImplementedError) diff --git a/embodichain/lab/sim/planners/motion_generator.py b/embodichain/lab/sim/planners/motion_generator.py index f26d411da..3da23af2f 100644 --- a/embodichain/lab/sim/planners/motion_generator.py +++ b/embodichain/lab/sim/planners/motion_generator.py @@ -16,6 +16,7 @@ from __future__ import annotations +import math from collections.abc import Mapping from copy import deepcopy from dataclasses import MISSING @@ -105,6 +106,9 @@ class MotionGenOptions: - The pre-interpolation only works for PlanState with MoveType.EEF_MOVE or MoveType.JOINT_MOVE. """ + interpolation_dt: float | None = None + """Explicit waypoint interval for deterministic interpolation.""" + interpolate_nums: int | list[int] = 10 """Number of interpolation points to generate between each pair of waypoints. @@ -133,6 +137,15 @@ def __post_init__(self) -> None: raise ValueError("velocity_limit must be greater than zero when set.") if self.acceleration_limit is not None and self.acceleration_limit <= 0.0: raise ValueError("acceleration_limit must be greater than zero when set.") + if self.interpolation_dt is not None: + if isinstance(self.interpolation_dt, bool) or not isinstance( + self.interpolation_dt, (int, float) + ): + raise TypeError("interpolation_dt must be a real number or None.") + if not math.isfinite(self.interpolation_dt) or self.interpolation_dt <= 0.0: + raise ValueError( + "interpolation_dt must be finite and greater than zero when set." + ) class MotionGenerator: @@ -538,6 +551,8 @@ def _generate_ik_interpolation( raise ValueError("IK interpolation requires start_qpos.") if options.sample_count is None: raise ValueError("IK interpolation requires sample_count.") + if options.interpolation_dt is None: + raise ValueError("IK interpolation requires explicit interpolation_dt.") start_qpos = options.start_qpos if start_qpos.dim() == 1: start_qpos = start_qpos.unsqueeze(0) @@ -570,9 +585,17 @@ def _generate_ik_interpolation( interp_num=options.sample_count, device=device, ) + dt = self._uniform_dt( + batch_size=batch_size, + waypoint_count=positions.shape[1], + step_dt=options.interpolation_dt, + device=device, + ) return PlanResult( success=torch.ones(batch_size, dtype=torch.bool, device=device), positions=positions, + dt=dt, + duration=dt.sum(dim=1), ) if move_type is not MoveType.EEF_MOVE: @@ -640,7 +663,36 @@ def _generate_ik_interpolation( ) held = start_qpos.unsqueeze(1).expand_as(positions) positions = torch.where(success[:, None, None], positions, held) - return PlanResult(success=success, positions=positions) + dt = self._uniform_dt( + batch_size=batch_size, + waypoint_count=positions.shape[1], + step_dt=options.interpolation_dt, + device=device, + ) + return PlanResult( + success=success, + positions=positions, + dt=dt, + duration=dt.sum(dim=1), + ) + + @staticmethod + def _uniform_dt( + *, + batch_size: int, + waypoint_count: int, + step_dt: float, + device: torch.device, + ) -> torch.Tensor: + """Return explicit uniform arrival intervals for interpolation.""" + dt = torch.zeros( + (batch_size, waypoint_count), + dtype=torch.float32, + device=device, + ) + if waypoint_count > 1: + dt[:, 1:] = step_dt + return dt def _normalize_plan_result( self, @@ -696,6 +748,21 @@ def _normalize_plan_result( if not torch.isfinite(positions).all(): raise ValueError("MotionGenerator returned non-finite positions.") + dt = result.dt + if not isinstance(dt, torch.Tensor): + raise ValueError( + "MotionGenerator planner results with positions require explicit dt." + ) + if dt.shape != positions.shape[:2]: + raise ValueError( + "MotionGenerator dt must match positions batch and sample " + f"dimensions, got {tuple(dt.shape)} and " + f"{tuple(positions.shape[:2])}." + ) + if dt.device != device or not torch.isfinite(dt).all() or (dt < 0).any(): + raise ValueError("MotionGenerator returned invalid time deltas.") + raw_duration = dt.sum(dim=1) + resampled = False preserve_samples = getattr(self.planner, "preserve_plan_samples", False) is True if ( @@ -709,6 +776,13 @@ def _normalize_plan_result( device=device, ) resampled = True + dt = torch.zeros( + positions.shape[:2], + dtype=result.dt.dtype, + device=device, + ) + if positions.shape[1] > 1: + dt[:, 1:] = raw_duration[:, None] / (positions.shape[1] - 1) def normalize_derivative( value: torch.Tensor | None, @@ -729,21 +803,7 @@ def normalize_derivative( velocities = normalize_derivative(result.velocities, "velocities") accelerations = normalize_derivative(result.accelerations, "accelerations") - dt = None if resampled else result.dt - if dt is not None: - if not isinstance(dt, torch.Tensor): - raise TypeError("MotionGenerator dt must be a torch.Tensor.") - if dt.shape != positions.shape[:2]: - raise ValueError( - "MotionGenerator dt must match positions batch and sample " - f"dimensions, got {tuple(dt.shape)} and " - f"{tuple(positions.shape[:2])}." - ) - if dt.device != device or not torch.isfinite(dt).all() or (dt < 0).any(): - raise ValueError("MotionGenerator returned invalid time deltas.") - duration: float | torch.Tensor = dt.sum(dim=1) - else: - duration = result.duration + duration = dt.sum(dim=1) if start_qpos is not None and not success.all(): held = ( diff --git a/embodichain/lab/sim/planners/neural_planner.py b/embodichain/lab/sim/planners/neural_planner.py index ed1083f3b..e5a9ffbe0 100644 --- a/embodichain/lab/sim/planners/neural_planner.py +++ b/embodichain/lab/sim/planners/neural_planner.py @@ -468,7 +468,8 @@ def plan( dtype=torch.float32, device=self.device, ) - dt = dt.unsqueeze(0).expand(b, -1) + dt = dt.unsqueeze(0).expand(b, -1).clone() + dt[:, 0] = 0.0 positions_t = positions_t.permute(1, 0, 2) xpos_t = xpos_t.permute(1, 0, 2, 3) velocities_t, accelerations_t = self._compute_vel_acc_via_finite_diff( @@ -482,11 +483,7 @@ def plan( accelerations=accelerations_t, xpos_list=xpos_t, dt=dt, - duration=torch.full( - (b,), - float(max(positions_t.shape[1] - 1, 0) * self.cfg.dt), - device=self.device, - ), + duration=dt.sum(dim=1), ) def _parse_waypoints( diff --git a/embodichain/lab/sim/planners/utils.py b/embodichain/lab/sim/planners/utils.py index 86b9a3cff..72a8c7ff3 100644 --- a/embodichain/lab/sim/planners/utils.py +++ b/embodichain/lab/sim/planners/utils.py @@ -184,7 +184,12 @@ class MoveType(Enum): @dataclass class PlanResult: - r"""Data class representing the result of a motion plan (env-batched).""" + r"""Data class representing the result of a motion plan (env-batched). + + A result that contains joint positions must also contain complete timing: + per-sample ``dt`` and a matching per-environment ``duration``. Failed plans + may omit all trajectory fields by leaving ``positions`` as ``None``. + """ success: bool | torch.Tensor = False """Per-env success, shape ``(B,)`` bool tensor (or scalar bool).""" @@ -204,9 +209,50 @@ class PlanResult: dt: torch.Tensor | None = None """Per-env time deltas, shape ``(B, N)``.""" - duration: float | torch.Tensor = 0.0 + 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: + raise ValueError("PlanResult timing requires positions.") + return + if not isinstance(self.positions, torch.Tensor) or self.positions.dim() != 3: + raise ValueError("PlanResult.positions must have shape (B, N, DOF).") + batch_size, waypoint_count, _ = self.positions.shape + if not isinstance(self.dt, torch.Tensor): + raise ValueError( + "PlanResult with positions requires explicit dt with shape (B, N)." + ) + if self.dt.shape != (batch_size, waypoint_count): + raise ValueError( + "PlanResult.dt must match positions batch and waypoint dimensions." + ) + if self.dt.device != self.positions.device: + raise ValueError("PlanResult.dt and positions must share a device.") + if not torch.isfinite(self.dt).all() or (self.dt < 0).any(): + raise ValueError("PlanResult.dt must contain finite non-negative values.") + 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).") + def is_all_success(self) -> bool: """Return True only when every env succeeded.""" if isinstance(self.success, torch.Tensor): diff --git a/embodichain/lab/sim/skills/profiles.py b/embodichain/lab/sim/skills/profiles.py index 3a19510fb..aa8d042c7 100644 --- a/embodichain/lab/sim/skills/profiles.py +++ b/embodichain/lab/sim/skills/profiles.py @@ -516,10 +516,21 @@ def __post_init__(self) -> None: @dataclass(frozen=True, slots=True, init=False) class SkillPolicyPreset: - """Versioned planning, recovery, and runner policy bundle.""" + """Versioned planning, recovery, and runner policy bundle. + + Args: + preset_id: Stable preset identifier. + schema_version: Preset schema version. Version 1 is currently supported. + motion_policy: Reusable atomic motion policy. + recovery_policy: Bounded action recovery policy. + runner_cfg: Execution transport and scheduling policy. + required_planner: Optional planner backend required by this preset. + """ preset_id: str schema_version: int + required_planner: str | None + """Optional planner backend required by this preset.""" _motion_policy: MotionPolicy _recovery_policy: RecoveryPolicy _runner_cfg: ExecutionRunnerCfg @@ -531,6 +542,7 @@ def __init__( motion_policy: MotionPolicy | None = None, recovery_policy: RecoveryPolicy | None = None, runner_cfg: ExecutionRunnerCfg | None = None, + required_planner: str | None = None, ) -> None: """Own one policy bundle without exposing mutable nested configuration.""" _validate_identifier(preset_id, field_name="SkillPolicyPreset.preset_id") @@ -541,6 +553,11 @@ def __init__( "Unsupported SkillPolicyPreset.schema_version " f"{schema_version}; supported versions are [1]." ) + if required_planner is not None: + _validate_identifier( + required_planner, + field_name="SkillPolicyPreset.required_planner", + ) selected_motion = MotionPolicy() if motion_policy is None else motion_policy selected_recovery = ( RecoveryPolicy() if recovery_policy is None else recovery_policy @@ -554,6 +571,7 @@ def __init__( raise TypeError("runner_cfg must be an ExecutionRunnerCfg.") object.__setattr__(self, "preset_id", preset_id) object.__setattr__(self, "schema_version", schema_version) + object.__setattr__(self, "required_planner", required_planner) object.__setattr__(self, "_motion_policy", deepcopy(selected_motion)) object.__setattr__(self, "_recovery_policy", deepcopy(selected_recovery)) object.__setattr__(self, "_runner_cfg", deepcopy(selected_runner)) @@ -581,6 +599,7 @@ def snapshot(self) -> SkillPolicyPreset: motion_policy=self.motion_policy, recovery_policy=self.recovery_policy, runner_cfg=self.runner_cfg, + required_planner=self.required_planner, ) @@ -1279,7 +1298,7 @@ def _validate_presets(self) -> None: """Validate planner-pinned presets against the selected engine backend.""" configured = self._engine.planning_services.planner_name for preset in self._profile.presets.values(): - required = preset.motion_policy.planner + required = preset.required_planner if required is not None and required != configured: raise ProfileValidationError( f"Preset {preset.preset_id!r} requires planner {required!r}, " diff --git a/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py b/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py index 7d076b322..f6dbb4306 100644 --- a/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py +++ b/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py @@ -433,7 +433,8 @@ def _plan_pick_place_cycle( hand_interp_steps=self.HAND_INTERP_STEPS, ), ), - ) + ), + self._action_engine.initial_context(control_dt=self.step_dt), ) pick_success = pick_compiled.plan_success pick_trajectory = pick_compiled.trajectory.positions diff --git a/embodichain_tasks/embodichain_tasks/tableware/blocks_ranking_rgb.py b/embodichain_tasks/embodichain_tasks/tableware/blocks_ranking_rgb.py index 5e09bac89..8a7fb182b 100644 --- a/embodichain_tasks/embodichain_tasks/tableware/blocks_ranking_rgb.py +++ b/embodichain_tasks/embodichain_tasks/tableware/blocks_ranking_rgb.py @@ -253,7 +253,8 @@ def _plan_block_segment( hand_interp_steps=HAND_INTERP_STEPS, ), ), - ) + ), + self._action_engine.initial_context(control_dt=self.step_dt), ) pick_success = pick_compiled.plan_success pick_trajectory = pick_compiled.trajectory.positions diff --git a/embodichain_tasks/embodichain_tasks/tableware/stack_blocks_two.py b/embodichain_tasks/embodichain_tasks/tableware/stack_blocks_two.py index 9001f0c73..d04794014 100644 --- a/embodichain_tasks/embodichain_tasks/tableware/stack_blocks_two.py +++ b/embodichain_tasks/embodichain_tasks/tableware/stack_blocks_two.py @@ -176,7 +176,8 @@ def _plan_stack( hand_interp_steps=HAND_INTERP_STEPS, ), ), - ) + ), + self._action_engine.initial_context(control_dt=self.step_dt), ) pick_success = pick_compiled.plan_success pick_trajectory = pick_compiled.trajectory.positions diff --git a/examples/sim/planners/curobo_planner.py b/examples/sim/planners/curobo_planner.py index b3105f248..a95e078a9 100644 --- a/examples/sim/planners/curobo_planner.py +++ b/examples/sim/planners/curobo_planner.py @@ -751,7 +751,7 @@ def main() -> None: engine = AtomicActionEngine(motion_generator) binding = ActionBinding(manipulators={"primary": control_part}) motion_policy = MotionPolicy( - motion_source="motion_gen", + strategy="motion_gen", plan_opts=CuroboPlanOptions( dynamic_obstacle_poses=( obstacle_poses if use_independent_worlds else None diff --git a/scripts/benchmark/atomic_action/move_held_object_benchmark.py b/scripts/benchmark/atomic_action/move_held_object_benchmark.py index 0f66c5b9d..f92e0b0de 100644 --- a/scripts/benchmark/atomic_action/move_held_object_benchmark.py +++ b/scripts/benchmark/atomic_action/move_held_object_benchmark.py @@ -238,7 +238,8 @@ def _prepare_held_state( hand_interp_steps=HAND_INTERP_STEPS, ), ), - ) + ), + atomic_engine.initial_context(control_dt=sim.sim_config.physics_dt), ) is_success = bool(result.plan_success.all().item()) traj = result.trajectory.positions diff --git a/scripts/benchmark/atomic_action/pickup_benchmark.py b/scripts/benchmark/atomic_action/pickup_benchmark.py index 4559d3e6e..39c87e67c 100644 --- a/scripts/benchmark/atomic_action/pickup_benchmark.py +++ b/scripts/benchmark/atomic_action/pickup_benchmark.py @@ -193,7 +193,8 @@ def _run_case( hand_interp_steps=HAND_INTERP_STEPS, ), ), - ) + ), + atomic_engine.initial_context(control_dt=sim.sim_config.physics_dt), ) ) is_success = bool(result.plan_success.all().item()) diff --git a/scripts/benchmark/atomic_action/place_benchmark.py b/scripts/benchmark/atomic_action/place_benchmark.py index 4c8242719..5a7311b79 100644 --- a/scripts/benchmark/atomic_action/place_benchmark.py +++ b/scripts/benchmark/atomic_action/place_benchmark.py @@ -226,7 +226,8 @@ def _prepare_held_state( hand_interp_steps=HAND_INTERP_STEPS, ), ), - ) + ), + atomic_engine.initial_context(control_dt=sim.sim_config.physics_dt), ) is_success = bool(result.plan_success.all().item()) traj = result.trajectory.positions diff --git a/scripts/benchmark/atomic_action/press_benchmark.py b/scripts/benchmark/atomic_action/press_benchmark.py index a5687af7f..1a9bf5d00 100644 --- a/scripts/benchmark/atomic_action/press_benchmark.py +++ b/scripts/benchmark/atomic_action/press_benchmark.py @@ -560,6 +560,7 @@ def _timed_atomic_run( atomic_engine: AtomicActionEngine, move_target: torch.Tensor, press_target: torch.Tensor, + control_dt: float, ) -> tuple[float, dict[str, float], float, bool, torch.Tensor]: """Run a timed atomic-action sequence and return timing/memory/results.""" _reset_peak_gpu_memory() @@ -588,7 +589,8 @@ def _timed_atomic_run( hand_interp_steps=HAND_INTERP_STEPS, ), ), - ) + ), + atomic_engine.initial_context(control_dt=control_dt), ) is_success = bool(result.plan_success.all().item()) traj = result.trajectory.positions @@ -634,6 +636,7 @@ def _run_press_case( atomic_engine=atomic_engine, move_target=move_target, press_target=press_target, + control_dt=sim.sim_config.physics_dt, ) video_path = None if should_record_case(args, recorded_count, bool(planning_success)): diff --git a/scripts/benchmark/motion_generation/planners/ik_interpolate.py b/scripts/benchmark/motion_generation/planners/ik_interpolate.py index fa241b386..75972d884 100644 --- a/scripts/benchmark/motion_generation/planners/ik_interpolate.py +++ b/scripts/benchmark/motion_generation/planners/ik_interpolate.py @@ -18,6 +18,8 @@ from __future__ import annotations +import math + import torch from embodichain.lab.sim.planners import PlanResult @@ -41,6 +43,16 @@ def build(self) -> None: def plan(self, case: BenchmarkCase) -> PlanResult: """Solve each waypoint sequentially while retaining per-env failures.""" robot = self.context.robot + interpolation_dt = self.spec.config.get("interpolation_dt") + if isinstance(interpolation_dt, bool) or not isinstance( + interpolation_dt, (int, float) + ): + raise ValueError( + "ik_interpolate requires an explicit numeric interpolation_dt." + ) + interpolation_dt = float(interpolation_dt) + if not math.isfinite(interpolation_dt) or interpolation_dt <= 0.0: + raise ValueError("interpolation_dt must be finite and greater than zero.") seed = case.start_qpos alive = torch.ones(case.batch_size, dtype=torch.bool, device=robot.device) targets = [seed] @@ -66,10 +78,14 @@ def plan(self, case: BenchmarkCase) -> PlanResult: interp_num=self.context.sample_interval, device=robot.device, ) + dt = torch.zeros(positions.shape[:2], dtype=torch.float32, device=robot.device) + if positions.shape[1] > 1: + dt[:, 1:] = interpolation_dt return PlanResult( success=alive, positions=positions, - duration=torch.zeros(case.batch_size, device=robot.device), + dt=dt, + duration=dt.sum(dim=1), ) diff --git a/scripts/benchmark/motion_generation/suites/coverage.yaml b/scripts/benchmark/motion_generation/suites/coverage.yaml index ebcd8faa9..6cc331254 100644 --- a/scripts/benchmark/motion_generation/suites/coverage.yaml +++ b/scripts/benchmark/motion_generation/suites/coverage.yaml @@ -28,7 +28,8 @@ planners: adapter: ik_interpolate role: diagnostic_baseline enabled: false - config: {} + config: + interpolation_dt: 0.025 - id: toppra adapter: toppra role: diagnostic_baseline diff --git a/scripts/benchmark/motion_generation/suites/smoke.yaml b/scripts/benchmark/motion_generation/suites/smoke.yaml index eafbbc5a6..459fe3f49 100644 --- a/scripts/benchmark/motion_generation/suites/smoke.yaml +++ b/scripts/benchmark/motion_generation/suites/smoke.yaml @@ -28,7 +28,8 @@ planners: adapter: ik_interpolate role: diagnostic_baseline enabled: false - config: {} + config: + interpolation_dt: 0.025 - id: toppra adapter: toppra role: diagnostic_baseline diff --git a/scripts/tutorials/atomic_action/assemble.py b/scripts/tutorials/atomic_action/assemble.py index e325f12bc..b0cbf56b6 100644 --- a/scripts/tutorials/atomic_action/assemble.py +++ b/scripts/tutorials/atomic_action/assemble.py @@ -344,7 +344,8 @@ def run_assemble_demo( ), skill_options=place_options, ), - ) + ), + engine.initial_context(control_dt=sim.sim_config.physics_dt), ) success = compiled.plan_success traj = compiled.trajectory.positions diff --git a/scripts/tutorials/atomic_action/control_dt.py b/scripts/tutorials/atomic_action/control_dt.py new file mode 100644 index 000000000..2e1615fbc --- /dev/null +++ b/scripts/tutorials/atomic_action/control_dt.py @@ -0,0 +1,168 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Compare one interpolated action at two explicit control periods.""" + +from __future__ import annotations + +import argparse +import sys +from pathlib import Path + +_REPO_ROOT = Path(__file__).resolve().parents[3] +if str(_REPO_ROOT) not in sys.path: + sys.path.insert(0, str(_REPO_ROOT)) + +import torch + +from embodichain.lab.sim.atomic_actions import ( + ActionBinding, + ActionInvocation, + AtomicActionEngine, + JointPositionGoal, + MotionPolicy, +) +from embodichain.utils import logger +from scripts.tutorials.atomic_action.tutorial_utils import ( + add_tutorial_robot, + create_toppra_motion_generator, + create_tutorial_argument_parser, + create_tutorial_simulation, + prepare_tutorial_scene, + replay_trajectory, + run_tutorial, +) + +SAMPLE_COUNT = 40 +FAST_CONTROL_STEPS = 2 +SLOW_CONTROL_STEPS = 8 +RESET_STEPS = 20 +POST_TRAJECTORY_STEPS = 40 + + +def parse_arguments() -> argparse.Namespace: + """Parse command-line arguments for the control-period tutorial.""" + parser = create_tutorial_argument_parser( + "Compare identical joint interpolation at two explicit control periods." + ) + return parser.parse_args() + + +def main() -> None: + """Replay the same geometric path with fast and slow waypoint timing.""" + args = parse_arguments() + sim = create_tutorial_simulation(args) + robot = add_tutorial_robot(sim, args.robot) + engine = AtomicActionEngine(motion_generator=create_toppra_motion_generator(robot)) + + initial_qpos = robot.get_qpos().clone() + start_arm_qpos = robot.get_qpos(name="arm")[0] + arm_limits = robot.get_qpos_limits(name="arm")[0] + offsets = torch.zeros_like(start_arm_qpos) + offsets[: min(4, offsets.numel())] = torch.tensor( + [0.30, 0.25, -0.20, -0.10][: min(4, offsets.numel())], + dtype=offsets.dtype, + device=offsets.device, + ) + target_arm_qpos = torch.minimum( + torch.maximum(start_arm_qpos + offsets, arm_limits[:, 0]), + arm_limits[:, 1], + ) + + invocation = ActionInvocation( + skill_id="move_joints", + goal=JointPositionGoal(target_arm_qpos), + binding=ActionBinding(manipulators={"primary": "arm"}), + motion_policy=MotionPolicy( + strategy="ik_interp", + sample_count=SAMPLE_COUNT, + ), + ) + physics_dt = float(sim.sim_config.physics_dt) + fast_control_dt = FAST_CONTROL_STEPS * physics_dt + slow_control_dt = SLOW_CONTROL_STEPS * physics_dt + fast = engine.compile( + (invocation,), + engine.initial_context(control_dt=fast_control_dt), + ) + slow = engine.compile( + (invocation,), + engine.initial_context(control_dt=slow_control_dt), + ) + if not fast.plan_success.all() or not slow.plan_success.all(): + logger.log_warning("Failed to compile one of the control-period plans.") + return + if not torch.allclose(fast.trajectory.positions, slow.trajectory.positions): + raise RuntimeError("control_dt unexpectedly changed the geometric path.") + + expected_ratio = slow_control_dt / fast_control_dt + actual_ratio = slow.trajectory.duration / fast.trajectory.duration + if not torch.allclose( + actual_ratio, + torch.full_like(actual_ratio, expected_ratio), + ): + raise RuntimeError("Trajectory duration does not scale with control_dt.") + + logger.log_info( + f"Both plans contain the same {fast.trajectory.waypoint_count} waypoints." + ) + logger.log_info( + f"Fast: control_dt={fast_control_dt:.3f}s, " + f"duration={fast.trajectory.duration.max().item():.3f}s." + ) + logger.log_info( + f"Slow: control_dt={slow_control_dt:.3f}s, " + f"duration={slow.trajectory.duration.max().item():.3f}s " + f"({expected_ratio:.1f}x slower)." + ) + + wait_for_user = prepare_tutorial_scene( + sim, + args, + "The two plans have identical positions. Press Enter to replay the fast one...", + ) + replay_trajectory( + sim, + robot, + fast.trajectory, + args, + video_prefix="control_dt_fast_auto_play", + hold_steps=POST_TRAJECTORY_STEPS, + ) + + robot.set_qpos(initial_qpos, target=False) + robot.set_qpos(initial_qpos, target=True) + zero_qvel = torch.zeros_like(robot.get_qvel()) + robot.set_qvel(zero_qvel, target=False) + robot.set_qvel(zero_qvel, target=True) + sim.update(step=RESET_STEPS) + + if wait_for_user: + input("Robot reset. Press Enter to replay the slow trajectory...") + replay_trajectory( + sim, + robot, + slow.trajectory, + args, + video_prefix="control_dt_slow_auto_play", + hold_steps=POST_TRAJECTORY_STEPS, + ) + if wait_for_user: + input("Press Enter to exit the simulation...") + + +if __name__ == "__main__": + run_tutorial(main) diff --git a/scripts/tutorials/atomic_action/coordinated_pickment.py b/scripts/tutorials/atomic_action/coordinated_pickment.py index 8d2e15a50..8c47a81c5 100644 --- a/scripts/tutorials/atomic_action/coordinated_pickment.py +++ b/scripts/tutorials/atomic_action/coordinated_pickment.py @@ -457,7 +457,8 @@ def run_coordinated_pickment_demo( ), skill_options=pickment_options, ), - ) + ), + engine.initial_context(control_dt=sim.sim_config.physics_dt), ) success = compiled.plan_success traj = compiled.trajectory.positions diff --git a/scripts/tutorials/atomic_action/coordinated_placement.py b/scripts/tutorials/atomic_action/coordinated_placement.py index c3c9244d4..b3775cff9 100644 --- a/scripts/tutorials/atomic_action/coordinated_placement.py +++ b/scripts/tutorials/atomic_action/coordinated_placement.py @@ -597,7 +597,7 @@ def run_coordinated_placement_demo( }, ) full_joint_ids = list(range(robot.dof)) - state = engine.initial_context() + state = engine.initial_context(control_dt=sim.sim_config.physics_dt) wait_for_user = prepare_tutorial_scene( sim, args, "Inspect the scene, then press Enter to compile both pick-ups..." diff --git a/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py b/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py index 1f0ceb1e6..293a3ed5a 100644 --- a/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py +++ b/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py @@ -437,6 +437,7 @@ def main() -> None: adapter = SimulationExecutionAdapter( sim, robot, + control_dt=COMMAND_CYCLE_TIME, scene_provider=scene_provider, ) @@ -451,7 +452,9 @@ def main() -> None: dtype=target_pose.dtype, device=target_pose.device, ) - engine = AtomicActionEngine(motion_generator=motion_gen) + engine = AtomicActionEngine( + motion_generator=motion_gen, + ) invocation = ActionInvocation( skill_id="move_end_effector", goal=EndEffectorPoseGoal(target_pose), @@ -459,7 +462,6 @@ def main() -> None: motion_policy=MotionPolicy( strategy="motion_gen", sample_count=SAMPLE_COUNT, - control_dt=COMMAND_CYCLE_TIME, ), recovery_policy=RecoveryPolicy( max_replans=2, @@ -491,7 +493,7 @@ def main() -> None: adapter, clock=adapter, # cuRobo can supply a trajectory duration, which takes precedence over - # MotionPolicy.control_dt. Keep a runner-side floor so the simulated + # engine fallback timing. Keep a runner-side floor so the simulated # controller receives enough feedback cycles to follow every waypoint. cfg=ExecutionRunnerCfg(minimum_cycle_time=COMMAND_CYCLE_TIME), ) diff --git a/scripts/tutorials/atomic_action/hand_over.py b/scripts/tutorials/atomic_action/hand_over.py index 5c75bbb73..c9719cac1 100644 --- a/scripts/tutorials/atomic_action/hand_over.py +++ b/scripts/tutorials/atomic_action/hand_over.py @@ -295,7 +295,8 @@ def run_handover_demo( ), skill_options=handover_options, ), - ) + ), + engine.initial_context(control_dt=sim.sim_config.physics_dt), ) success = compiled.plan_success traj = compiled.trajectory.positions diff --git a/scripts/tutorials/atomic_action/move_end_effector.py b/scripts/tutorials/atomic_action/move_end_effector.py index 6beb89f8e..0b80a6b91 100644 --- a/scripts/tutorials/atomic_action/move_end_effector.py +++ b/scripts/tutorials/atomic_action/move_end_effector.py @@ -105,7 +105,8 @@ def main() -> None: sample_count=MOVE_SAMPLE_INTERVAL, ), ), - ) + ), + engine.initial_context(control_dt=sim.sim_config.physics_dt), ) if not compiled.plan_success.all(): logger.log_warning("Failed to plan MoveEndEffector demo trajectory.") diff --git a/scripts/tutorials/atomic_action/move_held_object.py b/scripts/tutorials/atomic_action/move_held_object.py index 7c5f85f6d..38b8d0b59 100644 --- a/scripts/tutorials/atomic_action/move_held_object.py +++ b/scripts/tutorials/atomic_action/move_held_object.py @@ -186,7 +186,8 @@ def main() -> None: sample_count=MOVE_HELD_OBJECT_SAMPLE_INTERVAL, ), ), - ) + ), + engine.initial_context(control_dt=sim.sim_config.physics_dt), ) if not compiled.plan_success.all(): logger.log_warning("Failed to plan MoveHeldObject demo trajectory.") diff --git a/scripts/tutorials/atomic_action/move_joints.py b/scripts/tutorials/atomic_action/move_joints.py index 2c3ef3de1..9814819ab 100644 --- a/scripts/tutorials/atomic_action/move_joints.py +++ b/scripts/tutorials/atomic_action/move_joints.py @@ -113,7 +113,8 @@ def offset_from_home(offsets: tuple[float, ...]) -> torch.Tensor: ActionInvocation( "move_joints", JointPositionGoal(waypoints), binding, policy ), - ) + ), + engine.initial_context(control_dt=sim.sim_config.physics_dt), ) if not compiled.plan_success.all(): logger.log_warning("Failed to plan MoveJoints demo trajectory.") diff --git a/scripts/tutorials/atomic_action/moving_target_recovery.py b/scripts/tutorials/atomic_action/moving_target_recovery.py index 0a4546b9e..753e2eb76 100644 --- a/scripts/tutorials/atomic_action/moving_target_recovery.py +++ b/scripts/tutorials/atomic_action/moving_target_recovery.py @@ -250,6 +250,7 @@ def main() -> None: sim_runtime = SimulationExecutionAdapter( sim, robot, + control_dt=2.0 * sim.sim_config.physics_dt, scene_supplier=target_scene.snapshot, ) motion_gen = create_curobo_motion_generator(robot) @@ -303,7 +304,6 @@ def main() -> None: motion_policy=MotionPolicy( strategy="motion_gen", sample_count=PICK_SAMPLE_COUNT, - control_dt=2.0 * sim_runtime.physics_dt, ), recovery_policy=RecoveryPolicy( max_replans=2, diff --git a/scripts/tutorials/atomic_action/pickup.py b/scripts/tutorials/atomic_action/pickup.py index 4751dc4f5..a490cb76b 100644 --- a/scripts/tutorials/atomic_action/pickup.py +++ b/scripts/tutorials/atomic_action/pickup.py @@ -173,7 +173,8 @@ def main() -> None: hand_interp_steps=HAND_INTERP_STEPS, ), ), - ) + ), + engine.initial_context(control_dt=sim.sim_config.physics_dt), ) if not compiled.plan_success.all(): logger.log_warning("Failed to plan PickUp demo trajectory.") diff --git a/scripts/tutorials/atomic_action/place.py b/scripts/tutorials/atomic_action/place.py index 7465d5575..5bd8c5662 100644 --- a/scripts/tutorials/atomic_action/place.py +++ b/scripts/tutorials/atomic_action/place.py @@ -193,7 +193,8 @@ def main() -> None: hand_interp_steps=HAND_INTERP_STEPS, ), ), - ) + ), + engine.initial_context(control_dt=sim.sim_config.physics_dt), ) if not compiled.plan_success.all(): logger.log_warning("Failed to plan Place demo trajectory.") diff --git a/scripts/tutorials/atomic_action/press.py b/scripts/tutorials/atomic_action/press.py index ca30a1927..6e750366a 100644 --- a/scripts/tutorials/atomic_action/press.py +++ b/scripts/tutorials/atomic_action/press.py @@ -212,7 +212,8 @@ def main() -> None: hand_interp_steps=HAND_INTERP_STEPS, ), ), - ) + ), + engine.initial_context(control_dt=sim.sim_config.physics_dt), ) if not compiled.plan_success.all(): logger.log_warning("Failed to plan Press demo trajectory.") diff --git a/tests/benchmark/motion_generation/test_motion_generation_benchmark.py b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py index d98beaa66..5d5dace7a 100644 --- a/tests/benchmark/motion_generation/test_motion_generation_benchmark.py +++ b/tests/benchmark/motion_generation/test_motion_generation_benchmark.py @@ -172,10 +172,27 @@ def _valid_motion_case_and_positions() -> tuple[BenchmarkCase, torch.Tensor]: return case, positions +def _timed_plan_result( + positions: torch.Tensor, + *, + success: bool | torch.Tensor, +) -> PlanResult: + """Build a synthetic plan with explicit benchmark timing.""" + dt = torch.zeros(positions.shape[:2], device=positions.device) + if positions.shape[1] > 1: + dt[:, 1:] = 0.025 + return PlanResult( + success=success, + positions=positions, + dt=dt, + duration=dt.sum(dim=1), + ) + + def test_motion_valid_ignores_planner_reported_failure_in_outcomes_and_aggregates(): case, positions = _valid_motion_case_and_positions() outcomes = compute_case_outcomes( - PlanResult(success=False, positions=positions), + _timed_plan_result(positions, success=False), case, _MetricRobot(), "arm", @@ -245,7 +262,7 @@ def test_missing_positions_and_joint_limit_violation_fail_motion_valid(): positions = torch.zeros(1, 2, 7) positions[0, :, 0] = 2.0 violated = compute_case_outcomes( - PlanResult(success=True, positions=positions), + _timed_plan_result(positions, success=True), case, _MetricRobot(), "arm", @@ -265,7 +282,7 @@ def test_non_finite_trajectory_skips_joint_limit_metrics(): positions = torch.zeros(1, 2, 7) positions[0, 1, 0] = float("inf") outcomes = compute_case_outcomes( - PlanResult(success=True, positions=positions), + _timed_plan_result(positions, success=True), case, _MetricRobot(), "arm", @@ -1098,7 +1115,7 @@ def build(self) -> None: def plan(self, case: BenchmarkCase) -> PlanResult: steps = max(case.num_waypoints + 1, 2) positions = case.start_qpos.unsqueeze(1).expand(-1, steps, -1).clone() - return PlanResult(success=True, positions=positions) + return _timed_plan_result(positions, success=True) class _IncapableFake(PlannerAdapter): capabilities = frozenset({"eef_waypoint"}) diff --git a/tests/sim/atomic_actions/test_actions.py b/tests/sim/atomic_actions/test_actions.py index 06bafe4da..fb62cacec 100644 --- a/tests/sim/atomic_actions/test_actions.py +++ b/tests/sim/atomic_actions/test_actions.py @@ -84,6 +84,7 @@ ARM_DOF = 6 HAND_DOF = 2 ROBOT_DOF = ARM_DOF + HAND_DOF +CONTROL_DT = 1.0 / 60.0 DUAL_ARM_DOF = 2 * ARM_DOF DUAL_ROBOT_DOF = DUAL_ARM_DOF + 2 * HAND_DOF @@ -233,6 +234,7 @@ def _context( task=task or TaskState.empty(batch_size=NUM_ENVS, device="cpu"), scene=SceneSnapshot.empty() if scene is None else scene, env_ids=torch.arange(NUM_ENVS), + control_dt=CONTROL_DT, ) @@ -381,6 +383,7 @@ def _dual_context( task=task or TaskState.empty(NUM_ENVS, "cpu"), scene=SceneSnapshot.empty() if scene is None else scene, env_ids=torch.arange(NUM_ENVS), + control_dt=CONTROL_DT, ) @@ -510,6 +513,7 @@ def test_move_joints_uses_binding_and_preserves_uncontrolled_joints() -> None: task=TaskState.empty(NUM_ENVS, "cpu"), scene=SceneSnapshot.empty(), env_ids=torch.arange(NUM_ENVS), + control_dt=CONTROL_DT, ) plan = _plan_action( @@ -546,6 +550,7 @@ def test_pick_and_place_declare_effects_without_mutating_context() -> None: task=picked_task, scene=initial.scene, env_ids=initial.env_ids, + control_dt=initial.control_dt, ) place_plan = _plan_action( place, @@ -1157,7 +1162,9 @@ def plan_from_start( target_poses: torch.Tensor, n_waypoints: int, motion_policy: MotionPolicy, + interpolation_dt: float | None, ) -> tuple[bool, torch.Tensor]: + del interpolation_dt return True, start_qpos.unsqueeze(1).repeat(1, n_waypoints, 1) monkeypatch.setattr( diff --git a/tests/sim/atomic_actions/test_core.py b/tests/sim/atomic_actions/test_core.py index 5d4a133dd..65e7206b0 100644 --- a/tests/sim/atomic_actions/test_core.py +++ b/tests/sim/atomic_actions/test_core.py @@ -55,6 +55,7 @@ resolve_pose_goal, ) from embodichain.lab.sim.common import BatchEntity +from embodichain.lab.sim.planners import ToppraPlanOptions def _semantics( @@ -87,13 +88,18 @@ def _held( ) -def _context(scene: SceneSnapshot | None = None) -> PlanningContext: +def _context( + scene: SceneSnapshot | None = None, + *, + control_dt: float | None = None, +) -> PlanningContext: qpos = torch.zeros(2, 4) return PlanningContext( robot=RobotObservation(timestamp=1.0, qpos=qpos, qvel=torch.zeros_like(qpos)), task=TaskState.empty(batch_size=2, device="cpu"), scene=scene or SceneSnapshot.empty(), env_ids=torch.tensor([4, 7], dtype=torch.long), + control_dt=control_dt, ) @@ -165,9 +171,8 @@ def test_object_semantics_identity_fields_are_frozen() -> None: def test_motion_and_recovery_policy_validate_shared_parameters() -> None: - policy = MotionPolicy(sample_count=24, control_dt=0.01) + policy = MotionPolicy(sample_count=24) assert policy.sample_count == 24 - assert policy.control_dt == 0.01 assert policy.dynamic_collision_mode is DynamicCollisionMode.AUTO with pytest.raises(ValueError, match="sample_count"): MotionPolicy(sample_count=1) @@ -189,18 +194,22 @@ def test_motion_policy_normalizes_dynamic_collision_mode() -> None: def test_motion_policy_maps_to_motion_generator_strategy() -> None: + planner_options = ToppraPlanOptions( + constraints={"velocity": 0.2, "acceleration": 0.5} + ) policy = MotionPolicy( strategy="ik_interp", sample_count=24, - velocity_limit=0.2, - acceleration_limit=0.5, + plan_opts=planner_options, ) + planner_options.constraints["velocity"] = 1.0 start_qpos = torch.zeros(2, 6) options = policy.to_motion_gen_options( start_qpos=start_qpos, control_part="arm", sample_count=12, + interpolation_dt=0.02, ) assert options.strategy == "ik_interp" @@ -208,8 +217,11 @@ def test_motion_policy_maps_to_motion_generator_strategy() -> None: assert options.start_qpos is not start_qpos assert torch.equal(options.start_qpos, start_qpos) assert options.control_part == "arm" - assert options.velocity_limit == 0.2 - assert options.acceleration_limit == 0.5 + assert options.interpolation_dt == pytest.approx(0.02) + assert options.velocity_limit is None + assert options.acceleration_limit is None + assert isinstance(options.plan_opts, ToppraPlanOptions) + assert options.plan_opts.constraints == {"velocity": 0.2, "acceleration": 0.5} def test_task_state_normalizes_held_relations_and_masks_updates() -> None: @@ -523,12 +535,26 @@ def test_build_plan_uses_action_scene_dependency_hook() -> None: 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( + request, + context, + success=True, + trajectory=context.robot.qpos.unsqueeze(1), # type: ignore[arg-type] + ) plan = action.build_plan( request, context, success=True, - trajectory=context.robot.qpos.unsqueeze(1), + trajectory=trajectory, diagnostics=PlannerDiagnostics(backend="test"), ) @@ -611,12 +637,12 @@ def test_scene_snapshot_rejects_unknown_collision_entity() -> None: ) -def test_timed_trajectory_synthesizes_timing_and_holds_selected_rows() -> None: +def test_timed_trajectory_uses_explicit_uniform_timing_and_holds_rows() -> None: positions = torch.arange(24, dtype=torch.float32).reshape(2, 3, 4) - trajectory = TimedTrajectory.from_positions( + trajectory = TimedTrajectory.from_uniform_step( positions, env_ids=torch.tensor([4, 7]), - control_dt=0.02, + step_dt=0.02, ) held = trajectory.hold_rows( torch.tensor([True, False]), @@ -628,11 +654,20 @@ def test_timed_trajectory_synthesizes_timing_and_holds_selected_rows() -> None: assert torch.all(held.positions[1] == -1.0) +def test_planning_context_requires_explicit_interpolation_period() -> None: + with pytest.raises(ValueError, match="explicit PlanningContext.control_dt"): + _context().require_control_dt() + + assert _context(control_dt=0.02).require_control_dt() == pytest.approx(0.02) + with pytest.raises(ValueError, match="finite and greater than zero"): + _context(control_dt=0.0) + + def test_timed_trajectory_snapshot_owns_its_tensor_storage() -> None: - trajectory = TimedTrajectory.from_positions( + trajectory = TimedTrajectory.from_uniform_step( torch.arange(12, dtype=torch.float32).reshape(1, 3, 4), env_ids=torch.tensor([4]), - control_dt=0.02, + step_dt=0.02, ) snapshot = trajectory.snapshot() @@ -648,15 +683,15 @@ def test_timed_trajectory_snapshot_owns_its_tensor_storage() -> None: def test_timed_trajectory_concatenates_metadata() -> None: - first = TimedTrajectory.from_positions( + first = TimedTrajectory.from_uniform_step( torch.zeros(2, 2, 4), env_ids=torch.tensor([0, 1]), - control_dt=0.1, + step_dt=0.1, ) - second = TimedTrajectory.from_positions( + second = TimedTrajectory.from_uniform_step( torch.ones(2, 3, 4), env_ids=torch.tensor([0, 1]), - control_dt=0.2, + step_dt=0.2, ) result = TimedTrajectory.concatenate((first, second)) diff --git a/tests/sim/atomic_actions/test_engine.py b/tests/sim/atomic_actions/test_engine.py index 8704c357a..7baaaee2c 100644 --- a/tests/sim/atomic_actions/test_engine.py +++ b/tests/sim/atomic_actions/test_engine.py @@ -42,8 +42,11 @@ PressGoal, PressOptions, ResolvedActionRequest, + TimedTrajectory, ) +ACTION_DT = 0.02 + class StubAction(AtomicAction[JointPositionGoal, ActionOptions]): """Deterministic test action that commands every robot joint.""" @@ -68,7 +71,11 @@ def _plan( if torch.isnan(target).any(dim=1).any(): success &= ~torch.isnan(target).any(dim=1) target = torch.nan_to_num(target) - trajectory = torch.stack([context.robot.qpos, target], dim=1) + trajectory = TimedTrajectory.from_uniform_step( + torch.stack([context.robot.qpos, target], dim=1), + env_ids=context.env_ids, + step_dt=ACTION_DT, + ) return self.build_plan( request, context, @@ -270,6 +277,18 @@ def test_engine_binds_one_planning_service_to_every_action() -> None: assert second.planning_services is engine.planning_services +def test_engine_preserves_custom_action_timing() -> None: + engine = _engine() + engine.register(StubAction()) + + plan = engine.plan(_invocation(torch.ones(2, 3))) + + assert torch.allclose( + plan.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) diff --git a/tests/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index 1458b7367..33893da7c 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -81,11 +81,16 @@ def _plan( self.requests.append(request) pose = resolve_pose_goal(goal.xpos, context, name="xpos") target = pose[:, 0, 3].unsqueeze(1).expand_as(context.robot.qpos) + trajectory = TimedTrajectory.from_uniform_step( + torch.stack([context.robot.qpos, target], dim=1), + env_ids=context.env_ids, + step_dt=0.1, + ) return self.build_plan( request, context, success=True, - trajectory=torch.stack([context.robot.qpos, target], dim=1), + trajectory=trajectory, ) @@ -110,11 +115,16 @@ def _plan( object_to_eef=torch.eye(4), grasp_xpos=torch.eye(4), ) + trajectory = TimedTrajectory.from_uniform_step( + torch.stack([context.robot.qpos, target], dim=1), + env_ids=context.env_ids, + step_dt=0.1, + ) return self.build_plan( request, context, success=True, - trajectory=torch.stack([context.robot.qpos, target], dim=1), + trajectory=trajectory, expected_effects=StateDelta(held_object_updates={"arm": held}), ) @@ -156,9 +166,7 @@ def _plan( trajectory = TimedTrajectory.from_positions( positions, env_ids=context.env_ids, - control_dt=request.motion_policy.control_dt, dt=dt, - duration=dt.sum(dim=1), ) return self.build_plan( request, @@ -279,7 +287,6 @@ def _invocation( max_replans: int = 2, max_action_retries: int = 2, action_timeout: float = 30.0, - control_dt: float = 1.0 / 60.0, strategy: str = "ik_interp", dynamic_collision_mode: DynamicCollisionMode = DynamicCollisionMode.AUTO, ) -> ActionInvocation[EndEffectorPoseGoal]: @@ -289,7 +296,6 @@ def _invocation( binding=ActionBinding(manipulators={"primary": "arm"}), motion_policy=MotionPolicy( sample_count=2, - control_dt=control_dt, strategy=strategy, dynamic_collision_mode=dynamic_collision_mode, ), diff --git a/tests/sim/atomic_actions/test_runner.py b/tests/sim/atomic_actions/test_runner.py index bfeccc04b..6ea096545 100644 --- a/tests/sim/atomic_actions/test_runner.py +++ b/tests/sim/atomic_actions/test_runner.py @@ -190,7 +190,6 @@ def _plan( trajectory = TimedTrajectory.from_positions( positions, env_ids=context.env_ids, - control_dt=request.motion_policy.control_dt, dt=dt, ) effects = StateDelta() @@ -248,7 +247,7 @@ def _make_runner( skill_id="timed", goal=EndEffectorPoseGoal(goal_pose), binding=ActionBinding(manipulators={"primary": "arm"}), - motion_policy=MotionPolicy(sample_count=3, control_dt=FIRST_INTERVAL), + motion_policy=MotionPolicy(sample_count=3), recovery_policy=RecoveryPolicy( max_replans=2, tracking_error_threshold=0.05, @@ -391,7 +390,7 @@ def test_runner_surfaces_explicit_invocation_revision() -> None: skill_id="timed", goal=EndEffectorPoseGoal(revised_pose), binding=ActionBinding(manipulators={"primary": "arm"}), - motion_policy=MotionPolicy(sample_count=3, control_dt=FIRST_INTERVAL), + motion_policy=MotionPolicy(sample_count=3), recovery_policy=RecoveryPolicy( max_replans=2, tracking_error_threshold=0.05, diff --git a/tests/sim/atomic_actions/test_tutorial_utils.py b/tests/sim/atomic_actions/test_tutorial_utils.py index def9e17a3..12b432c67 100644 --- a/tests/sim/atomic_actions/test_tutorial_utils.py +++ b/tests/sim/atomic_actions/test_tutorial_utils.py @@ -412,7 +412,6 @@ def test_replay_timed_trajectory_uses_arrival_intervals() -> None: trajectory = TimedTrajectory.from_positions( torch.zeros(1, 3, 2), env_ids=torch.tensor([0], dtype=torch.long), - control_dt=0.1, dt=torch.tensor([[0.0, 0.2, 0.25]]), ) diff --git a/tests/sim/planners/test_motion_generator_batched.py b/tests/sim/planners/test_motion_generator_batched.py index 32d18fd9e..5684a6200 100644 --- a/tests/sim/planners/test_motion_generator_batched.py +++ b/tests/sim/planners/test_motion_generator_batched.py @@ -30,6 +30,25 @@ BATCH_SIZE = 2 CONTROLLED_DOF = 6 SAMPLE_COUNT = 8 +STEP_DT = 0.05 + + +def _timed_result( + positions: torch.Tensor, + *, + success: bool | torch.Tensor = True, + step_dt: float = STEP_DT, +) -> PlanResult: + """Build a planner result that satisfies the explicit timing contract.""" + dt = torch.zeros(positions.shape[:2], device=positions.device) + if positions.shape[1] > 1: + dt[:, 1:] = step_dt + return PlanResult( + success=success, + positions=positions, + dt=dt, + duration=dt.sum(dim=1), + ) @pytest.fixture(autouse=True) @@ -81,9 +100,9 @@ def with_motion_context(self, options, *, start_qpos, control_part): def plan(self, target_states, options): self.target_states = target_states - return PlanResult( + return _timed_result( + torch.zeros(1, 3, 2), success=torch.tensor([True]), - positions=torch.zeros(1, 3, 2), ) @@ -334,9 +353,9 @@ def _mock_planner(b=3, n=15, dofs=6): ) planner.robot.num_instances = b planner.robot.device = torch.device("cpu") - planner.plan.return_value = PlanResult( + planner.plan.return_value = _timed_result( + torch.zeros(b, n, dofs), success=torch.ones(b, dtype=torch.bool), - positions=torch.zeros(b, n, dofs), ) planner.preserve_plan_samples = False planner.default_plan_options.return_value = PlanOptions() @@ -375,9 +394,9 @@ def _mock_generator( planner.with_motion_context.side_effect = ( lambda options, *, start_qpos, control_part: options ) - planner.plan.return_value = result or PlanResult( + planner.plan.return_value = result or _timed_result( + torch.zeros(batch_size, 5, controlled_dof), success=torch.ones(batch_size, dtype=torch.bool), - positions=torch.zeros(batch_size, 5, controlled_dof), ) generator = object.__new__(MotionGenerator) generator.planner = planner @@ -450,6 +469,21 @@ def test_options_accept_only_declared_strategy_values(self): assert MotionGenOptions(strategy="ik_interp").strategy == "ik_interp" with pytest.raises(ValueError, match="strategy"): MotionGenOptions(strategy="planner") # type: ignore[arg-type] + with pytest.raises(ValueError, match="interpolation_dt"): + MotionGenOptions(interpolation_dt=0.0) + + def test_ik_interp_rejects_missing_timing(self): + generator = _mock_generator() + with pytest.raises(ValueError, match="explicit interpolation_dt"): + generator.generate( + [PlanState.from_qpos(torch.ones(BATCH_SIZE, CONTROLLED_DOF))], + MotionGenOptions( + strategy="ik_interp", + sample_count=SAMPLE_COUNT, + start_qpos=torch.zeros(BATCH_SIZE, CONTROLLED_DOF), + control_part="arm", + ), + ) def test_ik_interp_solves_batched_poses_without_calling_backend(self): generator = _mock_generator() @@ -468,6 +502,7 @@ def test_ik_interp_solves_batched_poses_without_calling_backend(self): sample_count=SAMPLE_COUNT, start_qpos=start, control_part="arm", + interpolation_dt=STEP_DT, ), ) @@ -488,9 +523,8 @@ def test_ik_interp_solves_batched_poses_without_calling_backend(self): def test_motion_gen_delegates_and_resamples_backend_result(self): raw_sample_count = 5 generator = _mock_generator( - result=PlanResult( - success=True, - positions=torch.zeros( + result=_timed_result( + torch.zeros( BATCH_SIZE, raw_sample_count, CONTROLLED_DOF, @@ -515,15 +549,20 @@ def test_motion_gen_delegates_and_resamples_backend_result(self): SAMPLE_COUNT, CONTROLLED_DOF, ) + assert result.dt is not None + assert result.duration is not None + assert result.dt.shape == (BATCH_SIZE, SAMPLE_COUNT) + assert result.duration.tolist() == pytest.approx( + [STEP_DT * (raw_sample_count - 1)] * BATCH_SIZE + ) generator.planner.plan.assert_called_once() def test_motion_gen_preserves_backend_samples_when_required(self): raw_sample_count = 5 generator = _mock_generator( preserve_plan_samples=True, - result=PlanResult( - success=True, - positions=torch.zeros( + result=_timed_result( + torch.zeros( BATCH_SIZE, raw_sample_count, CONTROLLED_DOF, @@ -556,6 +595,7 @@ def test_joint_target_falls_back_when_backend_has_no_joint_capability(self): sample_count=SAMPLE_COUNT, start_qpos=start, control_part="arm", + interpolation_dt=STEP_DT, ), ) @@ -594,9 +634,7 @@ class TestNormalizedPlanResult: def test_non_finite_positions_are_rejected(self): positions = torch.zeros(BATCH_SIZE, 5, CONTROLLED_DOF) positions[0, 0, 0] = float("nan") - generator = _mock_generator( - result=PlanResult(success=True, positions=positions) - ) + generator = _mock_generator(result=_timed_result(positions)) with pytest.raises(ValueError, match="non-finite"): generator.generate( @@ -628,9 +666,9 @@ def test_failed_rows_hold_start_qpos(self): positions = torch.zeros(BATCH_SIZE, 5, CONTROLLED_DOF) positions[1] = 1.0 generator = _mock_generator( - result=PlanResult( + result=_timed_result( + positions, success=torch.tensor([True, False]), - positions=positions, ) ) start = torch.zeros(BATCH_SIZE, CONTROLLED_DOF) diff --git a/tests/sim/planners/test_plan_state_batched.py b/tests/sim/planners/test_plan_state_batched.py index a48e8e2a0..5b5c8e6b1 100644 --- a/tests/sim/planners/test_plan_state_batched.py +++ b/tests/sim/planners/test_plan_state_batched.py @@ -51,18 +51,37 @@ def test_is_all_success_scalar(self): assert r.is_all_success() is True def test_batched_shapes(self): + dt = torch.zeros(2, 10) + dt[:, 1:] = 0.1 r = PlanResult( success=torch.tensor([True, False]), positions=torch.zeros(2, 10, 7), velocities=torch.zeros(2, 10, 7), accelerations=torch.zeros(2, 10, 7), - dt=torch.zeros(2, 10), - duration=torch.tensor([1.0, 0.0]), + dt=dt, + duration=dt.sum(dim=1), ) assert r.positions.shape == (2, 10, 7) assert r.dt.shape == (2, 10) assert r.duration.shape == (2,) + def test_positions_require_complete_matching_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/skills/test_profiles.py b/tests/sim/skills/test_profiles.py index 0f7be1fb8..d14d036a1 100644 --- a/tests/sim/skills/test_profiles.py +++ b/tests/sim/skills/test_profiles.py @@ -1102,7 +1102,8 @@ def test_generic_profile_supports_base_and_whole_body_without_arm_tool_fields() def test_presets_are_versioned_snapshots_and_validate_planner() -> None: preset = SkillPolicyPreset( "safe", - motion_policy=MotionPolicy(planner="stub_planner", sample_count=80), + motion_policy=MotionPolicy(sample_count=80), + required_planner="stub_planner", ) profile = RobotSkillProfile( "presets", @@ -1119,6 +1120,7 @@ def test_presets_are_versioned_snapshots_and_validate_planner() -> None: assert first is not second assert first.schema_version == 1 + assert first.required_planner == "stub_planner" assert first.motion_policy.sample_count == 80 mutable_runner = first.runner_cfg mutable_runner.command_timeout = 99.0 @@ -1129,6 +1131,8 @@ def test_presets_are_versioned_snapshots_and_validate_planner() -> None: bound.preset("safe", skill_id="typo") with pytest.raises(ValueError, match=r"supported versions are \[1\]"): SkillPolicyPreset("future", schema_version=2) + with pytest.raises(ValueError, match="required_planner"): + SkillPolicyPreset("invalid", required_planner="") incompatible = RobotSkillProfile( "bad_preset", @@ -1137,7 +1141,7 @@ def test_presets_are_versioned_snapshots_and_validate_planner() -> None: presets={ "other": SkillPolicyPreset( "other", - motion_policy=MotionPolicy(planner="other_planner"), + required_planner="other_planner", ) }, ) From 8ba314882b71d26ab06195bdae90cb4ed53a6364 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 19 Aug 2026 22:10:12 +0800 Subject: [PATCH 3/5] wip --- .../topics/atomic-actions/atomic-actions.md | 12 +++---- .../topics/motion-planning/motion-planning.md | 8 ++--- .../design/declarative_expert_program_plan.md | 2 +- .../overview/sim/atomic_actions/index.md | 5 +-- .../overview/sim/planners/motion_generator.md | 5 +-- docs/source/tutorial/atomic_actions.rst | 6 ++-- docs/source/tutorial/motion_gen.rst | 4 ++- embodichain/lab/sim/atomic_actions/core.py | 26 ++++++++------ .../lab/sim/atomic_actions/execution.py | 10 ++++-- embodichain/lab/sim/planners/base_planner.py | 9 ++--- .../lab/sim/planners/curobo/curobo_planner.py | 2 -- .../lab/sim/planners/motion_generator.py | 5 --- .../lab/sim/planners/neural_planner.py | 1 - .../lab/sim/planners/toppra_planner.py | 8 +---- embodichain/lab/sim/planners/utils.py | 35 +++++-------------- .../curobo_extraction/run_benchmark.py | 3 +- .../motion_generation/BENCHMARK_DESIGN.md | 2 +- .../planners/ik_interpolate.py | 1 - .../test_motion_generation_benchmark.py | 1 - tests/sim/atomic_actions/test_actions.py | 1 - .../test_endpoint_runtime_e2e.py | 1 - .../planners/test_motion_generator_batched.py | 1 - tests/sim/planners/test_plan_state_batched.py | 16 ++------- tests/sim/planners/test_toppra_batched.py | 2 +- 24 files changed, 65 insertions(+), 101 deletions(-) diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 8c3e0b753..16f020273 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -47,8 +47,8 @@ Each `AtomicActionEngine` exclusively owns one `ActionPlanningServices` instance, which contains its robot, one `MotionGenerator`/planner backend, and 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` and matching -`duration`, and actions must pass a complete `TimedTrajectory` to +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. @@ -542,10 +542,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 diff --git a/agent_context/topics/motion-planning/motion-planning.md b/agent_context/topics/motion-planning/motion-planning.md index 3502ce686..e15ef2c16 100644 --- a/agent_context/topics/motion-planning/motion-planning.md +++ b/agent_context/topics/motion-planning/motion-planning.md @@ -182,7 +182,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 +234,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,7 +266,7 @@ 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 @@ -308,7 +308,7 @@ 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. diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index 5635a8f5e..4c22c8899 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -719,7 +719,7 @@ 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. diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index 207d64b01..0e1754247 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -198,8 +198,9 @@ from leaking into an Action Agent schema. `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. 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 80de89811..9c14c502c 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -64,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``. 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/core.py b/embodichain/lab/sim/atomic_actions/core.py index e52b2dcf7..b014834bc 100644 --- a/embodichain/lab/sim/atomic_actions/core.py +++ b/embodichain/lab/sim/atomic_actions/core.py @@ -583,23 +583,20 @@ def build_command_plan( ) if not torch.equal(commands.env_ids, context.env_ids): raise ValueError("Command sequence env_ids must match the context.") - commands = self._authorize_command_targets(request, commands) success_mask = normalize_success_mask( success, num_envs=context.batch_size, device=self.device, name="Planning success", ) - masked_commands = TimedCommandSequence( - frames=tuple( - frame.with_active_mask(frame.active_mask & success_mask) - for frame in commands.frames - ), - env_ids=commands.env_ids, + commands = self._authorize_command_targets( + request, + commands, + active_mask=success_mask, ) segments = self._build_segments( segment_lengths, - frame_count=masked_commands.frame_count, + frame_count=commands.frame_count, ) if diagnostics is None: diagnostics = PlannerDiagnostics( @@ -608,7 +605,7 @@ def build_command_plan( return ActionPlan( skill_id=self.skill_id, plan_success=success_mask, - commands=masked_commands, + commands=commands, recovery_policy=request.recovery_policy, planned_scene_version=context.scene.version, planned_collision_world_revision=( @@ -630,6 +627,8 @@ def build_command_plan( 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. @@ -637,7 +636,8 @@ def _authorize_command_targets( 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. + 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: @@ -716,7 +716,11 @@ def _authorize_command_targets( frames.append( RuntimeCommandFrame( commands=tuple(endpoint_commands), - active_mask=frame.active_mask, + 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, ) diff --git a/embodichain/lab/sim/atomic_actions/execution.py b/embodichain/lab/sim/atomic_actions/execution.py index 032027fe3..8511519a1 100644 --- a/embodichain/lab/sim/atomic_actions/execution.py +++ b/embodichain/lab/sim/atomic_actions/execution.py @@ -354,6 +354,7 @@ def _install_prepared_revision( replacement_plan, replacement_context, ExecutionEventKind.INVOCATION_REVISED, + destination_continuity_validated=True, ) def _validate_revision_identity( @@ -569,14 +570,17 @@ 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.snapshot() + (target.transport_id, target.target_id): target for target in plan.commands.targets } replacement_destinations = frozenset(replacement_targets) - self._validate_destination_continuity(plan, event_kind) + if not destination_continuity_validated: + self._validate_destination_continuity(plan, event_kind) if ( event_kind not in ( diff --git a/embodichain/lab/sim/planners/base_planner.py b/embodichain/lab/sim/planners/base_planner.py index cf447b27d..c7ee87579 100644 --- a/embodichain/lab/sim/planners/base_planner.py +++ b/embodichain/lab/sim/planners/base_planner.py @@ -280,11 +280,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..da655da77 100644 --- a/embodichain/lab/sim/planners/curobo/curobo_planner.py +++ b/embodichain/lab/sim/planners/curobo/curobo_planner.py @@ -2084,12 +2084,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 3da23af2f..a3b4cf7e5 100644 --- a/embodichain/lab/sim/planners/motion_generator.py +++ b/embodichain/lab/sim/planners/motion_generator.py @@ -595,7 +595,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: @@ -673,7 +672,6 @@ def _generate_ik_interpolation( success=success, positions=positions, dt=dt, - duration=dt.sum(dim=1), ) @staticmethod @@ -803,8 +801,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) @@ -830,7 +826,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/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 66463bb2c..9c0ba58b3 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/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 b602743cd..430fa93b5 100644 --- a/tests/sim/atomic_actions/test_actions.py +++ b/tests/sim/atomic_actions/test_actions.py @@ -844,7 +844,6 @@ 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( diff --git a/tests/sim/atomic_actions/test_endpoint_runtime_e2e.py b/tests/sim/atomic_actions/test_endpoint_runtime_e2e.py index 7c6dd3688..6200b3596 100644 --- a/tests/sim/atomic_actions/test_endpoint_runtime_e2e.py +++ b/tests/sim/atomic_actions/test_endpoint_runtime_e2e.py @@ -149,7 +149,6 @@ def generate(states: list[object], *, options: object) -> PlanResult: success=torch.ones(positions.shape[0], dtype=torch.bool), positions=positions, dt=dt, - duration=dt.sum(dim=1), ) generator.generate.side_effect = generate diff --git a/tests/sim/planners/test_motion_generator_batched.py b/tests/sim/planners/test_motion_generator_batched.py index 5684a6200..b3449017a 100644 --- a/tests/sim/planners/test_motion_generator_batched.py +++ b/tests/sim/planners/test_motion_generator_batched.py @@ -47,7 +47,6 @@ def _timed_result( success=success, positions=positions, dt=dt, - duration=dt.sum(dim=1), ) 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 From cfb404dbaff75afde237653b68912bc799111ada Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 19 Aug 2026 15:24:43 +0000 Subject: [PATCH 4/5] wip --- agent_context/MAP.yaml | 3 + .../topics/atomic-actions/atomic-actions.md | 39 ++++--- .../topics/motion-planning/motion-planning.md | 32 +++--- embodichain/lab/sim/atomic_actions/engine.py | 83 +++++++++++++- embodichain/lab/sim/planners/base_planner.py | 82 ++++++++++---- .../lab/sim/planners/curobo/curobo_planner.py | 27 ++--- .../lab/sim/planners/motion_generator.py | 62 ++++------- embodichain/lab/sim/skills/scene.py | 17 ++- scripts/tutorials/atomic_action/assemble.py | 21 +--- scripts/tutorials/atomic_action/control_dt.py | 11 +- .../atomic_action/coordinated_pickment.py | 17 +-- .../atomic_action/coordinated_placement.py | 43 +++----- .../dynamic_obstacle_recovery.py | 11 +- scripts/tutorials/atomic_action/hand_over.py | 30 ++--- .../atomic_action/move_end_effector.py | 14 +-- .../atomic_action/move_held_object.py | 31 ++---- .../tutorials/atomic_action/move_joints.py | 20 ++-- .../atomic_action/moving_target_recovery.py | 11 +- scripts/tutorials/atomic_action/pickup.py | 12 +- scripts/tutorials/atomic_action/place.py | 21 +--- scripts/tutorials/atomic_action/press.py | 13 +-- scripts/tutorials/atomic_action/slide.py | 19 ++-- scripts/tutorials/atomic_action/twist.py | 13 +-- tests/sim/atomic_actions/test_engine.py | 104 ++++++++++++++++++ tests/sim/planners/test_base_planner.py | 76 +++++++++++++ tests/sim/planners/test_curobo_planner.py | 9 +- .../planners/test_motion_generator_batched.py | 74 +++++++------ tests/sim/skills/test_scene.py | 14 ++- 28 files changed, 565 insertions(+), 344 deletions(-) create mode 100644 tests/sim/planners/test_base_planner.py diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 0108e424e..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 @@ -678,6 +680,7 @@ topics: - build_pose_plan_states - build_joint_plan_states - engine.register + - engine.make_invocation - BUILTIN_ACTION_TYPES - load_builtins - engine.plan diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index d7fbfe0fc..1e77fb97e 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -154,6 +154,16 @@ disjointness, then emits the same generic `ActionBinding` with and this path deliberately does not perform profile resource discovery or capability matching. +`engine.make_invocation(skill_id, goal, ...)` is the convenience construction +boundary when callers do not need to retain a binding separately. Pass +`control_parts` for the direct path, or rely on a bound `RobotSkillProfile` and +optionally pass `resources` as `slot -> resource_id` selections. The two binding +sources are mutually exclusive. Without a profile, `control_parts` is required; +with a profile, omitting `resources` uses unique or configured-default profile +resolution. The method returns an ordinary `ActionInvocation` and does not plan +or execute it. It resolves bindings only; profile policy presets and runner +configuration remain semantic-runtime concerns. + Discovery boundaries are distinct: - `engine.actions` contains every installed action instance and is the @@ -509,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 diff --git a/agent_context/topics/motion-planning/motion-planning.md b/agent_context/topics/motion-planning/motion-planning.md index e15ef2c16..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. @@ -269,8 +271,8 @@ total duration and emits new explicit arrival intervals. 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 @@ -315,7 +317,7 @@ The decorator checks that every `PlanState` in `target_states` shares the same l - **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/embodichain/lab/sim/atomic_actions/engine.py b/embodichain/lab/sim/atomic_actions/engine.py index d7aa3944a..a7083fcb4 100644 --- a/embodichain/lab/sim/atomic_actions/engine.py +++ b/embodichain/lab/sim/atomic_actions/engine.py @@ -25,9 +25,10 @@ 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 @@ -227,6 +228,84 @@ def bind_control_parts( ) 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. diff --git a/embodichain/lab/sim/planners/base_planner.py b/embodichain/lab/sim/planners/base_planner.py index c7ee87579..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. diff --git a/embodichain/lab/sim/planners/curobo/curobo_planner.py b/embodichain/lab/sim/planners/curobo/curobo_planner.py index da655da77..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 diff --git a/embodichain/lab/sim/planners/motion_generator.py b/embodichain/lab/sim/planners/motion_generator.py index 51fe34e16..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() 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/scripts/tutorials/atomic_action/assemble.py b/scripts/tutorials/atomic_action/assemble.py index 9a5f9d1b7..c7429e62a 100644 --- a/scripts/tutorials/atomic_action/assemble.py +++ b/scripts/tutorials/atomic_action/assemble.py @@ -38,7 +38,6 @@ from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionInvocation, AssembleAffordance, AssembleGoal, AtomicActionEngine, @@ -318,31 +317,23 @@ def run_assemble_demo( assemble_to_base_pose=assemble_to_base, ) endpoint_mapping = {"primary": {"motion": "left_arm", "grasp": "left_hand"}} - pick_binding = engine.bind_control_parts( - "pick_up", - endpoint_mapping, - ) - place_binding = engine.bind_control_parts( - "place", - endpoint_mapping, - ) compiled = engine.compile( ( - ActionInvocation( + engine.make_invocation( "pick_up", GraspGoal(can_semantics), - pick_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), - place_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 066ee9fd5..617567cdc 100644 --- a/scripts/tutorials/atomic_action/control_dt.py +++ b/scripts/tutorials/atomic_action/control_dt.py @@ -29,7 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionInvocation, AtomicActionEngine, JointPositionGoal, MotionPolicy, @@ -81,14 +80,10 @@ def main() -> None: arm_limits[:, 1], ) - binding = engine.bind_control_parts( + invocation = engine.make_invocation( "move_joints", - {"primary": {"motion": "arm"}}, - ) - invocation = ActionInvocation( - skill_id="move_joints", - goal=JointPositionGoal(target_arm_qpos), - binding=binding, + 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 3208042fd..2b52be831 100644 --- a/scripts/tutorials/atomic_action/coordinated_pickment.py +++ b/scripts/tutorials/atomic_action/coordinated_pickment.py @@ -37,7 +37,6 @@ from embodichain.lab.sim import SimulationManager from embodichain.data import get_data_path from embodichain.lab.sim.atomic_actions import ( - ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, CoordinatedPickGoal, @@ -441,20 +440,16 @@ def run_coordinated_pickment_demo( ) start_time = time.time() - binding = engine.bind_control_parts( - "coordinated_pickment", - { - "left": {"motion": "left_arm", "grasp": "left_hand"}, - "right": {"motion": "right_arm", "grasp": "right_hand"}, - }, - ) compiled = engine.compile( ( - ActionInvocation( + engine.make_invocation( "coordinated_pickment", pickment_target, - binding, - 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 825a7a551..f24f8aba9 100644 --- a/scripts/tutorials/atomic_action/coordinated_placement.py +++ b/scripts/tutorials/atomic_action/coordinated_placement.py @@ -38,7 +38,6 @@ from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, CoordinatedPlacementOptions, @@ -616,35 +615,27 @@ def run_coordinated_placement_demo( sim.device, z_clearance=PAN_GRASP_Z_CLEARANCE, ) - left_pick_binding = engine.bind_control_parts( - "pick_up", - {"primary": {"motion": "left_arm", "grasp": "left_hand"}}, - ) - right_pick_binding = engine.bind_control_parts( - "pick_up", - {"primary": {"motion": "right_arm", "grasp": "right_hand"}}, - ) pick_invocations = ( - ActionInvocation( - skill_id="pick_up", - goal=GraspGoal( + engine.make_invocation( + "pick_up", + GraspGoal( semantics=bread_semantics, grasp_xpos=broadcast_pose_batch(bread_grasp_pose, num_envs=num_envs), ), - binding=left_pick_binding, + 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=right_pick_binding, + control_parts={"primary": {"motion": "right_arm", "grasp": "right_hand"}}, motion_policy=MotionPolicy( strategy="motion_gen", sample_count=PAN_PICK_SAMPLE_INTERVAL, @@ -792,19 +783,15 @@ def log_trajectory_execution(step_idx: int, total_steps: int) -> None: release=True, ) start_time = time.time() - placement_binding = engine.bind_control_parts( - "coordinated_placement", - { - "placing": {"motion": "left_arm", "grasp": "left_hand"}, - "support": {"motion": "right_arm", "grasp": "right_hand"}, - }, - ) placement_compiled = engine.compile( ( - ActionInvocation( - skill_id="coordinated_placement", - goal=coordinated_target, - binding=placement_binding, + 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 2f20fe098..dd8beedcb 100644 --- a/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py +++ b/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py @@ -32,7 +32,6 @@ from embodichain.lab.sim import SimulationManager, VisualMaterialCfg from embodichain.lab.sim.atomic_actions import ( - ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, ExecutionEventKind, @@ -469,14 +468,10 @@ def main() -> None: device=target_pose.device, ) engine = AtomicActionEngine(motion_generator=motion_gen) - binding = engine.bind_control_parts( + invocation = engine.make_invocation( "move_end_effector", - {"primary": {"motion": CONTROL_PART}}, - ) - invocation = ActionInvocation( - skill_id="move_end_effector", - goal=EndEffectorPoseGoal(target_pose), - binding=binding, + EndEffectorPoseGoal(target_pose), + control_parts={"primary": {"motion": CONTROL_PART}}, motion_policy=MotionPolicy( strategy="motion_gen", sample_count=SAMPLE_COUNT, diff --git a/scripts/tutorials/atomic_action/hand_over.py b/scripts/tutorials/atomic_action/hand_over.py index 03d61d65c..446596c86 100644 --- a/scripts/tutorials/atomic_action/hand_over.py +++ b/scripts/tutorials/atomic_action/hand_over.py @@ -35,7 +35,6 @@ from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( - ActionInvocation, GraspGoal, AtomicActionEngine, ControlPartCommandProfile, @@ -257,34 +256,29 @@ def run_handover_demo( # wait for object to drop for _ in range(20): sim.update(step=10) - pick_binding = engine.bind_control_parts( - "pick_up", - {"primary": {"motion": "left_arm", "grasp": "left_hand"}}, - ) - handover_binding = engine.bind_control_parts( - "hand_over", - { - "source": {"motion": "left_arm", "grasp": "left_hand"}, - "destination": {"motion": "right_arm", "grasp": "right_hand"}, - }, - ) compiled = engine.compile( ( - ActionInvocation( + engine.make_invocation( "pick_up", GraspGoal(object_semantics), - pick_binding, - 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), - handover_binding, - MotionPolicy( + control_parts={ + "source": {"motion": "left_arm", "grasp": "left_hand"}, + "destination": { + "motion": "right_arm", + "grasp": "right_hand", + }, + }, + motion_policy=MotionPolicy( strategy="motion_gen", sample_count=HANDOVER_SAMPLE_INTERVAL, ), diff --git a/scripts/tutorials/atomic_action/move_end_effector.py b/scripts/tutorials/atomic_action/move_end_effector.py index 46e24a90a..9993916e1 100644 --- a/scripts/tutorials/atomic_action/move_end_effector.py +++ b/scripts/tutorials/atomic_action/move_end_effector.py @@ -29,7 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionInvocation, AtomicActionEngine, EndEffectorPoseGoal, MotionPolicy, @@ -93,15 +92,10 @@ def main() -> None: compiled = engine.compile( ( - ActionInvocation( - skill_id="move_end_effector", - goal=EndEffectorPoseGoal( - broadcast_waypoint_pose_batch(poses, num_envs) - ), - binding=engine.bind_control_parts( - "move_end_effector", - {"primary": {"motion": "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 9083e8a39..797e157c4 100644 --- a/scripts/tutorials/atomic_action/move_held_object.py +++ b/scripts/tutorials/atomic_action/move_held_object.py @@ -30,7 +30,6 @@ from embodichain.data import get_data_path from embodichain.lab.sim.atomic_actions import ( - ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, EndEffectorPoseGoal, @@ -149,34 +148,22 @@ def main() -> None: motion_mapping = {"primary": {"motion": "arm"}} manipulation_mapping = {"primary": {"motion": "arm", "grasp": "hand"}} - move_binding = engine.bind_control_parts( - "move_end_effector", - motion_mapping, - ) - pick_binding = engine.bind_control_parts( - "pick_up", - manipulation_mapping, - ) - held_object_binding = engine.bind_control_parts( - "move_held_object", - manipulation_mapping, - ) compiled = engine.compile( ( - ActionInvocation( + engine.make_invocation( "move_end_effector", EndEffectorPoseGoal(move_target), - move_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), - pick_binding, - MotionPolicy( + control_parts=manipulation_mapping, + motion_policy=MotionPolicy( strategy="motion_gen", sample_count=PICK_SAMPLE_INTERVAL, ), @@ -186,11 +173,11 @@ def main() -> None: hand_interp_steps=HAND_INTERP_STEPS, ), ), - ActionInvocation( + engine.make_invocation( "move_held_object", HeldObjectPoseGoal(object_target), - held_object_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 d0caf61c1..0a35a5b9f 100644 --- a/scripts/tutorials/atomic_action/move_joints.py +++ b/scripts/tutorials/atomic_action/move_joints.py @@ -29,7 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, JointPositionGoal, @@ -99,21 +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 = engine.bind_control_parts( - "move_joints", - {"primary": {"motion": "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 8eb59cfb8..a31ee0c66 100644 --- a/scripts/tutorials/atomic_action/moving_target_recovery.py +++ b/scripts/tutorials/atomic_action/moving_target_recovery.py @@ -31,7 +31,6 @@ from embodichain.lab.sim import SimulationManager, VisualMaterialCfg from embodichain.lab.sim.atomic_actions import ( - ActionInvocation, Affordance, AtomicActionEngine, ControlPartCommandProfile, @@ -286,20 +285,16 @@ def main() -> None: ) }, ) - binding = engine.bind_control_parts( + pick_invocation = engine.make_invocation( "pick_up", - {"primary": {"motion": "arm", "grasp": "hand"}}, - ) - pick_invocation = ActionInvocation( - skill_id="pick_up", - goal=GraspGoal( + 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 7a59e3e45..b5450f6bc 100644 --- a/scripts/tutorials/atomic_action/pickup.py +++ b/scripts/tutorials/atomic_action/pickup.py @@ -29,7 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, GraspGoal, @@ -154,13 +153,10 @@ def main() -> None: compiled = engine.compile( ( - ActionInvocation( - skill_id="pick_up", - goal=GraspGoal(semantics), - binding=engine.bind_control_parts( - "pick_up", - {"primary": {"motion": "arm", "grasp": "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 9817df5f7..17a1bad8d 100644 --- a/scripts/tutorials/atomic_action/place.py +++ b/scripts/tutorials/atomic_action/place.py @@ -29,7 +29,6 @@ import torch from embodichain.lab.sim.atomic_actions import ( - ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, GraspGoal, @@ -156,21 +155,13 @@ def main() -> None: ) endpoint_mapping = {"primary": {"motion": "arm", "grasp": "hand"}} - pick_binding = engine.bind_control_parts( - "pick_up", - endpoint_mapping, - ) - place_binding = engine.bind_control_parts( - "place", - endpoint_mapping, - ) compiled = engine.compile( ( - ActionInvocation( + engine.make_invocation( "pick_up", GraspGoal(semantics), - pick_binding, - MotionPolicy( + control_parts=endpoint_mapping, + motion_policy=MotionPolicy( strategy="motion_gen", sample_count=PICK_SAMPLE_INTERVAL, ), @@ -180,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] ) ), - place_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 427302476..384b836ad 100644 --- a/scripts/tutorials/atomic_action/press.py +++ b/scripts/tutorials/atomic_action/press.py @@ -30,7 +30,6 @@ from embodichain.data import get_data_path from embodichain.lab.sim.atomic_actions import ( - ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, EntityState, @@ -207,19 +206,15 @@ def main() -> None: "Inspect the button target, then press Enter to plan Press...", ) - press_binding = engine.bind_control_parts( - "press", - {"primary": {"motion": "arm", "grasp": "hand"}}, - ) compiled = engine.compile( ( - ActionInvocation( - skill_id="press", - goal=PressGoal( + engine.make_invocation( + "press", + PressGoal( semantics, SceneEntityPose(BUTTON_SCENE_ENTITY_ID), ), - binding=press_binding, + 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 2edb65d4f..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,8 +167,8 @@ def create_drawer_semantics( def create_invocation( + engine: AtomicActionEngine, semantics: ObjectSemantics, - binding: ActionBinding, *, direction: Literal["pull", "push"], approach_distance: float, @@ -178,8 +177,8 @@ 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. - binding: Engine-owned motion and grasp endpoint binding. direction: Whether this invocation pulls open or pushes closed. approach_distance: Pre-grasp offset opposite the approach axis. translation_distance: Drawer travel distance for this operation. @@ -187,13 +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=binding, + control_parts={"primary": {"motion": "arm", "grasp": "hand"}}, motion_policy=MotionPolicy(sample_count=TRAJECTORY_SAMPLE_COUNT), skill_options=SlideOptions( direction=direction, @@ -249,10 +248,6 @@ def main() -> None: args, "Inspect the closed drawer, then press Enter to plan the pull...", ) - slide_binding = engine.bind_control_parts( - "slide", - {"primary": {"motion": "arm", "grasp": "hand"}}, - ) for scene_version, direction in enumerate(("pull", "push")): if direction == "push" and wait_for_user: @@ -265,8 +260,8 @@ def main() -> None: compiled = engine.compile( ( create_invocation( + engine, semantics, - slide_binding, direction=direction, approach_distance=args.approach_distance, translation_distance=args.translation_distance, diff --git a/scripts/tutorials/atomic_action/twist.py b/scripts/tutorials/atomic_action/twist.py index 4c101c7f8..44f6ebec6 100644 --- a/scripts/tutorials/atomic_action/twist.py +++ b/scripts/tutorials/atomic_action/twist.py @@ -30,7 +30,6 @@ from embodichain.data import get_data_path from embodichain.lab.sim.atomic_actions import ( - ActionInvocation, AtomicActionEngine, ControlPartCommandProfile, EntityState, @@ -187,20 +186,16 @@ def main() -> None: args, "Inspect the knob target, then press Enter to plan Twist...", ) - twist_binding = engine.bind_control_parts( - "twist", - {"primary": {"motion": "arm", "grasp": "hand"}}, - ) compiled = engine.compile( ( - ActionInvocation( - skill_id="twist", - goal=TwistGoal( + engine.make_invocation( + "twist", + TwistGoal( semantics, SceneEntityPose(KNOB_SCENE_ENTITY_ID), ), - binding=twist_binding, + 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/sim/atomic_actions/test_engine.py b/tests/sim/atomic_actions/test_engine.py index 2240fb5c9..da8b75408 100644 --- a/tests/sim/atomic_actions/test_engine.py +++ b/tests/sim/atomic_actions/test_engine.py @@ -51,6 +51,12 @@ SkillResourceSlot, TimedTrajectory, ) +from embodichain.lab.sim.skills import ( + ControlPartEndpoint, + ResourceBinding, + RobotResource, + RobotSkillProfile, +) ACTION_DT = 0.02 @@ -351,6 +357,104 @@ def test_engine_resolves_action_binding_from_robot_control_parts() -> None: 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"}), + }, + ) + ) + + invocation = engine.make_invocation( + "stub", + JointPositionGoal(torch.ones(2, 3)), + motion_policy=MotionPolicy(sample_count=2), + ) + endpoint = invocation.binding.endpoint("primary", "motion") + + assert endpoint.resource_id == "whole_robot" + assert endpoint.require_target(JointPositionTarget).control_part == "all" + assert engine.plan(invocation).plan_success.tolist() == [True, True] + + +def test_engine_make_invocation_requires_direct_binding_without_profile() -> None: + engine = _engine() + engine.register(StubAction()) + + with pytest.raises(ValueError, match="control_parts is required"): + engine.make_invocation( + "stub", + JointPositionGoal(torch.ones(2, 3)), + ) + + +def test_engine_make_invocation_rejects_resources_without_profile() -> None: + engine = _engine() + engine.register(StubAction()) + + with pytest.raises(ValueError, match="requires a bound RobotSkillProfile"): + engine.make_invocation( + "stub", + JointPositionGoal(torch.ones(2, 3)), + resources={"primary": "whole_robot"}, + ) + + +def test_engine_make_invocation_rejects_conflicting_binding_sources() -> None: + engine = _engine() + engine.register(StubAction()) + + with pytest.raises(ValueError, match="mutually exclusive"): + engine.make_invocation( + "stub", + JointPositionGoal(torch.ones(2, 3)), + control_parts={"primary": {"motion": "all"}}, + resources={"primary": "whole_robot"}, + ) + + def test_engine_resolves_invocation_control_override_into_request() -> None: engine = _engine( robot_dof=3, 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 8cfd6e2cd..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(): diff --git a/tests/sim/planners/test_motion_generator_batched.py b/tests/sim/planners/test_motion_generator_batched.py index d2ba14127..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, *, @@ -146,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) @@ -189,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 @@ -202,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() @@ -220,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 = { @@ -243,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): @@ -263,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() @@ -278,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 @@ -326,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/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: From f6e2b00bd1c5a2768f222b72fc2162cc4b4dc9d6 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Thu, 20 Aug 2026 00:29:35 +0800 Subject: [PATCH 5/5] wip --- tests/sim/atomic_actions/test_actions.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/tests/sim/atomic_actions/test_actions.py b/tests/sim/atomic_actions/test_actions.py index a99810939..89118dda4 100644 --- a/tests/sim/atomic_actions/test_actions.py +++ b/tests/sim/atomic_actions/test_actions.py @@ -192,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() @@ -432,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()