Files
SkillFactory/scripts/standing_protocol.py
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

182 lines
8.3 KiB
Python

"""Fixed standing evaluation inputs and independent CPU raw-trace reductions; no Kit."""
import itertools
import torch
from SkillFactory.legs.standing_ppo import canonical_hash
MODES = {
"holdout": (256, 14.0, 83000),
"full": (13, 14.0, 71000),
"stability": (26, 120.0, 85000),
"exploratory": (60, 14.0, 84000),
}
WIDTHS = {"base": 13, "foot": 3, "contact": 3, "mount": 6, "guide": 6, "load": 6, "q": 3, "dq": 3, "torque": 3}
def inputs_for(mode):
count, duration, seed = MODES[mode]
gen = torch.Generator().manual_seed(seed)
basis = torch.zeros(count, 6)
scale, shift = torch.ones(count), torch.zeros(count)
if mode == "exploratory":
for i, ((a, b), (sa, sb)) in enumerate(
itertools.product(itertools.combinations(range(6), 2), itertools.product((-1, 1), repeat=2))
):
basis[i, a], basis[i, b] = sa, sb
else:
scenarios = torch.arange(count) % 13
if mode == "holdout":
scenarios = scenarios[torch.randperm(count, generator=gen)]
scale = 0.6 + 0.4 * torch.rand(count, generator=gen)
shift = -0.25 + 0.5 * torch.rand(count, generator=gen)
mask = scenarios != 0
basis[torch.arange(count)[mask], (scenarios[mask] - 1) // 2] = torch.where(scenarios[mask] % 2 == 1, 1.0, -1.0)
return {
"mode": mode,
"count": count,
"duration_s": duration,
"seed": seed,
"basis": basis.tolist(),
"scale": scale.tolist(),
"shift_s": shift.tolist(),
"period_s": 14.0,
}
def load_at(time, basis, scale, shift, amplitude):
# time may be scalar or [steps,1]; trailing dimensions broadcast against environments.
phase = time.remainder(14.0) - shift
env = torch.zeros_like(phase)
for start in (3.0, 8.0):
env += torch.minimum((phase - start) / 0.5, (start + 3.0 - phase) / 0.5).clamp(0, 1)
return env[..., None] * scale[..., None] * basis * amplitude
def rotate_xyzw(q, v):
cross = torch.cross(q[..., :3], v, dim=-1)
return v + 2 * (q[..., 3:] * cross + torch.cross(q[..., :3], cross, dim=-1))
def calibration_errors(measured, expected, duration):
"""Never pool cycles: a bad plateau must not cancel or dilute."""
residuals, counts = [], []
for cycle in range(int(duration // 14) + 1):
for start, end in ((2.0, 3.0), (4.5, 5.5), (9.5, 10.5)):
a, b = cycle * 14 + start, cycle * 14 + end
if b > duration:
continue
lo, hi = round(a / 0.002) - 1, round(b / 0.002) - 1
residuals.append((measured[lo:hi].double().mean(0) - expected[lo:hi].double().mean(0)).abs())
counts.append(len(measured[lo:hi]))
return torch.stack(residuals), counts
def reduce_trace(data, meta, cfg):
"""Recompute safety/standing/calibration/coverage from tensors, not reported booleans."""
inputs = meta["inputs"]
n, count = round(inputs["duration_s"] / 0.002), inputs["count"]
if set(data) != set(WIDTHS) or any(
v.shape != (n, count, WIDTHS[k]) or v.dtype != torch.float32 or not torch.isfinite(v).all()
for k, v in data.items()
):
raise ValueError("Incomplete/nonfinite physical trace")
if inputs != inputs_for(inputs["mode"]) or meta["input_hash"] != canonical_hash(inputs):
raise ValueError("Protocol input mismatch")
if meta["physics_dt"] != 0.002 or meta["control_dt"] != 0.01:
raise ValueError("Unexpected simulation clocks")
times = torch.arange(1, n + 1, dtype=torch.float64) * 0.002
settled = times >= cfg["warmup_seconds"]
base = data["base"].double()
quat = base[..., 3:7]
up = torch.zeros_like(quat[..., :3])
up[..., 2] = 1
tilt = torch.acos(rotate_xyzw(quat, up)[..., 2].clamp(-1, 1))
error = (base[..., 2] - meta["height_m"]).abs()
foot = data["foot"][settled].double()
drift = (foot[..., :2] - foot[0, :, :2]).norm(dim=-1)
contact = data["contact"][settled, :, 2].double()
required = meta["total_mass_kg"] * 9.81 - data["load"][settled, :, 2].double()
limits, speeds, efforts = [torch.tensor(meta[k], dtype=torch.float64) for k in ("limits", "speeds", "efforts")]
q = data["q"].double()
excess = torch.maximum(limits[:, 0] - q, q - limits[:, 1]).clamp(min=0)
# Reconstruct entire applied load sequence independently of the simulator hook.
ctrl_t = (torch.arange(n) // 5).float() * 0.01
expected_load = load_at(
ctrl_t[:, None],
torch.tensor(inputs["basis"]),
torch.tensor(inputs["scale"]),
torch.tensor(inputs["shift_s"]),
torch.tensor(cfg["disturbance_amplitude"]),
)
load_error = (expected_load - data["load"]).abs().max().item()
metrics = {
"mean_height_error_m": error[settled].mean(0),
"max_height_error_m": error[settled].amax(0),
"max_height_error_all_m": error.amax(0),
"max_tilt_rad": tilt[settled].amax(0),
"max_foot_drift_m": drift.amax(0),
"contact_fraction": (contact > meta["total_mass_kg"] * 9.81 * 0.2).double().mean(0),
"vertical_balance_relative_error": (contact.mean(0) - required.mean(0)).abs() / required.mean(0),
"max_normalized_torque": (data["torque"].double() / efforts).abs().amax((0, 2)),
"max_normalized_velocity": (data["dq"].double() / speeds).abs().amax((0, 2)),
"max_joint_limit_excess_rad": excess.amax((0, 2)),
"max_guide_fz_N": data["guide"][..., 2].abs().amax(0).double(),
"fallen": ((base[..., 2] < meta["height_m"] - 0.10) | (tilt > torch.pi * 35 / 180)).any(0),
}
gravity = torch.zeros_like(base[..., :3])
gravity[..., 2] = -cfg["payload_mass_kg"] * 9.81
com = torch.tensor(cfg["payload_com_m"], dtype=torch.float64).expand_as(gravity)
lever = rotate_xyzw(quat, com)
oracle = (
torch.cat((gravity, torch.cross(lever, gravity, dim=-1)), -1) + data["guide"].double() + data["load"].double()
)
residual, counts = calibration_errors(data["mount"], oracle, inputs["duration_s"])
force, torque = residual[..., :3].max().item(), residual[..., 3:].max().item()
gates = {
"finite_complete_trace": True,
"floating_root": meta["floating_root"] is True,
"pgs_compatible": meta["solver"] == "PGS" and meta["tgs_force_flag"] is False,
"no_autoreset": meta["autoresets"] == 0,
"no_fall": not metrics["fallen"].any().item(),
"applied_load_matches_protocol": load_error <= 1e-4,
"quaternion_norm": (quat.norm(dim=-1) - 1).abs().max().item() <= 0.002,
"no_vertical_guide": metrics["max_guide_fz_N"].max().item() == 0,
}
for name, key in (
("height", "height_error_limit_m"),
("tilt", "tilt_limit_rad"),
("foot_drift", "foot_drift_limit_m"),
):
field = {"height": "max_height_error_m", "tilt": "max_tilt_rad", "foot_drift": "max_foot_drift_m"}[name]
gates[name] = bool((metrics[field] <= cfg[key]).all())
gates["contact"] = bool((metrics["contact_fraction"] >= cfg["contact_fraction_min"]).all())
gates["vertical_balance"] = bool(
(metrics["vertical_balance_relative_error"] <= cfg["vertical_balance_relative_tolerance"]).all()
)
gates["limits"] = all(
bool((metrics[k] <= cfg[limit]).all())
for k, limit in (
("max_normalized_torque", "normalized_effort_velocity_limit"),
("max_normalized_velocity", "normalized_effort_velocity_limit"),
("max_joint_limit_excess_rad", "joint_limit_excess_rad"),
)
)
# Quasi-static metrology gate only on fixed full-amplitude plateaus, not shifted transient windows.
if inputs["mode"] in ("full", "stability"):
gates.update(
sensor_static_force=min(counts) >= 499 and force <= cfg["sensor_static_force_tolerance_N"],
sensor_static_torque=min(counts) >= 499 and torque <= cfg["sensor_static_torque_tolerance_Nm"],
)
return {
"passed": all(gates.values()),
"gates": gates,
"physical_samples": n,
"mean_height_error_m": metrics["mean_height_error_m"].mean().item(),
"rows": [{k: v[i].item() for k, v in metrics.items()} for i in range(count)],
"load_reconstruction_max_error": load_error,
"calibration_counts": counts,
"sensor_static_max_force_error_N": force,
"sensor_static_max_torque_error_Nm": torque,
}