Files
SkillFactory/scripts/leg_worker.py
T
chenlin 7f39abb613 release: SkillFactory v0.0.1
Add declarative single-leg import, fixed-anchor benchmark, masked shared-policy PPO prototype, regression tests, and consolidated documentation.
2026-10-10 09:58:30 +08:00

360 lines
16 KiB
Python

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