7f39abb613
Add declarative single-leg import, fixed-anchor benchmark, masked shared-policy PPO prototype, regression tests, and consolidated documentation.
495 lines
20 KiB
Python
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()
|