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.
388 lines
18 KiB
Python
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()
|