feat(arm): 建模代码拆成每件一模块，查看器新增工程图与装配步骤页
- 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 历史里已有）
This commit is contained in:
+306
-14
@@ -388,22 +388,45 @@ function make(cfg) {
|
||||
const side = dir > 0 ? 'hi' : 'lo';
|
||||
const hard = HARD[id] && HARD[id][side];
|
||||
const hi0 = dir > 0 ? LIM[id].hi : LIM[id].lo;
|
||||
if (hard) return hi0; // 硬边：配置无关，静态值即真值
|
||||
let hi = sign * (LIM[id].cont ? 360 : Math.min(MOTOR || 180, 180)); // 连续轴（J1/J3/J6）无机械限位：放到 ±360°，不受 MOTOR 封顶
|
||||
/* 连续轴的搜索端点取 ±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]; // 当前姿已干涉：不放大
|
||||
if (at(hi)) return hi;
|
||||
let lo = q[k];
|
||||
while (Math.abs(hi - lo) > tol) {
|
||||
const mid = 0.5 * (lo + hi);
|
||||
if (at(mid)) lo = mid; else hi = mid;
|
||||
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;
|
||||
}
|
||||
@@ -1908,6 +1931,277 @@ function make(cfg) {
|
||||
};
|
||||
}
|
||||
|
||||
/* ---------- 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°（垂臂零位）放到
|
||||
@@ -1916,18 +2210,15 @@ function make(cfg) {
|
||||
且总关节角不升才采用，否则原样返回（最坏即静态行为，无回退风险）。 */
|
||||
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 的组合扫描只记录不收紧滑块）。这里对最终解做全姿态碰撞校验：
|
||||
撞了就沿候选表（按代价升序）找第一个安全的，代价容忍 1.5 倍 + 20°。 */
|
||||
撞了就沿候选表找安全的解，优先采用最贴近起点的安全位形。 */
|
||||
if (!collideFree(cur.q)) {
|
||||
let pick = null;
|
||||
for (let i = 0; i < cur.list.length && i < 40; i++) {
|
||||
const s = cur.list[i];
|
||||
if (s.cost <= cur.cost * 1.5 + 20 && collideFree(s.q)) { pick = s; break; }
|
||||
}
|
||||
if (pick) cur = Object.assign({}, cur, { q: pick.q.slice(), cost: pick.cost, swaped: true });
|
||||
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++) {
|
||||
@@ -1968,7 +2259,7 @@ function make(cfg) {
|
||||
if (!picked) break;
|
||||
cur = Object.assign({}, picked, { dynAxis: axis, dynBound: bound });
|
||||
}
|
||||
return cur;
|
||||
return opt.from ? routeOrVia(opt.from, target, cur, opt) : cur;
|
||||
}
|
||||
|
||||
/* ---------- 7. 自检：闭式 FK ↔ 矩阵链；逆解重构；姿态重构 ---------- */
|
||||
@@ -2022,6 +2313,7 @@ function make(cfg) {
|
||||
}
|
||||
|
||||
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,
|
||||
|
||||
Reference in New Issue
Block a user