Files
SkillFactory/scripts/verify_anchor.py
T
chenlin 7f39abb613 release: SkillFactory v0.0.1
Add declarative single-leg import, fixed-anchor benchmark, masked shared-policy PPO prototype, regression tests, and consolidated documentation.
2026-10-10 09:58:30 +08:00

495 lines
20 KiB
Python

#!/usr/bin/env python3
"""Deterministic exit-criteria tests for the real Go2 single-leg anchor."""
import math
import time
from anchor_common import launch
from runtime_common import critical_log_lines, write_json
def configure(parser):
parser.add_argument(
"--suite",
choices=["contract", "stability", "calibration", "contact", "reset", "long", "core", "full"],
default="full",
)
parser.add_argument("--duration", type=float, default=600.0)
parser.add_argument("--reset_count", type=int, default=1334)
def check_contract(env):
import torch
from SkillFactory.assets.go2_leg import JOINT_NAMES
from SkillFactory.tasks.manager_based.single_leg_anchor.mdp.observations import diagnostics
u = env.unwrapped
obs, _ = env.reset()
robot = u.scene["robot"]
assert robot.joint_names == JOINT_NAMES
assert robot.num_joints == 3
assert obs["policy"].shape == (u.num_envs, 19)
assert obs["policy"].dtype == torch.float32 and obs["policy"].is_cuda
assert torch.isfinite(obs["policy"]).all()
d = diagnostics(u)
assert d["joint_mask"].shape == (u.num_envs, 3) and d["joint_mask"].dtype == torch.bool
assert d["force_history_w"].shape == (u.num_envs, 5, 1, 3)
actions = torch.zeros((u.num_envs, 3), device=u.device)
u.action_manager.process_action(actions)
term = u.action_manager.get_term("joint_position")
assert torch.allclose(term.processed_actions, robot.data.default_joint_pos.torch, atol=1e-7)
u.action_manager.process_action(actions + 10.0)
assert torch.allclose(term.processed_actions, robot.data.default_joint_pos.torch + 0.25, atol=1e-7)
assert u.action_manager.action.max() == 1.0
limits = robot.data.soft_joint_pos_limits.torch
assert ((term.processed_actions >= limits[:, :, 0]) & (term.processed_actions <= limits[:, :, 1])).all()
counter = u._sim_step_counter
try:
env.step(torch.full_like(actions, float("nan")))
except ValueError:
pass
else:
raise AssertionError("NaN input was accepted")
assert u._sim_step_counter == counter
env.reset()
obs, _, done, timeout, _ = env.step(actions)
assert torch.isfinite(obs["policy"]).all() and not done.any() and not timeout.any()
return {
"passed": True,
"policy_shape": list(obs["policy"].shape),
"device": str(obs["policy"].device),
"joint_order": robot.joint_names,
"force_history_shape": list(d["force_history_w"].shape),
"nan_rejected_without_physics_step": True,
}
def _step(env, actions):
import torch
obs, _, done, timeout, _ = env.step(actions)
assert not done.any() and not timeout.any(), "Unexpected automatic reset"
assert torch.isfinite(obs["policy"]).all(), "Non-finite observation"
return obs
def check_stability(env):
import torch
from SkillFactory.tasks.manager_based.single_leg_anchor.mdp.observations import diagnostics
u = env.unwrapped
env.reset()
robot = u.scene["robot"]
initial = diagnostics(u)["anchor_pose_w"].clone()
actions = torch.zeros((u.num_envs, 3), device=u.device)
errors = torch.zeros((u.num_envs, 3), device=u.device)
max_drift = max_angle = 0.0
for step in range(6200):
actions[:] = 0.4 * math.sin(2 * math.pi * 0.5 * step * u.step_dt)
_step(env, actions)
d = diagnostics(u)
assert (d["gap"] >= 0.01).all(), "Stability test must be in air"
limits = robot.data.joint_pos_limits.torch
assert ((d["q"] >= limits[:, :, 0] - 0.01) & (d["q"] <= limits[:, :, 1] + 0.01)).all()
assert (d["applied_torque"].abs() <= robot.data.joint_effort_limits.torch + 1e-4).all()
max_drift = max(max_drift, (d["anchor_pose_w"][:, :3] - initial[:, :3]).abs().max().item())
dot = (d["anchor_pose_w"][:, 3:] * initial[:, 3:]).sum(dim=-1).abs().clamp(max=1.0)
max_angle = max(max_angle, (2 * torch.acos(dot)).max().item())
if step >= 200:
errors += (d["q"] - d["q_target"]).square()
rms = (errors / 6000).sqrt().max(dim=0).values
assert (rms <= 0.10).all(), f"Tracking RMS: {rms.tolist()}"
assert max_drift <= 1e-4 and max_angle <= 1e-3
return {
"passed": True,
"warmup_s": 2.0,
"duration_s": 60.0,
"amplitude_rad": 0.1,
"frequency_hz": 0.5,
"worst_env_rms_rad": rms.tolist(),
"root_drift_m": max_drift,
"root_angle_rad": max_angle,
}
def check_calibration(env):
import torch
u = env.unwrapped
env.reset()
actions = torch.zeros((u.num_envs, 3), device=u.device)
force = torch.zeros(u.num_envs, device=u.device)
for step in range(500):
_step(env, actions)
if step >= 300:
force += u.scene["probe_contact"].data.force_matrix_w.torch[:, 0, 0, 2]
force /= 200
assert ((force - 9.81).abs() / 9.81 <= 0.2).all(), force.tolist()
return {
"passed": True,
"probe_mass_kg": 1.0,
"expected_N": 9.81,
"mean_fz_min_N": force.min().item(),
"mean_fz_max_N": force.max().item(),
}
def check_contact(env):
import torch
from SkillFactory.tasks.manager_based.single_leg_anchor.mdp.observations import diagnostics
u = env.unwrapped
env.reset()
actions = torch.zeros((u.num_envs, 3), device=u.device)
air_max, press_min, penetration = 0.0, float("inf"), 0.0
latency = 0
transitions = []
for cycle in range(20):
# Alternate assignments across environments: verify contact channels
# and independent commands without changing the fixed anchor poses.
signs = torch.where(torch.arange(u.num_envs, device=u.device) % 2 == 0, 1.0, -1.0)
for phase in ("air", "press"):
actions[:, 2] = -signs if phase == "air" else signs
air_ids = actions[:, 2] < 0
press_ids = ~air_ids
gap_crossed = torch.zeros(u.num_envs, dtype=torch.bool, device=u.device)
latency_steps = torch.zeros(u.num_envs, dtype=torch.int32, device=u.device)
air_window, press_window, gap_window = [], [], []
for step in range(100):
_step(env, actions)
d = diagnostics(u)
f = d["normal_force_w"]
contact = f.norm(dim=-1) > 1.0
crossed = (d["gap"] <= 0.0001) & press_ids
gap_crossed |= crossed
pending = gap_crossed & ~contact & press_ids
latency_steps[pending] += 1
if step >= 70:
if air_ids.any():
assert (d["gap"][air_ids] >= 0.01).all(), ("air gap", d["gap"].tolist())
air_window.append(f[air_ids].norm(dim=-1))
if press_ids.any():
press_window.append(f[press_ids, 2])
gap_window.append(d["gap"][press_ids])
latency = max(latency, latency_steps.max().item())
assert latency_steps.max() <= 1, f"Contact response >1 control cycle: {latency_steps.tolist()}"
if air_window:
current = torch.stack(air_window).max().item()
air_max = max(air_max, current)
assert current < 0.1, f"Air force: {current}"
if press_window:
mean = torch.stack(press_window).mean(dim=0).min().item()
press_min = min(press_min, mean)
assert mean > 1.0, f"Pressed force: {mean}"
penetration = max(penetration, max(0.0, -torch.stack(gap_window).min().item()))
assert penetration <= 0.005, f"Penetration: {penetration}"
transitions.append([cycle, phase, d["gap"][0].item(), d["normal_force_w"][0, 2].item()])
return {
"passed": True,
"cycles": 20,
"air_force_max_N": air_max,
"press_fz_min_N": press_min,
"penetration_max_m": penetration,
"latency_control_steps_max": latency,
"trace": transitions,
}
def check_resets(env, count):
import torch
from isaaclab.sim import get_current_stage
u = env.unwrapped
robot = u.scene["robot"]
actions = torch.zeros((u.num_envs, 3), device=u.device)
all_ids = torch.arange(u.num_envs, device=u.device, dtype=torch.int32)
env.reset()
for _ in range(50):
actions[:, 2] = torch.where(all_ids % 2 == 0, 1.0, -1.0)
_step(env, actions)
stage = get_current_stage()
prim_count = sum(1 for _ in stage.Traverse())
sensor_id, robot_id = id(u.scene["foot_contact"]), id(robot)
torch.cuda.synchronize()
mem0 = torch.cuda.memory_allocated()
memory = []
max_q_error = max_vel = max_unselected = max_history = 0.0
latencies = []
for iteration in range(count):
mode = iteration % 4
ids = (
all_ids
if mode == 0
else (
all_ids[iteration % u.num_envs : iteration % u.num_envs + 1]
if mode == 1
else all_ids[iteration % 2 :: 2]
)
)
if mode == 3:
ids = all_ids[:0] # documented no-op edge handling at caller
selected = torch.zeros(u.num_envs, dtype=torch.bool, device=u.device)
selected[ids.long()] = True
q0, dq0 = robot.data.joint_pos.torch.clone(), robot.data.joint_vel.torch.clone()
action0 = u.action_manager.action.clone()
sim_count = u._sim_step_counter
start = time.perf_counter()
if ids.numel():
u.reset(env_ids=ids)
torch.cuda.synchronize()
latencies.append(time.perf_counter() - start)
assert u._sim_step_counter == sim_count, "Reset advanced physical time"
if ids.numel():
max_q_error = max(
max_q_error,
(robot.data.joint_pos.torch[ids] - robot.data.default_joint_pos.torch[ids]).abs().max().item(),
)
max_vel = max(max_vel, robot.data.joint_vel.torch[ids].abs().max().item())
assert max_q_error <= 1e-5 and max_vel <= 1e-5
assert u.episode_length_buf[ids].max() == 0
assert u.action_manager.action[ids].abs().max() == 0
assert u.action_manager.prev_action[ids].abs().max() == 0
assert torch.allclose(
robot.data.joint_pos_target.torch[ids], robot.data.default_joint_pos.torch[ids], atol=1e-5
)
h = u.scene["foot_contact"].data.net_forces_w_history.torch[ids]
max_history = max(max_history, h.abs().max().item())
assert max_history == 0.0, f"Stale contact history after reset: {max_history}"
other = ~selected
if other.any():
change = max(
(robot.data.joint_pos.torch[other] - q0[other]).abs().max().item(),
(robot.data.joint_vel.torch[other] - dq0[other]).abs().max().item(),
(u.action_manager.action[other] - action0[other]).abs().max().item(),
)
max_unselected = max(max_unselected, change)
assert change <= 1e-6, f"Unselected state changed during reset: {change}"
_step(env, actions)
assert id(u.scene["foot_contact"]) == sensor_id and id(u.scene["robot"]) == robot_id
if iteration == min(7, count - 1):
# Include verification scratch tensors and warm-up all subset shapes.
# The baseline must not precede the test's own snapshot allocations.
torch.cuda.synchronize()
mem0 = torch.cuda.memory_allocated()
free, total = torch.cuda.mem_get_info()
gpu0 = total - free
if iteration % 100 == 0:
torch.cuda.synchronize()
memory.append(torch.cuda.memory_allocated())
torch.cuda.synchronize()
mem1 = torch.cuda.memory_allocated()
free, total = torch.cuda.mem_get_info()
gpu1 = total - free
assert gpu1 <= gpu0 * 1.05, f"Device memory growth (including Warp/PhysX): {gpu0}->{gpu1}"
assert get_current_stage() == stage and sum(1 for _ in stage.Traverse()) == prim_count
assert mem1 <= mem0 * 1.05, f"CUDA memory growth: {mem0}->{mem1}"
return {
"passed": True,
"reset_operations": count,
"nonempty_reset_operations": count - count // 4,
"q_error_max_rad": max_q_error,
"dq_max_rad_s": max_vel,
"unselected_change_max": max_unselected,
"stale_history_max_N": max_history,
"cuda_before_bytes": mem0,
"cuda_after_bytes": mem1,
"device_used_before_bytes": gpu0,
"device_used_after_bytes": gpu1,
"cuda_samples_bytes": memory,
"reset_latency_mean_ms": 1000 * sum(latencies) / len(latencies),
"reset_latency_max_ms": 1000 * max(latencies),
"stage_prim_count": prim_count,
}
def check_long(env, duration):
import torch
from SkillFactory.tasks.manager_based.single_leg_anchor.mdp.observations import diagnostics
u = env.unwrapped
env.reset()
steps = round(duration / u.step_dt)
assert steps > 0
actions = torch.zeros((u.num_envs, 3), device=u.device)
initial = diagnostics(u)["anchor_pose_w"].clone()
drift = 0.0
forces = 0.0
counter = u._sim_step_counter
for _ in range(200):
_step(env, actions)
d = diagnostics(u) # warm the same diagnostic/scratch allocations as rollout
counter = u._sim_step_counter
torch.cuda.synchronize()
start = time.perf_counter()
mem0 = torch.cuda.memory_allocated()
free, total = torch.cuda.mem_get_info()
gpu0 = total - free
rows = []
for step in range(steps):
actions[:, 0] = 0.2 * math.sin(2 * math.pi * 0.5 * step * u.step_dt)
actions[:, 2] = torch.where(
torch.arange(u.num_envs, device=u.device) % 2 == 0,
0.8 * math.sin(2 * math.pi * 0.25 * step * u.step_dt),
-0.8 * math.sin(2 * math.pi * 0.25 * step * u.step_dt),
)
_step(env, actions)
d = diagnostics(u)
assert all(torch.isfinite(d[k]).all() for k in ("q", "dq", "computed_torque", "normal_force_w"))
limits = u.scene["robot"].data.joint_pos_limits.torch
assert ((d["q"] >= limits[:, :, 0] - 0.01) & (d["q"] <= limits[:, :, 1] + 0.01)).all()
assert (d["applied_torque"].abs() <= u.scene["robot"].data.joint_effort_limits.torch + 1e-4).all()
drift = max(drift, (d["anchor_pose_w"][:, :3] - initial[:, :3]).abs().max().item())
forces = max(forces, d["normal_force_w"][:, 2].max().item())
if step % 1000 == 0:
rows.append(
[
step * u.step_dt,
*d["q"][0].tolist(),
d["gap"][0].item(),
d["normal_force_w"][0, 2].item(),
torch.cuda.memory_allocated(),
]
)
if step % 10000 == 0:
print(f"[VERIFY] long rollout {step}/{steps}", flush=True)
torch.cuda.synchronize()
elapsed = time.perf_counter() - start
assert u._sim_step_counter - counter == steps * 5
assert (u.episode_length_buf == steps + 200).all()
assert drift <= 1e-4
mem1 = torch.cuda.memory_allocated()
free, total = torch.cuda.mem_get_info()
gpu1 = total - free
assert mem1 <= mem0 * 1.05
assert gpu1 <= gpu0 * 1.05, f"Device memory growth (including Warp/PhysX): {gpu0}->{gpu1}"
return {
"passed": True,
"duration_s": steps * u.step_dt,
"control_steps": steps,
"physics_steps": steps * 5,
"physics_dt": u.physics_dt,
"control_dt": u.step_dt,
"wall_seconds": elapsed,
"env_steps_per_second": steps * u.num_envs / elapsed,
"root_drift_m": drift,
"max_normal_fz_N": forces,
"cuda_before_bytes": mem0,
"cuda_after_bytes": mem1,
"cuda_peak_bytes": torch.cuda.max_memory_allocated(),
"device_used_before_bytes": gpu0,
"device_used_after_bytes": gpu1,
"device_total_bytes": total,
"trace": rows,
}
def run(env, args):
import json
if args.num_envs < 1 or args.reset_count < 8 or args.duration <= 0 or not math.isfinite(args.duration):
raise ValueError("Require num_envs >=1, reset_count >=8, finite duration >0")
result = {
"suite": args.suite,
"num_envs": args.num_envs,
"seed": args.seed,
"backend": "Kit PhysxCfg",
"contract": check_contract(env),
}
checks = {
"stability": lambda: check_stability(env),
"calibration": lambda: check_calibration(env),
"contact": lambda: check_contact(env),
"reset": lambda: check_resets(env, args.reset_count),
"long": lambda: check_long(env, args.duration),
}
names = ["contact", "reset", "long"] if args.suite == "core" else ([args.suite] if args.suite in checks else [])
for name in names:
print(f"[VERIFY] Starting {name}", flush=True)
result[name] = checks[name]()
if "trace" in result[name]:
import csv
headers = (
["cycle", "phase", "gap_m", "normal_fz_N"]
if name == "contact"
else [
"time_s",
"q_hip_rad",
"q_thigh_rad",
"q_calf_rad",
"gap_m",
"normal_fz_N",
"torch_allocated_bytes",
]
)
with (args.output / f"{name}.csv").open("w", newline="") as stream:
writer = csv.writer(stream)
writer.writerow(headers)
writer.writerows(result[name]["trace"])
(args.output / "checkpoint.json").write_text(json.dumps(result, indent=2) + "\n")
result["passed"] = True
result["milestone_complete"] = False # individual suites are not full acceptance
return result
def main():
# Full acceptance runs fresh Kit processes so the air anchor and probe are
# independent physical scenes, not reset-time mutations of a fixed joint.
import argparse
import json
import subprocess
import sys
from pathlib import Path
parser = argparse.ArgumentParser(add_help=False)
parser.add_argument("--suite", default="full")
parser.add_argument("--output", type=Path, default=Path("logs/week1_anchor/acceptance"))
args, argv = parser.parse_known_args()
if args.suite != "full" or "--help" in sys.argv or "-h" in sys.argv:
launch(__doc__, configure, run)
return
args.output.mkdir(parents=True, exist_ok=True)
write_json(args.output / "result.json", {"passed": False, "status": "running"})
results = {}
for suite in ("stability", "calibration", "core"):
output = args.output / suite
print(f"[VERIFY] Fresh process: {suite}", flush=True)
with (args.output / f"{suite}.log").open("w") as log:
child = subprocess.run(
[sys.executable, str(Path(__file__).resolve()), *argv, "--suite", suite, "--output", str(output)],
stdout=log,
stderr=subprocess.STDOUT,
)
critical = critical_log_lines(args.output / f"{suite}.log")
if child.returncode or critical:
write_json(
args.output / "result.json",
{
"passed": False,
"failed_suite": suite,
"returncode": child.returncode,
"critical_log_lines": critical[-20:],
"completed": results,
},
)
raise SystemExit(child.returncode or 1)
report = json.loads((output / "result.json").read_text())
if not report.get("passed"):
write_json(args.output / "result.json", {"passed": False, "failed_suite": suite, "report": report})
raise SystemExit(1)
results[suite] = report
core = results["core"]
complete = (
core["num_envs"] == 64
and core["long"]["duration_s"] >= 600.0
and core["reset"]["nonempty_reset_operations"] >= 1000
)
result = {"passed": True, "milestone_complete": complete, "results": results}
write_json(args.output / "result.json", result)
print(f"[VERIFY] Passed; complete acceptance={complete}; {args.output}/result.json", flush=True)
if __name__ == "__main__":
main()