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.
This commit is contained in:
@@ -0,0 +1,270 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Locked standing candidate physics trials; never optimizes or selects weights."""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import os
|
||||
import shutil
|
||||
import subprocess
|
||||
import sys
|
||||
import traceback
|
||||
from pathlib import Path
|
||||
|
||||
from runtime_common import finish_run, write_json
|
||||
|
||||
|
||||
def reset_probes(env, s):
|
||||
import torch
|
||||
|
||||
before, q = s.robot.data.root_state_w.torch.clone(), s.q().clone()
|
||||
ids = torch.arange(0, env.num_envs, 2, device=env.device)
|
||||
others = torch.arange(1, env.num_envs, 2, device=env.device)
|
||||
env.reset(env_ids=ids)
|
||||
indexed = bool(
|
||||
(s.mount_wrench()[ids] == 0).all()
|
||||
and (s.last_guide[ids] == 0).all()
|
||||
and (env.action_manager.action[ids] == 0).all()
|
||||
and torch.equal(before[others], s.robot.data.root_state_w.torch[others])
|
||||
and torch.equal(q[others], s.q()[others])
|
||||
)
|
||||
snapshot = {k: v.clone() for k, v in s.snapshot().items()}
|
||||
env.reset(env_ids=[])
|
||||
indexed &= all(torch.equal(v, s.snapshot()[k]) for k, v in snapshot.items())
|
||||
env.cfg.episode_length_s = 3 * env.step_dt
|
||||
env.reset()
|
||||
action = torch.full((env.num_envs, 3), 0.2, device=env.device)
|
||||
for _ in range(3):
|
||||
obs, _, term, timeout, info = env.step(action)
|
||||
timeout_ok = bool(
|
||||
not term.any()
|
||||
and timeout.all()
|
||||
and len(info["terminal_ids"]) == env.num_envs
|
||||
and torch.equal(info["terminal_observation"]["joint_state"][:, :, 2], action)
|
||||
and (obs["policy"]["joint_state"][:, :, 2] == 0).all()
|
||||
and (obs["policy"]["mount_wrench"] == 0).all()
|
||||
)
|
||||
before = env.action_manager.action.clone(), env.action_manager.prev_action.clone()
|
||||
rejected = 0
|
||||
for bad in (torch.zeros(env.num_envs, 4, device=env.device), torch.full_like(action, torch.nan)):
|
||||
try:
|
||||
env.step(bad)
|
||||
except ValueError:
|
||||
rejected += 1
|
||||
return {
|
||||
"indexed_reset": indexed,
|
||||
"timeout_terminal_reset": timeout_ok,
|
||||
"bad_action_atomic_rejection": rejected == 2
|
||||
and torch.equal(before[0], env.action_manager.action)
|
||||
and torch.equal(before[1], env.action_manager.prev_action),
|
||||
}
|
||||
|
||||
|
||||
def evaluate(env, args, policy, inputs):
|
||||
import torch
|
||||
from SkillFactory.legs.standing_ppo import canonical_hash
|
||||
from SkillFactory.tasks.manager_based.loaded_leg.mdp import state
|
||||
from standing_protocol import WIDTHS, load_at, reduce_trace
|
||||
|
||||
torch.set_num_threads(4)
|
||||
s = state(env)
|
||||
steps = round(inputs["duration_s"] / env.step_dt)
|
||||
total = steps * env.cfg.decimation
|
||||
trace = {k: torch.empty(total, env.num_envs, width, device=env.device) for k, width in WIDTHS.items()}
|
||||
params = [torch.tensor(inputs[k], device=env.device) for k in ("basis", "scale", "shift_s")]
|
||||
original_loads, original_update = s.apply_loads, env.scene.update
|
||||
frames = 0
|
||||
shots, actions, memory = [], [], []
|
||||
|
||||
def apply_loads():
|
||||
s.last_guide[:] = s.guidance()
|
||||
t = env.episode_length_buf.float() * env.step_dt
|
||||
s.last_disturbance[:] = load_at(t, *params, s.amplitude)
|
||||
wrench = s.last_guide + s.last_disturbance
|
||||
s.robot.permanent_wrench_composer.set_forces_and_torques_index(
|
||||
forces=wrench[:, None, :3],
|
||||
torques=wrench[:, None, 3:],
|
||||
positions=s.root()[0][:, None, :],
|
||||
body_ids=s.payload_ids,
|
||||
is_global=True,
|
||||
)
|
||||
|
||||
def observe(dt):
|
||||
nonlocal frames
|
||||
original_update(dt)
|
||||
if dt <= 0:
|
||||
return
|
||||
d = s.robot.data
|
||||
values = {
|
||||
"base": torch.cat(s.root(), -1),
|
||||
"foot": s.foot_center(),
|
||||
"contact": s.contact(),
|
||||
"mount": s.mount_world(),
|
||||
"guide": s.last_guide,
|
||||
"load": s.last_disturbance,
|
||||
"q": s.q(),
|
||||
"dq": d.joint_vel.torch[:, s.joints],
|
||||
"torque": d.applied_torque.torch[:, s.joints],
|
||||
}
|
||||
for key, value in values.items():
|
||||
trace[key][frames].copy_(value)
|
||||
frames += 1
|
||||
if not torch.isfinite(torch.cat(list(values.values()), -1)).all():
|
||||
raise RuntimeError("Non-finite physical/sensor state")
|
||||
|
||||
s.apply_loads, env.scene.update = apply_loads, observe
|
||||
try:
|
||||
for i in range(steps):
|
||||
obs = s.snapshot()
|
||||
action = policy.act(obs) if args.controller == "policy" else torch.zeros(env.num_envs, 3, device=env.device)
|
||||
if i % 200 == 0:
|
||||
shots.append({k: v.clone().cpu() for k, v in obs.items()})
|
||||
actions.append(action.clone().cpu())
|
||||
_, _, term, trunc, _ = env.step(action)
|
||||
if term.any() or trunc.any():
|
||||
raise RuntimeError("Unexpected auto-reset in acceptance")
|
||||
if args.mode == "stability" and (i + 1) % 100 == 0:
|
||||
output = subprocess.check_output(
|
||||
["nvidia-smi", "--query-compute-apps=pid,used_memory", "--format=csv,noheader,nounits"], text=True
|
||||
)
|
||||
rows = [line.split(",") for line in output.splitlines()]
|
||||
used = [int(row[1].strip()) * 1024**2 for row in rows if int(row[0].strip()) == os.getpid()]
|
||||
if len(used) != 1:
|
||||
raise RuntimeError("Missing/ambiguous GPU process memory sample")
|
||||
memory.append({"sim_seconds": (i + 1) * env.step_dt, "bytes": used[0]})
|
||||
if (i + 1) % 1000 == 0:
|
||||
print(f"{args.mode} {i + 1}/{steps}", flush=True)
|
||||
finally:
|
||||
s.apply_loads, env.scene.update = original_loads, original_update
|
||||
cpu = {k: v[:frames].cpu() for k, v in trace.items()}
|
||||
torch.save(cpu, args.output / "physics_trace.pt")
|
||||
if shots:
|
||||
torch.save(
|
||||
{
|
||||
"observations": {k: torch.cat([x[k] for x in shots]) for k in shots[0]},
|
||||
"actions": torch.cat(actions),
|
||||
},
|
||||
args.output / "inference_samples.pt",
|
||||
)
|
||||
write_json(args.output / "memory.json", memory)
|
||||
meta = {
|
||||
"inputs": inputs,
|
||||
"input_hash": canonical_hash(inputs),
|
||||
"physics_dt": env.physics_dt,
|
||||
"control_dt": env.step_dt,
|
||||
"height_m": env.cfg.anchor_height,
|
||||
"floating_root": not s.robot.is_fixed_base,
|
||||
"solver": "PGS" if env.cfg.sim.physics.solver_type == 0 else "TGS",
|
||||
"tgs_force_flag": env.cfg.sim.physics.enable_external_forces_every_iteration,
|
||||
"autoresets": 0,
|
||||
"total_mass_kg": s.robot.data.body_mass.torch[0].sum().item(),
|
||||
"mass_kg": dict(zip(s.robot.body_names, s.robot.data.body_mass.torch[0].tolist())),
|
||||
"limits": [[j["limits"]["lower"], j["limits"]["upper"]] for j in s.bundle["joints"]],
|
||||
"speeds": [j["limits"]["velocity"] for j in s.bundle["joints"]],
|
||||
"efforts": s.effort.tolist(),
|
||||
}
|
||||
write_json(args.output / "metadata.json", meta)
|
||||
result = reduce_trace(cpu, meta, env.cfg.support_parameters)
|
||||
if args.mode == "full":
|
||||
result["gates"].update(reset_probes(env, s))
|
||||
if args.mode == "stability":
|
||||
baseline = [v["bytes"] for v in memory if 2 <= v["sim_seconds"] <= 12]
|
||||
tail = [v["bytes"] for v in memory if 110 <= v["sim_seconds"] <= 120]
|
||||
growth = max(tail) / max(baseline) - 1 if baseline and tail else None
|
||||
result["memory_growth_fraction"] = growth
|
||||
result["gates"]["memory"] = len(baseline) >= 10 and len(tail) >= 10 and growth <= 0.05
|
||||
result.update(
|
||||
passed=all(result["gates"].values()),
|
||||
mode=args.mode,
|
||||
controller=args.controller,
|
||||
input_hash=meta["input_hash"],
|
||||
metadata=meta,
|
||||
accepted_skill=False,
|
||||
)
|
||||
return result
|
||||
|
||||
|
||||
def main():
|
||||
from isaaclab.app import AppLauncher
|
||||
|
||||
from isaaclab_tasks.utils import add_launcher_args, launch_simulation
|
||||
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--package", type=Path, required=True)
|
||||
parser.add_argument("--manifest", required=True)
|
||||
parser.add_argument("--mode", choices=("holdout", "full", "stability", "exploratory"), required=True)
|
||||
parser.add_argument("--controller", choices=("policy", "hold"), default="policy")
|
||||
parser.add_argument("--output", type=Path, required=True)
|
||||
add_launcher_args(parser)
|
||||
args = parser.parse_args()
|
||||
args.output.mkdir(parents=True, exist_ok=False)
|
||||
write_json(args.output / "result.json", {"passed": False, "status": "running", "accepted_skill": False})
|
||||
app = AppLauncher(args).app
|
||||
env, code, result = None, 1, {"passed": False}
|
||||
try:
|
||||
import gymnasium as gym
|
||||
import SkillFactory.tasks # noqa: F401
|
||||
from SkillFactory.legs.support import sha256
|
||||
from SkillFactory.tasks.manager_based.loaded_leg.cfg import LoadedLegEnvCfg
|
||||
from standing_protocol import inputs_for
|
||||
|
||||
sys.path.insert(0, str(args.package.resolve()))
|
||||
from frozen_standing import FrozenStanding
|
||||
|
||||
inputs = inputs_for(args.mode)
|
||||
config = LoadedLegEnvCfg()
|
||||
config.scene.num_envs = inputs["count"]
|
||||
config.episode_length_s = inputs["duration_s"] + 1
|
||||
config.terminations.fallen, config.terminations.invalid = None, None
|
||||
source_paths = [
|
||||
"scripts/evaluate_standing.py",
|
||||
"scripts/standing_protocol.py",
|
||||
"scripts/runtime_common.py",
|
||||
"scripts/supervise_sim.py",
|
||||
]
|
||||
training_identity = json.loads((args.package / "binding.json").read_text())["training_identity"]
|
||||
for path, digest in training_identity["sources"].items():
|
||||
if sha256(path) != digest:
|
||||
raise ValueError(f"Training-bound source changed: {path}")
|
||||
source_paths += list(training_identity["sources"])
|
||||
before = {p: sha256(p) for p in set(source_paths)}
|
||||
with launch_simulation(config, args):
|
||||
env = gym.make("SkillFactory-LoadedLegStand-v0", cfg=config).unwrapped
|
||||
env.reset(seed=inputs["seed"])
|
||||
policy = FrozenStanding(
|
||||
args.package,
|
||||
manifest_sha256=args.manifest,
|
||||
task_sha256=sha256(config.support_config),
|
||||
support_sha256=sha256(Path(config.support_parameters["support_asset"]) / "support.json"),
|
||||
device=env.device,
|
||||
)
|
||||
result = evaluate(env, args, policy, inputs)
|
||||
if sha256(args.package / "manifest.json") != args.manifest or any(
|
||||
sha256(p) != h for p, h in before.items()
|
||||
):
|
||||
raise RuntimeError("Test identity changed during physics")
|
||||
for path in before:
|
||||
target = args.output / "source_snapshot" / path
|
||||
target.parent.mkdir(parents=True, exist_ok=True)
|
||||
shutil.copyfile(path, target)
|
||||
result.update(
|
||||
package_manifest_sha256=args.manifest,
|
||||
actor_sha256=sha256(args.package / "actor.pt"),
|
||||
physics_trace_sha256=sha256(args.output / "physics_trace.pt"),
|
||||
source_hashes=before,
|
||||
)
|
||||
print(
|
||||
json.dumps(
|
||||
{k: v for k, v in result.items() if k not in ("rows", "metadata", "source_hashes")}, indent=2
|
||||
)
|
||||
)
|
||||
code = 0 if result["passed"] else 2
|
||||
except BaseException:
|
||||
error = traceback.format_exc()
|
||||
print(error, file=sys.stderr, flush=True)
|
||||
result = {"passed": False, "accepted_skill": False, "error": error}
|
||||
finally:
|
||||
finish_run(app, env, args.output / "result.json", result, code)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user