Files
VibemakerSandbox/app/test/policy-pipelines.test.js

210 lines
6.8 KiB
JavaScript

import test from "node:test";
import assert from "node:assert/strict";
import { ObservationPipeline } from "../src/platform/policy/ObservationPipeline.js";
import { ActionPipeline } from "../src/platform/policy/ActionPipeline.js";
import {
JointPdActuation,
NativeActuation,
} from "../src/platform/sim/actuation/JointPdActuation.js";
import { SkillManager } from "../src/platform/skills/SkillManager.js";
import {
InputRouter,
isEditingTarget,
} from "../src/platform/input/InputRouter.js";
test("button focus protects native Space/Enter without swallowing movement keys", () => {
const target = { closest: (selector) => (selector === "button" ? {} : null) };
assert.equal(isEditingTarget(target, "Space"), true);
assert.equal(isEditingTarget(target, "KeyW"), false);
assert.equal(isEditingTarget({ closest: () => ({}) }, "KeyW"), true);
});
function context() {
return {
config: { joints: { groups: { policy: ["b", "a"] } }, skills: [] },
policyNames: ["b", "a"],
defaults: new Map([
["a", 1],
["b", 2],
]),
lastAction: new Float32Array([0.3, 0.4]),
command: new Float32Array([0.15, 0, -0.6]),
targets: new Map([
["a", 0],
["b", 0],
]),
sim: {
mujoco: {
mjtObj: { mjOBJ_SENSOR: { value: 1 }, mjOBJ_BODY: { value: 2 } },
mj_name2id: (_m, type, name) => (name === "missing" ? -1 : 0),
},
model: { sensor_adr: [0], sensor_dim: [3] },
data: {
qpos: new Float64Array([1.5, 2.25]),
qvel: new Float64Array([0.1, 0.2]),
sensordata: new Float64Array([0.01, 0.02, 0.03]),
xquat: new Float64Array([1, 0, 0, 0]),
ctrl: new Float64Array(2),
},
joints: new Map([
["a", { qpos: 0, dof: 0, actuator: 1 }],
["b", { qpos: 1, dof: 1, actuator: 0 }],
]),
},
};
}
test("observation order/frame/defaults/history use policy order, not model order", () => {
const ctx = context(),
p = new ObservationPipeline(
{
obs: {
terms: [
{ type: "gyro", sensor: "gyro" },
{ type: "projected_gravity", body: "root" },
{ type: "joint_pos_rel", group: "policy" },
{ type: "joint_vel", group: "policy" },
{ type: "last_action", group: "policy" },
{ type: "command", size: 3 },
],
},
},
ctx,
);
const first = p.write().obs;
assert.equal(first.length, 15);
assert.deepEqual(
Array.from(first),
Array.from(
new Float32Array([
0.01, 0.02, 0.03, 0, -0, -1, 0.25, 0.5, 0.2, 0.1, 0.3, 0.4, 0.15, 0,
-0.6,
]),
),
);
ctx.sim.data.xquat.set([Math.SQRT1_2, Math.SQRT1_2, 0, 0]);
p.write();
assert.ok(Math.abs(first[4] + 1) < 1e-6);
assert.ok(Math.abs(first[5]) < 1e-6);
assert.equal(p.write().obs, first);
assert.throws(
() =>
new ObservationPipeline(
{ obs: { terms: [{ type: "gyro", sensor: "missing" }] } },
ctx,
),
/missing/,
);
});
test("body-frame linear velocity reads the named sensor and validates its dimension", () => {
const ctx = context();
const specs = {
obs: { terms: [{ type: "linear_velocity", sensor: "velocity" }] },
};
ctx.sim.model.sensor_adr[0] = 3;
ctx.sim.data.sensordata = new Float64Array([99, 98, 97, 1, -2, 3]);
assert.deepEqual(
Array.from(new ObservationPipeline(specs, ctx).write().obs),
[1, -2, 3],
);
ctx.sim.model.sensor_dim[0] = 1;
assert.throws(() => new ObservationPipeline(specs, ctx), /dimension 3/);
assert.throws(
() =>
new ObservationPipeline(
{ obs: { terms: [{ type: "linear_velocity", sensor: "missing" }] } },
ctx,
),
/missing sensor/,
);
});
test("actions resolve named defaults and native ctrl order; invalid output is atomic", () => {
const ctx = context(),
p = new ActionPipeline([{ group: "policy", scale: 2 }], ctx);
p.apply(new Float32Array([0.25, -0.5]));
new NativeActuation(ctx.sim, ctx.targets).apply();
assert.deepEqual(Array.from(ctx.sim.data.ctrl), [2.5, 0]);
const before = [...ctx.targets];
assert.throws(() => p.apply(new Float32Array([0.1, NaN])), /non-finite/);
assert.deepEqual([...ctx.targets], before);
assert.throws(() => p.apply(new Float32Array(3)), /expected/);
});
test("PD computes torque per substep and saturates without writing angle to motor", () => {
const ctx = context();
ctx.targets.set("a", 2);
ctx.targets.set("b", 0);
const pd = new JointPdActuation(ctx.sim, ctx.targets, {
kp: 2,
kd: 0.1,
torque_limit: 0.65,
});
pd.apply();
assert.deepEqual(Array.from(ctx.sim.data.ctrl), [-0.65, 0.65]);
ctx.sim.data.qpos[0] = 2;
pd.apply();
assert.ok(Math.abs(ctx.sim.data.ctrl[1] + 0.01) < 1e-12);
ctx.targets.set("b", NaN);
assert.throws(() => pd.apply(), /non-finite/);
});
test("head overlay blends, oscillates, stops at zero without touching primary targets", () => {
const skills = new SkillManager({
joints: { groups: { policy: ["leg"], head: ["head"] } },
skills: [
{ id: "walk", kind: "primary", owns: "policy" },
{
id: "head",
kind: "overlay",
owns: "head",
controller: {
type: "oscillator",
frequency: 0.35,
amplitude: Math.PI / 4,
blend: 0.5,
},
},
],
});
const targets = new Map([["leg", 0.123]]);
skills.toggle("head", 0, new Map([["head", 0.2]]));
skills.update(0.25, targets);
assert.equal(targets.get("head"), 0.1);
skills.update(0.5, targets);
skills.update(1, targets);
assert.ok(targets.get("head") > 0.6);
assert.equal(targets.get("leg"), 0.123);
skills.toggle("head", 1, new Map([["head", 0.4]]));
skills.update(1.25, targets);
assert.ok(Math.abs(targets.get("head") - 0.2) < 1e-10);
skills.update(1.5, targets);
assert.equal(targets.get("head"), 0);
skills.reset();
assert.equal(skills.overlays.get("head").enabled, false);
});
test("input holds aliases independently, clears focus loss, and ignores repeat", () => {
const target = new EventTarget(),
actions = [],
router = new InputRouter({
bindings: [
{ action: "move_forward", codes: ["KeyW", "ArrowUp"] },
{ action: "reset", codes: ["Space"] },
],
getVelocityLimits: () => [0.15, -0.1, 0.6],
});
router.onAction = (a) => actions.push(a);
router.init(target);
const key = (type, code, repeat = false) => {
const e = new Event(type);
Object.assign(e, { code, repeat });
target.dispatchEvent(e);
};
key("keydown", "KeyW");
key("keydown", "ArrowUp");
key("keyup", "KeyW");
assert.ok(router.command[0] > 0.1);
target.dispatchEvent(new Event("blur"));
assert.equal(router.command[0], 0);
key("keydown", "Space", true);
key("keydown", "Space");
assert.deepEqual(actions, ["reset"]);
router.dispose();
key("keydown", "KeyW");
assert.equal(router.command[0], 0);
});