1c837766cd
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.
330 lines
13 KiB
JavaScript
330 lines
13 KiB
JavaScript
// 通用运动控制器:把「语义关节」(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) || '';
|
||
}
|