From 4ea75966ee5316353a0829010983232274e5e522 Mon Sep 17 00:00:00 2001 From: Logic Date: Thu, 23 Apr 2026 16:43:02 +0800 Subject: [PATCH] docs(plan): add sim air insert ring bar implementation plan --- .../2026-04-23-sim-air-insert-ring-bar.md | 295 ++++++++++++++++++ 1 file changed, 295 insertions(+) create mode 100644 docs/superpowers/plans/2026-04-23-sim-air-insert-ring-bar.md diff --git a/docs/superpowers/plans/2026-04-23-sim-air-insert-ring-bar.md b/docs/superpowers/plans/2026-04-23-sim-air-insert-ring-bar.md new file mode 100644 index 0000000..02b5e6d --- /dev/null +++ b/docs/superpowers/plans/2026-04-23-sim-air-insert-ring-bar.md @@ -0,0 +1,295 @@ +# sim_air_insert_ring_bar Implementation Plan + +> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking. + +**Goal:** Add an independent dual-Diana MuJoCo task `sim_air_insert_ring_bar` with a square ring block, a square bar block, staged rewards, strict finite-geometry in-air insertion success detection, and a task-specific scripted policy. + +**Architecture:** Reuse the current dual-Diana EE-control stack and environment factory, but add a task-specific scene XML, robot asset entrypoint, sampling helpers, and a new task-specific environment module. Keep `sim_transfer` untouched while introducing pure-Python geometry helpers and focused tests so reward/success behavior can be regression tested without requiring a full MuJoCo rollout in every test. + +**Tech Stack:** Python, unittest, MuJoCo XML assets, existing dual-Diana environment classes, Hydra-compatible task naming/config patterns. + +--- + +## File Structure / Responsibilities + +- **Create:** `roboimi/assets/models/manipulators/DianaMed/ring_bar_objects.xml` + - Defines the rigid ring body and bar body, each with a free joint and stable box-based geoms. +- **Create:** `roboimi/assets/models/manipulators/DianaMed/bi_diana_ring_bar_ee.xml` + - Scene entrypoint that includes the shared world/table/robot assets plus the new object XML. +- **Modify:** `roboimi/assets/robots/diana_med.py` + - Add a task-specific robot asset class for the new scene XML without changing existing `BiDianaMed` behavior. +- **Modify:** `roboimi/utils/act_ex_utils.py` + - Add deterministic helpers to sample left/right planar placement regions for ring and bar objects. +- **Modify:** `roboimi/utils/constants.py` + - Register the new task name and default metadata. +- **Create:** `roboimi/envs/double_air_insert_env.py` + - New task-specific environment, finite-geometry success helpers, reset logic, reward logic, and task factory branch. +- **Modify:** `roboimi/envs/double_pos_ctrl_env.py` + - Route `make_sim_env()` to the new task-specific environment while keeping current `sim_transfer` logic unchanged. +- **Create:** `roboimi/demos/diana_air_insert_policy.py` + - Task-specific waypoint/open-loop scripted policy for grasp-lift-align-insert. +- **Modify:** `roboimi/demos/vla_scripts/eval_vla.py` + - Reset the new task with the correct sampled task state instead of assuming a single transfer box pose. +- **Create:** `tests/test_air_insert_env.py` + - Focused unit tests for sampling, reset helpers, reward progression, and strict success detection. +- **Modify:** `tests/test_eval_vla_headless.py` + - Add coverage that headless evaluation dispatches the correct reset sampler for the new task. +- **Modify:** `tests/test_robot_asset_paths.py` + - Verify the new robot asset class resolves its XML path correctly independent of cwd. + +--- + +### Task 1: Add failing tests for task registration, samplers, and asset wiring + +**Files:** +- Create: `tests/test_air_insert_env.py` +- Modify: `tests/test_eval_vla_headless.py` +- Modify: `tests/test_robot_asset_paths.py` +- Modify: `roboimi/utils/act_ex_utils.py` (later in implementation) +- Modify: `roboimi/utils/constants.py` (later in implementation) +- Modify: `roboimi/assets/robots/diana_med.py` (later in implementation) +- Modify: `roboimi/envs/double_pos_ctrl_env.py` (later in implementation) + +- [ ] **Step 1: Write failing tests for task config and sampling helpers** + +Add tests in `tests/test_air_insert_env.py` covering: +- `SIM_TASK_CONFIGS['sim_air_insert_ring_bar']` exists +- `sample_air_insert_ring_bar_pose()` (or equivalent helper) returns ring/bar positions with fixed z and correct left/right planar ranges +- output structure is explicit and easy for reset/eval code to consume + +- [ ] **Step 2: Write failing tests for environment factory dispatch and robot asset resolution** + +Add tests covering: +- `make_sim_env('sim_air_insert_ring_bar', headless=True)` dispatches to the new environment with rendering disabled +- a new robot asset class resolves the new XML path independent of cwd, similar to the existing `BiDianaMed` test pattern + +- [ ] **Step 3: Write failing tests for eval reset helper dispatch** + +Extend `tests/test_eval_vla_headless.py` so headless eval can reset the new task using the new sampler instead of hard-coding `sample_transfer_pose()`. + +- [ ] **Step 4: Run the targeted tests to verify they fail for the expected missing-feature reasons** + +Run: +`/home/droid/.conda/envs/roboimi/bin/python -m unittest tests.test_air_insert_env tests.test_eval_vla_headless tests.test_robot_asset_paths -v` + +Expected: +- FAIL because the new task config/helper/class/dispatch branch does not exist yet + +- [ ] **Step 5: Implement the minimal production code to satisfy the new task registration and helper tests** + +Implement only enough to make the new tests pass: +- add new task config entry +- add the new placement sampler +- add the new robot asset class +- add the factory dispatch branch / headless wiring +- update eval reset dispatch for the new task + +- [ ] **Step 6: Re-run the targeted tests to verify they pass** + +Run: +`/home/droid/.conda/envs/roboimi/bin/python -m unittest tests.test_air_insert_env tests.test_eval_vla_headless tests.test_robot_asset_paths -v` + +Expected: +- PASS for the new registration/sampler/dispatch/asset tests + +- [ ] **Step 7: Commit Task 1** + +Run: +`git add tests/test_air_insert_env.py tests/test_eval_vla_headless.py tests/test_robot_asset_paths.py roboimi/utils/act_ex_utils.py roboimi/utils/constants.py roboimi/assets/robots/diana_med.py roboimi/envs/double_pos_ctrl_env.py roboimi/demos/vla_scripts/eval_vla.py && git commit -m "feat(env): register sim air insert ring bar task"` + +--- + +### Task 2: Add the MuJoCo ring+bar scene assets and reset helpers + +**Files:** +- Create: `roboimi/assets/models/manipulators/DianaMed/ring_bar_objects.xml` +- Create: `roboimi/assets/models/manipulators/DianaMed/bi_diana_ring_bar_ee.xml` +- Create or Modify: `roboimi/envs/double_air_insert_env.py` +- Modify: `tests/test_air_insert_env.py` + +- [ ] **Step 1: Write failing tests for object reset helpers and scene-specific joint naming assumptions** + +In `tests/test_air_insert_env.py`, add unit tests for helper functions that: +- write ring pose to `ring_block_joint` +- write bar pose to `bar_block_joint` +- read back task state in a stable structure + +Use fake `mj_data` objects so tests stay fast and deterministic. + +- [ ] **Step 2: Run the focused test slice and verify it fails** + +Run: +`/home/droid/.conda/envs/roboimi/bin/python -m unittest tests.test_air_insert_env -v` + +Expected: +- FAIL because reset/state helper functions and joint conventions are not implemented yet + +- [ ] **Step 3: Implement the scene XML files and reset/state helper code** + +Implement: +- the object XML with one rigid ring body and one rigid bar body +- the task scene XML entrypoint using the shared world/table/robot includes +- reset helper(s) in `double_air_insert_env.py` that set qpos for both free joints with fixed quaternions +- task-state accessor(s) returning both object poses in a stable structure + +- [ ] **Step 4: Re-run the focused test slice and verify it passes** + +Run: +`/home/droid/.conda/envs/roboimi/bin/python -m unittest tests.test_air_insert_env -v` + +Expected: +- PASS for reset/state helper tests + +- [ ] **Step 5: Commit Task 2** + +Run: +`git add roboimi/assets/models/manipulators/DianaMed/ring_bar_objects.xml roboimi/assets/models/manipulators/DianaMed/bi_diana_ring_bar_ee.xml roboimi/envs/double_air_insert_env.py tests/test_air_insert_env.py && git commit -m "feat(scene): add ring and bar insertion scene assets"` + +--- + +### Task 3: Implement strict reward and finite-geometry success detection + +**Files:** +- Modify: `roboimi/envs/double_air_insert_env.py` +- Modify: `tests/test_air_insert_env.py` + +- [ ] **Step 1: Write failing tests for reward stages and strict success detection** + +Add tests in `tests/test_air_insert_env.py` for: +- left contact stage reward +- right contact stage reward +- ring lifted off table stage +- bar lifted off table stage +- positive success case where a finite bar truly passes through the aperture +- negative case where the centerline would pass but the finite square body would clip +- negative case where the bar has not crossed the ring thickness direction enough +- negative case where one/both objects are still on the table + +Structure the tests around pure helper functions and light fake contact/state objects so the geometry logic is directly regression tested. + +- [ ] **Step 2: Run the focused tests and verify they fail for missing reward/success logic** + +Run: +`/home/droid/.conda/envs/roboimi/bin/python -m unittest tests.test_air_insert_env -v` + +Expected: +- FAIL because the staged reward and finite-geometry insertion logic are not implemented yet + +- [ ] **Step 3: Implement minimal strict success helpers and reward logic** + +Implement in `roboimi/envs/double_air_insert_env.py`: +- pure helper(s) for transforming bar geometry into ring-local coordinates +- finite-geometry insertion predicate (not centerline-only) +- table-contact / airborne checks +- staged reward function returning the highest achieved stage with `max_reward = 5` + +- [ ] **Step 4: Re-run the focused tests to verify the logic passes** + +Run: +`/home/droid/.conda/envs/roboimi/bin/python -m unittest tests.test_air_insert_env -v` + +Expected: +- PASS for reward and success-detection regression tests + +- [ ] **Step 5: Commit Task 3** + +Run: +`git add roboimi/envs/double_air_insert_env.py tests/test_air_insert_env.py && git commit -m "feat(env): add strict air insertion reward and success logic"` + +--- + +### Task 4: Add the scripted policy and integration smoke coverage + +**Files:** +- Create: `roboimi/demos/diana_air_insert_policy.py` +- Modify: `tests/test_air_insert_env.py` +- Optionally Modify: `roboimi/demos/vla_scripts/eval_vla.py` (only if integration gaps remain after Task 1) + +- [ ] **Step 1: Write failing tests for scripted-policy action shape and basic generation** + +Add tests covering: +- the new policy produces a 16D action +- trajectory generation accepts sampled ring/bar state without error +- the first action is a valid open-gripper safe pose command + +Keep the tests unit-level; do not require a full MuJoCo rollout for every assertion. + +- [ ] **Step 2: Write a small failing integration/smoke test for stepping the new task path** + +If practical with mocks/fakes, add a smoke test that verifies the policy can be used with the new environment interface without shape/dispatch mismatches. + +- [ ] **Step 3: Run the scripted-policy tests and verify they fail** + +Run: +`/home/droid/.conda/envs/roboimi/bin/python -m unittest tests.test_air_insert_env -v` + +Expected: +- FAIL because the new scripted policy does not exist yet + +- [ ] **Step 4: Implement the waypoint-based scripted policy** + +Implement a conservative open-loop policy with phases: +- safe wait pose +- above-target approach +- descend + grasp +- dual lift +- airborne meeting alignment +- bar push-through insertion + +Use fixed orientations for version 1 and follow the existing repository style from `diana_policy.py`. + +- [ ] **Step 5: Re-run the scripted-policy tests to verify they pass** + +Run: +`/home/droid/.conda/envs/roboimi/bin/python -m unittest tests.test_air_insert_env -v` + +Expected: +- PASS for scripted-policy tests + +- [ ] **Step 6: Run the combined verification suite for this feature** + +Run: +`/home/droid/.conda/envs/roboimi/bin/python -m unittest tests.test_air_insert_env tests.test_eval_vla_headless tests.test_robot_asset_paths -v` + +Expected: +- PASS with 0 failures + +- [ ] **Step 7: Commit Task 4** + +Run: +`git add roboimi/demos/diana_air_insert_policy.py tests/test_air_insert_env.py tests/test_eval_vla_headless.py tests/test_robot_asset_paths.py roboimi/demos/vla_scripts/eval_vla.py && git commit -m "feat(policy): add scripted air insertion policy"` + +--- + +### Task 5: Final verification and implementation review + +**Files:** +- Review all files touched above + +- [ ] **Step 1: Run fresh end-to-end verification before claiming completion** + +Run: +`/home/droid/.conda/envs/roboimi/bin/python -m unittest tests.test_air_insert_env tests.test_eval_vla_headless tests.test_robot_asset_paths -v` + +Expected: +- PASS with 0 failures + +- [ ] **Step 2: Inspect git status and recent commits** + +Run: +`git status --short && git log --oneline --decorate -n 8` + +Expected: +- only intended feature files modified / committed + +- [ ] **Step 3: Request final code review for the completed feature** + +Use the requesting-code-review skill against the full diff from the feature branch starting point to current HEAD. + +- [ ] **Step 4: Address any review findings and re-run verification if code changes** + +If fixes are made, repeat the unittest command from Step 1. + +- [ ] **Step 5: Hand off using finishing-a-development-branch** + +After verification and review, use the finishing-a-development-branch skill to decide merge / PR / cleanup.