209 lines
9.9 KiB
Python
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()
|