"""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, }