Add dynamic scene recovery for atomic actions

[yuecideng]

Description

This PR adds dynamic-scene recovery to the typed atomic-action execution stack.

It introduces a SceneProvider boundary, a RigidObjectSceneProvider, global/per-environment collision-world revisions, and collision-sensitive phase invalidation. AtomicAction.plan() now owns scene binding and delegates skill logic to _plan(), so current obstacle poses are injected into copied planner options instead of being repeated across individual skill parameters. cuRobo implements the dynamic-world bridge, and ExecutionSession emits collision_world_changed before bounded per-environment replanning.

The PR also adds runnable moving-target and dynamic-obstacle recovery tutorials, focused multi-environment recovery tests, planner/provider tests, and corresponding Sphinx and agent-context updates.

Dependencies: #449. This is a stacked PR whose base is feat/atomic-action-pr2a-execution-runner; commit 7cbba908 is the PR2B-only change.

Fixes: N/A (no linked issue).

Type of change

• Breaking change (existing functionality will not work without user modification)

The extension hook for AtomicAction subclasses changes from overriding public plan() to implementing _plan(). This is intentional within the unmerged refactor stack and prevents actions from bypassing framework-owned scene binding.

Validation

• black --check --diff --color ./ — 562 files unchanged
• pytest -q tests/sim/atomic_actions tests/sim/planners/test_curobo_planner.py tests/sim/planners/test_motion_generator_batched.py — 181 passed, 4 skipped
• python -m compileall -q embodichain/lab/sim/atomic_actions scripts/tutorials/atomic_action
• agent_context/MAP.yaml YAML parsing
• Moving-target headless simulation — dynamic_goal_changed → replanned → completed after 83 accepted commands
• Dynamic-obstacle headless simulation — collision_world_changed → replanned → completed after 83 accepted commands
• Sphinx HTML build completed; existing repository-wide autodoc warnings remain
• Full test suite was not run; affected atomic-action and planner scopes were validated proportionally

Screenshots

Not applicable; this change adds runtime APIs and headless simulation tutorials.

Checklist

• I have run the black . command to format the code base.
• I have made corresponding changes to the documentation
• I have added tests that prove my fix is effective or that my feature works
• Dependencies have been updated, if applicable.

[greptile-apps]

Greptile Summary

This PR adds dynamic collision-scene binding and recovery to atomic-action execution.

• Introduces scene-provider abstractions and global or per-environment collision-world revisions.
• Binds current obstacle poses into copied planner options through the framework-owned action planning entry point.
• Adds cuRobo dynamic-world support and collision-sensitive execution replanning.
• Adds focused tests, tutorials, documentation, and agent guidance for dynamic-obstacle recovery.

Confidence Score: 5/5

The PR appears safe to merge because no blocking failure remains in the eligible follow-up-review scope.

No blocking failure remains.

Important Files Changed

Filename	Overview
embodichain/lab/sim/atomic_actions/core.py	Makes public planning framework-owned, delegates skill behavior to _plan(), and binds current collision poses into copied motion policies.
embodichain/lab/sim/atomic_actions/execution.py	Detects per-environment collision-world revision changes and performs bounded synchronized-cohort replanning.
embodichain/lab/sim/atomic_actions/sim_adapter.py	Adds rigid-object scene observation, pose-change thresholds, and per-environment collision revision tracking.
embodichain/lab/sim/atomic_actions/state.py	Extends scene snapshots with validated collision entities, revision expansion, and batched obstacle-pose extraction.
embodichain/lab/sim/planners/curobo/curobo_planner.py	Implements per-plan dynamic-obstacle binding and forwards simulator-world poses through cuRobo’s existing frame conversion.
embodichain/lab/sim/planners/motion_generator.py	Exposes planner collision-world capability and copies reusable options before binding live obstacles.

Sequence Diagram

sequenceDiagram
    participant Provider as SceneProvider
    participant Adapter as ExecutionAdapter
    participant Session as ExecutionSession
    participant Action as AtomicAction
    participant Generator as MotionGenerator
    participant Curobo as cuRobo
    Provider->>Adapter: SceneSnapshot with obstacle poses and revisions
    Adapter->>Session: PlanningContext
    Session->>Session: Compare planned and current revisions
    alt Collision world changed
        Session->>Action: Replan from latest context
        Action->>Generator: Bind current collision world
        Generator->>Curobo: Copied options with obstacle poses
        Curobo-->>Session: Updated collision-aware trajectory
    end

