093ab883be
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.
182 lines
8.3 KiB
Python
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,
|
|
}
|