""" Render hand-only condition frames from a Diffusion Policy action chunk. Pipeline: 1. Boot IsaacSim env (Humanoid-Open-Drawer-v0) 2. For each (room/table/seq_name/randomize_idx) init config: a. Reset env, run 100 init-pose warmup steps -> reach starting hand pose b. Get init obs: fixed_rgb (1280x720) + robot_qpos (50,) c. For each of K stochastic DP samples: - Restore env to same init (re-reset + re-warmup) [simple but slow] - Build DP obs (image+proprio history of n_obs_steps) - DP.predict_action -> 16 actions of dim 38 - Replay each action: ik_step + env.step + render_hand_frame - Save: init_frame.png (RGB, full-res) hand_frames.mp4 (16 hand-only RGB frames, white bg) actions.npz (the 16 actions) Output layout: /__init/sample/ init_frame.png hand_frames.mp4 actions.npz """ import argparse import json import os import pickle import random import sys from collections import deque from pathlib import Path sys.path.insert(0, "/home/hongyi/PWM/EgoVLA_Release") sys.path.insert(0, "/home/hongyi/PWM/external/diffusion_policy") from omni.isaac.lab.app import AppLauncher parser = argparse.ArgumentParser() parser.add_argument("--task", type=str, default="Humanoid-Open-Drawer-v0") parser.add_argument("--ckpt", type=str, default=None, help="DP checkpoint. Required unless --replay_actions is set.") parser.add_argument("--replay_actions", type=str, default=None, help="Debug: path to a collected actions.npz; skip DP and replay these actions instead.") parser.add_argument("--replay_seq_name", type=str, default=None, help="Debug: when --replay_actions is set, override seq_name (e.g. 'episode_0.hdf5').") parser.add_argument("--replay_rand_idx", type=int, default=None, help="Debug: when --replay_actions is set, override rand_idx (raw, post-shuffle).") parser.add_argument("--num_init_states", type=int, default=4, help="Number of distinct env init configurations.") parser.add_argument("--n_chunks", type=int, default=4, help="Number of stochastic DP samples per init state.") parser.add_argument("--horizon", type=int, default=16, help="Number of future actions per chunk (must match DP horizon).") parser.add_argument("--output_dir", type=str, default="/home/hongyi/PWM/policy_rl/action_conditions") parser.add_argument("--rand_offset", type=int, default=2000, help="Offset into shuffled randomize_idxes (avoid collection seeds 200-1000).") parser.add_argument("--img_size", type=int, default=96, help="DP image obs size (must match training).") AppLauncher.add_app_launcher_args(parser) args_cli = parser.parse_args() args_cli.headless = True args_cli.enable_cameras = True app_launcher = AppLauncher(args_cli) simulation_app = app_launcher.app import cv2 import numpy as np import torch import gymnasium as gym import dill import humanoid.tasks # noqa import omni.isaac.lab_tasks # noqa import omni.usd from pxr import UsdGeom from omni.isaac.lab_tasks.utils import parse_env_cfg from omni.isaac.lab.controllers import DifferentialIKController, DifferentialIKControllerCfg from humanoid.tasks.base_env import BaseEnv from human_plan.ego_bench_eval.utils import ik_step, TASK_INIT_EPISODE from diffusion_policy.workspace.train_diffusion_unet_hybrid_workspace import TrainDiffusionUnetHybridWorkspace seed_map = { "Humanoid-Open-Drawer-v0": 6, "Humanoid-Close-Drawer-v0": 7, "Humanoid-Open-Laptop-v0": 8, "Humanoid-Stack-Can-v0": 9, "Humanoid-Push-Box-v0": 10, "Humanoid-Flip-Mug-v0": 11, "Humanoid-Sort-Cans-v0": 12, "Humanoid-Pour-Balls-v0": 13, "Humanoid-Insert-Cans-v0": 14, "Humanoid-Unload-Cans-v0": 15, "Humanoid-Stack-Can-Into-Drawer-v0": 16, } # Same prim-hiding logic as collect_50_robot.py _HAND_LINK_PREFIXES = [ "L_hand_base_link", "L_index_", "L_middle_", "L_pinky_", "L_ring_", "L_thumb_", "R_hand_base_link", "R_index_", "R_middle_", "R_pinky_", "R_ring_", "R_thumb_", ] def _is_hand_prim(name): return any(name.startswith(p) for p in _HAND_LINK_PREFIXES) def _collect_hide_paths(stage): from pxr import Usd # noqa hide_paths = [] env_prim = stage.GetPrimAtPath("/World/envs/env_0") if env_prim.IsValid(): for child in env_prim.GetChildren(): if child.GetName() != "Robot": hide_paths.append(str(child.GetPath())) for p in ["/World/ground", "/World/Light", "/Visuals"]: prim = stage.GetPrimAtPath(p) if prim.IsValid(): hide_paths.append(p) robot_prim = stage.GetPrimAtPath("/World/envs/env_0/Robot") if robot_prim.IsValid(): for child in robot_prim.GetChildren(): name = child.GetName() if child.GetTypeName() in ("PhysicsRevoluteJoint", "PhysicsFixedJoint", "Material", "Scope"): continue if not _is_hand_prim(name): hide_paths.append(str(child.GetPath())) return hide_paths def _toggle_vis(stage, paths, visible): for p in paths: prim = stage.GetPrimAtPath(p) if prim.IsValid(): img = UsdGeom.Imageable(prim) if img: img.MakeVisible() if visible else img.MakeInvisible() def render_hand_frame(env, stage, hide_paths): _toggle_vis(stage, hide_paths, False) env.sim.render() env.scene.sensors["fixed_camera"].update(dt=env.sim.get_physics_dt()) rgb = env.scene.sensors["fixed_camera"].data.output["rgb"][0].cpu().numpy()[:, :, :3].copy() bg = np.all(rgb < 15, axis=2) rgb[bg] = 255 _toggle_vis(stage, hide_paths, True) env.sim.render() return rgb def load_dp_policy(ckpt_path: str, device: str = "cuda:0"): payload = torch.load(open(ckpt_path, "rb"), pickle_module=dill, map_location="cpu") cfg = payload["cfg"] ws = TrainDiffusionUnetHybridWorkspace(cfg) ws.load_payload(payload, exclude_keys=None, include_keys=None) policy = ws.ema_model if cfg.training.use_ema else ws.model policy.to(device) policy.eval() return policy, cfg def prep_dp_obs(rgb, qpos, n_obs_steps, img_size, device): """Build DP obs_dict from a single timestep, repeating to fill n_obs_steps history.""" img = cv2.resize(rgb, (img_size, img_size), interpolation=cv2.INTER_AREA) img = np.transpose(img, (2, 0, 1)).astype(np.float32) / 255.0 img_hist = np.repeat(img[None], n_obs_steps, axis=0) # (To, 3, H, W) qpos_hist = np.repeat(qpos[None].astype(np.float32), n_obs_steps, axis=0) return { "image": torch.from_numpy(img_hist).unsqueeze(0).to(device), "proprio": torch.from_numpy(qpos_hist).unsqueeze(0).to(device), } def split_action_38d(a): return a[:7], a[7:14], a[14:26], a[26:38] def warmup_to_init(env, ik_left, ik_right, lw, rw, lr_, rr_, action_buf, init_poses, task_name, seq_name, n_steps=100): ld = init_poses[task_name][seq_name][0]["left_dof"] rd = init_poses[task_name][seq_name][0]["right_dof"] le = init_poses[task_name][seq_name][0]["left_ee"] re = init_poses[task_name][seq_name][0]["right_ee"] env_results = None for _ in range(n_steps): ik_step(env, ik_left, ik_right, lw, rw, lr_, rr_, le, re, ld, rd, action_buf) env_results = env.step(action_buf) return env_results def main(): Path(args_cli.output_dir).mkdir(parents=True, exist_ok=True) replay_mode = args_cli.replay_actions is not None if replay_mode: print(f"REPLAY MODE: actions from {args_cli.replay_actions}", flush=True) rd = np.load(args_cli.replay_actions) # Build (T, 38) using same convention as convert_to_zarr.py le_full = rd["left_ee_pose"][:, 0, :].astype(np.float32) # (T, 7) re_full = rd["right_ee_pose"][:, 0, :].astype(np.float32) # (T, 7) lq_full = rd["left_qpos"].astype(np.float32) # (T, 12) rq_full = rd["right_qpos"].astype(np.float32) # (T, 12) replay_actions = np.concatenate([le_full, re_full, lq_full, rq_full], axis=-1) # (T, 38) replay_actions = replay_actions[: args_cli.horizon] print(f" Loaded {replay_actions.shape[0]} actions of dim {replay_actions.shape[1]}", flush=True) policy, cfg = None, None n_obs_steps = 1 else: assert args_cli.ckpt is not None, "Provide --ckpt or --replay_actions." print(f"Loading DP from: {args_cli.ckpt}", flush=True) policy, cfg = load_dp_policy(args_cli.ckpt, device="cuda:0") n_obs_steps = cfg.n_obs_steps print(f"DP horizon={cfg.horizon}, n_obs_steps={n_obs_steps}, n_action_steps={cfg.n_action_steps}", flush=True) assert args_cli.horizon <= cfg.horizon, \ f"--horizon ({args_cli.horizon}) cannot exceed DP's training horizon ({cfg.horizon})" task = args_cli.task task_name = task[9:-3] random.seed(seed_map[task]) rand_idxes = list(range(10000)) random.shuffle(rand_idxes) with open("/home/hongyi/PWM/EgoVLA_Release/init_poses_fixed_set_100traj.pkl", "rb") as f: init_poses = pickle.load(f) episode_list = TASK_INIT_EPISODE[task_name] # Env (room=1, table=2 fixed, randomize_idx varies per init) env_cfg = parse_env_cfg(task, num_envs=1) env_cfg.episode_length_s = 60 env_cfg.randomize = True env_cfg.spawn_background = True env_cfg.room_idx = 1 env_cfg.table_idx = 2 env: BaseEnv = gym.make(task, cfg=env_cfg) env.cfg.randomize_idx = rand_idxes[args_cli.rand_offset] env.reset() stage = omni.usd.get_context().get_stage() hide_paths = _collect_hide_paths(stage) ik_left = DifferentialIKController( DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls"), num_envs=1, device=env.sim.device, ) ik_right = DifferentialIKController( DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="pinv"), num_envs=1, device=env.sim.device, ) lw = torch.zeros(1, ik_left.action_dim, device=env.robot.device) lr_ = torch.zeros(1, ik_left.action_dim, device=env.robot.device) rw = torch.zeros(1, ik_right.action_dim, device=env.robot.device) rr_ = torch.zeros(1, ik_right.action_dim, device=env.robot.device) action_buf = torch.zeros((1, env.num_actions), device=env.robot.device) manifest = [] for init_i in range(args_cli.num_init_states): if replay_mode and args_cli.replay_seq_name is not None: seq_name = args_cli.replay_seq_name else: seq_name = episode_list[init_i % len(episode_list)][0] if replay_mode and args_cli.replay_rand_idx is not None: rand_idx = args_cli.replay_rand_idx else: rand_i = args_cli.rand_offset + init_i rand_idx = rand_idxes[rand_i] env.cfg.randomize_idx = rand_idx ik_left.reset(); ik_right.reset() env_results = warmup_to_init(env, ik_left, ik_right, lw, rw, lr_, rr_, action_buf, init_poses, task_name, seq_name) init_rgb = env_results[0]["fixed_rgb"][0].cpu().numpy() # (720,1280,3) uint8 RGB init_qpos = env_results[0]["qpos"][0].cpu().numpy().astype(np.float32) init_dir_base = Path(args_cli.output_dir) / f"{task_name}_{seq_name.replace('.hdf5','')}_init{init_i:03d}" init_dir_base.mkdir(parents=True, exist_ok=True) cv2.imwrite(str(init_dir_base / "init_frame.png"), init_rgb[:, :, ::-1]) for k in range(args_cli.n_chunks): # Re-reset env to same state (simple way to get a fresh start each sample) if k > 0: env.cfg.randomize_idx = rand_idx ik_left.reset(); ik_right.reset() env_results = warmup_to_init(env, ik_left, ik_right, lw, rw, lr_, rr_, action_buf, init_poses, task_name, seq_name) if replay_mode: actions = replay_actions else: # Build obs from current frame (use init frame after warmup) obs_dict = prep_dp_obs(init_rgb, init_qpos, n_obs_steps, args_cli.img_size, device="cuda:0") # Use action_pred (full horizon=16) instead of action (n_action_steps=8 slice). with torch.inference_mode(): out = policy.predict_action(obs_dict) actions = out["action_pred"][0].cpu().numpy() # (horizon, 38) actions = actions[: args_cli.horizon] sample_dir = init_dir_base / f"sample{k:02d}" sample_dir.mkdir(parents=True, exist_ok=True) np.savez(sample_dir / "actions.npz", actions=actions.astype(np.float32), init_qpos=init_qpos) # Replay actions, recording hand-only frames AND full obs for DP retraining. hand_frames = [] full_frames = [] qpos_traj = [] for a in actions: a_le, a_re, a_lh, a_rh = split_action_38d(a) ik_step(env, ik_left, ik_right, lw, rw, lr_, rr_, a_le, a_re, a_lh, a_rh, action_buf) env_results = env.step(action_buf) full_rgb = env_results[0]["fixed_rgb"][0].cpu().numpy() # RGB (720,1280,3) full_frames.append(full_rgb[:, :, ::-1].copy()) # BGR for VideoWriter qpos_traj.append(env_results[0]["qpos"][0].cpu().numpy().astype(np.float32)) hf = render_hand_frame(env, stage, hide_paths) # (720,1280,3) RGB hand_frames.append(hf[:, :, ::-1].copy()) # write BGR # Write hand-only condition video (for WM) h, w = hand_frames[0].shape[:2] vp = sample_dir / "hand_frames.mp4" vw = cv2.VideoWriter(str(vp), cv2.VideoWriter_fourcc(*"mp4v"), 15, (w, h)) for f in hand_frames: vw.write(f) vw.release() # Write full-obs video (for retraining DP on imagined rollouts) fv = sample_dir / "full_frames.mp4" fw = cv2.VideoWriter(str(fv), cv2.VideoWriter_fourcc(*"mp4v"), 15, (w, h)) for f in full_frames: fw.write(f) fw.release() # Save qpos trajectory (pre-step qpos is init_qpos, then qpos_traj[0..15] are post-step) np.savez( sample_dir / "qpos_traj.npz", qpos_traj=np.stack(qpos_traj, axis=0).astype(np.float32), init_qpos=init_qpos, ) manifest.append({ "init_state": str(init_dir_base.relative_to(args_cli.output_dir)), "sample": k, "init_frame": str((init_dir_base / "init_frame.png").relative_to(args_cli.output_dir)), "hand_frames": str(vp.relative_to(args_cli.output_dir)), "full_frames": str(fv.relative_to(args_cli.output_dir)), "qpos_traj": str((sample_dir / "qpos_traj.npz").relative_to(args_cli.output_dir)), "actions": str((sample_dir / "actions.npz").relative_to(args_cli.output_dir)), "horizon": int(args_cli.horizon), "task_name": task_name, "seq_name": seq_name, "rand_idx": int(rand_idx), }) print(f"[init {init_i+1}/{args_cli.num_init_states}, sample {k+1}/{args_cli.n_chunks}] " f"saved {len(hand_frames)} hand frames -> {vp}", flush=True) with open(Path(args_cli.output_dir) / "manifest.json", "w") as f: json.dump(manifest, f, indent=2) print(f"\nDone. {len(manifest)} (init, sample) rollouts. Manifest: {args_cli.output_dir}/manifest.json", flush=True) env.close() if __name__ == "__main__": main() simulation_app.close()