feat(ik): 避障路径改为多目标 Lazy RRT-Connect + 规划提速 32%
- 避障路径改为多目标 Lazy RRT-Connect + 捷径平滑;碰撞窄相位预测提速。 - 规划提速 32%:(2,4) 零件对类离线间隙表、撞点前移、判定记忆、routeFrom 惰性回退、平滑预算收紧。
This commit is contained in:
+26
-8
@@ -340,7 +340,7 @@ function animTo(q, ms, via) {
|
||||
end 保存求解器给的代表角,末帧落回它——与插值终点恰好相差 360° 的整数倍,
|
||||
同一物理角,视觉零跳变,同时保证滑块/关节表数值与求解结果一致。
|
||||
via:绕行中间位形(solveRoute 判直线被挡时给的两段式路径),逐段同样按短弧折算,
|
||||
每段各 ms 毫秒——渲染走的段与碰撞校验走的段是同一批,不存在"校验过了但动画穿模"。 */
|
||||
渲染走的段与碰撞校验走的段是同一批,不存在"校验过了但动画穿模"。 */
|
||||
const segs = [];
|
||||
let p = from;
|
||||
for (const raw of (via || []).concat([q])) {
|
||||
@@ -352,17 +352,34 @@ function animTo(q, ms, via) {
|
||||
segs.push(t); p = t;
|
||||
}
|
||||
const end = q.slice();
|
||||
const total = ms * segs.length;
|
||||
/* 时间参数化(2026-10-15):旧版每段固定 ms 且每段各自 smoothstep —— 每个中间位形都
|
||||
减速到 0 再起步(顿挫),且 5° 的小段和 150° 的大段用同样时间(小段慢吞吞、大段猛甩)。
|
||||
现在:各段时长 ∝ 该段"最慢轴所需时间"(按轴速比 VMAX 折算,近端轴更慢),
|
||||
整条折线只做一次全局 smoothstep:只在起点/终点加减速,途经中间位形不停顿。
|
||||
走的仍是校验过的那条折线(中间位形经捷径平滑后通常只剩 1~2 个,折角很少)。 */
|
||||
const VREL = [0.6, 0.6, 0.9, 0.9, 1.2, 1.8];
|
||||
const pts = [from].concat(segs), dur = [];
|
||||
for (let i = 1; i < pts.length; i++) {
|
||||
let t = 0;
|
||||
for (let j = 0; j < 6; j++) t = Math.max(t, Math.abs(pts[i][j] - pts[i - 1][j]) / VREL[j]);
|
||||
dur.push(Math.max(t, 1e-3));
|
||||
}
|
||||
const sum = dur.reduce((a, b) => a + b, 0);
|
||||
/* 总时长:ms 视为"标准 90° 移动"的时长,按路径实际长度伸缩,夹在 [0.6ms, 3.5ms] */
|
||||
const total = Math.max(ms * 0.6, Math.min(ms * 3.5, ms * sum / 150));
|
||||
const cum = [0]; for (const d of dur) cum.push(cum[cum.length - 1] + d / sum);
|
||||
anim = { from: from, to: segs[segs.length - 1], t0: performance.now(), ms: total };
|
||||
const tick = () => {
|
||||
if (!anim) return;
|
||||
const el = performance.now() - anim.t0;
|
||||
const si = Math.min(segs.length - 1, Math.floor(el / ms));
|
||||
const k = Math.min(1, (el - si * ms) / ms);
|
||||
const e = k * k * (3 - 2 * k);
|
||||
const a0 = si === 0 ? from : segs[si - 1], a1 = segs[si];
|
||||
const u = Math.min(1, el / total);
|
||||
const k = u;
|
||||
const e = 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];
|
||||
for (let i = 0; i < 6; i++) {
|
||||
const id = ORDER[i], a = a0[i] + (a1[i] - a0[i]) * e;
|
||||
const id = ORDER[i], a = a0[i] + (a1[i] - a0[i]) * f;
|
||||
ang[id] = a;
|
||||
const sl = $('s' + id), v = $('v' + id);
|
||||
if (sl) sl.value = a;
|
||||
@@ -492,7 +509,8 @@ function renderResult(res, src) {
|
||||
: res.blocked
|
||||
? '路径被立柱挡住 · 已停在边界(目标差 ' + dsc.toFixed(1) + ' mm)'
|
||||
: res.detour
|
||||
? (res.detour === 'retract_pass' ? '避障到达 · 模拟人类回退绕过立柱'
|
||||
? (res.detour === 'planned' ? '避障到达 · 规划绕行(' + ((res.via || []).length) + ' 个中间位形)'
|
||||
: res.detour === 'retract_pass' ? '避障到达 · 模拟人类回退绕过立柱'
|
||||
: (res.detour === 'gentle' ? '避障到达 · 微调协同避开立柱'
|
||||
: (res.detour === 'coordinated' ? '避障到达 · 平滑协同跨越立柱'
|
||||
: '绕行立柱后到达')))
|
||||
|
||||
Reference in New Issue
Block a user