Loading

_{Reviews (6): Last reviewed commit: "fix(atomic-actions): validate scene thre..." | Re-trigger Greptile}

[Copilot]

Pull request overview

This PR extends the typed atomic-action execution stack with dynamic-scene recovery by introducing a SceneProvider boundary, per-environment collision-world revision tracking, and collision-sensitive phase invalidation. It also formalizes controller-facing scheduling/ack handling via an ExecutionRunner, and updates atomic-action subclassing so framework-owned AtomicAction.plan() performs scene binding before delegating to _plan().

Changes:

• Add SceneProvider + RigidObjectSceneProvider and propagate global/per-env collision_world_revision through SceneSnapshot to enable collision-world-triggered replanning.
• Introduce ExecutionRunner (plus simulation adapters) to schedule commands from TimedTrajectory.dt, manage acknowledgements, and perform cancel-then-hold on failure.
• Update primitives/tests/docs/tutorials to the new _plan() hook and add end-to-end tutorials + focused test coverage for dynamic-goal and dynamic-obstacle recovery.

Reviewed changes

Copilot reviewed 38 out of 38 changed files in this pull request and generated no comments.

Show a summary per file

File	Description
tests/sim/planners/test_curobo_planner.py	Adds coverage for cuRobo obstacle-pose binding into per-plan options.
tests/sim/atomic_actions/test_tutorial_utils.py	Adds unit tests for dynamic-obstacle tutorial helpers (animation, clearance metrics).
tests/sim/atomic_actions/test_sim_adapter.py	Adds tests for the new simulation execution adapter and scene provider integration.
tests/sim/atomic_actions/test_runner.py	Adds comprehensive tests for non-blocking/blocking ExecutionRunner scheduling and failure handling.
tests/sim/atomic_actions/test_engine.py	Updates action hook to _plan() and validates subclasses cannot override framework plan().
tests/sim/atomic_actions/test_engine_per_env.py	Adds per-env collision-world revision recovery tests and motion-policy motion-source coverage.
tests/sim/atomic_actions/test_core.py	Adds tests for collision-world revision expansion/obstacle pose extraction and trajectory snapshot ownership.
tests/sim/atomic_actions/test_actions.py	Extends PickUp tests to cover late-bound grasp resolution and dynamic-goal replanning events.
scripts/tutorials/atomic_action/moving_target_recovery.py	New runnable tutorial demonstrating replanning when a visible grasp target moves.
scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py	New runnable tutorial demonstrating collision-world revision recovery with cuRobo dynamic obstacles.
embodichain/lab/sim/planners/curobo/curobo_planner.py	Implements supports_collision_world_updates and binds dynamic obstacle poses into cuRobo plan options.
embodichain/lab/sim/planners/base_planner.py	Adds generic collision-world update hook and capability flag to planner base API.
embodichain/lab/sim/atomic_actions/state.py	Extends SceneSnapshot with collision-world revision + collision entities; adds helpers to expand revisions and extract obstacle poses.
embodichain/lab/sim/atomic_actions/sim_adapter.py	Adds SimulationExecutionAdapter and RigidObjectSceneProvider for simulation observation/command/clock + scene revisions.
embodichain/lab/sim/atomic_actions/scene.py	Introduces the SceneProvider protocol boundary for scene snapshot production.
embodichain/lab/sim/atomic_actions/runner.py	Adds ExecutionRunner with acknowledgement handling, timing-based scheduling, and safe-stop behavior.
embodichain/lab/sim/atomic_actions/primitives/press.py	Migrates primitive to _plan() hook.
embodichain/lab/sim/atomic_actions/primitives/place.py	Migrates primitive to _plan() hook.
embodichain/lab/sim/atomic_actions/primitives/pick_up.py	Extends GraspGoal.grasp_xpos to support late-bound SceneEntityPose via pose-goal utilities and migrates to _plan().
embodichain/lab/sim/atomic_actions/primitives/move_joints.py	Migrates primitive to _plan() hook.
embodichain/lab/sim/atomic_actions/primitives/move_held_object.py	Migrates primitive to _plan() hook.
embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py	Migrates primitive to _plan() hook.
embodichain/lab/sim/atomic_actions/primitives/hand_over.py	Migrates primitive to _plan() hook.
embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py	Migrates primitive to _plan() hook.
embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py	Migrates primitive to _plan() hook.
embodichain/lab/sim/atomic_actions/plans.py	Adds TimedTrajectory.snapshot() and extends phase metadata with collision-world sensitivity and planned collision revision.
embodichain/lab/sim/atomic_actions/execution.py	Adds collision-world change detection/replan events, exposes latest_context/active_trajectory, and refines command hold scheduling semantics.
embodichain/lab/sim/atomic_actions/engine.py	Validates phases record the planned collision-world revision.
embodichain/lab/sim/atomic_actions/core.py	Makes plan() framework-owned (template method), enforces _plan() override, and binds collision-world obstacle poses into copied plan options.
embodichain/lab/sim/atomic_actions/init.py	Exposes new runner/scene/adapter APIs at the package boundary.
docs/source/tutorial/atomic_actions.rst	Updates tutorial documentation to use runner + scene provider and documents new recovery flows and _plan() hook.
docs/source/overview/sim/atomic_actions/index.md	Updates overview architecture docs to include runner, provider/ports, and collision-world revisions.
docs/source/overview/sim/atomic_actions/builtin_actions.md	Updates PickUp documentation to reflect late-bound grasp poses and dependency-based recovery.
docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.rst	Updates API reference exports to include the new runner/ports/adapters and execution enums.
agent_context/topics/motion-planning/motion-planning.md	Documents the generic dynamic-world bridge (with_collision_world) and planner opt-in requirements.
agent_context/topics/atomic-actions/atomic-actions.md	Updates agent context with runner/session/provider responsibilities and collision-world-driven recovery behavior.
agent_context/MAP.yaml	Adds new atomic-action topic keywords and file references for runner/scene/sim_adapter.
.agents/skills/add-atomic-action/SKILL.md	Updates the skill guidance to the new _plan() hook and documents runner/scene-provider considerations.

