Files
superspider-rl/scripts/superspider_stair_test.py
T

209 lines
9.9 KiB
Python

"""Evaluate single-step climbing with sticky failure and reproducible raw records.
Success requires crossing the step face by 5 cm, gaining >85% of step height,
and tilt <=30 degrees for >80% of the second half AND on the final sample.
Any termination, reset or nonfinite action/state fails that trial permanently.
Flat ground uses a separate locomotion control criterion, not a climb rate.
"""
from __future__ import annotations
import argparse
from dataclasses import asdict, dataclass, replace
from datetime import datetime, timezone
import hashlib
import importlib.metadata
import json
from pathlib import Path
import time
import mujoco
import numpy as np
import torch
import mjlab.tasks
import src.tasks
from mjlab.envs import ManagerBasedRlEnv
from mjlab.rl import MjlabOnPolicyRunner, RslRlVecEnvWrapper
from mjlab.tasks.registry import load_env_cfg, load_rl_cfg, load_runner_cls
from mjlab.terrains.terrain_generator import SubTerrainCfg, TerrainGeometry, TerrainOutput
from mjlab.terrains.utils import make_plane
from mjlab.utils.os import dump_yaml
from src.policy import load_policy
from src.resources import ROOT, checkpoint_path
from src.evaluation import summarize_trials
APPROACH = 1.5
PROTOCOL = "single_step_v2"
@dataclass(kw_only=True)
class SingleStepTerrainCfg(SubTerrainCfg):
"""平地 + 一级台阶。台阶从 x=APPROACH 起一直延伸到地块边缘。"""
step_height: float = 0.05
def function(self, difficulty, spec, rng) -> TerrainOutput:
del difficulty, rng
body = spec.body("terrain")
sx, sy = self.size
geoms = make_plane(body, self.size, 0.0, center_zero=False)
colors = [(0.45, 0.45, 0.48, 1.0)]
# 出生点放在地块靠 -x 的一侧,留 APPROACH 的助跑距离。
ox = sx / 2.0 - APPROACH
step_len = sx - ox - APPROACH # 台阶从 ox+APPROACH 延伸到地块末端
if self.step_height > 1e-6 and step_len > 0.1:
g = body.add_geom(
type=mujoco.mjtGeom.mjGEOM_BOX,
size=(step_len / 2.0, sy / 2.0, self.step_height / 2.0),
pos=(ox + APPROACH + step_len / 2.0, sy / 2.0, self.step_height / 2.0),
)
geoms.append(g)
colors.append((0.85, 0.55, 0.25, 1.0))
return TerrainOutput(
origin=np.array((ox, sy / 2.0, 0.0)),
geometries=[TerrainGeometry(geom=g, color=c) for g, c in zip(geoms, colors)],
)
def run(task, ckpt, step_h, output, n=24, secs=20.0, device="cpu", seed=42):
cfg = load_env_cfg(task, play=True)
cfg.seed = seed
cfg.scene.num_envs = n
cfg.scene.terrain.terrain_generator = replace(
cfg.scene.terrain.terrain_generator,
sub_terrains={"single_step": SingleStepTerrainCfg(size=(8.0, 8.0), step_height=step_h)},
curriculum=False, num_rows=2, num_cols=2,
)
cfg.scene.terrain.max_init_terrain_level = 0
cfg.curriculum.pop("terrain_levels", None)
cfg.curriculum.pop("command_vel", None)
for event in ("foot_friction", "base_com", "encoder_bias", "push_robot"):
cfg.events.pop(event, None)
cfg.events["reset_base"].params["pose_range"] = {"x": (0, 0), "y": (0, 0), "yaw": (0, 0)}
tw = cfg.commands["twist"]
tw.heading_command = False
tw.ranges.heading = None
tw.resampling_time_range = (1e9, 1e9)
tw.rel_standing_envs = 0.0
tw.rel_heading_envs = 0.0
# Matches the archived test: touching the riser is allowed, falling is not.
cfg.terminations.pop("illegal_contact", None)
cfg.episode_length_s = secs + 1.0
output.mkdir(parents=True, exist_ok=False)
dump_yaml(output / "env.yaml", asdict(cfg))
agent = load_rl_cfg(task)
agent.seed = seed
dump_yaml(output / "agent.yaml", asdict(agent))
env = ManagerBasedRlEnv(cfg=cfg, device=device)
try:
wrapped = RslRlVecEnvWrapper(env, clip_actions=agent.clip_actions)
runner = (load_runner_cls(task) or MjlabOnPolicyRunner)(wrapped, asdict(agent), device=device)
policy = load_policy(runner, ckpt, device)
robot = env.scene["robot"]
term = env.command_manager.get_term("twist")
obs, _ = wrapped.reset()
initial = robot.data.root_link_pos_w.detach().cpu().numpy().copy()
steps = round(secs / env.step_dt)
positions = np.empty((steps, n, 3), dtype=np.float32)
tilt = np.empty((steps, n), dtype=np.float32)
dones = np.zeros((steps, n), dtype=bool)
invalid = np.zeros((steps, n), dtype=bool)
term_names = list(env.termination_manager.active_terms)
termination_bits = np.zeros((steps, n, len(term_names)), dtype=bool)
for i in range(steps):
term.is_standing_env[:] = False
term.is_heading_env[:] = False
term.vel_command_b[:] = torch.tensor([0.30, 0.0, 0.0], device=env.device)
# The observation contains the command: refresh it before inference.
obs = wrapped.get_observations()
with torch.inference_mode():
actions = policy(obs)
finite = torch.isfinite(actions).all(dim=-1)
actions = torch.where(finite[:, None], actions, torch.zeros_like(actions))
obs, _, done, _ = wrapped.step(actions)
positions[i] = robot.data.root_link_pos_w.detach().cpu().numpy()
q = robot.data.root_link_quat_w
# MuJoCo quaternion order w,x,y,z; Rzz = 1 - 2*(x*x+y*y).
tilt[i] = torch.rad2deg(torch.acos((1 - 2*(q[:, 1]**2 + q[:, 2]**2)).clamp(-1, 1))).cpu().numpy()
dones[i] = done.bool().cpu().numpy()
invalid[i] = ~finite.cpu().numpy()
for j, name in enumerate(term_names):
termination_bits[i, :, j] = env.termination_manager.get_term(name).cpu().numpy()
raw = output / "rollout.npz"
np.savez_compressed(raw, time_s=np.arange(1, steps+1)*env.step_dt,
initial_xyz=initial, root_xyz=positions, tilt_deg=tilt,
done=dones, invalid_action=invalid,
termination_bits=termination_bits, termination_names=np.array(term_names))
result = summarize_trials(initial, positions, tilt, dones, invalid, step_h, APPROACH)
result.update(protocol=PROTOCOL, task=task, checkpoint=ckpt.name,
checkpoint_sha256=sha256(ckpt), seed=seed, device=device,
num_envs=n, seconds=secs, step_dt=env.step_dt,
command=[0.30, 0.0, 0.0], step_height_m=step_h,
raw_file=raw.name, raw_sha256=sha256(raw))
(output / "result.json").write_text(json.dumps(result, indent=2, allow_nan=False)+"\n")
return result
finally:
env.close()
def sha256(path):
return hashlib.sha256(Path(path).read_bytes()).hexdigest()
def main():
p = argparse.ArgumentParser(description=__doc__)
p.add_argument("checkpoint", nargs="?", help="One checkpoint; defaults to both student snapshots")
p.add_argument("--task", default="Custom-Superspider-Stairs-Distill")
p.add_argument("--heights", default="0,0.02,0.03,0.04,0.05,0.06,0.07,0.08,0.09,0.10")
p.add_argument("--num-envs", type=int, default=24)
p.add_argument("--seconds", type=float, default=20.0)
p.add_argument("--seed", type=int, default=42)
p.add_argument("--device", default="auto", help="auto, cpu or cuda:0")
p.add_argument("--threads", type=int, default=4)
p.add_argument("--output", type=Path, default=Path("evaluation-output") / datetime.now().strftime("%Y%m%d_%H%M%S"))
a = p.parse_args()
heights = [float(x) for x in a.heights.split(",")]
if a.num_envs < 1 or a.seconds <= 0 or a.threads < 1 or not heights or any(not np.isfinite(h) or h < 0 for h in heights) or len(set(heights)) != len(heights):
p.error("Require positive env count/duration/threads and unique finite nonnegative heights")
if not np.isfinite(a.seconds) or a.seconds < 0.04:
p.error("Duration must be finite and at least 0.04 s")
device = ("cuda:0" if torch.cuda.is_available() else "cpu") if a.device == "auto" else a.device
torch.set_num_threads(a.threads)
checkpoints = [checkpoint_path(a.checkpoint)] if a.checkpoint else [checkpoint_path("student_1600.pt"), checkpoint_path("student_5999.pt")]
a.output.mkdir(parents=True, exist_ok=False)
sources = [*ROOT.glob("src/**/*.py"), *ROOT.glob("scripts/*.py"),
*ROOT.glob("models/**/*.xml"), *ROOT.glob("models/**/*.urdf"), *ROOT.glob("models/**/*.STL")]
manifest = {str(x.relative_to(ROOT)): sha256(x) for x in sorted(sources)}
(a.output / "source_manifest.json").write_text(json.dumps(manifest, indent=2)+"\n")
packages = ["superspider-rl", "torch", "numpy", "mujoco", "mujoco-warp", "mjlab", "rsl-rl-lib", "warp-lang"]
versions = {}
for name in packages:
try: versions[name] = importlib.metadata.version(name)
except importlib.metadata.PackageNotFoundError: versions[name] = "source checkout"
report = {"protocol": PROTOCOL, "started_at_utc": datetime.now(timezone.utc).isoformat(),
"completed": False, "device": device, "versions": versions, "results": []}
report_path = a.output / "summary.json"
def save(): report_path.write_text(json.dumps(report, indent=2, allow_nan=False)+"\n")
save()
started = time.monotonic()
try:
for ckpt in checkpoints:
for index, h in enumerate(heights):
folder = a.output / ckpt.stem / f"{index:02d}_{h:.4f}m"
result = run(a.task, ckpt, h, folder, a.num_envs, a.seconds, device, a.seed)
result["directory"] = str(folder.relative_to(a.output))
report["results"].append(result)
save()
print(f"EVAL {ckpt.name} {h*100:.1f}cm: {result['success_count']}/{a.num_envs}, failed={result['failed_count']}", flush=True)
report["completed"] = True
except BaseException as exc:
report["error"] = repr(exc)
raise
finally:
report["elapsed_s"] = time.monotonic()-started
report["finished_at_utc"] = datetime.now(timezone.utc).isoformat()
save()
if __name__ == "__main__":
main()