feat: 查看器新增姿态控制算法与箭头拖拽交互（TCP位置+工具姿态6自由度联合IK），IK控制条改两行，补充定位图截图
- arm-ik-solver.js：位置闭式支附加 θ6 滚转解算（N=R5^T·R_des，工具轴与 θ6 无关的结构）；达标带（e≤1°）按 e 升序取种子 + escalating（惩罚×100 重扫）+ (θ4,θ5) 二维牛顿收口，分层保证"姿态达标再谈省关节角"；超可达空间走 6 维阻尼最小二乘（位置 mm+姿态 deg 联合）兜底；自检补姿态重构 - ik-glue.js：姿态控制开关 + 方位/仰角/滚转输入 + 取当前/垂直向下；目标点处方向箭头（弧球法调指向）与粉色侧球/箭杆（调滚转），就近仲裁不抢黄球拖拽；面板显示目标/实际姿态与残差（闭式/场景图双口径） - 界面语义统一为工具朝外法向，入栈绕工具 x 翻 180° 换算求解器内部约定
This commit is contained in:
+311
-20
@@ -34,6 +34,39 @@ const S = ArmIK.make({ joints: JMap, travel: TR, tcp: TCP0 });
|
||||
const LIM = S.LIM;
|
||||
let lastTool = 0;
|
||||
let TGT = [150, 128, 260];
|
||||
/* 参考位姿连续项权重：以当前手臂姿态为参考解算，目标微动时锁定同族分支，
|
||||
避免某轴在等价分支间大幅倒摆（实测 4mm 步长过边界最多跳 81.9° -> 1.2°） */
|
||||
const REF_W = 0.02;
|
||||
const curRef = () => ORDER.map(id => ang[id]);
|
||||
|
||||
/* ---------- 1b. 姿态控制状态 ---------- */
|
||||
let poseOn = false;
|
||||
const POSE = { az: 0, el: 90, roll: 0 }; // 目标姿态：工具轴方位角 / 仰角 + 绕轴滚转（度）
|
||||
const Rdes = () => ArmIK.poseMat(POSE.az, POSE.el, POSE.roll);
|
||||
const syncPoseInputs = () => {
|
||||
$('ikAz').value = (+POSE.az).toFixed(1);
|
||||
$('ikEl').value = (+POSE.el).toFixed(1);
|
||||
$('ikRoll').value = (+POSE.roll).toFixed(1);
|
||||
};
|
||||
const fillPoseFromCurrent = () => { // 用当前真实工具姿态回填（开启时避免姿态跳变）
|
||||
const a = ArmIK.poseAng(Uof(ORDER.map(id => ang[id])));
|
||||
POSE.az = a.az; POSE.el = a.el; POSE.roll = a.roll;
|
||||
syncPoseInputs();
|
||||
};
|
||||
const CAD2V = v => [v[0], v[2], -v[1]]; // CAD 向量 → 查看器坐标（与查看器 VC 一致）
|
||||
const V2CAD = v => [v[0], -v[2], v[1]]; // 查看器坐标 → CAD 向量（CAD2V 的逆）
|
||||
/* 用户姿态 ↔ 求解器姿态的换算：求解器内部工具坐标系 z 轴取法兰面法向（朝臂内，
|
||||
即 TCP 外伸的反向），而界面上的方位/仰角/滚转与箭头都以「工具朝外方向」描述。
|
||||
两者相差绕工具 x 轴 180°（x 不变、y/z 取反），入栈/出栈各翻一次即可。 */
|
||||
const RX180 = [1, 0, 0, 0, -1, 0, 0, 0, -1];
|
||||
const mul3g = (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 Rsolver = () => mul3g(Rdes(), RX180); // 交给求解器/场景校验（列 2 = 朝臂内）
|
||||
const Uof = q => mul3g(S.orient(q), RX180); // 手臂当前姿态 → 用户系（列 2 = 朝臂外）
|
||||
|
||||
/* ---------- 2. 场景标注 ---------- */
|
||||
const gTgt = new THREE.Group(); scene.add(gTgt);
|
||||
@@ -55,6 +88,83 @@ const ballTcp = new THREE.Mesh(new THREE.SphereGeometry(6.5, 20, 14),
|
||||
new THREE.MeshStandardMaterial({ color: 0x3ddc97, emissive: 0x14603f, metalness: .3, roughness: .35 }));
|
||||
JG.J6.add(ballTcp);
|
||||
|
||||
/* 当前工具轴线：TCP 球沿真实工具轴伸出（绿），与目标箭头对照看姿态差 */
|
||||
const curGeo = new THREE.BufferGeometry();
|
||||
curGeo.setAttribute('position', new THREE.BufferAttribute(new Float32Array(6), 3));
|
||||
const curLine = new THREE.Line(curGeo,
|
||||
new THREE.LineBasicMaterial({ color: 0x3ddc97, transparent: true, opacity: .95 }));
|
||||
scene.add(curLine);
|
||||
|
||||
/* 姿态拖动箭头（挂目标球组，随目标移动）：
|
||||
拖箭头头部 = 调工具轴指向（方位/俯仰，弧球式）；拖箭杆/捏环/粉色侧球 = 绕轴滚转。
|
||||
轴向为本地 +Z：杆/锥几何预旋到 +Z，gArrow 再用四元数对准目标工具轴。
|
||||
侧球挂在目标点上、沿工具 x 轴侧向偏移——箭头正对镜头时杆身投影缩成一团，
|
||||
侧球始终在侧向，任何视角都抓得到。 */
|
||||
const AXLEN = 150, HEADL = 26, HEADZ = AXLEN - HEADL / 2, GRIPZ = (AXLEN - HEADL) / 2, KNOBOFF = 0.72 * AXLEN;
|
||||
const gArrow = new THREE.Group(); gTgt.add(gArrow);
|
||||
const knob = new THREE.Mesh(new THREE.SphereGeometry(9, 18, 12),
|
||||
new THREE.MeshStandardMaterial({ color: 0xff8fb0, emissive: 0x5c1f38, metalness: .2, roughness: .4,
|
||||
transparent: true, opacity: .8 }));
|
||||
knob.userData.role = 'roll';
|
||||
gTgt.add(knob);
|
||||
const arrowShaft = (function () {
|
||||
const g = new THREE.CylinderGeometry(3.0, 3.0, AXLEN - HEADL, 12);
|
||||
g.rotateX(Math.PI / 2); // 圆柱默认轴向 Y → +Z
|
||||
const m = new THREE.Mesh(g,
|
||||
new THREE.MeshStandardMaterial({ color: 0xffb454, emissive: 0x6b4a10, metalness: .2, roughness: .4 }));
|
||||
m.position.z = (AXLEN - HEADL) / 2;
|
||||
m.userData.role = 'roll'; // 抓杆身 = 滚转
|
||||
return m;
|
||||
})();
|
||||
const arrowRing = (function () {
|
||||
const m = new THREE.Mesh(new THREE.TorusGeometry(10, 2.8, 10, 40),
|
||||
new THREE.MeshStandardMaterial({ color: 0xff8fb0, emissive: 0x5c1f38, metalness: .2, roughness: .4 }));
|
||||
m.rotation.x = Math.PI / 2; // 环面垂直于轴
|
||||
m.position.z = GRIPZ;
|
||||
m.userData.role = 'roll';
|
||||
return m;
|
||||
})();
|
||||
const arrowHead = (function () {
|
||||
const g = new THREE.ConeGeometry(7.5, HEADL, 18);
|
||||
g.rotateX(Math.PI / 2);
|
||||
const m = new THREE.Mesh(g,
|
||||
new THREE.MeshStandardMaterial({ color: 0xffd166, emissive: 0x7a5a10, metalness: .2, roughness: .35 }));
|
||||
m.position.z = HEADZ;
|
||||
m.userData.role = 'head';
|
||||
return m;
|
||||
})();
|
||||
gArrow.add(arrowShaft, arrowRing, arrowHead);
|
||||
const axisViewer = () => { // 目标工具轴在查看器系的方向（单位）
|
||||
const R = Rdes(), v = CAD2V([R[2], R[5], R[8]]); // 行主序：列 2 = 工具轴（朝臂外）
|
||||
return new THREE.Vector3(v[0], v[1], v[2]).normalize();
|
||||
};
|
||||
/* 姿态把手拾取返回 'axis' | 'roll' | null（同时不抢黄球的位置拖动）：
|
||||
· 箭头头与侧球都做「屏幕就近」检测，谁近听谁的（箭头正对镜头时两者投影会靠近）；
|
||||
· 都够近时黄球优先交回位置拖动；再往后才是箭杆/捏环/箭头锥体的几何射线。 */
|
||||
const knobScreen = () => {
|
||||
const p = knob.getWorldPosition(new THREE.Vector3()).project(cam);
|
||||
const r = cv.getBoundingClientRect();
|
||||
return { x: (p.x * .5 + .5) * r.width, y: (-p.y * .5 + .5) * r.height };
|
||||
};
|
||||
function pickPoseHandle(e) {
|
||||
const pt = projTgt();
|
||||
const db = Math.hypot(e.offsetX - pt.x, e.offsetY - pt.y);
|
||||
const hp = projArrow(HEADZ);
|
||||
const dh = Math.hypot(e.offsetX - hp.x, e.offsetY - hp.y);
|
||||
const kp = knobScreen();
|
||||
const dk = Math.hypot(e.offsetX - kp.x, e.offsetY - kp.y);
|
||||
if (dh < 22 && (dk >= 26 || dh <= dk) && dh <= db) return 'axis';
|
||||
if (dk < 26 && dk <= db) return 'roll';
|
||||
if (db < 26) return null; // 黄球抓取区：交回位置拖动
|
||||
ray2.setFromCamera(ndc2(e), cam);
|
||||
for (const h of ray2.intersectObjects([arrowShaft, arrowRing, arrowHead, knob], false)) {
|
||||
const r = h.object.userData.role;
|
||||
if (r === 'head') return 'axis';
|
||||
if (r === 'roll') return 'roll';
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
const linkGeo = new THREE.BufferGeometry();
|
||||
linkGeo.setAttribute('position', new THREE.BufferAttribute(new Float32Array(6), 3));
|
||||
const linkLine = new THREE.Line(linkGeo,
|
||||
@@ -76,6 +186,23 @@ function refreshTarget() {
|
||||
pa.needsUpdate = true;
|
||||
const d = Math.hypot(w.x - a.x, w.y - a.y, w.z - a.z);
|
||||
ballTcp.material.color.setHex(d < 1.2 ? 0x3ddc97 : 0xffb454);
|
||||
/* 目标箭头（指向工具轴）与当前工具轴线（绿，对照看姿态差） */
|
||||
gArrow.visible = poseOn && VIEW === 'motion';
|
||||
if (poseOn) {
|
||||
const R = Rdes();
|
||||
gArrow.quaternion.setFromUnitVectors(new THREE.Vector3(0, 0, 1), axisViewer());
|
||||
const xv = CAD2V([R[0], R[3], R[6]]); // 行主序：列 0 = 工具 x 轴；侧球沿它侧偏，始终离开箭头
|
||||
knob.position.set(xv[0] * KNOBOFF, xv[1] * KNOBOFF, xv[2] * KNOBOFF);
|
||||
}
|
||||
knob.visible = poseOn && VIEW === 'motion';
|
||||
const Rcur = S.orient(ORDER.map(id => ang[id]));
|
||||
const avc = CAD2V([Rcur[2], Rcur[5], Rcur[8]]); // 求解器列 2 = 法兰法向（朝臂内），取反为外伸方向
|
||||
const av = [-avc[0], -avc[1], -avc[2]];
|
||||
const ca = curGeo.attributes.position;
|
||||
ca.array[0] = w.x; ca.array[1] = w.y; ca.array[2] = w.z;
|
||||
ca.array[3] = w.x + av[0] * 60; ca.array[4] = w.y + av[1] * 60; ca.array[5] = w.z + av[2] * 60;
|
||||
ca.needsUpdate = true;
|
||||
curLine.visible = VIEW === 'motion';
|
||||
requestRender();
|
||||
}
|
||||
|
||||
@@ -117,6 +244,38 @@ function animTo(q, ms) {
|
||||
|
||||
/* ---------- 4. 结果渲染 ---------- */
|
||||
const badge = $('ikBadge');
|
||||
/* 场景图矩阵链复算姿态偏差（度）：
|
||||
· 场景侧：直接取 J6 组世界旋转作用在工具 x/z 轴上的方向（transformDirection，
|
||||
免去列主序手工换算），正交化后组装成查看器系旋转 Rs；
|
||||
· 目标侧：CAD 系旋转 Rtar 经坐标旋转 Rv（CAD→查看器）共轭到查看器系
|
||||
R_viewer = Rv·R_cad·Rvᵀ（单边变换列向量不够——基变了整个旋转都要转）。 */
|
||||
const RV3 = [1, 0, 0, 0, 0, 1, 0, -1, 0]; // CAD→查看器 3×3（行主序）
|
||||
const RV3T = [1, 0, 0, 0, 0, -1, 0, 1, 0]; // 转置
|
||||
function sceneOrientErr(q, Rtar) {
|
||||
const save = ORDER.map(id => ang[id]);
|
||||
for (let i = 0; i < 6; i++) { ang[ORDER[i]] = q[i]; applyJoint(ORDER[i]); }
|
||||
scene.updateMatrixWorld(true);
|
||||
const M = JG.J6.matrixWorld;
|
||||
const zs = new THREE.Vector3(0, 0, 1).transformDirection(M);
|
||||
let xs = new THREE.Vector3(1, 0, 0).transformDirection(M);
|
||||
xs.sub(zs.clone().multiplyScalar(xs.dot(zs))).normalize(); // 正交化（防场景缩放引入的分量）
|
||||
const ys = new THREE.Vector3().crossVectors(zs, xs);
|
||||
const Rs = [xs.x, ys.x, zs.x, xs.y, ys.y, zs.y, xs.z, ys.z, zs.z]; // 列 = 工具 x/y/z（查看器系）
|
||||
const Rt = []; // 目标：共轭到查看器系
|
||||
for (let i = 0; i < 3; i++) for (let j = 0; j < 3; j++) {
|
||||
let s = 0;
|
||||
for (let k = 0; k < 3; k++) s += RV3[i * 3 + k] * Rtar[k * 3 + j];
|
||||
Rt.push(s);
|
||||
}
|
||||
const Rt2 = [];
|
||||
for (let i = 0; i < 3; i++) for (let j = 0; j < 3; j++) {
|
||||
let s = 0;
|
||||
for (let k = 0; k < 3; k++) s += Rt[i * 3 + k] * RV3T[k * 3 + j];
|
||||
Rt2.push(s);
|
||||
}
|
||||
for (let i = 0; i < 6; i++) { ang[ORDER[i]] = save[i]; applyJoint(ORDER[i]); }
|
||||
return ArmIK.rotErrDeg(Rs, Rt2);
|
||||
}
|
||||
function renderResult(res, src) {
|
||||
const q = res.q || [0, 0, 0, 0, 0, 0];
|
||||
const tb = $('ikTab').tBodies[0]; tb.innerHTML = '';
|
||||
@@ -127,37 +286,57 @@ function renderResult(res, src) {
|
||||
'<td class="num">' + q[i].toFixed(2) + '\u00B0</td>' +
|
||||
'<td class="num dim">' + Math.abs(q[i]).toFixed(1) + '</td>' +
|
||||
'<td class="dim">' + (t.cont ? '\u00B1180' : t.lo + '..' + t.hi) + '</td>';
|
||||
if (i === 5 && !res.tool) tr.title = 'TCP 位于 J6 轴线上，转动 J6 不改变 TCP 位置，故取 0 使总角最小';
|
||||
if (i === 5 && !res.tool) tr.title = poseOn
|
||||
? '姿态控制开启：θ6 为绕工具轴的滚转角，由目标姿态（方位/仰角/滚转）解算'
|
||||
: 'TCP 位于 J6 轴线上，转动 J6 不改变 TCP 位置，故取 0 使总角最小';
|
||||
tb.appendChild(tr);
|
||||
}
|
||||
lastTool = res.tool || 0;
|
||||
const dcl = res.err != null ? res.err : 0;
|
||||
const dori = res.oerr != null ? res.oerr : 0;
|
||||
let doriSc = 0;
|
||||
const sc = (function () { // 用场景图真实矩阵链复算
|
||||
const save = ORDER.map(id => ang[id]);
|
||||
for (let i = 0; i < 6; i++) { ang[ORDER[i]] = q[i]; applyJoint(ORDER[i]); }
|
||||
scene.updateMatrixWorld(true);
|
||||
const w = new THREE.Vector3(...VC(tcpCad())).applyMatrix4(JG.J6.matrixWorld);
|
||||
if (poseOn) doriSc = sceneOrientErr(q, Rsolver());
|
||||
for (let i = 0; i < 6; i++) { ang[ORDER[i]] = save[i]; applyJoint(ORDER[i]); }
|
||||
return [w.x, -w.z, w.y];
|
||||
})();
|
||||
const dsc = Math.hypot(sc[0] - res.target[0], sc[1] - res.target[1], sc[2] - res.target[2]);
|
||||
const sum = q.reduce((a, b) => a + b, 0);
|
||||
const pa = poseOn ? ArmIK.poseAng(Uof(q)) : null;
|
||||
$('ikSum').innerHTML =
|
||||
'<i>总关节角 \u03A3|\u03B8|</i><b class="hi">' + S.costOf(q, null).toFixed(2) + '\u00B0</b>' +
|
||||
'<i>\u03A3\u03B8 代数和</i><span>' + sum.toFixed(1) + '\u00B0</span>' +
|
||||
'<i>残差 闭式/场景图</i><span class="' + (dcl < 1e-4 && dsc < .05 ? 'good' : 'bad') + '">' +
|
||||
dcl.toExponential(1) + ' / ' + dsc.toExponential(1) + ' mm</span>' +
|
||||
(poseOn ? '<i>目标姿态</i><span>方位 ' + POSE.az.toFixed(1) + '\u00B0 · 仰角 ' + POSE.el.toFixed(1) +
|
||||
'\u00B0 · 滚转 ' + POSE.roll.toFixed(1) + '\u00B0</span>' : '') +
|
||||
(poseOn ? '<i>实际姿态</i><span>' + pa.az.toFixed(1) + '\u00B0 · ' + pa.el.toFixed(1) +
|
||||
'\u00B0 · ' + pa.roll.toFixed(1) + '\u00B0</span>' : '') +
|
||||
(poseOn ? '<i>姿态残差 闭式/场景图</i><span class="' + (dori < .05 && doriSc < .1 ? 'good' : 'bad') + '">' +
|
||||
dori.toFixed(3) + '\u00B0 / ' + doriSc.toFixed(3) + '\u00B0</span>' : '') +
|
||||
'<i>可行采样点</i><span>' + res.nSol + '</span>' +
|
||||
'<i>耗时</i><span>' + res.ms.toFixed(0) + ' ms</span>' +
|
||||
'<i>目标</i><span>(' + res.target.map(v => v.toFixed(1)).join(', ') + ')</span>' +
|
||||
(res.resid != null ? '<i>距目标</i><span class="bad">' + res.resid.toFixed(2) + ' mm</span>' : '');
|
||||
const ok = res.reachable && dcl < 1e-3;
|
||||
badge.textContent = ok ? '已到达' : (res.reachable ? '校验偏差' : '不可达·最近位姿');
|
||||
badge.className = 'badge ' + (ok ? 'goodb' : (res.reachable ? 'warnb' : 'badb'));
|
||||
(res.resid != null ? '<i>距目标</i><span class="bad">' + res.resid.toFixed(2) + ' mm</span>' : '') +
|
||||
(poseOn && res.resid != null && res.oerr != null
|
||||
? '<i>姿态偏差</i><span class="bad">' + res.oerr.toFixed(2) + '\u00B0</span>' : '');
|
||||
const oriOk = !poseOn || (dori < 0.05 && doriSc < 0.1);
|
||||
/* 以残差判到达：阻尼最小二乘兜底也可能精确到达（扫描漏掉的奇异刀尖解），不能只看 reachable */
|
||||
const posOk = dsc < .05;
|
||||
badge.textContent = (posOk && oriOk) ? '已到达'
|
||||
: (posOk && poseOn && dori > 0.05 ? '位置到达·姿态残差 ' + dori.toFixed(2) + '\u00B0' : '不可达·最近位姿');
|
||||
badge.className = 'badge ' + ((posOk && oriOk) ? 'goodb'
|
||||
: (posOk ? 'warnb' : 'badb'));
|
||||
$('ikSrc').textContent = src;
|
||||
$('ikBar').innerHTML = '<b class="hi">\u03A3|\u03B8| ' + S.costOf(q, null).toFixed(2) + '\u00B0</b>' +
|
||||
'<span class="sep">|</span>\u6B8B\u5DEE ' +
|
||||
'<span class="' + (dsc < .05 ? 'good' : 'bad') + '">' + dsc.toExponential(1) + ' mm</span>' +
|
||||
(poseOn ? '<span class="sep">|</span>\u59FF\u6001\u6B8B\u5DEE <span class="' +
|
||||
(dori < .05 && doriSc < .1 ? 'good' : 'bad') + '">' + dori.toFixed(3) + '\u00B0</span>' : '') +
|
||||
'<span class="sep">|</span>\u53EF\u884C\u91C7\u6837\u70B9 ' + res.nSol +
|
||||
'<span class="sep">|</span>' + res.ms.toFixed(0) + ' ms';
|
||||
/* 备选互异构型 */
|
||||
@@ -196,10 +375,13 @@ function runSolve() {
|
||||
refreshTarget();
|
||||
badge.textContent = '计算中…'; badge.className = 'badge wait';
|
||||
setTimeout(() => {
|
||||
const opt = { tool: lastTool, step: +$('ikStep').value || 0.25 };
|
||||
const opt = { tool: lastTool, step: +$('ikStep').value || 0.25, ref: curRef(), refW: REF_W };
|
||||
if (poseOn) opt.pose = { R: Rsolver() };
|
||||
const res = S.solveTo(TGT, opt);
|
||||
renderResult(res, res.reachable ? '闭式全域扫描 · 冗余维精确覆盖'
|
||||
: '超工作空间 → 阻尼最小二乘最近位姿');
|
||||
renderResult(res, res.reachable
|
||||
? (poseOn ? '闭式全域扫描 · 位置+姿态联合 · 冗余维精确覆盖' : '闭式全域扫描 · 冗余维精确覆盖')
|
||||
: (poseOn ? '超可达位姿空间 → 阻尼最小二乘最近位姿（位置 mm + 姿态联合）'
|
||||
: '超工作空间 → 阻尼最小二乘最近位姿'));
|
||||
if (res.q) setJoints(res.q, true);
|
||||
}, 0);
|
||||
}
|
||||
@@ -213,9 +395,14 @@ function runSolveLive() {
|
||||
liveBusy = true;
|
||||
try {
|
||||
lastTool = +$('ikTool').value || 0;
|
||||
const res = S.solveTo(TGT, { tool: lastTool, step: +$('ikStep').value || 0.25 });
|
||||
renderResult(res, res.reachable ? '实时追踪 · 闭式全域扫描 · 冗余维精确覆盖'
|
||||
: '实时追踪 · 超工作空间（阻尼最小二乘最近位姿）');
|
||||
const opt = { tool: lastTool, step: +$('ikStep').value || 0.25, ref: curRef(), refW: REF_W };
|
||||
if (poseOn) opt.pose = { R: Rsolver() };
|
||||
const res = S.solveTo(TGT, opt);
|
||||
renderResult(res, res.reachable
|
||||
? (poseOn ? '实时追踪 · 闭式全域扫描 · 位置+姿态联合 · 冗余维精确覆盖'
|
||||
: '实时追踪 · 闭式全域扫描 · 冗余维精确覆盖')
|
||||
: (poseOn ? '实时追踪 · 超可达位姿空间（阻尼最小二乘最近位姿）'
|
||||
: '实时追踪 · 超工作空间（阻尼最小二乘最近位姿）'));
|
||||
if (res.q) setJoints(res.q, false);
|
||||
} finally { liveBusy = false; }
|
||||
if (livePending) { livePending = false; runSolveLive(); }
|
||||
@@ -231,31 +418,94 @@ function setMode(m) {
|
||||
mode = m;
|
||||
$('ikPick').classList.toggle('on', m === 'pick');
|
||||
$('ikRt').classList.toggle('on', m === 'rt');
|
||||
$('ikHint').textContent = m === 'pick' ? '视图单击：目标投到当前高度的水平面'
|
||||
: (m === 'rt' ? '实时追踪中：拖动黄球即算即动，手臂跟着目标走（松手精修）'
|
||||
: '拖动黄球 = 移动目标（Shift 仅上下）；求解在松手后自动执行');
|
||||
$('ikHint').textContent = (m === 'pick' ? '视图单击：目标投到当前高度的水平面'
|
||||
: (m === 'rt' ? '实时追踪中：拖黄球/箭头即算即动，手臂跟着走（松手精修）'
|
||||
: '拖黄球 = 移动目标（Shift 仅上下）；拖箭头 = 调工具轴指向；拖箭杆或粉球 = 滚转；松手自动求解'))
|
||||
+ (poseOn ? ' · 姿态控制中：箭头/粉球已激活，数字框可精确微调' : ' · 开「姿态控制」后出现可拖箭头');
|
||||
}
|
||||
function projTgt() {
|
||||
const p = gTgt.position.clone().project(cam);
|
||||
const r = cv.getBoundingClientRect();
|
||||
return { x: (p.x * .5 + .5) * r.width, y: (-p.y * .5 + .5) * r.height };
|
||||
}
|
||||
/* 箭头头部 / 滚转圆环的世界坐标 → 屏幕（用于命中判定） */
|
||||
function projArrow(localZ) {
|
||||
const p = new THREE.Vector3(0, 0, localZ).applyMatrix4(gArrow.matrixWorld).project(cam);
|
||||
const r = cv.getBoundingClientRect();
|
||||
return { x: (p.x * .5 + .5) * r.width, y: (-p.y * .5 + .5) * r.height };
|
||||
}
|
||||
const ndc2 = e => {
|
||||
const r = cv.getBoundingClientRect();
|
||||
return new THREE.Vector2(((e.clientX - r.left) / r.width) * 2 - 1,
|
||||
-((e.clientY - r.top) / r.height) * 2 + 1);
|
||||
};
|
||||
/* 指针在「目标处、垂直于工具轴的平面」上绕轴的转角（度）：滚转把手拖动用 */
|
||||
function rollAngleAt(e) {
|
||||
const n = axisViewer();
|
||||
const pl = new THREE.Plane().setFromNormalAndCoplanarPoint(n, gTgt.position);
|
||||
const hit = new THREE.Vector3();
|
||||
ray2.setFromCamera(ndc2(e), cam); // 该版本 setFromCamera 不返回 this，勿链式
|
||||
if (!ray2.ray.intersectPlane(pl, hit)) return 0;
|
||||
const v = hit.sub(gTgt.position);
|
||||
const up = Math.abs(n.y) > 0.9 ? new THREE.Vector3(1, 0, 0) : new THREE.Vector3(0, 1, 0);
|
||||
const u = new THREE.Vector3().crossVectors(n, up).normalize();
|
||||
const w = new THREE.Vector3().crossVectors(n, u);
|
||||
return Math.atan2(v.dot(w), v.dot(u)) * 180 / Math.PI;
|
||||
}
|
||||
/* 弧球式瞄准：指针射线与「以目标为球心、半径 = 箭长」的球求交，方向 = 交点 − 球心 */
|
||||
function aimAxis(e) {
|
||||
ray2.setFromCamera(ndc2(e), cam);
|
||||
const o = ray2.ray.origin, d = ray2.ray.direction, C = gTgt.position;
|
||||
const ox = o.x - C.x, oy = o.y - C.y, oz = o.z - C.z;
|
||||
const b = ox * d.x + oy * d.y + oz * d.z;
|
||||
const c = ox * ox + oy * oy + oz * oz - AXLEN * AXLEN;
|
||||
const disc = b * b - c;
|
||||
const t = Math.max(0, disc >= 0 ? -b - Math.sqrt(disc) : -b); // 未命中取射线上距球心最近点
|
||||
const v = new THREE.Vector3(o.x + d.x * t - C.x, o.y + d.y * t - C.y, o.z + d.z * t - C.z);
|
||||
if (v.lengthSq() < 1e-6) return;
|
||||
v.normalize();
|
||||
const vc = V2CAD([v.x, v.y, v.z]);
|
||||
POSE.az = Math.atan2(vc[1], vc[0]) * 180 / Math.PI;
|
||||
POSE.el = Math.acos(Math.max(-1, Math.min(1, vc[2]))) * 180 / Math.PI;
|
||||
syncPoseInputs(); refreshTarget();
|
||||
}
|
||||
cv.addEventListener('pointerdown', e => {
|
||||
if (e.button !== 0 || VIEW !== 'motion') return; // 仅运动模拟模式可操作目标点
|
||||
justPicked = false;
|
||||
if (mode === 'pick') { grab = { pick: true }; dragged = false; return; }
|
||||
const pt = projTgt();
|
||||
if (!(Math.hypot(e.offsetX - pt.x, e.offsetY - pt.y) < 26 || e.shiftKey)) return;
|
||||
const db = Math.hypot(e.offsetX - pt.x, e.offsetY - pt.y);
|
||||
if (poseOn) { // 箭头头 / 滚转把手（与黄球就近仲裁，见 pickPoseHandle）
|
||||
const part = pickPoseHandle(e);
|
||||
if (part === 'axis') { grab = { pose: 'axis' }; dragged = false; camSnap = snapCam(); return; }
|
||||
if (part === 'roll') { grab = { pose: 'roll', a0: rollAngleAt(e) }; dragged = false; camSnap = snapCam(); return; }
|
||||
}
|
||||
if (!(db < 26 || e.shiftKey)) return;
|
||||
grab = { sx: e.offsetX, sy: e.offsetY };
|
||||
camSnap = snapCam(); dragged = false;
|
||||
});
|
||||
cv.addEventListener('pointermove', e => {
|
||||
if (!grab || grab.pick) return;
|
||||
if (camSnap) putCam(camSnap); // 撤销查看器自身的轨道 / 平移
|
||||
cam.updateMatrixWorld(); // 轨道参数已还原，射线/列向量要用新矩阵
|
||||
if (grab.pose === 'axis') { // 拖箭头头：工具轴指向
|
||||
dragged = true;
|
||||
aimAxis(e);
|
||||
if (mode === 'rt') runSolveLive();
|
||||
return;
|
||||
}
|
||||
if (grab.pose === 'roll') { // 拖圆环：绕轴滚转
|
||||
dragged = true;
|
||||
const a1 = rollAngleAt(e);
|
||||
POSE.roll = S.wrap180(POSE.roll + S.wrap180(a1 - grab.a0));
|
||||
grab.a0 = a1;
|
||||
syncPoseInputs(); refreshTarget();
|
||||
if (mode === 'rt') runSolveLive();
|
||||
return;
|
||||
}
|
||||
const dx = e.offsetX - grab.sx, dy = e.offsetY - grab.sy;
|
||||
if (Math.abs(dx) + Math.abs(dy) > 2) dragged = true;
|
||||
if (camSnap) putCam(camSnap); // 撤销查看器自身的轨道 / 平移
|
||||
const k = (2 * rd * Math.tan(cam.fov * Math.PI / 360)) / Math.max(1, cv.clientHeight);
|
||||
cam.updateMatrixWorld();
|
||||
const right = new THREE.Vector3().setFromMatrixColumn(cam.matrixWorld, 0);
|
||||
const up = new THREE.Vector3().setFromMatrixColumn(cam.matrixWorld, 1);
|
||||
const cur = VC(TGT), v = new THREE.Vector3(cur[0], cur[1], cur[2]);
|
||||
@@ -317,6 +567,32 @@ $('ikHome').onclick = () => {
|
||||
};
|
||||
$('ikZero').onclick = () => { TGT = [150, 128, 260]; syncInputs(); refreshTarget(); runSolve(); };
|
||||
|
||||
/* --- 姿态控制：开关 / 参数 / 快捷 --- */
|
||||
const poseEls = ['ikAz', 'ikEl', 'ikRoll'].map($);
|
||||
if ($('ikPose')) $('ikPose').onclick = () => {
|
||||
poseOn = !poseOn;
|
||||
$('ikPose').classList.toggle('on', poseOn);
|
||||
$('ikPoseBox').style.display = poseOn ? 'flex' : 'none';
|
||||
if (poseOn) fillPoseFromCurrent(); // 开启即用当前工具姿态，避免求解跳变
|
||||
refreshTarget();
|
||||
setMode(mode);
|
||||
runSolve();
|
||||
};
|
||||
poseEls.forEach((el, i) => {
|
||||
if (!el) return;
|
||||
el.addEventListener('change', () => {
|
||||
POSE.az = +$('ikAz').value || 0;
|
||||
POSE.el = Math.max(0, Math.min(180, +$('ikEl').value || 0));
|
||||
POSE.roll = +$('ikRoll').value || 0;
|
||||
syncPoseInputs();
|
||||
if (poseOn) runSolve();
|
||||
});
|
||||
});
|
||||
if ($('ikPoseCur')) $('ikPoseCur').onclick = () => { fillPoseFromCurrent(); if (poseOn) runSolve(); };
|
||||
if ($('ikPoseDown')) $('ikPoseDown').onclick = () => {
|
||||
POSE.az = 0; POSE.el = 180; POSE.roll = 0; syncPoseInputs(); if (poseOn) runSolve();
|
||||
};
|
||||
|
||||
/* ---------- 8. 视图模式：三维模型（静态） / 装配图 / 运动模拟 ---------- */
|
||||
let VIEW = 'model';
|
||||
const MOTION_UI = ['ikbar', 'ikpanel', 'joints', 'jointsHead', 'footIk'];
|
||||
@@ -330,6 +606,7 @@ function setView(v, keepPose) {
|
||||
}
|
||||
gTgt.visible = isMotion; RING.visible = isMotion;
|
||||
ballTcp.visible = isMotion; linkLine.visible = isMotion;
|
||||
curLine.visible = isMotion;
|
||||
if (isMotion) {
|
||||
if (+exp.value) document.getElementById('bAsm').click(); // 运动模拟须在装配状态下进行
|
||||
runSolve(); // 进入即求解并动画到目标点
|
||||
@@ -344,7 +621,7 @@ document.querySelectorAll('.tab[data-view]').forEach(b =>
|
||||
|
||||
/* ---------- 9. 启动：自检 + 工作空间采样 ---------- */
|
||||
function boot() {
|
||||
const st = S.selfTest(600);
|
||||
const st = S.selfTest(600, 6);
|
||||
const ws = (function () {
|
||||
let lo = [1e9, 1e9, 1e9], hi = [-1e9, -1e9, -1e9], rmax = 0;
|
||||
for (let k = 0; k < 120000; k++) {
|
||||
@@ -368,7 +645,11 @@ function boot() {
|
||||
' mm（前伸 ' + ws.hi[0].toFixed(0) + '）</span>' +
|
||||
'<i>可达 y*</i><span>' + ws.lo[1].toFixed(0) + '…' + ws.hi[1].toFixed(0) + ' mm</span>' +
|
||||
'<i>可达 z*</i><span>' + ws.lo[2].toFixed(0) + '…' + ws.hi[2].toFixed(0) + ' mm</span>' +
|
||||
'<i>离 J1 轴*</i><span class="good">最远 ' + ws.rmax.toFixed(0) + ' mm</span>';
|
||||
'<i>离 J1 轴*</i><span class="good">最远 ' + ws.rmax.toFixed(0) + ' mm</span>' +
|
||||
'<i>姿态 IK 校验</i><span class="' + (st.poseHit >= st.poseTried * 0.9 ? 'good' : 'bad') + '">' +
|
||||
st.poseHit + '/' + st.poseTried + ' 命中 · 最大 ' + st.poseErr.toExponential(1) +
|
||||
'° · ' + st.poseMs.toFixed(0) + ' ms/次</span>' +
|
||||
'<i>姿态结构</i><span>J5∩J6 垂直相交于腕心；θ6 为绕工具轴滚转，不影响 TCP 位置</span>';
|
||||
/* 打包自检：确认所有依赖都在本文件内（便于单文件分发，防止被拆开后才出问题） */
|
||||
const ext = document.querySelectorAll(
|
||||
'script[src],link[href],img[src],iframe[src],source[src],embed[src],object[data],use[href]').length;
|
||||
@@ -400,5 +681,15 @@ boot();
|
||||
|
||||
window.IK = { solver: S, setTarget: p => { TGT = p.slice(); syncInputs(); refreshTarget(); },
|
||||
getTarget: () => TGT.slice(), run: runSolve, TCP0: TCP0, LIM: LIM,
|
||||
setView: setView, getView: () => VIEW };
|
||||
setView: setView, getView: () => VIEW,
|
||||
setPose: (az, el, roll, solve) => {
|
||||
POSE.az = +az || 0; POSE.el = Math.max(0, Math.min(180, +el || 0)); POSE.roll = +roll || 0;
|
||||
syncPoseInputs(); refreshTarget(); if (solve !== false) runSolve();
|
||||
},
|
||||
getPose: () => ({ on: poseOn, az: POSE.az, el: POSE.el, roll: POSE.roll }),
|
||||
/* 可拖控点的屏幕坐标（画布内像素）：黄球 / 箭头头 / 滚转侧球 */
|
||||
pickPts: () => poseOn ? { ball: projTgt(), head: projArrow(HEADZ), knob: knobScreen() } : null,
|
||||
/* 调试：某视口坐标下的把手拾取结果（'axis'/'roll'/null） */
|
||||
_pickHandle: (cx, cy) => poseOn ? pickPoseHandle({ clientX: cx, clientY: cy,
|
||||
offsetX: cx - cv.getBoundingClientRect().left, offsetY: cy - cv.getBoundingClientRect().top }) : null };
|
||||
})();
|
||||
Reference in New Issue
Block a user