3247 lines
178 KiB
JavaScript
3247 lines
178 KiB
JavaScript
/* ============================================================================
|
||
* arm-ik-solver.js — 六轴手臂逆运动学求解器（最小总关节角）
|
||
*
|
||
* 正运动学：World = A1(θ1)·A2(θ2)·A3(θ3)·A4(θ4)·A5(θ5)·A6(θ6)·TCP，
|
||
* Ai(θ) = T(Pi)·R(axis_i, θ)·T(−Pi)，与查看器场景图完全一致。
|
||
* 逆运动学：以 (θ4,θ5) 为二维冗余参数全域扫描，其余三轴闭式求解。
|
||
*
|
||
* MIT / 无依赖。可在浏览器（window.ArmIK）或 Node（module.exports）中加载。
|
||
* ==========================================================================*/
|
||
(function (root, factory) {
|
||
if (typeof module === 'object' && module.exports) module.exports = factory();
|
||
else root.ArmIK = factory();
|
||
}(typeof self !== 'undefined' ? self : this, function () {
|
||
'use strict';
|
||
|
||
const D2R = Math.PI / 180, R2D = 180 / Math.PI;
|
||
const ORDER = ['J1', 'J2', 'J3', 'J4', 'J5', 'J6'];
|
||
|
||
/* ---------- 小工具 ---------- */
|
||
const wrap180 = a => { a = (a + 180) % 360; if (a < 0) a += 360; return a - 180; };
|
||
const nrm = v => { const n = Math.hypot(v[0], v[1], v[2]) || 1; return [v[0] / n, v[1] / n, v[2] / n]; };
|
||
function rotAxis(d, a) { // 3x3 旋转：绕单位轴 d 转 a 弧度（右手法）
|
||
const c = Math.cos(a), s = Math.sin(a);
|
||
/* 轴对齐特化：查看器 6 个关节轴均为 ±X/±Y/±Z，Rodrigues 通用式退化为每轴 4 个非零元素
|
||
（数学恒等；绕 −X 转 a = 绕 X 转 −a）。姿态路径热点（rotAxis 曾占 ~11%）。 */
|
||
const x = d[0], y = d[1], z = d[2];
|
||
if (y === 0 && z === 0) {
|
||
if (x > 0) return [1, 0, 0, 0, c, -s, 0, s, c];
|
||
return [1, 0, 0, 0, c, s, 0, -s, c];
|
||
}
|
||
if (x === 0 && z === 0) {
|
||
if (y > 0) return [c, 0, s, 0, 1, 0, -s, 0, c];
|
||
return [c, 0, -s, 0, 1, 0, s, 0, c];
|
||
}
|
||
if (x === 0 && y === 0) {
|
||
if (z > 0) return [c, -s, 0, s, c, 0, 0, 0, 1];
|
||
return [c, s, 0, -s, c, 0, 0, 0, 1];
|
||
}
|
||
const C = 1 - c;
|
||
return [
|
||
x * x * C + c, x * y * C - z * s, x * z * C + y * s,
|
||
y * x * C + z * s, y * y * C + c, y * z * C - x * s,
|
||
z * x * C - y * s, z * y * C + x * s, z * z * C + c
|
||
];
|
||
}
|
||
const mul3 = (A, B) => {
|
||
const C = new Array(9);
|
||
for (let i = 0; i < 3; i++) for (let j = 0; j < 3; j++)
|
||
C[i * 3 + j] = A[i * 3] * B[j] + A[i * 3 + 1] * B[3 + j] + A[i * 3 + 2] * B[6 + j];
|
||
return C;
|
||
};
|
||
const mv3 = (A, v) => [
|
||
A[0] * v[0] + A[1] * v[1] + A[2] * v[2],
|
||
A[3] * v[0] + A[4] * v[1] + A[5] * v[2],
|
||
A[6] * v[0] + A[7] * v[1] + A[8] * v[2]
|
||
];
|
||
const cross3 = (a, b) => [a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]];
|
||
const T3 = M => [M[0], M[3], M[6], M[1], M[4], M[7], M[2], M[5], M[8]]; // 3x3 转置
|
||
const clamp1 = v => v < -1 ? -1 : (v > 1 ? 1 : v);
|
||
|
||
/* 两线段最近距离（Ericson《Real-Time Collision Detection》标准式）。
|
||
动态限位的度量基础：解析式必须敢被数值校验——node 侧对随机线段与
|
||
暴力密集采样对比（scratch 自检脚本），别只看"数值像是对的"。
|
||
内核收标量直传（碰撞热循环每次调用上万次，建数组的分配开销实测占一半），
|
||
数组版仅作对外包装/测试用。 */
|
||
function segSegDistRaw(x1, y1, z1, x2, y2, z2, x3, y3, z3, x4, y4, z4) {
|
||
const d1x = x2 - x1, d1y = y2 - y1, d1z = z2 - z1;
|
||
const d2x = x4 - x3, d2y = y4 - y3, d2z = z4 - z3;
|
||
const rx = x1 - x3, ry = y1 - y3, rz = z1 - z3;
|
||
const a = d1x * d1x + d1y * d1y + d1z * d1z;
|
||
const e = d2x * d2x + d2y * d2y + d2z * d2z;
|
||
const f = d2x * rx + d2y * ry + d2z * rz;
|
||
const cl01 = v => v < 0 ? 0 : (v > 1 ? 1 : v);
|
||
let s = 0, t = 0;
|
||
if (a <= 1e-12 && e <= 1e-12) { s = 0; t = 0; }
|
||
else if (a <= 1e-12) { s = 0; t = cl01(f / e); }
|
||
else {
|
||
const c = d1x * rx + d1y * ry + d1z * rz;
|
||
if (e <= 1e-12) { t = 0; s = cl01(-c / a); }
|
||
else {
|
||
const b = d1x * d2x + d1y * d2y + d1z * d2z;
|
||
const denom = a * e - b * b;
|
||
s = denom > 1e-12 ? cl01((b * f - c * e) / denom) : 0;
|
||
t = (b * s + f) / e;
|
||
if (t < 0) { t = 0; s = cl01(-c / a); }
|
||
else if (t > 1) { t = 1; s = cl01((b - c) / a); }
|
||
}
|
||
}
|
||
const cx = x1 + d1x * s - (x3 + d2x * t);
|
||
const cy = y1 + d1y * s - (y3 + d2y * t);
|
||
const cz = z1 + d1z * s - (z3 + d2z * t);
|
||
return Math.sqrt(cx * cx + cy * cy + cz * cz);
|
||
}
|
||
function segSegDist(p1, q1, p2, q2) {
|
||
return segSegDistRaw(p1[0], p1[1], p1[2], q1[0], q1[1], q1[2],
|
||
p2[0], p2[1], p2[2], q2[0], q2[1], q2[2]);
|
||
}
|
||
|
||
/* 两旋转矩阵间的夹角（度）：θ = acos((tr(AᵀB) − 1) / 2)，A、B 均为正交阵。
|
||
tr(AᵀB) = Σᵢⱼ AᵢⱼBᵢⱼ —— 行主序即两数组逐元素点积。 */
|
||
function rotErrDeg(A, B) {
|
||
let tr = 0;
|
||
for (let i = 0; i < 9; i++) tr += A[i] * B[i];
|
||
return Math.acos(clamp1((tr - 1) / 2)) * R2D;
|
||
}
|
||
|
||
/* 姿态参数（CAD 系）→ 工具坐标系旋转矩阵（行主序 9 数）。
|
||
az：工具轴（法兰面 +Z）绕 CAD Z 轴的方位角；el：工具轴自 CAD +Z 抬起的仰角（0=朝上，180=朝下）；
|
||
roll：绕工具轴自身的滚转角（右手系）。
|
||
列向量构成工具坐标系：z = 工具轴；x0 = 子午面基（−sin az, cos az, 0），恒垂直于 z；
|
||
x = 绕 z 滚转 roll 后的 x0；y = z × x 保证右手系。 */
|
||
function poseMat(azDeg, elDeg, rollDeg) {
|
||
const az = azDeg * D2R, el = elDeg * D2R, ro = rollDeg * D2R;
|
||
const ca = Math.cos(az), sa = Math.sin(az);
|
||
const z = [Math.sin(el) * ca, Math.sin(el) * sa, Math.cos(el)];
|
||
const x0 = [-sa, ca, 0];
|
||
const x = mv3(rotAxis(z, ro), x0);
|
||
const y = cross3(z, x);
|
||
return [x[0], y[0], z[0], x[1], y[1], z[1], x[2], y[2], z[2]];
|
||
}
|
||
|
||
/* 姿态矩阵 → {az, el, roll}（poseMat 的逆分解，度）。el ∈ [0,180]，az ∈ (−180,180]，roll ∈ (−180,180]
|
||
R = R0·Rx(roll)（R0 = poseMat(az,el,0)，列为 x0/y0/z）⇒ 工具 x 列 = cos(roll)·x0 + sin(roll)·y0 */
|
||
function poseAng(R) {
|
||
const z = [R[2], R[5], R[8]], x = [R[0], R[3], R[6]]; // 行主序：列 2 = 工具轴，列 0 = 工具 x
|
||
const az = Math.atan2(z[1], z[0]) * R2D;
|
||
const el = Math.atan2(Math.hypot(z[0], z[1]), z[2]) * R2D;
|
||
const x0 = [-Math.sin(az * D2R), Math.cos(az * D2R), 0];
|
||
const y0 = cross3(z, x0);
|
||
const d0 = x[0] * x0[0] + x[1] * x0[1] + x[2] * x0[2];
|
||
const d1 = x[0] * y0[0] + x[1] * y0[1] + x[2] * y0[2];
|
||
return { az: wrap180(az), el: el, roll: wrap180(Math.atan2(d1, d0) * R2D) };
|
||
}
|
||
|
||
/* ==========================================================================*/
|
||
function make(cfg) {
|
||
/* ---------- 0. 关节表 → 运动学常量 ---------- */
|
||
const JJ = cfg.joints, TR = cfg.travel || {};
|
||
const P = {}, D = {};
|
||
for (const id of ORDER) {
|
||
if (!JJ[id]) throw new Error('缺少关节 ' + id);
|
||
P[id] = JJ[id].rear.slice();
|
||
D[id] = nrm(JJ[id].d);
|
||
}
|
||
const LIM = {};
|
||
for (const id of ORDER) {
|
||
const t = TR[id] || { lo: -360, hi: 360, continuous: true };
|
||
/* 注意：LIM 是【求解器的规范代表角区间】，不是界面允许范围。
|
||
连续轴（J1/J3/J6）物理上无机械限位——界面滑块/动态限位已放到 ±360°（见下面的
|
||
joint sliders 与 dynLimit），但解一律取 |θ| 最小的等价角，故规范区间仍保持 ±180°。
|
||
若此处也放到 ±360°：closest 的随机重启按 [LIM.lo, LIM.hi] 撒点，会散布到 ±360°，
|
||
实测不可达兜底的连续轴最大 |θ| 从 176° 涨到 356°、平均 Σ|θ| +24%，动画会绕远路。
|
||
非连续轴仍用真实行程。 */
|
||
LIM[id] = { lo: +t.lo, hi: +t.hi, cont: !!t.continuous };
|
||
}
|
||
|
||
/* ---------- 0b. 动态碰撞模型（胶囊近似，cfg.collide 可选） ----------
|
||
数据由 scratch/fit_capsules.py 从 CAD 网格拟合写 docs/collide.json：
|
||
caps[i] = {g, cn, a, b, r}，g=0(root 固定)..6(J6)，端点为零位 CAD 世界坐标。
|
||
组 g 的胶囊受 A1..Ag 嵌套旋转（与查看器场景图/此处 fkChain 同一约定）。
|
||
只检 |Δg|≥2 的零件对：同组相对静止（零位单实体检查已保证不干涉），
|
||
相邻组相对位姿只由中间关节决定、与其它关节无关（sweep_v2 单关节硬限位
|
||
即此类，如 P04×P05/P06×P07/P07×P08），非相邻对（含撞固定立柱）才随姿态
|
||
大变——这正是静态限位过松/过紧的来源。
|
||
两级宽相位：零件级包围球（变换 19 个球心）先筛，球对分离即跳过该零件对
|
||
的全部胶囊对；只有邻近零件对才变换胶囊端点做线段距离。 */
|
||
const COL = cfg.collide || null;
|
||
const CAPG = COL ? COL.caps : null; // [{g,cn,a,b,r}]
|
||
const CAPM = COL ? +(COL.margin_mm != null ? COL.margin_mm : 1.0) : 0;
|
||
const MOTOR = COL ? +(COL.motor_limit_deg || 0) : 0; // 电机机械限位（度）
|
||
/* 静态限位每侧硬度（fit_capsules.py 按 travel_detail 首个卡死对判定）：
|
||
硬边 = 相邻组零件自碰撞，与其它关节无关 → 任何姿态都取静态值，不放宽；
|
||
可放宽 = 卡死对跨 ≥2 组（典型：腕/法兰撞固定立柱）→ 只在零位成立，
|
||
由动态模型按当前姿态重算。 */
|
||
const HARD = (COL && COL.hard) || {};
|
||
const ADJ = (COL && COL.adj) || {}; // 相邻打印件对的 1D 自由角区间（度）
|
||
const PART = [], PARTPAIR = []; // 零件级（宽相位）与零件对
|
||
/* 胶囊半长（窄相位前的胶囊级包围球预筛用，2026-10-15） */
|
||
const HLEN = CAPG ? CAPG.map(c => 0.5 * Math.hypot(c.b[0] - c.a[0], c.b[1] - c.a[1], c.b[2] - c.a[2])) : null;
|
||
if (COL) {
|
||
const pidx = {};
|
||
for (const nm in COL.parts) {
|
||
const caps = COL.parts[nm];
|
||
if (!caps.length) continue;
|
||
const g = CAPG[caps[0]].g;
|
||
let cx = 0, cy = 0, cz = 0;
|
||
for (const i of caps) {
|
||
cx += (CAPG[i].a[0] + CAPG[i].b[0]) / 2; cy += (CAPG[i].a[1] + CAPG[i].b[1]) / 2;
|
||
cz += (CAPG[i].a[2] + CAPG[i].b[2]) / 2;
|
||
}
|
||
cx /= caps.length; cy /= caps.length; cz /= caps.length;
|
||
let rad = 0, rmax = 0;
|
||
for (const i of caps) {
|
||
const c = CAPG[i];
|
||
rad = Math.max(rad, Math.hypot((c.a[0] + c.b[0]) / 2 - cx, (c.a[1] + c.b[1]) / 2 - cy,
|
||
(c.a[2] + c.b[2]) / 2 - cz)
|
||
+ 0.5 * Math.hypot(c.b[0] - c.a[0], c.b[1] - c.a[1], c.b[2] - c.a[2]) + c.r);
|
||
rmax = Math.max(rmax, c.r);
|
||
}
|
||
pidx[nm] = PART.length;
|
||
PART.push({ cn: CAPG[caps[0]].cn, g: g, caps: caps, c: [cx, cy, cz], rad: rad, rmax: rmax,
|
||
_i: PART.length });
|
||
}
|
||
/* 簇级宽相位：每件胶囊按主轴投影分桶（每桶 ≤CAPS_PER 枚）。桶球半径 ~15-25，
|
||
零位臂-立柱间隙 ~44mm 也只能筛掉一部分——实测再细分（目标半径 20mm）会让
|
||
簇球对测试量爆炸（62 万次，27ms/check），得不偿失；速度靠戳记缓存与用法层
|
||
的边界缓存解决（见 dynLimit 与 glue 的滑块夹取）。 */
|
||
const CAPS_PER = 4;
|
||
for (const pt of PART) {
|
||
const mids = pt.caps.map(i => [(CAPG[i].a[0] + CAPG[i].b[0]) / 2,
|
||
(CAPG[i].a[1] + CAPG[i].b[1]) / 2,
|
||
(CAPG[i].a[2] + CAPG[i].b[2]) / 2]);
|
||
let u = [1, 0, 0]; // 幂迭代求主轴（5 轮足够定向）
|
||
for (let it = 0; it < 5; it++) {
|
||
const sm = [0, 0, 0];
|
||
for (const m of mids) {
|
||
const d = m[0] * u[0] + m[1] * u[1] + m[2] * u[2];
|
||
sm[0] += d * m[0]; sm[1] += d * m[1]; sm[2] += d * m[2];
|
||
}
|
||
const n = Math.hypot(sm[0], sm[1], sm[2]);
|
||
if (n < 1e-9) { u = [1, 0, 0]; break; }
|
||
u = [sm[0] / n, sm[1] / n, sm[2] / n];
|
||
}
|
||
const ord = pt.caps.map((c, k) => [mids[k][0] * u[0] + mids[k][1] * u[1] + mids[k][2] * u[2], k])
|
||
.sort((a, b) => a[0] - b[0]).map(x => x[1]);
|
||
const G = Math.max(1, Math.ceil(pt.caps.length / CAPS_PER));
|
||
pt.clus = [];
|
||
for (let k = 0; k < G; k++) {
|
||
const cs = ord.slice(Math.floor(k * pt.caps.length / G),
|
||
Math.floor((k + 1) * pt.caps.length / G));
|
||
if (!cs.length) continue;
|
||
let mx = 0, my = 0, mz = 0;
|
||
for (const k2 of cs) { mx += mids[k2][0]; my += mids[k2][1]; mz += mids[k2][2]; }
|
||
mx /= cs.length; my /= cs.length; mz /= cs.length;
|
||
let rad = 0, rmax = 0;
|
||
for (const k2 of cs) {
|
||
const c = CAPG[pt.caps[k2]];
|
||
rad = Math.max(rad, Math.hypot(mids[k2][0] - mx, mids[k2][1] - my, mids[k2][2] - mz)
|
||
+ 0.5 * Math.hypot(c.b[0] - c.a[0], c.b[1] - c.a[1], c.b[2] - c.a[2]) + c.r);
|
||
rmax = Math.max(rmax, c.r);
|
||
}
|
||
pt.clus.push({ c: [mx, my, mz], rad: rad, rmax: rmax, caps: cs.map(k2 => pt.caps[k2]) });
|
||
}
|
||
}
|
||
const names = Object.keys(COL.parts).filter(n => COL.parts[n].length);
|
||
for (let x = 0; x < names.length; x++) for (let y = x + 1; y < names.length; y++) {
|
||
const A = PART[pidx[names[x]]], B = PART[pidx[names[y]]];
|
||
const dg = Math.abs(A.g - B.g);
|
||
if (dg < 2) continue; // 同组/相邻组 → 静态限位或 1D 间隔表
|
||
/* 相邻组打印件对（P04×P05 / P06×P07 / P07×P08）**不能**塞进胶囊模型：两件
|
||
同心跨在同一关节轴上，胶囊轴距≈0，给多少重叠容忍都会在零位误报（实测
|
||
P04×P03 超报 15mm、P06×P07 超报 14mm）。它们是"只由中间关节决定的 1D
|
||
约束"，由 fit_capsules 生成的 ADJ 区间表（collide.json 的 adj 字段）
|
||
在 dynLimit 里按角度查表处理。 */
|
||
/* 相邻组打印件对（P04×P05 / P06×P07 / P07×P08）纳入动态模型：其相对位姿只由
|
||
中间关节决定、与姿态无关，但在另一侧可能被动态对（撞立柱）抢先卡死，静态
|
||
单轴扫描只会记下先撞的那一对——不含它们就会在另一侧漏判（实测 J5+ 在
|
||
J4=-100 时放到电机限 106，真值 96，就是 P07×P08 在 +96 侧的自碰撞）。 */
|
||
|
||
const cl = [];
|
||
for (let ia = 0; ia < A.clus.length; ia++) for (let ib = 0; ib < B.clus.length; ib++) {
|
||
const ca = A.clus[ia], cb = B.clus[ib];
|
||
const pr = [];
|
||
for (const i of ca.caps) for (const j of cb.caps)
|
||
pr.push([i, j, CAPG[i].r + CAPG[j].r + CAPM, HLEN[i] + HLEN[j] + CAPG[i].r + CAPG[j].r + CAPM]);
|
||
if (pr.length) cl.push([ia, ib, pr, ca.rad + cb.rad, ca.rmax + cb.rmax]);
|
||
}
|
||
if (cl.length) PARTPAIR.push([A, B, cl, A.rad + B.rad, A.rmax + B.rmax]);
|
||
}
|
||
}
|
||
/* ---------- 0c. 两关节零件对类的预计算间隙表（2026-10-15） ----------
|
||
零件对类 (ga, gb) 且 gb−ga=2 时，两组的相对位姿只由中间两个关节决定（如 (2,4)：P05/M3 ×
|
||
P07/M5 只取决于 J3、J4）。实测 (2,4) 类长期相距 ~5 mm、平面相对，簇级宽相位筛不掉，
|
||
独占每次 collideFree 约 80% 的线段距离计算。离线把该类最小间隙按 1° 网格打成表
|
||
（collide.json 的 pair2d 字段，webapp/tools/gen_pair2d.cjs 生成），运行时取最近网格点，
|
||
按 Lipschitz 界给出间隙上下界：下界 > 0 → 该类整类跳过；上界 < 0 → 直接判撞；
|
||
夹在中间才回退精算。结果与精算逐位一致（保守），只省时间。
|
||
指纹不匹配（重新拟合过胶囊但没重生成表）时整表弃用，自动回退精算。 */
|
||
function classOf(A, B) { return A.g < B.g ? A.g * 7 + B.g : B.g * 7 + A.g; }
|
||
function classFingerprint(ci) {
|
||
let h = 2166136261 >>> 0;
|
||
const mix = v => { h = Math.imul(h ^ (Math.round(v * 1000) | 0), 16777619) >>> 0; };
|
||
mix(CAPM);
|
||
const inCls = new Set();
|
||
for (const pp of PARTPAIR) if (classOf(pp[0], pp[1]) === ci) { inCls.add(pp[0]._i); inCls.add(pp[1]._i); }
|
||
for (const pt of PART) { // 按 PART 固定顺序（PARTPAIR 运行期会前移重排）
|
||
if (!inCls.has(pt._i)) continue;
|
||
for (const i of pt.caps) { const c = CAPG[i]; mix(c.g); mix(c.r); for (const v of c.a) mix(v); for (const v of c.b) mix(v); }
|
||
}
|
||
return h.toString(16);
|
||
}
|
||
const P2D = {}; // ci → 表
|
||
if (COL && COL.pair2d) {
|
||
for (const key in COL.pair2d) {
|
||
const T = COL.pair2d[key], ci = +key;
|
||
const ga = Math.floor(ci / 7), gb = ci % 7;
|
||
if (gb - ga !== 2) continue;
|
||
if (T.fp !== classFingerprint(ci)) { if (typeof console !== 'undefined') console.warn('[IK] pair2d 类 ' + key + ' 指纹不符，弃用（重跑 webapp/tools/gen_pair2d.cjs）'); continue; }
|
||
let gap;
|
||
if (typeof atob === 'function') { const bin = atob(T.b64); gap = new Int16Array(bin.length >> 1); for (let i = 0; i < gap.length; i++) gap[i] = (bin.charCodeAt(2 * i) | (bin.charCodeAt(2 * i + 1) << 8)) << 16 >> 16; }
|
||
else { const buf = Buffer.from(T.b64, 'base64'); gap = new Int16Array(buf.length >> 1); for (let i = 0; i < gap.length; i++) gap[i] = buf.readInt16LE(2 * i); }
|
||
/* 杠杆界（mm/°）：高组胶囊轴端点 p 在低组坐标系里的速度 ≤ |ω_a|·dist(p, 轴a) + |ω_b|·dist(p, 轴b)，
|
||
轴b 与高组同刚体 → dist(p,轴b) 不变；dist(p,轴a) ≤ dist(枢轴b, 轴a) + |p − 枢轴b|（两者对两关节转动都不变）。
|
||
只用轴端点（半径不随运动变），距离对线段凸 → 端点取极值即可。 */
|
||
const ja = ga, jb = ga + 1; // 关节下标（0 基）：带动组 ga+1 与 ga+2
|
||
const Pa = P[ORDER[ja]], Da = D[ORDER[ja]], Pb = P[ORDER[jb]], Db = D[ORDER[jb]];
|
||
const perp = (p, P0, d) => { const v = [p[0] - P0[0], p[1] - P0[1], p[2] - P0[2]]; const t = v[0] * d[0] + v[1] * d[1] + v[2] * d[2]; return Math.hypot(v[0] - t * d[0], v[1] - t * d[1], v[2] - t * d[2]); };
|
||
const pbA = perp(Pb, Pa, Da);
|
||
let La = 0, Lb = 0;
|
||
for (const pp of PARTPAIR) {
|
||
if (classOf(pp[0], pp[1]) !== ci) continue;
|
||
const hi = pp[0].g === gb ? pp[0] : pp[1];
|
||
for (const i of hi.caps) for (const p of [CAPG[i].a, CAPG[i].b]) {
|
||
La = Math.max(La, pbA + Math.hypot(p[0] - Pb[0], p[1] - Pb[1], p[2] - Pb[2]));
|
||
Lb = Math.max(Lb, perp(p, Pb, Db));
|
||
}
|
||
}
|
||
P2D[ci] = { ja: ja, jb: jb, lo: T.lo, h: T.h, n: T.n, ca: !!LIM[ORDER[ja]].cont, gap: gap,
|
||
La: La * D2R, Lb: Lb * D2R, st: 0, gen: -1 };
|
||
}
|
||
}
|
||
/* 查表：1=该类必无干涉，−1=必干涉，0=不确定（需精算） */
|
||
function p2dStatus(T, q) {
|
||
let a = q[T.ja], b = q[T.jb];
|
||
if (T.ca) a = wrap180(a);
|
||
let ia = Math.round((a - T.lo[0]) / T.h), ib = Math.round((b - T.lo[1]) / T.h);
|
||
if (T.ca) ia = ((ia % T.n[0]) + T.n[0]) % T.n[0];
|
||
if (ia < 0 || ia >= T.n[0] || ib < 0 || ib >= T.n[1]) return 0;
|
||
let da = Math.abs(a - (T.lo[0] + ia * T.h)); if (T.ca) da = Math.abs(wrap180(a - (T.lo[0] + ia * T.h)));
|
||
const db = Math.abs(b - (T.lo[1] + ib * T.h));
|
||
const g = T.gap[ia * T.n[1] + ib] * 0.01, sl = T.La * da + T.Lb * db + 1e-6;
|
||
if (g - sl > 0) return 1;
|
||
if (g + sl < 0 && g > -327) return -1;
|
||
return 0;
|
||
}
|
||
/* 某类在姿态 q 下的精确最小间隙（生成表用）：需 cumXf 之后的变换缓冲，故走一次独立扫描 */
|
||
function classGap(q, ci) {
|
||
let best = Infinity;
|
||
const save = P2D[ci]; delete P2D[ci];
|
||
try {
|
||
scanPairs(q, (na, nb, d, need) => {}, ci, g => { if (g < best) best = g; });
|
||
} finally { if (save) P2D[ci] = save; }
|
||
return Math.min(best, 20); // ≥20 mm 一律记 20（下界，保守）
|
||
}
|
||
/* 组累积刚体变换 cum[k]：(R,t) 使 world = R·local + t，k=1..6（0 为恒等） */
|
||
/* 碰撞热路径预分配（一次性）：替代每次 check 重建 clc/cache/pcc（约 2500 个小数组/次，
|
||
GC 占单次判定 ~13%）。普通数组 + 代际戳，元素类型统一（数字 + 一个字符串 cn）——
|
||
实测优于 Float64Array 方案（混入字符串会污染元素类型反而更慢）。 */
|
||
const XFBUF = COL ? CAPG.map(c => { const e = new Array(8); e[6] = c.cn; e[7] = -1; return e; }) : null;
|
||
const CLBUF = COL ? PART.map(pt => pt.clus.map(() => { const e = new Array(4); e[3] = -1; return e; })) : null;
|
||
const PCBUF = COL ? PART.map(() => { const e = new Array(4); e[3] = -1; return e; }) : null;
|
||
let CGEN = 0;
|
||
function cumXf(q) {
|
||
let R = [1, 0, 0, 0, 1, 0, 0, 0, 1], T = [0, 0, 0];
|
||
const Rs = [R.slice()], Ts = [T.slice()];
|
||
for (let k = 0; k < 6; k++) {
|
||
const Pk = P[ORDER[k]], Rk = rotAxis(D[ORDER[k]], q[k] * D2R);
|
||
const rp = mv3(Rk, Pk); // A_k = T(Pk)·Rk·T(−Pk)
|
||
const tk = [Pk[0] - rp[0], Pk[1] - rp[1], Pk[2] - rp[2]];
|
||
const nR = mul3(R, Rk);
|
||
const nT = [R[0] * tk[0] + R[1] * tk[1] + R[2] * tk[2] + T[0],
|
||
R[3] * tk[0] + R[4] * tk[1] + R[5] * tk[2] + T[1],
|
||
R[6] * tk[0] + R[7] * tk[1] + R[8] * tk[2] + T[2]];
|
||
R = nR; T = nT;
|
||
Rs.push(R.slice()); Ts.push(T.slice());
|
||
}
|
||
return { R: Rs, T: Ts };
|
||
}
|
||
/* 逐对扫描（簇级宽相位 → 胶囊线段距离）。visit(cnA,cnB,d,need) 对每个距离
|
||
< need 的胶囊对回调；无回调（collideFree）时首对即短路返回。 */
|
||
let GAPCAP = 20;
|
||
function scanPairs(q, visit, onlyCls, gapCb) {
|
||
if (!COL) return false;
|
||
const { R: Rs, T: Ts } = cumXf(q);
|
||
/* 两关节类查表（只在纯判定时用；明细/生成表走精算） */
|
||
const INF = gapCb ? GAPCAP : 0; // 生成表：宽相位外扩 GAPCAP（默认 20 mm），表内 <GAPCAP 的间隙为精确值
|
||
if (!visit) for (const k in P2D) {
|
||
const T = P2D[k], st = p2dStatus(T, q);
|
||
if (st < 0) return true;
|
||
T.st = st;
|
||
}
|
||
/* clc/cache/pcc 全部预分配（声明见 cumXf 前）：CGEN 代际戳标记"本 check 已变换" */
|
||
CGEN++;
|
||
const clOf = (pi, ci) => {
|
||
const e = CLBUF[pi][ci];
|
||
if (e[3] !== CGEN && e[3] !== -2) {
|
||
const pt = PART[pi], R = Rs[pt.g], t = Ts[pt.g], c = pt.clus[ci].c;
|
||
e[0] = R[0] * c[0] + R[1] * c[1] + R[2] * c[2] + t[0];
|
||
e[1] = R[3] * c[0] + R[4] * c[1] + R[5] * c[2] + t[1];
|
||
e[2] = R[6] * c[0] + R[7] * c[1] + R[8] * c[2] + t[2];
|
||
e[3] = pt.g === 0 ? -2 : CGEN; // 组 0（立柱/底座）静止：变换一次永久有效
|
||
}
|
||
return e;
|
||
};
|
||
const pcOf = pi => {
|
||
const e = PCBUF[pi];
|
||
if (e[3] !== CGEN && e[3] !== -2) {
|
||
const pt = PART[pi], R = Rs[pt.g], t = Ts[pt.g], c = pt.c;
|
||
e[0] = R[0] * c[0] + R[1] * c[1] + R[2] * c[2] + t[0];
|
||
e[1] = R[3] * c[0] + R[4] * c[1] + R[5] * c[2] + t[1];
|
||
e[2] = R[6] * c[0] + R[7] * c[1] + R[8] * c[2] + t[2];
|
||
e[3] = pt.g === 0 ? -2 : CGEN;
|
||
}
|
||
return e;
|
||
};
|
||
const xf = i => {
|
||
const e = XFBUF[i];
|
||
if (e[7] === CGEN || e[7] === -2) return e;
|
||
const c = CAPG[i], R = Rs[c.g], t = Ts[c.g];
|
||
e[0] = R[0] * c.a[0] + R[1] * c.a[1] + R[2] * c.a[2] + t[0];
|
||
e[1] = R[3] * c.a[0] + R[4] * c.a[1] + R[5] * c.a[2] + t[1];
|
||
e[2] = R[6] * c.a[0] + R[7] * c.a[1] + R[8] * c.a[2] + t[2];
|
||
e[3] = R[0] * c.b[0] + R[1] * c.b[1] + R[2] * c.b[2] + t[0];
|
||
e[4] = R[3] * c.b[0] + R[4] * c.b[1] + R[5] * c.b[2] + t[1];
|
||
e[5] = R[6] * c.b[0] + R[7] * c.b[1] + R[8] * c.b[2] + t[2];
|
||
e[7] = c.g === 0 ? -2 : CGEN;
|
||
return e;
|
||
};
|
||
/* 宽相位阈值必须保守：球心距 > rad_A+rad_B+rmax_A+rmax_B+margin 才可筛除。
|
||
推导：胶囊轴上任点都在球内，故轴距 ≥ 球心距−rad_A−rad_B；要保证它对所有
|
||
胶囊对都 > r_i+r_j+margin（=need），阈值得带上双方最大胶囊半径与 margin。
|
||
早期版本只加 rad（漏了 rmax 与 margin），实测把 P01_C×P06 在 J1=20/J2=49
|
||
的 1618mm³ 深撞整对筛掉——球相交但轴距仍小于半径和的对被误跳。 */
|
||
let hit = false;
|
||
for (let k = 0; k < PARTPAIR.length; k++) {
|
||
const [A, B, cl, rsum0, rmax0] = PARTPAIR[k];
|
||
if (onlyCls !== undefined) { if (classOf(A, B) !== onlyCls) continue; }
|
||
else if (!visit) { const T = P2D[classOf(A, B)]; if (T && T.st === 1) continue; }
|
||
const pa = pcOf(A._i), pb = pcOf(B._i);
|
||
const ddx = pa[0] - pb[0], ddy = pa[1] - pb[1], ddz = pa[2] - pb[2];
|
||
const lim0 = rsum0 + rmax0 + CAPM;
|
||
if (ddx * ddx + ddy * ddy + ddz * ddz > (lim0 + INF) * (lim0 + INF)) continue; // 零件级分离
|
||
for (let m = 0; m < cl.length; m++) {
|
||
const [ia, ib, pr, rsum, rmaxS] = cl[m];
|
||
const p1 = clOf(A._i, ia), p2 = clOf(B._i, ib);
|
||
const dx = p1[0] - p2[0], dy = p1[1] - p2[1], dz = p1[2] - p2[2];
|
||
const lim = rsum + rmaxS + CAPM;
|
||
if (dx * dx + dy * dy + dz * dz > (lim + INF) * (lim + INF)) continue; // 簇球分离
|
||
for (let n = 0; n < pr.length; n++) {
|
||
const e1 = xf(pr[n][0]), e2 = xf(pr[n][1]);
|
||
const need = pr[n][2];
|
||
/* 胶囊级包围球预筛：中点距 > 半长和 + 半径和 + margin 时线段距离必然 ≥ need（保守、无漏判），
|
||
实测 segSegDistRaw 占碰撞判定 ~45% 耗时，绝大多数胶囊对在这一步就能跳过 */
|
||
const mx = 0.5 * (e1[0] + e1[3] - e2[0] - e2[3]), my = 0.5 * (e1[1] + e1[4] - e2[1] - e2[4]),
|
||
mz = 0.5 * (e1[2] + e1[5] - e2[2] - e2[5]), bl = pr[n][3];
|
||
if (mx * mx + my * my + mz * mz > (bl + INF) * (bl + INF)) continue;
|
||
const d = segSegDistRaw(e1[0], e1[1], e1[2], e1[3], e1[4], e1[5],
|
||
e2[0], e2[1], e2[2], e2[3], e2[4], e2[5]);
|
||
if (gapCb) { gapCb(d - need, pr[n][0], pr[n][1]); continue; }
|
||
if (d < need) {
|
||
hit = true;
|
||
if (visit) visit(e1[6], e2[6], d, need);
|
||
else {
|
||
/* 前移启发：撞上的零件对 / 簇对挪到表头。规划中大量判定落在同一处障碍
|
||
（典型：立柱 × 腕/法兰），下次首查即命中、提前短路。只改顺序，不改结果。 */
|
||
if (k > 0) { const e = PARTPAIR[k]; PARTPAIR.copyWithin(1, 0, k); PARTPAIR[0] = e; }
|
||
if (m > 0) { const e = cl[m]; cl.copyWithin(1, 0, m); cl[0] = e; }
|
||
return true;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
return hit;
|
||
}
|
||
/* 胶囊对清单 [[i, j, need]]（与 scanPairs 判定的对完全一致；ml/ 导出给 PyTorch 可微版用） */
|
||
function pairList() {
|
||
const out = [];
|
||
for (const [, , cl] of PARTPAIR) for (const c of cl) for (const pr of c[2]) out.push([pr[0], pr[1], pr[2]]);
|
||
return out;
|
||
}
|
||
/* 宽相位结构导出（ml/ PyTorch 版复刻同一套簇球筛选，保证 min(gap, cap) 与 JS 逐位同口径）：
|
||
clusters[k] = {g, c(局部簇心), rad, rmax, caps}；cpairs = [[ka, kb, rsum, rmaxS]]，簇对内胶囊对取全组合，need = r_i + r_j + CAPM */
|
||
function clusterExport() {
|
||
const clusters = [], base = new Map();
|
||
for (const pt of PART) { base.set(pt, clusters.length); for (const c of pt.clus) clusters.push({ g: pt.g, c: c.c.slice(), rad: c.rad, rmax: c.rmax, caps: c.caps.slice() }); }
|
||
const cpairs = [];
|
||
for (const [A, B, cl] of PARTPAIR) for (const c of cl) cpairs.push([base.get(A) + c[0], base.get(B) + c[1], c[3], c[4]]);
|
||
return { clusters: clusters, cpairs: cpairs, capm: CAPM };
|
||
}
|
||
/* 间隙 < thr 的胶囊对 [[i, j, gap]]，按 gap 升序（安全层投影用：只对这几对做有限差分） */
|
||
function criticalPairs(q, thr) {
|
||
const out = [];
|
||
if (!COL) return out;
|
||
const save = GAPCAP; GAPCAP = thr;
|
||
try { scanPairs(q, () => {}, undefined, (g, i, j) => { if (g < thr) out.push([i, j, g]); }); } finally { GAPCAP = save; }
|
||
out.sort((x, y) => x[2] - y[2]);
|
||
return out;
|
||
}
|
||
/* 单个胶囊对的间隙（距离 − need），只变换两根胶囊，微秒级 */
|
||
function pairGap(q, i, j) {
|
||
const { R: Rs, T: Ts } = cumXf(q);
|
||
const tf = (c, e) => { const R = Rs[c.g], T = Ts[c.g]; return [R[0] * e[0] + R[1] * e[1] + R[2] * e[2] + T[0], R[3] * e[0] + R[4] * e[1] + R[5] * e[2] + T[1], R[6] * e[0] + R[7] * e[1] + R[8] * e[2] + T[2]]; };
|
||
const A = CAPG[i], B = CAPG[j];
|
||
const a0 = tf(A, A.a), a1 = tf(A, A.b), b0 = tf(B, B.a), b1 = tf(B, B.b);
|
||
return segSegDistRaw(a0[0], a0[1], a0[2], a1[0], a1[1], a1[2], b0[0], b0[1], b0[2], b1[0], b1[1], b1[2]) - (A.r + B.r + CAPM);
|
||
}
|
||
function setDeterministic(on, msPerCheck) { VCLK = on ? { base: 0, per: msPerCheck || 0.5, tick: 0 } : null; }
|
||
/* 清空所有跨调用状态（路网增广、坏边/死边/已验段缓存、实时路径、全量 IK 缓存），使下一次求解与历史无关 */
|
||
function resetSession() {
|
||
RMX.Q.length = 0; RMX.adj.length = 0; RMX.adjR = null;
|
||
BADE.clear(); DEAD.clear(); FINE.clear();
|
||
RT = null; RTSTALL.key = null; RTSTALL.n = 0;
|
||
FULLIK.key = null; FULLIK.cur = null; FULLIK.T = null; FULLIK.at = 0;
|
||
}
|
||
/* 全胶囊对最小间隙（mm，= 距离 − need，负值为干涉；≥cap 一律记 cap，下界、保守）（2026-10-17，
|
||
ml/gen 距离下界认证用）：不走 pair2d 查表、不提前退出，比 collideFree 慢，只给离线/安全层用。 */
|
||
function clearance(q, cap) {
|
||
if (!COL) return Infinity;
|
||
cap = cap || 20;
|
||
let best = cap; const save = GAPCAP; GAPCAP = cap;
|
||
try { scanPairs(q, () => {}, undefined, g => { if (g < best) best = g; }); } finally { GAPCAP = save; }
|
||
return best;
|
||
}
|
||
/* 按组对的最小间隙表（7×7 扁平，[gA*7+gB]，gA<gB；未进入窄相位的对记 cap）。
|
||
组对 (gA,gB) 的相对位姿只受关节 gA..gB−1 影响，认证时只用这几轴的杠杆和。 */
|
||
function gapMatrix(q, cap) {
|
||
cap = cap || 20;
|
||
const M = new Float64Array(49).fill(cap);
|
||
if (!COL) return M;
|
||
const save = GAPCAP; GAPCAP = cap;
|
||
try {
|
||
scanPairs(q, () => {}, undefined, (g, i, j) => {
|
||
let a = CAPG[i].g, b = CAPG[j].g; if (a > b) { const t = a; a = b; b = t; }
|
||
if (g < M[a * 7 + b]) M[a * 7 + b] = g;
|
||
});
|
||
} finally { GAPCAP = save; }
|
||
return M;
|
||
}
|
||
/* 每轴杠杆上界 r_j（mm/°）：轴 j 转 1° 时，j 之后任一胶囊端点相对 j 之前部件的最大位移。
|
||
随机采样 n 个位形取最大值再放大 (1+pad)。胶囊间距对端点位移 1-Lipschitz，故
|
||
Σ r_j|Δθ_j| 是关节直线段上任意胶囊对间隙变化量的上界（用于 d(a)+d(b) > Σ r_j|Δθ_j| 认证）。 */
|
||
/* 分组杠杆表 T[k][g]（mm/°）：组 g 的胶囊端点到轴 k 的最大距离（g > k），比 leverBounds 的"全部远端"上界紧：
|
||
组对 (gA,gB) 的相对运动只由关节 gA..gB−1 带动组 gB 产生，故 Σ_k T[k][gB]·|Δθ_k| 是该组对间隙变化上界。 */
|
||
function leverTable(n, pad, seed) {
|
||
n = n || 4000; pad = pad === undefined ? 0.05 : pad;
|
||
let st = (seed || 7) >>> 0; const rnd = () => { st = (st * 1664525 + 1013904223) >>> 0; return st / 4294967296; };
|
||
const T = [0, 1, 2, 3, 4, 5].map(() => [0, 0, 0, 0, 0, 0, 0]);
|
||
for (let t = 0; t < n; t++) {
|
||
const q = ORDER.map(id => { const L = LIM[id]; return L.cont ? -180 + 360 * rnd() : L.lo + (L.hi - L.lo) * rnd(); });
|
||
const { R: Rs, T: Ts } = cumXf(q);
|
||
for (let j = 0; j < 6; j++) {
|
||
const Rj = Rs[j], tj = Ts[j], Pj = P[ORDER[j]], Dj = D[ORDER[j]];
|
||
const o = [Rj[0] * Pj[0] + Rj[1] * Pj[1] + Rj[2] * Pj[2] + tj[0], Rj[3] * Pj[0] + Rj[4] * Pj[1] + Rj[5] * Pj[2] + tj[1], Rj[6] * Pj[0] + Rj[7] * Pj[1] + Rj[8] * Pj[2] + tj[2]];
|
||
const u = [Rj[0] * Dj[0] + Rj[1] * Dj[1] + Rj[2] * Dj[2], Rj[3] * Dj[0] + Rj[4] * Dj[1] + Rj[5] * Dj[2], Rj[6] * Dj[0] + Rj[7] * Dj[1] + Rj[8] * Dj[2]];
|
||
const un = Math.hypot(u[0], u[1], u[2]);
|
||
for (const c of CAPG) {
|
||
if (c.g < j + 1) continue;
|
||
const R = Rs[c.g], Tt = Ts[c.g];
|
||
for (const e of [c.a, c.b]) {
|
||
const w = [R[0] * e[0] + R[1] * e[1] + R[2] * e[2] + Tt[0] - o[0], R[3] * e[0] + R[4] * e[1] + R[5] * e[2] + Tt[1] - o[1], R[6] * e[0] + R[7] * e[1] + R[8] * e[2] + Tt[2] - o[2]];
|
||
const dp = (w[0] * u[0] + w[1] * u[1] + w[2] * u[2]) / un;
|
||
const dd = Math.sqrt(Math.max(0, w[0] * w[0] + w[1] * w[1] + w[2] * w[2] - dp * dp));
|
||
if (dd > T[j][c.g]) T[j][c.g] = dd;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
/* 组 g 的端点距离是该组上界；但组对 (gA,gB) 里远端组 gB 之外，gB 与 gA 之间的组不参与该对，
|
||
而 gB 自身被 j<gB 的所有关节带动——直接取 T[j][gB] 即可 */
|
||
return T.map(row => row.map(v => v * D2R * (1 + pad)));
|
||
}
|
||
function leverBounds(n, pad, seed) {
|
||
n = n || 4000; pad = pad === undefined ? 0.05 : pad;
|
||
let st = (seed || 7) >>> 0; const rnd = () => { st = (st * 1664525 + 1013904223) >>> 0; return st / 4294967296; };
|
||
const r = [0, 0, 0, 0, 0, 0];
|
||
for (let t = 0; t < n; t++) {
|
||
const q = ORDER.map(id => { const L = LIM[id]; return L.cont ? -180 + 360 * rnd() : L.lo + (L.hi - L.lo) * rnd(); });
|
||
const { R: Rs, T: Ts } = cumXf(q);
|
||
for (let j = 0; j < 6; j++) {
|
||
const Rj = Rs[j], tj = Ts[j], Pj = P[ORDER[j]], Dj = D[ORDER[j]];
|
||
const o = [Rj[0] * Pj[0] + Rj[1] * Pj[1] + Rj[2] * Pj[2] + tj[0], Rj[3] * Pj[0] + Rj[4] * Pj[1] + Rj[5] * Pj[2] + tj[1], Rj[6] * Pj[0] + Rj[7] * Pj[1] + Rj[8] * Pj[2] + tj[2]];
|
||
const u = [Rj[0] * Dj[0] + Rj[1] * Dj[1] + Rj[2] * Dj[2], Rj[3] * Dj[0] + Rj[4] * Dj[1] + Rj[5] * Dj[2], Rj[6] * Dj[0] + Rj[7] * Dj[1] + Rj[8] * Dj[2]];
|
||
const un = Math.hypot(u[0], u[1], u[2]);
|
||
for (const c of CAPG) {
|
||
if (c.g < j + 1) continue;
|
||
const R = Rs[c.g], T = Ts[c.g];
|
||
for (const e of [c.a, c.b]) {
|
||
const w = [R[0] * e[0] + R[1] * e[1] + R[2] * e[2] + T[0] - o[0], R[3] * e[0] + R[4] * e[1] + R[5] * e[2] + T[1] - o[1], R[6] * e[0] + R[7] * e[1] + R[8] * e[2] + T[2] - o[2]];
|
||
const dp = (w[0] * u[0] + w[1] * u[1] + w[2] * u[2]) / un;
|
||
const dd = Math.sqrt(Math.max(0, w[0] * w[0] + w[1] * w[1] + w[2] * w[2] - dp * dp));
|
||
if (dd > r[j]) r[j] = dd;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
return r.map(v => v * D2R * (1 + pad));
|
||
}
|
||
/* 全姿态动态碰撞判定：True = 无动态干涉（模型近似，含 margin 外扩） */
|
||
const PST = { n: 0 };
|
||
/* 单次求解内的判定记忆：routeFrom / gatherGoals / 规划器会对同一批 IK 候选反复判定 */
|
||
let CFMEMO = null;
|
||
function collideFree(q) {
|
||
if (!COL) return true;
|
||
let key;
|
||
if (CFMEMO) { key = q[0] + ',' + q[1] + ',' + q[2] + ',' + q[3] + ',' + q[4] + ',' + q[5]; const v = CFMEMO.get(key); if (v !== undefined) return v; }
|
||
PST.n++;
|
||
const v = !scanPairs(q, null);
|
||
if (CFMEMO && CFMEMO.size < 20000) CFMEMO.set(key, v);
|
||
return v;
|
||
}
|
||
/* 干涉明细（调参/界面提示用）：返回 [{cnA, cnB, d, need}]，按 d−need 升序
|
||
（最贴合的排前面）。空数组 = 无动态干涉。 */
|
||
function collideDetail(q) {
|
||
const out = [];
|
||
if (!COL) return out;
|
||
scanPairs(q, (na, nb, d, need) => out.push({ cnA: na, cnB: nb, d: d, need: need }));
|
||
out.sort((x, y) => (x.d - x.need) - (y.d - y.need));
|
||
return out;
|
||
}
|
||
/* 某轴在给定姿态下的动态边界（度）：从 q[id] 向 dir 方向二分至首次干涉，
|
||
上限为电机机械限位。返回可安全抵达的极值。 */
|
||
function dynLimit(id, q, dir, tol) {
|
||
tol = tol || 0.25;
|
||
const k = ORDER.indexOf(id);
|
||
if (!COL || k < 0 || !dir) return q[k];
|
||
const sign = dir > 0 ? 1 : -1;
|
||
const side = dir > 0 ? 'hi' : 'lo';
|
||
const hard = HARD[id] && HARD[id][side];
|
||
const hi0 = dir > 0 ? LIM[id].hi : LIM[id].lo;
|
||
/* 连续轴的搜索端点取 ±180：转过的角度以 360 为周期，360° 与 0° 是同一个位姿，
|
||
拿它当端点探一次必然"安全"，二分会把区间内真实存在的撞点整段跳过（实测 J3 在
|
||
J4=-17.875 的折肘位形下首撞 148°，而旧口径直接返回 360）。 */
|
||
let hi = sign * (LIM[id].cont ? 180 : Math.min(MOTOR || 180, 180));
|
||
/* 相邻打印件对的 1D 自由区间（adj 表，精确 CAD 实测、与姿态无关）封顶：
|
||
典型漏判场景——J4=-100 时 J5+ 被 P07×P08 卡在 96°，而该对落在静态扫描的
|
||
视野外（零位时立柱先把 J5 卡在 34°），动态胶囊模型也看不见相邻对。 */
|
||
const adj = ADJ[id];
|
||
if (adj && dir > 0) hi = Math.min(hi, adj[1]);
|
||
if (adj && dir < 0) hi = Math.max(hi, adj[0]);
|
||
/* 硬边侧（静态限位由相邻打印件对决定、与姿态无关，胶囊模型看不见那一对）：
|
||
静态值只当区间上限，区间内仍要按当前姿态二分——否则折肘位形下动态检查被整个
|
||
跳过，滑块放到静态 -93°，而实测 J5 在 J3=-180° 时 -28° 末端法兰已撞立柱下段。 */
|
||
if (hard && Math.abs(hi0) < Math.abs(hi)) hi = hi0;
|
||
if (hi === q[k]) return q[k];
|
||
const at = v => { const qq = q.slice(); qq[k] = v; return collideFree(qq); };
|
||
if (!at(q[k])) return q[k]; // 当前姿已干涉：不放大
|
||
let lo = q[k];
|
||
if (!LIM[id].cont) { // 限位轴：折过去就是折过去，区间内单峰，二分即可
|
||
if (at(hi)) return hi;
|
||
while (Math.abs(hi - lo) > tol) {
|
||
const mid = 0.5 * (lo + hi);
|
||
if (at(mid)) lo = mid; else hi = mid;
|
||
}
|
||
return lo;
|
||
}
|
||
/* 连续轴（J1/J3/J6）：整周内可行集不连续——末段绕过立柱后会重新变安全，
|
||
"端点安全"根本不代表中间没撞（实测 J1=-81 位形向 + 方向 180° 端点安全，
|
||
但 -26° 起撞立柱中段）。所以先按固定步长粗扫出首个干涉角，再在该段内二分。
|
||
步长 4° 起（区间 360° 时约 40 次 collideFree ≈ 150 ms，只在滑块缓存未命中时发生）。 */
|
||
const step = (hi > q[k] ? 1 : -1) * Math.max(4, Math.abs(hi - q[k]) / 40);
|
||
let jam = null;
|
||
for (let v = lo + step; (hi > q[k] ? v < hi : v > hi); v += step) {
|
||
if (at(v)) lo = v; else { jam = v; break; }
|
||
}
|
||
if (jam === null) { if (at(hi)) return hi; jam = hi; }
|
||
while (Math.abs(jam - lo) > tol) {
|
||
const mid = 0.5 * (lo + jam);
|
||
if (at(mid)) lo = mid; else jam = mid;
|
||
}
|
||
return lo;
|
||
}
|
||
/* 六轴动态限位表（UI 显示/滑块夹取用） */
|
||
function dynLimits(q, tol) {
|
||
const out = {};
|
||
for (const id of ORDER) out[id] = { lo: dynLimit(id, q, -1, tol), hi: dynLimit(id, q, +1, tol) };
|
||
return out;
|
||
}
|
||
const TCP0 = (cfg.tcp || [P.J6[0], P.J6[1], 93]).slice(); // 缺省 TCP 落在 J6 轴线上（随整机前移自动更新）
|
||
const ax = D.J6; // 工具沿 J6 轴向外伸
|
||
const K = {
|
||
X0: P.J3[0], Y0: P.J6[1],
|
||
Z1: P.J1[2], Z2: P.J2[2], Z3: P.J3[2], Z4: P.J4[2], Z5: P.J5[2],
|
||
TCP0: TCP0, ax: ax
|
||
};
|
||
K.C0 = K.Z2 - K.Z1;
|
||
K.A43 = K.Z4 - K.Z3;
|
||
K.A32 = K.Z3 - K.Z2;
|
||
K.A54 = K.Z5 - K.Z4;
|
||
K.L5 = K.Z5 - TCP0[2]; // J5 轴 → TCP（沿 −Z）
|
||
/* 结构前提断言：J2/J4/J5 同向平行；J1 轴为 X；J3/J6 轴为 Z；TCP 落在 J6 轴线上 */
|
||
const AXP = v => Math.abs(Math.abs(v[2]) - 1) < 1e-9;
|
||
const WARN = [];
|
||
if (Math.abs(D.J1[0]) < 1e-9 || Math.abs(Math.abs(D.J1[0]) - 1) > 1e-9) WARN.push('J1 轴不是 X');
|
||
for (const id of ['J2', 'J4', 'J5']) if (Math.abs(Math.abs(D[id][1]) - 1) > 1e-9) WARN.push(id + ' 轴不是 Y');
|
||
if (!AXP(D.J3)) WARN.push('J3 轴不是 Z');
|
||
if (!AXP(D.J6)) WARN.push('J6 轴不是 Z');
|
||
if (Math.abs(P.J3[0] - P.J2[0]) > 1e-9 || Math.abs(P.J4[0] - P.J2[0]) > 1e-9 || Math.abs(P.J5[0] - P.J2[0]) > 1e-9)
|
||
WARN.push('J2/J4/J5 不在同一 x 平面');
|
||
if (Math.abs(TCP0[0] - P.J6[0]) > 1e-9 || Math.abs(TCP0[1] - P.J6[1]) > 1e-9)
|
||
WARN.push('TCP 不在 J6 轴线上（θ6 将影响位置）');
|
||
|
||
/* ---------- 1. 严格矩阵链正运动学（对照基准） ---------- */
|
||
function mul4(A, B) {
|
||
const C = new Array(16);
|
||
for (let i = 0; i < 4; i++) for (let j = 0; j < 4; j++) {
|
||
let s = 0;
|
||
for (let k = 0; k < 4; k++) s += A[i * 4 + k] * B[k * 4 + j];
|
||
C[i * 4 + j] = s;
|
||
}
|
||
return C;
|
||
}
|
||
function mv4(M, v) {
|
||
return [0, 1, 2, 3].map(i => M[i * 4] * v[0] + M[i * 4 + 1] * v[1] + M[i * 4 + 2] * v[2] + M[i * 4 + 3] * v[3]);
|
||
}
|
||
|
||
/* 标准 4x4 同质链 World = A1·A2·…·A6·TCP，Ai = T(Pi)·R(轴,θ)·T(−Pi)
|
||
—— 与查看器场景图（逐级左乘的 matrix 局部变换）逐位等价 */
|
||
function fkChain(q, tool) {
|
||
const tcp = [TCP0[0], TCP0[1], TCP0[2] + (+tool || 0) * ax[2]];
|
||
let M = [1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1];
|
||
for (let i = 0; i < 6; i++) {
|
||
const id = ORDER[i], p = P[id], R = rotAxis(D[id], q[i] * D2R);
|
||
const rp = mv3(R, p); // Ai 的旋转=R，平移=p−R·p
|
||
const A = [R[0], R[1], R[2], p[0] - rp[0],
|
||
R[3], R[4], R[5], p[1] - rp[1],
|
||
R[6], R[7], R[8], p[2] - rp[2], 0, 0, 0, 1];
|
||
M = mul4(M, A);
|
||
}
|
||
const w = mv4(M, [tcp[0], tcp[1], tcp[2], 1]);
|
||
return [w[0], w[1], w[2]];
|
||
}
|
||
|
||
/* ---------- 1b. 工具姿态链：前 thru 个关节旋转之积（CAD 系，右乘法向） ---------- */
|
||
/* 工具坐标系姿态 = R1·R2·…·R6。因 R6 只绕工具自身 z 轴转动：
|
||
· 工具轴（法兰面 +Z 指向）= (R1…R5)·ẑ —— 与 θ6 无关，这正是姿态任务可解的关键；
|
||
· θ6 只提供绕工具轴的滚转。 */
|
||
function orient(q, thru) {
|
||
let M = [1, 0, 0, 0, 1, 0, 0, 0, 1];
|
||
const n = thru == null ? 6 : thru;
|
||
for (let i = 0; i < n; i++) M = mul3(M, rotAxis(D[ORDER[i]], q[i] * D2R));
|
||
return M;
|
||
}
|
||
/* M5 第 3 列（工具轴）列向量传播：J4/J5 同轴 ⇒ R4·R5=R(θ4+θ5) 合并为一次旋转，
|
||
只乘单位向量、不构造 3×3、免 T3/mul3/rotErrDeg/acos。与 orient(q,5) 列 2 一致（≤1e-15）。 */
|
||
const SAMEAX = D.J4[0] === D.J5[0] && D.J4[1] === D.J5[1] && D.J4[2] === D.J5[2];
|
||
function axisCol(q) {
|
||
/* 零分配特化：J4/J5 同轴合并为 R(Y,θ4+θ5) 作用在 ẑ 上（= (sin,0,cos)），J3 为 −Z、
|
||
J2 为 Y、J1 为 X，逐级展开向量传播（与通用矩阵链数学恒等；每级系数为 0 的项直接省去）。
|
||
不满足结构前提时回落通用矩阵链。 */
|
||
if (SAMEAX && D.J4[1] === 1 && D.J1[0] === 1 && D.J2[1] === 1 && D.J3[2] === -1) {
|
||
const r45 = (q[3] + q[4]) * D2R;
|
||
const x5 = Math.sin(r45), z5 = Math.cos(r45);
|
||
const c3 = Math.cos(q[2] * D2R), s3 = Math.sin(q[2] * D2R);
|
||
const x3 = c3 * x5, y3 = -s3 * x5; // J3：R(−Z,θ3)·(x5,0,z5)
|
||
const c2 = Math.cos(q[1] * D2R), s2 = Math.sin(q[1] * D2R);
|
||
const x2 = c2 * x3 + s2 * z5, z2 = -s2 * x3 + c2 * z5; // J2：R(Y,θ2)
|
||
const c1 = Math.cos(q[0] * D2R), s1 = Math.sin(q[0] * D2R);
|
||
return [x2, c1 * y3 - s1 * z2, s1 * y3 + c1 * z2]; // J1：R(X,θ1)
|
||
}
|
||
let v;
|
||
if (SAMEAX) v = mv3(rotAxis(D.J4, (q[3] + q[4]) * D2R), [0, 0, 1]);
|
||
else v = mv3(mul3(rotAxis(D.J4, q[3] * D2R), rotAxis(D.J5, q[4] * D2R)), [0, 0, 1]);
|
||
v = mv3(rotAxis(D.J3, q[2] * D2R), v);
|
||
v = mv3(rotAxis(D.J2, q[1] * D2R), v);
|
||
v = mv3(rotAxis(D.J1, q[0] * D2R), v);
|
||
return v;
|
||
}
|
||
|
||
/* ---------- 2. 闭式正运动学（快速，用于求解内部） ---------- */
|
||
function fk(q, tool) {
|
||
const L = K.L5 + (+tool || 0);
|
||
const s1 = Math.sin(q[0] * D2R), c1 = Math.cos(q[0] * D2R);
|
||
const s2 = Math.sin(q[1] * D2R), c2 = Math.cos(q[1] * D2R);
|
||
const s3 = Math.sin(q[2] * D2R), c3 = Math.cos(q[2] * D2R);
|
||
const s4 = Math.sin(q[3] * D2R), c4 = Math.cos(q[3] * D2R);
|
||
const s5 = Math.sin(q[4] * D2R), c5 = Math.cos(q[4] * D2R);
|
||
const u = -L * s5, w = K.A54 - L * c5;
|
||
const dx = u * c4 + w * s4;
|
||
const dz = K.A43 - u * s4 + w * c4;
|
||
const Vx = dx, Vz = dz + K.A32;
|
||
const b = -Vx * s3;
|
||
const c = K.C0 + (-Vx * c3 * s2 + Vz * c2);
|
||
const a = Vx * c3 * c2 + Vz * s2;
|
||
return [K.X0 + a, K.Y0 + b * c1 - c * s1, K.Z1 + b * s1 + c * c1];
|
||
}
|
||
|
||
/* ---------- 3. 逆解：给定 (θ4,θ5) 的闭式分支 ---------- */
|
||
/* 单槽 sin/cos 缓存：网格扫描外层 θ4 固定，每行只算一次（浮点同值必命中，NaN 不等必重算） */
|
||
let _bt4 = NaN, _bt5 = NaN, _bs4 = 0, _bc4 = 0, _bs5 = 0, _bc5 = 0;
|
||
function branch(th4, th5, A, R, ph, tool) {
|
||
const L = K.L5 + (+tool || 0);
|
||
if (th4 !== _bt4) { _bs4 = Math.sin(th4 * D2R); _bc4 = Math.cos(th4 * D2R); _bt4 = th4; }
|
||
if (th5 !== _bt5) { _bs5 = Math.sin(th5 * D2R); _bc5 = Math.cos(th5 * D2R); _bt5 = th5; }
|
||
const s5 = _bs5, c5 = _bc5, s4 = _bs4, c4 = _bc4;
|
||
const u = -L * s5, w = K.A54 - L * c5;
|
||
const dx = u * c4 + w * s4;
|
||
const dz = K.A43 - u * s4 + w * c4;
|
||
const Vx = dx, Vz = dz + K.A32;
|
||
const out = [];
|
||
const n = (R * R + A * A - Vx * Vx - Vz * Vz - K.C0 * K.C0) / (2 * K.C0);
|
||
const cc = K.C0 + n;
|
||
if (Math.abs(Vx) < 1e-7) { // 退化：θ3 不影响 TCP
|
||
if (Math.abs(Vz) < 1e-9) return out;
|
||
const s2 = A / Vz, c2 = n / Vz;
|
||
if (Math.abs(s2 * s2 + c2 * c2 - 1) > 1e-12) return out; // 收紧：无 fk 复核后须自行保证精确（原松解本就会被 fk 拒）
|
||
const th1 = (Math.abs(cc) > 1e-12) ? (ph - Math.atan2(cc, 0)) * R2D : 0;
|
||
out.push([wrap180(th1), wrap180(Math.atan2(s2, c2) * R2D), 0, th4, th5, 0]);
|
||
return out;
|
||
}
|
||
const m2 = (n * n + A * A - Vz * Vz) / (Vx * Vx);
|
||
if (m2 < -1e-9 || m2 > 1 + 1e-9) return out;
|
||
const sig = Math.acos(Math.sqrt(Math.max(0, Math.min(1, m2))));
|
||
/* θ3 四支（±σ、±(π−σ)）的 cos/sin 只需一对三角函数值，按支下标取符号（原实现每支各算一次） */
|
||
const c3s = Math.cos(sig), s3s = Math.sin(sig);
|
||
for (let k = 0; k < 4; k++) {
|
||
const c3 = (k < 2) ? c3s : -c3s, s3 = (k === 0 || k === 2) ? s3s : -s3s;
|
||
const kap = Vx * c3, lam = Vz;
|
||
const det = kap * kap + lam * lam;
|
||
if (det < 1e-12) continue;
|
||
const th2 = Math.atan2((lam * A - kap * n) / det, (kap * A + lam * n) / det) * R2D;
|
||
const b = -Vx * s3;
|
||
const th1 = (Math.abs(b) + Math.abs(cc) > 1e-12) ? (ph - Math.atan2(cc, b)) * R2D : 0;
|
||
out.push([wrap180(th1), wrap180(th2), wrap180(Math.atan2(s3, c3) * R2D), th4, th5, 0]);
|
||
}
|
||
return out;
|
||
}
|
||
|
||
const LJ5 = [LIM.J1, LIM.J2, LIM.J3, LIM.J4, LIM.J5];
|
||
const inLimit = q => { // 取否定式：NaN 候选必须落选（限位数组预取，免每点字符串键查表）
|
||
for (let i = 0; i < 5; i++) {
|
||
const t = LJ5[i];
|
||
if (!(q[i] >= t.lo - 1e-9 && q[i] <= t.hi + 1e-9)) return false;
|
||
}
|
||
return true;
|
||
};
|
||
/* 选解代价。
|
||
- 不给 ref：Σ|θ|（对外显示口径；离线批算也走这一支）
|
||
- 给 ref：用户指定的判据「**所有电机需要转过的角度之和**」最小，
|
||
即相对当前姿态的行程最小。连续轴（J1/J3/J6）无机械限位，θ 与 θ±360 是同一
|
||
物理位形，故角差一律取 wrap180。末尾 1e-6·Σ|θ| 只作并列时的次序稳定项。
|
||
（旧实现是 Σ|θ| + lam·Σwrap(θ−ref)² 的二次连续项，已被本判据取代；
|
||
参数 lam 保留仅为兼容旧调用，不再参与计算。） */
|
||
const costOf = (q, W, ref, lam) => {
|
||
let c = 0;
|
||
for (let i = 0; i < 6; i++) { const w = W ? W[i] : 1; c += Math.abs(q[i]) * w; }
|
||
if (!ref) return c;
|
||
let t = 0;
|
||
for (let i = 0; i < 6; i++) { const w = W ? W[i] : 1; t += Math.abs(wrap180(q[i] - ref[i])) * w; }
|
||
return t + 1e-6 * c;
|
||
};
|
||
/* 确定性时钟（2026-10-17，ml/gen 离线老师用）：开启后各处毫秒预算改按"碰撞判定次数 × msPerCheck"计，
|
||
另每次读时钟 +1 µs 防纯计算循环不前进。规划结果只取决于输入，不随机器快慢/负载变化。默认关闭。 */
|
||
let VCLK = null;
|
||
const nowMs = () => VCLK ? VCLK.base + PST.n * VCLK.per + (VCLK.tick += 0.001)
|
||
: (typeof performance !== 'undefined' ? performance.now() : Date.now());
|
||
|
||
/* 姿态求解原语工厂：位置闭式支把 θ1..θ3 吸收掉之后，姿态任务只剩「工具轴指向 =
|
||
目标轴」这 2 个方程、2 个未知数 (θ4,θ5)，解是孤立点，滚转再由 θ6 闭式补上。
|
||
粗扫根枚举（solve 内）与实时续解（solveWarm）共用同一套。 */
|
||
function poseTools(target, tool, Rtar) {
|
||
const Aa = target[0] - K.X0, Bq = target[1] - K.Y0, Br = target[2] - K.Z1;
|
||
const Rr = Math.hypot(Bq, Br), phh = Math.atan2(Br, Bq);
|
||
const L4 = LIM.J4, L5 = LIM.J5;
|
||
let poseQAt, poseRes, poseNewton;
|
||
const Ad = [Rtar[2], Rtar[5], Rtar[8]];
|
||
const pz = cross3(Ad, [0, 0, 1]);
|
||
const bu = Math.hypot(pz[0], pz[1], pz[2]) > 0.5 ? nrm(pz) : nrm(cross3(Ad, [1, 0, 0]));
|
||
const bv = cross3(Ad, bu);
|
||
poseQAt = (t4, t5, qc) => { // 同支匹配：θ1..θ3 最近
|
||
const ss = branch(t4, t5, Aa, Rr, phh, tool);
|
||
let bb = null, bd = Infinity;
|
||
for (const s0 of ss) {
|
||
if (!inLimit(s0)) continue;
|
||
let d = 0; for (let i = 0; i < 3; i++) d += Math.abs(wrap180(s0[i] - qc[i]));
|
||
if (d < bd) { bd = d; bb = s0; }
|
||
}
|
||
return bb;
|
||
};
|
||
poseRes = q => {
|
||
const w = axisCol(q);
|
||
const dp = w[0] * Ad[0] + w[1] * Ad[1] + w[2] * Ad[2];
|
||
const tx = w[0] - dp * Ad[0], ty = w[1] - dp * Ad[1], tz = w[2] - dp * Ad[2];
|
||
return [tx * bu[0] + ty * bu[1] + tz * bu[2], tx * bv[0] + ty * bv[1] + tz * bv[2], dp];
|
||
};
|
||
poseNewton = q => { // 返回 {q, ne, exact}
|
||
if (!q) return null;
|
||
let lam = 1e-3, bq = q, e = poseRes(q), bn = Math.hypot(e[0], e[1]);
|
||
/* 切向残差在「工具轴与目标轴平行」处同样为零，反平族也收敛到 0，
|
||
所以同向判据只能在收口时验，不能当迭代中止条件（否则每支一进来就废）。 */
|
||
for (let it = 0; it < 28; it++) {
|
||
if (bn < 1e-12) break;
|
||
const dpx = 0.02;
|
||
const qa = poseQAt(bq[3] + dpx, bq[4], bq), qb = poseQAt(bq[3], bq[4] + dpx, bq);
|
||
if (!qa || !qb) break;
|
||
const ea = poseRes(qa), eb = poseRes(qb);
|
||
const j00 = (ea[0] - e[0]) / dpx, j01 = (eb[0] - e[0]) / dpx;
|
||
const j10 = (ea[1] - e[1]) / dpx, j11 = (eb[1] - e[1]) / dpx;
|
||
const a00 = j00 * j00 + j10 * j10, a01 = j00 * j01 + j10 * j11, a11 = j01 * j01 + j11 * j11;
|
||
const g0 = -(j00 * e[0] + j10 * e[1]), g1 = -(j01 * e[0] + j11 * e[1]);
|
||
const d0 = a00 * (1 + lam), d1 = a11 * (1 + lam);
|
||
const det = d0 * d1 - a01 * a01;
|
||
if (!(Math.abs(det) > 1e-20)) break;
|
||
let s0 = (d1 * g0 - a01 * g1) / det, s1 = (-a01 * g0 + d0 * g1) / det;
|
||
const mx = Math.max(Math.abs(s0), Math.abs(s1));
|
||
if (mx > 2) { s0 *= 2 / mx; s1 *= 2 / mx; }
|
||
const n4 = Math.max(L4.lo, Math.min(L4.hi, bq[3] + s0));
|
||
const n5 = Math.max(L5.lo, Math.min(L5.hi, bq[4] + s1));
|
||
const nq = poseQAt(n4, n5, bq);
|
||
if (!nq) { lam *= 8; if (lam > 1e8) break; continue; }
|
||
const en = poseRes(nq);
|
||
const nn = Math.hypot(en[0], en[1]);
|
||
if (nn < bn) { bq = nq; bn = nn; e = en; lam = Math.max(1e-10, lam * 0.4); }
|
||
else { lam *= 8; if (lam > 1e8) break; }
|
||
if (Math.abs(s0) < 1e-13 && Math.abs(s1) < 1e-13) break;
|
||
}
|
||
return { q: bq, ne: bn, exact: bn < 1e-12 && e[2] > 0.999999 };
|
||
};
|
||
return { qAt: poseQAt, res: poseRes, newton: poseNewton };
|
||
}
|
||
|
||
/* 给定 θ1..θ5 的闭式位置解，闭式补滚转角 θ6 并给出全姿态偏差（度）。
|
||
solve 的粗扫与 solveWarm 的续解共用同一份，避免两处判据漂移。 */
|
||
function rollClose(q, Rtar) {
|
||
const M5 = orient(q, 5);
|
||
const N = mul3(T3(M5), Rtar); // 需满足 N = Rz(−θ6)
|
||
const th6 = -Math.atan2(N[3], N[0]) * R2D;
|
||
const q2 = q.slice(); q2[5] = th6;
|
||
return { q: q2, oerr: rotErrDeg(mul3(M5, rotAxis(D.J6, th6 * D2R)), Rtar) };
|
||
}
|
||
|
||
/* ---------- 4. 主求解入口 ---------- */
|
||
function solve(target, opt) {
|
||
opt = opt || {};
|
||
const t0 = nowMs();
|
||
const tool = +opt.tool || 0;
|
||
const W = opt.weights || [1, 1, 1, 1, 1, 1];
|
||
const h = Math.max(0.02, +(opt.step || 0.25));
|
||
const A = target[0] - K.X0;
|
||
const BY = target[1] - K.Y0, BZ = target[2] - K.Z1;
|
||
const R = Math.hypot(BY, BZ), ph = Math.atan2(BZ, BY);
|
||
const tol = opt.tol || 1e-6, tol2 = tol * tol;
|
||
const KEEP = 4000;
|
||
/* 分层保留（2026-10-17）：候选表原先"按行程代价截前 KEEP 条"。行程以当前位形 ref 为参照，
|
||
手臂在立柱另一侧时，前 4000 条全挤在"离当前最近、但贴着立柱"的那几个盆地里——实测目标
|
||
(-136.8, 272.3, 245.5)：无 ref 时 4000 条里 3318 条无干涉，ref=立柱另一侧时 0 条，
|
||
于是判「目标在立柱区内」，换个位置再来又能到（用户报告的现象）。
|
||
改为：前一半按代价取最优，后一半在其余候选里按代价顺序等距抽样，保住远端盆地的代表。 */
|
||
const thinKeep = (arr, keep, cmp) => {
|
||
arr.sort(cmp);
|
||
if (arr.length <= keep) return arr;
|
||
const h = keep >> 1, out = arr.slice(0, h), rest = arr.length - h, m = keep - h;
|
||
for (let k = 0; k < m; k++) out.push(arr[h + Math.floor(k * rest / m)]);
|
||
return out;
|
||
};
|
||
let altPose = null;
|
||
/* 参考位姿（ref）：交互求解时以当前姿态为参考，选解判据取「所有电机需要转过的
|
||
角度之和」最小 —— 即这一步行程最短的构型（取代原先的 Σ|θ| 最小）。
|
||
动机：等价多分支（θ3 的 ±σ/±(π−σ)、肘上/肘下、以及 J1/J3 同号反号的镜像族）
|
||
之间只按 Σ|θ| 选优会随目标微动反复横跳（实测目标挪 20 mm 就翻族、绕一大圈），
|
||
而两族 Σ|θ| 常只差 0~20°，判据本身病态；改以行程为准后，宁可多花几度关节角，
|
||
也不让手臂绕远路。ref 缺省时退化为 Σ|θ| 最小（离线批算口径不变）。 */
|
||
const REF = (opt.ref && opt.ref.length === 6) ? opt.ref.slice() : null;
|
||
const LAM = +opt.refW || 0;
|
||
/* 姿态任务：除 TCP 位置外同时指定工具姿态（R 为行主序 9 数，CAD 系）。
|
||
解法完全复用位置闭式支：(θ4,θ5) 扫描给出一族精确到点的解，每支的姿态
|
||
M = R1…R5 随之确定；对每支解算滚转角 θ6 并只保留工具轴对齐（偏差≤POSETOL）者，
|
||
代价加 POSEW·e_ori² 把精修拉向零偏差。θ6 ∈ ±360° 连续，总在限位内。 */
|
||
const POSE = (opt.pose && opt.pose.R && opt.pose.R.length === 9) ? opt.pose.R.slice() : null;
|
||
const PT = POSE ? poseTools(target, tool, POSE) : null;
|
||
/* 优先级模式（双优先级范式）：
|
||
'pos' 位置优先 —— 位置必须精确；姿态取「位置精确解集内」残差最小者。
|
||
姿态在 POSETOL 内不可达时**不丢弃候选**，改从轴夹角最小的种子出发做
|
||
最小残差搜索，返回位置精确、姿态最近的解并如实上报 oerr。
|
||
'ori' 姿态优先 —— 姿态必须精确；位置取「姿态精确解集内」距目标最近者
|
||
（由 solveTo 调 closestOri 完成，见 §5c）。
|
||
省略（undefined）—— 与改动前逐字一致：POSETOL 硬门、超门即弃并回落 DLS。
|
||
既有调用方（pose_ik_check / bench_ik_split / ikopt 系列）行为不变。 */
|
||
const PRIO = (opt.priority === 'pos' || opt.priority === 'ori') ? opt.priority : null;
|
||
const POSETOL = 2.0; // 粗扫接受的最大姿态偏差（度）
|
||
const POSEFINE = 0.05; // 达标线：偏差 ≤ 此值视为姿态精确到达（机械上已无意义之差）
|
||
let poseW = 200.0; // 姿态偏差惩罚权重（对 deg²）：0.3° 偏差即折合 Σ|θ| 量级
|
||
// （数十度）的代价； escalating 时 ×100 直至出现 POSEFINE 内解
|
||
/* 轴夹角预筛：θ6 只提供绕工具轴的滚转、不改工具轴，故全姿态偏差 oerr ≥ 工具轴夹角
|
||
（20 万随机样本验证零违反）。轴夹角就超 POSETOL 的候选必被 closeRoll 拒，
|
||
先用一次点积挡掉——粗扫里这是姿态开销的大头（每候选省一次 orient 全矩阵 + acos）。 */
|
||
const A0 = POSE ? [POSE[2], POSE[5], POSE[8]] : null;
|
||
const AXREJ = POSE ? 2 * Math.sin(POSETOL * D2R / 2) ** 2 : 0; // = 1 − cos(POSETOL)
|
||
const closeRoll = q => rollClose(q, POSE); // 给定位置解，解 θ6 并返回姿态偏差（度）
|
||
/* 位置优先模式的「姿态最近解」种子：工具轴夹角恒 ≤ 全姿态偏差（θ6 只滚转不改轴），
|
||
故轴夹角最小的点最有希望成为 oerr 最小解。AXREJ 那道门只放行轴夹角 ≤POSETOL 的
|
||
候选，姿态在 POSETOL 外不可达时会把候选全挡下——这里另存一批廉价种子
|
||
（只用已算过的 1−w·A0，不算滚转），供落选后做最小残差搜索。 */
|
||
const SEEDN = 24;
|
||
let axisSeeds = [];
|
||
const keepSeed = (q, key) => {
|
||
let wi = -1;
|
||
if (axisSeeds.length >= SEEDN) { // 先与最差种子比：不比它好就直接丢（避免每次全表扫）
|
||
wi = 0;
|
||
for (let i = 1; i < SEEDN; i++) if (axisSeeds[i].key > axisSeeds[wi].key) wi = i;
|
||
if (key >= axisSeeds[wi].key) return;
|
||
}
|
||
for (const s of axisSeeds) if (Math.hypot(s.q[3] - q[3], s.q[4] - q[4]) <= 2 * h) return;
|
||
if (wi < 0) axisSeeds.push({ q: q, key: key });
|
||
else axisSeeds[wi] = { q: q, key: key };
|
||
};
|
||
|
||
const scoreBranch = (q, e) => { // 对一条位置精确支打分（位置误差 e 已由闭式保证）
|
||
let oerr = 0;
|
||
if (POSE) {
|
||
const w = axisCol(q);
|
||
const dp = w[0] * A0[0] + w[1] * A0[1] + w[2] * A0[2];
|
||
if (PRIO === 'pos') keepSeed(q, 1 - dp);
|
||
if (1 - dp > AXREJ) return null;
|
||
const cl = closeRoll(q);
|
||
if (cl.oerr > POSETOL) return null;
|
||
q = cl.q; oerr = cl.oerr;
|
||
}
|
||
const c = costOf(q, W);
|
||
const rc0 = REF ? costOf(q, W, REF, LAM) : c; // 含连续项、不含姿态惩罚（分层选解用）
|
||
let rc = rc0;
|
||
if (POSE) rc += poseW * oerr * oerr;
|
||
return { q: q, cost: c, rc0: rc0, rcost: rc, err: e, oerr: oerr };
|
||
};
|
||
const evalAt = (t4, t5) => {
|
||
let best = null;
|
||
const sols = branch(t4, t5, A, R, ph, tool);
|
||
for (let k = 0; k < sols.length; k++) {
|
||
const q = sols[k];
|
||
if (!inLimit(q)) continue;
|
||
const c = scoreBranch(q, 0); // 闭式支已精确命中目标；真残差在返回前对入选候选统一补算（O5）
|
||
if (c && (!best || c.rcost < best.rcost - 1e-12)) best = c;
|
||
}
|
||
return best;
|
||
};
|
||
/* 采样轴：含上下限位端点，避免恰在限位上的最优解被漏掉 */
|
||
const axis = (lo, hi, step) => {
|
||
const v = [];
|
||
const n = Math.floor((hi - lo) / step + 1e-9);
|
||
for (let i = 0; i <= n; i++) v.push(lo + i * step);
|
||
if (v[v.length - 1] < hi - 1e-9) v.push(hi);
|
||
return v;
|
||
};
|
||
const L4 = LIM.J4, L5 = LIM.J5;
|
||
const g4 = axis(L4.lo, L4.hi, h);
|
||
|
||
/* 逐行裁剪可行段：位置闭式支只在平面支有解时才存在，限位矩形里只有 7~27%
|
||
的网格点能过 branch 判据（实测 5 个目标 × 全部 θ4 行，每行可行集均为单一
|
||
连续段、零多段）。先用探测步 hp 找每行的可行段，再向两端各放宽一个 hp 兜住
|
||
段界的不确定性，段内仍按原步长 h 精扫 ⇒ 采样密度与全域扫一致、解的质量不动。 */
|
||
const rowFeasible = (t4, t5) => {
|
||
const ss = branch(t4, t5, A, R, ph, tool);
|
||
for (let k = 0; k < ss.length; k++) if (inLimit(ss[k])) return true;
|
||
return false;
|
||
};
|
||
/* 可行段探测步。原为 max(h,2)：2° 的下限与用户设置的扫描步无关，
|
||
而实测本机最窄的可行段只有 0.98°，于是整条窄带被逐行判为「无可行段」，
|
||
表现为 nSol==0 的误报「姿态不可达」（典型目标 6.86,-190.94,240.46）。
|
||
降到 0.5° 后该目标由 2.55° 近似变为 0.00000° 精确，位置任务代价 +14%、
|
||
姿态任务 +54%（与引入兜底全扫相比便宜一个数量级）。 */
|
||
/* 探测步：姿态任务的可行 (θ4,θ5) 集是一条窄带（实测最窄 0.98°），
|
||
2° 的探测步会整条漏掉 → 误报「姿态不可达」；位置任务的可行集宽，
|
||
2° 探测既准又快。故只在姿态任务下降细到 0.5°，位置任务保持逐位不变。 */
|
||
const hp = Math.max(h, POSE ? 0.5 : 2);
|
||
/* 段边界外扩量。原实现用 hp 兼顾「探测步」与「边界外扩」两件事，
|
||
探测步降到 0.5° 后外扩也随之收窄，实测位置任务 2/100 组的 Σ|θ| 会变大
|
||
（最大 +0.182°，最优解正好落在段边界上）。两者职责不同，故解耦：
|
||
探测步可以细（找全薄可行段），外扩必须足够（兜住段界的不确定性）。 */
|
||
const PAD = 2;
|
||
const rowSpans = t4 => {
|
||
const spans = [];
|
||
let lo = null;
|
||
for (let t5 = L5.lo; t5 < L5.hi + 1e-9; t5 += hp) {
|
||
const hi5 = Math.min(t5, L5.hi);
|
||
if (rowFeasible(t4, hi5)) { if (lo === null) lo = hi5; }
|
||
else if (lo !== null) { spans.push([lo - PAD, hi5 + PAD]); lo = null; }
|
||
}
|
||
if (lo !== null) spans.push([lo - PAD, L5.hi + PAD]);
|
||
return spans;
|
||
};
|
||
|
||
|
||
/* 每行可行段探测一次，粗扫与 4.1b 的根枚举各自复用。两者的行集刻意分开：
|
||
粗扫必须保持 h 的密度（根枚举目前还不保证完备，漏根时要靠粗扫的窄带候选兜底，
|
||
实测把粗扫一起放松会换来上百度级的代价劣化）；根枚举只在粗行集上跑，它是
|
||
加分项而不是替代品。 */
|
||
const spansScan = [];
|
||
for (let i = 0; i < g4.length; i++) spansScan.push(rowSpans(g4[i]));
|
||
const g4r = axis(L4.lo, L4.hi, hp), spansRoot = [];
|
||
for (let i = 0; i < g4r.length; i++) spansRoot.push(rowSpans(g4r[i]));
|
||
|
||
/* --- 4.1 全域粗扫：覆盖整个二维冗余 (J4,J5)，按行裁剪到可行段内 --- */
|
||
let all = [], samples = 0, nSol = 0, trimmed = false;
|
||
/* 姿态任务另收"达标带"：oerr ≤ 1° 的候选。达标解在 (θ4,θ5) 平面上是一条窄带，
|
||
按 rcost 取种子会整体错过它（带的 Σ|θ| 常很大）；按 oerr 排序取种子才能保证
|
||
局部加密窗口罩住曲线，再由模式搜索在带上找 Σ|θ| 最小点。 */
|
||
let band = [];
|
||
for (let i = 0; i < g4.length; i++) {
|
||
const spans = spansScan[i];
|
||
for (let sp = 0; sp < spans.length; sp++) {
|
||
const g5 = axis(Math.max(L5.lo, spans[sp][0]), Math.min(L5.hi, spans[sp][1]), h);
|
||
for (let j = 0; j < g5.length; j++) {
|
||
samples++;
|
||
const sv = evalAt(g4[i], g5[j]);
|
||
if (sv) { all.push(sv); nSol++; if (POSE && sv.oerr <= 1) band.push(sv); }
|
||
}
|
||
}
|
||
if (!POSE && all.length > KEEP * 12) { // 有界保留：代价最优一半 + 其余分层抽样（见 thinKeep）
|
||
all = thinKeep(all, KEEP, (x, y) => x.rcost - y.rcost);
|
||
trimmed = true;
|
||
}
|
||
}
|
||
if (POSE) { // 姿态模式同样需要有界保留（带优先，再补全局最优）
|
||
band.sort((x, y) => x.oerr - y.oerr);
|
||
const keepBand = band.slice(0, KEEP);
|
||
const keepSet = new Set(keepBand);
|
||
/* 位置精确族按姿态残差排的分层样本：精确姿态的解全撞立柱时，供上层退而取
|
||
"位置精确、姿态最接近的无干涉解"（findSafeTarget），不再整单判不可达 */
|
||
altPose = thinKeep(all.slice(), 800, (x, y) => x.oerr - y.oerr).map(s => ({ q: s.q.slice(), oerr: s.oerr, cost: s.cost, err: 0 }));
|
||
const rest = thinKeep(all.filter(s => !keepSet.has(s)), KEEP, (x, y) => x.rcost - y.rcost);
|
||
all = keepBand.concat(rest);
|
||
trimmed = band.length + rest.length < nSol;
|
||
} else all.sort((x, y) => x.rcost - y.rcost);
|
||
|
||
/* --- 4.1b 姿态孤立根枚举 ---
|
||
不再用细网格密度去撞那条窄带（原路径因此要背 POSETOL 硬筛、band 按 oerr
|
||
取种子、escalating ×100 重扫一整套补偿，残差也只到 ~0.02°）：逐行对残差
|
||
两个分量找变号括号，从括号中点起、只对发生变号的那一支跑阻尼高斯-牛顿
|
||
（奇异时加大阻尼而非退出）。同向判据只能收口时验——工具轴反平同样令切向
|
||
残差为 0，是这套判据最容易自证的地方。无根的刀尖构型收敛到 ‖e‖ 的真实最小。 */
|
||
if (POSE && opt.refine !== false) {
|
||
const roots = [], bests = [];
|
||
const seedNewton = (t4, t5, qc) => {
|
||
const q0 = PT.qAt(t4, t5, qc);
|
||
if (!q0) return;
|
||
const r = PT.newton(q0);
|
||
if (!r) return;
|
||
const tgtL = r.exact ? roots : bests;
|
||
let dup = false;
|
||
for (const x of tgtL) { let d = 0; for (let i = 0; i < 5; i++) d += Math.abs(x.q[i] - r.q[i]); if (d < 0.02) { dup = true; break; } }
|
||
if (!dup) tgtL.push(r);
|
||
};
|
||
for (let ri = 0; ri < g4r.length; ri++) {
|
||
const t4 = g4r[ri];
|
||
const rowBest = [null, null, null, null];
|
||
for (const sp of spansRoot[ri]) {
|
||
const a5 = Math.max(L5.lo, sp[0]), b5 = Math.min(L5.hi, sp[1]);
|
||
let prev = null;
|
||
for (let t5 = a5; t5 <= b5 + 1e-9; t5 += hp) {
|
||
const hi5 = Math.min(t5, b5);
|
||
const ss = branch(t4, hi5, A, R, ph, tool);
|
||
/* 按支下标对齐（不可行的支留 null 占位）：换个写法就会让 prev/cur 的
|
||
下标错开，括号判据随之失配，实测会整支漏掉一个真根、多付上百度关节角。 */
|
||
const cur = [];
|
||
for (let k = 0; k < ss.length; k++) {
|
||
if (!inLimit(ss[k])) { cur.push(null); continue; }
|
||
const e = PT.res(ss[k]);
|
||
cur.push({ q: ss[k], e: e });
|
||
if (e[2] > 0 && (!rowBest[k] || Math.hypot(e[0], e[1]) < rowBest[k].n))
|
||
rowBest[k] = { q: ss[k], t5: hi5, n: Math.hypot(e[0], e[1]) };
|
||
}
|
||
if (prev) {
|
||
for (let k = 0; k < Math.min(prev.length, cur.length); k++) {
|
||
const pv = prev[k], cu = cur[k];
|
||
if (!pv || !cu || !pv.e[2] || !cu.e[2]) continue;
|
||
if ((pv.e[0] < 0) !== (cu.e[0] < 0) || (pv.e[1] < 0) !== (cu.e[1] < 0))
|
||
seedNewton(t4, hi5 - hp / 2, pv.q); // 变号括号：中点起、沿该支
|
||
}
|
||
}
|
||
prev = cur;
|
||
}
|
||
/* 切向相切的根不产生变号，另按每支 ‖e‖ 最小的节点补种一次 */
|
||
for (const rb of rowBest) if (rb) seedNewton(t4, rb.t5, rb.q);
|
||
}
|
||
}
|
||
for (const r of roots) { // 精确根直接进候选池
|
||
const p = fk(r.q, tool);
|
||
const e2 = (p[0] - target[0]) ** 2 + (p[1] - target[1]) ** 2 + (p[2] - target[2]) ** 2;
|
||
if (!(e2 <= tol * tol)) continue;
|
||
const c = scoreBranch(r.q, Math.sqrt(e2));
|
||
if (c) { all.push(c); nSol++; if (c.oerr <= 1) band.push(c); }
|
||
}
|
||
if (!roots.length) for (const r of bests) { // 无根时的真实最小残差兜底
|
||
const p = fk(r.q, tool);
|
||
const e2 = (p[0] - target[0]) ** 2 + (p[1] - target[1]) ** 2 + (p[2] - target[2]) ** 2;
|
||
const c = e2 <= tol * tol ? scoreBranch(r.q, Math.sqrt(e2)) : null;
|
||
if (c) { all.push(c); if (c.oerr <= 1) band.push(c); }
|
||
}
|
||
}
|
||
|
||
/* --- 4.2 多起点局部加密 + 模式搜索 ---
|
||
姿态任务分层处理：先 escalating（poseW ×100 重扫）直到出现 oerr ≤ POSEFINE 的解，
|
||
再在达标候选里按含连续项的代价取优——避免"省几度关节角、丢 0.2° 姿态"的次优交易。 */
|
||
const refinePass = () => {
|
||
if (!all.length) return; // 一个候选都没有（姿态完全不可达）：无从精修
|
||
const seeds = [];
|
||
if (POSE) {
|
||
/* 姿态种子：达标带按 oerr 升序、空间去同（相距 ≤ 2h 只留最优），
|
||
保证种子贴在窄带上，局部窗口才能罩住精确解 */
|
||
for (const s of band) {
|
||
if (seeds.length >= 32) break;
|
||
let dup = false;
|
||
for (const t of seeds) if (Math.hypot(t.q[3] - s.q[3], t.q[4] - s.q[4]) <= 2 * h) { dup = true; break; }
|
||
if (!dup) seeds.push(s);
|
||
}
|
||
if (!seeds.length) seeds.push(all[0]);
|
||
} else { for (let i = 0; i < all.length && seeds.length < 32; i++) {
|
||
const s = all[i];
|
||
let dup = false;
|
||
for (const t of seeds) if (Math.hypot(t.q[3] - s.q[3], t.q[4] - s.q[4]) <= h * 1.5) { dup = true; break; }
|
||
if (!dup) seeds.push(s);
|
||
}
|
||
}
|
||
const cl = (v, a, b) => v < a ? a : (v > b ? b : v);
|
||
for (const s of seeds) { // 局部加密网格（h/8，±2h 窗口）
|
||
const hh = POSE ? h / 8 : (h <= 0.5 ? h / 4 : h / 8); // 精修是耗时地板：常用小步长（h≤0.5°）下加密网格降为 h/4（4× 提速，窗口保持 ±2h 不变）；实测 h>0.25° 的加密步长会跳过 <0.5° 宽的低代价窄谷（60,128,150 目标 +61°），故粗步长档完全保持原行为；姿态任务种子贴窄带、保持 h/8
|
||
const w4 = axis(Math.max(L4.lo, s.q[3] - 2 * h), Math.min(L4.hi, s.q[3] + 2 * h), hh);
|
||
const w5 = axis(Math.max(L5.lo, s.q[4] - 2 * h), Math.min(L5.hi, s.q[4] + 2 * h), hh);
|
||
let lb = s;
|
||
for (const a of w4) for (const b of w5) {
|
||
samples++;
|
||
const c = evalAt(a, b);
|
||
if (c) { nSol++; if (c.rcost < lb.rcost) lb = c; }
|
||
}
|
||
all.push(lb);
|
||
}
|
||
all.sort((x, y) => x.rcost - y.rcost);
|
||
const NB = [[1,0],[-1,0],[0,1],[0,-1],[1,1],[1,-1],[-1,1],[-1,-1]];
|
||
for (const s of all.slice(0, 24)) { // 模式搜索
|
||
let c4 = s.q[3], c5 = s.q[4], lb = s, step = h / 4, guard = 0;
|
||
while (step > 2e-4 && guard++ < 400) {
|
||
let moved = false;
|
||
for (const d of NB) {
|
||
const t4 = cl(c4 + d[0] * step, L4.lo, L4.hi), t5 = cl(c5 + d[1] * step, L5.lo, L5.hi);
|
||
if (t4 === c4 && t5 === c5) continue;
|
||
samples++;
|
||
const c = evalAt(t4, t5);
|
||
if (c && c.rcost < lb.rcost - 1e-10) { lb = c; c4 = t4; c5 = t5; moved = true; }
|
||
}
|
||
if (!moved) step *= 0.5;
|
||
}
|
||
all.push(lb);
|
||
}
|
||
};
|
||
if (opt.refine !== false) {
|
||
refinePass();
|
||
for (let k = 0; k < 3; k++) { // escalating：尚无 POSEFINE 内解则姿态权重 ×100 重扫
|
||
if (!POSE || all.some(s => s.oerr != null && s.oerr <= POSEFINE)) break;
|
||
poseW *= 100;
|
||
refinePass();
|
||
}
|
||
}
|
||
/* --- 4.3 姿态收口：(θ4,θ5) 二维牛顿迭代 ---
|
||
达标带是 (θ4,θ5) 平面上的斜窄谷（局部灵敏度可达 ~10°/°），8 邻域模式搜索会整在
|
||
谷壁上（实测最好只收到 0.106°）。这里直接对"工具轴在目标轴法平面上的切向偏差"
|
||
做牛顿迭代：每步在新 (θ4,θ5) 上重解闭式位置支、按 θ1..θ3 最近匹配取同支，
|
||
位置天然保持精确，轴偏差压到 ~1e-12 量级。 */
|
||
if (POSE && opt.refine !== false && all.length) {
|
||
const A0 = [POSE[2], POSE[5], POSE[8]]; // 目标工具轴（列 2）
|
||
const uz = cross3(A0, [0, 0, 1]);
|
||
const u = Math.hypot(uz[0], uz[1], uz[2]) > 0.5 ? nrm(uz) : nrm(cross3(A0, [1, 0, 0]));
|
||
const v = cross3(A0, u);
|
||
const err2 = q => {
|
||
const w = axisCol(q); // 列 2 = 工具轴（原取 [M5[6..8]] 实为第 2 行）
|
||
const d = w[0] * A0[0] + w[1] * A0[1] + w[2] * A0[2];
|
||
const tx = w[0] - d * A0[0], ty = w[1] - d * A0[1], tz = w[2] - d * A0[2];
|
||
return [tx * u[0] + ty * u[1] + tz * u[2], tx * v[0] + ty * v[1] + tz * v[2]];
|
||
};
|
||
const match = (t4, t5, qcur) => { // 同支匹配：θ1..θ3 最近
|
||
let best = null, bd = Infinity;
|
||
for (const s of branch(t4, t5, A, R, ph, tool)) {
|
||
if (!inLimit(s)) continue;
|
||
let d = 0;
|
||
for (let i = 0; i < 3; i++) d += Math.abs(wrap180(s[i] - qcur[i]));
|
||
if (d < bd) { bd = d; best = s; }
|
||
}
|
||
return best;
|
||
};
|
||
const newton = q0 => {
|
||
let q = q0.slice(), dp = 0.05;
|
||
for (let it = 0; it < 24; it++) {
|
||
const e0 = err2(q);
|
||
if (Math.hypot(e0[0], e0[1]) < 1e-12) break;
|
||
const c4 = q[3], c5 = q[4];
|
||
const qa = q.slice(); qa[3] = c4 + dp;
|
||
const qb = q.slice(); qb[4] = c5 + dp;
|
||
const e4 = err2(qa), e5 = err2(qb);
|
||
const j00 = (e4[0] - e0[0]) / dp, j01 = (e5[0] - e0[0]) / dp;
|
||
const j10 = (e4[1] - e0[1]) / dp, j11 = (e5[1] - e0[1]) / dp;
|
||
const det = j00 * j11 - j01 * j10;
|
||
if (Math.abs(det) < 1e-12) { // 近奇异（刀尖构型）：缩差分步长重试
|
||
if (dp > 0.002) { dp *= 0.25; continue; }
|
||
break;
|
||
}
|
||
let d4 = -(j11 * e0[0] - j01 * e0[1]) / det;
|
||
let d5 = -(-j10 * e0[0] + j00 * e0[1]) / det;
|
||
const mx = Math.max(Math.abs(d4), Math.abs(d5));
|
||
if (mx > 2) { d4 *= 2 / mx; d5 *= 2 / mx; } // 阻尼：单步 ≤ 2°
|
||
const t4 = Math.max(L4.lo, Math.min(L4.hi, c4 + d4));
|
||
const t5 = Math.max(L5.lo, Math.min(L5.hi, c5 + d5));
|
||
if (t4 === c4 && t5 === c5) break;
|
||
const nb = match(t4, t5, q);
|
||
if (!nb) { if (dp > 0.002) { dp *= 0.25; continue; } break; }
|
||
q = nb;
|
||
}
|
||
return q;
|
||
};
|
||
const cands = all.slice().sort((x, y) => x.oerr - y.oerr).slice(0, 16);
|
||
for (const s of cands) {
|
||
if (s.oerr <= POSEFINE) continue; // 已达标，无需收口
|
||
const tries = [s.q];
|
||
for (const d of [[0.15, 0], [-0.15, 0], [0, 0.15], [0, -0.15], [0.25, 0.25], [-0.25, -0.25]]) {
|
||
const qp = s.q.slice();
|
||
qp[3] = Math.max(L4.lo, Math.min(L4.hi, qp[3] + d[0]));
|
||
qp[4] = Math.max(L5.lo, Math.min(L5.hi, qp[4] + d[1]));
|
||
const nb = match(qp[3], qp[4], s.q);
|
||
if (nb) tries.push(nb); // 微扰重启：探盆地边界（奇异刀尖构型）
|
||
}
|
||
for (const t of tries) {
|
||
const q2 = newton(t);
|
||
if (!q2) continue;
|
||
const p2 = fk(q2, tool);
|
||
const e2 = Math.hypot(p2[0] - target[0], p2[1] - target[1], p2[2] - target[2]);
|
||
if (e2 > tol) continue;
|
||
const c2 = scoreBranch(q2, e2);
|
||
if (c2 && c2.oerr < s.oerr - 1e-9) { nSol++; all.push(c2); break; }
|
||
}
|
||
}
|
||
}
|
||
/* --- 4.4 终了精修：把代价最优的若干条入选候选都收敛成真根 ---
|
||
根枚举的变号括号只收「残差穿过零」的根，与某个精修候选相邻的那一支可能漏检；
|
||
漏检时若把达标门收死，就会为 0.01° 姿态多付几十度关节角。这里让入选集合自己
|
||
走到 ‖e‖=0，精确集合与最优集合随之重合。 */
|
||
if (POSE && opt.refine !== false && all.length) {
|
||
const top = all.slice().sort((x, y) => x.rc0 - y.rc0).slice(0, 12);
|
||
for (const s0 of top) {
|
||
if (s0.oerr != null && s0.oerr < 1e-6) continue;
|
||
const r = PT.newton(s0.q);
|
||
if (!r || !r.exact) continue;
|
||
const p = fk(r.q, tool);
|
||
const e2 = (p[0] - target[0]) ** 2 + (p[1] - target[1]) ** 2 + (p[2] - target[2]) ** 2;
|
||
if (!(e2 <= tol * tol)) continue;
|
||
const c = scoreBranch(r.q, Math.sqrt(e2));
|
||
if (c) { all.push(c); if (c.oerr <= 1) band.push(c); }
|
||
}
|
||
}
|
||
/* --- 4.5 位置优先的「姿态最近解」搜索（仅在 POSETOL 硬门把候选全挡下时启用）---
|
||
位置优先的语义是 min oerr s.t. 位置精确。姿态在 POSETOL 内不可达时上面全部
|
||
阶段都会得到空池（这也是改动前回落到 DLS 的原因）。此处不丢弃已有信息：
|
||
从轴夹角最小的若干种子出发，在 (θ4,θ5) 上做模式搜索直接最小化 oerr。
|
||
每个搜索点仍走 branch() 闭式位置支 ⇒ 位置保持精确，残差如实上报、不虚报。
|
||
常态（姿态精确可达）下本段完全不执行，扫描路径与耗时不变。 */
|
||
/* 原条件 !all.length 只在「一个候选都没有」时启用，实测姿态精确解常在
|
||
粗扫与根枚举之后仍未被生成（目标 17.9,194.8,436.5 与 383.9,241.8,824.2），
|
||
此时 all 非空但其中最好的也差 0.07°~1.33°，于是被误标为"精确到达"。
|
||
放宽为「没有任何 oerr ≤ POSEFINE 的候选」，让本段从轴夹角最小的种子
|
||
出发把残差收下去；姿态精确可达时本段仍不执行，扫描路径不变。 */
|
||
if (POSE && PRIO === 'pos' && !all.some(sv => sv.oerr != null && sv.oerr <= POSEFINE) && axisSeeds.length) {
|
||
const pack = (r) => {
|
||
const c = costOf(r.q, W);
|
||
const rc0 = REF ? costOf(r.q, W, REF, LAM) : c;
|
||
return { q: r.q, cost: c, rc0: rc0, rcost: rc0, err: r.err, oerr: r.oerr };
|
||
};
|
||
/* 在 (θ4,θ5) 上取最优 oerr（同一点的多条位置支都试） */
|
||
const oerrAt = (t4, t5) => {
|
||
let best = null;
|
||
const ss = branch(t4, t5, A, R, ph, tool);
|
||
for (let k = 0; k < ss.length; k++) {
|
||
const s0 = ss[k];
|
||
if (!inLimit(s0)) continue;
|
||
const p = fk(s0, tool);
|
||
const dx = p[0] - target[0], dy = p[1] - target[1], dz = p[2] - target[2];
|
||
const e2 = dx * dx + dy * dy + dz * dz;
|
||
if (!(e2 <= tol2)) continue;
|
||
const cl = closeRoll(s0);
|
||
if (!best || cl.oerr < best.oerr) best = { q: cl.q, oerr: cl.oerr, err: Math.sqrt(e2) };
|
||
}
|
||
return best;
|
||
};
|
||
const NB = [[1,0],[-1,0],[0,1],[0,-1],[1,1],[1,-1],[-1,1],[-1,-1]];
|
||
const cl4 = (v, a, b) => v < a ? a : (v > b ? b : v);
|
||
let bs = null;
|
||
for (const sd of axisSeeds) {
|
||
let lb = oerrAt(sd.q[3], sd.q[4]);
|
||
if (!lb) continue;
|
||
samples++;
|
||
let c4 = sd.q[3], c5 = sd.q[4], step = Math.max(h, 0.5), guard = 0;
|
||
while (step > 2e-4 && guard++ < 400) { // 模式搜索：步长减半直到收敛
|
||
let moved = false;
|
||
for (const d of NB) {
|
||
const t4 = cl4(c4 + d[0] * step, L4.lo, L4.hi), t5 = cl4(c5 + d[1] * step, L5.lo, L5.hi);
|
||
if (t4 === c4 && t5 === c5) continue;
|
||
samples++;
|
||
const c = oerrAt(t4, t5);
|
||
if (c && c.oerr < lb.oerr - 1e-12) { lb = c; c4 = t4; c5 = t5; moved = true; }
|
||
}
|
||
if (!moved) step *= 0.5;
|
||
}
|
||
if (lb && (!bs || lb.oerr < bs.oerr - 1e-12)) bs = lb;
|
||
}
|
||
if (bs) { nSol++; all.push(pack(bs)); }
|
||
}
|
||
all.sort((x, y) => x.rcost - y.rcost);
|
||
/* 采用解：姿态任务优先从 oerr ≤ POSEFINE 的达标候选里按含连续项代价选；无达标解才尽力而为 */
|
||
let pool = all, poolByOerr = false;
|
||
if (POSE) {
|
||
/* 有真根时优先取真根，但不为姿态精度付大额关节角：精确根只有在代价不高于
|
||
达标最优 +2° 时才接管（实测两者通常差 <0.05°，个别漏检情形能差到 40°）。 */
|
||
const acc = all.filter(s => s.oerr != null && s.oerr <= POSEFINE);
|
||
const ex = all.filter(s => s.oerr != null && s.oerr <= 1e-4);
|
||
if (acc.length) {
|
||
const ca = Math.min(...acc.map(s => s.rc0));
|
||
pool = (ex.length && Math.min(...ex.map(s => s.rc0)) <= ca + 2) ? ex : acc;
|
||
} else if (PRIO === 'pos') {
|
||
/* 位置优先：无姿态达标解 → 取姿态残差最小者（同量级时取 Σ|θ| 更小者） */
|
||
poolByOerr = true;
|
||
pool = all.slice();
|
||
}
|
||
}
|
||
pool.sort((x, y) => poolByOerr ? ((x.oerr - y.oerr) || (x.rc0 - y.rc0))
|
||
: (POSE ? x.rc0 - y.rc0 : x.rcost - y.rcost));
|
||
const best = pool.length ? pool[0] : null;
|
||
const list = thinKeep(pool.slice(), KEEP, (x, y) => POSE ? x.rc0 - y.rc0 : x.rcost - y.rcost);
|
||
if (best && list.indexOf(best) < 0) list.unshift(best);
|
||
/* 真残差补算：热循环里延后的 fk 校验在这里对入选候选统一补一次（err 为真值） */
|
||
if (best) {
|
||
const errOf = q => { const p = fk(q, tool); const dx = p[0] - target[0], dy = p[1] - target[1], dz = p[2] - target[2]; return Math.hypot(dx, dy, dz); };
|
||
best.err = errOf(best.q);
|
||
const nErr = Math.min(list.length, 64);
|
||
for (let i = 0; i < nErr; i++) if (list[i].err === 0) list[i].err = errOf(list[i].q);
|
||
}
|
||
return {
|
||
ok: !!best, reachable: !!best, target: target.slice(), tool: tool,
|
||
pose: !!POSE, priority: PRIO,
|
||
/* 姿态分级：reachable 只表示"有解"，不足以区分"精确"与"近似"（缺陷 3）。
|
||
poseFine=姿态偏差 ≤POSEFINE(0.05°) 才算精确到达；poseTier 供上层显式区分：
|
||
fine = 精确 approx = 落在 POSETOL(2°) 内的近似 poor = 连 POSETOL 都没进
|
||
none = 没有任何候选
|
||
reachablePose 是严格版（位置与姿态同时达标），需要严格语义时用它。 */
|
||
poseFine: POSE ? !!(best && best.oerr != null && best.oerr <= POSEFINE) : null,
|
||
poseTier: !POSE ? null : (!best ? 'none'
|
||
: (best.oerr != null && best.oerr <= POSEFINE ? 'fine'
|
||
: (best.oerr != null && best.oerr <= POSETOL ? 'approx' : 'poor'))),
|
||
reachablePose: POSE
|
||
? !!(best && best.oerr != null && best.oerr <= POSEFINE
|
||
&& best.err != null && best.err <= Math.sqrt(tol2)) : null,
|
||
q: best ? best.q.slice() : null, cost: best ? best.cost : null,
|
||
/* 本步行程 = 判据值：Σ|wrap(θ−ref)|（连续轴按等价角计），便于界面核对 */
|
||
moveSum: (REF && best) ? (() => {
|
||
let t = 0; for (let i = 0; i < 6; i++) t += Math.abs(wrap180(best.q[i] - REF[i])); return t;
|
||
})() : null,
|
||
err: best ? best.err : null, oerr: best ? best.oerr : null,
|
||
nSol: nSol, truncated: trimmed,
|
||
samples: samples, ms: nowMs() - t0, list: list, altPose: altPose
|
||
};
|
||
}
|
||
|
||
/* ---------- 5. 最近可达位姿（阻尼最小二乘，兜底用） ---------- */
|
||
function closest(target, tool, tries, ref) {
|
||
const dp = 0.05, lam = 4.0;
|
||
let bestQ = null, bestD = Infinity;
|
||
/* anchor：参考位姿锚定。准退化构型（手臂近似伸直）下 J3 等轴的雅可比列梯度≈0，
|
||
DLS 伪逆仍会给它分配微小步长，160 次迭代累积成大幅无用漂移（实测 82°）。
|
||
每次迭代施加 0.03·(q−anchor) 的回拉，把退化方向的漂移压掉，任务真正需要的
|
||
方向（远大于此回拉）不受影响。 */
|
||
const run = (q, anchor) => {
|
||
const J = new Float64Array(18);
|
||
for (let it = 0; it < 160; it++) {
|
||
const p = fk(q, tool);
|
||
const d = Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]);
|
||
if (d < bestD) { bestD = d; bestQ = q.slice(); }
|
||
if (d < 1e-9) break;
|
||
for (let c = 0; c < 6; c++) {
|
||
const s = q.slice(); s[c] += dp;
|
||
const pp = fk(s, tool);
|
||
J[c * 3] = (pp[0] - p[0]) / dp; J[c * 3 + 1] = (pp[1] - p[1]) / dp; J[c * 3 + 2] = (pp[2] - p[2]) / dp;
|
||
}
|
||
let m00 = lam, m01 = 0, m02 = 0, m11 = lam, m12 = 0, m22 = lam;
|
||
for (let c = 0; c < 6; c++) {
|
||
const ax2 = J[c * 3], ay = J[c * 3 + 1], az2 = J[c * 3 + 2];
|
||
m00 += ax2 * ax2; m01 += ax2 * ay; m02 += ax2 * az2;
|
||
m11 += ay * ay; m12 += ay * az2; m22 += az2 * az2;
|
||
}
|
||
const det = m00 * (m11 * m22 - m12 * m12) - m01 * (m01 * m22 - m12 * m02) + m02 * (m01 * m12 - m11 * m02);
|
||
if (Math.abs(det) < 1e-12) break;
|
||
const i00 = (m11 * m22 - m12 * m12) / det, i01 = -(m01 * m22 - m12 * m02) / det,
|
||
i02 = (m01 * m12 - m11 * m02) / det, i11 = (m00 * m22 - m02 * m02) / det,
|
||
i12 = -(m00 * m12 - m01 * m02) / det, i22 = (m00 * m11 - m01 * m01) / det;
|
||
const ex = p[0] - target[0], ey = p[1] - target[1], ez = p[2] - target[2];
|
||
const y0 = -(i00 * ex + i01 * ey + i02 * ez);
|
||
const y1 = -(i01 * ex + i11 * ey + i12 * ez);
|
||
const y2 = -(i02 * ex + i12 * ey + i22 * ez);
|
||
const dq = [0, 0, 0, 0, 0, 0]; let mx = 0;
|
||
for (let c = 0; c < 6; c++) {
|
||
dq[c] = J[c * 3] * y0 + J[c * 3 + 1] * y1 + J[c * 3 + 2] * y2;
|
||
mx = Math.max(mx, Math.abs(dq[c]));
|
||
}
|
||
const gain = mx > 12 ? 12 / mx : 1;
|
||
for (let c = 0; c < 6; c++) {
|
||
const t = LIM[ORDER[c]];
|
||
let stp = dq[c] * gain;
|
||
if (anchor) stp -= 0.03 * (q[c] - anchor[c]);
|
||
q[c] = Math.max(t.lo, Math.min(t.hi, q[c] + stp));
|
||
}
|
||
}
|
||
};
|
||
/* 零空间抛光：手臂伸直（目标在可达边界外）时 J3 轴与臂线共线，转 J3 不改变
|
||
TCP，DLS 对该方向梯度为零、停在哪里全看随机初始角。这里在"到目标距离几乎
|
||
不变"（≤slack）的前提下令姿态滑向最小总关节角，把随机角收敛回确定代表值。
|
||
同时兼顾连续性：ref 给出时从 ref 起步，邻近目标 successive 解算平滑跟进。 */
|
||
const distQ = q => Math.hypot.apply(null, fk(q, tool).map((v, i) => v - target[i]));
|
||
const polish = (q, slack) => {
|
||
if (!q) return;
|
||
const d0 = distQ(q);
|
||
let step = 8;
|
||
for (let pass = 0; pass < 4; pass++) {
|
||
let moved = true, guard = 0;
|
||
while (moved && guard++ < 40) {
|
||
moved = false;
|
||
for (let i = 0; i < 5; i++) { // J6 不改 TCP 位置，抛光无意义
|
||
const t = LIM[ORDER[i]];
|
||
for (const s of [1, -1]) {
|
||
const v = q[i] + s * step;
|
||
if (v < t.lo || v > t.hi) continue;
|
||
const old = q[i], c0 = costOf(q, null);
|
||
q[i] = v;
|
||
const d = distQ(q);
|
||
if (d <= d0 + slack && costOf(q, null) < c0 - 1e-9) moved = true;
|
||
else q[i] = old;
|
||
}
|
||
}
|
||
}
|
||
step *= 0.25;
|
||
}
|
||
};
|
||
let anchorQ = null, anchorD = Infinity;
|
||
if (ref && ref.length === 6) {
|
||
anchorQ = ref.slice();
|
||
run(anchorQ, ref); // 锚定轮：从当前姿态出发保连续
|
||
anchorD = distQ(anchorQ);
|
||
}
|
||
run([0, 0, 0, 0, 0, 0]);
|
||
/* 确定性拟随机重启：固定种子的 LCG，取代 Math.random —— 同一目标重复解算
|
||
结果必须逐位一致，否则边界目标每次刷新姿态都不同，就是"随机倒摆"的根源 */
|
||
let seed = 0x5bd1e995;
|
||
const rnd = () => { seed = (seed * 1103515245 + 12345) & 0x7fffffff; return seed / 0x7fffffff; };
|
||
const N = tries == null ? 160 : tries;
|
||
for (let k = 0; k < N; k++) {
|
||
const q = [0, 0, 0, 0, 0, 0];
|
||
for (let c = 0; c < 5; c++) { const t = LIM[ORDER[c]]; q[c] = t.lo + rnd() * (t.hi - t.lo); }
|
||
run(q);
|
||
}
|
||
/* 锚定解与全局最优距离差 ≤0.5mm 时优先锚定解：刚过边界的目标（差 mm 以内）
|
||
保住原构型自然伸出，不为零点几毫米换个 Fold 姿态；差得远时仍取全局最近 */
|
||
if (anchorQ && anchorD <= bestD + 0.5) { bestQ = anchorQ; bestD = anchorD; }
|
||
if (bestQ) { // 报告抛光后真实距离，不虚报
|
||
polish(bestQ, 0.05); // 距离容忍 0.05mm（远小于边界误差量级）
|
||
bestD = distQ(bestQ);
|
||
}
|
||
return { q: bestQ, dist: bestD };
|
||
}
|
||
|
||
/* ---------- 5b. 最近可达位姿（位置 + 姿态联合，阻尼最小二乘） ---------- */
|
||
/* 误差 6 维：位置 3（mm）+ 姿态 3（R_curᵀ·R_des 的旋转矢量，rad）。
|
||
姿态项按 20 mm/rad 折算进误差向量（≈0.35mm/度），阻尼项吸收量纲差。 */
|
||
function closest6(target, Rdes, tool, tries, ref) {
|
||
const OW = 20.0; // mm per rad
|
||
const dp = 0.05, lam = 6.0;
|
||
let bestQ = null, bestE = Infinity, bestOri = 0;
|
||
const errOf = q => {
|
||
const p = fk(q, tool), N = mul3(T3(orient(q)), Rdes);
|
||
const tr = N[0] + N[4] + N[8], ang = Math.acos(clamp1((tr - 1) / 2));
|
||
const s = Math.sin(ang);
|
||
const k = s > 1e-6 ? 1 / (2 * s) : 0.5; // 小角时直接用斜对称向量/2，免奇异
|
||
const rv = [(N[7] - N[5]) * k, (N[2] - N[6]) * k, (N[3] - N[1]) * k];
|
||
return [p[0] - target[0], p[1] - target[1], p[2] - target[2],
|
||
rv[0] * OW, rv[1] * OW, rv[2] * OW];
|
||
};
|
||
const norm6 = e => Math.hypot(e[0], e[1], e[2], e[3], e[4], e[5]);
|
||
const oriOf = q => { // 纯姿态偏差（度）
|
||
const N = mul3(T3(orient(q)), Rdes);
|
||
return Math.acos(clamp1((N[0] + N[4] + N[8] - 1) / 2)) * R2D;
|
||
};
|
||
/* 6×6 线性方程组：高斯消去 + 列主元（A = J·Jᵀ + λ²I，y = A⁻¹e，dq = Jᵀy） */
|
||
const solveLin = (A, b, n) => {
|
||
const M = A.slice(), x = b.slice();
|
||
for (let c = 0; c < n; c++) {
|
||
let p = c;
|
||
for (let r = c + 1; r < n; r++) if (Math.abs(M[r * n + c]) > Math.abs(M[p * n + c])) p = r;
|
||
if (Math.abs(M[p * n + c]) < 1e-14) return null;
|
||
if (p !== c) { for (let j = 0; j < n; j++) { const t = M[c * n + j]; M[c * n + j] = M[p * n + j]; M[p * n + j] = t; } const t = x[c]; x[c] = x[p]; x[p] = t; }
|
||
for (let r = 0; r < n; r++) {
|
||
if (r === c) continue;
|
||
const f = M[r * n + c] / M[c * n + c];
|
||
if (!f) continue;
|
||
for (let j = c; j < n; j++) M[r * n + j] -= f * M[c * n + j];
|
||
x[r] -= f * x[c];
|
||
}
|
||
}
|
||
for (let c = 0; c < n; c++) x[c] /= M[c * n + c];
|
||
return x;
|
||
};
|
||
const run = (q, anchor) => {
|
||
const J = new Float64Array(36); // 6×6（误差 × 关节）
|
||
for (let it = 0; it < 160; it++) {
|
||
const e = errOf(q);
|
||
const ne = norm6(e);
|
||
if (ne < bestE) { bestE = ne; bestQ = q.slice(); bestOri = oriOf(q); }
|
||
if (ne < 1e-9) break;
|
||
for (let c = 0; c < 6; c++) {
|
||
const s = q.slice(); s[c] += dp;
|
||
const ep = errOf(s);
|
||
for (let r = 0; r < 6; r++) J[r * 6 + c] = (ep[r] - e[r]) / dp;
|
||
}
|
||
const A = new Array(36).fill(0);
|
||
for (let i = 0; i < 6; i++) {
|
||
for (let j = 0; j < 6; j++) { let s = 0; for (let k = 0; k < 6; k++) s += J[i * 6 + k] * J[j * 6 + k]; A[i * 6 + j] = s + (i === j ? lam * lam : 0); }
|
||
}
|
||
const y = solveLin(A, e.map(v => -v), 6);
|
||
if (!y) break;
|
||
const dq = new Array(6);
|
||
let mx = 0;
|
||
for (let c = 0; c < 6; c++) { let s = 0; for (let r = 0; r < 6; r++) s += J[r * 6 + c] * y[r]; dq[c] = s; mx = Math.max(mx, Math.abs(s)); }
|
||
const gain = mx > 12 ? 12 / mx : 1;
|
||
for (let c = 0; c < 6; c++) {
|
||
const t = LIM[ORDER[c]];
|
||
let stp = dq[c] * gain;
|
||
if (anchor) stp -= 0.03 * (q[c] - anchor[c]);
|
||
q[c] = Math.max(t.lo, Math.min(t.hi, q[c] + stp));
|
||
}
|
||
}
|
||
};
|
||
/* 零空间抛光：在位置与姿态偏差都不增的前提下令 Σ|θ| 最小（步进、步长逐轮衰减） */
|
||
const slackOf = q => { const e = errOf(q); return [Math.hypot(e[0], e[1], e[2]), Math.hypot(e[3], e[4], e[5]) / OW]; };
|
||
const polish = (q, slack) => {
|
||
if (!q) return;
|
||
const [d0, o0] = slackOf(q);
|
||
let step = 8;
|
||
for (let pass = 0; pass < 4; pass++) {
|
||
let moved = true, guard = 0;
|
||
while (moved && guard++ < 40) {
|
||
moved = false;
|
||
for (let i = 0; i < 5; i++) { // J6 是纯滚转，抛它只会劣化姿态偏差
|
||
const t = LIM[ORDER[i]];
|
||
for (const s of [1, -1]) {
|
||
const v = q[i] + s * step;
|
||
if (v < t.lo || v > t.hi) continue;
|
||
const old = q[i], c0 = costOf(q, null);
|
||
q[i] = v;
|
||
const [d, o] = slackOf(q);
|
||
if (d <= d0 + slack && o <= o0 + 0.05 && costOf(q, null) < c0 - 1e-9) moved = true;
|
||
else q[i] = old;
|
||
}
|
||
}
|
||
}
|
||
step *= 0.25;
|
||
}
|
||
};
|
||
let anchorQ = null, anchorE = Infinity;
|
||
if (ref && ref.length === 6) {
|
||
anchorQ = ref.slice();
|
||
run(anchorQ, ref);
|
||
anchorE = norm6(errOf(anchorQ));
|
||
}
|
||
run([0, 0, 0, 0, 0, 0]);
|
||
let seed = 0x5bd1e995;
|
||
const rnd = () => { seed = (seed * 1103515245 + 12345) & 0x7fffffff; return seed / 0x7fffffff; };
|
||
const N = tries == null ? 160 : tries;
|
||
for (let k = 0; k < N; k++) {
|
||
const q = [0, 0, 0, 0, 0, 0];
|
||
for (let c = 0; c < 5; c++) { const t = LIM[ORDER[c]]; q[c] = t.lo + rnd() * (t.hi - t.lo); }
|
||
run(q);
|
||
}
|
||
if (anchorQ && anchorE <= bestE + 0.5) { bestQ = anchorQ; bestE = anchorE; }
|
||
if (bestQ) {
|
||
polish(bestQ, 0.05); // 位置 0.05mm / 姿态 0.05° 内不劣化
|
||
bestE = norm6(errOf(bestQ));
|
||
bestOri = oriOf(bestQ);
|
||
}
|
||
return { q: bestQ, dist: Math.hypot.apply(null, bestQ ? fk(bestQ, tool).map((v, i) => v - target[i]) : [1e9, 1e9, 1e9]), ori: bestOri };
|
||
}
|
||
|
||
/* ---------- 5c. 姿态优先的最近解：姿态必须精确，位置取最近 ----------
|
||
位置精确解集是 (θ4,θ5) 平面上的 2 维族；姿态精确（工具轴对齐 + θ6 闭式补滚转）解集
|
||
是 3 维族——两者不是同一集合，姿态优先必须离开位置精确族去搜索。本机构有降阶结构
|
||
（J4∥J5 ⇒ R4·R5 = R_y(θ4+θ5)，与 axisCol 同一约定）：
|
||
|
||
工具轴 w = Rx(θ1)·Ry(θ2)·Rz(−θ3)·Ry(s)·ẑ , s = θ4 + θ5
|
||
|
||
给定 (θ3, s) 后 (θ1,θ2) 可闭式解出（至多两支，见 axisSolve），剩余自由参数只有 θ4
|
||
（θ5 = s − θ4）⇒ 在 (θ3, s, θ4) 三维上粗扫取种子、再坐标下降精修即可。轴一旦精确对齐，
|
||
由 rollClose 定出的 θ6 令 oerr 恒为 0，不再有姿态残差；位置误差由 fk 如实给出。 */
|
||
function closestOri(target, Rdes, tool, ref) {
|
||
if (!SAMEAX) return null; // J4 与 J5 不同轴：降阶结构不成立，交回 DLS
|
||
const t = [Rdes[2], Rdes[5], Rdes[8]];
|
||
const L4 = LIM.J4, L5 = LIM.J5, L2 = LIM.J2;
|
||
const sLo = L4.lo + L5.lo, sHi = L4.hi + L5.hi;
|
||
/* 解 Rx(θ1)·Ry(θ2)·v = t（v = Rz(−θ3)·Ry(s)·ẑ），返回至多两支 [θ1,θ2]（度） */
|
||
const axisSolve = (th3, s) => {
|
||
const v = mv3(rotAxis(D.J3, th3 * D2R), mv3(rotAxis(D.J4, s * D2R), [0, 0, 1]));
|
||
const Rv = Math.hypot(v[0], v[2]);
|
||
if (Rv < 1e-12) return [];
|
||
const cs = t[0] / Rv;
|
||
if (cs < -1 || cs > 1) return [];
|
||
const psi = Math.atan2(v[2], v[0]), d2 = Math.acos(cs), out = [];
|
||
for (let k = 0; k < 2; k++) {
|
||
const th2 = psi + (k ? -d2 : d2);
|
||
const c2 = Math.cos(th2), s2 = Math.sin(th2);
|
||
const P = s2 * v[0] - c2 * v[2], Q = v[1], det = P * P + Q * Q;
|
||
if (det < 1e-14) continue; // 工具轴落在 ±X：θ1 退化，交给别的种子
|
||
if (th2 < L2.lo - 1e-9 || th2 > L2.hi + 1e-9) continue;
|
||
const th1 = Math.atan2((P * t[1] + Q * t[2]) / det, (Q * t[1] - P * t[2]) / det);
|
||
out.push([wrap180(th1 * R2D), wrap180(th2 * R2D)]);
|
||
}
|
||
return out;
|
||
};
|
||
const e2Of = q => {
|
||
const p = fk(q, tool);
|
||
const dx = p[0] - target[0], dy = p[1] - target[1], dz = p[2] - target[2];
|
||
return dx * dx + dy * dy + dz * dz;
|
||
};
|
||
/* 给定 (θ3, s, θ4) 组装关节角：θ1..θ3 闭式、θ5 = s − θ4、θ4 夹进可行区间 */
|
||
const qOf = (th3, s, th4, prev) => {
|
||
const br = axisSolve(th3, s);
|
||
if (!br.length) return null;
|
||
let b = br[0], bd = Infinity;
|
||
for (const x of br) { // 同支匹配：与上一步的 θ1/θ2 最近
|
||
const d = Math.abs(wrap180(x[0] - prev[0])) + Math.abs(wrap180(x[1] - prev[1]));
|
||
if (d < bd) { bd = d; b = x; }
|
||
}
|
||
const a4 = Math.max(L4.lo, s - L5.hi), b4 = Math.min(L4.hi, s - L5.lo);
|
||
if (a4 > b4 + 1e-9) return null;
|
||
const th4c = Math.max(a4, Math.min(b4, th4));
|
||
return [b[0], b[1], wrap180(th3), th4c, s - th4c, 0];
|
||
};
|
||
/* 粗扫 (θ3, s, θ4)：只保留每格最优，再按位置误差取空间去同的若干种子 */
|
||
const cand = [];
|
||
for (let th3 = -180; th3 <= 180 + 1e-9; th3 += 5) {
|
||
for (let s = sLo; s <= sHi + 1e-9; s += 5) {
|
||
const sf = Math.min(s, sHi);
|
||
const br = axisSolve(th3, sf);
|
||
if (!br.length) continue;
|
||
const a4 = Math.max(L4.lo, sf - L5.hi), b4 = Math.min(L4.hi, sf - L5.lo);
|
||
if (a4 > b4 + 1e-9) continue;
|
||
for (const b of br) {
|
||
let bc = null;
|
||
for (let th4 = a4; th4 <= b4 + 1e-9; th4 += 5) {
|
||
const th4c = Math.min(th4, b4);
|
||
const q = [b[0], b[1], wrap180(th3), th4c, sf - th4c, 0];
|
||
const e2 = e2Of(q);
|
||
if (!bc || e2 < bc.d2) bc = { q: q, d2: e2 };
|
||
}
|
||
if (bc) cand.push(bc);
|
||
}
|
||
}
|
||
}
|
||
if (!cand.length) return null;
|
||
cand.sort((x, y) => x.d2 - y.d2);
|
||
const seeds = [];
|
||
for (const c of cand) {
|
||
if (seeds.length >= 32) break;
|
||
let dup = false;
|
||
for (const sd of seeds)
|
||
if (Math.hypot(wrap180(sd.q[2] - c.q[2]), sd.q[3] - c.q[3], sd.q[4] - c.q[4]) < 15) { dup = true; break; }
|
||
if (!dup) seeds.push(c);
|
||
}
|
||
/* 坐标下降精修（θ3 / s / θ4 三方向，步长减半直到收敛） */
|
||
const refine = q0 => {
|
||
let q = q0.slice(), step = 5;
|
||
for (let pass = 0; pass < 60; pass++) {
|
||
let moved = false;
|
||
for (let i = 0; i < 3; i++) {
|
||
for (const sg of [1, -1]) {
|
||
const th3 = i === 0 ? q[2] + sg * step : q[2];
|
||
const s = (i === 1 ? q[3] + q[4] + sg * step : q[3] + q[4]);
|
||
if (s < sLo - 1e-9 || s > sHi + 1e-9) continue;
|
||
const th4 = i === 2 ? q[3] + sg * step : q[3];
|
||
const nq = qOf(th3, s, th4, q);
|
||
if (nq && e2Of(nq) < e2Of(q) - 1e-12) { q = nq; moved = true; }
|
||
}
|
||
}
|
||
if (!moved) { step *= 0.5; if (step < 1e-4) break; }
|
||
}
|
||
return q;
|
||
};
|
||
let bestQ = null, bestE = Infinity;
|
||
const pool = []; // 精修结果集：用于带 ref 的连续性仲裁
|
||
for (const sd of seeds) {
|
||
const q = refine(sd.q), e2 = e2Of(q);
|
||
pool.push({ q: q, d2: e2 });
|
||
if (e2 < bestE) { bestE = e2; bestQ = q; }
|
||
}
|
||
if (!bestQ) return null;
|
||
if (ref && ref.length === 6) {
|
||
/* 与「最近可达位姿」同一约定：位置差在 0.5 mm 内时优先保连续性，避免等价支间来回跳 */
|
||
let bd = Infinity, bq = null;
|
||
for (const p of pool) {
|
||
if (p.d2 > bestE + 1.0) continue; // d2 差 1.0 ≈ 距离差 0.5mm 量级
|
||
let d = 0;
|
||
for (let i = 0; i < 6; i++) d += Math.abs(wrap180(p.q[i] - ref[i]));
|
||
if (d < bd) { bd = d; bq = p.q; }
|
||
}
|
||
if (bq) bestQ = bq;
|
||
}
|
||
const rc = rollClose(bestQ, Rdes);
|
||
return { q: rc.q, dist: Math.sqrt(e2Of(bestQ)), ori: rc.oerr };
|
||
}
|
||
|
||
/* ---------- 5d. 混合优先：位置与姿态按可调权重折中 ----------
|
||
滑杆 w ∈ [0,1] 自由分配两端的比重：
|
||
w = 1 与 w = 0 由 solveTo 直接交给两条既有专用算法（位置优先 / 姿态优先），
|
||
结果逐位一致；本段只负责 0 < w < 1 的加权折中。
|
||
|
||
折中语义 =「沿折中历程的进度」，代价与归一化尺度都取两端锚点自身：
|
||
J(q) = w·(e/e_ref)² + (1−w)·(o/o_ref)²
|
||
e_ref = 姿态端锚点的位置差（姿态优先解的 TCP 位置差），
|
||
o_ref = 位置端锚点的姿态残差（位置优先解的姿态残差）。
|
||
两端锚点各把「本端最差方向」定标，于是滑杆读数与目标的绝对误差量级无关
|
||
（50% 即两端误差各自归一后对半）。两端锚点复用两条既有算法：
|
||
位置端 = solve(priority:'pos')（位置精确族 + 闭式滚转 + 孤立根收口；位置不可达
|
||
时退化为 closest() 的最近可达位置），姿态端 = closestOri()（工具轴闭式对齐 +
|
||
θ6 补滚转 ⇒ 姿态精确、位置最近）。
|
||
|
||
中间值以两锚点及关节空间插值为种子，对 6 维残差
|
||
[√w·(p−t)/e_ref , √(1−w)·RV°/o_ref] 做阻尼高斯-牛顿 + 单轴抛光，取 J 最小者
|
||
（J 同量级带内取相对当前姿态的行程 Σ|wrap(θ−ref)| 更小者）。两端任一维已精确（≤0.05）时
|
||
折中无意义，直接返回位置端解。全程无随机数：同一输入逐位可复现。 */
|
||
let mixScale = null; // 最近一次全量折中的归一化尺度（拖动快路沿用）
|
||
const mixClamp = v => Math.max(0, Math.min(1, v == null || !isFinite(+v) ? 0.5 : +v));
|
||
|
||
/* 折中共用内核：归一化代价 + 加权阻尼高斯-牛顿（§5d 全量与快路共用） */
|
||
function mixKernel(target, tool, Rtar, w, eRef, oRef) {
|
||
const kp = Math.sqrt(w) / eRef, ko = Math.sqrt(1 - w) / oRef;
|
||
const RS = 100; // 残差整体放大：量级与 §5b 的 mm 制对齐，阻尼比例可沿用
|
||
const lam = 6.0, dp = 0.05;
|
||
let samples = 0;
|
||
/* 闭式滚转（θ6）后的真实 (e mm, o 度) 与归一化代价 J */
|
||
const canon = q => {
|
||
const rc = rollClose(q, Rtar), p = fk(rc.q, tool);
|
||
const e = Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]);
|
||
samples++;
|
||
return { q: rc.q, e: e, o: rc.oerr, J: w * (e / eRef) ** 2 + (1 - w) * (rc.oerr / oRef) ** 2 };
|
||
};
|
||
/* 6 维残差（RS 倍放大）：位置 3 + 姿态旋转矢量 3（度制，范数 = 姿态夹角） */
|
||
const resid6 = q => {
|
||
const p = fk(q, tool), N = mul3(T3(orient(q)), Rtar);
|
||
const ang = Math.acos(clamp1((N[0] + N[4] + N[8] - 1) / 2));
|
||
const s = Math.sin(ang), k = s > 1e-8 ? ang / (2 * s) : 0.5; // 旋转矢量 = vee·k（弧度）
|
||
samples++;
|
||
return [(p[0] - target[0]) * kp * RS, (p[1] - target[1]) * kp * RS, (p[2] - target[2]) * kp * RS,
|
||
(N[7] - N[5]) * k * R2D * ko * RS, (N[2] - N[6]) * k * R2D * ko * RS, (N[3] - N[1]) * k * R2D * ko * RS];
|
||
};
|
||
/* 阻尼高斯-牛顿（与 §5b 同构：A = J·Jᵀ + λ²I，y = A⁻¹(−e)，dq = Jᵀy，步长 ≤12°） */
|
||
/* 6×6 线性方程组：高斯消去 + 列主元（A = J·Jᵀ + λ²I，y = A⁻¹e，dq = Jᵀy） */
|
||
const solveLin = (A, b, n) => {
|
||
const M = A.slice(), x = b.slice();
|
||
for (let c = 0; c < n; c++) {
|
||
let p = c;
|
||
for (let r = c + 1; r < n; r++) if (Math.abs(M[r * n + c]) > Math.abs(M[p * n + c])) p = r;
|
||
if (Math.abs(M[p * n + c]) < 1e-14) return null;
|
||
if (p !== c) { for (let j = 0; j < n; j++) { const t = M[c * n + j]; M[c * n + j] = M[p * n + j]; M[p * n + j] = t; } const t = x[c]; x[c] = x[p]; x[p] = t; }
|
||
for (let r = 0; r < n; r++) {
|
||
if (r === c) continue;
|
||
const f = M[r * n + c] / M[c * n + c];
|
||
if (!f) continue;
|
||
for (let j = c; j < n; j++) M[r * n + j] -= f * M[c * n + j];
|
||
x[r] -= f * x[c];
|
||
}
|
||
}
|
||
for (let c = 0; c < n; c++) x[c] /= M[c * n + c];
|
||
return x;
|
||
};
|
||
const dls = (seed, anchor) => {
|
||
const q = seed.slice(), Jm = new Float64Array(36);
|
||
let best = null;
|
||
for (let it = 0; it < 160; it++) {
|
||
const e = resid6(q);
|
||
let n2 = 0; for (let i = 0; i < 6; i++) n2 += e[i] * e[i];
|
||
if (!best || n2 < best.n2) best = { q: q.slice(), n2: n2 };
|
||
if (n2 < 1e-10) break;
|
||
for (let c = 0; c < 6; c++) {
|
||
const s = q.slice(); s[c] += dp;
|
||
const ep = resid6(s);
|
||
for (let r = 0; r < 6; r++) Jm[r * 6 + c] = (ep[r] - e[r]) / dp;
|
||
}
|
||
const A = new Array(36).fill(0);
|
||
for (let i = 0; i < 6; i++) {
|
||
for (let j = 0; j < 6; j++) { let s = 0; for (let k = 0; k < 6; k++) s += Jm[i * 6 + k] * Jm[j * 6 + k]; A[i * 6 + j] = s + (i === j ? lam * lam : 0); }
|
||
}
|
||
const y = solveLin(A, e.map(v => -v), 6);
|
||
if (!y) break;
|
||
const dq = new Array(6); let mx = 0, moved = 0;
|
||
for (let c = 0; c < 6; c++) { let s = 0; for (let r = 0; r < 6; r++) s += Jm[r * 6 + c] * y[r]; dq[c] = s; mx = Math.max(mx, Math.abs(s)); }
|
||
const gain = mx > 12 ? 12 / mx : 1;
|
||
for (let c = 0; c < 6; c++) {
|
||
const t = LIM[ORDER[c]];
|
||
let stp = dq[c] * gain;
|
||
if (anchor) stp -= 0.03 * (q[c] - anchor[c]); // 参考位姿锚定：退化方向不漂移
|
||
const v = Math.max(t.lo, Math.min(t.hi, q[c] + stp));
|
||
moved += Math.abs(v - q[c]);
|
||
q[c] = v;
|
||
}
|
||
if (moved < 1e-9) break;
|
||
}
|
||
return best ? best.q : null;
|
||
};
|
||
/* 单轴抛光：J 不再下降为止，步长逐轮减半（θ6 由 rollClose 闭式定，不参与抛光） */
|
||
const polish = q0 => {
|
||
let cur = canon(q0), q = cur.q, step = 1.5;
|
||
for (let pass = 0; pass < 24 && step > 1e-3; pass++) {
|
||
let moved = false;
|
||
for (let c = 0; c < 5; c++) {
|
||
const t = LIM[ORDER[c]];
|
||
for (const sg of [1, -1]) {
|
||
const v = q[c] + sg * step;
|
||
if (v < t.lo || v > t.hi) continue;
|
||
const qq = q.slice(); qq[c] = v;
|
||
const cc = canon(qq);
|
||
if (cc.J < cur.J - 1e-12) { cur = cc; q = cc.q; moved = true; }
|
||
}
|
||
}
|
||
if (!moved) step *= 0.5;
|
||
}
|
||
return cur;
|
||
};
|
||
return { canon: canon, dls: dls, polish: polish, nSamples: () => samples };
|
||
}
|
||
|
||
function mixSolve(target, opt) {
|
||
opt = opt || {};
|
||
if (!opt.pose || !opt.pose.R || opt.pose.R.length !== 9) return solve(target, opt); // 无姿态任务：退回纯位置求解
|
||
const t0 = nowMs();
|
||
const tool = +opt.tool || 0;
|
||
const W = opt.weights || [1, 1, 1, 1, 1, 1];
|
||
const REF = (opt.ref && opt.ref.length === 6) ? opt.ref.slice() : null;
|
||
const LAM = +opt.refW || 0;
|
||
const Rtar = opt.pose.R;
|
||
const w = mixClamp(opt.mixW);
|
||
const EXACT = 0.05; // 两端任一维已 ≤0.05 ⇒ 视为已精确到达（同 POSEFINE）
|
||
const scaleOf = (e, o) => { mixScale = { e: Math.max(e, 0.05), o: Math.max(o, 0.05) }; };
|
||
/* 位置端锚点：既有位置优先解（位置精确族内姿态残差最小者，含 §4.5 最近姿态搜索） */
|
||
const pa = solve(target, Object.assign({}, opt, { priority: 'pos' }));
|
||
let pEnd = null;
|
||
if (pa.ok && pa.q) pEnd = { q: pa.q.slice(), e: pa.err || 0, o: pa.oerr || 0 };
|
||
/* 姿态端锚点：姿态精确族的最小位置差（工具轴闭式对齐 + θ6 补滚转） */
|
||
const oa = closestOri(target, Rtar, tool, REF);
|
||
let oEnd = null;
|
||
if (oa && oa.q) {
|
||
const rc = rollClose(oa.q, Rtar), p = fk(rc.q, tool);
|
||
oEnd = { q: rc.q, e: Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]), o: rc.oerr };
|
||
}
|
||
const stamp = (res, pn, on) => Object.assign({}, res, {
|
||
priority: 'mix', mixW: w,
|
||
mixP: pn ? { e: pn.e, o: pn.o } : null, mixO: on ? { e: on.e, o: on.o } : null });
|
||
/* 两端中任一端在另一维也达标 ⇒ 已同时精确，折中无意义：返回该端（标准解）。
|
||
先判位置端：位置优先解本身就最优；位置端不达标而姿态端在位置上也达标时，
|
||
姿态端在「位置 + 姿态」两维同时占优（位置优先族搜不到的到达解，姿态端找到了），
|
||
此时必须返回姿态端，不能返回被占优的位置端解。 */
|
||
if (pEnd && pEnd.o <= EXACT) { scaleOf(oEnd ? oEnd.e : 1, pEnd.o); return stamp(pa, pEnd, oEnd); }
|
||
if (oEnd && oEnd.e <= EXACT) {
|
||
scaleOf(oEnd.e, pEnd ? pEnd.o : 1);
|
||
const cost = costOf(oEnd.q, W);
|
||
return stamp({ ok: true, reachable: true, target: target.slice(), tool: tool, pose: true,
|
||
q: oEnd.q.slice(), cost: cost, err: oEnd.e, oerr: oEnd.o,
|
||
nSol: 1, samples: 0, ms: nowMs() - t0,
|
||
list: [{ q: oEnd.q.slice(), cost: cost, rc0: cost, rcost: cost, err: oEnd.e, oerr: oEnd.o }] },
|
||
pEnd, oEnd);
|
||
}
|
||
/* 位置端：位置不可达时退化为最近可达位置（closest），姿态残差如实取 */
|
||
if (!pEnd) {
|
||
const cp = closest(target, tool, opt.tries, REF);
|
||
if (!cp.q) return pa; // 连最近可达位置都求不出：交回原结果（异常路径）
|
||
const rc = rollClose(cp.q, Rtar), p = fk(rc.q, tool);
|
||
pEnd = { q: rc.q, e: Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]), o: rc.oerr };
|
||
}
|
||
if (!oEnd) { // 姿态端锚点缺失（结构前提不满足，或限位内无姿态精确解）：
|
||
/* 按纯姿态优先的同一兜底取 6 维联合 DLS 最近位姿当姿态端，保证兜底语义一致 */
|
||
const c6 = closest6(target, Rtar, tool, opt.tries, REF);
|
||
if (c6 && c6.q) {
|
||
const rc = rollClose(c6.q, Rtar), p = fk(rc.q, tool);
|
||
oEnd = { q: rc.q, e: Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]), o: rc.oerr };
|
||
} else {
|
||
scaleOf(pEnd.e, pEnd.o);
|
||
return stamp(pa, pEnd, null);
|
||
}
|
||
}
|
||
const eRef = Math.max(oEnd.e, 0.01), oRef = Math.max(pEnd.o, 0.01);
|
||
scaleOf(eRef, oRef);
|
||
const K = mixKernel(target, tool, Rtar, w, eRef, oRef);
|
||
/* 种子：两端锚点 + 关节空间插值（落在两端之间的盆地里）+ 参考位姿 */
|
||
const seeds = [pEnd.q, oEnd.q];
|
||
for (const t of [0.25, 0.5, 0.75]) {
|
||
const q = pEnd.q.slice();
|
||
for (let i = 0; i < 6; i++) {
|
||
const tl = LIM[ORDER[i]];
|
||
q[i] = Math.max(tl.lo, Math.min(tl.hi, q[i] + wrap180(oEnd.q[i] - q[i]) * t));
|
||
}
|
||
seeds.push(q);
|
||
}
|
||
if (REF) seeds.push(REF);
|
||
const cands = [K.canon(pEnd.q), K.canon(oEnd.q)];
|
||
for (const sd of seeds) {
|
||
const r = K.dls(sd, REF);
|
||
if (r) cands.push(K.polish(r));
|
||
}
|
||
/* 选优：J 最小；J 同量级（2% 带内）取行程（相对当前姿态的 Σ|wrap(θ−ref)|）更小者，防等价支横跳 */
|
||
cands.sort((x, y) => x.J - y.J);
|
||
const j0 = cands[0].J, jBand = j0 + 0.02 * j0 + 1e-6;
|
||
let best = cands[0], bc = Infinity;
|
||
for (const c of cands) {
|
||
if (c.J > jBand) break;
|
||
const cst = REF ? costOf(c.q, W, REF, LAM) : costOf(c.q, W);
|
||
if (cst < bc - 1e-12) { bc = cst; best = c; }
|
||
}
|
||
const cost = costOf(best.q, W);
|
||
/* 候选表（供动态限位/碰撞改选）：与最优 J 同量级者，按行程（有 ref）升序 */
|
||
const list = [];
|
||
for (const c of cands) {
|
||
if (c !== best && c.J > j0 * 1.5 + 0.05) continue;
|
||
const cst = costOf(c.q, W);
|
||
const rc = REF ? costOf(c.q, W, REF, LAM) : cst;
|
||
list.push({ q: c.q.slice(), cost: cst, rc0: rc, rcost: rc, err: c.e, oerr: c.o });
|
||
}
|
||
list.sort((x, y) => x.rcost - y.rcost);
|
||
return {
|
||
ok: true, reachable: !!pa.ok, target: target.slice(), tool: tool, pose: true,
|
||
priority: 'mix', mixW: w,
|
||
mixP: { e: pEnd.e, o: pEnd.o }, mixO: { e: oEnd.e, o: oEnd.o },
|
||
q: best.q.slice(), cost: cost, err: best.e, oerr: best.o,
|
||
resid: best.e > EXACT ? best.e : null,
|
||
nSol: list.length, truncated: false, samples: K.nSamples(), ms: nowMs() - t0, list: list,
|
||
note: pa.ok ? null
|
||
: '混合优先（位置 ' + Math.round(w * 100) + '%）：目标位姿超可达空间 → 两端锚点折中最优（位置差 ' +
|
||
best.e.toFixed(2) + ' mm · 姿态差 ' + best.o.toFixed(2) + '°）'
|
||
};
|
||
}
|
||
|
||
/* 混合优先的拖动快路：沿上一帧折中解做一次加权 DLS + 抛光，归一化尺度沿用最近一次
|
||
全量折中的缓存（目标微动下两端误差量级不变）；位移超限（换盆地）返回 null 交回全量。
|
||
与 §6a 同一哲学：快路只负责「能快的时候快」，松手后的全量求解才是最终结果。 */
|
||
function mixWarm(target, opt) {
|
||
const q0 = opt.warm;
|
||
if (!q0 || q0.length !== 6 || !mixScale) return null;
|
||
const t0 = nowMs();
|
||
const tool = +opt.tool || 0;
|
||
const W = opt.weights || [1, 1, 1, 1, 1, 1];
|
||
const REF = (opt.ref && opt.ref.length === 6) ? opt.ref : null;
|
||
const w = mixClamp(opt.mixW);
|
||
const K = mixKernel(target, tool, opt.pose.R, w, mixScale.e, mixScale.o);
|
||
const q1 = K.polish(K.dls(q0, REF) || q0);
|
||
let dsh = 0;
|
||
for (let i = 0; i < 5; i++) dsh += Math.abs(wrap180(q1.q[i] - q0[i]));
|
||
if (dsh > 25) return null; // 同支位移过大：可能已换盆地，交回全量重算
|
||
const cost = costOf(q1.q, W);
|
||
return {
|
||
ok: true, reachable: true, warm: true, target: target.slice(), tool: tool, pose: true,
|
||
priority: 'mix', mixW: w, q: q1.q.slice(), cost: cost, err: q1.e, oerr: q1.o,
|
||
nSol: 1, truncated: false, samples: K.nSamples(), ms: nowMs() - t0,
|
||
list: [{ q: q1.q.slice(), cost: cost, rc0: cost, rcost: cost, err: q1.e, oerr: q1.o }]
|
||
};
|
||
}
|
||
|
||
/* ---------- 6. 一键求解（不可达时返回最近可达位姿/位姿） ---------- */
|
||
function solveTo(target, opt) {
|
||
opt = opt || {};
|
||
/* 混合优先（§5d）：两端（0% / 100%）直接交给两条既有专用算法——结果与「姿态优先 /
|
||
位置优先」逐位一致；中间值走加权折中搜索（拖动优先快路，校验不过回落全量）。 */
|
||
if (opt.pose && opt.pose.R && opt.pose.R.length === 9 && opt.priority === 'mix') {
|
||
const w = mixClamp(opt.mixW);
|
||
if (w >= 1 - 1e-9 || w <= 1e-9)
|
||
return solveTo(target, Object.assign({}, opt, { priority: w >= 0.5 ? 'pos' : 'ori' }));
|
||
const m = (opt.warm ? mixWarm(target, opt) : null) || mixSolve(target, opt);
|
||
if (m) return m;
|
||
}
|
||
if (opt.warm) { // 快路优先，校验不过自动回落全量
|
||
const w = solveWarm(target, opt);
|
||
if (w) return w;
|
||
}
|
||
const res = solve(target, opt);
|
||
if (res.ok) return res;
|
||
const pose = opt.pose && opt.pose.R && opt.pose.R.length === 9;
|
||
/* 姿态优先：目标点够不到指定姿态时，不用 6 维 DLS 折中位置与姿态，而是在
|
||
「姿态精确解集」上最小化位置误差——姿态保持精确、位置差如实上报。 */
|
||
if (pose && opt.priority === 'ori') {
|
||
const ro = closestOri(target, opt.pose.R, +opt.tool || 0, opt.ref);
|
||
if (ro && ro.q) return {
|
||
ok: false, reachable: false, target: target.slice(), tool: +opt.tool || 0,
|
||
pose: true, priority: 'ori',
|
||
q: ro.q.slice(), cost: costOf(ro.q, opt.weights), err: ro.dist, resid: ro.dist,
|
||
oerr: ro.ori, nSol: res.nSol, samples: res.samples, ms: res.ms, list: [],
|
||
note: '姿态优先：姿态精确到达，TCP 取最近可达点'
|
||
};
|
||
}
|
||
const cp = pose ? closest6(target, opt.pose.R, +opt.tool || 0, opt.tries, opt.ref)
|
||
: closest(target, +opt.tool || 0, opt.tries, opt.ref);
|
||
if (!cp.q) return res;
|
||
return {
|
||
ok: false, reachable: false, target: target.slice(), tool: +opt.tool || 0,
|
||
pose: pose, priority: res.priority,
|
||
q: cp.q.slice(), cost: costOf(cp.q, opt.weights), err: cp.dist, resid: cp.dist,
|
||
oerr: pose ? cp.ori : null,
|
||
nSol: res.nSol, samples: res.samples, ms: res.ms, list: [],
|
||
note: pose ? '目标位姿超出可达空间：返回最近位姿（阻尼最小二乘，位置 mm + 姿态联合）'
|
||
: '目标超出可达工作空间：返回最近可达位姿（阻尼最小二乘）'
|
||
};
|
||
}
|
||
|
||
/* ---------- 6a. 实时续解：跟随上一帧的解走，而不是每次重扫全冗余维 ----------
|
||
拖动时目标每帧只挪几毫米、姿态只转一两度，真解在 (θ4,θ5) 平面上连续移动；
|
||
全域粗扫（0.25° 下 6~30 万点）为这点位移付全价，是 100 ms 级延迟的唯一来源。
|
||
这里只在上一帧解附近取同一闭式支、走一小步同支搜索，姿态任务再用 poseTools
|
||
的牛顿把工具轴收口到精确。任一校验不过（换支、越限、位置残差超差、姿态没收口、
|
||
关节跳太大）一律返回 null 交回全量求解——快路只负责"能快的时候快"。 */
|
||
function solveWarm(target, opt) {
|
||
opt = opt || {};
|
||
const q0 = opt.warm;
|
||
if (!q0 || q0.length !== 6) return null;
|
||
const tool = +opt.tool || 0, tol = opt.tol || 1e-6, t0 = nowMs();
|
||
const W = opt.weights || [1, 1, 1, 1, 1, 1];
|
||
const REF = (opt.ref && opt.ref.length === 6) ? opt.ref : null;
|
||
const LAM = +opt.refW || 0;
|
||
const POSE = (opt.pose && opt.pose.R && opt.pose.R.length === 9) ? opt.pose.R.slice() : null;
|
||
const PRIO = (opt.priority === 'pos' || opt.priority === 'ori') ? opt.priority : null;
|
||
const A = target[0] - K.X0, BY = target[1] - K.Y0, BZ = target[2] - K.Z1;
|
||
const R = Math.hypot(BY, BZ), ph = Math.atan2(BZ, BY);
|
||
const PT = POSE ? poseTools(target, tool, POSE) : null;
|
||
const L4 = LIM.J4, L5 = LIM.J5;
|
||
const cl = (v, a, b) => v < a ? a : (v > b ? b : v);
|
||
let best = null, samples = 0;
|
||
/* 递进窗口：目标挪得远时上一帧附近可能已经没有同支解，逐档放宽搜索半径，
|
||
仍找不到才交回全量粗扫（姿态任务的命中率先受这一档影响最大）。 */
|
||
const radii = [Math.max(+(opt.warmRad || 4), 0.5), 14, 40];
|
||
for (const rad of radii) {
|
||
if (best) break;
|
||
for (const d4 of [0, 1, -1, 2, -2, 3, -3, 4, -4].map(v => v * rad / 4)) {
|
||
for (const d5 of [0, 1, -1, 2, -2, 3, -3, 4, -4].map(v => v * rad / 4)) {
|
||
const t4 = cl(q0[3] + d4, L4.lo, L4.hi), t5 = cl(q0[4] + d5, L5.lo, L5.hi);
|
||
const dshCap = rad > 20 ? 60 : 25; // 放宽窗口时允许同支稍大的移动
|
||
const ss = branch(t4, t5, A, R, ph, tool); samples++;
|
||
for (const s0 of ss) {
|
||
let dsh = 0; for (let i = 0; i < 5; i++) dsh += Math.abs(wrap180(s0[i] - q0[i]));
|
||
if (dsh > dshCap) continue; // 换了支/大幅倒摆：不是续解
|
||
let p = fk(s0, tool);
|
||
let e = Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]);
|
||
if (!(e <= tol)) continue; // 否定式：NaN 目标在此落选
|
||
let q = s0, oerr = 0, exact = true;
|
||
if (POSE) {
|
||
/* 精确优先但不强制：拖动中目标姿态每帧只转一两度，同支跟随的姿态残差
|
||
通常在 0.05° 内（松手的全量求解会纠偏），要求必须收到根会让快路频繁
|
||
失效、反而退回 100 ms 级全扫。 */
|
||
const r = PT.newton(s0);
|
||
let qq = null;
|
||
if (r && r.exact) qq = r.q;
|
||
else {
|
||
const e0 = PT.res(s0);
|
||
if (e0 && e0[2] > 0.99999 && Math.hypot(e0[0], e0[1]) < 1e-3) qq = s0;
|
||
/* 位置优先：姿态收不到根也照跟同支位置解（残差如实上报），
|
||
否则拖进「姿态不可达区」后每帧都要回落全量扫描。 */
|
||
else if (PRIO === 'pos') qq = s0;
|
||
}
|
||
if (!qq) continue;
|
||
p = fk(qq, tool);
|
||
e = Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]);
|
||
if (!(e <= tol)) continue;
|
||
const c2 = rollClose(qq, POSE);
|
||
q = c2.q; oerr = c2.oerr;
|
||
exact = !!(r && r.exact && qq === r.q);
|
||
if (PRIO !== 'pos' && !(oerr <= 0.05)) continue;
|
||
}
|
||
const cost = costOf(q, W);
|
||
const rc = REF ? costOf(q, W, REF, LAM) : cost;
|
||
/* 位置优先：主键是姿态残差（轴精确的根 oerr≈0 自然胜出），次键才是 Σ|θ| */
|
||
const better = PRIO === 'pos'
|
||
? (!best || oerr < best.oerr - 1e-9 ||
|
||
(Math.abs(oerr - best.oerr) <= 1e-9 && rc < best.rcost - 1e-12))
|
||
: (!best || (exact && !best.exact) || (exact === best.exact && rc < best.rcost - 1e-12));
|
||
if (better)
|
||
best = { q: q, cost: cost, rc0: REF ? costOf(q, W, REF, LAM) : cost, rcost: rc, err: e, oerr: oerr, exact: exact };
|
||
}
|
||
}
|
||
}
|
||
}
|
||
if (!best) return null;
|
||
return {
|
||
ok: true, reachable: true, warm: true, target: target.slice(), tool: tool, pose: !!POSE,
|
||
priority: PRIO,
|
||
q: best.q.slice(), cost: best.cost, err: best.err, oerr: POSE ? best.oerr : null,
|
||
nSol: samples, truncated: false, samples: samples, ms: nowMs() - t0, list: [best]
|
||
};
|
||
}
|
||
|
||
/* ---------- 6a5. 路径安全路由：端点安全 ≠ 过渡安全 ----------
|
||
查看器的动画是逐轴线性插值（ik-glue.js animTo，连续轴走短弧），所以"移动路径"
|
||
就是关节空间里 from→to 的一条直线段。实测把目标球从臂的一侧横拖到另一侧：
|
||
1300 帧里 870 帧（66.9%）处于穿模状态，主要是 立柱上段×J5 电机模组(189 帧)、
|
||
立柱中段×J6 电机模组(186 帧)——端点解本身也撞，而 live 拖拽走的 solveTo
|
||
一个碰撞判定都没做。这里把"这条直线能不能走"当一等判据：
|
||
1) 候选表按代价升序，取第一个"端点安全 + 直线过渡安全"的解；
|
||
2) 一个都没有 → 沿最优候选方向二分退回最近的可安全抵达位形（跟手但停在障碍前），标 blocked；
|
||
3) 当前位形本身已干涉（滑块被拖越界）→ 退回旧的"只看端点"口径，至少把臂解出来。
|
||
开销：首个候选通常一次就过（1 次端点 + PATH_N-1 次过渡 ≈ 8 次 collideFree）；
|
||
最坏 CAND_MAX 个候选全查 ≈ 60 次，实测在 Worker 里跑，不冻结主线程。 */
|
||
const PATH_N = 9, CAND_MAX = 24, SHORT_DEG = 6, FINE_DEG = 3;
|
||
/* 关节空间步长（连续轴按短弧折算后的最大单轴变化量） */
|
||
function stepDeg(from, to) {
|
||
let m = 0;
|
||
for (let i = 0; i < 6; i++) {
|
||
const d = LIM[ORDER[i]] && LIM[ORDER[i]].cont ? wrap180(to[i] - from[i]) : to[i] - from[i];
|
||
if (Math.abs(d) > m) m = Math.abs(d);
|
||
}
|
||
return m;
|
||
}
|
||
function transitPt(from, to, k) {
|
||
return from.map((v, i) => {
|
||
const d = LIM[ORDER[i]] && LIM[ORDER[i]].cont ? wrap180(to[i] - v) : to[i] - v;
|
||
return v + d * k;
|
||
});
|
||
}
|
||
/* 直线段过渡是否全程无干涉（不含端点：起点由调用侧保证，终点已单独查过）。
|
||
步长 ≤SHORT_DEG° 的跟随帧只查中点：实测拖拽中相邻解的步长多在几度以内，
|
||
9 点全扫会把 live 从 1~44ms 放大到 441ms/步（≈10×），而这么短的线段
|
||
两端安全 + 中点安全已经覆盖了实际风险（穿模体是厘米级，不是度级）。 */
|
||
function pathFree(from, to, n) {
|
||
n = n || PATH_N;
|
||
for (let i = 1; i < n; i++) if (!collideFree(transitPt(from, to, i / n))) return false;
|
||
return true;
|
||
}
|
||
function findSafeTarget(cur, from) {
|
||
if (cur.q && collideFree(cur.q)) return { q: cur.q, res: cur };
|
||
let best = null, bestCost = Infinity;
|
||
const n = Math.min((cur.list || []).length, 120);
|
||
for (let i = 0; i < n; i++) {
|
||
const s = cur.list[i];
|
||
if (s && s.q && collideFree(s.q)) {
|
||
const cst = costOf(s.q, null, from);
|
||
if (cst < bestCost) {
|
||
bestCost = cst;
|
||
best = { q: s.q, res: Object.assign({}, cur, { q: s.q.slice(), cost: s.cost, err: s.err, oerr: s.oerr }) };
|
||
}
|
||
}
|
||
}
|
||
if (best) return best;
|
||
const len = (cur.list || []).length;
|
||
if (len > 120) {
|
||
/* 尾部分层样本（thinKeep 保留的远端盆地代表）：全部过一遍取离起点最近的安全者，
|
||
而不是第一个碰到的（2026-10-17：原先取首个，常落到行程远大的盆地） */
|
||
const stride = Math.max(1, Math.floor((len - 120) / 60));
|
||
for (let i = 120; i < len; i += stride) {
|
||
const s = cur.list[i];
|
||
if (s && s.q && collideFree(s.q)) {
|
||
const cst = costOf(s.q, null, from);
|
||
if (cst < bestCost) { bestCost = cst; best = { q: s.q, res: Object.assign({}, cur, { q: s.q.slice(), cost: s.cost, err: s.err, oerr: s.oerr }) }; }
|
||
}
|
||
}
|
||
if (best) return best;
|
||
}
|
||
/* 姿态任务：精确姿态的解全撞（目标贴着立柱、姿态又朝立柱时常见）→ 位置精确族里取
|
||
姿态残差最小的无干涉解，标 poseRelaxed（界面显示"位置到达·姿态残差 x°"），
|
||
不再判「目标在立柱区内」（2026-10-17 用户报告） */
|
||
const alt = cur.altPose || [];
|
||
if (alt.length && cur.priority !== 'ori') {
|
||
const lim = Math.min(alt.length, 400);
|
||
for (let i = 0; i < lim; i += (i < 40 ? 1 : 4)) {
|
||
const s = alt[i];
|
||
if (s && s.q && collideFree(s.q))
|
||
return { q: s.q, res: Object.assign({}, cur, { q: s.q.slice(), cost: s.cost, err: 0, oerr: s.oerr, poseRelaxed: true }) };
|
||
}
|
||
}
|
||
return null;
|
||
}
|
||
function routeFrom(from, cur, lazy) {
|
||
if (!COL || !cur || !cur.q || !from) return cur;
|
||
if (!collideFree(from)) return cur; // 起点已在干涉里：先把自己拔出来再说
|
||
const seen = new Set(), cand = [];
|
||
const push = q => {
|
||
if (!q || q.length !== 6) return;
|
||
const key = q.map(v => v.toFixed(2)).join(',');
|
||
if (seen.has(key)) return;
|
||
seen.add(key); cand.push(q.slice());
|
||
};
|
||
push(cur.q);
|
||
for (const s of (cur.list || [])) push(s.q);
|
||
// 优先验证与起点 from 关节距离最近的候选解
|
||
cand.sort((a, b) => costOf(a, null, from) - costOf(b, null, from));
|
||
const cands = cand.slice(0, CAND_MAX);
|
||
let fineLeft = 4; // 细验预算：全通过的候选才会走满 n 次探测
|
||
for (const q of cands) {
|
||
if (!collideFree(q)) continue;
|
||
// 快速二分初筛：若中点干涉直接短路（1 次判定否决碰撞射线），节省大半无谓细验开销
|
||
if (!collideFree(transitPt(from, q, 0.5))) continue;
|
||
/* 粗筛（≤13 点）先廉价否掉明显穿模的候选，再用与审计同分辨率的 3° 细验定案 */
|
||
if (!pathFree(from, q, Math.min(13, pathN(from, q, SHORT_DEG)))) continue;
|
||
if (fineLeft <= 0) break; // 预算用尽就不接受没细验过的候选（宁可贴边停，不赌）
|
||
fineLeft--;
|
||
if (pathFree(from, q, pathN(from, q, FINE_DEG))) {
|
||
const same = q.every((v, i) => Math.abs(v - cur.q[i]) < 1e-9);
|
||
return Object.assign({}, cur, { q: q, routed: true, blocked: false, kept: same });
|
||
}
|
||
}
|
||
/* 没有一条直线能干净抵达：沿最优候选（优先端点安全的那个）方向二分退回安全边界，
|
||
让臂贴着障碍停住，而不是穿过去。 */
|
||
/* lazy：调用方接下来要跑规划器，贴边停的回退位形先不算（findSafeTarget + 二分 + 细验
|
||
约占被挡场景 routeFrom 耗时的一半），规划失败时再补算。 */
|
||
if (lazy) return Object.assign({}, cur, { routed: true, blocked: true, lazyBlocked: true });
|
||
const safeT = findSafeTarget(cur, from);
|
||
let bestQ = safeT ? safeT.q : null;
|
||
if (!bestQ) {
|
||
for (const q of cands) if (collideFree(q)) { bestQ = q; break; }
|
||
}
|
||
if (!bestQ) bestQ = cands[0] || cur.q;
|
||
let lo = 0, hi = 1; // lo=起点（已确认安全），hi=整跳（上面已证明不干净）
|
||
/* 先按端点二分找到"射线上的第一个撞点"（便宜：每次 1 个探测），
|
||
再对 from→q 整段做 3° 细验；细验不过就按 0.6 回缩重验。
|
||
早先直接用 9 点粗扫当谓词，等于把 19° 的采样间隔当成安全边界，实测漏过一帧。 */
|
||
while (hi - lo > 0.02) {
|
||
const mid = 0.5 * (lo + hi);
|
||
if (collideFree(transitPt(from, bestQ, mid))) lo = mid; else hi = mid;
|
||
}
|
||
let q = transitPt(from, bestQ, lo);
|
||
for (let t = 0; t < 5 && lo > 0.02 && !pathFree(from, q, pathN(from, q, FINE_DEG)); t++) {
|
||
lo *= 0.6; q = transitPt(from, bestQ, lo);
|
||
}
|
||
return Object.assign({}, cur, { q: q, safeT: safeT, routed: true, blocked: lo < 0.98, kept: false });
|
||
}
|
||
/* 拖拽/松手统一入口：solveTo + 路径路由（opt.from = 当前位形） */
|
||
const FULLIK = { key: null, cur: null, T: null, at: 0 };
|
||
function solveRoute(target, opt) {
|
||
opt = opt || {};
|
||
if (opt.from && !opt.ref) opt.ref = opt.from;
|
||
const rt = opt.quick && RM && !opt.noAround;
|
||
const t0 = nowMs();
|
||
let cur = solveTo(target, opt);
|
||
/* 实时管线：热跟随只给一个"同支续解"，它若撞了（目标那一支被立柱挡住），就得看别的构型——
|
||
补一次全量 IK（同一目标缓存复用：松手后目标不动，不必每帧重解） */
|
||
if (rt && cur && (!cur.q || !collideFree(cur.q))) {
|
||
const key = target.map(v => v.toFixed(2)).join(',') + '|' + (opt.tool || 0) + '|' + (opt.pose ? JSON.stringify(opt.pose) : '');
|
||
const near = FULLIK.T && FULLIK.key && Math.hypot(target[0] - FULLIK.T[0], target[1] - FULLIK.T[1], target[2] - FULLIK.T[2]) < 40
|
||
&& nowMs() - FULLIK.at < 400;
|
||
if (FULLIK.key === key) cur = FULLIK.cur;
|
||
else if (near) cur = Object.assign({}, FULLIK.cur, { approx: true }); // 拖动中：先朝附近目标的安全解走，目标停下再精解（避免每帧全量 IK 尖峰）
|
||
else {
|
||
const o2 = Object.assign({}, opt); delete o2.warm; o2.quick = false;
|
||
/* 只要目标解，不要路由：带着 from 时 solveDyn 会顺带整套 routeOrVia（慢），且返回的 q 可能是
|
||
"贴边停住"的位形——再被缓存成 FULLIK 目标，实时管线就朝停靠点规划（2026-10-17 贴立柱测试发现） */
|
||
delete o2.from;
|
||
/* solveDyn：按动态限位挑"本身无干涉"的解（全量表按代价截断在 4000 条，安全支常排在很后面甚至被截掉） */
|
||
const full = solveDyn(target, o2);
|
||
if (full && full.q) { cur = Object.assign({}, full); FULLIK.key = key; FULLIK.cur = cur; FULLIK.T = target.slice(); FULLIK.at = nowMs(); }
|
||
}
|
||
}
|
||
const t1 = nowMs();
|
||
const r = rt ? routeRT(opt.from, cur) : routeOrVia(opt.from, target, cur, opt);
|
||
if (r && typeof r === 'object') { r.tIk = t1 - t0; r.tRoute = nowMs() - t1; }
|
||
return r;
|
||
}
|
||
/* 两段式绕行：直线被立柱挡住、但目标本身有安全解时，先解一个"抬高版"的中间位形
|
||
（目标正上方 VIA_DZ），走直线过去，再从那里落回目标。实测 100 个拖拽步里
|
||
14 步属于这一类（有安全端点、直线到不了）。只在松手全量求解时用：
|
||
每多一个候选要多解一次 IK + 两段直线检查，实时拖拽扛不住。 */
|
||
const VIA_DZ = [260, 520];
|
||
function routeVia(from, target, opt) {
|
||
for (const dz of VIA_DZ) {
|
||
const Tw = [target[0], target[1], target[2] + dz];
|
||
const rw = solveRoute(Tw, Object.assign({}, opt, { noVia: true, noAround: true }));
|
||
if (!rw || !rw.q || rw.blocked || rw.unsafe || !collideFree(rw.q)) continue;
|
||
const r2 = solveRoute(target, Object.assign({}, opt, { from: rw.q.slice(), noVia: true, noAround: true, ref: rw.q }));
|
||
if (!r2 || !r2.q || r2.blocked || r2.unsafe || !collideFree(r2.q)) continue;
|
||
/* 中间位形表必须把两段各自的子绕行都接上：from→(rw.via)→rw.q→(r2.via)→q。
|
||
早先只放 [rw.q] 会把 rw/r2 自己的 via 覆盖掉，于是动画走的那条线并不是校验过的那条，
|
||
实测松手口径因此在离起点 3° 处穿模 6 帧（立柱中段×末端工具法兰）。 */
|
||
return Object.assign({}, r2, {
|
||
via: (rw.via || []).concat([rw.q.slice()], r2.via || []), detourDz: dz, detour: 'via'
|
||
});
|
||
}
|
||
return null;
|
||
}
|
||
/* 绕行使者：收臂 → 转 J1 → 再伸展。
|
||
CRUISE 表由 scratch/find_cruise_poses.js 搜出并校验：那组 J2/J4/J5 组合在
|
||
J1 整周每 10° 采样下 collideFree 全通过（280 组枚举里 243 组满足，取半径最贴身
|
||
且收法不同的 4 组）。几何一改必须重跑该脚本，否则巡航位形可能已经会撞。
|
||
三段分别是：当前位形→原地收臂（J1 不变）、收臂态把 J1 转到目标解的肩角、
|
||
再伸展到目标解。每段都过 pathFree，段内 J1 按 ≤10° 采样（与搜索口径一致）。 */
|
||
/* 巡航参考位形：按收缩幅度从浅到深排序。
|
||
[0, -15, ...] 为轻微避让，[0, -75, ...] 为深折叠。优先选用微调姿态，避免激进蜷缩。 */
|
||
const CRUISE = [
|
||
[0, -15, 0, -95, -60, 0], [0, -45, 0, -95, -60, 0],
|
||
[0, -75, 0, -95, -60, 0], [0, -90, 0, -65, -60, 0]
|
||
];
|
||
/* 转 J1 那一段按 5° 网格查表（懒算 + 记忆化） */
|
||
const CRUISE_OK = CRUISE.map(() => new Map());
|
||
function cruiseFree(ci, j1) {
|
||
const key = Math.round(j1 / 5) * 5;
|
||
const m = CRUISE_OK[ci];
|
||
let v = m.get(key);
|
||
if (v === undefined) { const q = CRUISE[ci].slice(); q[0] = key; v = collideFree(q); m.set(key, v); }
|
||
return v;
|
||
}
|
||
function turnFree(ci, j1a, j1b) {
|
||
const n = Math.max(1, Math.ceil(Math.abs(wrap180(j1b - j1a)) / 5));
|
||
for (let i = 0; i <= n; i++) if (!cruiseFree(ci, j1a + (wrap180(j1b - j1a)) * i / n)) return false;
|
||
return true;
|
||
}
|
||
/* 采样点数按线段在关节空间的步长自适应：每 SHORT_DEG(6°) 一个点，最少 3 点、最多 40 点 */
|
||
function pathN(from, to, deg) {
|
||
return Math.max(3, Math.min(40, Math.round(stepDeg(from, to) / (deg || SHORT_DEG)) + 1));
|
||
}
|
||
/* 轻量偏移量集合：用于避开障碍物边缘（抬肩/微收肘），避免整臂蜷缩 */
|
||
const GENTLE_OFFSETS = [
|
||
[-10, 0], [-20, 0], [-15, -10], [0, -15], [-20, -15],
|
||
[10, 0], [20, 0], [-30, 0], [-10, 10]
|
||
];
|
||
/* 智能平滑协同避障规划（模拟人类避障直觉）：
|
||
1) 微调避让：针对贴边擦碰，在直线弦中点微调 J2/J4，各轴保持平滑协同，绝不缩臂；
|
||
2) 协同单中间点 + 二分松弛：J1 全程平滑旋转，中间位形只做必要的外展/微收，并向直线插值松弛；
|
||
3) 模拟人类自然避障（回退一点 -> 绕行过去 -> 伸展到位）：
|
||
当手臂靠近立柱或跨度大时，在起点方位先原位/微转收臂回退，跨过立柱后再展开到位；
|
||
4) 保底安全回退：极端奇异位形下确保无碰撞。 */
|
||
function routeAroundPlan(from, cur, quick, safeT) {
|
||
let qT = safeT ? safeT.q : null, resT = safeT ? safeT.res : cur;
|
||
if (!qT) {
|
||
const found = findSafeTarget(cur, from);
|
||
if (found) { qT = found.q; resT = found.res; }
|
||
}
|
||
if (!qT) return null;
|
||
|
||
const chordMid = transitPt(from, qT, 0.5);
|
||
const dj1 = wrap180(qT[0] - from[0]);
|
||
const j1Mid = from[0] + dj1 * 0.5;
|
||
|
||
// 阶段 1：微调避障（针对贴边穿模：直接在弦中点微调 J2/J4，完全不蜷缩手臂）
|
||
for (const [dj2, dj4] of GENTLE_OFFSETS) {
|
||
const testQ = chordMid.slice();
|
||
testQ[1] += dj2; testQ[3] += dj4;
|
||
if (!collideFree(testQ)) continue;
|
||
const nA = Math.min(8, pathN(from, testQ, FINE_DEG));
|
||
const nB = Math.min(8, pathN(testQ, qT, FINE_DEG));
|
||
if (pathFree(from, testQ, nA) && pathFree(testQ, qT, nB)) {
|
||
return { res: resT, via: [testQ], detour: 'gentle' };
|
||
}
|
||
}
|
||
|
||
// 阶段 2：连续协同单中间点避障（J1 保持平滑转动，避障姿态向自然弦二分松弛）
|
||
const use = quick ? [0, 1, 2] : [0, 1, 2, 3];
|
||
for (const ci of use) {
|
||
const c = CRUISE[ci];
|
||
const qMidSafe = c.slice();
|
||
qMidSafe[0] = j1Mid;
|
||
if (!collideFree(qMidSafe)) continue;
|
||
const nA = Math.min(8, pathN(from, qMidSafe, FINE_DEG));
|
||
const nB = Math.min(8, pathN(qMidSafe, qT, FINE_DEG));
|
||
if (pathFree(from, qMidSafe, nA) && pathFree(qMidSafe, qT, nB)) {
|
||
// 二分松弛：寻找能安全通过的最小偏离量（靠近弦中点）
|
||
let lo = 0, hi = 1;
|
||
for (let it = 0; it < 4; it++) {
|
||
const midA = (lo + hi) * 0.5;
|
||
const testQ = transitPt(chordMid, qMidSafe, midA);
|
||
if (collideFree(testQ) && pathFree(from, testQ, 6) && pathFree(testQ, qT, 6)) {
|
||
hi = midA;
|
||
} else {
|
||
lo = midA;
|
||
}
|
||
}
|
||
const optMid = transitPt(chordMid, qMidSafe, hi);
|
||
return { res: resT, via: [optMid], detour: 'coordinated' };
|
||
}
|
||
}
|
||
|
||
// 阶段 3：模拟人类自然避障（回退一点 -> 绕行过去 -> 伸展到位）
|
||
// 用户需求："模拟人类，哪怕中间有东西挡了，应该回退一点然后再移动过去，而不是说挡着了就不计算了"
|
||
// 第一中间点在起点方位（微转 frac）处收臂回退，第二中间点在避开立柱后跨过，再伸展入目标
|
||
for (const ci of use) {
|
||
const c = CRUISE[ci];
|
||
for (const frac of [0.08, 0.0, 0.15, 0.25]) {
|
||
const w1 = c.slice(); w1[0] = from[0] + dj1 * frac;
|
||
const w2 = c.slice(); w2[0] = qT[0] - dj1 * frac;
|
||
if (!collideFree(w1) || !collideFree(w2)) continue;
|
||
const n1 = Math.min(8, pathN(from, w1, FINE_DEG));
|
||
const n2 = Math.min(8, pathN(w1, w2, FINE_DEG));
|
||
const n3 = Math.min(8, pathN(w2, qT, FINE_DEG));
|
||
if (pathFree(from, w1, n1) && pathFree(w1, w2, n2) && pathFree(w2, qT, n3)) {
|
||
return { res: resT, via: [w1, w2], detour: 'retract_pass' };
|
||
}
|
||
}
|
||
}
|
||
|
||
// 阶段 4：保底三段式（仅在所有平滑协同均被阻挡的极端奇异位形启用）
|
||
for (const ci of use) {
|
||
const c = CRUISE[ci];
|
||
const fold = c.slice(); fold[0] = from[0];
|
||
const turn = c.slice(); turn[0] = qT[0];
|
||
const ok = pathFree(from, fold, Math.min(8, pathN(from, fold, FINE_DEG))) &&
|
||
turnFree(ci, fold[0], turn[0]) &&
|
||
pathFree(turn, qT, Math.min(8, pathN(turn, qT, FINE_DEG)));
|
||
if (ok) return { res: resT, via: [fold, turn], detour: 'around' };
|
||
}
|
||
return null;
|
||
}
|
||
/* ---------- 6c. 采样式避障规划：多目标 RRT-Connect + 捷径平滑（2026-10-15） ----------
|
||
取代原先"微调偏移 → 固定巡航位形 → 回退绕行 → 收臂转 J1"的手工阶梯：
|
||
· 旧阶梯的 CRUISE 位形把 J3/J5/J6 强制拉回固定值（0/-60/0），哪怕它们和障碍无关，
|
||
所以每次绕行都会出现"腕部无故甩一圈、大臂自转归零再转回去"的怪动作；
|
||
· 旧阶梯每段只查 ≤8 个点（长段间隔可达 15°+），且只认一个目标解；
|
||
· 找不到就贴边停（blocked），用户看到的是"卡住/抖动"。
|
||
新做法：
|
||
1) 目标集 = 候选表里所有端点安全的 IK 解（按加权关节距离取前 GOAL_K 个），
|
||
而不是单一 qT —— 换一个肘/腕构型往往就有直线可走；
|
||
2) 双向 RRT-Connect（起点树 ↔ 多根目标树），关节空间按加权距离扩展，
|
||
连续轴（J1/J3/J6）在起点 ±180° 的展开坐标里采样，天然走短弧；
|
||
3) 边检测按 EDGE_RES 度二分顺序（先中点再细分，撞点通常 1~3 次就被否决）；
|
||
4) 捷径平滑：随机两点直连 + 贪心最远可达，去掉 RRT 的锯齿，
|
||
最终路径通常只剩 1~2 个中间位形，且只动"必须动"的轴；
|
||
5) 定种子 LCG：同一起点/目标结果可复现，不会每次松手走不同的路。 */
|
||
const PW = [1.6, 1.4, 1.0, 1.0, 0.7, 0.5]; // 加权：近端轴惯量大、扫掠体积大，动它代价更高
|
||
const EDGE_RES = 1.5, RRT_STEP = 20, GOAL_K = 6;
|
||
function dq(a, b) { // 展开坐标下的差（连续轴取短弧）
|
||
const o = new Array(6);
|
||
for (let i = 0; i < 6; i++) o[i] = LIM[ORDER[i]].cont ? wrap180(b[i] - a[i]) : b[i] - a[i];
|
||
return o;
|
||
}
|
||
function wdist(a, b) { // 加权 L2（最近邻 / 代价）
|
||
let s = 0;
|
||
for (let i = 0; i < 6; i++) { const x = (a[i] - b[i]) * PW[i]; s += x * x; }
|
||
return Math.sqrt(s);
|
||
}
|
||
function pathCost(pts) { let c = 0; for (let i = 1; i < pts.length; i++) c += wdist(pts[i - 1], pts[i]); return c; }
|
||
/* 二分顺序的边检测：端点由调用侧保证；先查中点、再 1/4、3/4……撞点早否决 */
|
||
function edgeFree(a, b, res) {
|
||
res = res || EDGE_RES;
|
||
let m = 0;
|
||
for (let i = 0; i < 6; i++) {
|
||
const d = Math.abs(b[i] - a[i]);
|
||
if (d >= 179 && LIM[ORDER[i]].cont) return false; // 动画按短弧插值：段内连续轴不得 ≥180°
|
||
m = Math.max(m, d);
|
||
}
|
||
const n = Math.max(1, Math.ceil(m / res));
|
||
if (n <= 1) return true;
|
||
if (BADE.size && BADE.has(segKey(a, b))) return false; // 半格复验判过的坏段（pathFine）
|
||
const q = new Array(6), Q = [[0, n]];
|
||
for (let h = 0; h < Q.length; h++) { // 广度优先二分：中点 → 四分点 → …
|
||
const [lo, hi] = Q[h];
|
||
if (hi - lo < 2) continue;
|
||
const k = (lo + hi) >> 1, t = k / n;
|
||
for (let i = 0; i < 6; i++) q[i] = a[i] + (b[i] - a[i]) * t;
|
||
if (!collideFree(q)) return false;
|
||
Q.push([lo, k], [k, hi]);
|
||
}
|
||
return true;
|
||
}
|
||
/* 把 q 展开到离 ref 最近的等价角（连续轴），供规划在连续坐标里工作 */
|
||
function unwrapTo(ref, q) { return q.map((v, i) => LIM[ORDER[i]].cont ? ref[i] + wrap180(v - ref[i]) : v); }
|
||
function inBox(q) {
|
||
for (let i = 0; i < 6; i++) { const t = LIM[ORDER[i]]; if (!t.cont && (q[i] < t.lo - 1e-6 || q[i] > t.hi + 1e-6)) return false; }
|
||
return true;
|
||
}
|
||
function gatherGoals(cur, from, kMax, listMax, cfMax) {
|
||
kMax = kMax || GOAL_K; listMax = listMax || 160; cfMax = cfMax || 1e9;
|
||
const seen = new Set(), out = [];
|
||
const add = q => {
|
||
if (!q || q.length !== 6) return;
|
||
const u = unwrapTo(from, q);
|
||
const key = u.map(v => Math.round(v)).join(',');
|
||
if (seen.has(key)) return;
|
||
seen.add(key); out.push(u);
|
||
};
|
||
add(cur.q);
|
||
const L = cur.list || [];
|
||
for (let i = 0; i < Math.min(L.length, listMax); i++) add(L[i] && L[i].q);
|
||
out.sort((a, b) => wdist(from, a) - wdist(from, b));
|
||
const goals = [], tried = [];
|
||
/* IK 选定的目标解（solveDyn/findSafeTarget 已保证它无干涉）永远先入选：原先它和候选表一起按
|
||
"离起点距离"排队，起点在立柱另一侧时，限量模式（cfMax=30）的判定额度全耗在离起点近、
|
||
贴立柱必撞的盆地上，轮不到它 → goals 为空 → 每帧"停在边界"（2026-10-17 贴立柱测试 ci5） */
|
||
if (cur.q && cur.q.length === 6) {
|
||
const u = unwrapTo(from, cur.q);
|
||
if (collideFree(u)) { goals.push(u); tried.push(u); cfMax--; }
|
||
}
|
||
for (const g of out) {
|
||
if (goals.length >= kMax || cfMax <= 0) break;
|
||
if (goals.some(h => wdist(g, h) < 8)) continue; // 同一盆地只留一个
|
||
if (cfMax < 1e8) { // 限量模式：撞过的盆地也不再重复判定
|
||
if (tried.some(h => wdist(g, h) < 8)) continue;
|
||
tried.push(g);
|
||
}
|
||
cfMax--;
|
||
if (collideFree(g)) goals.push(g);
|
||
}
|
||
return goals;
|
||
}
|
||
function shortcut(pts, deadline, rnd) {
|
||
pts = pts.map(p => p.slice());
|
||
/* 随机捷径：在两段上各取一点直连 */
|
||
for (let it = 0; it < 50 && pts.length > 2 && nowMs() < deadline; it++) {
|
||
const i = Math.floor(rnd() * (pts.length - 1)), j = Math.floor(rnd() * (pts.length - 1));
|
||
if (Math.abs(i - j) < 1) continue;
|
||
const a = Math.min(i, j), b = Math.max(i, j);
|
||
const ta = rnd(), tb = rnd();
|
||
const pa = pts[a].map((v, k) => v + (pts[a + 1][k] - v) * ta);
|
||
const pb = pts[b].map((v, k) => v + (pts[b + 1][k] - v) * tb);
|
||
if (wdist(pa, pb) >= pathCost(pts.slice(a, b + 2).concat()) - 1e-6) continue;
|
||
if (!edgeFree(pa, pb)) continue;
|
||
pts = pts.slice(0, a + 1).concat([pa, pb], pts.slice(b + 1));
|
||
}
|
||
/* 贪心：从当前点直连最远可达点，删掉多余中间位形 */
|
||
const out = [pts[0]];
|
||
let i = 0;
|
||
while (i < pts.length - 1) {
|
||
let j = pts.length - 1;
|
||
while (j > i + 1 && !edgeFree(pts[i], pts[j])) j--;
|
||
out.push(pts[j]); i = j;
|
||
}
|
||
/* 逐轴拉直：中间位形的每个轴若能取"前后两点按段长插值"的值且两侧边仍无碰撞，就拉直它。
|
||
去掉 RRT 留下的"某轴来回摆"（与障碍无关的轴不该动），这正是视觉上"怪动作"的来源。 */
|
||
for (let pass = 0; pass < 1 && out.length > 2; pass++) {
|
||
for (let i = 1; i < out.length - 1 && nowMs() < deadline; i++) {
|
||
const a = out[i - 1], b = out[i + 1];
|
||
const la = wdist(a, out[i]), lb = wdist(out[i], b), t = la / (la + lb || 1);
|
||
for (let k = 0; k < 6; k++) {
|
||
const v = a[k] + (b[k] - a[k]) * t;
|
||
if (Math.abs(v - out[i][k]) < 0.5) continue;
|
||
const q = out[i].slice(); q[k] = v;
|
||
if (collideFree(q) && edgeFree(a, q) && edgeFree(q, b)) out[i] = q;
|
||
}
|
||
}
|
||
}
|
||
return out;
|
||
}
|
||
/* Lazy RRT-Connect：生长时只查新节点本身 + 边中点（2 次判定），连通后再对整条路径
|
||
逐边 2° 细验；不通的边把子节点判死（连同其子树），继续生长。碰撞判定是唯一的大头
|
||
（~3 ms/次），惰性验证把每次迭代从 ~25 ms 降到 ~6 ms。 */
|
||
function rrtConnect(from, goals, budgetMs, seed) {
|
||
const t0 = nowMs(), deadline = t0 + budgetMs;
|
||
let s = seed >>> 0 || 1;
|
||
const rnd = () => { s = (s * 1664525 + 1013904223) >>> 0; return s / 4294967296; };
|
||
const A = [{ q: from.slice(), p: -1, ok: true, dead: false }];
|
||
const B = goals.map(g => ({ q: g.slice(), p: -1, ok: true, dead: false }));
|
||
const alive = (T, i) => { while (i >= 0) { if (T[i].dead) return false; i = T[i].p; } return true; };
|
||
const nearest = (T, q) => {
|
||
let bi = -1, bd = Infinity;
|
||
for (let i = 0; i < T.length; i++) { const d = wdist(T[i].q, q); if (d < bd && alive(T, i)) { bd = d; bi = i; } }
|
||
return bi;
|
||
};
|
||
const steer = (a, b) => {
|
||
let m = 0; for (let i = 0; i < 6; i++) m = Math.max(m, Math.abs(b[i] - a[i]));
|
||
if (m <= RRT_STEP) return b.slice();
|
||
const k = RRT_STEP / m; return a.map((v, i) => v + (b[i] - v) * k);
|
||
};
|
||
const extend = (T, q) => { // 0=trapped 1=advanced 2=reached
|
||
const ni = nearest(T, q);
|
||
if (ni < 0) return [0, -1];
|
||
const qn = steer(T[ni].q, q);
|
||
if (!inBox(qn) || !collideFree(qn)) return [0, -1];
|
||
const mid = T[ni].q.map((v, i) => 0.5 * (v + qn[i]));
|
||
if (!collideFree(mid)) return [0, -1];
|
||
T.push({ q: qn, p: ni, ok: false, dead: false }); // ok=false：边尚未细验
|
||
return [wdist(qn, q) < 1e-6 ? 2 : 1, T.length - 1];
|
||
};
|
||
/* 细验一条树枝（叶→根），返回 true=全通；失败则判死第一条坏边的子端 */
|
||
const verify = (T, i) => {
|
||
while (i >= 0 && T[i].p >= 0) {
|
||
const n = T[i];
|
||
if (!n.ok) {
|
||
if (!edgeFree(T[n.p].q, n.q)) { n.dead = true; return false; }
|
||
n.ok = true;
|
||
}
|
||
i = n.p;
|
||
}
|
||
return true;
|
||
};
|
||
const trace = (T, i) => { const o = []; while (i >= 0) { o.push(T[i].q); i = T[i].p; } return o; };
|
||
let Ta = A, Tb = B, swapped = false, it = 0;
|
||
while (nowMs() < deadline) {
|
||
it++;
|
||
let qr;
|
||
if (rnd() < 0.1) qr = Tb[Math.floor(rnd() * Tb.length)].q.slice();
|
||
else qr = ORDER.map((id, i) => { const t = LIM[id]; return t.cont ? from[i] - 180 + 360 * rnd() : t.lo + (t.hi - t.lo) * rnd(); });
|
||
const [st, ia] = extend(Ta, qr);
|
||
if (st) {
|
||
const qa = Ta[ia].q;
|
||
let r = [1, -1];
|
||
while (r[0] === 1) r = extend(Tb, qa);
|
||
if (r[0] === 2) {
|
||
/* 连接点在两棵树里是同一位形：两侧树枝都细验通过才算路径 */
|
||
const okA = verify(Ta, ia), okB = okA && verify(Tb, r[1]);
|
||
if (okA && okB) {
|
||
const pa = trace(Ta, ia), pb = trace(Tb, r[1]);
|
||
let path = pa.reverse().concat(pb.slice(1));
|
||
if (swapped) path = path.reverse();
|
||
return { path: path, iters: it, ms: nowMs() - t0, rnd: rnd };
|
||
}
|
||
}
|
||
}
|
||
const t = Ta; Ta = Tb; Tb = t; swapped = !swapped;
|
||
if (Ta.length + Tb.length > 6000) break;
|
||
}
|
||
return null;
|
||
}
|
||
/* ---------- 6d. 离线路网（PRM）+ 在线 Lazy A*（2026-10-15，实时版） ----------
|
||
障碍全是静态的（立柱 + 自身零件），关节空间的自由区不随时间变化，所以把"安全路网"离线建好：
|
||
webapp/tools/gen_roadmap.cjs 均匀采样无干涉节点，连 k 近邻并按 EDGE_RES 细验每条边，
|
||
随 collide.json 的 roadmap 字段发布（指纹校验，碰撞模型变了自动弃用）。
|
||
在线：起点 / 各目标接到路网的 k 个近邻（这些边"待验"），A* 求最短路，只细验路径上的待验边，
|
||
失败就删边重搜（Lazy PRM）。路网内部的边离线已验，在线零成本。最后限时贪心捷径拉直。 */
|
||
function wd(a, b) { // 展开无关的加权距离（连续轴短弧）
|
||
let s = 0;
|
||
for (let i = 0; i < 6; i++) { const d = LIM[ORDER[i]].cont ? wrap180(b[i] - a[i]) : b[i] - a[i]; const x = d * PW[i]; s += x * x; }
|
||
return Math.sqrt(s);
|
||
}
|
||
function modelFingerprint() {
|
||
let h = 2166136261 >>> 0;
|
||
const mix = v => { h = Math.imul(h ^ (Math.round(v * 1000) | 0), 16777619) >>> 0; };
|
||
mix(CAPM);
|
||
for (const c of CAPG) { mix(c.g); mix(c.r); for (const v of c.a) mix(v); for (const v of c.b) mix(v); }
|
||
for (const id of ORDER) { const t = LIM[id]; mix(t.lo); mix(t.hi); mix(t.cont ? 1 : 0); }
|
||
return h.toString(16);
|
||
}
|
||
const RM = (() => {
|
||
if (!COL || !COL.roadmap) return null;
|
||
const R = COL.roadmap;
|
||
if (R.fp !== modelFingerprint()) { if (typeof console !== 'undefined') console.warn('[IK] roadmap 指纹不符，弃用（重跑 webapp/tools/gen_roadmap.cjs）'); return null; }
|
||
const dec = (b64, Ctor) => {
|
||
if (typeof atob === 'function') { const bin = atob(b64), u8 = new Uint8Array(bin.length); for (let i = 0; i < bin.length; i++) u8[i] = bin.charCodeAt(i); return new Ctor(u8.buffer); }
|
||
const b = Buffer.from(b64, 'base64'); const u8 = new Uint8Array(b.length); u8.set(b); return new Ctor(u8.buffer);
|
||
};
|
||
const qi = dec(R.q, Int16Array), off = dec(R.off, Uint32Array), adj = dec(R.adj, Uint16Array);
|
||
const N = R.n, Q = [];
|
||
for (let i = 0; i < N; i++) { const q = new Array(6); for (let k = 0; k < 6; k++) q[k] = qi[i * 6 + k] / 100; Q.push(q); }
|
||
return { N: N, Q: Q, off: off, adj: adj };
|
||
})();
|
||
/* 会话内"终身"增广：每次在线细验通过的接入边连同起点 / 目标位形一起并入路网（只存在内存里）。
|
||
拖拽时起点和目标集中在一小片区域，几次之后那里路网变密、接入边变短且多半已验，
|
||
后续查询基本只剩 A*（亚毫秒）。上限 RMX_MAX 个节点，满了就不再并入。 */
|
||
const RMX = { Q: [], adj: [] }, RMX_MAX = 4000;
|
||
function rmKnn(q, k) {
|
||
const best = [];
|
||
const M = RM.N + RMX.Q.length;
|
||
for (let i = 0; i < M; i++) {
|
||
const d = wd(q, i < RM.N ? RM.Q[i] : RMX.Q[i - RM.N]);
|
||
if (best.length < k || d < best[best.length - 1][1]) {
|
||
let j = best.length; best.push(null);
|
||
while (j > 0 && best[j - 1][1] > d) { best[j] = best[j - 1]; j--; }
|
||
best[j] = [i, d]; if (best.length > k) best.pop();
|
||
}
|
||
}
|
||
return best;
|
||
}
|
||
const RM_K = 12;
|
||
/* 跨帧坏边表（2026-10-17）：原先 bad 只在单次查询内有效。目标贴着立柱时接入边大多不通，
|
||
40 ms 预算刚好耗在"重新验同一批坏边"上，下一帧又从头验——拖拽松手前永远规划不到，
|
||
实测 30 个贴立柱目标里 12 个一直停在边界。碰撞模型是静态的，坏边可以永久记住；
|
||
节点用位形坐标做键（起点/目标/增广节点的编号每次查询都会变）。 */
|
||
const BADE = new Set();
|
||
const DEAD = new Set(); // 半格复验否决的图边（路网/增广节点编号对）
|
||
function rmIdsAt(q) { // 位形 → 与之重合的路网/增广节点编号
|
||
const ids = [];
|
||
if (!RM) return ids;
|
||
const M = RM.N + RMX.Q.length;
|
||
for (let i = 0; i < M; i++) if (wd(q, i < RM.N ? RM.Q[i] : RMX.Q[i - RM.N]) < 1e-3) ids.push(i);
|
||
return ids;
|
||
}
|
||
const pkey = (i, p) => i < (RM ? RM.N : 0) ? 'r' + i : p.map(v => v.toFixed(2)).join(',');
|
||
/* 返回 { path:[from, ..., goal] (展开坐标) } 或 null */
|
||
function rmPlan(from, goals, budgetMs) {
|
||
if (!RM) return null;
|
||
const t0 = nowMs(), deadline = t0 + budgetMs, N = RM.N, NX = N + RMX.Q.length, S = NX, G0 = NX + 1;
|
||
const pos = i => i < N ? RM.Q[i] : i < NX ? RMX.Q[i - N] : i === S ? from : goals[i - G0];
|
||
const ekey = (a, b) => a < b ? a * 65536 + b : b * 65536 + a;
|
||
const bad = new Set(), good = new Set();
|
||
const extra = new Map(); // 本次查询的待验边：节点 → [[邻, 代价], ...]
|
||
const link = (a, b, c) => { if (!extra.has(a)) extra.set(a, []); if (!extra.has(b)) extra.set(b, []); extra.get(a).push([b, c]); extra.get(b).push([a, c]); };
|
||
for (const [i, d] of rmKnn(from, RM_K)) link(S, i, d);
|
||
goals.forEach((g, j) => {
|
||
for (const [i, d] of rmKnn(g, RM_K)) link(G0 + j, i, d);
|
||
link(S, G0 + j, wd(from, g)); // 直连也是候选：能直走 A* 自然选它
|
||
});
|
||
const isGoal = i => i >= G0;
|
||
const h = i => { let m = Infinity; const p = pos(i); for (const g of goals) m = Math.min(m, wd(p, g)); return m; };
|
||
let timedOut = false;
|
||
const skey = (a, b) => { const x = pkey(a, pos(a)), y = pkey(b, pos(b)); return x < y ? x + '|' + y : y + '|' + x; };
|
||
const edgeOk = (a, b) => { // 只对本次查询的待验边做在线细验
|
||
const k = ekey(a, b);
|
||
if (good.has(k)) return true;
|
||
if (bad.has(k)) return false;
|
||
const sk = skey(a, b);
|
||
if (BADE.has(sk)) { bad.add(k); return false; }
|
||
if (nowMs() > deadline) { timedOut = true; return false; }
|
||
const pa = pos(a), pb = unwrapTo(pa, pos(b));
|
||
const ok = edgeFree(pa, pb);
|
||
(ok ? good : bad).add(k);
|
||
if (!ok) { if (BADE.size > 300000) BADE.clear(); BADE.add(sk); }
|
||
return ok;
|
||
};
|
||
/* 已知坏的接入边直接不进图：省掉 A* 反复选中它们再被否决的轮次 */
|
||
for (const [u, lst] of extra) for (let j = lst.length - 1; j >= 0; j--) if (BADE.has(skey(u, lst[j][0]))) { bad.add(ekey(u, lst[j][0])); }
|
||
const augment = () => {
|
||
/* 增广（成功或超时都做：已验的接入边是下一帧的资产，跨帧"随时可停"地累积进度）：起点 / 用到的目标并入，带上所有已验通过的接入边 */
|
||
if (RMX.Q.length + 2 <= RMX_MAX) {
|
||
const newId = new Map();
|
||
const addNode = id => { if (newId.has(id)) return newId.get(id); const nid = N + RMX.Q.length; RMX.Q.push(pos(id).map((v, k) => LIM[ORDER[k]].cont ? wrap180(v) : v)); RMX.adj.push([]); newId.set(id, nid); return nid; };
|
||
const mapId = id => id >= NX ? addNode(id) : id;
|
||
RMX.adjR = RMX.adjR || new Map();
|
||
const addEdge = (a, b, c) => {
|
||
for (const [x, y] of [[a, b], [b, a]]) {
|
||
if (x >= N) RMX.adj[x - N].push([y, c]);
|
||
else { if (!RMX.adjR.has(x)) RMX.adjR.set(x, []); RMX.adjR.get(x).push([y, c]); }
|
||
}
|
||
};
|
||
for (const k of good) {
|
||
const a = Math.floor(k / 65536), b = k % 65536;
|
||
addEdge(mapId(a), mapId(b), wd(pos(a), pos(b)));
|
||
}
|
||
}
|
||
};
|
||
let rounds = 0;
|
||
while (nowMs() < deadline && rounds < 60) {
|
||
rounds++;
|
||
const gsc = new Map(), par = new Map(), heap = [];
|
||
const push = (f, n) => { heap.push([f, n]); let i = heap.length - 1; while (i > 0) { const p = (i - 1) >> 1; if (heap[p][0] <= heap[i][0]) break; [heap[p], heap[i]] = [heap[i], heap[p]]; i = p; } };
|
||
const pop = () => { const top = heap[0], last = heap.pop(); if (heap.length) { heap[0] = last; let i = 0; for (;;) { const l = 2 * i + 1, r = l + 1; let m = i; if (l < heap.length && heap[l][0] < heap[m][0]) m = l; if (r < heap.length && heap[r][0] < heap[m][0]) m = r; if (m === i) break; [heap[m], heap[i]] = [heap[i], heap[m]]; i = m; } } return top; };
|
||
gsc.set(S, 0); push(h(S), S);
|
||
let found = -1;
|
||
while (heap.length) {
|
||
const [f, u] = pop();
|
||
const gu = gsc.get(u);
|
||
if (f > gu + h(u) + 1e-9) continue;
|
||
if (isGoal(u)) { found = u; break; }
|
||
const relax = (v, c, lazy) => {
|
||
if (lazy && bad.has(ekey(u, v))) return;
|
||
if (!lazy && DEAD.size && DEAD.has(ekey(u, v))) return;
|
||
const gv = gu + c;
|
||
if (gv < (gsc.has(v) ? gsc.get(v) : Infinity)) { gsc.set(v, gv); par.set(v, [u, lazy]); push(gv + h(v), v); }
|
||
};
|
||
if (u < N) for (let e = RM.off[u]; e < RM.off[u + 1]; e++) { const v = RM.adj[e]; relax(v, wd(RM.Q[u], RM.Q[v]), false); }
|
||
if (u < NX) { const ax = u < N ? null : RMX.adj[u - N]; if (ax) for (const [v, c] of ax) relax(v, c, false); }
|
||
if (u < N) { const ax = RMX.adjR && RMX.adjR.get(u); if (ax) for (const [v, c] of ax) relax(v, c, false); }
|
||
const ex = extra.get(u); if (ex) for (const [v, c] of ex) relax(v, c, true);
|
||
}
|
||
if (found < 0) { augment(); return null; }
|
||
const ids = [], lz = [];
|
||
for (let v = found; v !== S; ) { const [u, lazy] = par.get(v); ids.push(v); lz.push(lazy); v = u; }
|
||
ids.push(S); ids.reverse(); lz.reverse();
|
||
let ok = true;
|
||
for (let i = 0; i + 1 < ids.length; i++) if (lz[i] && !edgeOk(ids[i], ids[i + 1])) { ok = false; break; }
|
||
if (timedOut) { augment(); return null; }
|
||
if (!ok) continue;
|
||
const path = [from.slice()];
|
||
for (let i = 1; i < ids.length; i++) path.push(unwrapTo(path[i - 1], pos(ids[i])));
|
||
augment();
|
||
return { path: path, ms: nowMs() - t0, rounds: rounds };
|
||
}
|
||
augment();
|
||
return null;
|
||
}
|
||
/* 限时贪心拉直：从当前点往后找最远可直达的点（由远到近试） */
|
||
function rmSmooth(path, deadline) {
|
||
if (path.length <= 2) return path;
|
||
const out = [path[0]];
|
||
let i = 0;
|
||
while (i < path.length - 1) {
|
||
let j = path.length - 1;
|
||
if (nowMs() < deadline) {
|
||
for (; j > i + 1; j--) {
|
||
if (edgeFree(out[out.length - 1], unwrapTo(out[out.length - 1], path[j]))) break;
|
||
if (nowMs() > deadline) { j = i + 1; break; }
|
||
}
|
||
} else j = i + 1;
|
||
out.push(unwrapTo(out[out.length - 1], path[j]));
|
||
i = j;
|
||
}
|
||
return out;
|
||
}
|
||
/* ---------- 6e. 实时热启动（拖拽连续重规划） ----------
|
||
拖拽时每次重规划的起点 = 手臂此刻在上一条路径上的位置。上一条路径每段都已细验，
|
||
"起点 → 所在段终点"是已验段的子段，免检；目标每帧只挪一点，新目标接到旧目标只需验一条短边。
|
||
于是一次重规划通常只验 1~2 条短边（几毫秒），而不是重新接入路网。
|
||
路径尾巴会累积小折线：每次先试"倒数第二点 → 新目标"直连，能连就替换掉旧目标。 */
|
||
let RT = null; // { path:[展开位形...] }
|
||
const RTSTALL = { key: null, n: 0 }; // 同一目标连续规划失败的次数（预算递增用）
|
||
function onPath(path, q, tol) { // q 是否在折线上：返回 [段号, 参数] 或 null
|
||
for (let i = 0; i + 1 < path.length; i++) {
|
||
const a = path[i], b = unwrapTo(a, path[i + 1]);
|
||
const qa = unwrapTo(a, q);
|
||
let num = 0, den = 0;
|
||
for (let k = 0; k < 6; k++) { const d = b[k] - a[k]; num += (qa[k] - a[k]) * d; den += d * d; }
|
||
const t = den > 1e-12 ? Math.min(1, Math.max(0, num / den)) : 0;
|
||
let err = 0;
|
||
for (let k = 0; k < 6; k++) err = Math.max(err, Math.abs(a[k] + (b[k] - a[k]) * t - qa[k]));
|
||
if (err <= tol) return [i, t];
|
||
}
|
||
return null;
|
||
}
|
||
function rtWarm(from, goals, deadline) {
|
||
if (!RT || RT.path.length < 2) return null;
|
||
const hit = onPath(RT.path, from, 0.05);
|
||
if (!hit) return null;
|
||
const rest = [from.slice()];
|
||
for (let i = hit[0] + 1; i < RT.path.length; i++) rest.push(unwrapTo(rest[rest.length - 1], RT.path[i]));
|
||
if (rest.length >= 2 && wd(rest[0], rest[1]) < 1e-6) rest.splice(1, 1);
|
||
const last = rest[rest.length - 1];
|
||
for (const g0 of goals) {
|
||
if (nowMs() > deadline) break;
|
||
const g = unwrapTo(last, g0);
|
||
/* 1) 直连（整段，能走就最好） 2) 倒数第二点 → 新目标 3) 旧目标 → 新目标（短边） */
|
||
if (edgeFree(rest[0], unwrapTo(rest[0], g))) return [rest[0], unwrapTo(rest[0], g)];
|
||
if (rest.length >= 2) {
|
||
const pre = rest[rest.length - 2];
|
||
if (edgeFree(pre, unwrapTo(pre, g))) return rest.slice(0, -1).concat([unwrapTo(pre, g)]);
|
||
}
|
||
if (rest.length >= 2 && edgeFree(last, g)) return rest.concat([g]);
|
||
}
|
||
return null;
|
||
}
|
||
/* 实时管线（拖拽 quick 模式且路网可用时）：只走"每步都细验过"的分支，并且每一步都有硬时限。
|
||
直连 → 热启动 → 路网 Lazy A*（40 ms）→ 都不行就沿直线走到最后一个安全点停住。
|
||
不再进入旧的启发式阶梯（慢，且其段只抽查少量点，拖拽仿真里实测会穿模）。 */
|
||
/* 交付前半格复验（2026-10-17）：规划期按 EDGE_RES=1.5° 细验，贴立柱时薄薄一层干涉能整个夹在
|
||
两个采样点之间（贴立柱测试 ci0：119.5° 的长边在 0.76 处 1° 复验撞）。交付给动画前把每段的
|
||
采样点错开半格再验一遍（等效 0.75°）；验过的段记住，拖拽中重复交付同一段不再付费。 */
|
||
const FINE = new Set();
|
||
function segKey(a, b) { return a.map(v => v.toFixed(2)).join(',') + '>' + b.map(v => v.toFixed(2)).join(','); }
|
||
function pathFine(path) {
|
||
for (let i = 1; i < path.length; i++) {
|
||
const a = path[i - 1], b = unwrapTo(a, path[i]), k = segKey(a, b);
|
||
if (FINE.has(k)) continue;
|
||
let m = 0; for (let j = 0; j < 6; j++) m = Math.max(m, Math.abs(b[j] - a[j]));
|
||
const n = Math.max(1, Math.ceil(m / EDGE_RES));
|
||
if (n > 1 || m > EDGE_RES * 0.5) {
|
||
const q = new Array(6);
|
||
for (let t = 0; t < n; t++) {
|
||
const u = (t + 0.5) / n;
|
||
for (let j = 0; j < 6; j++) q[j] = a[j] + (b[j] - a[j]) * u;
|
||
if (!collideFree(q)) {
|
||
if (BADE.size > 300000) BADE.clear();
|
||
BADE.add(segKey(a, b)); BADE.add(segKey(b, a));
|
||
/* 这段若是图里的已验边（路网边或增广边，A* 不再细验），把它判死，否则下一帧 A* 原样再走 */
|
||
const ia = rmIdsAt(a), ib = rmIdsAt(b);
|
||
for (const x of ia) for (const y of ib) DEAD.add(x < y ? x * 65536 + y : y * 65536 + x);
|
||
return false;
|
||
}
|
||
}
|
||
}
|
||
if (FINE.size > 20000) FINE.clear();
|
||
FINE.add(k);
|
||
}
|
||
return true;
|
||
}
|
||
function routeRT(from, cur) {
|
||
if (!COL || !cur || !cur.q || !from) return cur;
|
||
if (!collideFree(from)) return cur; // 起点已在干涉里：交给脱困逻辑
|
||
CFMEMO = new Map();
|
||
try {
|
||
const t0 = nowMs();
|
||
const goals = gatherGoals(cur, from, 3, 4000, 30);
|
||
const done = (path, detour, blocked) => {
|
||
/* 半格复验不过：这一帧不交付这条路径——沿旧路径继续/原地等，下一帧重规划（坏段已记入 BADE） */
|
||
if (!blocked && path.length >= 2 && !pathFine(path)) {
|
||
RT = null;
|
||
return Object.assign({}, cur, { q: from.map((v, i) => LIM[ORDER[i]].cont ? wrap180(v) : v), via: [],
|
||
routed: true, blocked: false, kept: false, detour: 'continue', pending: true, planMs: nowMs() - t0 });
|
||
}
|
||
RT = { path: path };
|
||
const q = path[path.length - 1];
|
||
return Object.assign({}, cur, { q: q.map((v, i) => LIM[ORDER[i]].cont ? wrap180(v) : v), via: path.slice(1, -1),
|
||
routed: true, blocked: !!blocked, kept: false, detour: detour, planMs: nowMs() - t0 });
|
||
};
|
||
if (goals.length) {
|
||
if (!BADE.has(segKey(from, goals[0])) && edgeFree(from, goals[0])) {
|
||
const r0 = done([from.slice(), goals[0]], null);
|
||
if (!r0.pending) return r0;
|
||
}
|
||
const w = rtWarm(from, goals, t0 + 25);
|
||
if (w) return done(w, w.length > 2 ? 'planned' : null);
|
||
/* 目标停住而规划连续失败时逐帧加预算 40→80→160→240 ms（2026-10-17）：贴立柱目标的接入边
|
||
又长又多半不通，单帧 40 ms 只够验一两条；目标不动时延迟无所谓，按住不放也能很快规划出来。
|
||
目标一挪（键变）立即回到 40 ms，拖动跟手不受影响。 */
|
||
const gk = goals[0].map(v => Math.round(v)).join(',');
|
||
const stall = RTSTALL.key === gk ? RTSTALL.n : 0;
|
||
const rp = rmPlan(from, goals, Math.min(240, 40 << stall));
|
||
if (rp) { RTSTALL.key = null; RTSTALL.n = 0; return done(rmSmooth(rp.path, nowMs() + 15), 'planned'); }
|
||
RTSTALL.key = gk; RTSTALL.n = Math.min(3, stall + 1);
|
||
/* 本帧没规划完：沿上一条（已验）路径继续走，下一帧接着规划——不急停、不乱走 */
|
||
if (RT && RT.path.length >= 2) {
|
||
const hit = onPath(RT.path, from, 0.05);
|
||
if (hit) {
|
||
const rest = [from.slice()];
|
||
for (let i = hit[0] + 1; i < RT.path.length; i++) rest.push(unwrapTo(rest[rest.length - 1], RT.path[i]));
|
||
if (rest.length >= 2) {
|
||
const q = rest[rest.length - 1];
|
||
return Object.assign({}, cur, { q: q.map((v, i) => LIM[ORDER[i]].cont ? wrap180(v) : v), via: rest.slice(1, -1),
|
||
routed: true, blocked: false, kept: false, detour: 'continue', pending: true, planMs: nowMs() - t0 });
|
||
}
|
||
}
|
||
}
|
||
}
|
||
/* 规划不到：沿"朝最近目标的直线"按 EDGE_RES 前进，停在最后一个无干涉采样点 */
|
||
const g = goals.length ? goals[0] : unwrapTo(from, cur.q);
|
||
let m = 0; for (let i = 0; i < 6; i++) m = Math.max(m, Math.abs(g[i] - from[i]));
|
||
const n = Math.max(1, Math.ceil(m / EDGE_RES));
|
||
let last = from.slice();
|
||
for (let k = 1; k <= n; k++) {
|
||
const q = from.map((v, i) => v + (g[i] - v) * k / n);
|
||
if (!collideFree(q)) break;
|
||
last = q;
|
||
}
|
||
return done([from.slice(), last], null, true);
|
||
} finally { CFMEMO = null; }
|
||
}
|
||
function planAround(from, cur, quick) {
|
||
const goals = gatherGoals(cur, from);
|
||
if (!goals.length) return null;
|
||
const budget = quick ? 150 : 1500;
|
||
/* 实时版：有离线路网就直接 Lazy A*（典型几十毫秒），直连边也在图里（A* 自然优先），失败再走下面的旧流程 */
|
||
if (RM) {
|
||
const t0 = nowMs();
|
||
if (quick) {
|
||
const w = rtWarm(from, goals, t0 + 40);
|
||
if (w) { RT = { path: w }; return { goal: w[w.length - 1], via: w.slice(1, -1), detour: w.length > 2 ? 'planned' : null, ms: nowMs() - t0, rm: 'warm' }; }
|
||
}
|
||
const rp = rmPlan(from, goals, quick ? 60 : 300);
|
||
if (rp) {
|
||
let sm = rmSmooth(rp.path, nowMs() + (quick ? 25 : 80));
|
||
/* 松手精修：再做一轮随机捷径 + 逐轴拉直（路网路径节点稀疏，有可观的多余行程） */
|
||
if (!quick && sm.length > 2) {
|
||
let seed = 2166136261; for (const v of from) seed = Math.imul(seed ^ Math.round(v * 10), 16777619);
|
||
let st = seed >>> 0 || 1; const rnd = () => { st = (st * 1664525 + 1013904223) >>> 0; return st / 4294967296; };
|
||
sm = shortcut(sm, nowMs() + 120, rnd);
|
||
}
|
||
RT = { path: sm };
|
||
return { goal: sm[sm.length - 1], via: sm.slice(1, -1), detour: 'planned', ms: nowMs() - t0, rm: true };
|
||
}
|
||
}
|
||
/* 先试直线（多目标、2° 细验）：换个构型能直走就不绕 */
|
||
for (const g of goals) if (edgeFree(from, g)) return { goal: g, via: [], detour: null };
|
||
/* 再试"单中间点微调"：弦中点抬肩 / 收肘（只动 J2/J4，其余轴保持直线插值），2° 细验。
|
||
贴边擦碰的大多数场景这里就能解决，动作最接近直线、最像人。 */
|
||
for (const g of goals.slice(0, 2)) {
|
||
const mid = from.map((v, i) => 0.5 * (v + g[i]));
|
||
for (const [d2, d4] of GENTLE_OFFSETS) {
|
||
const m = mid.slice(); m[1] += d2; m[3] += d4;
|
||
if (!inBox(m) || !collideFree(m)) continue;
|
||
if (edgeFree(from, m) && edgeFree(m, g)) return { goal: g, via: [m], detour: 'planned' };
|
||
}
|
||
}
|
||
let seed = 2166136261;
|
||
for (const v of from.concat(goals[0])) seed = Math.imul(seed ^ Math.round(v * 10), 16777619);
|
||
const r = rrtConnect(from, goals, budget, seed);
|
||
if (!r) return null;
|
||
const sm = shortcut(r.path, nowMs() + (quick ? 40 : 160), r.rnd);
|
||
const goal = sm[sm.length - 1];
|
||
return { goal: goal, via: sm.slice(1, -1), detour: 'planned', iters: r.iters, ms: r.ms };
|
||
}
|
||
function routeOrVia(from, target, cur, opt) {
|
||
CFMEMO = new Map();
|
||
try { return routeOrViaImpl(from, target, cur, opt); } finally { CFMEMO = null; }
|
||
}
|
||
function routeOrViaImpl(from, target, cur, opt) {
|
||
const lazy = !opt.noAround;
|
||
let r = routeFrom(from, cur, lazy);
|
||
if (!r.blocked || !from) { RT = from && r && r.q ? { path: [from.slice(), unwrapTo(from, r.q)] } : null; return r; }
|
||
/* 首选：采样规划（多目标 + 平滑），给出的路径已按 EDGE_RES 细验 */
|
||
if (!opt.noAround) {
|
||
const pl = planAround(from, cur, opt.quick);
|
||
if (pl) {
|
||
const res = Object.assign({}, cur, { q: pl.goal.map((v, i) => LIM[ORDER[i]].cont ? wrap180(v) : v), routed: true, blocked: false, kept: false });
|
||
if (pl.via.length) Object.assign(res, { via: pl.via, detour: pl.detour, planIters: pl.iters, planMs: pl.ms });
|
||
return res;
|
||
}
|
||
if (r.lazyBlocked) r = routeFrom(from, cur, false);
|
||
/* 规划预算内未连通（极少）：退回旧的启发式阶梯 */
|
||
const around = routeAroundPlan(from, cur, opt.quick, r.safeT);
|
||
/* 旧阶梯的段只抽查少量点（实测拖拽仿真里 coordinated 段会穿模）：执行前按 EDGE_RES 整条细验，
|
||
不过就宁可贴边停下（blocked），绝不把没验透的折线交给动画 */
|
||
if (around && polyFree(from, around.via, around.res.q))
|
||
return Object.assign({}, around.res, { via: around.via, routed: true, blocked: false, detour: around.detour || 'around' });
|
||
}
|
||
if (opt.noVia) return r;
|
||
const rv = routeVia(from, target, opt);
|
||
if (rv && rv.q && polyFree(from, rv.via || [], rv.q)) return rv;
|
||
return r;
|
||
}
|
||
function polyFree(from, via, q) { // 整条折线（展开后）按 EDGE_RES 细验
|
||
let p = from;
|
||
for (const raw of (via || []).concat([q])) {
|
||
const t = unwrapTo(p, raw);
|
||
if (!edgeFree(p, t)) return false;
|
||
p = t;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
/* ---------- 6b. 动态限位求解：静态解 + 贴边轴放宽重解 ----------
|
||
静态限位来自"其它轴全零"的单轴扫描，对非相邻碰撞对（典型：腕/法兰撞
|
||
固定立柱）系统性过紧——实测 J2 正端随 J1 从 +9.75°（垂臂零位）放到
|
||
+108.75°（肩侧抬 90°）。若静态解的某轴贴在边界上，就按当前姿态二分出
|
||
该轴的动态边界，临时放宽该轴限位后重解；新解必须通过全姿态碰撞判定、
|
||
且总关节角不升才采用，否则原样返回（最坏即静态行为，无回退风险）。 */
|
||
/* 姿态放宽兜底（2026-10-17）：目标贴着立柱时，指定姿态的解族（只保留姿态残差 ≤POSETOL 的）
|
||
可能整族都撞立柱，但这个位置本身完全够得到。位置优先/混合模式下改解一次纯位置任务，
|
||
对每个候选按 J6 闭式最优滚转（TCP 在 J6 轴上，J6 不改位置）算姿态残差，按残差升序取第一个
|
||
无干涉解。只在"无安全解"的兜底分支里跑（多一次全量 IK），姿态优先模式不放宽。 */
|
||
function bestRoll(q, Rt) {
|
||
const t = q.slice();
|
||
let bv = Infinity, bj = t[5];
|
||
for (let a = -180; a < 180; a += 4) { t[5] = a; const e = rotErrDeg(orient(t), Rt); if (e < bv) { bv = e; bj = a; } }
|
||
let lo = bj - 4, hi = bj + 4;
|
||
for (let k = 0; k < 30; k++) {
|
||
const m1 = lo + (hi - lo) / 3, m2 = hi - (hi - lo) / 3;
|
||
t[5] = m1; const e1 = rotErrDeg(orient(t), Rt); t[5] = m2; const e2 = rotErrDeg(orient(t), Rt);
|
||
if (e1 < e2) hi = m2; else lo = m1;
|
||
}
|
||
t[5] = 0.5 * (lo + hi);
|
||
const L6 = LIM[ORDER[5]];
|
||
if (L6.cont) t[5] = wrap180(t[5]); else t[5] = Math.min(L6.hi, Math.max(L6.lo, t[5]));
|
||
return { q: t, oerr: rotErrDeg(orient(t), Rt) };
|
||
}
|
||
function poseRelax(target, opt, cur) {
|
||
if (!opt.pose || !opt.pose.R || opt.priority === 'ori') return null;
|
||
const o2 = Object.assign({}, opt); delete o2.pose; delete o2.priority; delete o2.mixW; delete o2.warm;
|
||
delete o2.ref; // 不带行程参照：候选表不偏向起点那一侧（按姿态残差挑）
|
||
const r = solve(target, o2);
|
||
if (!r || !r.list || !r.list.length) return null;
|
||
const Rt = opt.pose.R;
|
||
const cand = r.list.map(s => bestRoll(s.q, Rt));
|
||
cand.sort((x, y) => x.oerr - y.oerr);
|
||
/* 去重：J1~J5 按 6° 栅格分桶，同桶只验姿态残差最小的那个（撞立柱的候选成片相邻，
|
||
不分桶时实测要验上千个才碰到第一个安全解） */
|
||
const seenB = new Set();
|
||
let checks = 0;
|
||
for (const c of cand) {
|
||
if (checks >= 300) break;
|
||
const key = c.q.slice(0, 5).map(v => Math.round(v / 6)).join(',');
|
||
if (seenB.has(key)) continue;
|
||
seenB.add(key);
|
||
checks++;
|
||
if (collideFree(c.q)) {
|
||
return { q: c.q, res: Object.assign({}, cur, { q: c.q.slice(), cost: costOf(c.q), err: 0, oerr: c.oerr,
|
||
reachable: true, ok: true, poseRelaxed: true, poseFine: false, poseTier: c.oerr <= 2 ? 'approx' : 'poor',
|
||
list: r.list }) };
|
||
}
|
||
}
|
||
return null;
|
||
}
|
||
function solveDyn(target, opt) {
|
||
opt = opt || {};
|
||
if (opt.from && !opt.ref) opt.ref = opt.from;
|
||
let cur = solveTo(target, opt);
|
||
if (!COL || opt.noDyn || !cur.q) return cur;
|
||
/* 静态限位盒不保证组合姿态无干涉（单轴扫描各轴独立取限位，组合折叠会撞——
|
||
sweep_v2 的组合扫描只记录不收紧滑块）。这里对最终解做全姿态碰撞校验：
|
||
撞了就沿候选表找安全的解，优先采用最贴近起点的安全位形。 */
|
||
if (!collideFree(cur.q)) {
|
||
const safeT = findSafeTarget(cur, opt.from || opt.ref) || poseRelax(target, opt, cur);
|
||
if (safeT) cur = Object.assign({}, safeT.res, { swaped: true });
|
||
else cur = Object.assign({}, cur, { unsafe: true });
|
||
}
|
||
for (let pass = 0; pass < 2; pass++) {
|
||
const q = cur.q;
|
||
let axis = null, dir = 0;
|
||
for (let i = 0; i < 5; i++) {
|
||
const t = LIM[ORDER[i]];
|
||
if (t.cont) continue;
|
||
if (q[i] >= t.hi - 0.5) { axis = ORDER[i]; dir = +1; break; }
|
||
if (q[i] <= t.lo + 0.5) { axis = ORDER[i]; dir = -1; break; }
|
||
}
|
||
if (!axis) break;
|
||
const bound = dynLimit(axis, q, dir, 0.5);
|
||
if (Math.abs(bound - q[ORDER.indexOf(axis)]) < 1.0) break; // 动态边界没放宽
|
||
const save = { lo: LIM[axis].lo, hi: LIM[axis].hi };
|
||
let r1 = null;
|
||
try {
|
||
if (dir > 0) LIM[axis].hi = bound; else LIM[axis].lo = bound;
|
||
/* 增益 <5° 不值得一次全量重解（实测重解与首解同量级 ~190ms），直接跳过；
|
||
大幅放宽（典型：J1=90° 时 J2 从 +9 放到 +106）才值得。 */
|
||
if (bound - q[ORDER.indexOf(axis)] < (dir > 0 ? 5 : -5)) break;
|
||
/* 重解步长粗一档：只需要"比静态更优"的解（接受判据就是 cost 不升），
|
||
不需要全局最优——全量精扫贵在与首解同量级，粗档约省 3~4 倍。 */
|
||
const step1 = Math.max((+opt.step || 0.25) * 2, 0.5);
|
||
r1 = solveTo(target, Object.assign({}, opt, { noDyn: true, step: step1 }));
|
||
} finally {
|
||
LIM[axis].lo = save.lo; LIM[axis].hi = save.hi;
|
||
}
|
||
if (!r1 || !r1.q || r1.cost >= cur.cost - 1e-9) break; // 没有更优 → 收工
|
||
let picked = null;
|
||
if (collideFree(r1.q)) picked = r1;
|
||
else for (const s of r1.list) { // 热循环不查碰撞：候选表里找第一个安全的
|
||
if (s.cost < cur.cost - 1e-9 && collideFree(s.q)) {
|
||
picked = Object.assign({}, r1, { q: s.q.slice(), cost: s.cost });
|
||
break;
|
||
}
|
||
}
|
||
if (!picked) break;
|
||
cur = Object.assign({}, picked, { dynAxis: axis, dynBound: bound });
|
||
}
|
||
return opt.from ? routeOrVia(opt.from, target, cur, opt) : cur;
|
||
}
|
||
|
||
/* ---------- 7. 自检：闭式 FK ↔ 矩阵链；逆解重构；姿态重构 ---------- */
|
||
function selfTest(n, nPose) {
|
||
n = n || 400;
|
||
nPose = nPose == null ? 8 : nPose;
|
||
let maxFk = 0;
|
||
for (let k = 0; k < n; k++) {
|
||
const q = [];
|
||
for (let i = 0; i < 6; i++) { const t = LIM[ORDER[i]]; q.push(t.cont ? (Math.random() * 720 - 360) : t.lo + Math.random() * (t.hi - t.lo)); }
|
||
const a = fk(q, 0), b = fkChain(q, 0);
|
||
maxFk = Math.max(maxFk, Math.hypot(a[0] - b[0], a[1] - b[1], a[2] - b[2]));
|
||
}
|
||
let maxIk = 0, hit = 0, m = 0;
|
||
for (let k = 0; k < Math.min(n, 200); k++) {
|
||
const q = [];
|
||
for (let i = 0; i < 6; i++) { const t = LIM[ORDER[i]]; q.push(t.lo + Math.random() * (t.hi - t.lo)); }
|
||
const T = fk(q, 0);
|
||
const A = T[0] - K.X0, BY = T[1] - K.Y0, BZ = T[2] - K.Z1;
|
||
const sols = branch(q[3], q[4], A, Math.hypot(BY, BZ), Math.atan2(BZ, BY), 0);
|
||
if (!sols.length) continue;
|
||
m++;
|
||
let best = Infinity;
|
||
for (const s of sols) best = Math.min(best, Math.hypot.apply(null, fk(s, 0).map((v, i) => v - T[i])));
|
||
maxIk = Math.max(maxIk, best);
|
||
if (best < 1e-6) hit++;
|
||
}
|
||
const ps = nPose > 0 ? poseSelfTest(nPose) : { poseHit: 0, poseTried: 0, poseErr: 0, poseMs: 0 };
|
||
return { maxFkErr: maxFk, maxIkErr: maxIk, ikHit: hit, ikTried: m, structWarn: WARN, K: K, LIM: LIM,
|
||
poseHit: ps.poseHit, poseTried: ps.poseTried, poseErr: ps.poseErr, poseMs: ps.poseMs };
|
||
}
|
||
|
||
/* 姿态重构自检：取随机可行位形的真实工具姿态为任务，闭式扫描应精确命中。
|
||
单次约 70 ms（step=1 全量扫描），200 组同步跑 ≈ 14 s——放在页面启动路径里会把
|
||
首屏从 ~1.4 s 拖到 ~15 s。故单独成函数，供界面分片调用。 */
|
||
function poseSelfTest(nPose) {
|
||
let pHit = 0, pTried = 0, pErr = 0, pMs = 0;
|
||
for (let k = 0; k < nPose; k++) {
|
||
const q = [];
|
||
for (let i = 0; i < 6; i++) { const t = LIM[ORDER[i]]; q.push(t.lo + Math.random() * (t.hi - t.lo)); }
|
||
const T = fk(q, 0), R = orient(q);
|
||
pTried++;
|
||
const r = solve(T, { pose: { R: R }, step: 1.0, tol: 1e-6 });
|
||
pMs += r.ms;
|
||
if (r.ok && r.oerr != null) {
|
||
pErr = Math.max(pErr, r.oerr);
|
||
if (r.oerr < 0.05 && r.err < 1e-3) pHit++;
|
||
}
|
||
}
|
||
return { poseHit: pHit, poseTried: pTried, poseErr: pErr, poseMs: pTried ? pMs / pTried : 0 };
|
||
}
|
||
|
||
return { fk: fk, fkChain: fkChain, solve: solve, solveTo: solveTo, solveWarm: solveWarm, solveDyn: solveDyn,
|
||
solveRoute: solveRoute, routeFrom: routeFrom, transitPt: transitPt, planAround: planAround, edgeFree: edgeFree, PST: PST, unwrapTo: unwrapTo, rtWarm: rtWarm, rtReset: () => { RT = null; }, rmPlan: rmPlan, rmSmooth: rmSmooth, modelFingerprint: modelFingerprint, RM: RM, wd: wd, classGap: classGap, classFingerprint: classFingerprint, P2D: P2D,
|
||
mixSolve: mixSolve, closest: closest, closest6: closest6, closestOri: closestOri, orient: orient,
|
||
branch: branch, selfTest: selfTest, poseSelfTest: poseSelfTest,
|
||
collideFree: collideFree, collideDetail: collideDetail, clearance: clearance, gapMatrix: gapMatrix, leverBounds: leverBounds, leverTable: leverTable, setDeterministic: setDeterministic, resetSession: resetSession, pairList: pairList, clusterExport: clusterExport, criticalPairs: criticalPairs, pairGap: pairGap, TCP0: TCP0, CAPM: CAPM,
|
||
dynLimit: dynLimit, dynLimits: dynLimits,
|
||
K: K, LIM: LIM, P: P, D: D, ORDER: ORDER, wrap180: wrap180, costOf: costOf };
|
||
}
|
||
|
||
return { make: make, ORDER: ORDER, poseMat: poseMat, poseAng: poseAng, rotErrDeg: rotErrDeg,
|
||
segSegDist: segSegDist };
|
||
})); |