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:
+60
-16
@@ -97,10 +97,11 @@ small{color:var(--dim)}
|
||||
.hudlink{pointer-events:auto;cursor:pointer;color:var(--ac);text-decoration:underline;margin-left:8px}
|
||||
@media (max-width:760px){#bomTab{top:auto;bottom:36px}#bomDock{top:46px;bottom:26px}}
|
||||
|
||||
/* ===================== IK 控制条（新增） ===================== */
|
||||
#ikbar{grid-column:1/-1;display:flex;align-items:center;gap:6px;padding:0 10px;height:40px;
|
||||
background:linear-gradient(180deg,#1c212b,#161922);border-bottom:1px solid var(--line);
|
||||
overflow-x:auto;overflow-y:hidden;white-space:nowrap;flex-wrap:nowrap}
|
||||
/* ===================== IK 控制条（两行：目标+姿态 / 模式+状态） ===================== */
|
||||
#ikbar{grid-column:1/-1;display:flex;flex-direction:column;gap:4px;padding:5px 10px;
|
||||
background:linear-gradient(180deg,#1c212b,#161922);border-bottom:1px solid var(--line)}
|
||||
#ikbar .ikrow{display:flex;align-items:center;gap:6px;min-height:26px;
|
||||
overflow-x:auto;overflow-y:hidden;white-space:nowrap}
|
||||
#ikbar .ik-t{color:var(--ac);font-weight:700;letter-spacing:.8px;flex:none;font-size:11.5px;
|
||||
border:1px solid var(--ac);border-radius:4px;padding:1px 6px}
|
||||
#ikbar .lb{color:var(--dim);flex:none;font-size:12px}
|
||||
@@ -114,6 +115,9 @@ small{color:var(--dim)}
|
||||
#ikbar .good{color:var(--ok)} #ikbar .bad{color:#ff6b6b} #ikbar .hi{color:var(--warn)}
|
||||
#ikSolve{background:#1d3f74;border-color:var(--ac);color:#fff;font-weight:600}
|
||||
#ikSolve:hover{background:#24508f}
|
||||
#ikPose.on{background:#3a2b63;border-color:#a06bff;color:#fff;font-weight:600}
|
||||
#ikPoseBox{display:none;align-items:center;gap:6px}
|
||||
#ikPoseBox input[type=number]{width:54px}
|
||||
|
||||
/* ===================== IK 结果面板（左栏顶部） ===================== */
|
||||
#ikpanel{margin:0 0 10px;border:1px solid var(--line);border-radius:6px;background:#141821;padding:7px 8px}
|
||||
@@ -178,9 +182,9 @@ body.motion #treeBlock{display:none!important}
|
||||
#left{grid-row:3}
|
||||
#stage{grid-row:3}
|
||||
#app>footer{grid-row:4}
|
||||
/* BOM 抽屉：运动模拟多一条 IK 控制条，需整体下移 40px */
|
||||
/* BOM 抽屉：运动模拟多一条两行 IK 控制条（实测 75px），需整体下移 */
|
||||
#bomDock{top:46px} #bomTab{top:64px}
|
||||
body.motion #bomDock{top:86px} body.motion #bomTab{top:100px}
|
||||
body.motion #bomDock{top:124px} body.motion #bomTab{top:140px}
|
||||
@media (max-width:760px){
|
||||
#bomTab,body.motion #bomTab{top:auto;bottom:36px}
|
||||
#bomDock,body.motion #bomDock{top:86px;bottom:26px}
|
||||
@@ -212,6 +216,7 @@ body.motion #bomDock{top:86px} body.motion #bomTab{top:100px}
|
||||
</header>
|
||||
|
||||
<div id="ikbar">
|
||||
<div class="ikrow">
|
||||
<span class="ik-t">IK</span>
|
||||
<span class="lb">目标点 X</span><input type="number" id="ikX" step="1" title="CAD 世界坐标 X / mm">
|
||||
<span class="lb">Y</span><input type="number" id="ikY" step="1" title="CAD 世界坐标 Y / mm">
|
||||
@@ -228,22 +233,33 @@ body.motion #bomDock{top:86px} body.motion #bomTab{top:100px}
|
||||
<option value="0.1">精细 0.1°</option>
|
||||
<option value="0.05">极高 0.05°</option>
|
||||
</select>
|
||||
<button id="ikSolve" title="求解并把手臂动画移动到该点（取 Σ|θ| 最小的构型）">求解并移动</button>
|
||||
<button id="ikSolve" title="求解并把手臂动画移动到该位姿（取 Σ|θ| 最小的构型）">求解并移动</button>
|
||||
<button id="ikPose" title="开启姿态控制：除 TCP 位置外，同时指定末端工具姿态（工具轴指向 + 绕轴滚转）">姿态控制</button>
|
||||
<span id="ikPoseBox">
|
||||
<span class="lb">方位</span><input type="number" id="ikAz" step="5" value="0" title="工具轴（法兰朝外方向）绕 CAD Z 轴的方位角 / 度">
|
||||
<span class="lb">仰角</span><input type="number" id="ikEl" step="5" value="90" title="工具轴（法兰朝外方向）自 CAD +Z 抬起的仰角 / 度：0 = 竖直向上，90 = 水平，180 = 竖直向下">
|
||||
<span class="lb">滚转</span><input type="number" id="ikRoll" step="5" value="0" title="绕工具轴自身的滚转角 / 度（右手系）">
|
||||
<button id="ikPoseCur" title="把当前真实工具姿态读回三个输入框">取当前</button>
|
||||
<button id="ikPoseDown" title="快捷设为工具轴竖直向下（爪取常用姿态）">垂直向下</button>
|
||||
</span>
|
||||
</div>
|
||||
<div class="ikrow">
|
||||
<button id="ikPick" title="开启后在视图中单击，把目标点投到当前高度的水平面上">视图拾取</button>
|
||||
<button id="ikRt" title="开启后拖动黄球即算即动：手臂实时追踪目标点移动（松手做一次精修求解）">实时追踪</button>
|
||||
<button id="ikHome" title="所有关节角归零">归零位</button>
|
||||
<button id="ikZero" title="载入一个示例目标并求解">示例目标</button>
|
||||
<span class="vsep"></span>
|
||||
<span class="hint" id="ikHint">拖动黄球 = 移动目标（Shift 约束竖直）；松手自动求解</span>
|
||||
<span class="hint" id="ikHint">拖黄球 = 移动目标（Shift 仅上下）；拖箭头 = 调工具轴指向；拖箭杆或粉球 = 滚转；松手自动求解</span>
|
||||
<span class="vsep"></span>
|
||||
<span class="st" id="ikBar">—</span>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
|
||||
<aside id="left">
|
||||
<div id="ikpanel">
|
||||
<div class="ikh">
|
||||
<h3>逆运动学 · 最小总关节角</h3>
|
||||
<h3>逆运动学 · 位置与姿态 · 最小总关节角</h3>
|
||||
<span id="ikBadge" class="badge wait">待求解</span>
|
||||
</div>
|
||||
<table id="ikTab">
|
||||
@@ -261,9 +277,16 @@ body.motion #bomDock{top:86px} body.motion #bomTab{top:100px}
|
||||
<div class="doc">
|
||||
<b>模式</b>：本页「<b>运动模拟</b>」承载全部运动学能力；「三维模型」为纯静态模型视图（关节锁定在设计零位、无目标点与求解控件），「装配图」为二维图纸。运动模拟下左栏只保留 IK 结果与关节角度，零件树/打印清单自动收走。<hr>
|
||||
<b>目标</b>：使 TCP（末端工具法兰 P09 安装面中心，可加“工具长”）到达给定点，并在一切满足限位的可行构型中取
|
||||
<b>Σ|θᵢ| 最小</b>者。
|
||||
<b>Σ|θᵢ| 最小</b>者（交互连续求解时叠加参考位姿连续项，见第 6 条）。开启上方“<b>姿态控制</b>”后，
|
||||
约束升级为 <b>位置 3 + 姿态 3</b>（工具轴指向 + 绕轴滚转），见第 5 条。
|
||||
<br><b>姿态操控</b>：姿态开启后目标球处出现方向箭头与粉色侧球——<b>箭头指向即工具朝外方向</b>（法兰面法向朝外，也就是工具伸出的方向）。
|
||||
<b>拖箭头头部</b>用弧球法（指针射线与以目标为球心、半径＝箭长的球求交，方向＝交点−球心）连续调整方位/俯仰；
|
||||
<b>拖箭杆或粉色侧球</b>按指针绕轴的转角增量改滚转；右侧数字框随时同步，可精确微调，
|
||||
「取当前」把真实工具姿态读回、「垂直向下」一键设为爪取常用姿态。
|
||||
<br><b>约定</b>：求解器内部工具坐标系 z 轴取法兰面法向（朝臂内，与 TCP 外伸反向）；界面上的方位/仰角/滚转、
|
||||
箭头与绿色当前轴线均以朝外法向描述，入栈时绕工具 x 轴翻 180° 换算，两者看到的物理姿态完全一致。
|
||||
<ol>
|
||||
<li><b>先消去一维</b>：TCP 恰好落在 J6 轴线上，绕 J6 转动不改变 TCP 位置 ⇒ 对“到点”任务取
|
||||
<li><b>先消去一维</b>（仅位置任务）：TCP 恰好落在 J6 轴线上，绕 J6 转动不改变 TCP 位置 ⇒ 对“到点”任务取
|
||||
<code>θ6 = 0</code> 即已最优。</li>
|
||||
<li><b>识别结构</b>：J2 / J4 / J5 三轴严格平行（轴向 Y，过 x = 18.5），J1 轴为 X，J3 轴沿臂长方向 Z。
|
||||
记 <code>A = X−X0</code>、<code>R = |(Y−Y0, Z−Z1)|</code>、<code>φ = atan2(Z−Z1, Y−Y0)</code>，
|
||||
@@ -283,8 +306,23 @@ body.motion #bomDock{top:86px} body.motion #bomTab{top:100px}
|
||||
<li><b>遍历冗余</b>：(θ4, θ5) 就是二维冗余。在 (θ4, θ5) 的限位盒内<b>全域扫描</b>（区间端点必采，含上下限位），
|
||||
每个网格点闭式求出全部支 ⇒ <b>覆盖整个可行解族</b>；再对最优的若干互异候选做 h/8 局部加密 + 模式搜索，
|
||||
得到 <b>全局最小 Σ|θ|</b>，误差量级 ≤ 扫描步长。</li>
|
||||
<li><b>姿态任务（6 自由度）</b>：工具坐标系姿态 <code>R = R₁·R₂·…·R₆</code>（与场景图 6 级矩阵链一致）。
|
||||
结构上 <b>J5 轴与 J6 轴在腕心 (18.5, 128, 222) 垂直相交</b>，因此 θ6 只提供绕工具自身轴的滚转：
|
||||
<u>工具轴指向 = (R₁…R₅)·ẑ，与 θ6 无关，TCP 位置也与 θ6 无关</u>。每条位置闭式支都附带确定的 R₅
|
||||
（= R₁…R₅），滚转角随之闭式解出：<code>N = R₅ᵀ·R_des</code>（需为 Rz(−θ6)）⇒
|
||||
<code>θ6 = −atan2(N₁₀, N₀₀)</code>；姿态偏差 <code>e = ∠(R₅·Rz(−θ6), R_des)</code> ≤ 2° 的支才收入。</li>
|
||||
<li><b>姿态达标优先</b>：达标解在 (θ4,θ5) 平面上是一条斜窄谷（奇异邻域灵敏度可达 ~100°/°），
|
||||
按 Σ|θ| 取种子会整体错过它。达标带（e ≤ 1°）按 e 升序取种子、局部窗口加密、必要时 escalating
|
||||
（姿态惩罚 ×100 重扫），最后做 <b>二维牛顿收口</b>：对“工具轴在目标轴法平面上的切向偏差”迭代，
|
||||
每步在新 (θ4,θ5) 上重解闭式位置支（按 θ1..θ3 最近匹配取同支，位置天然保持精确），把 e 压到 ~10⁻³° 量级。
|
||||
<b>最终只在 e ≤ 0.05° 的达标候选里按含连续项代价选优——不为省几度关节角牺牲姿态精度</b>；
|
||||
确有 ~5% 的均匀随机姿型落在奇异“刀尖”上（姿态仅在奇异点精确可达），此时按最近位姿返回并如实报告残差。</li>
|
||||
<li><b>连续优先（防倒摆）</b>：等价多分支（θ3 的 ±σ/±(π−σ)、肘上/肘下）之间只按 Σ|θ| 选优时，目标微动就会在分支间
|
||||
跳来跳去，某轴大幅倒摆（实测 4mm 步长过可达边界最大跳 81.9°）。交互求解以<b>当前手臂姿态</b>为参考位姿，
|
||||
代价加入 <code>λ·Σwrap(θᵢ−refᵢ)²</code>（λ=0.02）：同族分支被锁定，仅当近支越限或误差超差才换支。
|
||||
A/B 实测（4 条扫描路径）：>10° 跳变 14→5 次，最大跳变 163.5°→16.8°，平均 Σ|θ| 不升反降。</li>
|
||||
<li><b>代表角</b>：J1 / J3 / J6 为 ±180° 整周轴，取 |θ| 最小的等价角；J2 / J4 / J5 有限位，越限即丢弃。</li>
|
||||
<li><b>自检</b>：每次求解都用闭式 FK 与场景图真实 6 级矩阵链各算一次 TCP 并与目标比对（见上表残差）。</li>
|
||||
<li><b>自检</b>：每次求解都用闭式 FK 与场景图真实 6 级矩阵链各算一次 TCP 和工具姿态并与目标比对（见上表残差）。</li>
|
||||
</ol>
|
||||
<hr>
|
||||
<b>验证数据（本机实测，可复现）</b>
|
||||
@@ -294,18 +332,24 @@ body.motion #bomDock{top:86px} body.motion #bomTab{top:100px}
|
||||
<tr><td>闭式 FK vs 6 级矩阵链（1500 组随机位形）</td><td class="good">4.6 × 10⁻¹³ mm</td></tr>
|
||||
<tr><td>逆解分支重构（200 组随机可行位形）</td><td class="good">200 / 200 命中</td></tr>
|
||||
<tr><td>θ6 不变性（0° → 137°，TCP 位移）</td><td class="good">0.00 mm</td></tr>
|
||||
<tr><td>姿态参数 ↔ 旋转矩阵往返（2000 组随机）</td><td class="good">2.4 × 10⁻⁶°</td></tr>
|
||||
<tr><td>姿态重构达标率（200 组随机可行位形，0.5° 步长）</td><td class="good">185–191 / 200（4–7% 奇异刀尖构型残差 ≤ 2°，如实报告）</td></tr>
|
||||
<tr><td>滚转角交互 roll 0→90（参考位姿连续）</td><td class="good">Δθ6 = 89.99°，θ1..θ5 变化 < 0.01°</td></tr>
|
||||
<tr><td>60 个可达目标：失败次数 / 最大残差</td><td class="good">0 次 / 3.5 × 10⁻¹³ mm</td></tr>
|
||||
<tr><td>相对 0.05° 极细网格参考的最大劣化</td><td class="good">0.02°</td></tr>
|
||||
<tr><td>vs 4000 次随机多起点数值解</td><td class="good">0 / 20 次劣于数值解</td></tr>
|
||||
<tr><td>单次求解耗时（默认 0.25°）</td><td>约 0.1 s（1° ≈ 30 ms，0.05° ≈ 1.1 s）</td></tr>
|
||||
<tr><td>等价分支跳变 · 4 条扫描路径（修复前 → 后）</td><td class="good">>10° 跳变 14 → 5 次；最大单步 163.5° → 16.8°</td></tr>
|
||||
<tr><td>单次求解耗时（默认 0.25°）</td><td>约 0.1 s（1° ≈ 30 ms，0.05° ≈ 1.1 s；开姿态后同级或更快）</td></tr>
|
||||
</tbody>
|
||||
</table>
|
||||
<hr>
|
||||
<b>限位</b>：J2 ∈ [−106, 9]，J4 ∈ [−111, 16]，J5 ∈ [−95, 34]，J1 / J3 / J6 为 ±180°（可整周）。
|
||||
<br><b>局限</b>：本算法为纯运动学求解，只受各关节行程限位约束，<u>不含跨关节自碰撞校核</u>。
|
||||
原模型限位来自全对全动态扫描；若需要更舒展的姿态，可从上方“互异构型”列表改选次优解。
|
||||
<br><b>不可达</b>：若目标超出工作空间，自动改用阻尼最小二乘求最近可达位姿并标注距离。
|
||||
<br><b>API</b>：控制台可用 <code>IK.solver.solve([x,y,z],{step:0.25,tool:0,weights:[1,1,1,1,1,1]})</code>。
|
||||
<br><b>不可达</b>：若目标超出工作空间，自动求最近可达位姿——阻尼最小二乘<b>从当前姿态锚定起步</b>（抑制退化方向的无用漂移）、固定种子的确定性重启（同一点重复解算逐位一致）、再经零空间抛光取最小总关节角的代表值；报告的距离为抛光后真实值。
|
||||
<br><b>姿态不可达</b>：姿态任务超可达空间时，阻尼最小二乘误差向量升级为 6 维（位置 mm + 姿态 deg，按 20 mm/rad 折算），返回位置与姿态联合最近的位姿，两个残差分别显示。
|
||||
<br><b>API</b>：控制台可用 <code>IK.solver.solve([x,y,z],{step:0.25,tool:0,pose:{R:ArmIK.poseMat(az,el,roll)}})</code>；
|
||||
<code>IK.setPose(az,el,roll)</code> 可直接设定目标姿态。
|
||||
</div>
|
||||
</details>
|
||||
</div>
|
||||
@@ -333,7 +377,7 @@ body.motion #bomDock{top:86px} body.motion #bomTab{top:100px}
|
||||
</div>
|
||||
|
||||
<footer>
|
||||
<span>左键旋转 · 滚轮缩放 · 右键平移（拖动） · 单击选中高亮 · <b>BOM 点螺丝行 = 高亮定位该规格螺钉（穿透显示）</b> · <b>右键单击隐藏部件（或按H键）</b> · 左侧全选/勾选恢复<span id="footIk"> · <b>运动模拟</b>：拖动黄色目标球到点（Shift 仅上下）· 松手自动求解</span></span>
|
||||
<span>左键旋转 · 滚轮缩放 · 右键平移（拖动） · 单击选中高亮 · <b>BOM 点螺丝行 = 高亮定位该规格螺钉（穿透显示）</b> · <b>右键单击隐藏部件（或按H键）</b> · 左侧全选/勾选恢复<span id="footIk"> · <b>运动模拟</b>：拖动黄色目标球到点（Shift 仅上下）· 开「姿态控制」后可拖箭头调末端朝向、拖箭杆或粉色侧球调滚转</span></span>
|
||||
<span class="sp"></span>
|
||||
<span id="stat"></span>
|
||||
</footer>
|
||||
|
||||
Reference in New Issue
Block a user