093ab883be
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.
421 lines
18 KiB
Python
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()
|