#!/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()