Files

189 lines
10 KiB
Python

#!/usr/bin/env python3
"""Stage a completed RP1 BeyondMimic STUDENT actor + reference. Never starts training.
Use roboparty_train/.venv/bin/python. Stages skills disabled; enable only after
real physics playback/transition validation. Trusted local checkpoints only.
"""
import argparse
import hashlib
import json
from pathlib import Path
import re
import numpy as np
import onnx
from onnx.reference import ReferenceEvaluator
import torch
import yaml
def sha(path):
return hashlib.sha256(path.read_bytes()).hexdigest()
def dump(path, value):
temp = path.with_suffix(path.suffix + ".tmp")
temp.write_text(json.dumps(value, indent=2, ensure_ascii=False, allow_nan=False) + "\n")
temp.replace(path)
def parameter(value, name):
if not isinstance(value, dict):
return float(value)
matches = [float(v) for p, v in value.items() if re.fullmatch(p, name)]
if len(matches) != 1:
raise ValueError(f"Actuator parameter is ambiguous/missing: {name}")
return matches[0]
def main():
p = argparse.ArgumentParser(description=__doc__)
p.add_argument("--checkpoint", type=Path, required=True)
p.add_argument("--skill", choices=["dance", "wave"], required=True)
p.add_argument("--duration", type=float, help="Episode seconds; dance defaults to the trained 10s horizon, wave to its full clip")
p.add_argument("--package", type=Path, default=Path(__file__).resolve().parents[1] / "public/robots/rp1")
args = p.parse_args()
cp, target, skill_id = args.checkpoint.resolve(), args.package.resolve(), args.skill
params = cp.parent / "params"
env = yaml.load((params / "env.yaml").read_text(), Loader=yaml.BaseLoader)
agent = yaml.load((params / "agent.yaml").read_text(), Loader=yaml.BaseLoader)
config = json.loads((target / "robot.json").read_text())
robot, group = env["scene"]["robot"], env["observations"]["student"]
names = config["joints"]["groups"]["policy"]
action = env["actions"]["joint_pos"]
expected_terms = ["command", "base_ang_vel", "projected_gravity", "joint_pos", "joint_vel", "actions"]
expected_funcs = ["generated_commands", "base_ang_vel", "projected_gravity", "joint_pos_rel", "joint_vel_rel", "last_action"]
terms = [(k, v) for k, v in group.items() if isinstance(v, dict) and "func" in v]
if not (
agent["obs_groups"]["actor"] == ["student"]
and group["history_length"] == "1"
and group["concatenate_terms"] == "true"
and [k for k, _ in terms] == expected_terms
and [v["func"].split(":")[-1] for _, v in terms] == expected_funcs
and all(v.get("scale") == "null" and v.get("clip") == "null"
and v.get("modifiers") == "null" and v.get("delay_max_lag") == "0" for _, v in terms)
and agent["actor"]["activation"] == "elu"
and agent["actor"]["obs_normalization"] == "true"
and agent["clip_actions"] == "null"
and robot["joint_ordering"] == action["joint_names"] == names
and action["preserve_order"] == action["use_default_offset"] == "true"
and action["clip"] == "null" and float(action["offset"]) == 0
and len(names) == 24
and all(float(v) == 0 for v in robot["init_state"]["joint_vel"].values())
and config["sim"] == {"timestep": float(env["sim"]["dt"]), "decimation": int(env["decimation"])}
):
raise ValueError("Unsupported logged actor/observation/action contract; update explicitly")
actuator = robot["actuators"]["all_joints"]
for n in names:
for out, key in {"kp": "stiffness", "kd": "damping", "torque_limit": "actuator_effort_limit",
"velocity_limit": "actuator_velocity_limit", "keep_velocity": "keep_velocity"}.items():
if abs(config["actuation"]["joints"][n][out] - parameter(actuator[key], n)) > 1e-8:
raise ValueError(f"Per-skill actuator contract differs: {n}/{out}")
before = sha(cp)
saved = torch.load(cp, map_location="cpu", weights_only=True)
if int(saved["iter"]) < int(agent["max_iterations"]) - 1 or cp.stem != f"model_{saved['iter']}":
raise ValueError("Only the final configured checkpoint may be imported")
state = saved["actor_state_dict"]
shapes = [(512, 126), (256, 512), (128, 256), (24, 128)]
if any(tuple(state[f"mlp.{2*i}.weight"].shape) != shape for i, shape in enumerate(shapes)):
raise ValueError("Unsupported actor architecture")
if any(not torch.isfinite(v).all() for v in state.values()) or (state["obs_normalizer._std"] < 0).any():
raise ValueError("Invalid checkpoint tensors")
class Actor(torch.nn.Module):
def __init__(self):
super().__init__()
self.register_buffer("mean", state["obs_normalizer._mean"])
self.register_buffer("std", state["obs_normalizer._std"])
layers = []
for i, (out_size, in_size) in enumerate(shapes):
layer = torch.nn.Linear(in_size, out_size)
layer.load_state_dict({"weight": state[f"mlp.{2*i}.weight"], "bias": state[f"mlp.{2*i}.bias"]})
layers.append(layer)
if i < len(shapes) - 1:
layers.append(torch.nn.ELU())
self.mlp = torch.nn.Sequential(*layers)
def forward(self, obs):
return self.mlp((obs - self.mean) / (self.std + 1e-2))
motion_file = Path(env["commands"]["motion"]["motion_file"])
with np.load(motion_file, allow_pickle=False) as motion:
source_names = list(motion["joint_names"].astype(str))
order = [source_names.index(n) for n in names]
pos, vel = motion["joint_pos"][:, order], motion["joint_vel"][:, order]
fps = float(motion["fps"].item())
dt = config["sim"]["timestep"] * config["sim"]["decimation"]
if abs(fps * dt - 1) > 1e-9 or pos.shape != vel.shape or pos.shape[1] != 24 or len(pos) < 2:
raise ValueError("Motion/control clock or shape mismatch (no silent resampling)")
if not (np.isfinite(pos).all() and np.isfinite(vel).all()):
raise ValueError("Non-finite motion")
duration = args.duration if args.duration is not None else min(10.0, len(pos) * dt)
if not np.isfinite(duration) or not 0 < duration <= len(pos) * dt:
raise ValueError("Duration must be within the reference clip")
defaults = {n: float(robot["init_state"]["joint_pos"][n]) for n in names}
for folder in ["policies", "motions"]:
(target / folder).mkdir(exist_ok=True)
policy = target / f"policies/{skill_id}.export.onnx"
actor = Actor().eval()
torch.onnx.export(actor, torch.zeros(1, 126), str(policy), input_names=["obs"],
output_names=["actions"], opset_version=17, dynamo=False)
model = onnx.load(policy)
onnx.helper.set_model_props(model, {"checkpoint": cp.name, "checkpoint_sha256": before,
"iteration": str(saved["iter"]), "actor_observation_group": "student",
"joint_names": ",".join(names), "default_joint_pos": ",".join(map(str, defaults.values())),
"motion_sha256": sha(motion_file), "status": "completed-training"})
onnx.checker.check_model(model)
onnx.save(model, policy)
evaluator = ReferenceEvaluator(model)
rng = np.random.default_rng(42)
samples = [np.zeros((1, 126), np.float32), *[rng.normal(size=(1, 126)).astype(np.float32) for _ in range(16)]]
standing = np.r_[pos[0], vel[0], [0, 0, 0], [0, 0, -1], np.zeros(72)].astype(np.float32)[None]
samples.append(standing)
error = 0.0
for obs in samples:
with torch.no_grad():
expected = actor(torch.from_numpy(obs)).numpy()
actual = evaluator.run(None, {"obs": obs})[0]
np.testing.assert_allclose(actual, expected, rtol=2e-4, atol=2e-5)
error = max(error, float(np.max(np.abs(actual - expected))))
if before != sha(cp):
raise ValueError("Checkpoint changed during export")
policy.replace(target / f"policies/{skill_id}.onnx")
binary = target / f"motions/{skill_id}.f32"
temp = binary.with_suffix(".tmp")
temp.write_bytes(np.concatenate([pos.ravel(), vel.ravel()]).astype("<f4").tobytes())
temp.replace(binary)
dump(target / f"motions/{skill_id}.json", {"schema": 1, "layout": "positions_then_velocities",
"dtype": "float32-le", "step_dt": dt, "frames": len(pos), "joint_names": names,
"data": f"motions/{skill_id}.f32"})
skill = next(s for s in config["skills"] if s["id"] == skill_id)
skill.update({"available": False, "duration": duration, "blend": 0.5,
"defaults": defaults, "reference": f"motions/{skill_id}.json",
"observations": {"obs": {"terms": [{"type": "motion_command", "group": "policy"},
{"type": "gyro", "sensor": config["observations"]["obs"]["terms"][0]["sensor"]},
{"type": "projected_gravity", "body": "base_link"},
{"type": "joint_pos_rel", "group": "policy"}, {"type": "joint_vel", "group": "policy"},
{"type": "last_action", "group": "policy"}]}},
"actions": [{"type": "joint_position", "group": "policy", "scale": float(action["scale"]), "offset": "default"}],
"controller": {"type": "onnx", "policy": f"policies/{skill_id}.onnx"}})
next(b for b in config["input"]["bindings"] if b["action"] == f"skill:{skill_id}")["disabled"] = True
provenance_path = target / "source-manifest.json"
provenance = json.loads(provenance_path.read_text())
provenance.setdefault("skills", {})[skill_id] = {"checkpoint": str(cp), "checkpoint_sha256": before,
"iteration": int(saved["iter"]), "configured_iterations": int(agent["max_iterations"]),
"motion": str(motion_file), "motion_sha256": sha(motion_file), "frames": len(pos), "fps": fps,
"actor_observation_group": "student", "export_max_abs_error": error,
"training_params_sha256": {n: sha(params / n) for n in ["env.yaml", "agent.yaml"]},
"deployed_duration": duration, "validation": "staged-disabled"}
for f in [f"policies/{skill_id}.onnx", f"motions/{skill_id}.json", f"motions/{skill_id}.f32"]:
provenance["files"] = [r for r in provenance["files"] if r["file"] != f]
provenance["files"].append({"file": f, "sha256": sha(target / f)})
dump(provenance_path, provenance)
dump(target / "robot.json", config)
print(f"Staged {skill_id}: final {saved['iter']}, 126→24, {len(pos)} frames / {len(pos)*dt:.2f}s, max error {error:.3g}. DISABLED pending playback validation.")
if __name__ == "__main__":
main()