feat(arm): 电机双向夹紧与2mm厚度公差让位，立柱减重槽及文档同步更新
This commit is contained in:
@@ -0,0 +1,101 @@
|
||||
<svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 980 700" font-family="'Segoe UI','PingFang SC','Microsoft YaHei',sans-serif">
|
||||
<rect x="0" y="0" width="980" height="700" fill="#ffffff"/>
|
||||
<defs>
|
||||
<marker id="ar" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="7" markerHeight="7" orient="auto-start-reverse">
|
||||
<path d="M0,0 L10,5 L0,10 z" fill="#5a7d9a"/>
|
||||
</marker>
|
||||
<marker id="arb" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="7" markerHeight="7" orient="auto-start-reverse">
|
||||
<path d="M0,0 L10,5 L0,10 z" fill="#7c5cd6"/>
|
||||
</marker>
|
||||
</defs>
|
||||
<text x="490" y="34" text-anchor="middle" font-size="21" font-weight="bold" fill="#16324f">系统总览：一个事实源、两个碰撞模型、一条数据链</text>
|
||||
|
||||
<!-- 列标题 -->
|
||||
<text x="175" y="70" text-anchor="middle" font-size="15" font-weight="bold" fill="#1f6f43">① 建模侧（Python · CadQuery）</text>
|
||||
<text x="490" y="70" text-anchor="middle" font-size="15" font-weight="bold" fill="#8a5a00">② 数据契约（docs/*.json）</text>
|
||||
<text x="805" y="70" text-anchor="middle" font-size="15" font-weight="bold" fill="#1f4f8f">③ 网页查看器（单文件 HTML）</text>
|
||||
|
||||
<!-- ① 建模侧 -->
|
||||
<rect x="40" y="88" width="270" height="86" rx="8" fill="#eef8f1" stroke="#1f6f43" stroke-width="1.6"/>
|
||||
<text x="175" y="112" text-anchor="middle" font-size="15" font-weight="bold" fill="#14472c">human_layout.py</text>
|
||||
<text x="175" y="133" text-anchor="middle" font-size="12.5" fill="#2c5c44">唯一事实源：6 根关节轴 rear/d、</text>
|
||||
<text x="175" y="151" text-anchor="middle" font-size="12.5" fill="#2c5c44">连杆高度链、台架/机身界面</text>
|
||||
|
||||
<rect x="40" y="206" width="270" height="86" rx="8" fill="#eef8f1" stroke="#1f6f43" stroke-width="1.6"/>
|
||||
<text x="175" y="230" text-anchor="middle" font-size="15" font-weight="bold" fill="#14472c">arm_model.py · arm_build.py</text>
|
||||
<text x="175" y="251" text-anchor="middle" font-size="12.5" fill="#2c5c44">参数化 CAD：9 打印件 + 6 电机模组</text>
|
||||
<text x="175" y="269" text-anchor="middle" font-size="12.5" fill="#2c5c44">装配体 / STEP / STL / 质量属性</text>
|
||||
|
||||
<rect x="40" y="324" width="270" height="70" rx="8" fill="#f6f8fa" stroke="#8aa2b5" stroke-width="1.2"/>
|
||||
<text x="175" y="348" text-anchor="middle" font-size="13" font-weight="bold" fill="#3d5164">strength_check.py · torque_balance.py</text>
|
||||
<text x="175" y="368" text-anchor="middle" font-size="12" fill="#5c7185">同一关节表：截面刚度 / 重力力矩网格搜索</text>
|
||||
|
||||
<rect x="40" y="426" width="270" height="70" rx="8" fill="#fdf2ee" stroke="#b0562f" stroke-width="1.4"/>
|
||||
<text x="175" y="450" text-anchor="middle" font-size="13.5" font-weight="bold" fill="#7c3a1c">sweep_v2.py（离线权威碰撞）</text>
|
||||
<text x="175" y="470" text-anchor="middle" font-size="12" fill="#8a5433">多进程 + GJK 预筛 + OCCT 精确布尔</text>
|
||||
|
||||
<rect x="40" y="528" width="270" height="62" rx="8" fill="#fdf2ee" stroke="#b0562f" stroke-width="1.2" stroke-dasharray="5 3"/>
|
||||
<text x="175" y="552" text-anchor="middle" font-size="13" font-weight="bold" fill="#7c3a1c">scratch/fit_capsules.py</text>
|
||||
<text x="175" y="571" text-anchor="middle" font-size="12" fill="#8a5433">CAD 网格 → 实时胶囊模型拟合</text>
|
||||
|
||||
<line x1="175" y1="174" x2="175" y2="204" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#ar)"/>
|
||||
<line x1="175" y1="292" x2="175" y2="322" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#ar)"/>
|
||||
<line x1="175" y1="394" x2="175" y2="424" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#ar)"/>
|
||||
|
||||
<!-- ② 数据契约 -->
|
||||
<g>
|
||||
<rect x="365" y="426" width="250" height="56" rx="8" fill="#fff8ea" stroke="#c99418" stroke-width="1.4"/>
|
||||
<text x="490" y="449" text-anchor="middle" font-size="13.5" font-weight="bold" fill="#7a5a08">travel.json / travel_detail.json</text>
|
||||
<text x="490" y="468" text-anchor="middle" font-size="11.5" fill="#8f6d1c">单关节硬限位（滑块）+ 组合折叠明细</text>
|
||||
<rect x="365" y="528" width="250" height="56" rx="8" fill="#fff8ea" stroke="#c99418" stroke-width="1.4"/>
|
||||
<text x="490" y="551" text-anchor="middle" font-size="13.5" font-weight="bold" fill="#7a5a08">collide.json</text>
|
||||
<text x="490" y="570" text-anchor="middle" font-size="11.5" fill="#8f6d1c">1995 胶囊 + adj 机械区间 + 硬边标记</text>
|
||||
<rect x="365" y="324" width="250" height="56" rx="8" fill="#fff8ea" stroke="#c99418" stroke-width="1.4"/>
|
||||
<text x="490" y="347" text-anchor="middle" font-size="13.5" font-weight="bold" fill="#7a5a08">torque_balance.json · strength.json</text>
|
||||
<text x="490" y="366" text-anchor="middle" font-size="11.5" fill="#8f6d1c">逐关节力矩负载率 / 截面安全系数</text>
|
||||
<rect x="365" y="222" width="250" height="56" rx="8" fill="#fff8ea" stroke="#c99418" stroke-width="1.4"/>
|
||||
<text x="490" y="245" text-anchor="middle" font-size="13.5" font-weight="bold" fill="#7a5a08">mesh_arm.json（网格）</text>
|
||||
<text x="490" y="264" text-anchor="middle" font-size="11.5" fill="#8f6d1c">AB.export → build_viewer.py 打包</text>
|
||||
</g>
|
||||
|
||||
<line x1="310" y1="455" x2="363" y2="455" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#ar)"/>
|
||||
<line x1="310" y1="559" x2="363" y2="559" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#ar)"/>
|
||||
<line x1="310" y1="355" x2="363" y2="355" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#ar)"/>
|
||||
<line x1="310" y1="255" x2="363" y2="252" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#ar)"/>
|
||||
<text x="336" y="446" text-anchor="middle" font-size="11" fill="#5c7185">写出</text>
|
||||
<text x="336" y="550" text-anchor="middle" font-size="11" fill="#5c7185">写出</text>
|
||||
|
||||
<!-- ③ 查看器 -->
|
||||
<rect x="670" y="120" width="270" height="96" rx="8" fill="#edf3fc" stroke="#1f4f8f" stroke-width="1.6"/>
|
||||
<text x="805" y="145" text-anchor="middle" font-size="15" font-weight="bold" fill="#153a6e">arm-ik-solver.js（ArmIK）</text>
|
||||
<text x="805" y="166" text-anchor="middle" font-size="12.5" fill="#3a5a82">闭式支 + (θ4,θ5) 扫描 + 牛顿收口</text>
|
||||
<text x="805" y="184" text-anchor="middle" font-size="12.5" fill="#3a5a82">DLS 兜底 · 实时胶囊碰撞 · 动态限位</text>
|
||||
<text x="805" y="202" text-anchor="middle" font-size="12.5" fill="#3a5a82">纯 JS 无依赖 · 主线程 / Worker 同源</text>
|
||||
|
||||
<rect x="670" y="248" width="270" height="76" rx="8" fill="#edf3fc" stroke="#1f4f8f" stroke-width="1.6"/>
|
||||
<text x="805" y="273" text-anchor="middle" font-size="15" font-weight="bold" fill="#153a6e">ik-glue.js（交互层）</text>
|
||||
<text x="805" y="294" text-anchor="middle" font-size="12.5" fill="#3a5a82">拖拽 / 箭头姿态 / 实时追踪 / Worker</text>
|
||||
<text x="805" y="312" text-anchor="middle" font-size="12.5" fill="#3a5a82">滑块动态限位夹取（边界缓存）</text>
|
||||
|
||||
<rect x="670" y="356" width="270" height="70" rx="8" fill="#edf3fc" stroke="#1f4f8f" stroke-width="1.6"/>
|
||||
<text x="805" y="381" text-anchor="middle" font-size="15" font-weight="bold" fill="#153a6e">three.js 场景图</text>
|
||||
<text x="805" y="402" text-anchor="middle" font-size="12.5" fill="#3a5a82">6 级嵌套 pivot（与 FK 链逐位一致）</text>
|
||||
<text x="805" y="420" text-anchor="middle" font-size="12.5" fill="#3a5a82">FK 渲染 + 残差复核（双通道对账）</text>
|
||||
|
||||
<line x1="615" y1="460" x2="668" y2="168" stroke="#7c5cd6" stroke-width="1.8" marker-end="url(#arb)"/>
|
||||
<line x1="615" y1="556" x2="668" y2="300" stroke="#7c5cd6" stroke-width="1.8" marker-end="url(#arb)"/>
|
||||
<line x1="615" y1="250" x2="668" y2="392" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#ar)"/>
|
||||
<line x1="805" y1="216" x2="805" y2="246" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#ar)"/>
|
||||
<line x1="805" y1="324" x2="805" y2="354" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#ar)"/>
|
||||
<text x="636" y="508" font-size="11.5" fill="#7c5cd6">限位 / 碰撞注入求解器与 UI</text>
|
||||
<text x="621" y="262" font-size="11" fill="#5c7185" stroke="#ffffff" stroke-width="3" paint-order="stroke">几何网格</text>
|
||||
|
||||
<!-- human_layout → solver 直接注入 -->
|
||||
<path d="M 310 130 C 480 96, 560 100, 668 138" fill="none" stroke="#1f6f43" stroke-width="1.8" stroke-dasharray="6 4" marker-end="url(#ar)"/>
|
||||
<text x="470" y="84" text-anchor="middle" font-size="11.5" fill="#1f6f43">关节 rear/d 常量经 build_viewer 注入两处（CAD 与网页同源）</text>
|
||||
|
||||
<!-- 底部工作流 -->
|
||||
<rect x="40" y="620" width="900" height="56" rx="8" fill="#f4f6f8" stroke="#9db0c0" stroke-width="1.2"/>
|
||||
<text x="490" y="643" text-anchor="middle" font-size="12.5" fill="#3d5164">改几何的同步纪律：arm_model.py 重建 → AB.export 写 mesh_arm.json → build_viewer.py 重打包 → 渲染出图确认 →</text>
|
||||
<text x="490" y="663" text-anchor="middle" font-size="12.5" fill="#3d5164">几何变更必须重跑 sweep_v2.py（行程/碰撞权威）；collide.json 由 fit_capsules.py 重新拟合。</text>
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 8.8 KiB |
@@ -0,0 +1,125 @@
|
||||
<svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 980 760" font-family="'Segoe UI','PingFang SC','Microsoft YaHei',sans-serif">
|
||||
<rect x="0" y="0" width="980" height="760" fill="#ffffff"/>
|
||||
<defs>
|
||||
<marker id="axr" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
|
||||
<path d="M0,0 L10,5 L0,10 z" fill="#d64545"/>
|
||||
</marker>
|
||||
<marker id="axb" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
|
||||
<path d="M0,0 L10,5 L0,10 z" fill="#3572c9"/>
|
||||
</marker>
|
||||
<marker id="dim" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6" markerHeight="6" orient="auto-start-reverse">
|
||||
<path d="M0,0 L10,5 L0,10 z" fill="#8a6d3b"/>
|
||||
</marker>
|
||||
</defs>
|
||||
<text x="490" y="34" text-anchor="middle" font-size="21" font-weight="bold" fill="#16324f">运动链与关节轴（正视简化示意）</text>
|
||||
<text x="490" y="58" text-anchor="middle" font-size="13" fill="#5c7185">沿 −Y 看：X 向右、Z 向上、Y 指向纸内（⊗）。零位 θ=0，臂竖直下垂。</text>
|
||||
|
||||
<!-- 地面与台架 -->
|
||||
<line x1="70" y1="700" x2="910" y2="700" stroke="#8a9aa8" stroke-width="3"/>
|
||||
<rect x="150" y="676" width="290" height="24" fill="#dfe8f0" stroke="#7ba3c7" stroke-width="1.2"/>
|
||||
<rect x="228" y="80" width="52" height="598" fill="#dfe8f0" stroke="#7ba3c7" stroke-width="1.2"/>
|
||||
<rect x="276" y="78" width="288" height="26" fill="#dfe8f0" stroke="#7ba3c7" stroke-width="1.2"/>
|
||||
<text x="254" y="666" text-anchor="middle" font-size="11.5" fill="#5c7185">立柱 P01（台架）</text>
|
||||
<text x="295" y="694" text-anchor="middle" font-size="11.5" fill="#5c7185">底座 P02</text>
|
||||
|
||||
<!-- 臂主干 -->
|
||||
<rect x="592" y="86" width="16" height="522" rx="8" fill="#c3d2e0" stroke="#7ba3c7" stroke-width="1"/>
|
||||
<rect x="560" y="72" width="80" height="36" rx="8" fill="#dbe7f3" stroke="#7ba3c7" stroke-width="1.4"/>
|
||||
|
||||
<!-- J1: X 轴 -->
|
||||
<line x1="508" y1="90" x2="692" y2="90" stroke="#d64545" stroke-width="2.4" marker-start="url(#axr)" marker-end="url(#axr)"/>
|
||||
<circle cx="600" cy="90" r="7" fill="#7c5cd6"/>
|
||||
<text x="702" y="82" font-size="13" font-weight="bold" fill="#16324f">J1 肩抬举</text>
|
||||
<text x="702" y="100" font-size="12" fill="#5c7185">轴 +X（滚转，前后方向）</text>
|
||||
|
||||
<!-- J2: Y ⊗ -->
|
||||
<circle cx="600" cy="210" r="10" fill="#ffffff" stroke="#1f9d63" stroke-width="2.2"/>
|
||||
<line x1="593" y1="203" x2="607" y2="217" stroke="#1f9d63" stroke-width="2.2"/>
|
||||
<line x1="607" y1="203" x2="593" y2="217" stroke="#1f9d63" stroke-width="2.2"/>
|
||||
<text x="620" y="202" font-size="13" font-weight="bold" fill="#16324f">J2 肩俯仰</text>
|
||||
<text x="620" y="220" font-size="12" fill="#5c7185">轴 +Y（⊗，与 J4/J5 平行同向）</text>
|
||||
|
||||
<!-- J3: Z 自转 -->
|
||||
<line x1="600" y1="225" x2="600" y2="352" stroke="#3572c9" stroke-width="1.6" stroke-dasharray="7 5" marker-start="url(#axb)" marker-end="url(#axb)"/>
|
||||
<ellipse cx="600" cy="255" rx="26" ry="9" fill="none" stroke="#3572c9" stroke-width="1.6"/>
|
||||
<circle cx="600" cy="255" r="5.5" fill="#7c5cd6"/>
|
||||
<text x="640" y="250" font-size="13" font-weight="bold" fill="#16324f">J3 大臂自转</text>
|
||||
<text x="640" y="268" font-size="12" fill="#5c7185">轴 −Z（臂自身轴线）</text>
|
||||
|
||||
<!-- J4 -->
|
||||
<circle cx="600" cy="368" r="10" fill="#ffffff" stroke="#1f9d63" stroke-width="2.2"/>
|
||||
<line x1="593" y1="361" x2="607" y2="375" stroke="#1f9d63" stroke-width="2.2"/>
|
||||
<line x1="607" y1="361" x2="593" y2="375" stroke="#1f9d63" stroke-width="2.2"/>
|
||||
<text x="620" y="360" font-size="13" font-weight="bold" fill="#16324f">J4 肘部俯仰</text>
|
||||
<text x="620" y="378" font-size="12" fill="#5c7185">轴 +Y（⊗）</text>
|
||||
|
||||
<!-- J5 -->
|
||||
<circle cx="600" cy="478" r="10" fill="#ffffff" stroke="#1f9d63" stroke-width="2.2"/>
|
||||
<line x1="593" y1="471" x2="607" y2="485" stroke="#1f9d63" stroke-width="2.2"/>
|
||||
<line x1="607" y1="471" x2="593" y2="485" stroke="#1f9d63" stroke-width="2.2"/>
|
||||
<text x="620" y="470" font-size="13" font-weight="bold" fill="#16324f">J5 腕部俯仰</text>
|
||||
<text x="620" y="488" font-size="12" fill="#5c7185">轴 +Y（⊗，与 J4 同向 ⇒ R4·R5 可合并）</text>
|
||||
|
||||
<!-- J6 -->
|
||||
<line x1="600" y1="492" x2="600" y2="596" stroke="#3572c9" stroke-width="1.6" stroke-dasharray="7 5" marker-start="url(#axb)" marker-end="url(#axb)"/>
|
||||
<ellipse cx="600" cy="524" rx="24" ry="8" fill="none" stroke="#3572c9" stroke-width="1.6"/>
|
||||
<circle cx="600" cy="524" r="5.5" fill="#7c5cd6"/>
|
||||
<text x="640" y="520" font-size="13" font-weight="bold" fill="#16324f">J6 腕自转</text>
|
||||
<text x="640" y="538" font-size="12" fill="#5c7185">轴 −Z（滚转，TCP 在轴线上 ⇒ 不动位置）</text>
|
||||
|
||||
<!-- TCP -->
|
||||
<circle cx="600" cy="607" r="6" fill="#e08a1e"/>
|
||||
<text x="620" y="612" font-size="13" font-weight="bold" fill="#b06a10">TCP</text>
|
||||
<text x="620" y="630" font-size="11.5" fill="#5c7185">工具法兰安装面中心（J6 组打印件最低面）</text>
|
||||
|
||||
<!-- 左侧尺寸链 -->
|
||||
<g stroke="#8a6d3b" stroke-width="1.3">
|
||||
<line x1="480" y1="90" x2="480" y2="210" marker-start="url(#dim)" marker-end="url(#dim)"/>
|
||||
<line x1="480" y1="210" x2="480" y2="255" marker-start="url(#dim)" marker-end="url(#dim)"/>
|
||||
<line x1="480" y1="255" x2="480" y2="368" marker-start="url(#dim)" marker-end="url(#dim)"/>
|
||||
<line x1="480" y1="368" x2="480" y2="478" marker-start="url(#dim)" marker-end="url(#dim)"/>
|
||||
<line x1="480" y1="478" x2="480" y2="524" marker-start="url(#dim)" marker-end="url(#dim)"/>
|
||||
<line x1="470" y1="90" x2="490" y2="90"/><line x1="470" y1="210" x2="490" y2="210"/>
|
||||
<line x1="470" y1="255" x2="490" y2="255"/><line x1="470" y1="368" x2="490" y2="368"/>
|
||||
<line x1="470" y1="478" x2="490" y2="478"/><line x1="470" y1="524" x2="490" y2="524"/>
|
||||
</g>
|
||||
<g font-size="11.5" fill="#8a6d3b" text-anchor="end">
|
||||
<text x="472" y="154">C0 = 120</text>
|
||||
<text x="472" y="236">45</text>
|
||||
<text x="472" y="315">113</text>
|
||||
<text x="472" y="427">110（前臂）</text>
|
||||
<text x="472" y="505">46</text>
|
||||
</g>
|
||||
<g font-size="11" fill="#8a9aa8">
|
||||
<text x="498" y="122">z=610</text><text x="498" y="226">z=490</text><text x="498" y="271">z=445</text>
|
||||
<text x="498" y="384">z=332</text><text x="498" y="494">z=222</text><text x="498" y="540">z=176</text>
|
||||
</g>
|
||||
|
||||
<!-- 右侧段名 -->
|
||||
<g font-size="12" fill="#3d5164">
|
||||
<text x="800" y="154" text-anchor="middle">肩关节组</text>
|
||||
<line x1="770" y1="92" x2="770" y2="212" stroke="#b9c8d6" stroke-width="1.2"/>
|
||||
<text x="800" y="295" text-anchor="middle">大臂 158</text>
|
||||
<line x1="770" y1="212" x2="770" y2="370" stroke="#b9c8d6" stroke-width="1.2"/>
|
||||
<text x="800" y="428" text-anchor="middle">前臂 110</text>
|
||||
<line x1="770" y1="370" x2="770" y2="480" stroke="#b9c8d6" stroke-width="1.2"/>
|
||||
<text x="800" y="560" text-anchor="middle">腕组 46 + 法兰</text>
|
||||
<line x1="770" y1="480" x2="770" y2="606" stroke="#b9c8d6" stroke-width="1.2"/>
|
||||
</g>
|
||||
|
||||
<!-- 图例 -->
|
||||
<rect x="70" y="560" width="300" height="120" rx="8" fill="#f4f6f8" stroke="#9db0c0" stroke-width="1.2"/>
|
||||
<text x="86" y="586" font-size="13" font-weight="bold" fill="#16324f">图例</text>
|
||||
<line x1="86" y1="606" x2="126" y2="606" stroke="#d64545" stroke-width="2.4" marker-start="url(#axr)" marker-end="url(#axr)"/>
|
||||
<text x="136" y="610" font-size="12" fill="#3d5164">X 轴（水平，前后）</text>
|
||||
<circle cx="106" cy="630" r="8" fill="#ffffff" stroke="#1f9d63" stroke-width="2"/>
|
||||
<line x1="101" y1="625" x2="111" y2="635" stroke="#1f9d63" stroke-width="2"/>
|
||||
<line x1="111" y1="625" x2="101" y2="635" stroke="#1f9d63" stroke-width="2"/>
|
||||
<text x="136" y="634" font-size="12" fill="#3d5164">Y 轴（指向纸内）</text>
|
||||
<line x1="106" y1="648" x2="106" y2="668" stroke="#3572c9" stroke-width="2" marker-start="url(#axb)" marker-end="url(#axb)"/>
|
||||
<text x="136" y="662" font-size="12" fill="#3d5164">Z 轴（竖直，J3/J6 实为 −Z）</text>
|
||||
|
||||
<rect x="70" y="700" width="0" height="0" fill="none"/>
|
||||
<text x="490" y="730" text-anchor="middle" font-size="12.5" fill="#3d5164">结构前提（求解器启动即断言）：J2/J4/J5 平行同向且同一 x 平面（X_ARM≈19.5）、J1 ∥ X、J3/J6 ∥ Z、TCP 落在 J6 轴线上。</text>
|
||||
<text x="490" y="750" text-anchor="middle" font-size="12.5" fill="#3d5164">这五条性质让六轴问题可以降阶为「二维冗余扫描 + 闭式支」，任何一条不成立时求解器会打出 WARN 并回落通用路径。</text>
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 8.4 KiB |
@@ -0,0 +1,69 @@
|
||||
<svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 980 440" font-family="'Segoe UI','PingFang SC','Microsoft YaHei',sans-serif">
|
||||
<rect x="0" y="0" width="980" height="440" fill="#ffffff"/>
|
||||
<defs>
|
||||
<marker id="fb" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
|
||||
<path d="M0,0 L10,5 L0,10 z" fill="#3572c9"/>
|
||||
</marker>
|
||||
</defs>
|
||||
<text x="490" y="34" text-anchor="middle" font-size="21" font-weight="bold" fill="#16324f">正运动学：枢轴变换与链式累积</text>
|
||||
|
||||
<!-- 左：单关节枢轴 -->
|
||||
<text x="230" y="66" text-anchor="middle" font-size="14.5" font-weight="bold" fill="#1f4f8f">单关节：绕“不在原点的轴”旋转</text>
|
||||
<line x1="230" y1="110" x2="230" y2="300" stroke="#3572c9" stroke-width="1.6" stroke-dasharray="7 5"/>
|
||||
<rect x="212" y="160" width="36" height="120" rx="10" fill="#e7edf3" stroke="#9db0c0" stroke-width="1.4" stroke-dasharray="5 4"/>
|
||||
<rect x="212" y="160" width="36" height="120" rx="10" fill="#dbe9fb" stroke="#3572c9" stroke-width="1.6" transform="rotate(32 230 160)"/>
|
||||
<ellipse cx="230" cy="160" rx="46" ry="14" fill="none" stroke="#3572c9" stroke-width="1.8" marker-end="url(#fb)"/>
|
||||
<circle cx="230" cy="160" r="7" fill="#7c5cd6"/>
|
||||
<text x="296" y="148" font-size="12.5" fill="#3d5164">枢轴点 P + 轴方向 d</text>
|
||||
<text x="296" y="166" font-size="12.5" fill="#3d5164">θ=0 位形（虚线）→ θ 位形（蓝）</text>
|
||||
<text x="296" y="184" font-size="12.5" fill="#3d5164">关节角 θi = 绕轴 d 的有符号转角</text>
|
||||
|
||||
<rect x="60" y="318" width="400" height="104" rx="8" fill="#f4f6f8" stroke="#9db0c0" stroke-width="1.2"/>
|
||||
<text x="80" y="344" font-size="13" fill="#16324f" font-family="Consolas,monospace">A(θ) = T(P) · R(d,θ) · T(−P)</text>
|
||||
<text x="80" y="368" font-size="13" fill="#16324f" font-family="Consolas,monospace">x' = R·(x − P) + P</text>
|
||||
<text x="80" y="396" font-size="12" fill="#5c7185">先平移枢轴点到原点 → 绕轴转 θ → 平移回去。关节表只存</text>
|
||||
<text x="80" y="412" font-size="12" fill="#5c7185">P（rear）与 d（单位轴向量），六个关节共 12 个常量。</text>
|
||||
|
||||
<!-- 右：链 -->
|
||||
<text x="735" y="66" text-anchor="middle" font-size="14.5" font-weight="bold" fill="#1f4f8f">六链累积：World = A₁·A₂·A₃·A₄·A₅·A₆·TCP</text>
|
||||
<g font-size="14" font-weight="bold" text-anchor="middle">
|
||||
<rect x="510" y="130" width="52" height="44" rx="6" fill="#dbe9fb" stroke="#3572c9" stroke-width="1.5"/>
|
||||
<text x="536" y="157" fill="#153a6e">A1</text>
|
||||
<rect x="578" y="130" width="52" height="44" rx="6" fill="#dbe9fb" stroke="#3572c9" stroke-width="1.5"/>
|
||||
<text x="604" y="157" fill="#153a6e">A2</text>
|
||||
<rect x="646" y="130" width="52" height="44" rx="6" fill="#dbe9fb" stroke="#3572c9" stroke-width="1.5"/>
|
||||
<text x="672" y="157" fill="#153a6e">A3</text>
|
||||
<rect x="714" y="130" width="52" height="44" rx="6" fill="#dbe9fb" stroke="#3572c9" stroke-width="1.5"/>
|
||||
<text x="740" y="157" fill="#153a6e">A4</text>
|
||||
<rect x="782" y="130" width="52" height="44" rx="6" fill="#dbe9fb" stroke="#3572c9" stroke-width="1.5"/>
|
||||
<text x="808" y="157" fill="#153a6e">A5</text>
|
||||
<rect x="850" y="130" width="52" height="44" rx="6" fill="#dbe9fb" stroke="#3572c9" stroke-width="1.5"/>
|
||||
<text x="876" y="157" fill="#153a6e">A6</text>
|
||||
</g>
|
||||
<g font-size="11" fill="#5c7185" text-anchor="middle">
|
||||
<text x="536" y="190">θ1</text><text x="604" y="190">θ2</text><text x="672" y="190">θ3</text>
|
||||
<text x="740" y="190">θ4</text><text x="808" y="190">θ5</text><text x="876" y="190">θ6</text>
|
||||
</g>
|
||||
<g stroke="#5a7d9a" stroke-width="1.5">
|
||||
<line x1="472" y1="152" x2="508" y2="152"/>
|
||||
<line x1="562" y1="152" x2="576" y2="152"/>
|
||||
<line x1="630" y1="152" x2="644" y2="152"/>
|
||||
<line x1="698" y1="152" x2="712" y2="152"/>
|
||||
<line x1="766" y1="152" x2="780" y2="152"/>
|
||||
<line x1="834" y1="152" x2="848" y2="152"/>
|
||||
<line x1="902" y1="152" x2="918" y2="152"/>
|
||||
</g>
|
||||
<text x="462" y="157" text-anchor="end" font-size="13" font-weight="bold" fill="#16324f">World</text>
|
||||
<text x="960" y="157" text-anchor="end" font-size="13" font-weight="bold" fill="#b06a10">TCP</text>
|
||||
<text x="735" y="216" text-anchor="middle" font-size="12" fill="#5c7185">每级只读父级位姿与自己 —— 与 three.js 场景图 6 级嵌套 pivot 逐位一致</text>
|
||||
|
||||
<rect x="510" y="240" width="420" height="192" rx="8" fill="#f4f6f8" stroke="#9db0c0" stroke-width="1.2"/>
|
||||
<text x="530" y="266" font-size="13" font-weight="bold" fill="#16324f">两套实现，互为对账：</text>
|
||||
<text x="530" y="290" font-size="12.5" fill="#3d5164">· fkChain()：严格 4×4 同质矩阵链 —— 对照基准，慢而直白</text>
|
||||
<text x="530" y="312" font-size="12.5" fill="#3d5164">· fk()：按结构常量展开三角函数的闭式快路径 —— 求解器</text>
|
||||
<text x="530" y="332" font-size="12.5" fill="#3d5164"> 热路径专用，每姿态仅 ~20 次乘加</text>
|
||||
<text x="530" y="360" font-size="12.5" fill="#3d5164">· 自检：5000 个随机位形，两实现的 TCP 差 ≤ 5.7e-13 mm</text>
|
||||
<text x="530" y="384" font-size="12.5" fill="#3d5164">· 工具长度 tool 沿 J6 轴外伸：TCP_z 随 tool 平移（ax=J6 轴）</text>
|
||||
<text x="530" y="406" font-size="11.5" fill="#8a9aa8">指纹校验：预置自检值绑定（限位表+常量+源码）的 FNV-1a 指纹，</text>
|
||||
<text x="530" y="422" font-size="11.5" fill="#8a9aa8">模型或求解器一变即提示重算。</text>
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 5.5 KiB |
@@ -0,0 +1,103 @@
|
||||
<svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 980 920" font-family="'Segoe UI','PingFang SC','Microsoft YaHei',sans-serif">
|
||||
<rect x="0" y="0" width="980" height="920" fill="#ffffff"/>
|
||||
<defs>
|
||||
<marker id="pa" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
|
||||
<path d="M0,0 L10,5 L0,10 z" fill="#5a7d9a"/>
|
||||
</marker>
|
||||
<marker id="pao" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
|
||||
<path d="M0,0 L10,5 L0,10 z" fill="#b06a10"/>
|
||||
</marker>
|
||||
</defs>
|
||||
<text x="490" y="34" text-anchor="middle" font-size="21" font-weight="bold" fill="#16324f">逆运动学求解流水线（solve → solveDyn）</text>
|
||||
|
||||
<!-- 主流程 -->
|
||||
<g>
|
||||
<rect x="265" y="60" width="450" height="64" rx="8" fill="#dbe9fb" stroke="#3572c9" stroke-width="1.6"/>
|
||||
<text x="280" y="84" font-size="12.5" font-weight="bold" fill="#153a6e">① 输入</text>
|
||||
<text x="280" y="102" font-size="12" fill="#2a4a6e">目标 TCP (x,y,z) · 工具长 tool · 可选姿态 R(az,el,roll)</text>
|
||||
<text x="280" y="118" font-size="12" fill="#2a4a6e">ref = 当前姿态：取「本步行程最小」的构型</text>
|
||||
|
||||
<rect x="265" y="148" width="450" height="64" rx="8" fill="#f4f8fb" stroke="#7ba3c7" stroke-width="1.4"/>
|
||||
<text x="280" y="172" font-size="12.5" font-weight="bold" fill="#16324f">② 冗余维准备：(θ4,θ5) 网格 + 逐行可行段探测</text>
|
||||
<text x="280" y="190" font-size="12" fill="#3d5164">步长 h=0.25°；先按探测步 hp 找每行有解的连续段（位置 2° / 姿态 0.5°）</text>
|
||||
<text x="280" y="206" font-size="12" fill="#3d5164">段端外扩 PAD=2° 兜住段界不确定性；段内仍按 h 精扫，密度不降</text>
|
||||
|
||||
<rect x="265" y="236" width="450" height="64" rx="8" fill="#f4f8fb" stroke="#7ba3c7" stroke-width="1.4"/>
|
||||
<text x="280" y="260" font-size="12.5" font-weight="bold" fill="#16324f">③ 全域粗扫：每格 branch() 闭式解 θ1..θ3（≤4 支）</text>
|
||||
<text x="280" y="279" font-size="12" fill="#3d5164">位置由闭式支精确命中（零残差），扫描只优化冗余维；候选有界保留 4000</text>
|
||||
|
||||
<rect x="265" y="324" width="450" height="64" rx="8" fill="#fdf6ec" stroke="#c99418" stroke-width="1.3"/>
|
||||
<text x="280" y="348" font-size="12.5" font-weight="bold" fill="#7a5a08">④ [姿态任务] 孤立根枚举：残差变号括号 → 阻尼高斯-牛顿收根</text>
|
||||
<text x="280" y="367" font-size="12" fill="#6b5312">切向相切的根不变号，按每支 ‖e‖ 最小的节点补种；精确根直接进候选池</text>
|
||||
|
||||
<rect x="265" y="412" width="450" height="64" rx="8" fill="#f4f8fb" stroke="#7ba3c7" stroke-width="1.4"/>
|
||||
<text x="280" y="436" font-size="12.5" font-weight="bold" fill="#16324f">⑤ 局部加密 + 模式搜索：32 种子 × 窗口 ±2h（步长 h/8）</text>
|
||||
<text x="280" y="455" font-size="12" fill="#3d5164">8 邻域模式搜索步长逐轮减半至 2e-4°，把每条谷底走到最低</text>
|
||||
|
||||
<rect x="265" y="500" width="450" height="64" rx="8" fill="#fdf6ec" stroke="#c99418" stroke-width="1.3"/>
|
||||
<text x="280" y="524" font-size="12.5" font-weight="bold" fill="#7a5a08">⑥ [姿态任务] 二维牛顿收口 + escalating 加压</text>
|
||||
<text x="280" y="542" font-size="12" fill="#6b5312">对工具轴切向偏差做 (θ4,θ5) 牛顿迭代，轴残差 →1e-12、位置保持精确</text>
|
||||
<text x="280" y="558" font-size="12" fill="#6b5312">仍无 oerr≤0.05° 达标解则姿态权重 ×100 重扫（最多 3 轮）</text>
|
||||
|
||||
<rect x="265" y="588" width="450" height="64" rx="8" fill="#f4f8fb" stroke="#7ba3c7" stroke-width="1.4"/>
|
||||
<text x="280" y="612" font-size="12.5" font-weight="bold" fill="#16324f">⑦ 选解：cost = Σ w·|wrap180(θ−ref)| + 1e-6·Σ|θ|</text>
|
||||
<text x="280" y="630" font-size="12" fill="#3d5164">姿态任务先取 oerr≤0.05° 达标集，集内再比代价</text>
|
||||
<text x="280" y="646" font-size="12" fill="#3d5164">精确根只在代价 ≤ 达标最优+2° 时接管（防漏检多付几十度）</text>
|
||||
|
||||
<rect x="265" y="676" width="450" height="64" rx="8" fill="#eef8f1" stroke="#1f6f43" stroke-width="1.4"/>
|
||||
<text x="280" y="700" font-size="12.5" font-weight="bold" fill="#14472c">⑧ solveDyn 碰撞后处理（装了胶囊模型时）</text>
|
||||
<text x="280" y="718" font-size="12" fill="#2c5c44">解先过全姿态碰撞校验：不安全 → 候选表找 ≤1.5×cost+20° 的安全解</text>
|
||||
<text x="280" y="734" font-size="12" fill="#2c5c44">贴边轴二分动态边界放宽（≥5° 才值得重解，重解 cost 不升才采用）</text>
|
||||
|
||||
<rect x="265" y="764" width="450" height="58" rx="8" fill="#dbe9fb" stroke="#3572c9" stroke-width="1.6"/>
|
||||
<text x="280" y="788" font-size="12.5" font-weight="bold" fill="#153a6e">输出：q[6] · 残差 err / oerr · 本步行程 moveSum · 候选表 list</text>
|
||||
<text x="280" y="806" font-size="12" fill="#2a4a6e">徽章分级：已到达 / 姿态到达·位置最近 / 混合折中 / 不可达·最近位姿</text>
|
||||
</g>
|
||||
|
||||
<g stroke="#5a7d9a" stroke-width="1.8">
|
||||
<line x1="490" y1="124" x2="490" y2="146" marker-end="url(#pa)"/>
|
||||
<line x1="490" y1="212" x2="490" y2="234" marker-end="url(#pa)"/>
|
||||
<line x1="490" y1="300" x2="490" y2="322" marker-end="url(#pa)"/>
|
||||
<line x1="490" y1="388" x2="490" y2="410" marker-end="url(#pa)"/>
|
||||
<line x1="490" y1="476" x2="490" y2="498" marker-end="url(#pa)"/>
|
||||
<line x1="490" y1="564" x2="490" y2="586" marker-end="url(#pa)"/>
|
||||
<line x1="490" y1="652" x2="490" y2="674" marker-end="url(#pa)"/>
|
||||
<line x1="490" y1="740" x2="490" y2="762" marker-end="url(#pa)"/>
|
||||
</g>
|
||||
|
||||
<!-- 右侧：快路 -->
|
||||
<rect x="740" y="236" width="215" height="196" rx="10" fill="#ffffff" stroke="#b06a10" stroke-width="1.6" stroke-dasharray="7 5"/>
|
||||
<text x="755" y="262" font-size="13" font-weight="bold" fill="#b06a10">solveWarm 实时快路</text>
|
||||
<text x="755" y="284" font-size="11.5" fill="#6b5312">拖拽中：只在上帧解附近的</text>
|
||||
<text x="755" y="302" font-size="11.5" fill="#6b5312">(θ4,θ5) 递进窗口 ±4→14→40°</text>
|
||||
<text x="755" y="320" font-size="11.5" fill="#6b5312">同支跟随（Σ|Δθ|≤25°）+ 残差</text>
|
||||
<text x="755" y="338" font-size="11.5" fill="#6b5312">≤1e-6 + 姿态牛顿收口</text>
|
||||
<text x="755" y="362" font-size="11.5" font-weight="bold" fill="#b06a10">任一校验不过 → null</text>
|
||||
<text x="755" y="380" font-size="11.5" fill="#6b5312">自动回落全量扫描（③）</text>
|
||||
<text x="755" y="400" font-size="11" fill="#8a9aa8">快路只负责「能快时快」，</text>
|
||||
<text x="755" y="416" font-size="11" fill="#8a9aa8">松手后全量求解才是终值</text>
|
||||
<path d="M 738 268 C 727 240, 727 130, 719 96" fill="none" stroke="#b06a10" stroke-width="1.5" stroke-dasharray="6 4" marker-end="url(#pao)"/>
|
||||
<path d="M 847 434 C 847 480, 700 560, 721 600" fill="none" stroke="#b06a10" stroke-width="1.5" stroke-dasharray="6 4" marker-end="url(#pao)"/>
|
||||
<text x="742" y="470" font-size="11" fill="#b06a10">校验不过回落</text>
|
||||
|
||||
<!-- 左侧：兜底 -->
|
||||
<rect x="25" y="324" width="215" height="230" rx="10" fill="#ffffff" stroke="#8a5a8f" stroke-width="1.6" stroke-dasharray="7 5"/>
|
||||
<text x="40" y="350" font-size="13" font-weight="bold" fill="#7a3a80">兜底：目标不可达时</text>
|
||||
<text x="40" y="372" font-size="11.5" fill="#5c3a60">closest() 位置 DLS：160 迭代 ×</text>
|
||||
<text x="40" y="390" font-size="11.5" fill="#5c3a60">160 次 LCG 确定性重启（同输入</text>
|
||||
<text x="40" y="408" font-size="11.5" fill="#5c3a60">逐位一致）+ anchor 回拉 0.03</text>
|
||||
<text x="40" y="426" font-size="11.5" fill="#5c3a60">压退化漂移 + 零空间抛光收敛</text>
|
||||
<text x="40" y="450" font-size="11.5" fill="#5c3a60">closest6() 位置+姿态 6 维联合</text>
|
||||
<text x="40" y="468" font-size="11.5" fill="#5c3a60">DLS（姿态按 20mm/rad 折算）</text>
|
||||
<text x="40" y="492" font-size="11.5" fill="#5c3a60">closestOri() 姿态优先：闭式轴</text>
|
||||
<text x="40" y="510" font-size="11.5" fill="#5c3a60">对齐 + (θ3,s,θ4) 坐标下降，</text>
|
||||
<text x="40" y="528" font-size="11.5" fill="#5c3a60">姿态精确、位置取最近</text>
|
||||
<text x="40" y="548" font-size="11" fill="#8a9aa8">距离差 ≤0.5mm 时优先锚定解</text>
|
||||
<path d="M 263 620 C 180 620, 140 590, 133 556" fill="none" stroke="#8a5a8f" stroke-width="1.5" stroke-dasharray="6 4" marker-end="url(#pao)"/>
|
||||
<text x="148" y="648" font-size="11" fill="#8a5a8f">0 采样点 / 不可达</text>
|
||||
<path d="M 133 322 C 140 220, 200 130, 262 96" fill="none" stroke="#8a5a8f" stroke-width="1.5" stroke-dasharray="6 4" marker-end="url(#pao)"/>
|
||||
<text x="120" y="200" font-size="11" fill="#8a5a8f">返回最近位姿</text>
|
||||
|
||||
<text x="490" y="860" text-anchor="middle" font-size="12.5" fill="#3d5164">热路径性能手段：轴对齐特化 rotAxis（免 Rodrigues 通用式）· sin/cos 单槽缓存 · 关节限位数组预取 · 残差 fk 复核延后到入选候选统一补算</text>
|
||||
<text x="490" y="884" text-anchor="middle" font-size="12.5" fill="#3d5164">全量扫描 40~430 ms（后台 Worker）；实时快路毫秒级 —— 这是拖拽能跟手的原因</text>
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 9.1 KiB |
@@ -0,0 +1,88 @@
|
||||
<svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 980 620" font-family="'Segoe UI','PingFang SC','Microsoft YaHei',sans-serif">
|
||||
<rect x="0" y="0" width="980" height="620" fill="#ffffff"/>
|
||||
<defs>
|
||||
<marker id="ta" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
|
||||
<path d="M0,0 L10,5 L0,10 z" fill="#8a9aa8"/>
|
||||
</marker>
|
||||
<marker id="to" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
|
||||
<path d="M0,0 L10,5 L0,10 z" fill="#e08a1e"/>
|
||||
</marker>
|
||||
</defs>
|
||||
<text x="490" y="34" text-anchor="middle" font-size="21" font-weight="bold" fill="#16324f">冗余维 (θ4,θ5) 平面：可行带、加密窗口与牛顿收口</text>
|
||||
|
||||
<!-- 绘图区 -->
|
||||
<rect x="110" y="80" width="740" height="430" fill="#f8fafc" stroke="#b9c8d6" stroke-width="1.2"/>
|
||||
<!-- 可行带（斜窄带，宽度做了夸张） -->
|
||||
<polygon points="245,452 830,168 830,132 245,416" fill="#d9f2e5" opacity="0.9"/>
|
||||
<text x="585" y="215" font-size="12" fill="#1f7a4d" transform="rotate(-26 585 215)">可行带：branch 判据筛掉 73~93% 的格点</text>
|
||||
|
||||
<!-- 不可行散点 -->
|
||||
<g fill="#c3ccd4" font-size="10">
|
||||
<text x="200" y="140">×</text><text x="330" y="120">×</text><text x="500" y="110">×</text>
|
||||
<text x="700" y="128">×</text><text x="790" y="180">×</text><text x="170" y="300">×</text>
|
||||
<text x="300" y="260">×</text><text x="430" y="210">×</text><text x="640" y="150">×</text>
|
||||
<text x="220" y="470">×</text><text x="480" y="480">×</text><text x="700" y="460">×</text>
|
||||
<text x="820" y="430">×</text><text x="360" y="420">×</text><text x="560" y="440">×</text>
|
||||
</g>
|
||||
<!-- 可行带内格点 -->
|
||||
<g fill="#2f9d68">
|
||||
<circle cx="270" cy="428" r="2.4"/><circle cx="310" cy="410" r="2.4"/><circle cx="352" cy="390" r="2.4"/>
|
||||
<circle cx="395" cy="368" r="2.4"/><circle cx="438" cy="348" r="2.4"/><circle cx="480" cy="328" r="2.4"/>
|
||||
<circle cx="522" cy="308" r="2.4"/><circle cx="565" cy="288" r="2.4"/><circle cx="608" cy="268" r="2.4"/>
|
||||
<circle cx="650" cy="248" r="2.4"/><circle cx="692" cy="228" r="2.4"/><circle cx="735" cy="206" r="2.4"/>
|
||||
<circle cx="778" cy="186" r="2.4"/><circle cx="812" cy="168" r="2.4"/>
|
||||
<circle cx="292" cy="418" r="2.4"/><circle cx="375" cy="378" r="2.4"/><circle cx="459" cy="338" r="2.4"/>
|
||||
<circle cx="543" cy="298" r="2.4"/><circle cx="628" cy="256" r="2.4"/><circle cx="712" cy="216" r="2.4"/>
|
||||
<circle cx="796" cy="178" r="2.4"/>
|
||||
</g>
|
||||
|
||||
<!-- 行可行段 -->
|
||||
<g stroke="#3572c9" stroke-width="5" opacity="0.75">
|
||||
<line x1="228" y1="440" x2="360" y2="376"/>
|
||||
<line x1="415" y1="352" x2="548" y2="288"/>
|
||||
<line x1="602" y1="264" x2="735" y2="200"/>
|
||||
</g>
|
||||
<text x="238" y="486" font-size="12" fill="#2a4a6e">逐行可行段 rowSpans（探测步 hp，段端外扩 PAD=2°）：</text>
|
||||
<text x="238" y="504" font-size="12" fill="#2a4a6e">每行通常是单一连续段，段内仍按 h 精扫 —— 密度不降、成本大降</text>
|
||||
|
||||
<!-- 加密窗口 -->
|
||||
<rect x="478" y="278" width="92" height="92" fill="none" stroke="#e08a1e" stroke-width="1.8" stroke-dasharray="6 4"/>
|
||||
<text x="580" y="368" font-size="12" fill="#b06a10">加密窗口 ±2h（步长 h/8，</text>
|
||||
<text x="580" y="386" font-size="12" fill="#b06a10">小步长档 h/4）罩住种子</text>
|
||||
|
||||
<!-- 模式搜索折线 -->
|
||||
<polyline points="500,340 524,318 512,296 540,282 560,296 578,282" fill="none" stroke="#e08a1e" stroke-width="1.8" marker-end="url(#to)"/>
|
||||
<text x="700" y="330" font-size="12" fill="#b06a10">8 邻域模式搜索：</text>
|
||||
<text x="700" y="348" font-size="12" fill="#b06a10">步长减半，停在窄谷谷壁</text>
|
||||
|
||||
<!-- 牛顿收口点 -->
|
||||
<g>
|
||||
<path d="M 596 244 l 4.5 10.5 11 1 -8.5 7.5 2.5 11 -9.5 -6 -9.5 6 2.5 -11 -8.5 -7.5 11 -1 z" fill="#d64545"/>
|
||||
<line x1="602" y1="254" x2="640" y2="286" stroke="#d64545" stroke-width="1" stroke-dasharray="3 3"/>
|
||||
<text x="648" y="296" font-size="12.5" font-weight="bold" fill="#b03030">牛顿收口点</text>
|
||||
<text x="648" y="314" font-size="12" fill="#b03030">轴残差 →1e-12（位置保持精确）</text>
|
||||
</g>
|
||||
|
||||
<!-- 坐标轴 -->
|
||||
<g stroke="#8a9aa8" stroke-width="1.4">
|
||||
<line x1="110" y1="510" x2="850" y2="510" marker-start="url(#ta)"/>
|
||||
<line x1="110" y1="510" x2="110" y2="80" marker-end="url(#ta)"/>
|
||||
<line x1="130" y1="510" x2="130" y2="516"/><line x1="245" y1="510" x2="245" y2="516"/>
|
||||
<line x1="411" y1="510" x2="411" y2="516"/><line x1="576" y1="510" x2="576" y2="516"/>
|
||||
<line x1="741" y1="510" x2="741" y2="516"/><line x1="830" y1="510" x2="830" y2="516"/>
|
||||
<line x1="104" y1="500" x2="110" y2="500"/><line x1="104" y1="396" x2="110" y2="396"/>
|
||||
<line x1="104" y1="302" x2="110" y2="302"/><line x1="104" y1="207" x2="110" y2="207"/>
|
||||
<line x1="104" y1="100" x2="110" y2="100"/>
|
||||
</g>
|
||||
<g font-size="11.5" fill="#5c7185">
|
||||
<text x="126" y="530">−111°</text><text x="238" y="530">−90°</text><text x="404" y="530">−60°</text>
|
||||
<text x="569" y="530">−30°</text><text x="734" y="530">0°</text><text x="812" y="530">+16°</text>
|
||||
<text x="66" y="504">−93°</text><text x="66" y="400">−60°</text><text x="66" y="306">−30°</text>
|
||||
<text x="76" y="211">0°</text><text x="80" y="104">+34°</text>
|
||||
<text x="840" y="552" font-size="13" font-weight="bold" fill="#16324f">θ4（J4 肘部俯仰）</text>
|
||||
<text x="66" y="70" font-size="13" font-weight="bold" fill="#16324f">θ5（J5 腕俯仰）</text>
|
||||
</g>
|
||||
|
||||
<text x="490" y="582" text-anchor="middle" font-size="12.5" fill="#3d5164">每个格点的位置解由闭式支精确给出 —— 扫描不是在「逼近」目标，而是在冗余维里「挑构型」；</text>
|
||||
<text x="490" y="602" text-anchor="middle" font-size="12.5" fill="#3d5164">姿态任务的达标带实测最窄 0.98°，2° 探测步会整条漏掉（误报不可达），故姿态任务探测步降到 0.5°。</text>
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 5.9 KiB |
@@ -0,0 +1,80 @@
|
||||
<svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 980 580" font-family="'Segoe UI','PingFang SC','Microsoft YaHei',sans-serif">
|
||||
<rect x="0" y="0" width="980" height="580" fill="#ffffff"/>
|
||||
<defs>
|
||||
<marker id="ga" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
|
||||
<path d="M0,0 L10,5 L0,10 z" fill="#8a9aa8"/>
|
||||
</marker>
|
||||
</defs>
|
||||
<text x="490" y="34" text-anchor="middle" font-size="21" font-weight="bold" fill="#16324f">闭式支 branch()：给定 (θ4,θ5)，θ1..θ3 一步解出（简化示意）</text>
|
||||
|
||||
<!-- 左panel -->
|
||||
<text x="255" y="66" text-anchor="middle" font-size="14.5" font-weight="bold" fill="#1f4f8f">第 1 步：(θ4,θ5) ⇒ 腕心平面坐标 V</text>
|
||||
<line x1="150" y1="120" x2="150" y2="330" stroke="#3572c9" stroke-width="1.6" stroke-dasharray="7 5"/>
|
||||
<text x="150" y="352" text-anchor="middle" font-size="11.5" fill="#5c7185">J4 轴（+Y，⊗）</text>
|
||||
<circle cx="150" cy="140" r="8" fill="#ffffff" stroke="#1f9d63" stroke-width="2"/>
|
||||
<line x1="144" y1="134" x2="156" y2="146" stroke="#1f9d63" stroke-width="2"/>
|
||||
<line x1="156" y1="134" x2="144" y2="146" stroke="#1f9d63" stroke-width="2"/>
|
||||
<line x1="150" y1="140" x2="278" y2="188" stroke="#3572c9" stroke-width="5" stroke-linecap="round"/>
|
||||
<circle cx="278" cy="188" r="8" fill="#ffffff" stroke="#1f9d63" stroke-width="2"/>
|
||||
<line x1="272" y1="182" x2="284" y2="194" stroke="#1f9d63" stroke-width="2"/>
|
||||
<line x1="284" y1="182" x2="272" y2="194" stroke="#1f9d63" stroke-width="2"/>
|
||||
<text x="292" y="186" font-size="11.5" fill="#5c7185">J5 轴</text>
|
||||
<line x1="278" y1="188" x2="330" y2="252" stroke="#3572c9" stroke-width="4" stroke-linecap="round"/>
|
||||
<circle cx="330" cy="252" r="6" fill="#e08a1e"/>
|
||||
<text x="344" y="258" font-size="12" font-weight="bold" fill="#b06a10">腕心方向（L = L5+tool）</text>
|
||||
<path d="M 150 170 A 30 30 0 0 0 178 150.5" fill="none" stroke="#7c5cd6" stroke-width="1.6"/>
|
||||
<text x="186" y="178" font-size="12" fill="#7c5cd6">θ4</text>
|
||||
<path d="M 262 216 A 30 30 0 0 1 296 214" fill="none" stroke="#7c5cd6" stroke-width="1.6"/>
|
||||
<text x="272" y="232" font-size="12" fill="#7c5cd6">θ5</text>
|
||||
<text x="150" y="120" font-size="11.5" fill="#5c7185">肘（J4）</text>
|
||||
|
||||
<rect x="50" y="386" width="420" height="152" rx="8" fill="#f4f6f8" stroke="#9db0c0" stroke-width="1.2"/>
|
||||
<text x="68" y="412" font-size="12.5" fill="#16324f" font-family="Consolas,monospace">u = −L·sinθ5 w = A54 − L·cosθ5</text>
|
||||
<text x="68" y="436" font-size="12.5" fill="#16324f" font-family="Consolas,monospace">dx = u·cosθ4 + w·sinθ4</text>
|
||||
<text x="68" y="460" font-size="12.5" fill="#16324f" font-family="Consolas,monospace">dz = A43 − u·sinθ4 + w·cosθ4</text>
|
||||
<text x="68" y="484" font-size="12.5" fill="#16324f" font-family="Consolas,monospace">V = (dx, dz + A32)</text>
|
||||
<text x="68" y="514" font-size="11.5" fill="#5c7185">J4∥J5 同向 ⇒ R4·R5 = R(θ4+θ5)：腕心只由这两个角决定，</text>
|
||||
<text x="68" y="530" font-size="11.5" fill="#5c7185">姿态任务里工具轴对齐也因此只差 2 个未知数。</text>
|
||||
|
||||
<!-- 右panel -->
|
||||
<text x="735" y="66" text-anchor="middle" font-size="14.5" font-weight="bold" fill="#1f4f8f">第 2 步：肩部圆交 + 肘部四支 ⇒ θ1/θ2/θ3</text>
|
||||
<!-- 俯视：J1 轴与 φ -->
|
||||
<circle cx="600" cy="130" r="9" fill="#ffffff" stroke="#d64545" stroke-width="2"/>
|
||||
<circle cx="600" cy="130" r="2" fill="#d64545"/>
|
||||
<text x="576" y="112" font-size="11.5" fill="#5c7185">J1 轴（+X，⊙）</text>
|
||||
<circle cx="700" cy="96" r="5" fill="#e08a1e"/>
|
||||
<text x="712" y="94" font-size="11.5" fill="#b06a10">目标（柱坐标 R, φ）</text>
|
||||
<line x1="600" y1="130" x2="700" y2="96" stroke="#e08a1e" stroke-width="1.6" stroke-dasharray="5 4"/>
|
||||
<path d="M 640 117 A 42 42 0 0 0 636 108" fill="none" stroke="#7c5cd6" stroke-width="1.6"/>
|
||||
<text x="648" y="106" font-size="12" fill="#7c5cd6">φ</text>
|
||||
<text x="566" y="158" font-size="12" fill="#16324f" font-family="Consolas,monospace">θ1 = φ − atan2(cc, b)</text>
|
||||
|
||||
<!-- 正视：肩 C0 偏置与肘三角 -->
|
||||
<circle cx="620" cy="290" r="9" fill="#ffffff" stroke="#1f9d63" stroke-width="2"/>
|
||||
<line x1="614" y1="284" x2="626" y2="296" stroke="#1f9d63" stroke-width="2"/>
|
||||
<line x1="626" y1="284" x2="614" y2="296" stroke="#1f9d63" stroke-width="2"/>
|
||||
<text x="560" y="294" text-anchor="end" font-size="11.5" fill="#5c7185">J1（正视）</text>
|
||||
<circle cx="620" cy="382" r="9" fill="#ffffff" stroke="#1f9d63" stroke-width="2"/>
|
||||
<line x1="614" y1="376" x2="626" y2="388" stroke="#1f9d63" stroke-width="2"/>
|
||||
<line x1="626" y1="376" x2="614" y2="388" stroke="#1f9d63" stroke-width="2"/>
|
||||
<text x="560" y="386" text-anchor="end" font-size="11.5" fill="#5c7185">J2</text>
|
||||
<line x1="620" y1="299" x2="620" y2="373" stroke="#8a6d3b" stroke-width="1.3" marker-start="url(#ga)" marker-end="url(#ga)"/>
|
||||
<text x="632" y="341" font-size="11.5" fill="#8a6d3b">C0=120</text>
|
||||
<line x1="620" y1="382" x2="700" y2="322" stroke="#3572c9" stroke-width="4" stroke-linecap="round"/>
|
||||
<line x1="700" y1="322" x2="820" y2="290" stroke="#3572c9" stroke-width="4" stroke-linecap="round"/>
|
||||
<circle cx="820" cy="290" r="6" fill="#e08a1e"/>
|
||||
<text x="834" y="294" font-size="12" font-weight="bold" fill="#b06a10">腕心（由 V 决定）</text>
|
||||
<path d="M 676 342 A 44 44 0 0 0 666 314" fill="none" stroke="#7c5cd6" stroke-width="1.6"/>
|
||||
<text x="688" y="318" font-size="12" fill="#7c5cd6">σ</text>
|
||||
<text x="690" y="368" font-size="11.5" fill="#5c7185">肘部三角：σ=acos(√m2)</text>
|
||||
<text x="690" y="386" font-size="11.5" fill="#5c7185">θ3 四支：±σ 与 ±(π−σ)</text>
|
||||
<text x="690" y="404" font-size="11.5" fill="#5c7185">（肘上/肘下 × 同号/反号镜像族）</text>
|
||||
|
||||
<rect x="510" y="428" width="440" height="110" rx="8" fill="#f4f6f8" stroke="#9db0c0" stroke-width="1.2"/>
|
||||
<text x="528" y="454" font-size="12.5" fill="#16324f" font-family="Consolas,monospace">n = (R²+A²−Vx²−Vz²−C0²) / (2·C0)，cc = C0+n</text>
|
||||
<text x="528" y="478" font-size="12.5" fill="#16324f" font-family="Consolas,monospace">θ2 = atan2((λA−κn)/det, (κA+λn)/det) κ=Vx·c3, λ=Vz</text>
|
||||
<text x="528" y="502" font-size="12.5" fill="#16324f" font-family="Consolas,monospace">b = −Vx·sinθ3 （θ1 由柱角 φ 反推）</text>
|
||||
<text x="528" y="526" font-size="11.5" fill="#8a5433">退化防线：|Vx|<1e-7、det<1e-12、m2 越界 → 该支无解，直接落选</text>
|
||||
|
||||
<text x="490" y="560" text-anchor="middle" font-size="12.5" fill="#3d5164">示意省略了 X0/Y0 基准平移与 tool 常量；四支解的位置全部精确命中目标 ——「扫描代替数值搜索」的根基。</text>
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 6.6 KiB |
@@ -0,0 +1,76 @@
|
||||
<svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 980 750" font-family="'Segoe UI','PingFang SC','Microsoft YaHei',sans-serif">
|
||||
<rect x="0" y="0" width="980" height="750" fill="#ffffff"/>
|
||||
<defs>
|
||||
<marker id="fa" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
|
||||
<path d="M0,0 L10,5 L0,10 z" fill="#5a7d9a"/>
|
||||
</marker>
|
||||
</defs>
|
||||
<text x="490" y="34" text-anchor="middle" font-size="21" font-weight="bold" fill="#16324f">离线碰撞检测（sweep_v2.py）：逐层过滤漏斗</text>
|
||||
<text x="490" y="58" text-anchor="middle" font-size="13" fill="#5c7185">目标：把「每姿态都做 NURBS 布尔」压到只剩真正贴 face 的零件对 —— 精确性一分不减，调用量掉几个数量级</text>
|
||||
|
||||
<g font-size="12.5">
|
||||
<rect x="120" y="76" width="600" height="46" rx="8" fill="#dbe9fb" stroke="#3572c9" stroke-width="1.4"/>
|
||||
<circle cx="145" cy="99" r="12" fill="#3572c9"/><text x="145" y="104" text-anchor="middle" fill="#ffffff" font-weight="bold">1</text>
|
||||
<text x="168" y="95" font-weight="bold" fill="#153a6e">全对全候选对</text>
|
||||
<text x="168" y="113" fill="#2a4a6e">19 件 ≈ 171 对（9 打印件 + 6 电机 + 台架 P01/P02），按姿态刚体变换</text>
|
||||
<line x1="420" y1="122" x2="420" y2="136" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#fa)"/>
|
||||
|
||||
<rect x="140" y="138" width="560" height="46" rx="8" fill="#e6eef7" stroke="#5a86b5" stroke-width="1.3"/>
|
||||
<circle cx="165" cy="161" r="12" fill="#5a86b5"/><text x="165" y="166" text-anchor="middle" fill="#ffffff" font-weight="bold">2</text>
|
||||
<text x="188" y="157" font-weight="bold" fill="#153a6e">跳过刚性安装对</text>
|
||||
<text x="188" y="175" fill="#2a4a6e">MOUNT_PAIRS 12 对：电机 ↔ 被驱动件由螺钉锁死，无相对运动</text>
|
||||
<line x1="420" y1="184" x2="420" y2="198" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#fa)"/>
|
||||
|
||||
<rect x="160" y="200" width="520" height="46" rx="8" fill="#e6eef7" stroke="#5a86b5" stroke-width="1.3"/>
|
||||
<circle cx="185" cy="223" r="12" fill="#5a86b5"/><text x="185" y="228" text-anchor="middle" fill="#ffffff" font-weight="bold">3</text>
|
||||
<text x="208" y="219" font-weight="bold" fill="#153a6e">组跨度规则</text>
|
||||
<text x="208" y="237" fill="#2a4a6e">该姿态中没有活动关节跨越的对跳过（零位时全部必查——静止不代表不干涉）</text>
|
||||
<line x1="420" y1="246" x2="420" y2="260" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#fa)"/>
|
||||
|
||||
<rect x="180" y="262" width="480" height="46" rx="8" fill="#e6eef7" stroke="#5a86b5" stroke-width="1.3"/>
|
||||
<circle cx="205" cy="285" r="12" fill="#5a86b5"/><text x="205" y="290" text-anchor="middle" fill="#ffffff" font-weight="bold">4</text>
|
||||
<text x="228" y="281" font-weight="bold" fill="#153a6e">轴区间预剪枝（单关节扫描专用，精确安全）</text>
|
||||
<text x="228" y="299" fill="#2a4a6e">沿关节轴的轴向/径向区间永不相交的对，任何角度都不可能碰撞</text>
|
||||
<line x1="420" y1="308" x2="420" y2="322" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#fa)"/>
|
||||
|
||||
<rect x="200" y="324" width="440" height="46" rx="8" fill="#e6eef7" stroke="#5a86b5" stroke-width="1.3"/>
|
||||
<circle cx="225" cy="347" r="12" fill="#5a86b5"/><text x="225" y="352" text-anchor="middle" fill="#ffffff" font-weight="bold">5</text>
|
||||
<text x="248" y="343" font-weight="bold" fill="#153a6e">AABB 分离</text>
|
||||
<text x="248" y="361" fill="#2a4a6e">8 角点变换后取包围盒，0.1mm 余量；分离即跳过</text>
|
||||
<line x1="420" y1="370" x2="420" y2="384" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#fa)"/>
|
||||
|
||||
<rect x="220" y="386" width="400" height="46" rx="8" fill="#e6eef7" stroke="#5a86b5" stroke-width="1.3"/>
|
||||
<circle cx="245" cy="409" r="12" fill="#5a86b5"/><text x="245" y="414" text-anchor="middle" fill="#ffffff" font-weight="bold">6</text>
|
||||
<text x="268" y="405" font-weight="bold" fill="#153a6e">包围球分离</text>
|
||||
<text x="268" y="423" fill="#2a4a6e">球心随组变换、半径旋转不变；球分离 ⇒ 零件必分离</text>
|
||||
<line x1="420" y1="432" x2="420" y2="446" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#fa)"/>
|
||||
|
||||
<rect x="230" y="444" width="400" height="66" rx="8" fill="#fdf2e2" stroke="#c99418" stroke-width="1.4"/>
|
||||
<circle cx="255" cy="477" r="12" fill="#c99418"/><text x="255" y="482" text-anchor="middle" fill="#ffffff" font-weight="bold">7</text>
|
||||
<text x="278" y="468" font-weight="bold" fill="#7a5a08">GJK 凸包预筛（保守证书）</text>
|
||||
<text x="278" y="487" fill="#6b5312">0.5mm 凸包 ⊕ 0.8mm 球：确证分离才允许跳过</text>
|
||||
<text x="278" y="500" fill="#6b5312" font-size="11">可能干涉/数值退化一律回落精确布尔——结果与纯布尔一致</text>
|
||||
<line x1="420" y1="510" x2="420" y2="516" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#fa)"/>
|
||||
|
||||
<rect x="255" y="518" width="350" height="66" rx="8" fill="#fdf2e2" stroke="#c99418" stroke-width="1.4"/>
|
||||
<circle cx="280" cy="551" r="12" fill="#c99418"/><text x="280" y="556" text-anchor="middle" fill="#ffffff" font-weight="bold">8</text>
|
||||
<text x="303" y="542" font-weight="bold" fill="#7a5a08">顶点侵入实锤（反方向 shortcut）</text>
|
||||
<text x="303" y="561" fill="#6b5312">一方网格顶点（≤240 采样）落入对方实体 >1mm</text>
|
||||
<text x="303" y="574" fill="#6b5312" font-size="11">⇒ 直接判干涉，免 4~12s 布尔；只证「撞」不证「自由」</text>
|
||||
<line x1="420" y1="584" x2="420" y2="590" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#fa)"/>
|
||||
|
||||
<rect x="275" y="592" width="300" height="52" rx="8" fill="#fbe9e7" stroke="#c0533a" stroke-width="1.5"/>
|
||||
<circle cx="300" cy="618" r="12" fill="#c0533a"/><text x="300" y="623" text-anchor="middle" fill="#ffffff" font-weight="bold">9</text>
|
||||
<text x="323" y="612" font-weight="bold" fill="#7c2a18">OCCT 精确布尔 Common</text>
|
||||
<text x="323" y="630" fill="#8a3a26">实体交体积 > 2mm³ 记干涉（附干涉质心）</text>
|
||||
<text x="323" y="640" fill="#8a3a26" font-size="11">SetRunParallel(True) · 多进程分发姿态</text>
|
||||
</g>
|
||||
|
||||
<path d="M 745 480 C 800 480, 820 540, 820 590 C 820 620, 790 630, 566 618" fill="none" stroke="#c0533a" stroke-width="1.5" stroke-dasharray="6 4" marker-end="url(#fa)"/>
|
||||
<text x="806" y="470" text-anchor="middle" font-size="12" fill="#7c2a18">布尔每对 0.5~12s</text>
|
||||
<text x="806" y="488" text-anchor="middle" font-size="12" fill="#7c2a18">前面每层都在为它省调用</text>
|
||||
|
||||
<rect x="60" y="660" width="860" height="66" rx="8" fill="#f4f6f8" stroke="#9db0c0" stroke-width="1.2"/>
|
||||
<text x="490" y="684" text-anchor="middle" font-size="12.5" fill="#3d5164">扫描策略：单关节双向粗扫 2°（小批量、首个卡死角即停）+ 二分逼近 0.2° → travel.json（滑块硬限位）</text>
|
||||
<text x="490" y="704" text-anchor="middle" font-size="12.5" fill="#3d5164">组合姿态收敛 6°（一个关节保持极限、扫其余，两轮迭代）→ 只写 travel_detail.json，不压缩滑块 ｜ workers = min(12, CPU−2, 内存/2.5GB)</text>
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 7.1 KiB |
@@ -0,0 +1,70 @@
|
||||
<svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 980 640" font-family="'Segoe UI','PingFang SC','Microsoft YaHei',sans-serif">
|
||||
<rect x="0" y="0" width="980" height="640" fill="#ffffff"/>
|
||||
<defs>
|
||||
<marker id="ca" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
|
||||
<path d="M0,0 L10,5 L0,10 z" fill="#5a7d9a"/>
|
||||
</marker>
|
||||
</defs>
|
||||
<text x="490" y="34" text-anchor="middle" font-size="21" font-weight="bold" fill="#16324f">实时碰撞模型：CAD 网格 → 胶囊体 + 两级宽相位</text>
|
||||
|
||||
<!-- 左：胶囊化手臂示意 -->
|
||||
<text x="235" y="64" text-anchor="middle" font-size="14.5" font-weight="bold" fill="#1f4f8f">手臂按运动组胶囊化（fit_capsules.py 拟合）</text>
|
||||
<line x1="90" y1="590" x2="400" y2="590" stroke="#8a9aa8" stroke-width="2.5"/>
|
||||
<rect x="150" y="110" width="34" height="478" rx="6" fill="#e2e8ee" stroke="#9db0c0" stroke-width="1"/>
|
||||
<text x="167" y="102" text-anchor="middle" font-size="11" fill="#5c7185">台架 g0（root 固定）</text>
|
||||
|
||||
<rect x="196" y="128" width="52" height="86" rx="26" fill="#aecbeb" stroke="#3572c9" stroke-width="1.4"/>
|
||||
<text x="256" y="150" font-size="11.5" fill="#153a6e">g1 · J1 组（肩吊臂）</text>
|
||||
<rect x="196" y="226" width="52" height="70" rx="26" fill="#b7dcC8" stroke="#1f9d63" stroke-width="1.4"/>
|
||||
<text x="256" y="248" font-size="11.5" fill="#14573a">g2 · J2 组（肩叉）</text>
|
||||
<rect x="196" y="306" width="52" height="96" rx="26" fill="#a8d5d8" stroke="#1f7a8f" stroke-width="1.4"/>
|
||||
<text x="256" y="336" font-size="11.5" fill="#14525e">g3 · J3 组（大臂骨架）</text>
|
||||
<rect x="196" y="412" width="52" height="78" rx="26" fill="#c9d9a8" stroke="#6f8f2f" stroke-width="1.4"/>
|
||||
<text x="256" y="436" font-size="11.5" fill="#4a5f1c">g4 · J4 组（小臂骨架）</text>
|
||||
<rect x="196" y="498" width="52" height="42" rx="21" fill="#f2d5a8" stroke="#c98a18" stroke-width="1.4"/>
|
||||
<text x="256" y="512" font-size="11.5" fill="#7a5308">g5 · J5 组（腕偏转座）</text>
|
||||
<rect x="196" y="546" width="0" height="0" fill="none"/>
|
||||
<rect x="198" y="548" width="48" height="20" rx="10" fill="#f0bcb0" stroke="#c0533a" stroke-width="1.4"/>
|
||||
<text x="256" y="564" font-size="11.5" fill="#7c2a18">g6 · J6 组（工具法兰）</text>
|
||||
<text x="235" y="608" text-anchor="middle" font-size="11.5" fill="#5c7185">胶囊 = 线段端点 a,b + 半径 r，端点是零位 CAD 世界坐标；</text>
|
||||
<text x="235" y="626" text-anchor="middle" font-size="11.5" fill="#5c7185">组 g 的胶囊随 A1..Ag 嵌套旋转（与 FK 链同一累积变换）。</text>
|
||||
|
||||
<!-- 右上：宽相位级联 -->
|
||||
<text x="700" y="64" text-anchor="middle" font-size="14.5" font-weight="bold" fill="#1f4f8f">两级宽相位 → 胶囊精确距</text>
|
||||
<rect x="470" y="80" width="470" height="58" rx="8" fill="#e6eef7" stroke="#5a86b5" stroke-width="1.3"/>
|
||||
<text x="486" y="102" font-size="12.5" font-weight="bold" fill="#153a6e">第 1 级 · 零件包围球（19 球）</text>
|
||||
<text x="486" y="122" font-size="11.5" fill="#2a4a6e">球心距 > rad_A+rad_B+rmax_A+rmax_B+margin 才筛除（阈值推导防漏判）</text>
|
||||
<line x1="705" y1="138" x2="705" y2="152" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#ca)"/>
|
||||
<rect x="470" y="154" width="470" height="58" rx="8" fill="#e6eef7" stroke="#5a86b5" stroke-width="1.3"/>
|
||||
<text x="486" y="176" font-size="12.5" font-weight="bold" fill="#153a6e">第 2 级 · 簇球（每零件胶囊按主轴分桶，每簇 ≤4 枚）</text>
|
||||
<text x="486" y="196" font-size="11.5" fill="#2a4a6e">主轴 = 胶囊中点的幂迭代方向（5 轮）；再细分实测得不偿失</text>
|
||||
<line x1="705" y1="212" x2="705" y2="226" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#ca)"/>
|
||||
<rect x="470" y="228" width="470" height="58" rx="8" fill="#dbe9fb" stroke="#3572c9" stroke-width="1.4"/>
|
||||
<text x="486" y="250" font-size="12.5" font-weight="bold" fill="#153a6e">第 3 级 · 胶囊对：线段-线段距离（Ericson 标准式）</text>
|
||||
<text x="486" y="270" font-size="11.5" fill="#2a4a6e">d(轴_a, 轴_b) < r_i + r_j + margin(0.3mm) ⇒ 干涉；无 visit 回调时首对即短路</text>
|
||||
|
||||
<!-- 胶囊距小图 -->
|
||||
<g transform="translate(490,306)">
|
||||
<rect x="0" y="18" width="120" height="34" rx="17" fill="#cfe3f5" stroke="#3572c9" stroke-width="1.4"/>
|
||||
<line x1="17" y1="35" x2="103" y2="35" stroke="#3572c9" stroke-width="1.2" stroke-dasharray="4 3"/>
|
||||
<rect x="210" y="0" width="110" height="30" rx="15" fill="#f2d5a8" stroke="#c98a18" stroke-width="1.4"/>
|
||||
<line x1="225" y1="15" x2="305" y2="15" stroke="#c98a18" stroke-width="1.2" stroke-dasharray="4 3"/>
|
||||
<line x1="110" y1="30" x2="212" y2="18" stroke="#c0533a" stroke-width="1.8" marker-start="url(#ca)" marker-end="url(#ca)"/>
|
||||
<text x="128" y="12" font-size="12" font-weight="bold" fill="#c0533a">d（轴间距）</text>
|
||||
<text x="120" y="86" font-size="12" fill="#16324f" font-family="Consolas,monospace">自由 ⇔ d ≥ r_i + r_j + margin</text>
|
||||
<text x="120" y="106" font-size="11" fill="#5c7185">胶囊并集 ⊇ 零件本体 ⇒ 「自由」可信、「干涉」需离线布尔确认</text>
|
||||
</g>
|
||||
|
||||
<!-- 右下：规则 -->
|
||||
<rect x="470" y="428" width="470" height="128" rx="8" fill="#fdf6ec" stroke="#c99418" stroke-width="1.3"/>
|
||||
<text x="486" y="452" font-size="12.5" font-weight="bold" fill="#7a5a08">三条关键规则</text>
|
||||
<text x="486" y="474" font-size="12" fill="#6b5312">· 只检 |Δg|≥2 的零件对（同组相对静止、零位已保证不干涉）；</text>
|
||||
<text x="486" y="492" font-size="12" fill="#6b5312"> 相邻对 P04×P05/P06×P07/P07×P08 同心跨轴，塞进模型零位必误报</text>
|
||||
<text x="486" y="510" font-size="12" fill="#6b5312"> ⇒ 改用 adj 一维自由区间表（CAD 实测、与姿态无关）</text>
|
||||
<text x="486" y="528" font-size="12" fill="#6b5312">· adj：J2 [−107°,+128°] · J4 [−112°,+112°] · J5 [−96°,+96°]</text>
|
||||
<text x="486" y="546" font-size="12" fill="#6b5312">· 硬边（lo 侧相邻件自碰撞）静态值即真值；可放宽侧（撞立柱）按姿态实时二分</text>
|
||||
|
||||
<rect x="470" y="566" width="470" height="58" rx="8" fill="#eef8f1" stroke="#1f6f43" stroke-width="1.3"/>
|
||||
<text x="486" y="588" font-size="12.5" font-weight="bold" fill="#14472c">性能：1995 胶囊 · collideFree ≈ 5ms/次（零位最坏）</text>
|
||||
<text x="486" y="608" font-size="11.5" fill="#2c5c44">预分配缓冲 + 代际戳缓存（免 GC）· 轴对齐特化旋转 · 热循环标量直传免建数组</text>
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 6.5 KiB |
@@ -0,0 +1,72 @@
|
||||
<svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 980 560" font-family="'Segoe UI','PingFang SC','Microsoft YaHei',sans-serif">
|
||||
<rect x="0" y="0" width="980" height="560" fill="#ffffff"/>
|
||||
<defs>
|
||||
<marker id="da" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
|
||||
<path d="M0,0 L10,5 L0,10 z" fill="#5a7d9a"/>
|
||||
</marker>
|
||||
<marker id="dor" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
|
||||
<path d="M0,0 L10,5 L0,10 z" fill="#e08a1e"/>
|
||||
</marker>
|
||||
</defs>
|
||||
<text x="490" y="34" text-anchor="middle" font-size="21" font-weight="bold" fill="#16324f">动态限位：静态值只是零位快照，真边界由碰撞模型按姿态二分</text>
|
||||
|
||||
<!-- 数轴（J5 例） -->
|
||||
<text x="490" y="70" text-anchor="middle" font-size="14" font-weight="bold" fill="#1f4f8f">以 J5 腕俯仰 + 向为例（数值取自实机数据）</text>
|
||||
<line x1="120" y1="200" x2="880" y2="200" stroke="#3d5164" stroke-width="2.5" marker-start="url(#da)" marker-end="url(#da)"/>
|
||||
<g font-size="11.5" fill="#5c7185" text-anchor="middle">
|
||||
<text x="120" y="222">−360°</text><text x="500" y="222">0°</text><text x="880" y="222">+360°（电机限位）</text>
|
||||
<line x1="500" y1="194" x2="500" y2="206" stroke="#3d5164" stroke-width="1.5"/>
|
||||
<line x1="120" y1="194" x2="120" y2="206" stroke="#8a9aa8" stroke-width="1.5"/>
|
||||
<line x1="880" y1="194" x2="880" y2="206" stroke="#8a9aa8" stroke-width="1.5"/>
|
||||
</g>
|
||||
<!-- adj 机械区间 -->
|
||||
<rect x="399" y="188" width="202" height="24" rx="4" fill="#dbe9fb" opacity="0.85" stroke="#3572c9" stroke-width="1.2"/>
|
||||
<text x="500" y="136" text-anchor="middle" font-size="12" fill="#2a4a6e">adj 机械区间（相邻件 CAD 实测）：J5 = [−96°, +96°]</text>
|
||||
<!-- 静态扫描限位 -->
|
||||
<line x1="536" y1="180" x2="536" y2="220" stroke="#1f9d63" stroke-width="3"/>
|
||||
<text x="536" y="250" text-anchor="middle" font-size="12" fill="#1f7a4d">零位静态扫描 +34°（撞立柱 P01_A）</text>
|
||||
<!-- 动态边界 -->
|
||||
<line x1="601" y1="180" x2="601" y2="220" stroke="#e08a1e" stroke-width="3"/>
|
||||
<text x="672" y="272" text-anchor="middle" font-size="12" font-weight="bold" fill="#b06a10">当前姿态动态边界 +96°</text>
|
||||
<!-- 当前角 -->
|
||||
<path d="M 521 194 l 6 12 h -12 z" fill="#16324f"/>
|
||||
<text x="489" y="172" text-anchor="end" font-size="12" font-weight="bold" fill="#16324f">当前 θ</text>
|
||||
<!-- 二分括号 -->
|
||||
<g stroke="#e08a1e" stroke-width="1.4" fill="none" stroke-dasharray="5 4">
|
||||
<path d="M 521 216 C 521 230, 601 230, 601 224" marker-end="url(#dor)"/>
|
||||
<path d="M 561 216 C 561 227, 581 227, 581 224" marker-end="url(#dor)"/>
|
||||
</g>
|
||||
<text x="490" y="292" text-anchor="middle" font-size="12" fill="#b06a10">dynLimit：从当前角向限位方向二分「collideFree」翻转点（tol 0.25°）</text>
|
||||
|
||||
<rect x="120" y="304" width="760" height="58" rx="8" fill="#f4f6f8" stroke="#9db0c0" stroke-width="1.2"/>
|
||||
<text x="490" y="326" text-anchor="middle" font-size="12.5" fill="#3d5164">为什么静态限位不可信：它来自「其它轴全零」的一次性扫描。实测 J2 正向静态 +9.75°（垂臂零位撞立柱），</text>
|
||||
<text x="490" y="346" text-anchor="middle" font-size="12.5" fill="#3d5164">J1=+90° 时动态边界放到 +108.75° —— 撞立柱这类「跨 ≥2 组」的约束必须按姿态重算，滑块才不锁死。</text>
|
||||
|
||||
<!-- solveDyn 流程 -->
|
||||
<text x="490" y="394" text-anchor="middle" font-size="14.5" font-weight="bold" fill="#1f4f8f">solveDyn：静态解 → 碰撞校验 → 贴边轴放宽重解（最多 2 轮）</text>
|
||||
<g font-size="11.5">
|
||||
<rect x="70" y="408" width="150" height="50" rx="8" fill="#e6eef7" stroke="#5a86b5" stroke-width="1.3"/>
|
||||
<text x="145" y="429" text-anchor="middle" font-weight="bold" fill="#153a6e">静态限位求解</text>
|
||||
<text x="145" y="447" text-anchor="middle" fill="#2a4a6e">solveTo（全流水线）</text>
|
||||
<line x1="220" y1="433" x2="240" y2="433" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#da)"/>
|
||||
<rect x="242" y="408" width="150" height="50" rx="8" fill="#e6eef7" stroke="#5a86b5" stroke-width="1.3"/>
|
||||
<text x="317" y="429" text-anchor="middle" font-weight="bold" fill="#153a6e">全姿态碰撞校验</text>
|
||||
<text x="317" y="447" text-anchor="middle" fill="#2a4a6e">不安全→候选表选安全解</text>
|
||||
<line x1="392" y1="433" x2="412" y2="433" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#da)"/>
|
||||
<rect x="414" y="408" width="160" height="50" rx="8" fill="#e6eef7" stroke="#5a86b5" stroke-width="1.3"/>
|
||||
<text x="494" y="429" text-anchor="middle" font-weight="bold" fill="#153a6e">贴边轴检测（0.5°内）</text>
|
||||
<text x="494" y="447" text-anchor="middle" fill="#2a4a6e">非连续轴 · 非硬边</text>
|
||||
<line x1="574" y1="433" x2="594" y2="433" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#da)"/>
|
||||
<rect x="596" y="408" width="150" height="50" rx="8" fill="#fdf2e2" stroke="#c99418" stroke-width="1.3"/>
|
||||
<text x="671" y="429" text-anchor="middle" font-weight="bold" fill="#7a5a08">dynLimit 二分边界</text>
|
||||
<text x="671" y="447" text-anchor="middle" fill="#6b5312">放宽 ≥5° 才值得重解</text>
|
||||
<line x1="746" y1="433" x2="766" y2="433" stroke="#5a7d9a" stroke-width="1.6" marker-end="url(#da)"/>
|
||||
<rect x="768" y="408" width="150" height="50" rx="8" fill="#eef8f1" stroke="#1f6f43" stroke-width="1.3"/>
|
||||
<text x="843" y="429" text-anchor="middle" font-weight="bold" fill="#14472c">放宽后重解（粗档）</text>
|
||||
<text x="843" y="447" text-anchor="middle" fill="#2c5c44">cost 不升才采用</text>
|
||||
</g>
|
||||
|
||||
<rect x="120" y="482" width="760" height="62" rx="8" fill="#fdf6ec" stroke="#c99418" stroke-width="1.3"/>
|
||||
<text x="490" y="506" text-anchor="middle" font-size="12.5" fill="#6b5312">滑块夹取 clampAxis：缓存 [轴, 其余轴 1° 量化姿态] → {lo, hi}；拖同一轴直接取缓存，</text>
|
||||
<text x="490" y="526" text-anchor="middle" font-size="12.5" fill="#6b5312">拖过边界才重算一次二分（~40ms）。当前姿已干涉时退回机械区间 —— 否则 dynLimit 会把界外值当边界，滑块再也回不来。</text>
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 6.2 KiB |
@@ -0,0 +1,56 @@
|
||||
<svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 980 440" font-family="'Segoe UI','PingFang SC','Microsoft YaHei',sans-serif">
|
||||
<rect x="0" y="0" width="980" height="440" fill="#ffffff"/>
|
||||
<defs>
|
||||
<marker id="zb" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
|
||||
<path d="M0,0 L10,5 L0,10 z" fill="#3572c9"/>
|
||||
</marker>
|
||||
<marker id="zo" viewBox="0 0 10 10" refX="9" refY="5" markerWidth="6.5" markerHeight="6.5" orient="auto-start-reverse">
|
||||
<path d="M0,0 L10,5 L0,10 z" fill="#e08a1e"/>
|
||||
</marker>
|
||||
</defs>
|
||||
<text x="490" y="34" text-anchor="middle" font-size="21" font-weight="bold" fill="#16324f">工具姿态参数化：方位角 az · 仰角 el · 滚转 roll</text>
|
||||
|
||||
<!-- 左：球面示意 -->
|
||||
<circle cx="240" cy="220" r="130" fill="#f4f8fb" stroke="#b9c8d6" stroke-width="1.4"/>
|
||||
<ellipse cx="240" cy="220" rx="130" ry="40" fill="none" stroke="#b9c8d6" stroke-width="1" stroke-dasharray="4 3"/>
|
||||
<line x1="240" y1="220" x2="240" y2="70" stroke="#3572c9" stroke-width="2" marker-end="url(#zb)"/>
|
||||
<text x="252" y="80" font-size="12.5" fill="#2a4a6e">CAD +Z</text>
|
||||
<line x1="240" y1="220" x2="400" y2="220" stroke="#8a9aa8" stroke-width="1.4"/>
|
||||
<text x="406" y="224" font-size="12" fill="#5c7185">方位基线</text>
|
||||
<!-- 工具轴 -->
|
||||
<line x1="240" y1="220" x2="296" y2="96" stroke="#e08a1e" stroke-width="2.6" marker-end="url(#zo)"/>
|
||||
<text x="306" y="94" font-size="12.5" font-weight="bold" fill="#b06a10">工具轴</text>
|
||||
<text x="306" y="112" font-size="11.5" fill="#b06a10">（法兰 +Z 外伸）</text>
|
||||
<!-- el 弧 -->
|
||||
<path d="M 240 128 A 92 92 0 0 1 277 136" fill="none" stroke="#7c5cd6" stroke-width="1.7"/>
|
||||
<text x="92" y="140" font-size="12.5" fill="#7c5cd6">el（仰角，自 +Z 抬起）</text>
|
||||
<text x="92" y="158" font-size="12.5" fill="#7c5cd6">0…180°</text>
|
||||
<line x1="216" y1="136" x2="258" y2="131" stroke="#7c5cd6" stroke-width="1" stroke-dasharray="3 3"/>
|
||||
<!-- roll 环绕工具轴 -->
|
||||
<ellipse cx="270" cy="153" rx="20" ry="9" fill="none" stroke="#d64545" stroke-width="1.7" transform="rotate(-24 270 153)"/>
|
||||
<text x="92" y="196" font-size="12.5" fill="#b03030">roll（绕轴滚转）</text>
|
||||
<text x="92" y="214" font-size="12.5" fill="#b03030">= θ6，绕工具轴</text>
|
||||
<line x1="212" y1="198" x2="256" y2="160" stroke="#d64545" stroke-width="1" stroke-dasharray="3 3"/>
|
||||
<!-- az 弧（水平面内） -->
|
||||
<path d="M 362 220 A 122 38 0 0 0 330 182" fill="none" stroke="#1f9d63" stroke-width="1.7"/>
|
||||
<text x="320" y="262" font-size="12.5" fill="#1f7a4d">az（绕 Z 方位角，−180…180°）</text>
|
||||
<circle cx="240" cy="220" r="4" fill="#16324f"/>
|
||||
<text x="160" y="248" font-size="12" fill="#5c7185">法兰中心</text>
|
||||
|
||||
<!-- 右：公式 -->
|
||||
<rect x="470" y="70" width="470" height="180" rx="8" fill="#f4f6f8" stroke="#9db0c0" stroke-width="1.2"/>
|
||||
<text x="488" y="96" font-size="13" font-weight="bold" fill="#16324f">poseMat(az, el, roll) → 工具系旋转矩阵（列 = x/y/z）</text>
|
||||
<text x="488" y="122" font-size="12.5" fill="#16324f" font-family="Consolas,monospace">z = (sin el·cos az, sin el·sin az, cos el)</text>
|
||||
<text x="488" y="146" font-size="12.5" fill="#16324f" font-family="Consolas,monospace">x0 = (−sin az, cos az, 0) （子午面基，恒⊥z）</text>
|
||||
<text x="488" y="170" font-size="12.5" fill="#16324f" font-family="Consolas,monospace">x = R(z, roll)·x0 y = z × x</text>
|
||||
<text x="488" y="200" font-size="11.5" fill="#5c7185">poseAng() 为其逆分解；el ∈ [0,180]，az/roll ∈ (−180,180]。</text>
|
||||
<text x="488" y="220" font-size="11.5" fill="#5c7185">界面以「工具朝外」描述；求解器内部法兰法向朝臂内，</text>
|
||||
<text x="488" y="236" font-size="11.5" fill="#5c7185">两者相差绕工具 x 轴 180°（RX180），入栈/出栈各翻一次。</text>
|
||||
|
||||
<rect x="470" y="266" width="470" height="140" rx="8" fill="#fdf6ec" stroke="#c99418" stroke-width="1.3"/>
|
||||
<text x="488" y="292" font-size="13" font-weight="bold" fill="#7a5a08">为什么姿态任务只剩 2 个未知数</text>
|
||||
<text x="488" y="316" font-size="12" fill="#6b5312">· θ6 只提供绕工具轴的滚转，不改变工具轴 ⇒ 工具轴 = (R1…R5)·ẑ</text>
|
||||
<text x="488" y="338" font-size="12" fill="#6b5312">· 滚转由 rollClose 闭式补出：θ6 = −atan2(N[1,0], N[0,0])，N = R5ᵀ·Rtar</text>
|
||||
<text x="488" y="360" font-size="12" fill="#6b5312">· 推论：全姿态偏差 oerr ≥ 工具轴夹角（20 万随机样本验证零违反）——</text>
|
||||
<text x="488" y="380" font-size="12" fill="#6b5312"> 粗扫用一次点积预筛（1−cos2°）挡掉必被拒的候选，省掉矩阵链+acos</text>
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 4.6 KiB |
File diff suppressed because one or more lines are too long
|
After Width: | Height: | Size: 650 KiB |
Reference in New Issue
Block a user