Spaces:
Running
Running
File size: 4,890 Bytes
4cefcfa | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 | /* Next Balance compute core: line-by-line port of balance_core.py. */
(function (root, factory) {
if (typeof module === "object" && module.exports) module.exports = factory();
else root.Core = factory();
})(typeof self !== "undefined" ? self : this, function () {
"use strict";
const DT = 0.01, G = 9.81, U_MAX = 400.0, M_PLATFORM = 2.0, EPS = 0.01, ALPHA = 0.3, BETA = 0.05, Z_REF = 0.5, T_STEP = 0.5, T_RAMP = 1.0;
const ASSETS = ["model.json"];
function init(assets) { return { params: assets["model.json"].meta.params }; }
function simulate(sc, ctrl, params, recordEvery) {
recordEvery = recordEvery || 1;
const rng = NLRT.mulberry32((sc.seed * 31 + 7) >>> 0);
const mNom = M_PLATFORM + sc.payload0, n = Math.round(sc.T / DT);
let z = 0, v = 0, zh = 0, vh = 0, integ = 0;
const queue = []; for (let i = 0; i < sc.delay + 1; i++) queue.push(mNom * G);
let theta = [mNom, 3.0, 2.0];
let P = [[10, 0, 0], [0, 10, 0], [0, 0, 10]];
let fz = [0, 0], fu = [mNom * G, 0], ft = [0, 0];
const w = params.wf != null ? params.wf : 20.0, lam = params.lam != null ? params.lam : 0.995;
const imax = params.imax != null ? params.imax : 0.5;
const out = { t: [], z: [], u: [], m_hat: [], energy: 0 };
for (let k = 0; k < n; k++) {
const t = k * DT;
const m = M_PLATFORM + sc.payload0 + (t >= sc.t_load ? sc.dm : 0);
const gf = t >= sc.t_fric ? sc.fric_gain : 1.0, c = sc.c0 * gf, F = sc.F0 * gf;
const ref = Z_REF * Math.min(1.0, Math.max(0.0, (t - T_STEP) / T_RAMP));
const meas = z + sc.noise * rng.normal();
const zp = zh + vh * DT, res = meas - zp;
zh = zp + ALPHA * res; vh = vh + (BETA / DT) * res;
const e = ref - zh;
integ = Math.max(-imax, Math.min(imax, integ + e * DT));
let u;
if (ctrl === "pid") {
u = params.kp * e + params.ki * integ - params.kd * vh + mNom * G;
} else {
const uApplied = queue[0], tv = Math.tanh(vh / EPS);
const az = w * w * (meas - fz[0]) - 2 * w * fz[1];
const au = w * w * (uApplied - fu[0]) - 2 * w * fu[1];
const at = w * w * (tv - ft[0]) - 2 * w * ft[1];
fz = [fz[0] + DT * fz[1], fz[1] + DT * az];
fu = [fu[0] + DT * fu[1], fu[1] + DT * au];
ft = [ft[0] + DT * ft[1], ft[1] + DT * at];
const phi = [az + G, fz[1], ft[0]], y = fu[0];
const Pphi = [0, 1, 2].map(i => { let s = 0; for (let j = 0; j < 3; j++) s += P[i][j] * phi[j]; return s; });
let s = 0; for (let i = 0; i < 3; i++) s += phi[i] * Pphi[i];
const den = lam + s, K = Pphi.map(x => x / den);
let s2 = 0; for (let i = 0; i < 3; i++) s2 += phi[i] * theta[i];
const err = y - s2;
theta = theta.map((th, i) => th + K[i] * err);
P = [0, 1, 2].map(i => [0, 1, 2].map(j => (P[i][j] - K[i] * Pphi[j]) / lam));
theta = [Math.min(30, Math.max(0.5, theta[0])), Math.min(60, Math.max(0, theta[1])), Math.min(60, Math.max(0, theta[2]))];
const wn = params.wn, zeta = params.zeta;
const acmd = wn * wn * e - 2 * zeta * wn * vh + params.ki_a * integ;
u = theta[0] * (acmd + G) + theta[1] * vh;
}
u = Math.max(0, Math.min(U_MAX, u));
queue.push(u);
const uAct = queue.shift();
const a = (uAct - c * v - F * Math.tanh(v / EPS)) / m - G;
v = v + a * DT; z = z + v * DT;
out.energy += uAct * uAct * DT;
if (k % recordEvery === 0) { out.t.push(Math.round(t * 1e4) / 1e4); out.z.push(z); out.u.push(uAct); out.m_hat.push(ctrl !== "pid" ? theta[0] : mNom); }
}
return out;
}
function metrics(sc, tr, band, recordEvery) {
band = band || 0.01; recordEvery = recordEvery || 1;
const { t, z } = tr;
let iae = 0; for (let i = 0; i < t.length; i++) iae += Math.abs(z[i] - Z_REF * Math.min(1.0, Math.max(0.0, (t[i] - T_STEP) / T_RAMP)));
iae *= DT * recordEvery;
let maxDev = 0, lastOut = null, over = 0, anyStep = false;
for (let i = 0; i < t.length; i++) {
if (t[i] >= sc.t_load) { maxDev = Math.max(maxDev, Math.abs(z[i] - Z_REF)); if (Math.abs(z[i] - Z_REF) > band) lastOut = t[i]; }
else if (t[i] >= T_STEP) { anyStep = true; over = Math.max(over, z[i] - Z_REF); }
}
return { max_deviation_m: maxDev, settling_time_s: lastOut === null ? 0 : lastOut - sc.t_load,
settled: lastOut === null || lastOut < t[t.length - 1] - 0.5, control_energy_kN2s: tr.energy / 1e6,
step_overshoot_m: anyStep ? Math.max(0, over) : 0, final_error_m: Math.abs(z[z.length - 1] - Z_REF), iae_ms: iae };
}
function runCase(ctx, input) {
const p = ctx.params[input.ctrl];
const tr = simulate(input.scenario, input.ctrl, p, 1);
const zs = tr.z.filter((_, i) => i % 50 === 0);
return { metrics: metrics(input.scenario, tr), z_every_50: zs };
}
return { ASSETS, init, simulate, metrics, runCase, DT, Z_REF };
});
|