9fe9d27865
Consolidate next-stage docs, archive superseded narratives, and preserve frozen document inputs. Retain invalid P2 pairs, partial RootXY training, failed attempts and nonselectable checkpoints as explicit limitations. Include supervised evaluation/training, desktop GPU admission, bounded evidence and atomic-publication monitor repairs with CPU regressions. Ignore local dotenv configuration. Validation: 1403 passed, 6 skipped for contact-cycle and related contracts/CLI tests. Full-suite legacy ABI and registration tests require Python 3.12/Torch 2.10/pxr; no runtime checks relaxed. No new physical run or accepted skill.
299 lines
13 KiB
Python
299 lines
13 KiB
Python
#!/usr/bin/env python3
|
|
"""Locked standing candidate physics trials; never optimizes or selects weights."""
|
|
|
|
import argparse
|
|
import json
|
|
import math
|
|
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 = [], [], []
|
|
arrows = None
|
|
if args.force_arrows:
|
|
from standing_force_visualizer import DisturbanceForceArrows
|
|
|
|
arrows = DisturbanceForceArrows(args.force_arrow_scale)
|
|
print(
|
|
f"Orange arrows: actual world disturbance force, {args.force_arrow_scale:g} m/N; no torque/guide/reaction"
|
|
)
|
|
|
|
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,
|
|
)
|
|
if arrows is not None and frames % env.cfg.decimation == 0:
|
|
arrows.update(s) # display only, after applying this control step's actual load
|
|
|
|
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
|
|
if arrows is not None:
|
|
arrows.close()
|
|
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,
|
|
visualization={
|
|
"force_arrows": args.force_arrows,
|
|
"force_arrow_scale_m_per_N": args.force_arrow_scale,
|
|
"source": "actual applied disturbance force in world frame; excludes torque, guide, reaction",
|
|
},
|
|
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)
|
|
parser.add_argument(
|
|
"--force-arrows", action="store_true", help="Draw actual disturbance force in Kit (visual only)"
|
|
)
|
|
parser.add_argument(
|
|
"--force-arrow-scale", type=float, default=0.03, help="Arrow length per newton, m/N (default: 0.03)"
|
|
)
|
|
add_launcher_args(parser)
|
|
args = parser.parse_args()
|
|
if not math.isfinite(args.force_arrow_scale) or args.force_arrow_scale <= 0:
|
|
parser.error("--force-arrow-scale must be finite and positive")
|
|
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",
|
|
]
|
|
if args.force_arrows:
|
|
source_paths.append("scripts/standing_force_visualizer.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()
|