diff --git a/.agents/skills/add-atomic-action/SKILL.md b/.agents/skills/add-atomic-action/SKILL.md index d2e3b00a5..18bffb961 100644 --- a/.agents/skills/add-atomic-action/SKILL.md +++ b/.agents/skills/add-atomic-action/SKILL.md @@ -5,9 +5,11 @@ description: Add a new simulation atomic action or motion primitive to EmbodiCha # Add Atomic Action -Add an action-owned goal and a side-effect-free `AtomicAction.plan()` -implementation. Keep task-graph/MLLM logic, simulator stepping, controller I/O, -and physical-effect commits outside the action. +Add an action-owned goal and a side-effect-free `AtomicAction._plan()` +implementation. The inherited public `plan()` entry point binds the current +collision scene before calling the skill hook. Keep task-graph/MLLM logic, +simulator stepping, controller I/O, and physical-effect commits outside the +action. ## Read the current contracts @@ -20,10 +22,12 @@ Inspect only the files relevant to the requested skill: | Role-to-resource binding | `embodichain/lab/sim/atomic_actions/bindings.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` | | Trajectory helpers | `embodichain/lab/sim/atomic_actions/trajectory.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` | The public contract is: @@ -107,7 +111,7 @@ class Push(AtomicAction[PushGoal]): super().__init__(motion_generator, cfg or PushCfg()) self.builder = TrajectoryBuilder(motion_generator) - def plan( + def _plan( self, invocation: ActionInvocation[PushGoal], context: PlanningContext, @@ -146,6 +150,9 @@ class Push(AtomicAction[PushGoal]): Follow these invariants: - Call `require_goal()` before planning. +- Implement `_plan()` rather than overriding the framework-owned public + `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`. @@ -158,6 +165,9 @@ Follow these invariants: applies them only after verification. - Set `scene_dependencies` indirectly by using `SceneEntityPose` in the goal; `build_plan()` records them for dynamic invalidation. +- Do not add dynamic-obstacle arguments to a skill. A `SceneProvider` declares + `collision_entity_ids`; supported planners receive those entity poses through + the framework-owned `plan()` entry point. ## 4. Register and invoke @@ -186,8 +196,10 @@ invocation = ActionInvocation( compiled = engine.compile((invocation,)) ``` -Use `engine.start(...).tick(...)` instead when dynamic scene updates or online -error recovery are required. +For dynamic scene updates or online error recovery, create a session with +`engine.start(...)`, then connect it to observation, command, and clock ports +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 @@ -212,6 +224,8 @@ Add pure pytest tests under `tests/sim/atomic_actions/`. Cover: - side-effect-free context handling; - masked `StateDelta` application for task effects; - `SceneEntityPose` replanning when the action accepts a dynamic goal; +- collision-world revision replanning when the action uses a dynamic-world + planner; - effect verification when the action declares a non-empty delta. Run focused tests, format changed Python files with the pinned Black version, @@ -229,4 +243,5 @@ then use the `pre-commit-check` skill before committing. | Return an arm-only tensor | Embed into full robot DoF. | | 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; let the caller own execution. | +| Step the simulator from the action | Emit plans; connect execution through `ExecutionRunner`. | +| Override public `plan()` | Implement `_plan()` so scene binding cannot be bypassed. | diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 81dbd04bf..1b6149a87 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -435,6 +435,14 @@ topics: - ActionPlan - PlanningContext - ExecutionSession + - ExecutionRunner + - ObservationProvider + - CommandSink + - SimulationExecutionAdapter + - SceneProvider + - RigidObjectSceneProvider + - collision world revision + - dynamic obstacle - StateDelta - held_objects - ActionBinding @@ -455,6 +463,9 @@ topics: - embodichain/lab/sim/atomic_actions/state.py - embodichain/lab/sim/atomic_actions/plans.py - embodichain/lab/sim/atomic_actions/execution.py + - embodichain/lab/sim/atomic_actions/runner.py + - embodichain/lab/sim/atomic_actions/scene.py + - embodichain/lab/sim/atomic_actions/sim_adapter.py - embodichain/lab/sim/atomic_actions/engine.py - embodichain/lab/sim/atomic_actions/trajectory.py - embodichain/lab/sim/atomic_actions/primitives/ diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 9441ee8a0..fd108dd47 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -8,6 +8,11 @@ Atomic actions are side-effect-free, environment-batched planners: plan = action.plan(invocation: ActionInvocation, context: PlanningContext) ``` +`plan()` is the framework-owned public template method. It binds collision +entities from the current scene into a copied planner policy, then delegates to +the skill-specific `_plan()` hook. New actions must implement `_plan()` and +must not override `plan()`. + There is no `ActionTarget`, `WorldState`, `ActionResult`, `execute()`, or `AtomicActionEngine.run()` compatibility surface. @@ -46,13 +51,22 @@ snapshot every time the action plans. Its entity ID is recorded in ```python session = engine.start(invocations, initial_context) -tick = session.tick(latest_context, effect_success=None) +runner = ExecutionRunner( + session, + observation_provider, + command_sink, + clock=execution_clock, +) +result = runner.step(effect_success=None) ``` -An `ExecutionSession` emits at most one `JointCommand` per tick and monitors: +`ExecutionSession` owns deterministic planning progress and recovery state. It +emits at most one `JointCommand` per tick and monitors: - joint tracking error against the previous command; - translation/rotation drift of referenced scene entities; +- per-environment collision-world revision changes for collision-sensitive + phases; - phase timeout; - planner and semantic-effect failure. @@ -61,6 +75,60 @@ or exhausted failures are reported as structured `ExecutionEvent` objects. A non-empty `StateDelta` is not committed until the caller supplies an external `effect_success` mask. +`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; +- `ExecutionClock` supplies monotonic time and backend waiting; +- non-blocking `step()` dispatches only when the current command's + `hold_duration` has elapsed; +- `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. + +`TimedTrajectory.dt[:, i]` is the interval leading to sample `i`. +`ExecutionSession` maps this to `JointCommand.hold_duration`; the final sample +uses its own interval 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 +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. + +`SceneProvider.snapshot(timestamp=..., env_ids=...)` is the scene-observation +boundary used by execution adapters. `SceneSnapshot.collision_entity_ids` +identifies obstacle poses consumed by a planner, while +`collision_world_revision` is either global or per environment. A newer +revision invalidates only affected batch rows. `RigidObjectSceneProvider` +tracks live simulation objects, filters sub-threshold pose noise, advances the +general scene version, and maintains per-environment collision revisions. + +The public `AtomicAction.plan()` copies `MotionPolicy` and forwards collision +entity poses through `BasePlanner.with_collision_world()`. Backends opt in via +`supports_collision_world_updates`; cuRobo implements this bridge using +`CuroboPlanOptions.dynamic_obstacle_poses`. Thus replanning uses 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. + +The latest validated session context is retained for safe hold if the first +live observation fails. Environment IDs must remain stable and ordered for the +entire session; robot and scene timestamps and scene versions must be monotonic. + +Runnable closed-loop examples live under `scripts/tutorials/atomic_action/`: +`tracking_error_recovery.py`, `moving_target_recovery.py`, and +`dynamic_obstacle_recovery.py`. Each injects one disturbance, reports the +structured invalidation/replan events, and requires terminal completion. + ## Parameter ownership Goal dataclasses carry only semantic task intent. They do not carry robot part @@ -90,10 +158,12 @@ lift distances, and grasp constraints. 1. Define a frozen action-owned goal dataclass with `goal_kind`. 2. Declare `skill_id`, `GoalType`, and required semantic roles on the action. -3. Validate with `require_goal(invocation)`. -4. Plan from `context.robot.qpos`; never read an implicit live start state. -5. Return full-robot positions or a `TimedTrajectory` through `build_plan()`. -6. Declare symbolic changes with `StateDelta`; do not mutate context or commit +3. Implement `_plan()`; do not override the framework-owned `plan()` method. +4. Validate with `require_goal(invocation)`. +5. Plan from `context.robot.qpos`; never read an implicit live start state. +6. Return full-robot positions or a `TimedTrajectory` through `build_plan()`. +7. Declare symbolic changes with `StateDelta`; do not mutate context or commit physical effects during planning. -7. Keep scene stepping, controller I/O, and task-graph/MLLM logic outside the - atomic action. +8. Keep scene stepping, controller I/O, and task-graph/MLLM logic outside the + atomic action. Put execution-loop I/O behind the runner protocols rather than + calling a simulator or device from `plan()` or `ExecutionSession`. diff --git a/agent_context/topics/motion-planning/motion-planning.md b/agent_context/topics/motion-planning/motion-planning.md index 5a036ea9a..bbc710b38 100644 --- a/agent_context/topics/motion-planning/motion-planning.md +++ b/agent_context/topics/motion-planning/motion-planning.md @@ -113,6 +113,15 @@ 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 +`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`. Atomic actions call this +hook from their framework-owned `plan()` template when a `SceneSnapshot` +declares collision entities; individual skills must not construct backend +obstacle options themselves. + ### MotionGenerator Unified interface for trajectory planning with optional pre-interpolation. @@ -198,14 +207,17 @@ Helper: `PlanResult.is_all_success() -> bool` returns `True` only when every env 1. Create a `BasePlanner` subclass with a `plan()` method decorated with `@validate_plan_options`. 2. Create a `BasePlannerCfg` subclass with a unique `planner_type` string. 3. Optionally create a `PlanOptions` subclass for planner-specific options. -4. Register in `MotionGenerator._support_planner_dict`: +4. For a planner that accepts live obstacles, set + `supports_collision_world_updates = True` and implement + `with_collision_world()` without mutating caller-owned reusable options. +5. Register in `MotionGenerator._support_planner_dict`: ```python _support_planner_dict = { "toppra": (ToppraPlanner, ToppraPlannerCfg), "neural": (NeuralPlanner, NeuralPlannerCfg), } ``` -5. Export from `embodichain/lab/sim/planners/__init__.py`. +6. Export from `embodichain/lab/sim/planners/__init__.py`. ### validate_plan_options decorator @@ -232,3 +244,4 @@ 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. 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 5e7990410..5e2ab756b 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 @@ -31,9 +31,23 @@ embodichain.lab.sim.atomic_actions AtomicAction AtomicActionEngine ExecutionSession + ExecutionRunner + ExecutionRunnerCfg + RunnerStep + RunnerStatus + ObservationProvider + CommandSink + CommandAcknowledgement + CommandAckStatus + CommandDispatch + CommandOperation + ExecutionClock + SimulationExecutionAdapter ExecutionTick JointCommand ExecutionEvent + ExecutionEventKind + ExecutionStatus .. rubric:: Built-in goals and actions @@ -120,6 +134,46 @@ Engine and execution .. autoclass:: ExecutionSession :members: +.. autoclass:: ExecutionRunner + :members: + +.. autoclass:: ExecutionRunnerCfg + :members: + :exclude-members: __init__, copy, replace, to_dict + +.. autoclass:: ObservationProvider + :members: + +.. autoclass:: CommandSink + :members: + +.. autoclass:: ExecutionClock + :members: + +.. autoclass:: MonotonicExecutionClock + :members: + +.. autoclass:: SimulationExecutionAdapter + :members: + +.. autoclass:: CommandAcknowledgement + :members: + +.. autoclass:: CommandAckStatus + :members: + +.. autoclass:: CommandDispatch + :members: + +.. autoclass:: CommandOperation + :members: + +.. autoclass:: RunnerStep + :members: + +.. autoclass:: RunnerStatus + :members: + .. autoclass:: ExecutionTick :members: @@ -129,6 +183,12 @@ Engine and execution .. autoclass:: ExecutionEvent :members: +.. autoclass:: ExecutionEventKind + :members: + +.. autoclass:: ExecutionStatus + :members: + Semantic objects and helpers ---------------------------- diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index 286023b90..542e16e18 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -7,7 +7,7 @@ builtin_actions ``` Atomic actions turn typed, grounded skill requests into full-robot timed motion. -Planning is side-effect free and execution is incremental. +Planning is side-effect free; execution is incremental and controller-independent. ```text Action Agent / task graph @@ -21,11 +21,18 @@ grounder + capability binder AtomicAction.plan(invocation, PlanningContext) | | ActionPlan + StateDelta - +------------------------------+ - | | - v v -AtomicActionEngine.compile ExecutionSession.tick -(fixed-scene/offline) (dynamic/closed-loop) + +---------------------------------+ + | | + v v +AtomicActionEngine.compile ExecutionSession +(fixed-scene/offline) (recovery state machine) + | + ExecutionRunner.step / run_until_blocked + ^ | + fresh observation timed JointCommand + | v + ObservationProvider CommandSink + + ExecutionClock ``` ## Contracts @@ -39,8 +46,17 @@ AtomicActionEngine.compile ExecutionSession.tick and phase timeout. - `PlanningContext`: measured `RobotObservation`, verified `TaskState`, versioned `SceneSnapshot`, and stable environment IDs. +- `SceneProvider`: captures ordered scene entities plus global or per-environment + collision-world revisions. `RigidObjectSceneProvider` supplies this contract + for live simulation objects. - `ActionPlan`: one or more scene-bound phases, timed trajectories, completion conditions, diagnostics, and uncommitted `StateDelta` effects. +- `ObservationProvider`: captures a fresh `PlanningContext` for each due + feedback cycle. +- `CommandSink`: sends, holds, or cancels commands and returns a structured + acknowledgement. +- `ExecutionClock`: supplies monotonic scheduling; simulation adapters advance + physics while real backends use wall or controller time. ## Static and dynamic use @@ -48,11 +64,28 @@ AtomicActionEngine.compile ExecutionSession.tick `CompiledTrajectory`. It projects terminal qpos and expected task effects only inside the returned context; it never changes simulator state. +The public `AtomicAction.plan()` template method binds collision entity poses +into copied backend options, then calls the skill-specific `_plan()` hook. +Individual skills therefore do not own dynamic-obstacle parameters. + `AtomicActionEngine.start()` creates an `ExecutionSession`. Each `tick()` takes the latest context and emits at most one `JointCommand`. The session detects -tracking error, phase timeout, and movement of entities referenced by -`SceneEntityPose`, then replans from the latest observation within the configured -budget. Non-empty symbolic effects require external verification before commit. +tracking error, phase timeout, movement of entities referenced by +`SceneEntityPose`, and newer collision-world revisions, then replans from the +latest observation within the configured budget. Collision revisions are +per-environment when the provider can identify affected rows. Non-empty symbolic +effects require external verification before commit. + +`ExecutionRunner` owns the outer execution lifecycle. Its non-blocking `step()` +observes only when the next waypoint is due, dispatches commands using the +trajectory's per-sample `dt`, and records acknowledgements. Its convenience +`run_until_blocked()` loop sleeps 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 observation and command protocols +without changing action planning or recovery state. ## Example @@ -70,5 +103,16 @@ engine.register(MoveEndEffector(motion_generator, MoveEndEffectorCfg())) compiled = engine.compile((invocation,)) ``` +For feedback-driven execution: + +```python +adapter = SimulationExecutionAdapter(sim, robot) +initial = adapter.observe(TaskState.empty(robot.get_qpos().shape[0], robot.device)) +session = engine.start((invocation,), initial) +runner = ExecutionRunner(session, adapter, adapter, clock=adapter) +result = runner.run_until_blocked() +``` + See [Built-in actions](builtin_actions.md) for the shipped skill catalog and -[the tutorial](../../../tutorial/atomic_actions.rst) for closed-loop usage. +[the tutorial](../../../tutorial/atomic_actions.rst) for closed-loop usage and +the runnable tracking-error and dynamic-obstacle recovery examples. diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index c2777124e..924b1eba5 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -62,6 +62,7 @@ must be resolved from the latest scene snapshot: from embodichain.lab.sim.atomic_actions import ( EndEffectorPoseGoal, RecoveryPolicy, + RigidObjectSceneProvider, SceneEntityPose, ) @@ -78,16 +79,58 @@ must be resolved from the latest scene snapshot: ), ) + from embodichain.lab.sim.atomic_actions import ( + ExecutionRunner, + SimulationExecutionAdapter, + TaskState, + ) + + scene_provider = RigidObjectSceneProvider({"moving_tray": moving_tray}) + adapter = SimulationExecutionAdapter( + sim, + robot, + scene_provider=scene_provider, + ) + task = TaskState.empty(robot.get_qpos().shape[0], robot.device) + initial_context = adapter.observe(task) session = engine.start((invocation,), initial_context) - while session.status.value == "running": - tick = session.tick(latest_context) - if tick.command is not None: - send_joint_command(tick.command) + runner = ExecutionRunner(session, adapter, adapter, clock=adapter) + result = runner.run_until_blocked() + +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. + +For an application that already owns its event loop, call the non-blocking +:meth:`~embodichain.lab.sim.atomic_actions.ExecutionRunner.step` method. A step +with ``is_waiting`` set has not consumed a new observation or effect result; use +its ``wait_duration`` to schedule the next call. + +The complete simulation example deliberately changes a measured joint position, +observes ``tracking_error`` and ``replanned`` events, and finishes the regenerated +trajectory: + +.. code-block:: bash -The session emits one command per tick. It compares observations with the last -command, detects material motion of referenced scene entities, enforces phase -timeouts, and replans from the latest observation within the recovery budget. -It does not own the simulator or controller loop. + python scripts/tutorials/atomic_action/tracking_error_recovery.py --headless + +The moving-goal counterpart changes a rigid object's pose while an +``EndEffectorPoseGoal(SceneEntityPose(...))`` is executing: + +.. code-block:: bash + + python scripts/tutorials/atomic_action/moving_target_recovery.py --headless + +For collision-aware execution, declare tracked rigid objects as collision +entities. The provider advances a per-environment collision-world revision when +an obstacle moves; the active phase is invalidated and a supporting planner, +such as cuRobo, receives the latest obstacle poses during replanning: + +.. code-block:: bash + + python scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py --headless Task-state effects ------------------ @@ -99,19 +142,26 @@ external per-environment verification mask: .. code-block:: python - tick = session.tick(latest_context) - if any(event.kind.value == "effect_verification_required" for event in tick.events): - verified = verify_grasp_or_release() - tick = session.tick(latest_context, effect_success=verified) + def verify_effect(context, tick): + return verify_grasp_or_release(context) + + result = runner.run_until_blocked(effect_verifier=verify_effect) This prevents a successful trajectory plan from being mistaken for a successful -physical grasp or release. +physical grasp or release. If verification is asynchronous, omit the callback; +``run_until_blocked`` returns at the verification boundary and the application +can later resume with ``runner.step(effect_success=verified)`` when the next +cycle is due, or call ``run_until_blocked(effect_verifier=...)`` again. The +runner remembers the pending boundary even though the session emits its event +only once. Adding an action ---------------- Define an action-owned frozen goal dataclass with a stable ``goal_kind``. Then -implement ``plan(invocation, context)`` and declare the stable skill metadata: +implement the protected ``_plan(invocation, context)`` hook and declare the +stable skill metadata. The inherited public ``plan()`` method must not be +overridden because it binds the latest collision scene first: .. code-block:: python @@ -128,7 +178,7 @@ implement ``plan(invocation, context)`` and declare the stable skill metadata: GoalType: ClassVar[type] = PushGoal manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) - def plan( + def _plan( self, invocation: ActionInvocation[PushGoal], context: PlanningContext, diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index e196d8664..06f9caef6 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -20,7 +20,9 @@ through :meth:`AtomicAction.plan`. Planning is side-effect free: it returns an :class:`ActionPlan` with timed motion, completion criteria, diagnostics, and uncommitted expected task-state effects. :class:`AtomicActionEngine` can compile -a static sequence; closed-loop execution belongs to an execution session. +a static sequence. For closed-loop use, :class:`ExecutionSession` owns recovery +state while :class:`ExecutionRunner` connects it to observations, commands, and +time. """ from __future__ import annotations @@ -91,6 +93,28 @@ PressCfg, PressGoal, ) +from .runner import ( + CommandAcknowledgement, + CommandAckStatus, + CommandDispatch, + CommandOperation, + CommandSink, + EffectVerifier, + ExecutionClock, + ExecutionRunner, + ExecutionRunnerCfg, + MonotonicExecutionClock, + ObservationProvider, + RunnerStatus, + RunnerStep, + RunnerStepCallback, +) +from .scene import SceneProvider +from .sim_adapter import ( + RigidObjectSceneProvider, + RigidObjectSceneProviderCfg, + SimulationExecutionAdapter, +) from .state import ( CoordinatedHeldObjectState, EntityState, @@ -115,6 +139,11 @@ "AtomicAction", "AtomicActionEngine", "CompiledTrajectory", + "CommandAcknowledgement", + "CommandAckStatus", + "CommandDispatch", + "CommandOperation", + "CommandSink", "CompletionCondition", "CompletionConditionKind", "CoordinatedHeldObjectState", @@ -126,8 +155,12 @@ "CoordinatedPlacementGoal", "EndEffectorPoseGoal", "EntityState", + "EffectVerifier", + "ExecutionClock", "ExecutionEvent", "ExecutionEventKind", + "ExecutionRunner", + "ExecutionRunnerCfg", "ExecutionSession", "ExecutionStatus", "ExecutionTick", @@ -140,6 +173,7 @@ "JointPositionGoal", "JointCommand", "MotionPolicy", + "MonotonicExecutionClock", "MoveEndEffector", "MoveEndEffectorCfg", "MoveHeldObject", @@ -149,6 +183,7 @@ "NamedJointPositionGoal", "ObjectActionGoal", "ObjectSemantics", + "ObservationProvider", "PhaseSpec", "PickUp", "PickUpCfg", @@ -163,11 +198,18 @@ "PressCfg", "PressGoal", "RecoveryPolicy", + "RigidObjectSceneProvider", + "RigidObjectSceneProviderCfg", "RobotObservation", + "RunnerStatus", + "RunnerStep", + "RunnerStepCallback", + "SceneProvider", "SceneSnapshot", "SceneEntityPose", "SkillDescriptor", "StateDelta", + "SimulationExecutionAdapter", "TaskState", "TimedTrajectory", "TrajectoryBuilder", diff --git a/embodichain/lab/sim/atomic_actions/core.py b/embodichain/lab/sim/atomic_actions/core.py index 6f49f52fa..fb9312f81 100644 --- a/embodichain/lab/sim/atomic_actions/core.py +++ b/embodichain/lab/sim/atomic_actions/core.py @@ -19,7 +19,8 @@ from __future__ import annotations from abc import ABC, abstractmethod -from dataclasses import dataclass, field +from copy import deepcopy +from dataclasses import dataclass, field, replace from typing import Any, ClassVar, Generic, TYPE_CHECKING import torch @@ -149,6 +150,15 @@ class AtomicAction(Generic[GoalT], ABC): agent_visible: ClassVar[bool] = True """Whether an Action Agent should expose this skill by default.""" + def __init_subclass__(cls, **kwargs: Any) -> None: + """Reject skill classes that bypass framework-owned scene binding.""" + super().__init_subclass__(**kwargs) + if "plan" in cls.__dict__: + raise TypeError( + "AtomicAction subclasses must implement _plan(); the public " + "plan() method is framework-owned." + ) + def __init__( self, motion_generator: MotionGenerator, @@ -170,6 +180,69 @@ def descriptor(cls) -> SkillDescriptor: agent_visible=cls.agent_visible, ) + def plan( + self, + invocation: ActionInvocation[GoalT], + context: PlanningContext, + ) -> ActionPlan: + """Plan one invocation against an immutable observed context. + + The public entry point binds collision entities from the scene snapshot + into a copied motion policy before delegating to :meth:`_plan`. This + keeps dynamic obstacle plumbing out of individual skill parameters and + guarantees replanning consumes the latest collision-world snapshot. + + Args: + invocation: Fully typed and embodiment-bound action request. + context: Latest observed robot, task, and scene state. + + Returns: + Scene-bound action plan with expected, uncommitted effects. + """ + self.require_goal(invocation) + prepared = self._prepare_invocation(invocation, context) + return self._plan(prepared, context) + + def _prepare_invocation( + self, + invocation: ActionInvocation[GoalT], + context: PlanningContext, + ) -> ActionInvocation[GoalT]: + """Bind the current collision snapshot without mutating caller policy.""" + if not self._uses_collision_world(invocation, context): + return invocation + planner = self.motion_generator.planner + policy = deepcopy(invocation.motion_policy) + options = ( + deepcopy(policy.plan_opts) + if policy.plan_opts is not None + else planner.default_plan_options() + ) + poses = context.scene.collision_obstacle_poses( + batch_size=context.batch_size, + device=context.robot.qpos.device, + dtype=context.robot.qpos.dtype, + ) + policy.plan_opts = planner.with_collision_world( + options, + obstacle_poses=poses, + ) + return replace(invocation, motion_policy=policy) + + def _uses_collision_world( + self, + invocation: ActionInvocation[GoalT], + context: PlanningContext, + ) -> bool: + """Return whether this planning attempt consumes collision revisions.""" + planner = getattr(self.motion_generator, "planner", None) + return ( + invocation.motion_policy.motion_source == "motion_gen" + and invocation.motion_policy.collision_check + and bool(context.scene.collision_entity_ids) + and getattr(planner, "supports_collision_world_updates", False) is True + ) + def require_goal(self, invocation: ActionInvocation[GoalT]) -> GoalT: """Validate an invocation and return its concrete goal. @@ -304,9 +377,15 @@ def build_plan( ), recovery_policy=invocation.recovery_policy, scene_dependencies=collect_scene_dependencies(invocation.goal), + collision_world_sensitive=self._uses_collision_world( + invocation, context + ), ), trajectory=timed, planned_scene_version=context.scene.version, + planned_collision_world_revision=( + context.scene.collision_world_revisions(context.batch_size) + ), diagnostics=diagnostics, ) return ActionPlan( @@ -358,15 +437,15 @@ def failed_plan( ) @abstractmethod - def plan( + def _plan( self, invocation: ActionInvocation[GoalT], context: PlanningContext, ) -> ActionPlan: - """Plan one invocation without stepping simulation or committing state. + """Implement skill-specific side-effect-free planning. Args: - invocation: Fully typed and embodiment-bound action request. + invocation: Request with context-bound motion planner options. context: Latest observed robot, task, and scene state. Returns: diff --git a/embodichain/lab/sim/atomic_actions/engine.py b/embodichain/lab/sim/atomic_actions/engine.py index 369870604..d270b40c3 100644 --- a/embodichain/lab/sim/atomic_actions/engine.py +++ b/embodichain/lab/sim/atomic_actions/engine.py @@ -281,6 +281,15 @@ def _validate_plan( raise ValueError( "Every action phase must record the planning scene version." ) + collision_revision = context.scene.collision_world_revisions(context.batch_size) + if any( + phase.planned_collision_world_revision != collision_revision + for phase in plan.phases + ): + raise ValueError( + "Every action phase must record the planning collision-world " + "revision." + ) __all__ = [ diff --git a/embodichain/lab/sim/atomic_actions/execution.py b/embodichain/lab/sim/atomic_actions/execution.py index 4def86424..450f974cc 100644 --- a/embodichain/lab/sim/atomic_actions/execution.py +++ b/embodichain/lab/sim/atomic_actions/execution.py @@ -47,6 +47,7 @@ class ExecutionEventKind(str, Enum): REPLANNED = "replanned" TRACKING_ERROR = "tracking_error" DYNAMIC_GOAL_CHANGED = "dynamic_goal_changed" + COLLISION_WORLD_CHANGED = "collision_world_changed" PHASE_TIMEOUT = "phase_timeout" PHASE_COMPLETED = "phase_completed" EFFECT_VERIFICATION_REQUIRED = "effect_verification_required" @@ -86,6 +87,8 @@ class JointCommand: velocities: torch.Tensor | None active_mask: torch.Tensor env_ids: torch.Tensor + hold_duration: torch.Tensor + """Per-environment time to hold this command before the next observation.""" def __post_init__(self) -> None: if self.positions.dim() != 2: @@ -103,15 +106,29 @@ def __post_init__(self) -> None: 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) @@ -194,6 +211,11 @@ def task_state(self) -> TaskState: """Verified symbolic task state accumulated by this session.""" return self._task_state + @property + def latest_context(self) -> PlanningContext: + """Latest validated context with the session's verified task state.""" + return self._context + def tick( self, context: PlanningContext, @@ -218,6 +240,18 @@ def tick( 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( @@ -347,6 +381,13 @@ def _recover_if_needed( ExecutionEventKind.PHASE_TIMEOUT, "Phase timeout exceeded.", ) + collision_mask = execution_mask & self._collision_world_change_mask(phase) + if collision_mask.any(): + return self._attempt_replan( + collision_mask, + ExecutionEventKind.COLLISION_WORLD_CHANGED, + "The collision world changed after this trajectory was planned.", + ) if self._last_command is not None: tracking_error = torch.amax( torch.abs(self._context.robot.qpos - self._last_command), dim=1 @@ -542,11 +583,14 @@ def _command_at( ) self._last_command = positions.clone() self._last_command_mask = active_mask.clone() + next_index = min(waypoint_index + 1, phase.trajectory.waypoint_count - 1) + hold_duration = phase.trajectory.dt[:, next_index] return JointCommand( positions=positions, velocities=velocities, active_mask=active_mask, env_ids=phase.trajectory.env_ids, + hold_duration=hold_duration, ) def _hold_command(self) -> JointCommand: @@ -556,6 +600,11 @@ def _hold_command(self) -> JointCommand: velocities=torch.zeros_like(self._context.robot.qpos), active_mask=torch.zeros_like(self._eligible), env_ids=self._context.env_ids, + hold_duration=torch.zeros( + self._context.batch_size, + dtype=torch.float32, + device=self._context.robot.qpos.device, + ), ) def _terminal_error(self, phase: PlannedPhase) -> torch.Tensor: @@ -601,6 +650,22 @@ def _dynamic_scene_change_mask(self, phase: PlannedPhase) -> torch.Tensor: ) return changed + def _collision_world_change_mask(self, phase: PlannedPhase) -> torch.Tensor: + """Detect collision-world revisions newer than the active phase plan.""" + if not phase.spec.collision_world_sensitive: + return torch.zeros_like(self._eligible) + current = torch.tensor( + self._context.scene.collision_world_revisions(self._context.batch_size), + dtype=torch.long, + device=self._eligible.device, + ) + planned = torch.tensor( + phase.planned_collision_world_revision, + dtype=torch.long, + device=self._eligible.device, + ) + return current > planned + def _batched_entity_pose(self, state: EntityState) -> torch.Tensor: """Broadcast an entity pose to the session batch.""" pose = state.pose.to( diff --git a/embodichain/lab/sim/atomic_actions/plans.py b/embodichain/lab/sim/atomic_actions/plans.py index 5da8dda35..47a6f56a2 100644 --- a/embodichain/lab/sim/atomic_actions/plans.py +++ b/embodichain/lab/sim/atomic_actions/plans.py @@ -361,6 +361,9 @@ class PhaseSpec: scene_dependencies: tuple[str, ...] = () """Scene entities whose motion can invalidate this phase plan.""" + collision_world_sensitive: bool = False + """Whether collision-world revision changes invalidate this phase.""" + def __post_init__(self) -> None: if not isinstance(self.name, str) or not self.name: raise ValueError("PhaseSpec.name must be non-empty.") @@ -372,6 +375,8 @@ def __post_init__(self) -> None: "scene_dependencies must contain unique non-empty entity ids." ) object.__setattr__(self, "scene_dependencies", dependencies) + if not isinstance(self.collision_world_sensitive, bool): + raise TypeError("collision_world_sensitive must be a bool.") @dataclass(frozen=True, slots=True) @@ -381,11 +386,27 @@ class PlannedPhase: spec: PhaseSpec trajectory: TimedTrajectory planned_scene_version: int + planned_collision_world_revision: tuple[int, ...] diagnostics: PlannerDiagnostics def __post_init__(self) -> None: 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: + raise ValueError( + "planned_collision_world_revision must contain one value per " + "trajectory environment." + ) + if any( + isinstance(value, bool) or not isinstance(value, int) or value < 0 + for value in revisions + ): + raise ValueError( + "planned_collision_world_revision must contain non-negative " + "integers." + ) + object.__setattr__(self, "planned_collision_world_revision", revisions) @dataclass(frozen=True, slots=True, eq=False) diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py index 73a744b6c..e8d8ce8b4 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py @@ -757,7 +757,7 @@ def _plan_synchronized_object_motion( self._interpolate_qpos_keyframes(right_traj, keyframe_indices, n_waypoints), ) - def plan( + def _plan( self, invocation: ActionInvocation[CoordinatedPickGoal], context: PlanningContext, diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py index d743e4675..c71c95f38 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py @@ -183,7 +183,7 @@ def __init__( hand_dof=self.support_hand_dof, ) - def plan( + def _plan( self, invocation: ActionInvocation[CoordinatedPlacementGoal], context: PlanningContext, diff --git a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py index 8c251ff0f..7392bdbcc 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py +++ b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py @@ -193,7 +193,7 @@ def __init__( # Public contract # ------------------------------------------------------------------ - def plan( + def _plan( self, invocation: ActionInvocation[GraspGoal], context: PlanningContext, 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 29c38db26..ce34067c2 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py @@ -69,7 +69,7 @@ def __init__( super().__init__(motion_generator, cfg or MoveEndEffectorCfg()) self.builder = TrajectoryBuilder(motion_generator) - def plan( + def _plan( self, invocation: ActionInvocation[EndEffectorPoseGoal], context: PlanningContext, 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 0705e6840..68bc99b43 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py @@ -104,7 +104,7 @@ def __init__( ) self.hand_close_qpos = self.cfg.hand_close_qpos.to(self.device) - def plan( + def _plan( self, invocation: ActionInvocation[HeldObjectPoseGoal], context: PlanningContext, diff --git a/embodichain/lab/sim/atomic_actions/primitives/move_joints.py b/embodichain/lab/sim/atomic_actions/primitives/move_joints.py index 3fc9fcc83..0e5ebc295 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_joints.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_joints.py @@ -99,7 +99,7 @@ def __init__( self.builder = TrajectoryBuilder(motion_generator) self.named_joint_positions = self.cfg.named_joint_positions or {} - def plan( + def _plan( self, invocation: ActionInvocation[JointPositionGoal | NamedJointPositionGoal], context: PlanningContext, diff --git a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py index 4a812802d..df58747db 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py @@ -238,7 +238,7 @@ def _get_full_pickup_trajectory( ) return is_success, full - def plan( + def _plan( self, invocation: ActionInvocation[GraspGoal], context: PlanningContext, diff --git a/embodichain/lab/sim/atomic_actions/primitives/place.py b/embodichain/lab/sim/atomic_actions/primitives/place.py index 80b4c47c7..fa0a8af14 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/place.py +++ b/embodichain/lab/sim/atomic_actions/primitives/place.py @@ -169,7 +169,7 @@ def __init__( if self.cfg.cartesian_waypoint_count < 1: logger.log_error("cartesian_waypoint_count must be at least 1.", ValueError) - def plan( + def _plan( self, invocation: ActionInvocation[PlaceGoal | AssembleGoal], context: PlanningContext, diff --git a/embodichain/lab/sim/atomic_actions/primitives/press.py b/embodichain/lab/sim/atomic_actions/primitives/press.py index ddd0ea9c7..2fe0714ce 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/press.py +++ b/embodichain/lab/sim/atomic_actions/primitives/press.py @@ -101,7 +101,7 @@ def __init__( hand_dof=self.hand_dof, ) - def plan( + def _plan( self, invocation: ActionInvocation[PressGoal], context: PlanningContext, diff --git a/embodichain/lab/sim/atomic_actions/runner.py b/embodichain/lab/sim/atomic_actions/runner.py new file mode 100644 index 000000000..9bf077ce5 --- /dev/null +++ b/embodichain/lab/sim/atomic_actions/runner.py @@ -0,0 +1,791 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Controller-independent scheduling for closed-loop atomic-action execution.""" + +from __future__ import annotations + +from collections.abc import Callable +from dataclasses import dataclass +from enum import Enum +import math +import time +from typing import Protocol, runtime_checkable + +import torch + +from embodichain.utils import configclass + +from .execution import ( + ExecutionEventKind, + ExecutionSession, + ExecutionStatus, + ExecutionTick, + JointCommand, +) +from .state import PlanningContext, TaskState + + +class CommandAckStatus(str, Enum): + """Outcome reported by a command transport or controller.""" + + ACCEPTED = "accepted" + REJECTED = "rejected" + TIMED_OUT = "timed_out" + + +@dataclass(frozen=True, slots=True) +class CommandAcknowledgement: + """Synchronous acknowledgement returned by a :class:`CommandSink`.""" + + status: CommandAckStatus + """Transport/controller acknowledgement status.""" + + message: str = "" + """Human-readable diagnostic intended for logs, not policy branching.""" + + def __post_init__(self) -> None: + if not isinstance(self.status, CommandAckStatus): + raise TypeError("status must be a CommandAckStatus.") + if not isinstance(self.message, str): + raise TypeError("message must be a string.") + + @property + def accepted(self) -> bool: + """Whether the controller accepted the requested operation.""" + return self.status is CommandAckStatus.ACCEPTED + + @classmethod + def accepted_ack(cls, message: str = "") -> CommandAcknowledgement: + """Build an accepted acknowledgement. + + Args: + message: Optional controller diagnostic. + + Returns: + Accepted acknowledgement. + """ + return cls(CommandAckStatus.ACCEPTED, message) + + +class CommandOperation(str, Enum): + """Command-sink operation recorded by an execution runner.""" + + SEND = "send" + HOLD = "hold" + CANCEL = "cancel" + + +@dataclass(frozen=True, slots=True) +class CommandDispatch: + """Auditable record of one controller operation and acknowledgement.""" + + operation: CommandOperation + acknowledgement: CommandAcknowledgement + + def __post_init__(self) -> None: + if not isinstance(self.operation, CommandOperation): + raise TypeError("operation must be a CommandOperation.") + if not isinstance(self.acknowledgement, CommandAcknowledgement): + raise TypeError("acknowledgement must be a CommandAcknowledgement.") + + +@runtime_checkable +class ObservationProvider(Protocol): + """Source of fresh planning contexts for feedback-driven execution.""" + + def observe(self, task_state: TaskState) -> PlanningContext: + """Capture the latest robot and scene state. + + Args: + task_state: Runner-owned, externally verified symbolic task state. + + Returns: + Fresh context with stable, ordered environment IDs. + """ + + +@runtime_checkable +class CommandSink(Protocol): + """Controller boundary used by :class:`ExecutionRunner`.""" + + def send( + self, + command: JointCommand, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Submit an active joint command and acknowledge its acceptance. + + Args: + command: Full-robot command with an explicit active mask. Inactive + rows contain hold targets and must not retain stale commands. + timeout: Maximum acknowledgement latency in seconds. + + Returns: + Transport or controller acknowledgement. + """ + + def hold( + self, + command: JointCommand, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Hold the supplied observed position as a safety command. + + Args: + command: Full-robot observed-position hold command. + timeout: Maximum acknowledgement latency in seconds. + + Returns: + Transport or controller acknowledgement. + """ + + def cancel(self, *, timeout: float) -> CommandAcknowledgement: + """Cancel any controller-side command that has not completed. + + Args: + timeout: Maximum acknowledgement latency in seconds. + + Returns: + Transport or controller acknowledgement. + """ + + +@runtime_checkable +class ExecutionClock(Protocol): + """Clock abstraction used for deterministic and simulation scheduling.""" + + def now(self) -> float: + """Return a monotonic timestamp in seconds. + + Returns: + Monotonic timestamp in seconds. + """ + + def sleep(self, duration: float) -> None: + """Wait or advance the execution backend by ``duration`` seconds. + + Args: + duration: Non-negative duration in seconds. + """ + + +class MonotonicExecutionClock: + """Wall-clock implementation backed by :mod:`time`.""" + + def now(self) -> float: + """Return the current monotonic wall-clock time. + + Returns: + Monotonic wall-clock timestamp in seconds. + """ + return time.monotonic() + + def sleep(self, duration: float) -> None: + """Sleep for a non-negative wall-clock duration. + + Args: + duration: Requested duration in seconds. + """ + if not math.isfinite(duration) or duration < 0.0: + raise ValueError("duration must be finite and non-negative.") + time.sleep(duration) + + +@configclass +class ExecutionRunnerCfg: + """Transport and scheduling policy for an :class:`ExecutionRunner`.""" + + command_timeout: float = 1.0 + """Maximum time allowed for a command acknowledgement.""" + + safe_stop_timeout: float = 1.0 + """Maximum time allowed for each cancel or hold acknowledgement.""" + + minimum_cycle_time: float = 1.0e-3 + """Minimum delay between feedback cycles, including passive hold cycles.""" + + hold_on_completion: bool = True + """Whether to issue a final hold after the session completes.""" + + def __post_init__(self) -> None: + for name in ("command_timeout", "safe_stop_timeout"): + value = getattr(self, name) + if not math.isfinite(value) or value <= 0.0: + raise ValueError(f"{name} must be finite and greater than zero.") + if not math.isfinite(self.minimum_cycle_time) or self.minimum_cycle_time < 0.0: + raise ValueError("minimum_cycle_time must be finite and non-negative.") + if not isinstance(self.hold_on_completion, bool): + raise TypeError("hold_on_completion must be a bool.") + + +class RunnerStatus(str, Enum): + """Lifecycle status owned by an :class:`ExecutionRunner`.""" + + RUNNING = "running" + COMPLETED = "completed" + FAILED = "failed" + CANCELLED = "cancelled" + + +@dataclass(frozen=True, slots=True, eq=False) +class RunnerStep: + """Result of one non-blocking execution-runner update.""" + + status: RunnerStatus + timestamp: float + wait_duration: float + context: PlanningContext | None + tick: ExecutionTick | None + dispatches: tuple[CommandDispatch, ...] + command_count: int + message: str | None = None + """Terminal or failure diagnostic, when available.""" + + def __post_init__(self) -> None: + if not isinstance(self.status, RunnerStatus): + raise TypeError("status must be a RunnerStatus.") + if not math.isfinite(self.timestamp) or self.timestamp < 0.0: + raise ValueError("timestamp must be finite and non-negative.") + if not math.isfinite(self.wait_duration) or self.wait_duration < 0.0: + raise ValueError("wait_duration must be finite and non-negative.") + if self.command_count < 0: + raise ValueError("command_count must be non-negative.") + if self.message is not None and not isinstance(self.message, str): + raise TypeError("message must be a string or None.") + object.__setattr__(self, "dispatches", tuple(self.dispatches)) + + @property + def is_waiting(self) -> bool: + """Whether no session tick was due during this update.""" + return ( + self.status is RunnerStatus.RUNNING + and self.tick is None + and self.wait_duration > 0.0 + ) + + +EffectVerifier = Callable[[PlanningContext, ExecutionTick], torch.Tensor | None] +"""Callback that verifies a pending semantic effect for each environment.""" + +RunnerStepCallback = Callable[[RunnerStep], None] +"""Optional observer called after every blocking runner-loop iteration.""" + + +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`. + :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. + + Args: + session: Stateful atomic-action execution session. + observation_provider: Source of fresh robot and scene observations. + command_sink: Controller or simulation command boundary. + clock: Optional scheduler clock. Defaults to monotonic wall time. + cfg: Optional acknowledgement, scheduling, and completion policy. + """ + + def __init__( + self, + session: ExecutionSession, + observation_provider: ObservationProvider, + command_sink: CommandSink, + *, + clock: ExecutionClock | None = None, + cfg: ExecutionRunnerCfg | None = None, + ) -> None: + if not isinstance(session, ExecutionSession): + raise TypeError("session must be an ExecutionSession.") + if not isinstance(observation_provider, ObservationProvider): + raise TypeError("observation_provider must implement ObservationProvider.") + if not isinstance(command_sink, CommandSink): + raise TypeError("command_sink must implement CommandSink.") + if clock is not None and not isinstance(clock, ExecutionClock): + raise TypeError("clock must implement ExecutionClock.") + if cfg is not None and not isinstance(cfg, ExecutionRunnerCfg): + raise TypeError("cfg must be an ExecutionRunnerCfg.") + self._session = session + self._observation_provider = observation_provider + self._command_sink = command_sink + self._clock = clock or MonotonicExecutionClock() + self.cfg = cfg or ExecutionRunnerCfg() + self._status = RunnerStatus.RUNNING + self._next_step_at = self._clock_now() + self._last_context: PlanningContext | None = session.latest_context + self._command_count = 0 + self._message: str | None = None + self._effect_verification_pending = False + self._effect_context: PlanningContext | None = None + self._effect_tick: ExecutionTick | None = None + + @property + def session(self) -> ExecutionSession: + """Execution session advanced by this runner.""" + return self._session + + @property + def status(self) -> RunnerStatus: + """Current runner lifecycle status.""" + return self._status + + @property + def command_count(self) -> int: + """Number of active commands accepted by the sink.""" + return self._command_count + + @property + def effect_verification_pending(self) -> bool: + """Whether execution is waiting for an external semantic-effect result.""" + return self._effect_verification_pending + + def step( + self, + *, + effect_success: torch.Tensor | None = None, + ) -> RunnerStep: + """Perform one due observation/session/controller update without sleeping. + + Args: + effect_success: Optional per-environment verification mask. If this + call occurs before the next cycle is due, it is not consumed and + must be supplied again on a later call. + + Returns: + Runner status, optional session tick, controller acknowledgements, + and time remaining before another update is due. + """ + now = self._clock_now() + if self._status is not RunnerStatus.RUNNING: + return self._result(timestamp=now) + wait_duration = self._remaining_wait(now) + if wait_duration > 0.0: + return self._result( + timestamp=now, + wait_duration=wait_duration, + ) + + try: + context = self._observation_provider.observe(self._session.task_state) + if not isinstance(context, PlanningContext): + raise TypeError( + "ObservationProvider.observe() must return PlanningContext." + ) + except Exception as exc: + return self._fail( + f"Observation provider failed: {type(exc).__name__}: {exc}", + context=self._last_context, + ) + self._last_context = context + + try: + tick = self._session.tick(context, effect_success=effect_success) + except Exception as exc: + return self._fail( + f"Execution session failed: {type(exc).__name__}: {exc}", + context=context, + ) + self._update_effect_boundary(context, tick, effect_success) + + dispatches: list[CommandDispatch] = [] + if tick.command is not None: + operation = ( + CommandOperation.SEND + if bool(tick.command.active_mask.any().item()) + else CommandOperation.HOLD + ) + dispatch = self._dispatch(operation, tick.command) + dispatches.append(dispatch) + if not dispatch.acknowledgement.accepted: + failure = dispatch.acknowledgement + message = ( + "Controller did not accept the requested command: " + f"{failure.status.value}." + ) + if failure.message: + message += f" {failure.message}" + return self._fail( + message, + context=context, + tick=tick, + dispatches=dispatches, + ) + if operation is CommandOperation.SEND: + self._command_count += 1 + interval = self._command_interval(tick.command) + self._next_step_at = self._clock_now() + interval + else: + self._next_step_at = self._clock_now() + + if tick.status is ExecutionStatus.COMPLETED: + if self.cfg.hold_on_completion: + hold_dispatch = self._dispatch( + CommandOperation.HOLD, + self._hold_command(context), + ) + dispatches.append(hold_dispatch) + if not hold_dispatch.acknowledgement.accepted: + failure = hold_dispatch.acknowledgement + message = ( + "Final safety hold was not accepted: " + f"{failure.status.value}." + ) + if failure.message: + message += f" {failure.message}" + return self._fail( + message, + context=context, + tick=tick, + dispatches=dispatches, + ) + self._status = RunnerStatus.COMPLETED + self._next_step_at = self._clock_now() + elif tick.status is ExecutionStatus.FAILED: + return self._fail( + "Execution session exhausted its recovery budget.", + context=context, + tick=tick, + dispatches=dispatches, + ) + + return self._result( + timestamp=self._clock_now(), + context=context, + tick=tick, + dispatches=dispatches, + wait_duration=self._remaining_wait(self._clock_now()), + ) + + def cancel(self, reason: str = "Execution cancelled by caller.") -> RunnerStep: + """Cancel controller work and hold the latest observed position. + + Args: + reason: Human-readable cancellation reason. + + Returns: + Terminal runner step. The status is ``cancelled`` only when both + cancel and hold are acknowledged; otherwise it is ``failed``. + """ + if not isinstance(reason, str) or not reason: + raise ValueError("reason must be a non-empty string.") + now = self._clock_now() + if self._status is not RunnerStatus.RUNNING: + return self._result(timestamp=now) + context = self._observe_for_stop() + dispatches = self._safe_stop(context) + if all(item.acknowledgement.accepted for item in dispatches): + self._status = RunnerStatus.CANCELLED + self._message = reason + else: + self._status = RunnerStatus.FAILED + self._message = f"{reason} Safe stop acknowledgement failed." + self._clear_effect_boundary() + self._next_step_at = self._clock_now() + return self._result( + timestamp=self._clock_now(), + context=context, + dispatches=dispatches, + ) + + def run_until_blocked( + self, + *, + effect_verifier: EffectVerifier | None = None, + on_step: RunnerStepCallback | None = None, + max_steps: int = 100_000, + ) -> RunnerStep: + """Run with clock-driven waiting until terminal or effect verification blocks. + + Args: + effect_verifier: Optional callback used after an + ``effect_verification_required`` event. Without one, the method + returns the running step so the caller can verify externally. + on_step: Optional callback for tracing or tutorial visualization. + max_steps: Hard bound on loop iterations. + + Returns: + Terminal step, or a running step blocked on external verification. + """ + if max_steps <= 0: + raise ValueError("max_steps must be greater than zero.") + pending_effect: torch.Tensor | None = None + now = self._clock_now() + last_result = self._result( + timestamp=now, + wait_duration=self._remaining_wait(now), + context=self._effect_context, + tick=self._effect_tick, + ) + if self._effect_verification_pending: + if ( + effect_verifier is None + or self._effect_context is None + or self._effect_tick is None + ): + return last_result + try: + pending_effect = effect_verifier( + self._effect_context, + self._effect_tick, + ) + except Exception as exc: + return self._fail( + f"Effect verifier failed: {type(exc).__name__}: {exc}", + context=self._effect_context, + tick=self._effect_tick, + ) + if pending_effect is None: + return last_result + for _ in range(max_steps): + result = self.step(effect_success=pending_effect) + if result.tick is not None: + pending_effect = None + if on_step is not None: + try: + on_step(result) + except Exception as exc: + return self._fail( + f"Runner step callback failed: {type(exc).__name__}: {exc}", + context=result.context or self._last_context, + tick=result.tick, + dispatches=list(result.dispatches), + ) + last_result = result + if result.status is not RunnerStatus.RUNNING: + return result + verification_required = result.tick is not None and any( + event.kind is ExecutionEventKind.EFFECT_VERIFICATION_REQUIRED + for event in result.tick.events + ) + if verification_required: + if effect_verifier is None or result.context is None: + return result + try: + pending_effect = effect_verifier(result.context, result.tick) + except Exception as exc: + return self._fail( + f"Effect verifier failed: {type(exc).__name__}: {exc}", + context=result.context, + tick=result.tick, + dispatches=list(result.dispatches), + ) + if pending_effect is None: + return result + if result.wait_duration > 0.0: + try: + self._clock.sleep(result.wait_duration) + except Exception as exc: + return self._fail( + f"Execution clock failed: {type(exc).__name__}: {exc}", + context=result.context or self._last_context, + tick=result.tick, + dispatches=list(result.dispatches), + ) + return self._fail( + f"Execution runner exceeded max_steps={max_steps}.", + context=last_result.context or self._last_context, + tick=last_result.tick, + dispatches=list(last_result.dispatches), + ) + + def _update_effect_boundary( + self, + context: PlanningContext, + tick: ExecutionTick, + effect_success: torch.Tensor | None, + ) -> None: + """Remember or clear the external effect-verification boundary.""" + verification_required = any( + event.kind is ExecutionEventKind.EFFECT_VERIFICATION_REQUIRED + for event in tick.events + ) + if verification_required: + self._effect_verification_pending = True + self._effect_context = context + self._effect_tick = tick + elif effect_success is not None and self._effect_verification_pending: + self._clear_effect_boundary() + + def _clear_effect_boundary(self) -> None: + """Clear a remembered external effect-verification boundary.""" + self._effect_verification_pending = False + self._effect_context = None + self._effect_tick = None + + def _clock_now(self) -> float: + """Read and validate the injected monotonic clock.""" + value = float(self._clock.now()) + if not math.isfinite(value) or value < 0.0: + raise ValueError("ExecutionClock.now() must be finite and non-negative.") + return value + + def _command_interval(self, command: JointCommand) -> float: + """Resolve a synchronized batch interval from per-environment durations.""" + durations = ( + command.hold_duration[command.active_mask] + if command.active_mask.any() + else command.hold_duration + ) + requested = float(durations.max().item()) if durations.numel() else 0.0 + return max(requested, self.cfg.minimum_cycle_time) + + def _remaining_wait(self, now: float) -> float: + """Return scheduled wait while absorbing float32 timing roundoff.""" + remaining = self._next_step_at - now + tolerance = max(1.0e-9, self.cfg.minimum_cycle_time * 1.0e-6) + return remaining if remaining > tolerance else 0.0 + + def _dispatch( + self, + operation: CommandOperation, + command: JointCommand | None, + ) -> CommandDispatch: + """Call one sink operation and convert exceptions to rejection acks.""" + try: + if operation is CommandOperation.SEND: + assert command is not None + acknowledgement = self._command_sink.send( + command, + timeout=self.cfg.command_timeout, + ) + elif operation is CommandOperation.HOLD: + assert command is not None + acknowledgement = self._command_sink.hold( + command, + timeout=self.cfg.safe_stop_timeout, + ) + else: + acknowledgement = self._command_sink.cancel( + timeout=self.cfg.safe_stop_timeout + ) + if not isinstance(acknowledgement, CommandAcknowledgement): + raise TypeError( + "CommandSink methods must return CommandAcknowledgement." + ) + except Exception as exc: + acknowledgement = CommandAcknowledgement( + CommandAckStatus.REJECTED, + f"{type(exc).__name__}: {exc}", + ) + return CommandDispatch(operation, acknowledgement) + + def _observe_for_stop(self) -> PlanningContext | None: + """Best-effort observation used to build a cancellation hold command.""" + try: + context = self._observation_provider.observe(self._session.task_state) + if not isinstance(context, PlanningContext): + return self._last_context + self._last_context = context + return context + except Exception: + return self._last_context + + def _safe_stop( + self, + context: PlanningContext | None, + ) -> list[CommandDispatch]: + """Attempt controller cancellation followed by an observed-position hold.""" + dispatches = [self._dispatch(CommandOperation.CANCEL, None)] + if context is not None: + dispatches.append( + self._dispatch(CommandOperation.HOLD, self._hold_command(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, + *, + context: PlanningContext | None, + tick: ExecutionTick | None = None, + dispatches: list[CommandDispatch] | None = None, + ) -> RunnerStep: + """Enter failed state after a best-effort cancel-then-hold sequence.""" + records = list(dispatches or ()) + records.extend(self._safe_stop(context)) + self._status = RunnerStatus.FAILED + self._message = message + self._clear_effect_boundary() + self._next_step_at = self._clock_now() + return self._result( + timestamp=self._clock_now(), + context=context, + tick=tick, + dispatches=records, + ) + + def _result( + self, + *, + timestamp: float, + wait_duration: float = 0.0, + context: PlanningContext | None = None, + tick: ExecutionTick | None = None, + dispatches: list[CommandDispatch] | tuple[CommandDispatch, ...] = (), + ) -> RunnerStep: + """Build an immutable runner result.""" + return RunnerStep( + status=self._status, + timestamp=timestamp, + wait_duration=wait_duration, + context=context, + tick=tick, + dispatches=tuple(dispatches), + command_count=self._command_count, + message=self._message, + ) + + +__all__ = [ + "CommandAckStatus", + "CommandAcknowledgement", + "CommandDispatch", + "CommandOperation", + "CommandSink", + "EffectVerifier", + "ExecutionClock", + "ExecutionRunner", + "ExecutionRunnerCfg", + "MonotonicExecutionClock", + "ObservationProvider", + "RunnerStatus", + "RunnerStep", + "RunnerStepCallback", +] diff --git a/embodichain/lab/sim/atomic_actions/scene.py b/embodichain/lab/sim/atomic_actions/scene.py new file mode 100644 index 000000000..2ecc42170 --- /dev/null +++ b/embodichain/lab/sim/atomic_actions/scene.py @@ -0,0 +1,54 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Scene-observation boundary for dynamic atomic-action execution.""" + +from __future__ import annotations + +from typing import Protocol, runtime_checkable + +import torch + +from .state import SceneSnapshot + + +@runtime_checkable +class SceneProvider(Protocol): + """Produce scene snapshots correlated with execution environments. + + Implementations own scene-change detection and revision advancement. A + snapshot's entity rows must follow the supplied ``env_ids`` order. Scene + and collision-world revisions must never regress for a stable environment. + """ + + def snapshot( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> SceneSnapshot: + """Capture the latest versioned scene state. + + Args: + timestamp: Observation timestamp supplied by the execution backend. + env_ids: Stable ordered environment correlation IDs. + + Returns: + Scene snapshot whose batched entities follow ``env_ids`` order. + """ + + +__all__ = ["SceneProvider"] diff --git a/embodichain/lab/sim/atomic_actions/sim_adapter.py b/embodichain/lab/sim/atomic_actions/sim_adapter.py new file mode 100644 index 000000000..9e915163a --- /dev/null +++ b/embodichain/lab/sim/atomic_actions/sim_adapter.py @@ -0,0 +1,471 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Simulation ports for :class:`~.runner.ExecutionRunner`.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +import math +from typing import TYPE_CHECKING + +import torch + +from embodichain.utils import configclass + +from .execution import JointCommand +from .runner import ( + CommandAcknowledgement, + CommandAckStatus, +) +from .scene import SceneProvider +from .state import ( + EntityState, + PlanningContext, + RobotObservation, + SceneSnapshot, + TaskState, +) + +if TYPE_CHECKING: + from embodichain.lab.sim.objects import RigidObject, Robot + from embodichain.lab.sim.sim_manager import SimulationManager + + +@configclass +class RigidObjectSceneProviderCfg: + """Material-pose thresholds used to advance scene revisions.""" + + translation_threshold: float = 1.0e-4 + """Minimum translation in metres considered a scene change.""" + + rotation_threshold: float = 1.0e-3 + """Minimum rotation in radians considered a scene change.""" + + def __post_init__(self) -> None: + if self.translation_threshold < 0.0: + raise ValueError("translation_threshold must be non-negative.") + if self.rotation_threshold < 0.0: + raise ValueError("rotation_threshold must be non-negative.") + + +class RigidObjectSceneProvider: + """Observe simulation rigid objects and maintain scene revisions. + + The provider increments the general scene version when any tracked entity + moves materially. For IDs declared as collision entities it additionally + increments a per-environment collision-world revision, allowing one batch + row to invalidate its trajectory without failing unrelated rows. + + Args: + entities: Stable entity IDs mapped to live simulation rigid objects. + collision_entity_ids: Tracked IDs consumed as dynamic planner obstacles. + cfg: Optional material-change thresholds. + """ + + def __init__( + self, + entities: Mapping[str, RigidObject], + *, + collision_entity_ids: Sequence[str] = (), + cfg: RigidObjectSceneProviderCfg | None = None, + ) -> None: + normalized = dict(entities) + if not normalized: + raise ValueError("entities must contain at least one rigid object.") + if not all( + isinstance(entity_id, str) and entity_id for entity_id in normalized + ): + raise ValueError("Scene entity IDs must be non-empty strings.") + collision_ids = tuple(collision_entity_ids) + if len(set(collision_ids)) != len(collision_ids): + raise ValueError("collision_entity_ids must be unique.") + missing = set(collision_ids).difference(normalized) + if missing: + raise ValueError( + "collision_entity_ids reference untracked objects: " + f"{sorted(missing)}." + ) + self.entities = normalized + self.collision_entity_ids = collision_ids + self.cfg = cfg if cfg is not None else RigidObjectSceneProviderCfg() + self._last_timestamp: float | None = None + self._env_ids: torch.Tensor | None = None + self._last_poses: dict[str, torch.Tensor] = {} + self._scene_version = 0 + self._collision_revisions: list[int] = [] + + def snapshot( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> SceneSnapshot: + """Capture object poses and advance material-change revisions. + + Args: + timestamp: Current simulation observation time. + env_ids: Stable correlation IDs whose order matches object rows. + + Returns: + Versioned scene snapshot with per-environment collision revisions. + """ + if not math.isfinite(timestamp) or timestamp < 0.0: + raise ValueError("timestamp must be finite and non-negative.") + if self._last_timestamp is not None and timestamp < self._last_timestamp: + raise ValueError("Scene provider timestamps must be monotonic.") + if ( + not isinstance(env_ids, torch.Tensor) + or env_ids.dtype != torch.long + or env_ids.dim() != 1 + or env_ids.numel() == 0 + ): + raise ValueError("env_ids must be a non-empty 1D int64 tensor.") + stable_ids = env_ids.detach().to("cpu") + if self._env_ids is None: + self._env_ids = stable_ids.clone() + self._collision_revisions = [0] * int(env_ids.numel()) + elif not torch.equal(stable_ids, self._env_ids): + raise ValueError("Scene provider env_ids must remain stable and ordered.") + + poses = { + entity_id: self._read_pose(entity_id, entity, int(env_ids.numel())) + for entity_id, entity in self.entities.items() + } + if self._last_poses: + changed_by_entity = { + entity_id: self._pose_change_mask( + self._last_poses[entity_id], current_pose + ) + for entity_id, current_pose in poses.items() + } + if any(mask.any().item() for mask in changed_by_entity.values()): + self._scene_version += 1 + collision_changed = torch.zeros(env_ids.numel(), dtype=torch.bool) + for entity_id in self.collision_entity_ids: + collision_changed |= changed_by_entity[entity_id] + for row in collision_changed.nonzero(as_tuple=False).flatten().tolist(): + self._collision_revisions[row] += 1 + + self._last_timestamp = timestamp + self._last_poses = { + entity_id: pose.clone() for entity_id, pose in poses.items() + } + return SceneSnapshot( + timestamp=timestamp, + version=self._scene_version, + entities={ + entity_id: EntityState(pose) for entity_id, pose in poses.items() + }, + collision_world_revision=tuple(self._collision_revisions), + collision_entity_ids=self.collision_entity_ids, + ) + + @staticmethod + def _read_pose( + entity_id: str, + entity: RigidObject, + batch_size: int, + ) -> torch.Tensor: + """Read and validate one rigid-object pose batch.""" + pose = entity.get_local_pose(to_matrix=True) + if not isinstance(pose, torch.Tensor): + raise TypeError( + f"Scene entity {entity_id!r} get_local_pose() must return a tensor." + ) + if pose.shape == (4, 4): + return pose.unsqueeze(0).expand(batch_size, -1, -1).clone() + if pose.shape != (batch_size, 4, 4): + raise ValueError( + f"Scene entity {entity_id!r} pose must have shape " + f"({batch_size}, 4, 4)." + ) + return pose.clone() + + def _pose_change_mask( + self, + previous: torch.Tensor, + current: torch.Tensor, + ) -> torch.Tensor: + """Return a CPU mask of rows with material pose changes.""" + current = current.to(device=previous.device, dtype=previous.dtype) + translation = torch.linalg.vector_norm( + current[:, :3, 3] - previous[:, :3, 3], dim=1 + ) + relative_rotation = torch.bmm( + previous[:, :3, :3].transpose(1, 2), + current[:, :3, :3], + ) + cosine = ( + (relative_rotation.diagonal(dim1=1, dim2=2).sum(dim=1) - 1.0) / 2.0 + ).clamp(-1.0, 1.0) + rotation = torch.acos(cosine) + return ( + ( + (translation > self.cfg.translation_threshold) + | (rotation > self.cfg.rotation_threshold) + ) + .detach() + .to("cpu") + ) + + +class SimulationExecutionAdapter: + """Adapt a simulation robot to observation, command, and clock protocols. + + The adapter writes joint targets synchronously. Time advances only through + :meth:`sleep`, which converts the requested runner interval to an integral + number of physics updates. This makes :meth:`ExecutionRunner.run_until_blocked` + deterministic and avoids wall-clock sleeps in headless simulation. + + Args: + 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. + 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. + initial_time: Initial elapsed simulation time in seconds. + """ + + def __init__( + self, + simulation: SimulationManager, + robot: Robot, + *, + physics_dt: float | None = None, + env_ids: torch.Tensor | None = None, + scene_provider: SceneProvider | None = None, + initial_time: float = 0.0, + ) -> None: + if not math.isfinite(initial_time) or initial_time < 0.0: + raise ValueError("initial_time must be finite and non-negative.") + resolved_physics_dt = ( + float(simulation.sim_config.physics_dt) + if physics_dt is None + else float(physics_dt) + ) + if not math.isfinite(resolved_physics_dt) or resolved_physics_dt <= 0.0: + raise ValueError("physics_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).") + if env_ids is None: + env_ids = torch.arange(qpos.shape[0], dtype=torch.long, device=qpos.device) + if ( + not isinstance(env_ids, torch.Tensor) + or env_ids.dtype != torch.long + or env_ids.shape != (qpos.shape[0],) + ): + raise ValueError("env_ids must be int64 with one ID per robot row.") + if env_ids.device != qpos.device: + raise ValueError("env_ids and robot state must share a device.") + if torch.unique(env_ids).numel() != env_ids.numel(): + raise ValueError("env_ids must be unique.") + + self.simulation = simulation + self.robot = robot + self.physics_dt = resolved_physics_dt + self.env_ids = env_ids.clone() + self._robot_env_indices = list(range(qpos.shape[0])) + if scene_provider is not None and not isinstance(scene_provider, SceneProvider): + raise TypeError("scene_provider must implement SceneProvider.") + self.scene_provider = scene_provider + self._elapsed_time = float(initial_time) + + def now(self) -> float: + """Return elapsed simulation time in seconds. + + Returns: + Elapsed simulation time in seconds. + """ + return self._elapsed_time + + def sleep(self, duration: float) -> None: + """Advance physics by at least the requested duration. + + Args: + duration: Requested simulated duration in seconds. + """ + if not math.isfinite(duration) or duration < 0.0: + raise ValueError("duration must be finite and non-negative.") + if duration == 0.0: + return + step_count = max(1, math.ceil(duration / self.physics_dt)) + self.simulation.update(physics_dt=self.physics_dt, step=step_count) + self._elapsed_time += step_count * self.physics_dt + + def observe(self, task_state: TaskState) -> PlanningContext: + """Capture full-robot state and the latest supplied scene snapshot. + + Args: + task_state: Verified symbolic state owned by the execution session. + + Returns: + Planning context timestamped with elapsed simulation time. + """ + qpos = self.robot.get_qpos() + qvel = self._read_optional_tensor("get_qvel") + if qvel is None: + qvel = torch.zeros_like(qpos) + qeffort = self._read_optional_tensor("get_qf") + scene = ( + SceneSnapshot(timestamp=self._elapsed_time, version=0) + if self.scene_provider is None + else self.scene_provider.snapshot( + timestamp=self._elapsed_time, + env_ids=self.env_ids, + ) + ) + if not isinstance(scene, SceneSnapshot): + raise TypeError("scene_provider must return a SceneSnapshot.") + return PlanningContext( + robot=RobotObservation( + timestamp=self._elapsed_time, + qpos=qpos, + qvel=qvel, + qeffort=qeffort, + ), + task=task_state, + scene=scene, + env_ids=self.env_ids, + ) + + def send( + self, + command: JointCommand, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Write active targets and observed-position holds as one batch. + + 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. + timeout: Positive acknowledgement deadline. Simulation writes are + synchronous, so this is validated but otherwise unused. + + Returns: + Accepted acknowledgement or a rejected diagnostic. + """ + self._validate_timeout(timeout) + try: + self._validate_command(command) + if not command.active_mask.any(): + return CommandAcknowledgement.accepted_ack("No active rows.") + self.robot.set_qpos( + command.positions, + env_ids=self._robot_env_indices, + ) + if command.velocities is not None: + self.robot.set_qvel( + command.velocities, + 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, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Set every represented environment to an observed-position hold. + + Args: + command: Full-robot hold positions. ``active_mask`` is intentionally + ignored because safety hold applies to every environment row. + timeout: Positive acknowledgement deadline. + + Returns: + Accepted acknowledgement or a rejected diagnostic. + """ + self._validate_timeout(timeout) + try: + self._validate_command(command) + self.robot.set_qpos( + command.positions, + env_ids=self._robot_env_indices, + ) + if command.velocities is not None: + self.robot.set_qvel( + command.velocities, + env_ids=self._robot_env_indices, + ) + return CommandAcknowledgement.accepted_ack() + except Exception as exc: + return CommandAcknowledgement( + CommandAckStatus.REJECTED, + f"{type(exc).__name__}: {exc}", + ) + + def cancel(self, *, timeout: float) -> CommandAcknowledgement: + """Acknowledge cancellation of synchronous simulation target writes. + + Args: + timeout: Positive acknowledgement deadline. + + Returns: + Accepted acknowledgement. The following ``hold`` call installs the + actual safe target. + """ + self._validate_timeout(timeout) + return CommandAcknowledgement.accepted_ack( + "Simulation commands are synchronous; no queued command remained." + ) + + def _read_optional_tensor(self, method_name: str) -> torch.Tensor | None: + """Read an optional full-robot tensor from the robot API.""" + method = getattr(self.robot, method_name, None) + if not callable(method): + return None + try: + value = method() + except (AttributeError, NotImplementedError): + 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)}." + ) + if not torch.equal(command.env_ids, self.env_ids): + raise ValueError("Command env_ids must match the simulation adapter.") + + @staticmethod + def _validate_timeout(timeout: float) -> None: + """Validate an acknowledgement timeout.""" + if not math.isfinite(timeout) or timeout <= 0.0: + raise ValueError("timeout must be finite and greater than zero.") + + +__all__ = [ + "RigidObjectSceneProvider", + "RigidObjectSceneProviderCfg", + "SimulationExecutionAdapter", +] diff --git a/embodichain/lab/sim/atomic_actions/state.py b/embodichain/lab/sim/atomic_actions/state.py index 8b2044ab8..985599ac3 100644 --- a/embodichain/lab/sim/atomic_actions/state.py +++ b/embodichain/lab/sim/atomic_actions/state.py @@ -430,12 +430,40 @@ class SceneSnapshot: timestamp: float version: int entities: Mapping[str, EntityState] = field(default_factory=dict) + collision_world_revision: int | tuple[int, ...] = 0 + """Global or per-environment collision-world revision.""" + + collision_entity_ids: tuple[str, ...] = () + """Entity IDs whose poses update a planner's dynamic collision world.""" def __post_init__(self) -> None: if self.timestamp < 0.0: raise ValueError("SceneSnapshot.timestamp must be non-negative.") if self.version < 0: raise ValueError("SceneSnapshot.version must be non-negative.") + revision = self.collision_world_revision + if isinstance(revision, bool): + raise TypeError("collision_world_revision must contain integers.") + if isinstance(revision, int): + if revision < 0: + raise ValueError( + "collision_world_revision must contain non-negative values." + ) + else: + if not isinstance(revision, tuple) or not revision: + raise TypeError( + "collision_world_revision must be an integer or a non-empty " + "tuple of integers." + ) + if any( + isinstance(value, bool) or not isinstance(value, int) + for value in revision + ): + raise TypeError("collision_world_revision must contain integers.") + if any(value < 0 for value in revision): + raise ValueError( + "collision_world_revision must contain non-negative values." + ) normalized: dict[str, EntityState] = {} for entity_id, state in self.entities.items(): if not isinstance(entity_id, str) or not entity_id: @@ -445,7 +473,78 @@ def __post_init__(self) -> None: "SceneSnapshot entities must contain EntityState values." ) normalized[entity_id] = state + collision_entity_ids = tuple(self.collision_entity_ids) + if len(set(collision_entity_ids)) != len(collision_entity_ids) or not all( + isinstance(entity_id, str) and entity_id + for entity_id in collision_entity_ids + ): + raise ValueError( + "collision_entity_ids must contain unique non-empty entity IDs." + ) + missing = set(collision_entity_ids).difference(normalized) + if missing: + raise ValueError( + "collision_entity_ids reference missing scene entities: " + f"{sorted(missing)}." + ) object.__setattr__(self, "entities", MappingProxyType(normalized)) + object.__setattr__(self, "collision_entity_ids", collision_entity_ids) + + def collision_world_revisions(self, batch_size: int) -> tuple[int, ...]: + """Expand the collision revision to one value per environment. + + Args: + batch_size: Number of environments represented by the planning context. + + Returns: + Per-environment monotonic revision tuple. + + Raises: + ValueError: If an explicit revision tuple does not match the batch. + """ + if batch_size <= 0: + raise ValueError("batch_size must be positive.") + revision = self.collision_world_revision + if isinstance(revision, int): + return (revision,) * batch_size + if len(revision) == 1: + return revision * batch_size + if len(revision) != batch_size: + raise ValueError( + "collision_world_revision must be global or have one value per " + f"environment; got {len(revision)} values for batch {batch_size}." + ) + return revision + + def collision_obstacle_poses( + self, + *, + batch_size: int, + device: torch.device, + dtype: torch.dtype, + ) -> Mapping[str, torch.Tensor]: + """Return collision obstacle poses in planning batch order. + + Args: + batch_size: Number of planning environments. + device: Planner tensor device. + dtype: Planner tensor dtype. + + Returns: + Mapping from configured collision entity ID to ``(B, 4, 4)`` pose. + """ + poses: dict[str, torch.Tensor] = {} + for entity_id in self.collision_entity_ids: + pose = self.entities[entity_id].pose.to(device=device, dtype=dtype) + if pose.shape == (4, 4): + pose = pose.unsqueeze(0).expand(batch_size, -1, -1) + elif pose.shape != (batch_size, 4, 4): + raise ValueError( + f"Collision entity {entity_id!r} pose must match planning " + f"batch size {batch_size}." + ) + poses[entity_id] = pose.clone() + return MappingProxyType(poses) @classmethod def empty(cls) -> SceneSnapshot: @@ -473,6 +572,13 @@ def __post_init__(self) -> None: raise ValueError("TaskState and RobotObservation batch sizes must match.") if self.task.device != self.robot.qpos.device: raise ValueError("TaskState and RobotObservation must share a device.") + self.scene.collision_world_revisions(self.robot.batch_size) + for entity_id, state in self.scene.entities.items(): + if state.pose.dim() == 3 and state.pose.shape[0] != self.robot.batch_size: + raise ValueError( + f"Scene entity {entity_id!r} pose batch must match the " + "planning context." + ) if not isinstance(self.env_ids, torch.Tensor): raise TypeError("env_ids must be a torch.Tensor.") if self.env_ids.dtype != torch.long: diff --git a/embodichain/lab/sim/planners/base_planner.py b/embodichain/lab/sim/planners/base_planner.py index ab66cbae9..8d001d2bb 100644 --- a/embodichain/lab/sim/planners/base_planner.py +++ b/embodichain/lab/sim/planners/base_planner.py @@ -20,6 +20,7 @@ import functools from abc import ABC, abstractmethod +from collections.abc import Mapping from dataclasses import MISSING from embodichain.utils import logger @@ -174,6 +175,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.""" + def supports_move_type(self, move_type: MoveType) -> bool: """Return whether the planner accepts a movement target type directly. @@ -213,6 +217,26 @@ def with_motion_context( """ return options + def with_collision_world( + self, + options: PlanOptions, + *, + obstacle_poses: Mapping[str, torch.Tensor], + ) -> 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. + + Args: + options: Backend-specific options to enrich. + obstacle_poses: Batched world poses keyed by stable obstacle ID. + + Returns: + Planning options unchanged for a backend without world updates. + """ + return options + @validate_plan_options @abstractmethod def plan( diff --git a/embodichain/lab/sim/planners/curobo/curobo_planner.py b/embodichain/lab/sim/planners/curobo/curobo_planner.py index 47b9f5f7b..8a8ea8a16 100644 --- a/embodichain/lab/sim/planners/curobo/curobo_planner.py +++ b/embodichain/lab/sim/planners/curobo/curobo_planner.py @@ -33,7 +33,7 @@ import os import threading import time -from collections.abc import Iterator +from collections.abc import Iterator, Mapping from contextlib import contextmanager, nullcontext from copy import deepcopy from dataclasses import dataclass @@ -708,6 +708,7 @@ 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: @@ -799,6 +800,32 @@ def with_motion_context( options.control_part = control_part return options + def with_collision_world( + self, + options: PlanOptions, + *, + obstacle_poses: Mapping[str, torch.Tensor], + ) -> CuroboPlanOptions: + """Bind snapshot obstacle poses to one cuRobo planning attempt. + + Args: + options: Reusable caller options copied by the atomic-action layer. + obstacle_poses: Batched simulator-world poses keyed by configured + dynamic obstacle name. + + Returns: + cuRobo options containing an owned obstacle-pose mapping. + """ + if not isinstance(options, CuroboPlanOptions): + logger.log_error("CuroboPlanner requires CuroboPlanOptions", TypeError) + merged = { + name: pose.clone() + for name, pose in (options.dynamic_obstacle_poses or {}).items() + } + merged.update({name: pose.clone() for name, pose in obstacle_poses.items()}) + options.dynamic_obstacle_poses = merged or None + return options + @validate_plan_options(options_cls=CuroboPlanOptions) def plan( self, diff --git a/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py b/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py new file mode 100644 index 000000000..074c6154d --- /dev/null +++ b/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py @@ -0,0 +1,237 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Demonstrate collision-world invalidation and dynamic obstacle replanning.""" + +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.gym.utils.gym_utils import add_env_launcher_args_to_parser +from embodichain.lab.sim.atomic_actions import ( + ActionBinding, + ActionInvocation, + AtomicActionEngine, + EndEffectorPoseGoal, + ExecutionEventKind, + ExecutionRunner, + ExecutionRunnerCfg, + MotionPolicy, + MoveEndEffector, + RecoveryPolicy, + RigidObjectSceneProvider, + RunnerStatus, + RunnerStep, + SimulationExecutionAdapter, + TaskState, +) +from embodichain.lab.sim.cfg import RigidBodyAttributesCfg +from embodichain.lab.sim.objects import RigidObjectCfg +from embodichain.lab.sim.planners import MotionGenCfg, MotionGenerator +from embodichain.lab.sim.planners.curobo.curobo_planner import ( + CuroboPlannerCfg, + CuroboWorldCfg, +) +from embodichain.lab.sim.robots import FrankaPandaCfg +from embodichain.lab.sim.shapes import CubeCfg +from embodichain.utils import logger +from scripts.tutorials.atomic_action.tutorial_utils import ( + create_tutorial_simulation, + prepare_tutorial_scene, + run_tutorial, + serve_tutorial_scene, + start_auto_play_recording, + stop_auto_play_recording, +) + +ROBOT_UID = "dynamic_scene_franka" +OBSTACLE_UID = "dynamic_obstacle" +CONTROL_PART = "arm" +SAMPLE_COUNT = 80 +MOVE_AFTER_COMMAND = 3 +OBSTACLE_Y_OFFSET = 0.18 +POST_EXECUTION_UPDATES = 80 + + +def parse_arguments() -> argparse.Namespace: + """Parse command-line arguments for the dynamic-obstacle tutorial.""" + parser = argparse.ArgumentParser( + description="Demonstrate collision-world revision recovery with cuRobo." + ) + add_env_launcher_args_to_parser(parser) + parser.add_argument("--auto_play", action="store_true") + parser.add_argument( + "--no_obstacle_motion", + action="store_true", + help="Execute without moving the obstacle after planning.", + ) + return parser.parse_args() + + +def main() -> None: + """Move an obstacle during execution and replan from the latest snapshot.""" + args = parse_arguments() + sim = create_tutorial_simulation(args) + robot = sim.add_robot( + cfg=FrankaPandaCfg.from_dict({"uid": ROBOT_UID, "robot_type": "panda"}) + ) + obstacle = sim.add_rigid_object( + cfg=RigidObjectCfg( + uid=OBSTACLE_UID, + shape=CubeCfg(size=[0.16, 0.18, 0.30]), + attrs=RigidBodyAttributesCfg(), + body_type="kinematic", + init_pos=[0.45, -0.20, 0.20], + init_rot=[0.0, 0.0, 0.0], + ) + ) + motion_gen = MotionGenerator( + MotionGenCfg( + planner_cfg=CuroboPlannerCfg( + robot_uid=ROBOT_UID, + world=CuroboWorldCfg( + rigid_objects=[obstacle], + obstacle_representation="cuboid", + dynamic_obstacle_names=[OBSTACLE_UID], + multi_env=args.num_envs > 1, + ), + ) + ) + ) + scene_provider = RigidObjectSceneProvider( + {OBSTACLE_UID: obstacle}, + collision_entity_ids=(OBSTACLE_UID,), + ) + adapter = SimulationExecutionAdapter( + sim, + robot, + scene_provider=scene_provider, + ) + + current_pose = robot.compute_fk( + qpos=robot.get_qpos(name=CONTROL_PART), + name=CONTROL_PART, + to_matrix=True, + ) + target_pose = current_pose.clone() + target_pose[:, :3, 3] += torch.tensor( + [0.22, 0.24, 0.12], + dtype=target_pose.dtype, + device=target_pose.device, + ) + engine = AtomicActionEngine(motion_generator=motion_gen) + engine.register(MoveEndEffector(motion_gen)) + invocation = ActionInvocation( + skill_id="move_end_effector", + goal=EndEffectorPoseGoal(target_pose), + binding=ActionBinding(manipulators={"primary": CONTROL_PART}), + motion_policy=MotionPolicy( + motion_source="motion_gen", + sample_count=SAMPLE_COUNT, + ), + recovery_policy=RecoveryPolicy( + max_replans=2, + tracking_error_threshold=0.20, + phase_timeout=30.0, + ), + invocation_id="dynamic-obstacle-demo", + ) + task_state = TaskState.empty(robot.get_qpos().shape[0], robot.device) + session = engine.start((invocation,), adapter.observe(task_state)) + runner = ExecutionRunner( + session, + adapter, + adapter, + clock=adapter, + cfg=ExecutionRunnerCfg(minimum_cycle_time=adapter.physics_dt), + ) + + wait_for_user = prepare_tutorial_scene( + sim, + args, + "Inspect the scene, then press Enter to start dynamic replanning...", + ) + obstacle_moved = False + recovery_observed = False + + def on_step(step: RunnerStep) -> None: + nonlocal obstacle_moved, recovery_observed + if ( + not args.no_obstacle_motion + and not obstacle_moved + and step.command_count >= MOVE_AFTER_COMMAND + ): + pose = obstacle.get_local_pose(to_matrix=True).clone() + pose[:, 1, 3] += OBSTACLE_Y_OFFSET + obstacle.set_local_pose(pose) + obstacle_moved = True + logger.log_warning( + "Moved the collision obstacle; the next scene snapshot should " + "invalidate the active trajectory." + ) + if step.tick is None: + return + for event in step.tick.events: + if event.kind in { + ExecutionEventKind.COLLISION_WORLD_CHANGED, + ExecutionEventKind.REPLANNED, + ExecutionEventKind.RECOVERY_EXHAUSTED, + }: + rows = event.env_mask.nonzero(as_tuple=False).flatten().tolist() + logger.log_info( + f"Execution event {event.kind.value}: env rows={rows}; " + f"{event.message}" + ) + recovery_observed |= ( + event.kind is ExecutionEventKind.COLLISION_WORLD_CHANGED + ) + + recording_started = start_auto_play_recording( + sim, + args, + video_prefix="dynamic_obstacle_recovery_auto_play", + ) + try: + result = runner.run_until_blocked(on_step=on_step) + for _ in range(POST_EXECUTION_UPDATES): + adapter.sleep(adapter.physics_dt) + finally: + stop_auto_play_recording(sim, recording_started) + + if result.status is not RunnerStatus.COMPLETED: + raise RuntimeError(f"Closed-loop execution failed: {result.message}") + if not args.no_obstacle_motion and not recovery_observed: + raise RuntimeError("Obstacle motion did not invalidate the trajectory.") + logger.log_info( + f"Execution completed after {result.command_count} accepted commands.", + color="green", + ) + + serve_tutorial_scene(sim, args) + if wait_for_user: + input("Press Enter to exit the simulation...") + + +if __name__ == "__main__": + run_tutorial(main) diff --git a/scripts/tutorials/atomic_action/moving_target_recovery.py b/scripts/tutorials/atomic_action/moving_target_recovery.py new file mode 100644 index 000000000..33d8177ad --- /dev/null +++ b/scripts/tutorials/atomic_action/moving_target_recovery.py @@ -0,0 +1,219 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Demonstrate late-bound goal recovery when a target entity moves.""" + +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.gym.utils.gym_utils import add_env_launcher_args_to_parser +from embodichain.lab.sim.atomic_actions import ( + ActionBinding, + ActionInvocation, + AtomicActionEngine, + EndEffectorPoseGoal, + ExecutionEventKind, + ExecutionRunner, + ExecutionRunnerCfg, + MotionPolicy, + MoveEndEffector, + RecoveryPolicy, + RigidObjectSceneProvider, + RunnerStatus, + RunnerStep, + SceneEntityPose, + SimulationExecutionAdapter, + TaskState, +) +from embodichain.lab.sim.cfg import RigidBodyAttributesCfg +from embodichain.lab.sim.objects import RigidObjectCfg +from embodichain.lab.sim.shapes import CubeCfg +from embodichain.utils import logger +from scripts.tutorials.atomic_action.tutorial_utils import ( + add_ur5_gripper_robot, + create_toppra_motion_generator, + create_tutorial_simulation, + prepare_tutorial_scene, + run_tutorial, + serve_tutorial_scene, + start_auto_play_recording, + stop_auto_play_recording, +) + +TARGET_UID = "moving_target" +CONTROL_PART = "arm" +SAMPLE_COUNT = 80 +MOVE_AFTER_COMMAND = 3 +TARGET_Y_OFFSET = 0.14 +TOOL_HEIGHT = 0.30 +POST_EXECUTION_UPDATES = 80 + + +def parse_arguments() -> argparse.Namespace: + """Parse command-line arguments for the moving-target tutorial.""" + parser = argparse.ArgumentParser( + description="Demonstrate SceneEntityPose late-binding and replanning." + ) + add_env_launcher_args_to_parser(parser) + parser.add_argument("--auto_play", action="store_true") + parser.add_argument( + "--no_target_motion", + action="store_true", + help="Execute without moving the target after planning.", + ) + return parser.parse_args() + + +def main() -> None: + """Move a referenced entity during execution and follow its new pose.""" + args = parse_arguments() + sim = create_tutorial_simulation(args) + robot = add_ur5_gripper_robot(sim) + target = sim.add_rigid_object( + cfg=RigidObjectCfg( + uid=TARGET_UID, + shape=CubeCfg(size=[0.06, 0.06, 0.06]), + attrs=RigidBodyAttributesCfg(enable_collision=False), + body_type="kinematic", + init_pos=[0.40, -0.14, 0.03], + init_rot=[0.0, 0.0, 0.0], + ) + ) + motion_gen = create_toppra_motion_generator(robot) + scene_provider = RigidObjectSceneProvider({TARGET_UID: target}) + adapter = SimulationExecutionAdapter( + sim, + robot, + scene_provider=scene_provider, + ) + + current_eef = robot.compute_fk( + qpos=robot.get_qpos(name=CONTROL_PART), + name=CONTROL_PART, + to_matrix=True, + ) + target_to_tool = torch.eye( + 4, + dtype=current_eef.dtype, + device=current_eef.device, + ) + target_to_tool[:3, :3] = current_eef[0, :3, :3] + target_to_tool[2, 3] = TOOL_HEIGHT + engine = AtomicActionEngine(motion_generator=motion_gen) + engine.register(MoveEndEffector(motion_gen)) + invocation = ActionInvocation( + skill_id="move_end_effector", + goal=EndEffectorPoseGoal( + SceneEntityPose(TARGET_UID, relative_pose=target_to_tool) + ), + binding=ActionBinding(manipulators={"primary": CONTROL_PART}), + motion_policy=MotionPolicy( + sample_count=SAMPLE_COUNT, + control_dt=2.0 * adapter.physics_dt, + ), + recovery_policy=RecoveryPolicy( + max_replans=2, + tracking_error_threshold=0.15, + goal_translation_threshold=0.02, + phase_timeout=20.0, + ), + invocation_id="moving-target-demo", + ) + task_state = TaskState.empty(robot.get_qpos().shape[0], robot.device) + session = engine.start((invocation,), adapter.observe(task_state)) + runner = ExecutionRunner( + session, + adapter, + adapter, + clock=adapter, + cfg=ExecutionRunnerCfg(minimum_cycle_time=adapter.physics_dt), + ) + + wait_for_user = prepare_tutorial_scene( + sim, + args, + "Inspect the target, then press Enter to start moving-goal recovery...", + ) + target_moved = False + recovery_observed = False + + def on_step(step: RunnerStep) -> None: + nonlocal target_moved, recovery_observed + if ( + not args.no_target_motion + and not target_moved + and step.command_count >= MOVE_AFTER_COMMAND + ): + pose = target.get_local_pose(to_matrix=True).clone() + pose[:, 1, 3] += TARGET_Y_OFFSET + target.set_local_pose(pose) + target_moved = True + logger.log_warning( + "Moved the referenced target; the next snapshot should rebind " + "the goal and invalidate the active trajectory." + ) + if step.tick is None: + return + for event in step.tick.events: + if event.kind in { + ExecutionEventKind.DYNAMIC_GOAL_CHANGED, + ExecutionEventKind.REPLANNED, + ExecutionEventKind.RECOVERY_EXHAUSTED, + }: + rows = event.env_mask.nonzero(as_tuple=False).flatten().tolist() + logger.log_info( + f"Execution event {event.kind.value}: env rows={rows}; " + f"{event.message}" + ) + recovery_observed |= event.kind is ExecutionEventKind.DYNAMIC_GOAL_CHANGED + + recording_started = start_auto_play_recording( + sim, + args, + video_prefix="moving_target_recovery_auto_play", + ) + try: + result = runner.run_until_blocked(on_step=on_step) + for _ in range(POST_EXECUTION_UPDATES): + adapter.sleep(adapter.physics_dt) + finally: + stop_auto_play_recording(sim, recording_started) + + if result.status is not RunnerStatus.COMPLETED: + raise RuntimeError(f"Closed-loop execution failed: {result.message}") + if not args.no_target_motion and not recovery_observed: + raise RuntimeError("Target motion did not invalidate the trajectory.") + logger.log_info( + f"Execution completed after {result.command_count} accepted commands.", + color="green", + ) + + serve_tutorial_scene(sim, args) + if wait_for_user: + input("Press Enter to exit the simulation...") + + +if __name__ == "__main__": + run_tutorial(main) diff --git a/scripts/tutorials/atomic_action/tracking_error_recovery.py b/scripts/tutorials/atomic_action/tracking_error_recovery.py new file mode 100644 index 000000000..9f1b6bff3 --- /dev/null +++ b/scripts/tutorials/atomic_action/tracking_error_recovery.py @@ -0,0 +1,235 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Demonstrate closed-loop recovery from an injected joint tracking error.""" + +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.gym.utils.gym_utils import add_env_launcher_args_to_parser +from embodichain.lab.sim.atomic_actions import ( + ActionBinding, + ActionInvocation, + AtomicActionEngine, + ExecutionEventKind, + ExecutionRunner, + ExecutionRunnerCfg, + JointPositionGoal, + MotionPolicy, + MoveJoints, + PlanningContext, + RecoveryPolicy, + RunnerStatus, + RunnerStep, + SimulationExecutionAdapter, + TaskState, +) +from embodichain.lab.sim.objects import Robot +from embodichain.utils import logger +from scripts.tutorials.atomic_action.tutorial_utils import ( + add_ur5_gripper_robot, + create_toppra_motion_generator, + create_tutorial_simulation, + prepare_tutorial_scene, + run_tutorial, + serve_tutorial_scene, + start_auto_play_recording, + stop_auto_play_recording, +) + +SAMPLE_COUNT = 80 +INJECTION_AFTER_COMMAND = 3 +TRACKING_ERROR_OFFSET = 0.35 +TRACKING_ERROR_THRESHOLD = 0.08 +POST_EXECUTION_UPDATES = 80 + + +class _OneShotTrackingErrorInjector: + """Decorate simulation observations with one deterministic disturbance.""" + + def __init__( + self, + adapter: SimulationExecutionAdapter, + robot: Robot, + *, + joint_id: int, + offset: float, + ) -> None: + self._adapter = adapter + self._robot = robot + self._joint_id = joint_id + self._offset = offset + self._pending = False + self.injected = False + + def arm(self) -> None: + """Request a disturbance immediately before the next observation.""" + if not self.injected: + self._pending = True + + def observe(self, task_state: TaskState) -> PlanningContext: + """Inject one physical-state offset, then capture the observation. + + Args: + task_state: Session-owned verified task state. + + Returns: + Latest simulation planning context. + """ + if self._pending: + qpos = self._robot.get_qpos().clone() + qpos[:, self._joint_id] += self._offset + self._robot.set_qpos(qpos, target=False) + self._robot.set_qvel(torch.zeros_like(qpos), target=False) + self._pending = False + self.injected = True + logger.log_warning( + "Injected a joint-position disturbance before observation; " + "the session should detect tracking error and replan." + ) + return self._adapter.observe(task_state) + + +def parse_arguments() -> argparse.Namespace: + """Parse command-line arguments for the recovery tutorial.""" + parser = argparse.ArgumentParser( + description="Demonstrate ExecutionRunner tracking-error recovery." + ) + add_env_launcher_args_to_parser(parser) + parser.add_argument("--auto_play", action="store_true") + parser.add_argument( + "--no_error_injection", + action="store_true", + help="Run the closed-loop trajectory without the demonstration disturbance.", + ) + return parser.parse_args() + + +def main() -> None: + """Execute MoveJoints and recover after a one-shot state disturbance.""" + args = parse_arguments() + sim = create_tutorial_simulation(args) + robot = add_ur5_gripper_robot(sim) + motion_gen = create_toppra_motion_generator(robot) + adapter = SimulationExecutionAdapter(sim, robot) + + target = torch.tensor( + [0.35, -1.20, 1.30, -1.65, -1.57, 0.20], + dtype=torch.float32, + device=sim.device, + ) + engine = AtomicActionEngine(motion_generator=motion_gen) + engine.register(MoveJoints(motion_gen)) + invocation = ActionInvocation( + skill_id="move_joints", + goal=JointPositionGoal(target), + binding=ActionBinding(manipulators={"primary": "arm"}), + motion_policy=MotionPolicy( + sample_count=SAMPLE_COUNT, + control_dt=2.0 * adapter.physics_dt, + ), + recovery_policy=RecoveryPolicy( + max_replans=2, + tracking_error_threshold=TRACKING_ERROR_THRESHOLD, + phase_timeout=20.0, + ), + ) + task_state = TaskState.empty(robot.get_qpos().shape[0], robot.device) + initial_context = adapter.observe(task_state) + session = engine.start((invocation,), initial_context) + + arm_joint_id = robot.get_joint_ids(name="arm")[0] + observation_provider = _OneShotTrackingErrorInjector( + adapter, + robot, + joint_id=arm_joint_id, + offset=TRACKING_ERROR_OFFSET, + ) + runner = ExecutionRunner( + session, + observation_provider, + adapter, + clock=adapter, + cfg=ExecutionRunnerCfg(minimum_cycle_time=adapter.physics_dt), + ) + + wait_for_user = prepare_tutorial_scene( + sim, + args, + "Inspect the robot, then press Enter to start closed-loop execution...", + ) + recovery_observed = False + + def on_step(step: RunnerStep) -> None: + nonlocal recovery_observed + if ( + not args.no_error_injection + and not observation_provider.injected + and step.command_count >= INJECTION_AFTER_COMMAND + ): + observation_provider.arm() + if step.tick is None: + return + for event in step.tick.events: + if event.kind in { + ExecutionEventKind.TRACKING_ERROR, + ExecutionEventKind.REPLANNED, + ExecutionEventKind.RECOVERY_EXHAUSTED, + }: + env_ids = event.env_mask.nonzero(as_tuple=False).flatten().tolist() + logger.log_info( + f"Execution event {event.kind.value}: env rows={env_ids}; " + f"{event.message}" + ) + recovery_observed |= event.kind is ExecutionEventKind.REPLANNED + + recording_started = start_auto_play_recording( + sim, + args, + video_prefix="tracking_error_recovery_auto_play", + ) + try: + result = runner.run_until_blocked(on_step=on_step) + for _ in range(POST_EXECUTION_UPDATES): + adapter.sleep(adapter.physics_dt) + finally: + stop_auto_play_recording(sim, recording_started) + + if result.status is not RunnerStatus.COMPLETED: + raise RuntimeError(f"Closed-loop execution failed: {result.message}") + if not args.no_error_injection and not recovery_observed: + raise RuntimeError("The injected tracking error did not trigger replanning.") + logger.log_info( + f"Execution completed after {result.command_count} accepted commands.", + color="green", + ) + + serve_tutorial_scene(sim, args) + if wait_for_user: + input("Press Enter to exit the simulation...") + + +if __name__ == "__main__": + run_tutorial(main) diff --git a/tests/sim/atomic_actions/test_core.py b/tests/sim/atomic_actions/test_core.py index 574b49b8e..c012e13d2 100644 --- a/tests/sim/atomic_actions/test_core.py +++ b/tests/sim/atomic_actions/test_core.py @@ -175,6 +175,34 @@ def test_scene_entity_pose_enforces_confidence() -> None: ) +def test_scene_snapshot_expands_global_collision_world_revision() -> None: + pose = torch.eye(4).repeat(2, 1, 1) + snapshot = SceneSnapshot( + timestamp=0.0, + version=0, + entities={"obstacle": EntityState(pose)}, + collision_world_revision=3, + collision_entity_ids=("obstacle",), + ) + + assert snapshot.collision_world_revisions(2) == (3, 3) + obstacle_poses = snapshot.collision_obstacle_poses( + batch_size=2, + device=torch.device("cpu"), + dtype=torch.float32, + ) + assert torch.equal(obstacle_poses["obstacle"], pose) + + +def test_scene_snapshot_rejects_unknown_collision_entity() -> None: + with pytest.raises(ValueError, match="missing scene entities"): + SceneSnapshot( + timestamp=0.0, + version=0, + collision_entity_ids=("missing",), + ) + + def test_timed_trajectory_synthesizes_timing_and_holds_selected_rows() -> None: positions = torch.arange(24, dtype=torch.float32).reshape(2, 3, 4) trajectory = TimedTrajectory.from_positions( diff --git a/tests/sim/atomic_actions/test_engine.py b/tests/sim/atomic_actions/test_engine.py index 9ae563a20..986a87aba 100644 --- a/tests/sim/atomic_actions/test_engine.py +++ b/tests/sim/atomic_actions/test_engine.py @@ -48,7 +48,7 @@ class StubAction(AtomicAction[JointPositionGoal]): GoalType: ClassVar[type] = JointPositionGoal manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) - def plan( + def _plan( self, invocation: ActionInvocation[JointPositionGoal], context: PlanningContext, @@ -104,6 +104,28 @@ def test_global_registry_uses_stable_skill_id() -> None: unregister_action("stub") +def test_action_subclass_cannot_override_framework_plan() -> None: + with pytest.raises(TypeError, match="must implement _plan"): + + class InvalidAction(AtomicAction[JointPositionGoal]): + skill_id: ClassVar[str] = "invalid" + GoalType: ClassVar[type] = JointPositionGoal + + def plan( + self, + invocation: ActionInvocation[JointPositionGoal], + context: PlanningContext, + ) -> ActionPlan: + raise NotImplementedError + + def _plan( + self, + invocation: ActionInvocation[JointPositionGoal], + context: PlanningContext, + ) -> ActionPlan: + raise NotImplementedError + + def test_engine_compile_projects_terminal_state_between_actions() -> None: engine = _engine() engine.register(StubAction(engine.motion_generator, ActionCfg(name="stub"))) diff --git a/tests/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index 12318ce36..b5bf562aa 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -48,6 +48,7 @@ TaskState, ) from embodichain.lab.sim.atomic_actions.goals import resolve_pose_goal +from embodichain.lab.sim.planners import PlanOptions class DynamicAction(AtomicAction[EndEffectorPoseGoal]): @@ -61,7 +62,7 @@ def __init__(self, motion_generator) -> None: super().__init__(motion_generator, ActionCfg(name="dynamic")) self.plan_count = 0 - def plan( + def _plan( self, invocation: ActionInvocation[EndEffectorPoseGoal], context: PlanningContext, @@ -83,7 +84,7 @@ class EffectAction(DynamicAction): skill_id: ClassVar[str] = "effect" - def plan( + def _plan( self, invocation: ActionInvocation[EndEffectorPoseGoal], context: PlanningContext, @@ -108,12 +109,12 @@ def plan( ) -def _engine() -> tuple[AtomicActionEngine, DynamicAction]: +def _engine(batch_size: int = 1) -> tuple[AtomicActionEngine, DynamicAction]: robot = Mock() robot.device = torch.device("cpu") robot.dof = 2 - robot.get_qpos.return_value = torch.zeros(1, 2) - robot.get_qvel.return_value = torch.zeros(1, 2) + robot.get_qpos.return_value = torch.zeros(batch_size, 2) + robot.get_qvel.return_value = torch.zeros(batch_size, 2) generator = Mock() generator.robot = robot generator.device = torch.device("cpu") @@ -146,17 +147,51 @@ def _context( ) +def _collision_context( + timestamp: float, + qpos: torch.Tensor, + obstacle_x: torch.Tensor, + collision_revision: int | tuple[int, ...], +) -> PlanningContext: + """Build a scene whose obstacle is independent from the action goal.""" + batch_size = int(qpos.shape[0]) + target_pose = torch.eye(4).repeat(batch_size, 1, 1) + target_pose[:, 0, 3] = 0.2 + obstacle_pose = torch.eye(4).repeat(batch_size, 1, 1) + obstacle_pose[:, 0, 3] = obstacle_x + return PlanningContext( + robot=RobotObservation( + timestamp=timestamp, + qpos=qpos, + qvel=torch.zeros_like(qpos), + ), + task=TaskState.empty(batch_size=batch_size, device="cpu"), + scene=SceneSnapshot( + timestamp=timestamp, + version=int(timestamp > 0.0), + entities={ + "target": EntityState(target_pose), + "obstacle": EntityState(obstacle_pose), + }, + collision_world_revision=collision_revision, + collision_entity_ids=("obstacle",), + ), + env_ids=torch.arange(batch_size, dtype=torch.long), + ) + + def _invocation( *, max_replans: int = 2, max_phase_retries: int = 2, phase_timeout: float = 30.0, + motion_source: str = "ik_interp", ) -> ActionInvocation[EndEffectorPoseGoal]: return ActionInvocation( skill_id="dynamic", goal=EndEffectorPoseGoal(SceneEntityPose("target")), binding=ActionBinding(manipulators={"primary": "arm"}), - motion_policy=MotionPolicy(sample_count=2), + motion_policy=MotionPolicy(sample_count=2, motion_source=motion_source), recovery_policy=RecoveryPolicy( max_replans=max_replans, max_phase_retries=max_phase_retries, @@ -197,6 +232,96 @@ def test_scene_motion_replans_late_bound_goal() -> None: assert tick.command is not None +def test_collision_world_change_replans_with_latest_obstacle_pose() -> None: + engine, action = _engine() + planner = engine.motion_generator.planner + planner.supports_collision_world_updates = True + planner.default_plan_options.return_value = PlanOptions() + planner.with_collision_world.side_effect = ( + lambda options, *, obstacle_poses: options + ) + initial_qpos = torch.zeros(1, 2) + initial = _collision_context( + 0.0, + initial_qpos, + torch.tensor([0.4]), + (0,), + ) + session = engine.start( + (_invocation(motion_source="motion_gen"),), + initial, + ) + session.tick(initial) + + changed = _collision_context( + 0.1, + initial_qpos, + torch.tensor([0.6]), + (1,), + ) + tick = session.tick(changed) + + kinds = {event.kind for event in tick.events} + assert ExecutionEventKind.COLLISION_WORLD_CHANGED in kinds + assert ExecutionEventKind.REPLANNED in kinds + assert action.plan_count == 2 + latest_obstacles = planner.with_collision_world.call_args.kwargs["obstacle_poses"] + assert latest_obstacles["obstacle"][0, 0, 3] == pytest.approx(0.6) + assert tick.command is not None + + +def test_collision_world_exhaustion_only_disables_changed_environment() -> None: + engine, _ = _engine(batch_size=2) + planner = engine.motion_generator.planner + planner.supports_collision_world_updates = True + planner.default_plan_options.return_value = PlanOptions() + planner.with_collision_world.side_effect = ( + lambda options, *, obstacle_poses: options + ) + qpos = torch.zeros(2, 2) + initial = _collision_context( + 0.0, + qpos, + torch.tensor([0.4, 0.4]), + (0, 0), + ) + session = engine.start( + ( + _invocation( + max_replans=0, + motion_source="motion_gen", + ), + ), + initial, + ) + session.tick(initial) + + changed = _collision_context( + 0.1, + qpos, + torch.tensor([0.4, 0.6]), + (0, 1), + ) + tick = session.tick(changed) + + collision_event = next( + event + for event in tick.events + if event.kind is ExecutionEventKind.COLLISION_WORLD_CHANGED + ) + exhausted_event = next( + event + for event in tick.events + if event.kind is ExecutionEventKind.RECOVERY_EXHAUSTED + ) + assert collision_event.env_mask.tolist() == [False, True] + assert exhausted_event.env_mask.tolist() == [False, True] + assert tick.status is ExecutionStatus.RUNNING + assert tick.eligible_mask.tolist() == [True, False] + assert tick.command is not None + assert tick.command.active_mask.tolist() == [True, False] + + def test_tracking_error_fails_when_replan_budget_is_zero() -> None: engine, _ = _engine() session = engine.start( @@ -265,6 +390,20 @@ def test_session_rejects_regressing_scene_snapshot() -> None: session.tick(_context(1.0, 0.0, 0.2, 1)) +def test_session_rejects_regressing_collision_world_revision() -> None: + engine, _ = _engine() + qpos = torch.zeros(1, 2) + initial = _collision_context(0.0, qpos, torch.tensor([0.4]), (2,)) + session = engine.start( + (_invocation(motion_source="motion_gen"),), + initial, + ) + regressed = _collision_context(0.1, qpos, torch.tensor([0.4]), (1,)) + + with pytest.raises(ValueError, match="Collision-world revisions"): + session.tick(regressed) + + def test_nonempty_effect_is_committed_only_after_external_verification() -> None: engine, _ = _engine() effect = EffectAction(engine.motion_generator) diff --git a/tests/sim/atomic_actions/test_runner.py b/tests/sim/atomic_actions/test_runner.py new file mode 100644 index 000000000..04cb7d7e8 --- /dev/null +++ b/tests/sim/atomic_actions/test_runner.py @@ -0,0 +1,439 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Tests for controller-independent atomic-action execution scheduling.""" + +from __future__ import annotations + +from collections import deque +from typing import ClassVar +from unittest.mock import Mock + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + ActionBinding, + ActionCfg, + ActionInvocation, + ActionPlan, + Affordance, + AtomicAction, + AtomicActionEngine, + CommandAcknowledgement, + CommandAckStatus, + CommandOperation, + EndEffectorPoseGoal, + ExecutionEventKind, + ExecutionRunner, + ExecutionRunnerCfg, + HeldObjectState, + JointCommand, + MotionPolicy, + ObjectSemantics, + PlanningContext, + RecoveryPolicy, + RobotObservation, + RunnerStatus, + SceneSnapshot, + StateDelta, + TaskState, + TimedTrajectory, +) + +BATCH_SIZE = 1 +ROBOT_DOF = 2 +FIRST_INTERVAL = 0.1 +SECOND_INTERVAL = 0.2 +TARGET_POSITION = 1.0 + + +class FakeClock: + """Deterministic clock used by non-blocking runner tests.""" + + def __init__(self) -> None: + self.time = 0.0 + self.sleeps: list[float] = [] + + def now(self) -> float: + """Return deterministic time.""" + return self.time + + def sleep(self, duration: float) -> None: + """Advance deterministic time.""" + self.sleeps.append(duration) + self.time += duration + + def advance(self, duration: float) -> None: + """Advance time outside the runner's blocking loop.""" + self.time += duration + + +class FakeObservationProvider: + """In-memory robot observation provider.""" + + def __init__(self, clock: FakeClock, batch_size: int = BATCH_SIZE) -> None: + self.clock = clock + self.qpos = torch.zeros(batch_size, ROBOT_DOF) + self.fail = False + + def observe(self, task_state: TaskState) -> PlanningContext: + """Return the current in-memory robot state.""" + if self.fail: + raise RuntimeError("observation unavailable") + return PlanningContext( + robot=RobotObservation( + timestamp=self.clock.now(), + qpos=self.qpos, + qvel=torch.zeros_like(self.qpos), + ), + task=task_state, + scene=SceneSnapshot(timestamp=self.clock.now(), version=0), + env_ids=torch.arange(self.qpos.shape[0], dtype=torch.long), + ) + + +class FakeCommandSink: + """Recording command sink with configurable acknowledgements and tracking.""" + + 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.held: list[JointCommand] = [] + self.cancel_count = 0 + + def send( + self, + command: JointCommand, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Record an active command and optionally update observed qpos.""" + self.sent.append(command) + status = ( + self.send_statuses.popleft() + if self.send_statuses + else CommandAckStatus.ACCEPTED + ) + follows = self.follow_commands.popleft() if self.follow_commands else True + if status is CommandAckStatus.ACCEPTED and follows: + self.provider.qpos = command.positions.clone() + return CommandAcknowledgement(status) + + def hold( + self, + command: JointCommand, + *, + timeout: float, + ) -> CommandAcknowledgement: + """Record and apply a hold command.""" + self.held.append(command) + self.provider.qpos = command.positions.clone() + return CommandAcknowledgement.accepted_ack() + + def cancel(self, *, timeout: float) -> CommandAcknowledgement: + """Record controller cancellation.""" + self.cancel_count += 1 + return CommandAcknowledgement.accepted_ack() + + +class TimedAction(AtomicAction[EndEffectorPoseGoal]): + """Test action with explicit non-uniform command intervals.""" + + skill_id: ClassVar[str] = "timed" + GoalType: ClassVar[type] = EndEffectorPoseGoal + manipulator_roles: ClassVar[tuple[str, ...]] = ("primary",) + + def __init__(self, motion_generator, *, with_effect: bool = False) -> None: + super().__init__(motion_generator, ActionCfg(name="timed")) + self.with_effect = with_effect + self.plan_count = 0 + + def _plan( + self, + invocation: ActionInvocation[EndEffectorPoseGoal], + context: PlanningContext, + ) -> ActionPlan: + """Plan three samples with intervals 0.1 s and 0.2 s.""" + goal = self.require_goal(invocation) + self.plan_count += 1 + assert isinstance(goal.xpos, torch.Tensor) + target_value = float(goal.xpos[0, 3]) + target = torch.full_like(context.robot.qpos, target_value) + midpoint = torch.lerp(context.robot.qpos, target, 0.5) + positions = torch.stack([context.robot.qpos, midpoint, target], dim=1) + dt = torch.tensor( + [[0.0, FIRST_INTERVAL, SECOND_INTERVAL]], + dtype=torch.float32, + ).repeat(context.batch_size, 1) + if context.batch_size > 1: + dt[1, 1:] *= 2.0 + trajectory = TimedTrajectory.from_positions( + positions, + env_ids=context.env_ids, + control_dt=invocation.motion_policy.control_dt, + dt=dt, + ) + effects = StateDelta() + if self.with_effect: + semantics = ObjectSemantics( + affordance=Affordance(), geometry={}, label="runner-object" + ) + held = HeldObjectState( + semantics=semantics, + object_to_eef=torch.eye(4), + grasp_xpos=torch.eye(4), + ) + effects = StateDelta(held_object_updates={"arm": held}) + return self.build_plan( + invocation, + context, + success=True, + trajectory=trajectory, + expected_effects=effects, + ) + + +def _make_runner( + *, + with_effect: bool = False, + batch_size: int = BATCH_SIZE, +) -> tuple[ + ExecutionRunner, + FakeClock, + FakeObservationProvider, + FakeCommandSink, + TimedAction, +]: + clock = FakeClock() + provider = FakeObservationProvider(clock, batch_size) + sink = FakeCommandSink(provider) + robot = Mock() + robot.device = torch.device("cpu") + robot.dof = ROBOT_DOF + robot.get_qpos.return_value = torch.zeros(batch_size, ROBOT_DOF) + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "stub" + action = TimedAction(generator, with_effect=with_effect) + engine = AtomicActionEngine(generator) + engine.register(action) + initial_task = TaskState.empty(batch_size, "cpu") + initial_context = provider.observe(initial_task) + goal_pose = torch.eye(4) + goal_pose[0, 3] = TARGET_POSITION + invocation = ActionInvocation( + skill_id="timed", + goal=EndEffectorPoseGoal(goal_pose), + binding=ActionBinding(manipulators={"primary": "arm"}), + motion_policy=MotionPolicy(sample_count=3, control_dt=FIRST_INTERVAL), + recovery_policy=RecoveryPolicy( + max_replans=2, + tracking_error_threshold=0.05, + phase_timeout=10.0, + ), + ) + session = engine.start((invocation,), initial_context) + runner = ExecutionRunner( + session, + provider, + sink, + clock=clock, + cfg=ExecutionRunnerCfg(minimum_cycle_time=0.01), + ) + return runner, clock, provider, sink, action + + +def test_runner_dispatches_only_when_timed_waypoint_is_due() -> None: + runner, clock, _, sink, _ = _make_runner() + + first = runner.step() + early = runner.step() + clock.advance(FIRST_INTERVAL) + second = runner.step() + + assert first.command_count == 1 + assert first.wait_duration == pytest.approx(FIRST_INTERVAL) + assert early.is_waiting + assert len(sink.sent) == 2 + assert second.command_count == 2 + assert second.wait_duration == pytest.approx(SECOND_INTERVAL) + + +def test_runner_uses_the_longest_active_batch_interval_as_a_barrier() -> None: + runner, _, _, _, _ = _make_runner(batch_size=2) + + first = runner.step() + + assert first.wait_duration == pytest.approx(2.0 * FIRST_INTERVAL) + + +def test_runner_completes_and_holds_after_last_command_settles() -> None: + runner, clock, _, sink, _ = _make_runner() + + runner.step() + clock.advance(FIRST_INTERVAL) + runner.step() + clock.advance(SECOND_INTERVAL) + runner.step() + clock.advance(SECOND_INTERVAL) + completed = runner.step() + + assert completed.status is RunnerStatus.COMPLETED + assert completed.command_count == 3 + assert [item.operation for item in completed.dispatches] == [CommandOperation.HOLD] + assert len(sink.held) == 1 + + +@pytest.mark.parametrize( + "status", + [CommandAckStatus.REJECTED, CommandAckStatus.TIMED_OUT], +) +def test_runner_safely_stops_when_command_is_not_accepted( + status: CommandAckStatus, +) -> None: + runner, _, _, sink, _ = _make_runner() + sink.send_statuses.append(status) + + failed = runner.step() + + assert failed.status is RunnerStatus.FAILED + assert [item.operation for item in failed.dispatches] == [ + CommandOperation.SEND, + CommandOperation.CANCEL, + CommandOperation.HOLD, + ] + assert sink.cancel_count == 1 + assert failed.message is not None and status.value in failed.message + + +def test_runner_cancel_performs_cancel_then_hold() -> None: + runner, _, _, sink, _ = _make_runner() + + cancelled = runner.cancel("operator stop") + repeated = runner.step() + + assert cancelled.status is RunnerStatus.CANCELLED + assert [item.operation for item in cancelled.dispatches] == [ + CommandOperation.CANCEL, + CommandOperation.HOLD, + ] + assert cancelled.message == "operator stop" + assert repeated.status is RunnerStatus.CANCELLED + assert repeated.dispatches == () + assert sink.cancel_count == 1 + + +def test_runner_replans_from_observation_after_tracking_error() -> None: + runner, clock, _, sink, action = _make_runner() + sink.follow_commands.extend([True, False, True]) + + runner.step() + clock.advance(FIRST_INTERVAL) + runner.step() + clock.advance(SECOND_INTERVAL) + recovered = runner.step() + + assert action.plan_count == 2 + assert recovered.tick is not None + event_kinds = {event.kind for event in recovered.tick.events} + assert ExecutionEventKind.TRACKING_ERROR in event_kinds + assert ExecutionEventKind.REPLANNED in event_kinds + assert recovered.status is RunnerStatus.RUNNING + + +def test_runner_fails_safely_when_observation_provider_raises() -> None: + runner, _, provider, sink, _ = _make_runner() + provider.fail = True + + failed = runner.step() + + assert failed.status is RunnerStatus.FAILED + assert [item.operation for item in failed.dispatches] == [ + CommandOperation.CANCEL, + CommandOperation.HOLD, + ] + assert len(sink.held) == 1 + assert sink.cancel_count == 1 + assert failed.message is not None and "observation unavailable" in failed.message + + +def test_blocking_runner_uses_clock_and_completes() -> None: + runner, clock, _, _, _ = _make_runner() + + completed = runner.run_until_blocked() + + assert completed.status is RunnerStatus.COMPLETED + assert completed.command_count == 3 + assert clock.sleeps == pytest.approx( + [FIRST_INTERVAL, SECOND_INTERVAL, SECOND_INTERVAL] + ) + + +def test_blocking_runner_safely_stops_when_the_clock_fails() -> None: + runner, clock, _, sink, _ = _make_runner() + + def fail_sleep(duration: float) -> None: + raise RuntimeError("clock backend unavailable") + + clock.sleep = fail_sleep + + failed = runner.run_until_blocked() + + assert failed.status is RunnerStatus.FAILED + assert [item.operation for item in failed.dispatches[-2:]] == [ + CommandOperation.CANCEL, + CommandOperation.HOLD, + ] + assert sink.cancel_count == 1 + assert failed.message is not None and "clock backend unavailable" in failed.message + + +def test_blocking_runner_verifies_effect_before_committing_task_state() -> None: + runner, _, _, _, _ = _make_runner(with_effect=True) + + completed = runner.run_until_blocked( + effect_verifier=lambda context, tick: torch.ones( + context.batch_size, dtype=torch.bool + ) + ) + + assert completed.status is RunnerStatus.COMPLETED + assert completed.tick is not None + assert completed.tick.task_state.get_held_object("arm") is not None + + +def test_blocking_runner_resumes_a_stored_effect_verification_boundary() -> None: + runner, _, _, _, _ = _make_runner(with_effect=True) + + blocked = runner.run_until_blocked() + + assert blocked.status is RunnerStatus.RUNNING + 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 runner.effect_verification_pending is False + assert completed.status is RunnerStatus.COMPLETED + assert completed.tick is not None + assert completed.tick.task_state.get_held_object("arm") is not None diff --git a/tests/sim/atomic_actions/test_sim_adapter.py b/tests/sim/atomic_actions/test_sim_adapter.py new file mode 100644 index 000000000..db4c1ce7a --- /dev/null +++ b/tests/sim/atomic_actions/test_sim_adapter.py @@ -0,0 +1,227 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Tests for the simulation execution-runner adapter.""" + +from __future__ import annotations + +from unittest.mock import Mock + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + CommandAckStatus, + JointCommand, + RigidObjectSceneProvider, + RigidObjectSceneProviderCfg, + SceneSnapshot, + SimulationExecutionAdapter, + TaskState, +) + +BATCH_SIZE = 2 +ROBOT_DOF = 3 +PHYSICS_DT = 0.01 + + +def _simulation_and_robot() -> tuple[Mock, Mock]: + simulation = Mock() + simulation.sim_config.physics_dt = PHYSICS_DT + robot = Mock() + robot.get_qpos.return_value = torch.zeros(BATCH_SIZE, ROBOT_DOF) + robot.get_qvel.return_value = torch.full((BATCH_SIZE, ROBOT_DOF), 0.1) + robot.get_qf.return_value = torch.full((BATCH_SIZE, ROBOT_DOF), 0.2) + return simulation, robot + + +def _command(*, env_ids: torch.Tensor | None = None) -> JointCommand: + return JointCommand( + positions=torch.ones(BATCH_SIZE, ROBOT_DOF), + velocities=torch.full((BATCH_SIZE, ROBOT_DOF), 0.5), + active_mask=torch.tensor([True, False]), + env_ids=( + torch.arange(BATCH_SIZE, dtype=torch.long) if env_ids is None else env_ids + ), + hold_duration=torch.full((BATCH_SIZE,), 0.1), + ) + + +def test_simulation_adapter_observes_full_robot_state() -> None: + simulation, robot = _simulation_and_robot() + adapter = SimulationExecutionAdapter(simulation, robot) + + context = adapter.observe(TaskState.empty(BATCH_SIZE, "cpu")) + + assert context.robot.timestamp == 0.0 + assert torch.equal(context.robot.qpos, robot.get_qpos.return_value) + assert torch.equal(context.robot.qvel, robot.get_qvel.return_value) + assert torch.equal(context.robot.qeffort, robot.get_qf.return_value) + assert context.scene.version == 0 + + +@pytest.mark.parametrize("error", [AttributeError, NotImplementedError]) +def test_simulation_adapter_treats_unavailable_effort_as_optional( + error: type[Exception], +) -> None: + simulation, robot = _simulation_and_robot() + robot.get_qf.side_effect = error("effort unavailable") + adapter = SimulationExecutionAdapter(simulation, robot) + + context = adapter.observe(TaskState.empty(BATCH_SIZE, "cpu")) + + assert context.robot.qeffort is None + + +def test_simulation_adapter_sends_active_rows_and_inactive_holds_together() -> None: + simulation, robot = _simulation_and_robot() + adapter = SimulationExecutionAdapter(simulation, robot) + command = _command() + + acknowledgement = adapter.send(command, timeout=1.0) + + 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) + assert robot.set_qpos.call_args.kwargs["env_ids"] == [0, 1] + assert robot.set_qvel.call_args.kwargs["env_ids"] == [0, 1] + + +def test_simulation_adapter_keeps_stable_ids_separate_from_robot_indices() -> None: + simulation, robot = _simulation_and_robot() + stable_ids = torch.tensor([10, 20], dtype=torch.long) + adapter = SimulationExecutionAdapter(simulation, robot, env_ids=stable_ids) + command = _command(env_ids=stable_ids) + + acknowledgement = adapter.send(command, timeout=1.0) + + assert acknowledgement.status is CommandAckStatus.ACCEPTED + assert robot.set_qpos.call_args.kwargs["env_ids"] == [0, 1] + + +def test_simulation_adapter_hold_targets_every_environment() -> None: + simulation, robot = _simulation_and_robot() + adapter = SimulationExecutionAdapter(simulation, robot) + command = _command() + + acknowledgement = adapter.hold(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]) + + +def test_simulation_adapter_sleep_advances_integral_physics_steps() -> None: + simulation, robot = _simulation_and_robot() + adapter = SimulationExecutionAdapter(simulation, robot) + + adapter.sleep(0.025) + + simulation.update.assert_called_once_with(physics_dt=PHYSICS_DT, step=3) + assert adapter.now() == pytest.approx(0.03) + + +def test_simulation_adapter_supplies_time_and_ids_to_scene_provider() -> None: + simulation, robot = _simulation_and_robot() + timestamps: list[float] = [] + observed_env_ids: list[torch.Tensor] = [] + + class RecordingSceneProvider: + """Record adapter correlation arguments for this test.""" + + def snapshot( + self, + *, + timestamp: float, + env_ids: torch.Tensor, + ) -> SceneSnapshot: + timestamps.append(timestamp) + observed_env_ids.append(env_ids.clone()) + return SceneSnapshot(timestamp=timestamp, version=3) + + adapter = SimulationExecutionAdapter( + simulation, + robot, + scene_provider=RecordingSceneProvider(), + ) + adapter.sleep(PHYSICS_DT) + + context = adapter.observe(TaskState.empty(BATCH_SIZE, "cpu")) + + assert timestamps == pytest.approx([PHYSICS_DT]) + assert torch.equal(observed_env_ids[0], torch.arange(BATCH_SIZE)) + assert context.scene.version == 3 + + +def test_rigid_object_scene_provider_tracks_per_environment_collision_revision() -> ( + None +): + obstacle = Mock() + initial_pose = torch.eye(4).repeat(BATCH_SIZE, 1, 1) + obstacle.get_local_pose.return_value = initial_pose + provider = RigidObjectSceneProvider( + {"obstacle": obstacle}, + collision_entity_ids=("obstacle",), + ) + env_ids = torch.arange(BATCH_SIZE, dtype=torch.long) + + initial = provider.snapshot(timestamp=0.0, env_ids=env_ids) + moved_pose = initial_pose.clone() + moved_pose[1, 0, 3] = 0.01 + obstacle.get_local_pose.return_value = moved_pose + changed = provider.snapshot(timestamp=PHYSICS_DT, env_ids=env_ids) + + assert initial.version == 0 + assert initial.collision_world_revisions(BATCH_SIZE) == (0, 0) + assert changed.version == 1 + assert changed.collision_world_revisions(BATCH_SIZE) == (0, 1) + assert changed.collision_entity_ids == ("obstacle",) + assert torch.equal(changed.entities["obstacle"].pose, moved_pose) + + +def test_rigid_object_scene_provider_filters_subthreshold_pose_noise() -> None: + obstacle = Mock() + initial_pose = torch.eye(4).repeat(BATCH_SIZE, 1, 1) + obstacle.get_local_pose.return_value = initial_pose + provider = RigidObjectSceneProvider( + {"obstacle": obstacle}, + collision_entity_ids=("obstacle",), + cfg=RigidObjectSceneProviderCfg(translation_threshold=0.01), + ) + env_ids = torch.arange(BATCH_SIZE, dtype=torch.long) + provider.snapshot(timestamp=0.0, env_ids=env_ids) + noisy_pose = initial_pose.clone() + noisy_pose[:, 0, 3] = 0.001 + obstacle.get_local_pose.return_value = noisy_pose + + unchanged = provider.snapshot(timestamp=PHYSICS_DT, env_ids=env_ids) + + assert unchanged.version == 0 + assert unchanged.collision_world_revisions(BATCH_SIZE) == (0, 0) + + +def test_simulation_adapter_rejects_changed_environment_identity() -> None: + simulation, robot = _simulation_and_robot() + adapter = SimulationExecutionAdapter(simulation, robot) + command = _command(env_ids=torch.tensor([1, 0], dtype=torch.long)) + + acknowledgement = adapter.send(command, timeout=1.0) + + assert acknowledgement.status is CommandAckStatus.REJECTED + assert "env_ids" in acknowledgement.message + robot.set_qpos.assert_not_called() diff --git a/tests/sim/planners/test_curobo_planner.py b/tests/sim/planners/test_curobo_planner.py index 81edea96e..6bea60da1 100644 --- a/tests/sim/planners/test_curobo_planner.py +++ b/tests/sim/planners/test_curobo_planner.py @@ -174,6 +174,24 @@ def test_curobo_world_cfg_uses_v2_safe_default_collision_cache(): assert cfg.obstacle_representation == "sphere" +def test_curobo_collision_world_binding_merges_owned_obstacle_poses(): + planner = object.__new__(CuroboPlanner) + configured_pose = torch.eye(4).unsqueeze(0) + observed_pose = torch.eye(4).unsqueeze(0) + observed_pose[:, 0, 3] = 0.5 + options = CuroboPlanOptions(dynamic_obstacle_poses={"configured": configured_pose}) + + bound = planner.with_collision_world( + options, + obstacle_poses={"observed": observed_pose}, + ) + + assert bound is options + assert set(bound.dynamic_obstacle_poses) == {"configured", "observed"} + assert torch.equal(bound.dynamic_obstacle_poses["observed"], observed_pose) + assert bound.dynamic_obstacle_poses["observed"] is not observed_pose + + def test_auto_gen_defaults_keep_sphere_count_low(): """The voxel sphere estimate must be scaled down so planning stays fast.""" auto = CuroboPlannerCfg(robot_uid="franka").auto_gen