#!/usr/bin/env python3 """Finite single-leg verification or on-policy rollout in a fresh Kit worker.""" import argparse import json import math import sys import traceback from pathlib import Path from runtime_common import finish_run, write_json def clone_batch(batch): return {name: value.detach().clone() for name, value in batch.items()} def verify(env, args): import torch from SkillFactory.tasks.manager_based.leg_skill.mdp import state from isaaclab.sim import get_current_stage adapter = state(env) stage = get_current_stage() count = sum(1 for _ in stage.Traverse()) initial_anchor = adapter.robot.data.body_link_pose_w.torch[:, adapter.anchor].clone() worst_fk, max_torque = 0.0, 0.0 timeouts = 0 for step in range(args.steps): action = torch.zeros(env.num_envs, env.cfg.max_joints, device=env.device) phase = torch.arange(env.num_envs, device=env.device) * 0.1 + step * env.step_dt * 3.141592653589793 joint_phase = torch.arange(len(adapter.joint_ids), device=env.device) * 0.35 action[:, : len(adapter.joint_ids)] = 0.4 * (phase[:, None] + joint_phase).sin() expected = (adapter.nominal + action[:, : len(adapter.joint_ids)] * adapter.scale).clamp( min=adapter.soft[:, 0], max=adapter.soft[:, 1] ) obs, _, terminated, truncated, info = env.step(adapter.actions(action)) if terminated.any(): raise RuntimeError("Unexpected invalid-state termination") snapshot = adapter.snapshot() for name, value in obs["policy"].items(): assert torch.isfinite(value).all() assert torch.equal(value, snapshot[name]), ("Observation term/snapshot mismatch", name) assert ( (adapter.robot.data.joint_pos_target.torch[:, adapter.joint_ids][~truncated] - expected[~truncated]) .abs() .max() .item() < 1e-6 if (~truncated).any() else True ) assert torch.equal( obs["policy"]["joint_observation"][:, : len(adapter.joint_ids), 2][~truncated], action[:, : len(adapter.joint_ids)][~truncated], ) fk_error = (adapter.fk.foot_position(adapter.q()) - adapter.foot_position()).abs().max().item() worst_fk = max(worst_fk, fk_error) assert fk_error < 1e-4, ("URDF/PhysX frame mismatch", fk_error) torque = (adapter.robot.data.applied_torque.torch[:, adapter.joint_ids] / adapter.effort).abs().max().item() max_torque = max(max_torque, torque) assert torque <= 1.00001 if truncated.any(): assert info["terminal_tokens"] is not None assert len(info["terminal_ids"]) == truncated.sum().item() terminal = info["terminal_tokens"] terminal_q = adapter.nominal + terminal["joint_observation"][:, : len(adapter.joint_ids), 0] * adapter.scale assert ( adapter.fk.foot_position(terminal_q) / adapter.length - terminal["effector_observation"][:, 0, :3] ).abs().max().item() < 1e-4 assert torch.equal( terminal["joint_observation"][:, : len(adapter.joint_ids), 2], action[info["terminal_ids"], : len(adapter.joint_ids)], ) assert obs["policy"]["joint_observation"][truncated].abs().max().item() == 0.0 timeouts += truncated.sum().item() robot = adapter.robot q_before, dq_before, action_before = ( adapter.q().clone(), robot.data.joint_vel.torch[:, adapter.joint_ids].clone(), env.action_manager.action.clone(), ) ids = torch.arange(0, env.num_envs, 2, device=env.device) untouched = torch.arange(1, env.num_envs, 2, device=env.device) old_goal = env.command_manager.get_command("reach").clone() old_time = env._sim_step_counter env.reset(env_ids=ids) assert (adapter.q()[ids] - adapter.nominal).abs().max().item() < 1e-5 assert robot.data.joint_vel.torch[ids].abs().max().item() < 1e-5 assert (robot.data.joint_pos_target.torch[ids][:, adapter.joint_ids] - adapter.nominal).abs().max().item() < 1e-5 assert env.action_manager.action[ids].abs().max().item() == 0.0 assert env.scene["foot_contact"].data.net_forces_w_history.torch[ids].abs().max().item() == 0.0 assert env.episode_length_buf[ids].max().item() == 0 if len(untouched): assert (adapter.q()[untouched] - q_before[untouched]).abs().max().item() <= 1e-6 assert ( robot.data.joint_vel.torch[untouched][:, adapter.joint_ids] - dq_before[untouched] ).abs().max().item() <= 1e-6 assert torch.equal(env.action_manager.action[untouched], action_before[untouched]) assert torch.equal(env.command_manager.get_command("reach")[untouched], old_goal[untouched]) before_empty = clone_batch(adapter.snapshot()) env.reset(env_ids=[]) assert all(torch.equal(v, adapter.snapshot()[k]) for k, v in before_empty.items()) for invalid in ([-1], [env.num_envs], [0, 0], [0.5]): try: env.reset(env_ids=invalid) except ValueError: pass else: raise AssertionError("Invalid reset indices accepted") native_bad = env.action_manager.action.clone() native_bad[:, 0] = torch.nan manager_before = (env.action_manager.action.clone(), env.action_manager.prev_action.clone()) try: env.step(native_bad) except ValueError: pass else: raise AssertionError("Non-finite native action accepted") assert torch.equal(env.action_manager.action, manager_before[0]) assert torch.equal(env.action_manager.prev_action, manager_before[1]) assert env.single_observation_space["policy"]["joint_mask"].dtype.kind == "b" bad = torch.zeros(env.num_envs, env.cfg.max_joints, device=env.device) bad[:, 0] = torch.nan try: adapter.actions(bad) except ValueError: pass else: raise AssertionError("Non-finite active action accepted") padded = torch.zeros_like(bad) padded[:, len(adapter.joint_ids) :] = torch.nan assert torch.isfinite(adapter.actions(padded)).all() assert env._sim_step_counter == old_time assert sum(1 for _ in stage.Traverse()) == count drift = (robot.data.body_link_pose_w.torch[:, adapter.anchor] - initial_anchor).abs().max().item() assert drift <= 1e-4 return { "passed": True, "mode": "verify", "part_id": adapter.bundle["spec"]["part_id"], "num_envs": env.num_envs, "steps": args.steps, "physics_dt": env.physics_dt, "control_dt": env.step_dt, "joint_order": adapter.bundle["joint_order"], "max_fk_error_m": worst_fk, "max_normalized_applied_torque": max_torque, "fixed_anchor_pose_error": drift, "timeouts_checked": timeouts, "indexed_reset": True, "empty_reset_noop": True, "stage_prim_count": count, "policy_shapes": {k: list(v.shape) for k, v in adapter.snapshot().items()}, "trained_skill": False, } def rollout(env, args): import torch from SkillFactory.legs.policy import load_policy from SkillFactory.tasks.manager_based.leg_skill.mdp import state if args.weights is None: raise ValueError("rollout requires --weights") model = load_policy(args.weights, env.device).eval() adapter = state(env) observation, _ = env.reset(seed=args.seed) snapshots, actions, logs, values, rewards, next_values, done = [], [], [], [], [], [], [] with torch.no_grad(): for _ in range(args.horizon): batch = clone_batch(observation["policy"]) action, log_prob, value = model.act(batch) observation, reward, terminated, truncated, info = env.step(adapter.actions(action)) if terminated.any(): raise RuntimeError("Invalid physical state during rollout") nxt = model.value(observation["policy"]) if truncated.any(): terminal_value = model.value(info["terminal_tokens"]) ids = info["terminal_ids"] nxt[ids] = torch.where(truncated[ids], terminal_value, nxt[ids]) nxt[terminated] = 0.0 assert torch.isfinite(reward).all() and torch.isfinite(nxt).all() assert all(torch.isfinite(v).all() for v in observation["policy"].values()) snapshots.append(batch) actions.append(action) logs.append(log_prob) values.append(value) rewards.append(reward.clone()) next_values.append(nxt) done.append((terminated | truncated).clone()) data = {"observation": {name: torch.stack([batch[name] for batch in snapshots]).cpu() for name in snapshots[0]}} for name, items in { "actions": actions, "log_prob": logs, "values": values, "rewards": rewards, "next_values": next_values, "done": done, }.items(): data[name] = torch.stack(items).cpu() torch.save(data, args.output / "rollout.pt") return { "passed": True, "mode": "rollout", "part_id": adapter.bundle["spec"]["part_id"], "horizon": args.horizon, "num_envs": env.num_envs, "mean_reward_per_control_step": data["rewards"].mean().item(), "episode_end_count": data["done"].sum().item(), "source": adapter.bundle["source"], "trained_skill": False, } def contact_trial(env, args): import torch from SkillFactory.tasks.manager_based.leg_skill.mdp import state adapter = state(env) press = [float(v) for v in args.contact_action.split(",")] if len(press) != len(adapter.joint_ids): raise ValueError("contact-action must have one value per declared actuator") action = torch.zeros(env.num_envs, env.cfg.max_joints, device=env.device) action[:, : len(press)] = torch.tensor(press, device=env.device) rows = {} for phase, sign in (("air", -1.0), ("press", 1.0)): forces, gaps = [], [] for step in range(100): env.step(adapter.actions(action * sign)) if step >= 70: forces.append(env.scene["foot_contact"].data.force_matrix_w.torch[:, 0, 0].clone()) gaps.append(adapter.gap().clone()) force, gap = torch.stack(forces), torch.stack(gaps) rows[phase] = { "mean_fz_N": force[:, :, 2].mean(dim=0).min().item(), "max_force_N": force.norm(dim=-1).max().item(), "min_gap_m": gap.min().item(), } if phase == "air": assert gap.min().item() >= 0.01 and force.norm(dim=-1).max().item() < 0.1, rows else: assert force[:, :, 2].mean(dim=0).min().item() > 1.0 and gap.min().item() >= -0.005, rows ids = torch.arange(0, env.num_envs, 2, device=env.device) env.reset(env_ids=ids) assert env.scene["foot_contact"].data.net_forces_w_history.torch[ids].abs().max().item() == 0.0 return rows def main(): if not __debug__: raise RuntimeError("Worker checks require Python without -O") from isaaclab.app import AppLauncher from isaaclab_tasks.utils import add_launcher_args, launch_simulation parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("--bundle", type=Path, required=True) parser.add_argument("--mode", choices=("verify", "rollout"), default="verify") parser.add_argument("--num_envs", type=int, default=16) parser.add_argument("--max-joints", type=int, default=8) parser.add_argument("--steps", type=int, default=300) parser.add_argument("--horizon", type=int, default=32) parser.add_argument("--episode-length", type=float, default=0.2) parser.add_argument("--weights", type=Path) parser.add_argument("--contact-action", help="Optional configured press action, e.g. 0,0,1; verify only") parser.add_argument("--seed", type=int, default=42) parser.add_argument("--output", type=Path, default=Path("logs/leg_interface/verify")) add_launcher_args(parser) args = parser.parse_args() if ( args.num_envs < 1 or args.max_joints < 1 or args.steps < 10 or args.horizon < 1 or not math.isfinite(args.episode_length) or args.episode_length <= 0 ): parser.error("Require positive finite counts/duration and steps>=10") if args.mode == "rollout" and args.weights is None: parser.error("rollout requires --weights") if args.contact_action is not None: if args.mode != "verify": parser.error("contact-action is for verification only") try: press = [float(value) for value in args.contact_action.split(",")] except ValueError: parser.error("contact-action requires comma-separated finite numbers") if not press or not all(math.isfinite(value) for value in press): parser.error("contact-action requires comma-separated finite numbers") args.output.mkdir(parents=True, exist_ok=True) result = {"passed": False, "status": "running"} write_json(args.output / "result.json", result) app = AppLauncher(args).app code, env = 0, None try: import gymnasium as gym import SkillFactory.tasks # noqa: F401 import torch from SkillFactory.tasks.manager_based.leg_skill.leg_env_cfg import LegEnvCfg from pxr import UsdPhysics from isaaclab.sim import get_current_stage cfg = LegEnvCfg(bundle_dir=str(args.bundle.resolve()), max_joints=args.max_joints) cfg.scene.num_envs = args.num_envs cfg.seed = args.seed cfg.episode_length_s = args.episode_length if args.contact_action is not None: from SkillFactory.legs.urdf import forward, multiply, pose bundle = cfg.leg_bundle nominal = {j["name"]: j["q_default"] for j in bundle["spec"]["joints"]} poses, _ = forward(bundle["tree"], nominal) foot = bundle["foot"] collision = multiply(poses[foot["body"]], pose(foot["xyz"], foot["rpy"])) extent = ( foot["radius"] if foot["type"] == "sphere" else sum(abs(collision[2][i]) * foot["size"][i] / 2 for i in range(3)) ) cfg.scene.robot.init_state.pos = (0.0, 0.0, extent - collision[2][3] + 0.01) cfg.terminations.time_out = None with launch_simulation(cfg, args): torch.manual_seed(args.seed) torch.set_float32_matmul_precision("highest") env = gym.make("SkillFactory-LegSkill-v0", cfg=cfg).unwrapped env.reset(seed=args.seed) stage = get_current_stage() grounds = [ str(p.GetPath()) for p in stage.Traverse() if str(p.GetPath()).startswith("/World/ground/") and p.HasAPI(UsdPhysics.CollisionAPI) ] assert grounds == cfg.scene.foot_contact.filter_prim_paths_expr assert env.scene["foot_contact"].num_sensors == 1 assert env.scene["foot_contact"].data.force_matrix_w.torch.shape == (args.num_envs, 1, 1, 3) result = verify(env, args) if args.mode == "verify" else rollout(env, args) if args.contact_action is not None: result["contact_trial"] = contact_trial(env, args) masses = dict(zip(env.scene["robot"].body_names, env.scene["robot"].data.body_mass.torch[0].tolist())) result["effective_body_mass_kg"] = masses result["manifest_sha256"] = cfg.leg_bundle["conversion"]["manifest_sha256"] result["seed"] = args.seed result["device"] = env.device print(json.dumps(result, indent=2)) except BaseException: error = traceback.format_exc() print(error, file=sys.stderr, flush=True) result = {"passed": False, "error": error} code = 1 finally: finish_run(app, env, args.output / "result.json", result, code) if __name__ == "__main__": main()