Files
SkillFactory/scripts/train_loaded_stand.py
chenlin 093ab883be feat: release guided Go2 standing skill with explicit drift waiver
Add loaded-standing physics, PPO training, fixed-candidate acceptance, isolated inference, and fail-closed publication. Preserve the original 120-second drift failure and bind the operator-approved exception to the exact candidate and evidence.

Include pending Reach training/release infrastructure and archived packages. Leave pre-existing short-worker visualization changes and large generated training artifacts out of this commit.
2026-10-12 17:26:03 +08:00

421 lines
18 KiB
Python

#!/usr/bin/env python3
"""Persistent-Kit PPO for the current loaded-standing task. Training is not acceptance."""
import argparse
import json
import math
import os
import signal
import sys
import time
import traceback
from pathlib import Path
from runtime_common import critical_log_lines, write_json
def source_identity(cfg):
import hashlib
root = Path(__file__).resolve().parents[1]
files = [Path(__file__), root / "scripts/runtime_common.py", root / "scripts/supervise_sim.py"]
files += list((root / "source/SkillFactory/SkillFactory/legs").glob("*.py"))
files += list((root / "source/SkillFactory/SkillFactory/tasks/manager_based/loaded_leg").glob("*.py"))
files += list((root / "source/SkillFactory/SkillFactory/tasks/manager_based/leg_skill").glob("*.py"))
files += list((root / "source/SkillFactory/SkillFactory/tasks/manager_based/single_leg_anchor/mdp").glob("*.py"))
task = json.loads(Path(cfg["task_config"]).read_text())
files += [
Path(cfg["task_config"]).resolve(),
root / "source/SkillFactory/SkillFactory/legs/resources/ground.usda",
(root / task["support_asset"] / "support.json"),
(root / task["source_bundle"] / "manifest.json"),
]
return {str(p.relative_to(root)): hashlib.sha256(p.read_bytes()).hexdigest() for p in files}
def checked_checkpoint(args, cfg, model, optimizer, identity, update, elapsed, best, label):
from SkillFactory.legs.standing_ppo import save_checkpoint
if source_identity(cfg) != identity["sources"]:
raise RuntimeError("Training source changed while running")
if args.console_log and critical_log_lines(args.console_log):
raise RuntimeError("Critical Kit log; refusing checkpoint success")
save_checkpoint(args.output / "checkpoints" / f"{label}.pt", model, optimizer, identity, update, elapsed, best)
write_json(
args.output / "latest_checkpoint.json",
{"path": f"checkpoints/{label}.pt", "update": update, "resume_physics_exact": False, "accepted_skill": False},
)
size = sum(p.stat().st_size for p in args.output.rglob("*") if p.is_file())
if size > cfg["max_disk_bytes"]:
raise RuntimeError("Training disk budget exceeded")
def train(env, cfg, args):
import shutil
import torch
from SkillFactory.legs.standing_ppo import (
FEATURE_VERSION,
StandingActorCritic,
canonical_hash,
collect_rollout,
features,
ppo_step,
restore_checkpoint,
restore_rng,
rng_state,
selection_height_error,
)
from SkillFactory.tasks.manager_based.loaded_leg.mdp import state
torch.set_num_threads(4)
torch.set_float32_matmul_precision("highest")
torch.manual_seed(cfg["seed"])
torch.cuda.manual_seed_all(cfg["seed"])
model = StandingActorCritic(cfg["hidden"], cfg["initial_std"]).to(env.device)
optimizer = torch.optim.Adam(model.parameters(), lr=cfg["learning_rate"], eps=1e-5)
s = state(env)
if (
s.robot.is_fixed_base
or env.cfg.sim.physics.solver_type != 0
or env.cfg.sim.physics.enable_external_forces_every_iteration
):
raise RuntimeError("Require floating base, PGS and compatible external-force settings")
identity = {
"config": canonical_hash(cfg),
"sources": source_identity(cfg),
"features": FEATURE_VERSION,
"probe_episode_s": args.probe_episode_s,
"device": str(env.device),
"torch_version": str(torch.__version__),
}
root = Path(__file__).resolve().parents[1]
if not args.resume:
write_json(args.output / "config.json", cfg)
write_json(args.output / "identity.json", identity)
for path in identity["sources"]:
target = args.output / "source_snapshot" / path
target.parent.mkdir(parents=True, exist_ok=True)
shutil.copyfile(root / path, target)
elif json.loads((args.output / "identity.json").read_text()) != identity:
raise ValueError("Run config/source identity changed; cannot resume")
update, elapsed_before, best = 0, 0.0, None
if args.resume:
checkpoint = restore_checkpoint(args.resume, model, optimizer, identity)
update, elapsed_before, best = checkpoint["update"], checkpoint["elapsed_s"], checkpoint["best"]
start = time.monotonic()
def elapsed():
return elapsed_before + time.monotonic() - start
request = {"stop": False}
signal.signal(signal.SIGTERM, lambda *_: request.update(stop=True))
signal.signal(signal.SIGINT, lambda *_: request.update(stop=True))
hardware_limits = torch.tensor(
[[j["limits"]["lower"], j["limits"]["upper"]] for j in s.bundle["joints"]], device=env.device
)
speed_limits = torch.tensor([j["limits"]["velocity"] for j in s.bundle["joints"]], device=env.device)
audit = {
"samples": 0,
"finite": torch.tensor(True, device=env.device),
"maxima": torch.zeros(4, device=env.device),
"phase": "training",
"training_samples": 0,
"training_load_sign_samples": torch.zeros(12, dtype=torch.int64, device=env.device),
}
original_update = env.scene.update
def observe(dt):
original_update(dt)
if dt <= 0:
return
audit["samples"] += 1
if audit["phase"] == "training":
audit["training_samples"] += 1
audit["training_load_sign_samples"] += torch.cat(
((s.last_disturbance > 0).sum(0), (s.last_disturbance < 0).sum(0))
)
d = s.robot.data
torque = (d.applied_torque.torch[:, s.joints] / s.effort).abs().amax()
velocity = (d.joint_vel.torch[:, s.joints] / speed_limits).abs().amax()
excess = torch.maximum(hardware_limits[:, 0] - s.q(), s.q() - hardware_limits[:, 1]).clamp(min=0).amax()
audit["maxima"] = torch.maximum(
audit["maxima"], torch.stack((torque, velocity, excess, s.last_guide[:, 2].abs().amax()))
)
audit["finite"] &= (
torch.isfinite(d.root_state_w.torch).all()
& torch.isfinite(s.mount_wrench()).all()
& torch.isfinite(s.q()).all()
)
env.scene.update = observe
def audit_report():
values = audit["maxima"].cpu().tolist()
if not audit["finite"].item() or not all(math.isfinite(v) for v in values):
raise RuntimeError("Invalid physical/sensor state during training")
c = env.cfg.support_parameters
if (
max(values[:2]) > c["normalized_effort_velocity_limit"]
or values[2] > c["joint_limit_excess_rad"]
or values[3] != 0
):
raise RuntimeError(f"Training physical safety gate failed: {values}")
return {
"physics_samples": audit["samples"],
"training_physics_samples_this_process": audit["training_samples"],
"training_load_sign_env_samples_positive_then_negative": audit["training_load_sign_samples"].cpu().tolist(),
"max_torque_ratio": values[0],
"max_velocity_ratio": values[1],
"max_joint_limit_excess_rad": values[2],
"max_guide_fz_N": values[3],
"finite": True,
}
def environment_reset():
return features(env.reset()[0]["policy"])
@torch.no_grad()
def validation(hold=False):
saved_rng, distribution, episode = rng_state(), env.cfg.load_randomization, env.cfg.episode_length_s
audit["phase"] = "validation"
env.cfg.load_randomization = None
env.cfg.episode_length_s = env.cfg.support_parameters["episode_seconds"]
obs, _ = env.reset(seed=cfg["validation_seed"])
generator = torch.Generator(device=env.device).manual_seed(cfg["validation_seed"])
s.scenarios[:] = torch.arange(env.num_envs, device=env.device) % 13
for target, key in ((s.load_scale, "validation_amplitude_scale"), (s.time_shift, "validation_time_shift_s")):
low, high = cfg[key]
target[:] = low + (high - low) * torch.rand(env.num_envs, device=env.device, generator=generator)
test_inputs = {
"scenarios": s.scenarios.cpu().tolist(),
"scale": s.load_scale.cpu().tolist(),
"shift": s.time_shift.cpu().tolist(),
}
failed = torch.zeros(env.num_envs, device=env.device, dtype=torch.bool)
error_sum, contact_sum = (
torch.zeros(env.num_envs, device=env.device),
torch.zeros(env.num_envs, device=env.device),
)
peak_error, peak_tilt = torch.zeros_like(error_sum), torch.zeros_like(error_sum)
measured_steps = 0
steps = round(env.cfg.support_parameters["episode_seconds"] / env.step_dt)
x = features(obs["policy"])
for step in range(steps):
action = torch.zeros(env.num_envs, 3, device=env.device) if hold else model.act(x, deterministic=True)[0]
obs, _, terminated, truncated, info = env.step(action)
token = {k: v.clone() for k, v in obs["policy"].items()}
ids = info["terminal_ids"]
if ids is not None:
for k in token:
token[k][ids.long()] = info["terminal_observation"][k]
failed |= terminated
# Timeout at the configured final step is valid; an earlier timeout is not.
if step < steps - 1:
failed |= truncated
if (step + 1) * env.step_dt >= env.cfg.support_parameters["warmup_seconds"]:
measured_steps += 1
error = (token["base_state"][:, 2] - token["goal"][:, 0]).abs()
error = selection_height_error(error, failed)
error_sum += error
peak_error = torch.maximum(peak_error, error)
q = token["base_state"][:, 3:7] # xyzw in installed Lab
tilt = torch.acos((1 - 2 * (q[:, 0].square() + q[:, 1].square())).clamp(-1, 1))
peak_tilt = torch.maximum(peak_tilt, tilt)
total_mass = s.robot.data.body_mass.torch[0].sum()
contact_sum += ((token["foot_state"][:, 8] > 0.2 * total_mass * 9.81) & ~failed).float()
x = features(obs["policy"])
result = {
"kind": "validation_selection_only",
"hold": hold,
"seed": cfg["validation_seed"],
"update": update,
"input_hash": canonical_hash(test_inputs),
"inputs": test_inputs,
"failure_fraction": failed.float().mean().item(),
"mean_height_error_m": (error_sum / measured_steps).mean().item(),
"max_selection_height_error_m": peak_error.max().item(),
"max_tilt_rad": peak_tilt.max().item(),
"min_contact_fraction": (contact_sum / measured_steps).min().item(),
"control_samples_after_settle": measured_steps,
"physics_safety": audit_report(),
}
result["score"] = result["mean_height_error_m"] + 0.10 * result["failure_fraction"]
env.cfg.load_randomization, env.cfg.episode_length_s = distribution, episode
restore_rng(saved_rng)
audit["phase"] = "training"
return result
def checkpoint(label):
checked_checkpoint(args, cfg, model, optimizer, identity, update, elapsed(), best, label)
result = {
"passed": False,
"status": "running",
"accepted_skill": False,
"formal_training_complete": False,
"pid": os.getpid(),
"run_kind": "engineering_preflight" if args.probe_episode_s else "formal_training",
"planned_updates": cfg["updates"],
"planned_transitions": cfg["updates"] * cfg["horizon"] * cfg["num_envs"],
"identity": identity,
"resume": str(args.resume) if args.resume else None,
}
write_json(args.output / "progress.json", result)
metrics_path = args.output / "metrics.jsonl"
def log(row):
with metrics_path.open("a") as stream:
stream.write(json.dumps(row, allow_nan=False) + "\n")
try:
if not args.resume:
checkpoint("update_000000")
baseline = validation(hold=True)
write_json(args.output / "hold_validation.json", baseline)
log(baseline)
x = environment_reset()
while update < cfg["updates"]:
if (
request["stop"]
or (args.output / "STOP").exists()
or elapsed() >= cfg["max_wall_seconds"]
or (args.stop_after and update >= args.stop_after)
):
checkpoint(f"interrupted_{update:06d}")
result.update(
status="paused",
reason="stop request/update boundary/resource budget",
update=update,
elapsed_s=elapsed(),
transitions=update * cfg["horizon"] * cfg["num_envs"],
)
break
rollout_start = time.monotonic()
data, x, failures, timeouts = collect_rollout(env, model, x, cfg)
safety = audit_report()
metrics = ppo_step(model, optimizer, data, cfg)
update += 1
row = {
"kind": "train",
"update": update,
"transitions": update * cfg["horizon"] * cfg["num_envs"],
"mean_step_reward": data["reward"].mean().item(),
"fall_terminations": failures,
"timeouts": timeouts,
"update_wall_s": time.monotonic() - rollout_start,
"elapsed_s": elapsed(),
"physics_safety": safety,
**metrics,
}
log(row)
result.update(
status="running",
update=update,
elapsed_s=elapsed(),
transitions=row["transitions"],
latest=row,
best=best,
)
write_json(args.output / "progress.json", result)
print(
f"PPO {update}/{cfg['updates']} transitions={row['transitions']} "
f"reward={row['mean_step_reward']:.5f} failures={failures} wall={elapsed():.1f}s",
flush=True,
)
if update % cfg["validation_interval"] == 0:
validation_row = validation()
log(validation_row)
write_json(args.output / "validation" / f"update_{update:06d}.json", validation_row)
if best is None or validation_row["score"] < best["score"]:
best = {
"update": update,
"score": validation_row["score"],
"selection": "validation only, minimum penalized mean height error",
}
checkpoint("best_validation")
write_json(args.output / "selection.json", best)
x = environment_reset()
if update == 1 or update % cfg["checkpoint_interval"] == 0:
checkpoint(f"update_{update:06d}")
else:
checkpoint(f"update_{update:06d}")
result.update(
status="completed",
passed=True,
formal_training_complete=not bool(args.probe_episode_s),
update=update,
transitions=update * cfg["horizon"] * cfg["num_envs"],
elapsed_s=elapsed(),
best=best,
note="Training complete only; independent holdout/physics acceptance and frozen package still required",
)
except BaseException:
result.update(status="failed", passed=False, update=update, error=traceback.format_exc(), elapsed_s=elapsed())
raise
finally:
env.scene.update = original_update
write_json(args.output / "progress.json", result)
write_json(args.output / "result.json", result)
return result
def main():
from isaaclab.app import AppLauncher
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--config", type=Path, default=Path("configs/training/go2_fl_loaded_stand_ppo_v1.json"))
parser.add_argument("--output", type=Path, required=True)
parser.add_argument("--resume", type=Path)
parser.add_argument("--console-log", type=Path)
parser.add_argument("--stop-after", type=int)
parser.add_argument(
"--probe-episode-s", type=float, default=0.0, help="Engineering preflight only; changes identity"
)
AppLauncher.add_app_launcher_args(parser)
args = parser.parse_args()
if args.stop_after is not None and args.stop_after < 1 or args.probe_episode_s < 0:
parser.error("Invalid preflight/stop count")
args.output.mkdir(parents=True, exist_ok=bool(args.resume))
write_json(args.output / "result.json", {"passed": False, "status": "running", "accepted_skill": False})
app = AppLauncher(args).app
env, result, code = None, {"passed": False, "status": "failed", "accepted_skill": False}, 1
try:
import gymnasium as gym
import SkillFactory.tasks # noqa: F401
from SkillFactory.legs.standing_ppo import load_training_config
from isaaclab_tasks.utils import parse_env_cfg
cfg = load_training_config(args.config)
env_cfg = parse_env_cfg("SkillFactory-LoadedLegStand-v0", device=args.device, num_envs=cfg["num_envs"])
if env_cfg.support_config != cfg["task_config"]:
raise ValueError("Task configuration binding mismatch")
env_cfg.seed = cfg["seed"]
env_cfg.load_randomization = {
"amplitude_scale": cfg["train_amplitude_scale"],
"time_shift_s": cfg["train_time_shift_s"],
}
if args.probe_episode_s:
env_cfg.episode_length_s = args.probe_episode_s
env = gym.make("SkillFactory-LoadedLegStand-v0", cfg=env_cfg).unwrapped
result = train(env, cfg, args)
code = 0 if result["status"] in ("completed", "paused") else 1
except BaseException:
result.update(passed=False, status="failed", error=traceback.format_exc())
print(result["error"], file=sys.stderr)
finally:
if env is not None:
try:
env.close()
except BaseException:
result.update(passed=False, status="failed", shutdown_error=traceback.format_exc())
code = 1
write_json(args.output / "result.json", result)
# Explicitly preserve paused != completed; supervisor only attests process/log cleanup.
app.close(exit_code=code)
if __name__ == "__main__":
main()