Files
VibemakerSandbox/app/src/platform/runtime/RobotRuntime.js
T
chenlin fc5ee4f2c6 feat: release v0.2 multi-robot simulator
Add declarative robot packages, shared simulation lifecycle and Microban ONNX/PD walking with independent head control.

Unify comic UI and chase camera; default robot packages to a 4.5m arena with real collision walls. Include regression tests, asset provenance/licenses and verification evidence.
2026-10-28 14:00:26 +08:00

384 lines
12 KiB
JavaScript

import * as THREE from "three";
import { OrbitControls } from "three/addons/controls/OrbitControls.js";
import { loadRuntimes } from "./runtimes.js";
import { ControlScheduler } from "./ControlScheduler.js";
import { fetchChecked } from "../loader/PackageLoader.js";
import { readOnnxMetadata, resolveJointDefaults } from "../loader/metadata.js";
import { SimCore } from "../sim/SimCore.js";
import {
JointPdActuation,
NativeActuation,
} from "../sim/actuation/JointPdActuation.js";
import { ObservationPipeline } from "../policy/ObservationPipeline.js";
import { ActionPipeline } from "../policy/ActionPipeline.js";
import { OnnxController } from "../policy/controllers/OnnxController.js";
import { SkillManager } from "../skills/SkillManager.js";
import { InputRouter } from "../input/InputRouter.js";
import { RigRenderer } from "../render/RigRenderer.js";
import { ChaseCamera } from "../render/ChaseCamera.js";
export class RobotRuntime {
static async load(options) {
const { pkg, world } = options,
config = pkg.config,
{ mujoco, ort } = await loadRuntimes();
let sim, session, rig, worldPresentation;
try {
sim = await SimCore.load(mujoco, pkg, world);
const primary = config.skills.find((s) => s.kind === "primary");
if (primary.controller.type !== "onnx")
throw new Error("Primary controller must be ONNX");
const bytes = await fetchChecked(
pkg.asset(primary.controller.policy),
"bytes",
);
const metadata = readOnnxMetadata(bytes),
names = [...sim.joints.keys()];
const values =
config.joints.source === "onnx_metadata"
? resolveJointDefaults(metadata, names)
: names.map((n) => config.joints.defaults[n]);
const defaults = new Map(names.map((n, i) => [n, values[i]]));
for (const [name, value] of Object.entries(config.joints.defaults)) {
if (defaults.has(name) && Math.abs(defaults.get(name) - value) > 1e-6)
throw new Error(`joints.defaults.${name}: metadata conflict`);
defaults.set(name, value);
}
if ([...defaults.values()].some((v) => !Number.isFinite(v)))
throw new Error("joints.defaults: missing/invalid reference pose");
sim.reset(defaults);
session = await ort.InferenceSession.create(bytes, {
executionProviders: ["wasm"],
});
const ctx = {
sim,
config,
defaults,
policyNames: config.joints.groups[primary.owns],
lastAction: new Float32Array(config.joints.groups[primary.owns].length),
command: new Float32Array(config.commands.size),
targets: new Map(
names.map((n) => [n, sim.data.qpos[sim.joints.get(n).qpos]]),
),
};
const obs = new ObservationPipeline(config.observations, ctx),
actions = new ActionPipeline(config.actions, ctx);
const controller = new OnnxController(ort, session, obs, actions.size);
obs.write();
const smoke = await controller.run();
if (smoke.length !== actions.size || !smoke.every(Number.isFinite))
throw new Error("ONNX: standing-pose smoke test failed");
rig = new RigRenderer(sim.model, sim.data, { ...config.render, world });
worldPresentation = await options.createWorldPresentation?.({
sim,
rig,
world,
});
return new RobotRuntime({
...options,
config,
sim,
ctx,
obs,
actions,
controller,
rig,
worldPresentation,
});
} catch (error) {
worldPresentation?.dispose();
rig?.dispose();
await session?.release();
sim?.dispose();
throw error;
}
}
constructor(options) {
Object.assign(this, options);
this.onStatus ??= () => {};
this.onTelemetry ??= () => {};
this.fallen = false;
this.odo = 0;
this.lastPosition = new Float64Array(
this.sim.data.qpos.slice(this.sim.rootQpos, this.sim.rootQpos + 2),
);
this.disposed = false;
this.active = false;
this.positions = new Map();
this.skills = new SkillManager(this.config);
this.actuation =
this.config.actuation.type === "joint_pd"
? new JointPdActuation(
this.sim,
this.ctx.targets,
this.config.actuation,
)
: new NativeActuation(this.sim, this.ctx.targets);
this.input = new InputRouter({
bindings: this.config.input.bindings,
getVelocityLimits: () => this.config.commands.limits,
isEnabled: (action) =>
this.active &&
(action === "reset" ||
action === "chaseToggle" ||
(this.scheduler.running && !this.fallen)),
});
this.input.onAction = (action) => {
if (action === "reset")
this.reset().catch((error) =>
this.onStatus({ runtimeError: error.message }),
);
else if (action === "chaseToggle") this.restoreCamera();
else if (action.startsWith("toggle:")) this.toggle(action.slice(7));
};
this.tickCount = 0;
this.hzAt = performance.now();
this.ctrlHz = 0;
this.frames = 0;
this.fpsAt = this.hzAt;
this.telemetryAt = 0;
this.scheduler = new ControlScheduler(() => this.controlStep(), {
dt: this.config.sim.timestep * this.config.sim.decimation,
onError: (error) => {
this.input.clear();
this.onStatus({
runtimeError: error.message,
paused: true,
runtimePhase: "error",
});
},
});
this.scene.add(this.rig.root);
if (this.worldPresentation) this.scene.add(this.worldPresentation.root);
this.follow = new THREE.Vector3();
}
activate({ paused = false } = {}) {
if (this.disposed) throw new Error("Runtime disposed");
this.active = true;
this.controls = new OrbitControls(this.camera, this.renderer.domElement);
this.controls.enableDamping = true;
this.controls.minDistance = 0.2;
this.controls.maxDistance = 4;
this.controls.maxPolarAngle = Math.PI / 2 - 0.02;
this.chaseCamera = new ChaseCamera(
this.camera,
this.controls,
this.config.render.camera,
);
this.restoreCamera();
this.input.init();
if (!paused) this.resume();
else this.onStatus({ paused: true });
}
cameraPose() {
const q = this.sim.data.qpos,
a = this.sim.rootQpos,
quat = this.sim.data.xquat,
b = this.sim.root * 4;
this.follow.set(q[a], q[a + 2], -q[a + 1]);
return Math.atan2(
2 * (quat[b] * quat[b + 3] + quat[b + 1] * quat[b + 2]),
1 - 2 * (quat[b + 2] ** 2 + quat[b + 3] ** 2),
);
}
restoreCamera() {
const yaw = this.cameraPose();
this.chaseCamera.restore(this.follow, yaw);
}
status() {
return {
actuationLabel:
this.config.actuation.type === "joint_pd"
? "Simplified actuation · joint PD"
: "",
};
}
async deactivate() {
this.active = false;
await this.pause();
}
reactivate({ paused = false } = {}) {
this.active = true;
if (!paused) this.resume();
}
async pause() {
this.input.clear();
await this.scheduler.pause();
this.onStatus({ paused: true });
}
resume() {
if (this.disposed) return;
this.active = true;
this.input.clear();
this.scheduler.start();
this.onStatus({ paused: false });
}
reset() {
return (this.resetPending ??= this.#reset().finally(() => {
this.resetPending = null;
}));
}
async #reset() {
const running = this.scheduler.running;
await this.scheduler.pause();
this.input.clear();
this.ctx.command.fill(0);
this.ctx.lastAction.fill(0);
this.skills.reset();
this.sim.reset(this.ctx.defaults);
this.odo = 0;
this.actuation.resetStats?.();
this.lastPosition.set(
this.sim.data.qpos.subarray(this.sim.rootQpos, this.sim.rootQpos + 2),
);
for (const [name, joint] of this.sim.joints)
this.ctx.targets.set(name, this.sim.data.qpos[joint.qpos]);
this.fallen = false;
this.onStatus({ fallen: false });
this.rig.sync();
if (running) this.scheduler.start();
}
triggerAction(action) {
if (this.input.isEnabled(action)) this.input.onAction(action);
}
toggle(id) {
for (const [name, j] of this.sim.joints)
this.positions.set(name, this.sim.data.qpos[j.qpos]);
this.skills.toggle(id, this.sim.data.time, this.positions);
}
async controlStep() {
if (!this.fallen) {
this.ctx.command.set(this.input.command);
this.obs.write();
this.actions.apply(await this.controller.run());
this.skills.update(this.sim.data.time, this.ctx.targets);
}
for (let i = 0; i < this.config.sim.decimation; i++) {
this.actuation.apply();
this.sim.step();
}
const q = this.sim.data.qpos,
a = this.sim.rootQpos,
quat = this.sim.data.xquat,
b = this.sim.root * 4,
x = quat[b + 1],
y = quat[b + 2];
const gz = -(1 - 2 * (x * x + y * y));
if (
!q.every(Number.isFinite) ||
!this.sim.data.qvel.every(Number.isFinite) ||
!Number.isFinite(gz)
)
throw new Error("Physics produced non-finite state");
if (
!this.fallen &&
(gz > this.config.termination.tilt_z ||
q[a + 2] < this.config.termination.min_height)
) {
this.fallen = true;
this.input.clear();
for (const [name, j] of this.sim.joints)
this.ctx.targets.set(name, q[j.qpos]);
this.onStatus({ fallen: true });
}
this.odo += Math.hypot(
q[a] - this.lastPosition[0],
q[a + 1] - this.lastPosition[1],
);
this.lastPosition[0] = q[a];
this.lastPosition[1] = q[a + 1];
this.tickCount++;
const now = performance.now();
if (now - this.hzAt >= 1000) {
this.ctrlHz = Math.round((this.tickCount * 1000) / (now - this.hzAt));
this.tickCount = 0;
this.hzAt = now;
}
}
frame() {
if (this.disposed) return;
this.rig.sync();
if (this.chaseCamera) {
const yaw = this.cameraPose();
this.worldPresentation?.update(this.follow);
this.chaseCamera.update(this.follow, yaw, {
turning: Math.abs(this.input.command[2]) > 0.05,
});
}
const now = performance.now();
this.frames++;
if (now - this.telemetryAt > 250) {
const v = this.sim.data.qvel,
a = this.sim.rootDof;
this.onTelemetry({
fps: Math.round((this.frames * 1000) / (now - this.fpsAt)),
ctrlHz: this.ctrlHz,
speed: Math.hypot(v[a], v[a + 1]),
odo: this.odo,
peers: 0,
});
this.telemetryAt = now;
}
}
debug() {
const self = this;
return {
get model() {
return self.sim.model;
},
get data() {
return self.sim.data;
},
mujoco: this.sim.mujoco,
camera: this.camera,
controls: this.controls,
get chaseCam() {
return self.chaseCamera.enabled;
},
scene: this.scene,
world: this.world,
command: this.input.command,
lastAction: this.ctx.lastAction,
skills: this.skills,
buildObs: () => {
this.ctx.command.set(this.input.command);
return Object.values(this.obs.write())[0];
},
pause: () => this.pause(),
resume: () => this.resume(),
step: (n) => this.scheduler.step(n),
resetSim: () => this.reset(),
toggle: (id) => this.toggle(id),
get fallen() {
return self.fallen;
},
get actuationStats() {
const a = self.actuation;
return {
samples: a.samples ?? 0,
saturated: a.saturated ?? 0,
saturationRatio: a.samples ? a.saturated / a.samples : 0,
longestSaturatedSeconds:
(a.maxStreak ?? 0) * self.config.sim.timestep,
};
},
get running() {
return self.scheduler.running;
},
};
}
async dispose() {
if (this.disposed) return;
this.disposed = true;
this.active = false;
await this.scheduler.dispose();
this.input.dispose();
this.chaseCamera?.dispose();
this.controls?.dispose();
this.worldPresentation?.dispose();
this.rig.dispose();
await this.controller.dispose();
this.sim.dispose();
}
}