feat: 新增IK实时求解与运动模拟功能，集成正逆运动学查看器，补充定位图截图
This commit is contained in:
+197
-6
@@ -9,7 +9,7 @@
|
||||
html,body{margin:0;height:100%;background:var(--bg);color:var(--tx);
|
||||
font:13px/1.55 "Microsoft YaHei","Segoe UI",system-ui,sans-serif;overflow:hidden;
|
||||
-webkit-user-select:none;user-select:none}
|
||||
#app{display:grid;grid-template-columns:274px minmax(430px,1fr);grid-template-rows:46px 1fr 26px;
|
||||
#app{display:grid;grid-template-columns:322px minmax(430px,1fr);grid-template-rows:46px auto 1fr 26px;
|
||||
height:100%;min-width:0;overflow-x:auto}
|
||||
header{grid-column:1/-1;display:flex;align-items:center;gap:8px;padding:0 12px;flex-wrap:nowrap;
|
||||
overflow-x:auto;overflow-y:hidden;background:var(--pan);border-bottom:1px solid var(--line)}
|
||||
@@ -22,7 +22,7 @@ button:hover{border-color:var(--ac);color:#fff}
|
||||
button.on{background:#22406b;border-color:var(--ac)}
|
||||
aside{background:var(--pan);overflow:auto;padding:8px;min-width:0}
|
||||
#left{border-right:1px solid var(--line)}
|
||||
#app.l{grid-template-columns:274px minmax(430px,1fr)}
|
||||
#app.l{grid-template-columns:322px minmax(430px,1fr)}
|
||||
#stage{position:relative;overflow:hidden;min-width:0;
|
||||
background:radial-gradient(120% 120% at 50% 0%,#1b2028 0%,#0d0f13 70%)}
|
||||
canvas{display:block;width:100%;height:100%}
|
||||
@@ -30,7 +30,7 @@ footer{grid-column:1/-1;background:var(--pan);border-top:1px solid var(--line);
|
||||
display:flex;align-items:center;gap:14px;padding:0 12px;color:var(--dim);font-size:12px;
|
||||
white-space:nowrap;overflow:hidden}
|
||||
@media (max-width:1150px){
|
||||
#app{grid-template-columns:250px minmax(400px,1fr)}
|
||||
#app{grid-template-columns:300px minmax(400px,1fr)}
|
||||
}
|
||||
@media (max-width:760px){
|
||||
#app{grid-template-columns:minmax(0,1fr)}
|
||||
@@ -96,6 +96,95 @@ small{color:var(--dim)}
|
||||
#screws tbody tr.scrHl{background:#1d3f74;box-shadow:inset 3px 0 0 var(--warn)}
|
||||
.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}
|
||||
#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}
|
||||
#ikbar input[type=number]{width:68px;background:#0d1015;color:var(--tx);border:1px solid var(--line);
|
||||
border-radius:4px;padding:3px 5px;font:inherit;font-variant-numeric:tabular-nums;flex:none}
|
||||
#ikbar input[type=number]:focus{outline:none;border-color:var(--ac)}
|
||||
#ikbar .vsep{width:1px;height:20px;background:var(--line);flex:none;margin:0 3px}
|
||||
#ikbar .st{color:var(--dim);flex:none;font-size:11.5px;font-variant-numeric:tabular-nums}
|
||||
#ikbar .sep{color:#3b4453;margin:0 5px}
|
||||
#ikbar .hint{color:var(--dim);font-size:11px;flex:none}
|
||||
#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}
|
||||
|
||||
/* ===================== IK 结果面板（左栏顶部） ===================== */
|
||||
#ikpanel{margin:0 0 10px;border:1px solid var(--line);border-radius:6px;background:#141821;padding:7px 8px}
|
||||
#ikpanel .ikh{display:flex;align-items:center;gap:6px;margin-bottom:5px}
|
||||
#ikpanel .ikh h3{margin:0;font-size:12px;color:var(--ac);letter-spacing:.3px}
|
||||
.badge{margin-left:auto;font-size:10.5px;padding:1px 7px;border-radius:9px;border:1px solid;flex:none;
|
||||
white-space:nowrap}
|
||||
.badge.wait{color:var(--dim);border-color:var(--line)}
|
||||
.badge.goodb{color:#0c1a12;background:var(--ok);border-color:var(--ok)}
|
||||
.badge.warnb{color:#241a06;background:var(--warn);border-color:var(--warn)}
|
||||
.badge.badb{color:#2a0a0a;background:#ff6b6b;border-color:#ff6b6b}
|
||||
#ikTab{font-size:11px}
|
||||
#ikTab td.num{text-align:right;font-variant-numeric:tabular-nums;white-space:nowrap}
|
||||
#ikTab td.dim{color:var(--dim);white-space:nowrap}
|
||||
#ikpanel .kv{margin-top:6px}
|
||||
#ikpanel .kv i{white-space:nowrap}
|
||||
#ikpanel .kv span,#ikpanel .kv b{overflow-wrap:anywhere;word-break:break-word;min-width:0}
|
||||
#ikpanel .good{color:var(--ok)} #ikpanel .bad{color:#ff6b6b}
|
||||
#ikpanel .kv .hi{color:var(--warn);font-variant-numeric:tabular-nums}
|
||||
#ikAltN{font-size:10.5px;color:var(--dim);margin:7px 0 3px;line-height:1.45;white-space:normal}
|
||||
.altrow{display:flex;align-items:center;gap:5px;padding:2px 4px;border-radius:4px;cursor:pointer;
|
||||
font-size:11px;font-variant-numeric:tabular-nums;color:var(--dim)}
|
||||
.altrow:hover{background:rgba(76,154,255,.16)}
|
||||
.altrow .ai{color:#5b6478;width:10px;flex:none;text-align:right}
|
||||
.altrow b{color:var(--ac);width:54px;flex:none;text-align:right;font-weight:500}
|
||||
.altrow .aq{overflow:hidden;text-overflow:ellipsis;white-space:nowrap}
|
||||
#ikpanel details{margin-top:8px;border-top:1px solid var(--line);padding-top:6px}
|
||||
#ikpanel details>summary{cursor:pointer;color:var(--ac);font-size:11.5px;outline:none}
|
||||
#ikpanel details .doc{margin-top:5px;font-size:11px;color:var(--dim);line-height:1.7;white-space:normal;
|
||||
-webkit-user-select:text;user-select:text}
|
||||
#ikpanel details .doc b{color:var(--tx)}
|
||||
#ikpanel details .doc code{color:var(--warn);font-family:Consolas,"Courier New",monospace;font-size:10.5px}
|
||||
#ikpanel details .doc ol{margin:3px 0 3px 16px;padding:0}
|
||||
#ikpanel details .doc li{margin:3px 0}
|
||||
#ikpanel details .doc hr{border:none;border-top:1px solid var(--line);margin:6px 0}
|
||||
#ikpanel .src{font-size:10.5px;color:var(--dim);margin-top:5px;white-space:normal;line-height:1.5}
|
||||
#ikChkBox{margin-top:7px}
|
||||
#ikChkBox summary{margin-bottom:2px}
|
||||
|
||||
/* ============ 视图模式：三维模型（静态） / 装配图 / 运动模拟 ============ */
|
||||
/* 三维模型 与 装配图 下，全部运动学 UI 一律隐藏 */
|
||||
body:not(.motion) #ikbar,
|
||||
body:not(.motion) #ikpanel,
|
||||
body:not(.motion) #joints,
|
||||
body:not(.motion) #jointsHead,
|
||||
body:not(.motion) #footIk{display:none!important}
|
||||
/* 运动模拟只看运动：左栏去掉零件树与打印清单（其它模式保留） */
|
||||
body.motion #treeBlock{display:none!important}
|
||||
|
||||
/* 运动模拟左栏紧凑化：目标一屏放下、不下拉（实测原需下拉 127px） */
|
||||
#ikpanel{margin:0 0 6px;padding:5px 8px}
|
||||
#ikpanel .ikh{margin-bottom:3px}
|
||||
#ikpanel #ikTab th,#ikpanel #ikTab td{padding:1px 4px}
|
||||
#ikAltN{margin:5px 0 2px}
|
||||
.altrow{padding:1px 4px}
|
||||
#joints .j-box{margin-bottom:4px}
|
||||
#joints input[type=range]{height:15px}
|
||||
/* 显式行定位：IK 控制条在「三维模型/装配图」下被移除后，
|
||||
若不固定行号，网格自动排布会把页脚挤到 1fr 行并压扁（已实测） */
|
||||
#app>header{grid-row:1}
|
||||
#ikbar{grid-row:2}
|
||||
#left{grid-row:3}
|
||||
#stage{grid-row:3}
|
||||
#app>footer{grid-row:4}
|
||||
/* BOM 抽屉：运动模拟多一条 IK 控制条，需整体下移 40px */
|
||||
#bomDock{top:46px} #bomTab{top:64px}
|
||||
body.motion #bomDock{top:86px} body.motion #bomTab{top:100px}
|
||||
@media (max-width:760px){
|
||||
#bomTab,body.motion #bomTab{top:auto;bottom:36px}
|
||||
#bomDock,body.motion #bomDock{top:86px;bottom:26px}
|
||||
}
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
@@ -104,7 +193,7 @@ small{color:var(--dim)}
|
||||
<b id="ttl">人形机器人手臂</b>
|
||||
<span class="q" id="ver"></span>
|
||||
<button class="tab on" data-view="model">三维模型</button>
|
||||
<button class="tab" data-view="sheet">装配图</button>
|
||||
<button class="tab" data-view="sheet">装配图</button><button class="tab" data-view="motion">运动模拟</button>
|
||||
<span class="sp"></span>
|
||||
<button id="bAsm" class="on">装配视图</button>
|
||||
<button id="bExp">爆炸视图</button>
|
||||
@@ -122,18 +211,118 @@ small{color:var(--dim)}
|
||||
<button id="bL" title="显示/隐藏左栏">左栏</button>
|
||||
</header>
|
||||
|
||||
<div id="ikbar">
|
||||
<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">
|
||||
<span class="lb">Z</span><input type="number" id="ikZ" step="1" title="CAD 世界坐标 Z / mm（桌面 z=0，向上为正）">
|
||||
<span class="vsep"></span>
|
||||
<span class="lb">工具长</span><input type="number" id="ikTool" value="0" step="5" title="在工具法兰面之外、沿 J6 轴线再伸出的工具长度 / mm">
|
||||
<span class="lb">mm</span>
|
||||
<span class="vsep"></span>
|
||||
<span class="lb">扫描精度</span>
|
||||
<select id="ikStep" title="冗余维 (J4,J5) 的扫描步长：越小越接近全局最优，耗时越长">
|
||||
<option value="1">快速 1°</option>
|
||||
<option value="0.5">较快 0.5°</option>
|
||||
<option value="0.25" selected>标准 0.25°</option>
|
||||
<option value="0.1">精细 0.1°</option>
|
||||
<option value="0.05">极高 0.05°</option>
|
||||
</select>
|
||||
<button id="ikSolve" title="求解并把手臂动画移动到该点（取 Σ|θ| 最小的构型）">求解并移动</button>
|
||||
<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="vsep"></span>
|
||||
<span class="st" id="ikBar">—</span>
|
||||
</div>
|
||||
|
||||
|
||||
<aside id="left">
|
||||
<div class="row" style="justify-content:space-between;margin:8px 0 4px">
|
||||
<div id="ikpanel">
|
||||
<div class="ikh">
|
||||
<h3>逆运动学 · 最小总关节角</h3>
|
||||
<span id="ikBadge" class="badge wait">待求解</span>
|
||||
</div>
|
||||
<table id="ikTab">
|
||||
<thead><tr><th>轴</th><th>关节</th><th>角度</th><th>|θ|</th><th>限位</th></tr></thead>
|
||||
<tbody></tbody>
|
||||
</table>
|
||||
<div class="kv" id="ikSum"></div>
|
||||
<div class="src" id="ikSrc"></div>
|
||||
<div id="ikAltN"></div>
|
||||
<div id="ikAlts"></div>
|
||||
<details id="ikChkBox"><summary>结构与精度自检 / 工作空间</summary><div class="kv" id="ikChk"></div>
|
||||
<div class="kv" id="ikDep"></div></details>
|
||||
<details id="ikDoc">
|
||||
<summary>算法说明与公式推导</summary>
|
||||
<div class="doc">
|
||||
<b>模式</b>：本页「<b>运动模拟</b>」承载全部运动学能力；「三维模型」为纯静态模型视图（关节锁定在设计零位、无目标点与求解控件），「装配图」为二维图纸。运动模拟下左栏只保留 IK 结果与关节角度，零件树/打印清单自动收走。<hr>
|
||||
<b>目标</b>：使 TCP（末端工具法兰 P09 安装面中心，可加“工具长”）到达给定点，并在一切满足限位的可行构型中取
|
||||
<b>Σ|θᵢ| 最小</b>者。
|
||||
<ol>
|
||||
<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>，
|
||||
则 <code>X = X0 + a</code>，而 <code>(Y, Z)</code> 由 <code>(b, c)</code> 绕 J1 轴旋转 φ 得到：<br>
|
||||
<code>a = Vx·cosθ3·cosθ2 + Vz·sinθ2</code>，<code>b = −Vx·sinθ3</code>，
|
||||
<code>c = C0 + (−Vx·cosθ3·sinθ2 + Vz·cosθ2)</code>，
|
||||
其中 <code>Vx, Vz</code> 只是 (θ4, θ5) 的函数。</li>
|
||||
<li><b>两式联立消元（关键一步）</b>：由 <code>a = A</code> 得恒等式
|
||||
<code>A² + n² = Vx² + Vz²</code>（<code>n = −Vx·cosθ3·sinθ2 + Vz·cosθ2</code>）；
|
||||
代入 <code>b² + c² = R²</code> 后 θ3 的项<u>全部抵消</u>，于是<br>
|
||||
<code>n = (R² + A² − Vx² − Vz² − C0²) / (2·C0)</code> —— <b>唯一确定</b>；<br>
|
||||
再由 <code>n² + A² = Vx²·cos²θ3 + Vz²</code> 得
|
||||
<code>cos²θ3 = (n² + A² − Vz²) / Vx²</code> ⇒ θ3 共四支 <code>±σ, ±(π−σ)</code>；<br>
|
||||
最后 θ2 与 θ1 各由一个 2×2 线性方程组唯一确定：
|
||||
<code>cosθ2 = (κA + Vz·n)/(κ² + Vz²)</code>，<code>sinθ2 = (Vz·A − κ·n)/(κ² + Vz²)</code>，
|
||||
<code>κ = Vx·cosθ3</code>；<code>θ1 = φ − atan2(c, b)</code>。</li>
|
||||
<li><b>遍历冗余</b>：(θ4, θ5) 就是二维冗余。在 (θ4, θ5) 的限位盒内<b>全域扫描</b>（区间端点必采，含上下限位），
|
||||
每个网格点闭式求出全部支 ⇒ <b>覆盖整个可行解族</b>；再对最优的若干互异候选做 h/8 局部加密 + 模式搜索，
|
||||
得到 <b>全局最小 Σ|θ|</b>，误差量级 ≤ 扫描步长。</li>
|
||||
<li><b>代表角</b>：J1 / J3 / J6 为 ±180° 整周轴，取 |θ| 最小的等价角；J2 / J4 / J5 有限位，越限即丢弃。</li>
|
||||
<li><b>自检</b>：每次求解都用闭式 FK 与场景图真实 6 级矩阵链各算一次 TCP 并与目标比对（见上表残差）。</li>
|
||||
</ol>
|
||||
<hr>
|
||||
<b>验证数据（本机实测，可复现）</b>
|
||||
<table>
|
||||
<thead><tr><th>检查项</th><th>结果</th></tr></thead>
|
||||
<tbody>
|
||||
<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>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>
|
||||
</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>。
|
||||
</div>
|
||||
</details>
|
||||
</div>
|
||||
|
||||
<div class="row" id="jointsHead" style="justify-content:space-between;margin:8px 0 4px">
|
||||
<h3 style="margin:0">关节角度（实时联动）</h3>
|
||||
<button id="resetJoints" class="tab" style="padding:1px 7px;font-size:11px" title="将所有关节角度复位为 0°">全部归零</button>
|
||||
</div>
|
||||
<div id="joints"></div>
|
||||
<div id="treeBlock">
|
||||
<h3>零件树</h3>
|
||||
<div class="row"><button id="allOn" class="tab">全选</button><button id="allOff" class="tab">全不选</button>
|
||||
<button id="onlyPrint" class="tab">只看打印件</button></div>
|
||||
<div id="tree"></div>
|
||||
<h3>打印清单（PLA）</h3>
|
||||
<div class="kv" id="sum"></div>
|
||||
</div>
|
||||
</aside>
|
||||
|
||||
<div id="stage">
|
||||
@@ -144,7 +333,7 @@ small{color:var(--dim)}
|
||||
</div>
|
||||
|
||||
<footer>
|
||||
<span>左键旋转 · 滚轮缩放 · 右键平移（拖动） · 单击选中高亮 · <b>BOM 点螺丝行 = 高亮定位该规格螺钉（穿透显示）</b> · <b>右键单击隐藏部件（或按H键）</b> · 左侧全选/勾选恢复</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>
|
||||
@@ -823,5 +1012,7 @@ requestAnimationFrame(loop);
|
||||
requestRender();
|
||||
})();
|
||||
</script>
|
||||
<script>/*__IKSOLVER__*/</script>
|
||||
<script>/*__IKGLUE__*/</script>
|
||||
</body>
|
||||
</html>
|
||||
|
||||
Reference in New Issue
Block a user