Files
SkillFactory/scripts/verify_loaded_leg.py
T
chenlin 093ab883be feat: release guided Go2 standing skill with explicit drift waiver
Add loaded-standing physics, PPO training, fixed-candidate acceptance, isolated inference, and fail-closed publication. Preserve the original 120-second drift failure and bind the operator-approved exception to the exact candidate and evidence.

Include pending Reach training/release infrastructure and archived packages. Leave pre-existing short-worker visualization changes and large generated training artifacts out of this commit.
2026-10-12 17:26:03 +08:00

388 lines
18 KiB
Python

#!/usr/bin/env python3
"""Real-physics loaded-standing prototype validation, never a learned-policy acceptance."""
import argparse
import json
import math
import sys
import traceback
from pathlib import Path
from runtime_common import finish_run, write_json
def evaluate(env, args):
import torch
from SkillFactory.legs.support import CONTRACT, negative_control_collapsed
from SkillFactory.legs.tokens import LegKinematics
from SkillFactory.tasks.manager_based.loaded_leg.mdp import state
from isaaclab.utils.math import quat_apply, quat_apply_inverse
s, cfg = state(env), env.cfg.support_parameters
robot, count = s.robot, env.num_envs
if robot.is_fixed_base:
raise RuntimeError("Standing cannot use a world-fixed root")
mass = robot.data.body_mass.torch[0]
total_mass = mass.sum().item()
nominal = s.nominal.expand(count, -1)
fk = LegKinematics(s.bundle, env.device)
columns = []
for j in range(3):
delta = torch.zeros_like(nominal)
delta[:, j] = 0.0001
columns.append((fk.foot_position(nominal + delta) - fk.foot_position(nominal - delta)) / 0.0002)
jacobian = torch.stack(columns, -1)
inverse = torch.linalg.pinv(jacobian)
command_q = nominal.clone()
target_pos = env.scene.env_origins.clone()
target_pos[:, 2] += env.cfg.anchor_height
limits = torch.tensor([[j["limits"]["lower"], j["limits"]["upper"]] for j in s.bundle["joints"]], device=env.device)
speed_limit = torch.tensor([j["limits"]["velocity"] for j in s.bundle["joints"]], device=env.device)
stats = {
k: torch.zeros(count, device=env.device)
for k in (
"max_height_error_m",
"max_base_drop_m",
"max_tilt_rad",
"max_foot_drift_m",
"contact_samples",
"sum_fz_N",
"sum_required_fz_N",
"max_normalized_torque",
"max_normalized_velocity",
"max_joint_limit_excess_rad",
"max_guide_fz_N",
)
}
sample_count, all_count = 0, 0
calibration_windows = ((2.0, 3.0), (4.5, 5.5), (9.5, 10.5))
calibration_counts = [0, 0, 0]
sensor_sum = torch.zeros(3, count, 6, device=env.device)
expected_sum = torch.zeros_like(sensor_sum)
foot_origin = None
peak_load = torch.zeros(count, 6, device=env.device)
min_load, max_load = torch.zeros_like(peak_load), torch.zeros_like(peak_load)
original_update = env.scene.update
steps = round(args.duration / env.step_dt)
physics_trace = {
name: torch.empty(steps * env.cfg.decimation, count, width, device=env.device)
for name, width in (
("guide_wrench_world", 6),
("disturbance_world", 6),
("mount_wrench_world", 6),
("base_state_world", 13),
("foot_normal_force_world", 3),
("joint_pos", 3),
("joint_vel", 3),
("applied_torque", 3),
)
}
def observe(dt):
nonlocal sample_count, all_count, foot_origin
original_update(dt)
all_count += 1
t = all_count * env.physics_dt
pos, quat, vel, omega = s.root()
wrench, force, foot = s.mount_world(), s.contact(), s.foot_center()
values = [robot.data.root_state_w.torch, s.q(), robot.data.joint_vel.torch, wrench, force]
for name, value in (
("guide_wrench_world", s.last_guide),
("disturbance_world", s.last_disturbance),
("mount_wrench_world", wrench),
("base_state_world", torch.cat((pos, quat, vel, omega), -1)),
("foot_normal_force_world", force),
("joint_pos", s.q()),
("joint_vel", robot.data.joint_vel.torch[:, s.joints]),
("applied_torque", robot.data.applied_torque.torch[:, s.joints]),
):
physics_trace[name][all_count - 1].copy_(value)
if any(not torch.isfinite(v).all() for v in values):
raise RuntimeError("Non-finite physical/sensor state")
torque = (robot.data.applied_torque.torch[:, s.joints] / s.effort).abs().amax(-1)
velocity = (robot.data.joint_vel.torch[:, s.joints] / speed_limit).abs().amax(-1)
excess = torch.maximum(limits[:, 0] - s.q(), s.q() - limits[:, 1]).clamp(min=0).amax(-1)
for key, value in (
("max_normalized_torque", torque),
("max_normalized_velocity", velocity),
("max_joint_limit_excess_rad", excess),
("max_guide_fz_N", s.last_guide[:, 2].abs()),
("max_base_drop_m", (env.cfg.anchor_height - pos[:, 2]).clamp(min=0)),
):
stats[key] = torch.maximum(stats[key], value)
peak_load[:] = torch.maximum(peak_load, s.last_disturbance.abs())
min_load[:] = torch.minimum(min_load, s.last_disturbance)
max_load[:] = torch.maximum(max_load, s.last_disturbance)
if t >= cfg["warmup_seconds"]:
sample_count += 1
if foot_origin is None:
foot_origin = foot.clone()
for key, value in (
("max_height_error_m", (pos[:, 2] - env.cfg.anchor_height).abs()),
("max_tilt_rad", s.tilt()),
("max_foot_drift_m", (foot[:, :2] - foot_origin[:, :2]).norm(dim=-1)),
):
stats[key] = torch.maximum(stats[key], value)
stats["contact_samples"] += (force[:, 2] > total_mass * 9.81 * 0.2).float()
stats["sum_fz_N"] += force[:, 2]
stats["sum_required_fz_N"] += total_mass * 9.81 - s.last_disturbance[:, 2]
window = next((i for i, (a, b) in enumerate(calibration_windows) if a <= t < b), None)
if window is not None:
gravity = torch.zeros(count, 3, device=env.device)
gravity[:, 2] = -mass[s.payload] * 9.81
lever = quat_apply(quat, torch.tensor(cfg["payload_com_m"], device=env.device).expand(count, -1))
expected = torch.cat((gravity, torch.cross(lever, gravity, dim=-1)), -1) + s.last_guide + s.last_disturbance
sensor_sum[window].add_(wrench)
expected_sum[window].add_(expected)
calibration_counts[window] += 1
if window == 0 and calibration_counts[window] == 400:
import warp as wp
write_json(
args.output / "sensor_diagnostic.json",
{
"sensor_body_names": s.sensor.body_names,
"robot_body_names": robot.body_names,
"sensor_index": s.sensor_index,
"body_pose_w": robot.data.body_link_pose_w.torch[0].tolist(),
"force_joint": s.sensor.data.force.torch[0].tolist(),
"torque_joint": s.sensor.data.torque.torch[0].tolist(),
"joint_pos_b": wp.to_torch(s.sensor._joint_pos_b).tolist(),
"joint_quat_b_xyzw": wp.to_torch(s.sensor._joint_quat_b).tolist(),
"root_twist": torch.cat((vel, omega), -1).tolist(),
"expected_mount_wrench_world": expected.tolist(),
},
)
env.scene.update = observe
trace = {
name: [] for name in ("base_state", "mount_wrench", "guide_wrench", "foot_state", "disturbance_world", "action")
}
try:
for i in range(steps):
if args.controller == "height":
pos, quat, vel, _ = s.root()
error = quat_apply_inverse(quat, target_pos - pos - 0.12 * vel)
dq = -(inverse @ error[:, :, None]).squeeze(-1)
command_q += 2.0 * env.step_dt * dq
command_q.clamp_(min=nominal - cfg["action_scale_rad"], max=nominal + cfg["action_scale_rad"])
action = (command_q - nominal) / cfg["action_scale_rad"]
else:
action = torch.zeros(count, 3, device=env.device)
obs, _, terminated, truncated, _ = env.step(action)
if terminated.any() or truncated.any():
raise RuntimeError("Verification must not auto-reset")
for key in ("base_state", "mount_wrench", "guide_wrench", "foot_state"):
trace[key].append(obs["policy"][key].clone().cpu())
trace["disturbance_world"].append(s.last_disturbance.clone().cpu())
trace["action"].append(action.clone().cpu())
finally:
env.scene.update = original_update
torch.save({k: torch.stack(v) if v else [] for k, v in trace.items()}, args.output / "trace.pt")
torch.save({k: v[:all_count].cpu() for k, v in physics_trace.items()}, args.output / "physics_trace.pt")
n = max(sample_count, 1)
rows = []
for i in range(count):
row = {key: value[i].item() for key, value in stats.items() if key.startswith("max_")}
row.update(
scenario=int(s.scenarios[i]),
contact_fraction=stats["contact_samples"][i].item() / n,
mean_foot_fz_N=stats["sum_fz_N"][i].item() / n,
mean_required_fz_N=stats["sum_required_fz_N"][i].item() / n,
applied_peak_wrench=peak_load[i].tolist(),
applied_min_wrench=min_load[i].tolist(),
applied_max_wrench=max_load[i].tolist(),
)
row["vertical_balance_relative_error"] = (
abs(row["mean_foot_fz_N"] - row["mean_required_fz_N"]) / row["mean_required_fz_N"]
)
rows.append(row)
denominators = torch.tensor(calibration_counts, device=env.device).clamp(min=1)[:, None, None]
sensor_mean = sensor_sum / denominators
expected_mean = expected_sum / denominators
residual = (sensor_mean - expected_mean).abs()
covered = True
for row in rows:
expected_min, expected_max = [0.0] * 6, [0.0] * 6
scenario = row["scenario"]
if scenario:
axis = (scenario - 1) // 2
if scenario % 2:
expected_max[axis] = cfg["disturbance_amplitude"][axis]
else:
expected_min[axis] = -cfg["disturbance_amplitude"][axis]
covered &= row["applied_min_wrench"] == expected_min and row["applied_max_wrench"] == expected_max
gates = {
"floating_root": not robot.is_fixed_base,
"physical_steps": all_count == steps * env.cfg.decimation,
"signed_disturbance_coverage": covered,
"full_protocol": count >= 13
and args.duration >= cfg["episode_seconds"]
and set(s.scenarios.tolist()) == set(range(13)),
"no_vertical_guide": all(r["max_guide_fz_N"] == 0 for r in rows),
"height": all(r["max_height_error_m"] <= cfg["height_error_limit_m"] for r in rows),
"tilt": all(r["max_tilt_rad"] <= cfg["tilt_limit_rad"] for r in rows),
"foot_drift": all(r["max_foot_drift_m"] <= cfg["foot_drift_limit_m"] for r in rows),
"contact": all(r["contact_fraction"] >= cfg["contact_fraction_min"] for r in rows),
"vertical_balance": all(
r["vertical_balance_relative_error"] <= cfg["vertical_balance_relative_tolerance"] for r in rows
),
"limits": all(
r["max_normalized_torque"] <= cfg["normalized_effort_velocity_limit"]
and r["max_normalized_velocity"] <= cfg["normalized_effort_velocity_limit"]
and r["max_joint_limit_excess_rad"] <= cfg["joint_limit_excess_rad"]
for r in rows
),
"sensor_static_force": min(calibration_counts) >= 499
and residual[..., :3].max().item() <= cfg["sensor_static_force_tolerance_N"],
"sensor_static_torque": min(calibration_counts) >= 499
and residual[..., 3:].max().item() <= cfg["sensor_static_torque_tolerance_Nm"],
}
# Indexed/empty reset contract, outside all physics statistics.
before = s.robot.data.root_state_w.torch.clone()
before_q = s.q().clone()
ids = torch.arange(0, count, 2, device=env.device)
others = torch.arange(1, count, 2, device=env.device)
env.reset(env_ids=ids)
reset_ok = (
s.mount_wrench()[ids].abs().max().item() == 0
and s.last_guide[ids].abs().max().item() == 0
and env.action_manager.action[ids].abs().max().item() == 0
)
if len(others):
reset_ok &= torch.equal(before[others], s.robot.data.root_state_w.torch[others]) and torch.equal(
before_q[others], s.q()[others]
)
snapshot = {k: v.clone() for k, v in s.snapshot().items()}
env.reset(env_ids=[])
reset_ok &= all(torch.equal(v, s.snapshot()[k]) for k, v in snapshot.items())
gates["indexed_reset"] = bool(reset_ok)
# A separate short real timeout probe exercises pre-reset terminal observations.
env.cfg.episode_length_s = 3 * env.step_dt
env.reset()
probe_action = torch.full((count, 3), 0.2, device=env.device)
for _ in range(3):
obs, _, term, timeout, info = env.step(probe_action)
gates["timeout_terminal_reset"] = bool(
not term.any()
and timeout.all()
and len(info["terminal_ids"]) == count
and torch.equal(info["terminal_observation"]["joint_state"][:, :, 2], probe_action)
and (obs["policy"]["joint_state"][:, :, 2] == 0).all()
and (obs["policy"]["mount_wrench"] == 0).all()
)
old_actions = (env.action_manager.action.clone(), env.action_manager.prev_action.clone())
rejected = 0
for bad in (torch.zeros(count, 4, device=env.device), torch.full((count, 3), torch.nan, device=env.device)):
try:
env.step(bad)
except ValueError:
rejected += 1
gates["bad_action_atomic_rejection"] = (
rejected == 2
and torch.equal(old_actions[0], env.action_manager.action)
and torch.equal(old_actions[1], env.action_manager.prev_action)
)
passed = all(gates.values())
return {
"passed": passed,
"contract": CONTRACT,
"trained_skill": False,
"controller": args.controller,
"guide_enabled": env.cfg.guide_enabled,
"gates": gates,
"num_envs": count,
"duration_s": args.duration,
"physics_dt": env.physics_dt,
"control_dt": env.step_dt,
"physical_samples": all_count,
"settled_physical_samples": sample_count,
"rows": rows,
"sensor_static_measured_world": sensor_mean.tolist(),
"sensor_static_expected_world": expected_mean.tolist(),
"calibration_windows_s": calibration_windows,
"calibration_samples": calibration_counts,
"sensor_static_max_force_error_N": residual[..., :3].max().item(),
"sensor_static_max_torque_error_Nm": residual[..., 3:].max().item(),
"solver": "PGS" if env.cfg.sim.physics.solver_type == 0 else "TGS",
"device": env.device,
"effective_mass_kg": dict(zip(robot.body_names, mass.tolist())),
"total_mass_kg": total_mass,
"sensor_convention": "parent-on-child, joint frame, mount origin; untared N/Nm",
"missing_source_inertial_links": env.cfg.support_manifest["missing_source_inertial_links"],
"no_autoresets_in_physics_trial": True,
"timeout_probe_control_steps": 3,
"negative_control_collapsed": args.controller == "disabled" and negative_control_collapsed(rows),
}
def main():
if not __debug__:
raise RuntimeError("Verification requires assertions enabled")
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/standing/go2_fl_support_v0.json"))
parser.add_argument("--num_envs", type=int, default=13)
parser.add_argument("--duration", type=float, default=14)
parser.add_argument("--controller", choices=("height", "hold", "disabled"), default="height")
parser.add_argument("--free-base", action="store_true", help="Disable passive XY/rotation guidance; experimental")
parser.add_argument("--scenario-offset", type=int, default=0)
parser.add_argument("--output", type=Path, required=True)
add_launcher_args(parser)
args = parser.parse_args()
if args.num_envs < 1 or not math.isfinite(args.duration) or args.duration <= 0:
parser.error("Require positive environments/duration")
args.output.mkdir(parents=True, exist_ok=False)
write_json(args.output / "result.json", {"passed": False, "status": "running"})
app = AppLauncher(args).app
env, code, result = None, 1, {"passed": False}
try:
import gymnasium as gym
import SkillFactory.tasks # noqa: F401
from SkillFactory.legs.support import sha256
from SkillFactory.tasks.manager_based.loaded_leg.cfg import LoadedLegEnvCfg
cfg = LoadedLegEnvCfg(support_config=str(args.config.resolve()))
cfg.scene.num_envs, cfg.guide_enabled = args.num_envs, not args.free_base
cfg.scenario_offset = args.scenario_offset
cfg.episode_length_s = args.duration + 1.0 # Keep time_out registered for the later reset probe.
cfg.terminations.fallen = None
cfg.terminations.invalid = None # Per-physics audit raises instead, never auto-reset failed states.
if args.controller == "disabled":
cfg.scene.robot.actuators["leg"].stiffness = 0.0
cfg.scene.robot.actuators["leg"].damping = 0.0
with launch_simulation(cfg, args):
env = gym.make("SkillFactory-LoadedLegStand-v0", cfg=cfg).unwrapped
env.reset(seed=cfg.seed)
result = evaluate(env, args)
result.update(
config_sha256=sha256(args.config),
support_manifest_sha256=sha256(Path(cfg.support_parameters["support_asset"]) / "support.json"),
)
write_json(args.output / "config.json", cfg.support_parameters)
print(
json.dumps(
{
k: v
for k, v in result.items()
if k not in ("rows", "sensor_static_measured_world", "sensor_static_expected_world")
},
indent=2,
)
)
code = 0 if result["passed"] else 2
except BaseException:
error = traceback.format_exc()
print(error, file=sys.stderr, flush=True)
result = {"passed": False, "error": error}
finally:
finish_run(app, env, args.output / "result.json", result, code)
if __name__ == "__main__":
main()