Files

276 lines
9.1 KiB
JavaScript

import test from "node:test";
import assert from "node:assert/strict";
import { readFileSync } from "node:fs";
import { validatePackage } from "../src/platform/loader/SchemaValidator.js";
import { ObservationPipeline } from "../src/platform/policy/ObservationPipeline.js";
import { TermRegistry } from "../src/platform/policy/TermRegistry.js";
import { PolicyStateMachine } from "../src/platform/skills/PolicyStateMachine.js";
import { JointPdActuation } from "../src/platform/sim/actuation/JointPdActuation.js";
import { InputRouter } from "../src/platform/input/InputRouter.js";
import { OnnxPolicy } from "../src/platform/policy/controllers/OnnxPolicy.js";
const config = JSON.parse(
readFileSync(new URL("../public/robots/rp1/robot.json", import.meta.url)),
);
test("RP1 contract has 24 named actions, 243 term-major observations and fail-closed availability gates", () => {
validatePackage(config);
const names = config.joints.groups.policy;
assert.equal(names.length, 24);
assert.equal(config.actions[0].scale, 0.25);
assert.equal(config.sim.timestep * config.sim.decimation, 0.02);
assert.equal(config.observations.obs.history, 3);
assert.equal(config.observations.obs.layout, "term_major");
assert.deepEqual(
config.observations.obs.terms.map((t) => t.type),
[
"gyro",
"projected_gravity",
"command",
"joint_pos_rel",
"joint_vel",
"last_action",
],
);
assert.equal(
names.filter((n) => config.actuation.joints[n].kp === 120).length,
8,
);
const invalid = structuredClone(config);
invalid.skills[1].available = true;
delete invalid.skills[1].controller.policy;
invalid.input.bindings.find((b) => b.action === "skill:dance").disabled =
true;
assert.throws(() => validatePackage(invalid), /policy/);
invalid.skills[1].controller.policy = "policies/dance.onnx";
assert.throws(() => validatePackage(invalid), /disabled/);
invalid.input.bindings.find((b) => b.action === "skill:dance").disabled =
false;
validatePackage(invalid);
invalid.skills[1].owns = "missing";
assert.throws(() => validatePackage(invalid), /group/);
});
test("history is oldest→newest within each term, backfills first sample, debug doesn't advance, reset clears", () => {
const registry = new TermRegistry([
[
"value",
{
size: (t) => t.size,
write: (out, offset, ctx, op) =>
out.set(ctx.value.slice(0, op.term.size), offset),
},
],
]);
const ctx = { value: [1, 2] };
const specs = {
obs: {
history: 3,
layout: "term_major",
terms: [
{ type: "value", size: 2 },
{ type: "value", size: 1 },
],
},
};
const pipeline = new ObservationPipeline(specs, ctx, { registry });
assert.deepEqual([...pipeline.write().obs], [1, 2, 1, 2, 1, 2, 1, 1, 1]);
ctx.value = [3, 4];
assert.deepEqual([...pipeline.write().obs], [1, 2, 1, 2, 3, 4, 1, 1, 3]);
ctx.value = [5, 6];
assert.deepEqual(
[...pipeline.write({ advance: false }).obs],
[1, 2, 1, 2, 5, 6, 1, 1, 5],
);
assert.deepEqual([...pipeline.write().obs], [1, 2, 3, 4, 5, 6, 1, 3, 5]);
pipeline.reset();
pipeline.write({ advance: false });
ctx.value = [9, 10];
assert.deepEqual([...pipeline.write().obs], [9, 10, 9, 10, 9, 10, 9, 9, 9]);
ctx.value = [5, 6];
specs.obs.layout = "frame_major";
const frame = new ObservationPipeline(specs, ctx, { registry });
frame.write();
ctx.value = [7, 8];
assert.deepEqual([...frame.write().obs], [5, 6, 5, 5, 6, 5, 7, 8, 7]);
});
function stateMachine() {
const c = structuredClone(config);
c.skills[1].available = true;
c.skills[1].duration = 10;
c.skills[1].blend = 0.5;
c.skills[2].available = false;
c.joints.groups.policy = ["joint"];
return new PolicyStateMachine(c);
}
test("exclusive full-body FSM waits for actual stop, blends, times out and returns to walk", () => {
const fsm = stateMachine(),
targets = new Map([["joint", 1]]);
assert.equal(fsm.request("wave", 0), false);
assert.equal(fsm.request("dance", 0), true);
assert.equal(fsm.request("dance", 0), false);
fsm.step(0.5, [0, 0, 0], 0.8, 0, targets);
assert.equal(fsm.state, "settling");
fsm.step(1, [0, 0, 0], 0, 0, targets);
assert.equal(fsm.step(1.5, [0, 0, 0], 0, 0, targets), "dance");
targets.set("joint", 3);
fsm.blend(1.75, targets);
assert.equal(targets.get("joint"), 2);
fsm.step(2.1, [0, 0, 0], 0, 0, targets);
assert.equal(fsm.state, "playing");
assert.equal(fsm.step(11.6, [0, 0, 0], 0, 0, targets), "walk");
assert.equal(fsm.state, "returning");
fsm.step(12.2, [0, 0, 0], 0, 0, targets);
assert.equal(fsm.state, "walk");
fsm.request("dance", 13);
fsm.step(19, [0, 0, 0], 1, 0, targets);
assert.equal(fsm.state, "walk");
});
test("episode blending never overwrites independent overlay joint targets", () => {
const fsm = stateMachine(),
targets = new Map([
["joint", 1],
["head", 2],
]);
fsm.request("dance", 0);
fsm.step(0.1, [0, 0, 0], 0, 0, targets);
fsm.step(0.6, [0, 0, 0], 0, 0, targets);
targets.set("joint", 3);
targets.set("head", 7);
fsm.blend(0.85, targets);
assert.equal(targets.get("joint"), 2);
assert.equal(targets.get("head"), 7);
});
test("movement cancels pending skills and interrupts active skills with a return blend; reset clears", () => {
const fsm = stateMachine(),
targets = new Map();
fsm.request("dance", 0);
fsm.step(0.1, [0.4, 0, 0], 0, 0, targets);
assert.equal(fsm.state, "walk");
fsm.request("dance", 1);
fsm.step(1.1, [0, 0, 0], 0, 0, targets);
fsm.step(1.6, [0, 0, 0], 0, 0, targets);
assert.equal(fsm.active, "dance");
fsm.step(1.7, [0, 0, 0.6], 0, 0, targets);
assert.equal(fsm.state, "returning");
assert.equal(fsm.active, "walk");
fsm.reset();
assert.equal(fsm.state, "walk");
assert.equal(fsm.pending, null);
});
test("named per-joint gains and four-quadrant torque-speed envelope apply atomically", () => {
const sim = {
joints: new Map([
["a", { qpos: 0, dof: 0, actuator: 1 }],
["b", { qpos: 1, dof: 1, actuator: 0 }],
]),
data: { qpos: [0, 0], qvel: [10, -10], ctrl: new Float64Array(2) },
};
const targets = new Map([
["a", 2],
["b", -2],
]);
const pd = new JointPdActuation(sim, targets, {
kp: 1,
kd: 0,
torque_limit: 10,
joints: {
a: { kp: 20, velocity_limit: 10, keep_velocity: 5 },
b: { kp: 20, velocity_limit: 10, keep_velocity: 5 },
},
});
pd.apply();
assert.deepEqual([...sim.data.ctrl], [0, 0]);
sim.data.qvel = [-10, 10];
pd.apply();
assert.deepEqual([...sim.data.ctrl], [-10, 10]);
targets.set("b", NaN);
assert.throws(() => pd.apply(), /non-finite/);
assert.deepEqual([...sim.data.ctrl], [-10, 10]);
});
test("independent skills own histories/defaults and share only targets; failed smoke releases session", async () => {
let released = 0;
const session = {
inputNames: ["obs"],
outputNames: ["actions"],
inputMetadata: [
{ name: "obs", isTensor: true, type: "float32", shape: [1, 24] },
],
outputMetadata: [
{ name: "actions", isTensor: true, type: "float32", shape: [1, 24] },
],
run: async () => ({ actions: { data: new Float32Array(24).fill(1) } }),
release: async () => released++,
};
const ort = {
Tensor: class {
constructor(type, data) {
this.data = data;
}
},
InferenceSession: { create: async () => session },
};
const names = config.joints.groups.policy;
const shared = {
config,
policyNames: names,
defaults: new Map(names.map((n) => [n, 0])),
command: new Float32Array(3),
targets: new Map(),
sim: {
joints: new Map(names.map((n, i) => [n, { qpos: i }])),
data: { qpos: new Float64Array(24) },
},
};
const skill = {
id: "fixture",
defaults: Object.fromEntries(names.map((n) => [n, 0.5])),
observations: {
obs: { terms: [{ type: "joint_pos_rel", group: "policy" }] },
},
};
const policy = await OnnxPolicy.load(
ort,
new Uint8Array(),
config,
shared,
skill,
);
assert.equal(policy.ctx.targets, shared.targets);
assert.equal(shared.defaults.get(names[0]), 0);
assert.equal(policy.obs.write().obs[0], -0.5);
await policy.run();
assert.equal(shared.targets.get(names[0]), 0.75);
policy.reset();
assert.equal(policy.ctx.lastAction[0], 0);
await policy.dispose();
session.run = async () => ({
actions: { data: new Float32Array(24).fill(NaN) },
});
await assert.rejects(
() => OnnxPolicy.load(ort, new Uint8Array(), config, shared, skill),
/smoke/,
);
assert.equal(released, 2);
});
test("unavailable-skill keyboard bindings never dispatch", () => {
const router = new InputRouter({
bindings: config.input.bindings.map((b) => ({
...b,
disabled: b.action.startsWith("skill:") ? true : b.disabled,
})),
getVelocityLimits: () => config.commands.limits,
});
const target = new EventTarget(),
calls = [];
router.onAction = (action) => calls.push(action);
router.init(target);
for (const code of ["KeyJ", "KeyH", "Space"]) {
const e = new Event("keydown");
Object.assign(e, { code });
target.dispatchEvent(e);
}
assert.deepEqual(calls, ["reset"]);
router.dispose();
});