Files
Jerry 1c837766cd feat: integrate robot assembly and simulation with resilient loading
Add backend-driven assembly and simulation experiences, MAKE branding, mobile rendering budgets and desktop-only CAD/PCB routes. Improve logo loading progress, document image recovery and URDF request handling; include dev packaging and regression coverage.
2026-10-30 18:38:39 +08:00

330 lines
13 KiB
JavaScript
Raw Permalink Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
// 通用运动控制器:把「语义关节」(leg.FL.hip / arm.L.elbow / head.yaw …) 通过 jointMap
// 翻译成 URDF 里的真实关节名,再由各类型模板 (archetype) 生成动作。
// 新机器人只需要写配置,不需要写代码;只有出现全新的结构类型才需要新增模板。
//
// 模板约定(与具体 URDF 无关,方向不一致时用 jointSign 翻转):
// hip 正值 = 脚往后摆 knee 正值 = 屈膝(抬脚)
// ankle 正值 = 脚尖上抬 wheel 正值 = 向前滚动
// arm.* 为相对 stance 的弧度偏移;arm.*.gripper 为 0(闭)~1(开) 归一化值
const MOVABLE = new Set(['revolute', 'continuous', 'prismatic']);
const TAU = Math.PI * 2;
const damp = (cur, target, rate, dt) => cur + (target - cur) * (1 - Math.exp(-rate * dt));
const clamp = (v, lo, hi) => Math.min(hi, Math.max(lo, v));
function globToRegex(glob) {
return new RegExp('^' + glob.replace(/[.+^${}()|[\]\\]/g, '\\$&').replace(/\*/g, '[^.]+') + '$');
}
// 把带通配符的 { "leg.*.knee": -1 } 展开成 { "leg.FL.knee": -1, ... }
function expand(obj = {}, keys) {
const out = {};
for (const [pattern, value] of Object.entries(obj)) {
if (!pattern.includes('*')) { out[pattern] = value; continue; }
const re = globToRegex(pattern);
for (const k of keys) if (re.test(k)) out[k] = value;
}
return out;
}
export class MotionController {
constructor(robot, config = {}) {
this.robot = robot;
this.config = config;
this.joints = robot.joints;
this.movable = Object.values(robot.joints).filter(j => MOVABLE.has(j.jointType));
this.map = {};
this.missing = [];
for (const [sem, name] of Object.entries(config.jointMap || {})) {
if (robot.joints[name]) this.map[sem] = robot.joints[name];
else this.missing.push(`${sem} → ${name}`);
}
if (this.missing.length) console.warn('[make] jointMap 中找不到这些关节:', this.missing);
const semKeys = Object.keys(this.map);
this.sign = expand(config.jointSign, semKeys);
// stance 既可以写语义名(支持通配符),也可以直接写 URDF 关节名
this.stance = {};
const stanceSem = expand(config.stance, semKeys);
for (const [k, v] of Object.entries(stanceSem)) {
const joint = this.map[k] || robot.joints[k];
if (joint) this.stance[joint.name] = v;
}
this.gait = config.gait || {};
this.archetypes = [].concat(config.archetype || 'generic').map(a => {
if (!ARCHETYPES[a]) console.warn(`[make] 未知类型 ${a},使用 generic`);
return ARCHETYPES[a] || ARCHETYPES.generic;
});
this.locomotion = this.archetypes.some(a => a.locomotion);
this.usesBaseJoints = ['base.x', 'base.y', 'base.yaw'].some(k => this.map[k]);
this.mode = 'showcase';
this.t = 0;
this.phase = 0;
this.input = { forward: 0, turn: 0, action: false };
this.smooth = { forward: 0, turn: 0 };
this.base = { x: 0, y: 0, yaw: 0 };
this.velocity = { v: 0, w: 0 };
this.state = {}; // 模板的持久状态(轮子累计角度、机械臂驾驶位置等)
this.targets = {}; // jointName -> 目标值(平滑逼近)
this.direct = {}; // jointName -> 直接设置的值(连续旋转关节)
}
has(sem) { return !!this.map[sem]; }
// 语义关节 = stance + sign * offset
set(sem, offset) {
const j = this.map[sem];
if (!j) return;
const base = this.stance[j.name] ?? 0;
this.targets[j.name] = base + (this.sign[sem] ?? 1) * offset;
}
// 归一化 0..1 映射到关节限位区间(夹爪等)
setNormalized(sem, n) {
const j = this.map[sem];
if (!j) return;
const lo = Number(j.limit.lower), hi = Number(j.limit.upper);
this.targets[j.name] = lo + clamp(n, 0, 1) * (hi - lo);
}
setDirect(sem, value) {
const j = this.map[sem];
if (j) this.direct[j.name] = (this.sign[sem] ?? 1) * value;
}
semKeys(re) { return Object.keys(this.map).filter(k => re.test(k)); }
applyStance() {
for (const j of this.movable) j.setJointValue(this.stance[j.name] ?? 0);
}
resetBase() {
this.base = { x: 0, y: 0, yaw: 0 };
this.state = {};
}
setInput(input) { Object.assign(this.input, input); }
update(dt) {
const live = this.mode === 'drive';
// 展示模式放慢节奏:所有正弦动作和步态都按 tempo 缩放,看起来更从容
const tempo = live ? 1 : (this.config.showcaseTempo ?? 0.5);
const sdt = dt * tempo;
this.t += sdt;
this.smooth.forward = damp(this.smooth.forward, live ? this.input.forward : 0, 6, dt);
this.smooth.turn = damp(this.smooth.turn, live ? this.input.turn : 0, 6, dt);
this.velocity = { v: 0, w: 0 };
this.targets = {};
this.direct = {};
for (const j of this.movable) {
if (j.jointType !== 'continuous') this.targets[j.name] = this.stance[j.name] ?? 0;
}
for (const a of this.archetypes) a.update(this, sdt);
head(this);
if (live) {
const { v, w } = this.velocity;
this.base.yaw += w * dt;
this.base.x += v * Math.cos(this.base.yaw) * dt;
this.base.y += v * Math.sin(this.base.yaw) * dt;
if (this.usesBaseJoints) {
this.direct[this.map['base.x']?.name] = this.base.x;
this.direct[this.map['base.y']?.name] = this.base.y;
this.direct[this.map['base.yaw']?.name] = this.base.yaw;
delete this.direct.undefined;
}
}
const k = this.config.smoothing ?? 10;
for (const [name, target] of Object.entries(this.targets)) {
const j = this.joints[name];
j.setJointValue(damp(j.angle, target, k, dt));
}
for (const [name, value] of Object.entries(this.direct)) this.joints[name].setJointValue(value);
}
// 驾驶强度:展示模式下模板自己决定,驾驶模式取键盘输入
drive(showcaseForward = 0.5) {
if (this.mode === 'drive') return { fwd: this.smooth.forward, turn: this.smooth.turn };
return { fwd: showcaseForward, turn: 0 };
}
}
// 所有类型共用:头部左右看
function head(m) {
if (!m.has('head.yaw') && !m.has('head.pitch')) return;
if (m.mode === 'drive') {
m.set('head.yaw', m.smooth.turn * 0.5);
m.set('head.pitch', 0);
} else {
const amp = m.gait.headAmp ?? 1; // 展示时左右张望的幅度
m.set('head.yaw', 0.55 * amp * Math.sin(m.t * 0.45));
m.set('head.pitch', 0.18 * amp * Math.sin(m.t * 0.7 + 1));
}
}
function legGait(m, dt, legs, defaults, extra) {
const g = { ...defaults, ...m.gait };
const showcase = m.mode !== 'drive';
const { fwd, turn } = m.drive(0);
// 展示时步幅小、强弱缓慢起伏,像在原地轻轻踱步,而不是一直高抬腿
const ease = 0.35 + 0.65 * (0.5 + 0.5 * Math.sin(m.t * 0.7));
const activity = showcase ? 0.4 * ease : Math.min(1, Math.max(Math.abs(fwd), Math.abs(turn)));
if (activity > 0.02) m.phase = (m.phase + dt * TAU * g.cadence) % TAU;
const offsets = {};
for (const [leg, { phase, side }] of Object.entries(legs)) {
// 原地踏步(展示)时只抬脚不前后摆;驾驶时左右腿差速实现转向
const legDrive = showcase ? 0.3 * ease : clamp(fwd - turn * side * g.turnMix, -1, 1);
const phi = m.phase + phase;
const hip = g.stepLen * legDrive * Math.sin(phi);
const dir = legDrive >= 0 ? 1 : -1;
const knee = g.lift * activity * Math.max(0, -Math.cos(phi) * dir);
m.set(`leg.${leg}.hip`, hip);
m.set(`leg.${leg}.knee`, knee);
// 脚掌保持水平:踝 = -(髋 + 膝)。有的机器人髋和膝的转向相反,用 gait.footComp 调整系数
const fc = g.footComp || { hip: 1, knee: 1 };
m.set(`leg.${leg}.ankle`, extra?.flatFoot ? -(hip * fc.hip + knee * fc.knee) : -knee * 0.4);
// 有髋部偏航关节(leg.X.yaw)时转向靠扭髋:
// 着地的脚钉在地上,髋部随身体转动反向扭(脚在世界里方向不变);
// 抬起的脚提前朝转弯方向扭好,落地后接着当支撑脚
if (m.map[`leg.${leg}.yaw`]) {
const ys = (m.state.hipYaw ??= {});
const w = showcase ? 0 : turn * g.turnRate;
const stepTime = Math.PI / (TAU * g.cadence);
const reach = Math.min(0.45, Math.abs(w) * stepTime * 0.5) * Math.sign(w);
const swing = Math.cos(phi) < 0;
let y = ys[leg] ?? 0;
y = swing ? y + (reach - y) * Math.min(1, dt * 12) : y - w * dt;
if (!w) y *= Math.max(0, 1 - dt * 4); // 不转弯时慢慢回正
ys[leg] = Math.max(-0.45, Math.min(0.45, y));
m.set(`leg.${leg}.yaw`, ys[leg]);
}
offsets[leg] = hip;
}
if (!showcase) m.velocity = { v: fwd * g.speed, w: turn * g.turnRate };
return offsets;
}
export const ARCHETYPES = {
// 兜底:每个可动关节在限位内正弦摆动,任何 URDF 都能动起来(只用于展示)
generic: {
locomotion: false,
update(m) {
if (m.mode === 'drive') {
m.velocity = { v: m.smooth.forward * (m.gait.speed ?? 0.3), w: m.smooth.turn * (m.gait.turnRate ?? 1.2) };
return;
}
m.movable.forEach((j, i) => {
if (m.usesBaseJoints && Object.values(m.map).includes(j) && /^base\./.test(semOf(m, j))) return;
const w = 0.8 + (i % 3) * 0.15;
if (j.jointType === 'continuous') { m.direct[j.name] = m.t * 0.6; return; }
const lo = Number(j.limit.lower), hi = Number(j.limit.upper);
if (!(hi > lo)) return;
const range = hi - lo;
const amp = j.jointType === 'prismatic' ? range * 0.4 : Math.min(0.6, range * 0.25);
const center = clamp(m.stance[j.name] ?? 0, lo + amp, hi - amp);
m.targets[j.name] = center + amp * Math.sin(m.t * w + i * 0.9);
});
},
},
// 四足:对角小跑 (FL+RR / FR+RL)
quadruped: {
locomotion: true,
update(m, dt) {
legGait(m, dt, {
FL: { phase: 0, side: 1 }, RR: { phase: 0, side: -1 },
FR: { phase: Math.PI, side: -1 }, RL: { phase: Math.PI, side: 1 },
}, { stepLen: 0.35, lift: 0.7, cadence: 1.5, speed: 0.25, turnRate: 1.2, turnMix: 0.8 });
},
},
// 六足:三角步态 (L1+R2+L3 / R1+L2+R3),hip 一般是水平摆动的 coxa
hexapod: {
locomotion: true,
update(m, dt) {
const A = 0, B = Math.PI;
legGait(m, dt, {
L1: { phase: A, side: 1 }, R2: { phase: A, side: -1 }, L3: { phase: A, side: 1 },
R1: { phase: B, side: -1 }, L2: { phase: B, side: 1 }, R3: { phase: B, side: -1 },
}, { stepLen: 0.3, lift: 0.5, cadence: 1.7, speed: 0.18, turnRate: 1.0, turnMix: 1 });
},
},
// 双足:左右交替,脚掌保持水平,手臂反向摆动
biped: {
locomotion: true,
update(m, dt) {
const hips = legGait(m, dt, {
L: { phase: 0, side: 1 }, R: { phase: Math.PI, side: -1 },
}, { stepLen: 0.3, lift: 0.6, cadence: 1.3, speed: 0.15, turnRate: 0.9, turnMix: 0.6 }, { flatFoot: true });
m.set('arm.L.shoulder', -(hips.L ?? 0) * 1.4);
m.set('arm.R.shoulder', -(hips.R ?? 0) * 1.4);
},
},
// 机械臂:支持多条臂,语义 arm.<part> 或 arm.<side>.<part>
arm: {
locomotion: false,
update(m, dt) {
const parts = /^arm\.(?:([A-Za-z0-9]+)\.)?(base|shoulder|elbow|wrist_pitch|wrist_roll|gripper)$/;
const groups = [...new Set(m.semKeys(parts).map(k => k.match(parts)[1] ? `arm.${k.match(parts)[1]}.` : 'arm.'))];
const steer = m.mode === 'drive' && !m.locomotion;
groups.forEach((p, i) => {
if (steer) {
const s = (m.state[p] ??= { base: 0, reach: 0, grip: 1, last: false });
s.base = clamp(s.base + m.smooth.turn * 1.4 * dt, -2.5, 2.5);
s.reach = clamp(s.reach + m.smooth.forward * 0.9 * dt, -1, 1);
if (m.input.action && !s.last) s.grip = s.grip > 0.5 ? 0 : 1;
s.last = m.input.action;
m.set(p + 'base', s.base);
m.set(p + 'shoulder', s.reach * 0.8);
m.set(p + 'elbow', -s.reach * 0.6);
m.set(p + 'wrist_pitch', -s.reach * 0.2);
m.setNormalized(p + 'gripper', s.grip);
return;
}
if (m.mode === 'drive') return; // 跟随底盘移动时手臂保持 stance
const k = i * Math.PI;
const amp = m.gait.armAmp ?? 1; // 装在移动底盘上的手臂可以调小幅度
m.set(p + 'base', 0.9 * amp * Math.sin(m.t * 0.35 + k));
m.set(p + 'shoulder', 0.35 * amp * Math.sin(m.t * 0.6 + k));
m.set(p + 'elbow', -0.45 * amp * Math.sin(m.t * 0.6 + k + 0.6));
m.set(p + 'wrist_pitch', 0.4 * amp * Math.sin(m.t * 0.8 + k));
m.set(p + 'wrist_roll', 1.2 * amp * Math.sin(m.t * 0.4 + k));
m.setNormalized(p + 'gripper', 0.5 + 0.5 * Math.sin(m.t * 1.3 + k));
});
},
},
// 轮式差速底盘:语义 wheel.<FL|FR|RL|RR|L|R>,名字含 L 为左轮
wheeled: {
locomotion: true,
update(m, dt) {
const g = { speed: 0.4, turnRate: 1.5, wheelRadius: 0.05, track: 0.3, ...m.gait };
const showcase = m.mode !== 'drive';
const v = showcase ? 0.12 * Math.sin(m.t * 0.6) : m.smooth.forward * g.speed;
const w = showcase ? 0 : m.smooth.turn * g.turnRate;
for (const key of m.semKeys(/^wheel\./)) {
const left = /L/.test(key.slice(6));
const wheelV = v - w * (g.track / 2) * (left ? 1 : -1);
m.state[key] = (m.state[key] ?? 0) + (wheelV / g.wheelRadius) * dt;
m.setDirect(key, m.state[key]);
}
if (!showcase) m.velocity = { v, w };
},
},
};
function semOf(m, joint) {
return Object.keys(m.map).find(k => m.map[k] === joint) || '';
}