---

💡 Add a code-review agent skill or configure MCP servers for context-aware, tailored reviews. Learn more in the docs.

[Copilot]

Pull request overview

Copilot reviewed 33 out of 33 changed files in this pull request and generated no new comments.

Suppressed comments (2)

embodichain/lab/sim/atomic_actions/sim_adapter.py:62

• RigidObjectSceneProviderCfg.post_init only checks non-negativity; NaN/inf thresholds would silently disable pose-change detection (comparisons with NaN are always False). Validate finiteness as well so invalid configs fail fast.

    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.")

embodichain/lab/sim/atomic_actions/state.py:547

• SceneSnapshot.collision_obstacle_poses() clones each pose, but planners that opt into collision-world updates (e.g., CuroboPlanner.with_collision_world) clone again to guarantee owned option storage. This double-cloning adds avoidable allocations on every (re)plan. Consider returning the (possibly expanded) pose tensor without cloning here and let the planner own the copy.

                    f"Collision entity {entity_id!r} pose must match planning "
                    f"batch size {batch_size}."
                )
            poses[entity_id] = pose.clone()
        return MappingProxyType(poses)

[Copilot]

Pull request overview

Copilot reviewed 36 out of 36 changed files in this pull request and generated no new comments.

[Copilot]

Pull request overview

Copilot reviewed 36 out of 36 changed files in this pull request and generated no new comments.

[Copilot]

Pull request overview

Copilot reviewed 36 out of 36 changed files in this pull request and generated no new comments.

Suppressed comments (1)

scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py:288

• _minimum_cuboid_clearance() converts world-frame offsets into the cuboid’s local frame using the cuboid rotation matrix directly. Since get_local_pose(to_matrix=True) returns a homogeneous transform with R mapping local→world, the world→local rotation should use Rᵀ; using R will produce incorrect clearances for general orientations (even if the current tests don’t catch it due to abs()/symmetries).

    relative = path - cuboid_pose[:, None, :3, 3]
    local_points = torch.matmul(relative, cuboid_pose[:, :3, :3])
    local_offset = torch.abs(local_points) - half_extent