PWM / PWM_code /policy_rl /render_action_condition.py
hchen657Naza's picture
Add files using upload-large-folder tool
dd253d4 verified
Raw History Blame Contribute Delete
16.2 kB
"""
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:
<output_dir>/<task>_<seq>_init<i>/sample<k>/
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()