Files
SkillFactory/scripts/verify_contact_cycle.py
T
chenlin 9fe9d27865 feat: checkpoint contact-cycle evaluation and RootXY PPO progress
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.
2026-10-15 14:18:17 +08:00

263 lines
15 KiB
Python

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