189 lines
10 KiB
Python
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()
|