7f39abb613
Add declarative single-leg import, fixed-anchor benchmark, masked shared-policy PPO prototype, regression tests, and consolidated documentation.
360 lines
16 KiB
Python
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()
|