feat(ik): 实时避障版:离线路网 PRM + 在线 Lazy A* + 终身增广 + 拖拽热启动;实时管线全程细验(拖拽穿模帧 430→0,重规划 P50 5 ms);前端每帧碰撞守护

This commit is contained in:
Hark
2026-10-18 06:40:22 +08:00
committed by cbb
parent 318ad0e264
commit 46bdb98dd8
6 changed files with 512 additions and 21 deletions
+26 -2
View File
@@ -330,9 +330,30 @@ function setJoints(q, doAnim, via, ms) {
refreshTarget();
if (COL) refreshJointLabelsLazy(ORDER.map(id => ang[id]));
}
/* 实时碰撞守护(2026-10-15):动画每帧对当前显示位形做一次碰撞判定(~1.2 ms/帧),
命中立即停住并亮红标。规划给的折线本已按 1.5° 细验,这里是最后一道独立防线,
也让"此刻是否碰撞"实时可见。统计供测试/控制台:window.__RT_GUARD()。 */
const RTG = { frames: 0, hits: 0, ms: 0, last: null };
window.__RT_GUARD = () => Object.assign({}, RTG, { avgMs: RTG.frames ? RTG.ms / RTG.frames : 0 });
function rtGuard(qNow) {
if (!S || !S.collideFree) return true;
const t = performance.now();
const ok = S.collideFree(qNow);
RTG.ms += performance.now() - t; RTG.frames++;
if (!ok) {
RTG.hits++; RTG.last = qNow.slice();
const b = document.getElementById('ikBadge');
if (b) { b.textContent = '实时碰撞守护:检测到干涉,已停住'; b.className = 'badge badb'; }
}
return ok;
}
function animTo(q, ms, via) {
/* 实时跟随:上一段动画还在走时被新规划打断,新动画从"正在运动"接续——
不再从零速缓入(否则每次重规划都顿一下),只在终点缓出。 */
const moving = !!anim;
if (anim) { anim = null; }
const from = ORDER.map(id => ang[id]);
const guardOn = !S.collideFree || S.collideFree(from); // 起点已在干涉里(脱困动作)不设守护
/* 连续轴(J1 肩抬举 / J3 大臂自转 / J6 腕自转)走「短弧」。
三者物理上无机械限位,θ 与 θ±360 是同一物理位形(applyJoint 走 cos/sin);
若按数值直接线性插值,+151.9° → −167.0° 会反向扫 318.9° 绕一整圈(实测)。
@@ -374,12 +395,15 @@ function animTo(q, ms, via) {
const el = performance.now() - anim.t0;
const u = Math.min(1, el / total);
const k = u;
const e = u * u * (3 - 2 * u); // 全局一次缓入缓出
const e = moving ? 1 - (1 - u) * (1 - u) // 接续运动:只缓出
: u * u * (3 - 2 * u); // 静止起步:全局一次缓入缓出
let si = 0; while (si < dur.length - 1 && e > cum[si + 1]) si++;
const f = Math.min(1, Math.max(0, (e - cum[si]) / (cum[si + 1] - cum[si])));
const a0 = pts[si], a1 = pts[si + 1];
const qNow = a0.map((v, i) => v + (a1[i] - v) * f);
if (guardOn && !rtGuard(qNow)) { anim = null; return; } // 停在上一帧(已判无干涉)的位形
for (let i = 0; i < 6; i++) {
const id = ORDER[i], a = a0[i] + (a1[i] - a0[i]) * f;
const id = ORDER[i], a = qNow[i];
ang[id] = a;
const sl = $('s' + id), v = $('v' + id);
if (sl) sl.value = a;