#!/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()