scripts/README_yam_motionplanner.md

# YAM Grape → Box: PRM Motion Generation

Single-arm **pick-and-place** on the YAM bimanual robot (`Template-YAM-Play-v0`):
the right arm plans a collision-free approach with a **task-space PRM**, performs a
**real friction grasp** (no kinematic attach), lifts, carries, and drops the object into a
box built from primitives. Includes a scriptable **N-episode benchmark** with success stats.

All paths below are **relative to the repo root**. Run commands from there.

## 0. Quickstart — from scratch
```bash
# 1) clone the repo and check out THIS branch
#    (it is based on yizhou/yam_rebase, so the branch already contains everything you need)
git clone https://gitlab-master.nvidia.com/dir/cosmos-sdg/vfm-dexmimicgen.git
cd vfm-dexmimicgen
git checkout yu/motionplanner_yam

# 2) activate the Isaac Lab env (Isaac Lab release 3.0.0-beta2 must already be installed;
#    see the top-level README "Installation"), then install this repo + video deps
source "$HOME/miniconda3/etc/profile.d/conda.sh"; conda activate env_isaaclab
python -m pip install -e .
pip install imageio imageio-ffmpeg pillow

# 3) link the object assets (provides grape.usd / apple.usd / table)
ln -s /path/to/dexmimicgen_custom_assets ./custom_assets

# 4) run one grape -> box episode
export OMNI_KIT_ACCEPT_EULA=YES OMNI_KIT_ALLOW_ROOT=1 PYTHONUNBUFFERED=1 CUDA_HOME=$CONDA_PREFIX
python scripts/yam_grasp_prm.py --headless --obj grape --basket \
  --obj_xy=0.00,0.10 --box_xy=0.06,-0.26 --episode 0 --video outputs/ep0.mp4
# -> outputs/ep0.mp4  (labelled video: APPROACH/DESCEND/GRASP/LIFT/CARRY/RELEASE + SUCCESS/FAIL)
```
That's the whole loop. The 10-episode benchmark is §4. Details/flags/output format below.

## 1. Pipeline
1. **T (task):** which object / where — nearest-arm or LLM (`scripts/vlm_allocate.py`, needs `HF_TOKEN`).
2. **M (motion):** task-space PRM — sample → k-NN graph → Dijkstra → shortcut → resample (`scripts/yam_prm.py`).
3. **Execute:** absolute differential-IK (DLS) tracks the planned polyline; grasp = close-until-contact + lift by friction.
4. **Overlay:** every frame is annotated with the current action, target, EEF, error, and SUCCESS/FAIL.

## 2. Prerequisites (what you need *beyond* this repo)
This repo ships the **environment code** (robot, scene, task, planner). Three things are
**external** and are not — and should not be — committed here:

1. **Isaac Sim + Isaac Lab** `release 3.0.0-beta2` — the simulator platform. Install per the
   top-level `README.md`, then `python -m pip install -e .`. (Multi-GB, licensed; treat like CUDA.)
2. **`custom_assets`** — object/table USDs (incl. `grape.usd`, `apple.usd`), symlinked in:
   `ln -s /path/to/dexmimicgen_custom_assets ./custom_assets`. The **box is primitive** (no asset),
   but the **grape/apple tasks need these USDs**.
3. **Python video deps** (once, into `env_isaaclab`): `pip install imageio imageio-ffmpeg pillow`
   (for mp4 export + the per-frame overlay).

Then, each shell:
```bash
source "$HOME/miniconda3/etc/profile.d/conda.sh"   # adjust to your conda
conda activate env_isaaclab
export OMNI_KIT_ACCEPT_EULA=YES OMNI_KIT_ALLOW_ROOT=1 PYTHONUNBUFFERED=1 CUDA_HOME=$CONDA_PREFIX
```

Scene/robot config lives in `source/bimanual/tasks/manager_based/yam/`:
- `yam.py` — home/init pose `joints=[-1,94,85,-84,-5,0]°`; gripper actuator (damping 60, effort 40) so the jaw clamps.
- `yam_bimanual_env.py` — objects (grape, cans, apple) and their positions/scales.

The box is **not** an asset — it is built at runtime from Cuboids (license-clean), positioned by `--box_xy`.

## 3. Run one episode (grape → box)
```bash
python scripts/yam_grasp_prm.py --headless --obj grape --basket \
  --obj_xy=0.00,0.10 --box_xy=0.06,-0.26 --episode 0 \
  --video outputs/ep0.mp4
```
Key flags:
| flag | meaning |
|------|---------|
| `--obj`    | object name in the env (`grape`, `can`, `can2`, `apple`) |
| `--basket` | build a primitive box and place the object into it |
| `--obj_xy` | env-local `x,y` (m) to place the object; use `--obj_xy=-0.03,0.10` for negatives |
| `--box_xy` | env-local `x,y` (m) of the box |
| `--episode`| index shown in the overlay |
| `--video`  | output mp4 path (dirs auto-created) |

Reachability: grape grasp is reliable near `x∈[-0.05,0.02], y∈[0.08,0.10]` (base-relative distance ≲ 0.37 m); farther spots fail on purpose and show up in the stats.

## 4. Run the N-episode benchmark + stitch
```bash
OUT=outputs/grape_eps bash scripts/run_grape_episodes.sh      # runs the episode list, ~52 s each
OUT=outputs/grape_eps python scripts/concat_episodes.py       # -> outputs/grape_eps/episodes.mp4
```
Edit the `GX` / `BX` arrays in `run_grape_episodes.sh` to change positions or episode count.

## 5. Output format
Written under the `--video` dir (or `$OUT`):
- `epN.mp4` — H.264, 720×544, 6 fps. Top-left overlay per frame: `EPISODE`, `RESULT`, `ACTION`
  (1.APPROACH → 2.PRM APPROACH → 3.DESCEND → 4.CLOSE-GRASP → 5.LIFT → 6.CARRY → 7.LOWER → 8.RELEASE → 9.RETREAT),
  `gripper`, `target(root)`, `eef(root)`, `err(m)`.
- `epN_pose.json` — `{home, steps:[{phase, joints[6], gripper, eef[3], apple_z}], prm_path_world, obstacles}`.
- `ep_results.txt` — one line per episode: `EP<i> grape=<x,y> box=<x,y> :: ... EPISODE_RESULT: SUCCESS|FAIL ...`.
- `episodes.mp4` — all episodes stitched with per-episode title cards + a final `SUCCESS RATE: k / N` card.

Success = object ends inside the box footprint (`|Δx|,|Δy| < 0.11 m`, resting height).

## 6. Reference numbers
- **~52 s / episode** (each is a fresh Isaac Sim launch); PRM solve itself is < 1 s.
- Example 10-episode run: **6 / 10 success (60%)**. Failures are grasp misses at the reach edge
  (diff-IK descend steady-state error ~4–7 cm in z); placement rarely fails once grasped (box-reach err < 0.03 m).

## 7. Other tasks (same engine)
```bash
python scripts/yam_grasp_prm.py --headless --obj can  --basket --video outputs/can_box.mp4     # can -> box
python scripts/yam_dualarm.py   --headless --video outputs/dualarm.mp4                          # two arms in parallel
python scripts/yam_longhorizon.py --headless --objects can,can2,grape --video outputs/lh.mp4    # multi-object into one box
python scripts/vlm_allocate.py                                                                  # LLM task allocation (needs HF_TOKEN)
```