cbb
b0efb44368
- src/parts/p01~p09 + src/part_tools.py：几何、件号/配色/说明与装配帧归属随各自件文件走，arm_model.py 只留装配流水线并对件级符号做只读转发；P04_CH / P07_CH / P06_WIN_XZ / FENCE_OPEN_CUT 这类开关必须写所在件文件，写 arm_model.X 不生效，A/B 判据为翻转后体积变化
- docs/sheet_{exploded,front,side,joint}.svg + draw_sheet.py：四张工程图进查看器页签，支持图层开关、件号高亮与 BOM/3D 联动跳转
- travel/travel_detail/collide/bom/screws 随最新几何重生成；补线束走线、拆卸定位点与 P04 承压面剖切验证图
- .gitignore 排除 .bak/ 与 *.bak_* 源码快照（旧版本 git 历史里已有）
2326 lines
123 KiB
JavaScript
2326 lines
123 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 = []; // 零件级（宽相位）与零件对
|
||
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]);
|
||
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]);
|
||
}
|
||
}
|
||
/* 组累积刚体变换 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）时首对即短路返回。 */
|
||
function scanPairs(q, visit) {
|
||
if (!COL) return false;
|
||
const { R: Rs, T: Ts } = cumXf(q);
|
||
/* clc/cache/pcc 全部预分配（声明见 cumXf 前）：CGEN 代际戳标记"本 check 已变换" */
|
||
CGEN++;
|
||
const clOf = (pi, ci) => {
|
||
const e = CLBUF[pi][ci];
|
||
if (e[3] !== CGEN) {
|
||
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] = CGEN;
|
||
}
|
||
return e;
|
||
};
|
||
const pcOf = pi => {
|
||
const e = PCBUF[pi];
|
||
if (e[3] !== CGEN) {
|
||
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] = CGEN;
|
||
}
|
||
return e;
|
||
};
|
||
const xf = i => {
|
||
const e = XFBUF[i];
|
||
if (e[7] === CGEN) 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] = 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];
|
||
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 * lim0) 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 * lim) 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];
|
||
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 (d < need) {
|
||
hit = true;
|
||
if (visit) visit(e1[6], e2[6], d, need);
|
||
else return true;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
return hit;
|
||
}
|
||
/* 全姿态动态碰撞判定：True = 无动态干涉（模型近似，含 margin 外扩） */
|
||
function collideFree(q) {
|
||
return !COL || !scanPairs(q, null);
|
||
}
|
||
/* 干涉明细（调参/界面提示用）：返回 [{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;
|
||
};
|
||
const nowMs = () => (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;
|
||
/* 参考位姿（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) { // 有界保留：只留代价最优者
|
||
all.sort((x, y) => x.rcost - y.rcost);
|
||
all.length = KEEP;
|
||
trimmed = true;
|
||
}
|
||
}
|
||
if (POSE) { // 姿态模式同样需要有界保留（带优先，再补全局最优）
|
||
band.sort((x, y) => x.oerr - y.oerr);
|
||
const keepBand = band.slice(0, KEEP);
|
||
const keepSet = new Set(keepBand);
|
||
const rest = all.filter(s => !keepSet.has(s)).sort((x, y) => x.rcost - y.rcost).slice(0, KEEP);
|
||
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 = pool.slice().sort((x, y) => POSE ? x.rc0 - y.rc0 : x.rcost - y.rcost).slice(0, KEEP);
|
||
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
|
||
};
|
||
}
|
||
|
||
/* ---------- 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) {
|
||
const stride = Math.max(1, Math.floor((len - 120) / 40));
|
||
for (let i = 120; i < len; i += stride) {
|
||
const s = cur.list[i];
|
||
if (s && s.q && collideFree(s.q)) {
|
||
return { q: s.q, res: Object.assign({}, cur, { q: s.q.slice(), cost: s.cost, err: s.err, oerr: s.oerr }) };
|
||
}
|
||
}
|
||
}
|
||
return null;
|
||
}
|
||
function routeFrom(from, cur) {
|
||
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 });
|
||
}
|
||
}
|
||
/* 没有一条直线能干净抵达：沿最优候选（优先端点安全的那个）方向二分退回安全边界，
|
||
让臂贴着障碍停住，而不是穿过去。 */
|
||
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 = 当前位形） */
|
||
function solveRoute(target, opt) {
|
||
opt = opt || {};
|
||
if (opt.from && !opt.ref) opt.ref = opt.from;
|
||
return routeOrVia(opt.from, target, solveTo(target, opt), opt);
|
||
}
|
||
/* 两段式绕行：直线被立柱挡住、但目标本身有安全解时，先解一个"抬高版"的中间位形
|
||
（目标正上方 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;
|
||
}
|
||
function routeOrVia(from, target, cur, opt) {
|
||
const r = routeFrom(from, cur);
|
||
if (!r.blocked || !from) return r;
|
||
const around = opt.noAround ? null : routeAroundPlan(from, cur, opt.quick, r.safeT);
|
||
if (around) return Object.assign({}, around.res, { via: around.via, routed: true, blocked: false, detour: around.detour || 'around' });
|
||
if (opt.noVia) return r;
|
||
return routeVia(from, target, opt) || r;
|
||
}
|
||
|
||
/* ---------- 6b. 动态限位求解：静态解 + 贴边轴放宽重解 ----------
|
||
静态限位来自"其它轴全零"的单轴扫描，对非相邻碰撞对（典型：腕/法兰撞
|
||
固定立柱）系统性过紧——实测 J2 正端随 J1 从 +9.75°（垂臂零位）放到
|
||
+108.75°（肩侧抬 90°）。若静态解的某轴贴在边界上，就按当前姿态二分出
|
||
该轴的动态边界，临时放宽该轴限位后重解；新解必须通过全姿态碰撞判定、
|
||
且总关节角不升才采用，否则原样返回（最坏即静态行为，无回退风险）。 */
|
||
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);
|
||
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,
|
||
mixSolve: mixSolve, closest: closest, closest6: closest6, closestOri: closestOri, orient: orient,
|
||
branch: branch, selfTest: selfTest, poseSelfTest: poseSelfTest,
|
||
collideFree: collideFree, collideDetail: collideDetail,
|
||
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 };
|
||
})); |