#!/usr/bin/env python3 """Development-only physical preflight. No optimizer, checkpoint selection or holdout access.""" import argparse import hashlib import json import shutil import sys import traceback from pathlib import Path from runtime_common import finish_run, write_json def evaluate(env, args, thresholds): import torch from SkillFactory.legs.contact_cycle.mechanics import inverse_rotate from SkillFactory.legs.contact_cycle.metrics import summarize from SkillFactory.legs.tokens import LegKinematics from SkillFactory.tasks.manager_based.contact_cycle.mdp import failed, state s = state(env) fk = LegKinematics(s.bundle, env.device) nominal = s.nominal.expand(env.num_envs, -1) q_ik = nominal.clone() limits = torch.tensor([[j["limits"]["lower"], j["limits"]["upper"]] for j in s.bundle["joints"]], device=env.device) speeds = torch.tensor([j["limits"]["velocity"] for j in s.bundle["joints"]], device=env.device) stiffness = torch.tensor([j["kp"] for j in s.bundle["spec"]["joints"]], device=env.device) trace = {} def foot(q): transform = fk.forward(q)[s.bundle["foot"]["body"]] return transform[:, :3, 3] + (transform[:, :3, :3] @ s.offset[:, :, None]).squeeze(-1) def jacobian(q): columns = [] for j in range(3): delta = torch.zeros_like(q) delta[:, j] = 0.0005 columns.append((foot(q+delta)-foot(q-delta))/0.001) return torch.stack(columns, -1) original = env.scene.update def observe(dt): original(dt) if dt <= 0: return phase, progress, target, desired, _ = s.reference() pos = s.root()[0] excess = torch.maximum(limits[:, 0]-s.q(), s.q()-limits[:, 1]).clamp(min=0).amax(-1) ratio = torch.maximum((s.robot.data.applied_torque.torch[:, s.joints]/s.effort).abs().amax(-1), (s.robot.data.joint_vel.torch[:, s.joints]/speeds).abs().amax(-1)) mass = s.robot.data.body_mass.torch gravity = torch.zeros(env.num_envs, s.robot.num_bodies, 3, device=env.device) gravity[..., 2] = -9.81 * mass com = s.robot.data.body_com_pos_w.torch payload_force = (s.external + s.guide + s.auxiliary)[:, s.payload, :3] + gravity[:, s.payload] payload_torque = (s.external + s.guide + s.auxiliary)[:, s.payload, 3:] expected_mount = torch.cat((payload_force, payload_torque + torch.cross( com[:, s.payload] - s.robot.data.body_link_pos_w.torch[:, s.anchor], payload_force, dim=-1)), -1) values = { "time": s.time, "phase": phase, "progress": progress, "scenario": s.scenarios, "foot": s.foot_center(), "target": target, "desired_force": desired, "weight": s.weight, "normal": s.contact(), "friction": s.friction(), "gap": s.foot_center()[:, 2]-s.bundle["foot"]["radius"], "slip_speed": s.slip_velocity(), "height_error": (pos[:, 2]-s.port_reference()[0][:, 2]).abs(), "joint_excess": excess, "limit_ratio": ratio, "joint_pos": s.q(), "joint_vel": s.robot.data.joint_vel.torch[:, s.joints], "applied_torque": s.robot.data.applied_torque.torch[:, s.joints], "base_state": torch.cat(s.root(), -1), "mount_wrench": s.mount_wrench(), "external": s.external, "guide": s.guide, "auxiliary": s.auxiliary, "contact_latch": s.latch.contact, "event_fired": s.schedule.fired, "event_envelope": s.schedule.envelope, "event_integral": s.schedule.integral, "event_completed": s.schedule.completed, "would_fail": failed(env), "tilt": s.tilt(), "linear_momentum": (mass[..., None] * s.robot.data.body_com_lin_vel_w.torch).sum(1), "total_external_force": (s.external[..., :3] + s.guide[..., :3] + s.auxiliary[..., :3] + gravity).sum(1) + s.contact() + s.friction(), "mount_world": s.mount_world(), "expected_static_mount_world": expected_mount, } for k, v in values.items(): trace.setdefault(k, []).append(v.detach().cpu().clone()) env.scene.update = observe steps = round(sum(s.cc["phase_durations_s"]) / env.step_dt) observations, actions = [], [] try: for _ in range(steps): # Analytic engineering controller with privileged root pose/FK. Not a deployable learned policy. _, _, target, fn, _ = s.reference() pos, quat, _, _ = s.root() local = inverse_rotate(quat, target-pos) for _ in range(3): j = jacobian(q_ik) dq = torch.linalg.solve(j @ j.transpose(-1, -2) + torch.eye(3, device=env.device)*1e-5, (local-foot(q_ik))[:, :, None]) q_ik += (j.transpose(-1, -2) @ dq).squeeze(-1).clamp(-0.08, 0.08) q_ik.clamp_(min=nominal-0.5, max=nominal+0.5) f = torch.zeros_like(local) f[:, 2] = fn torque = -(jacobian(q_ik).transpose(-1, -2) @ inverse_rotate(quat, f)[:, :, None]).squeeze(-1) command = q_ik + torque / stiffness action = ((command-nominal)/0.5).clamp(-1, 1) if args.controller == "disabled": action.zero_() obs, _, terminated, truncated, _ = env.step(action.float()) if terminated.any() or truncated.any(): raise RuntimeError("Physical validation must retain failures without automatic reset") observations.append(obs["policy"].cpu().clone()) actions.append(action.cpu().clone()) finally: env.scene.update = original data = {k: torch.stack(v) for k, v in trace.items()} torch.save(data, args.output / "physics_trace.pt") torch.save({"policy": torch.stack(observations) if observations else [], "action": torch.stack(actions) if actions else []}, args.output / "control_trace.pt") rows = summarize(data, env.physics_dt, thresholds, s.cc["scenarios"]) momentum_residual = ((data["linear_momentum"][1:] - data["linear_momentum"][:-1]) / env.physics_dt - data["total_external_force"][1:]) momentum_rms = momentum_residual.square().mean(0).sqrt().norm(dim=-1) static = (data["time"][:, 0] >= 2.0) & (data["time"][:, 0] < 2.8) sensor_residual = (data["mount_world"][static].mean(0) - data["expected_static_mount_world"][static].mean(0)).abs() expected_events = torch.tensor([[i == e["scenario"] for e in s.cc["events"]] for i in s.scenarios.tolist()], device="cpu") # Reset probes are outside the physics trial. Live nonzero wrench + selective reset tests persistence. before = {"root": s.robot.data.root_state_w.torch.clone(), "q": s.q().clone(), "time": s.time.clone(), "fired": s.schedule.fired.clone(), "sensor": s.sensed.clone()} ids = torch.arange(0, env.num_envs, 2, device=env.device) other = torch.arange(1, env.num_envs, 2, device=env.device) composer = s.robot.permanent_wrench_composer seeded = torch.arange(1, env.num_envs + 1, device=env.device).float()[:, None, None] seeded = seeded.expand(-1, s.robot.num_bodies, 3).contiguous() composer.set_forces_and_torques_index(forces=seeded, torques=seeded*0.1, positions=None, is_global=True) before_force, before_torque = composer.out_force_b.torch.clone(), composer.out_torque_b.torch.clone() env.reset(env_ids=ids) composer_reset = bool((composer.out_force_b.torch[ids] == 0).all() and (composer.out_torque_b.torch[ids] == 0).all() and torch.equal(before_force[other], composer.out_force_b.torch[other]) and torch.equal(before_torque[other], composer.out_torque_b.torch[other])) isolated = all(torch.equal(a[other], b[other]) for a, b in ( (before["root"], s.robot.data.root_state_w.torch), (before["q"], s.q()), (before["time"], s.time), (before["fired"], s.schedule.fired), (before["sensor"], s.sensed))) cleared = bool((s.external[ids] == 0).all() and (s.auxiliary[ids] == 0).all() and (s.time[ids] == 0).all() and not s.schedule.fired[ids].any() and (s.action_delay.data[:, ids] == 0).all() and not s.latch.contact[ids].any() and (s.schedule.integral[ids] == 0).all() and (s.sensor_delay.data[:, ids] == s.raw_sensors()[ids]).all() and (s.sensed[ids] == s.raw_sensors()[ids]).all()) snapshot = s.policy().clone() env.reset(env_ids=[]) empty = torch.equal(snapshot, s.policy()) env.cfg.episode_length_s = 3 * env.step_dt env.reset() for _ in range(3): obs, _, terminated, truncated, info = env.step(torch.zeros(env.num_envs, 3, device=env.device)) terminal = bool(truncated.all() and not terminated.any() and info["terminal_observation"] is not None and info["terminal_observation"]["policy"].shape == (env.num_envs, 32) and (s.time == 0).all() and not s.schedule.fired.any()) before = s.time.clone() rejected = False try: env.step(torch.full((env.num_envs, 3), float("nan"), device=env.device)) except ValueError: rejected = torch.equal(before, s.time) expected_integral = torch.tensor([e["duration_s"] - e["ramp_s"] for e in s.cc["events"]]) delivered = (data["event_integral"][-1] - expected_events * expected_integral).abs() <= 0.002 gates = {"floating_root": not s.robot.is_fixed_base, "finite_trace": all(r["finite"] for r in rows), "full_phases": all(r["complete"] for r in rows), "indexed_reset": isolated and cleared, "empty_reset": empty, "composer_reset": composer_reset, "timeout_terminal": terminal, "bad_action_rejected": rejected, "guide_vertical_zero": bool((data["guide"][..., 2] == 0).all()), "all_scenario_metrics": all(r["success"] for r in rows), "event_coverage": torch.equal(data["event_fired"][-1], expected_events) and torch.equal(data["event_completed"][-1], expected_events) and bool(delivered.all()), "scenario_coverage": set(data["scenario"][0].tolist()) == set(range(len(s.cc["scenarios"]))), "linear_momentum_balance": bool((momentum_rms <= 5.0).all()), "static_mount_force": bool((sensor_residual[:, :3] <= 1.5).all()), "static_mount_torque": bool((sensor_residual[:, 3:] <= 0.3).all())} return {"passed": all(gates.values()), "accepted_skill": False, "trained_skill": False, "kind": "development_physics_preflight", "controller": args.controller, "controller_uses_privileged_state": True, "seed": args.seed, "gates": gates, "rows": rows, "physics_dt": env.physics_dt, "control_dt": env.step_dt, "physics_steps": len(data["time"]), "support_enabled": env.cfg.auxiliary_enabled, "effective_mass_kg": dict(zip(s.robot.body_names, s.robot.data.body_mass.torch[0].tolist())), "event_triggered_per_env": data["event_fired"][-1].tolist(), "event_integral_s_per_env": data["event_integral"][-1].tolist(), "event_completed_per_env": data["event_completed"][-1].tolist(), "linear_momentum_balance_rms_N_per_env": momentum_rms.tolist(), "static_mount_residual_N_Nm_per_env": sensor_residual.tolist(), "note": "Preflight is not independent acceptance, multi-seed training, or hardware generalization."} 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("--config", type=Path, default=Path("configs/contact_cycle/go2_fl_v1.json")) parser.add_argument("--thresholds", type=Path, default=Path("configs/contact_cycle/acceptance_v1.json")) parser.add_argument("--output", type=Path, required=True) parser.add_argument("--num_envs", type=int, default=4) parser.add_argument("--seed", type=int, default=90101) parser.add_argument("--no-auxiliary", action="store_true") parser.add_argument("--controller", choices=("ik", "disabled"), default="ik") add_launcher_args(parser) args = parser.parse_args() if args.num_envs < 4: parser.error("At least four environments required for development scenarios") args.output.mkdir(parents=True, exist_ok=False) root = Path(__file__).resolve().parents[1] from SkillFactory.legs.contact_cycle.config import load_config, require_scenario_coverage require_scenario_coverage(load_config(args.config), args.num_envs) sources = [Path(__file__), args.config, args.thresholds, root/"scripts/runtime_common.py", root/"scripts/supervise_sim.py", root/"configs/standing/go2_fl_support_v0.json", root/"assets/generated/support/go2_fl_3kg_v0/support.json", root/"assets/generated/legs/go2_fl/manifest.json"] sources += list((root/"source/SkillFactory/SkillFactory").rglob("*.py")) identity = {str(p): hashlib.sha256(p.read_bytes()).hexdigest() for p in sources} for p in sources: relative = (p.resolve().relative_to(root) if p.resolve().is_relative_to(root) else Path("external")/hashlib.sha256(p.read_bytes()).hexdigest()/p.name) target = args.output/"source_snapshot"/relative target.parent.mkdir(parents=True, exist_ok=True) shutil.copyfile(p, target) write_json(args.output/"identity.json", identity) write_json(args.output/"invocation.json", {"argv": sys.argv, "thresholds": json.loads(args.thresholds.read_text())}) write_json(args.output/"result.json", {"passed": False, "status": "running", "accepted_skill": False}) app = AppLauncher(args).app env, result, code = None, {"passed": False, "accepted_skill": False}, 1 try: import gymnasium as gym import SkillFactory.tasks # noqa: F401 from SkillFactory.tasks.manager_based.contact_cycle.cfg import ContactCycleEnvCfg cfg = ContactCycleEnvCfg(cycle_config=str(args.config)) cfg.scene.num_envs = args.num_envs cfg.seed = args.seed cfg.auxiliary_enabled = not args.no_auxiliary cfg.episode_length_s += 1 cfg.terminations.failed = None # Full failed trajectory retained for audit, never censored by reset. if args.controller == "disabled": cfg.scene.robot.actuators["leg"].stiffness = 0 cfg.scene.robot.actuators["leg"].damping = 0 with launch_simulation(cfg, args): env = gym.make("SkillFactory-Go2FL-ContactCycle-v1", cfg=cfg).unwrapped env.reset(seed=args.seed) result = evaluate(env, args, json.loads(args.thresholds.read_text())["gates"]) write_json(args.output/"config.json", cfg.cycle_parameters) if any(hashlib.sha256(Path(p).read_bytes()).hexdigest() != digest for p, digest in identity.items()): raise RuntimeError("Bound source/config changed during physical trial") code = 0 if result["passed"] else 2 except BaseException: result.update(passed=False, error=traceback.format_exc()) print(result["error"], file=sys.stderr) finally: finish_run(app, env, args.output/"result.json", result, code) if __name__ == "__main__": main()