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