空回微抬改成一条厂家 MoveL，并按实测抬高降回。默认 Z50 会在还差约 0.5mm 时就报到位，微升降改用 ZFine 和到位等级 1。合到 0 时固件常报 grip=1，手指已在合拢端就继续顺时针，避免最后一转永远不发。快转样条按峰速不超过 0.90 rad/s 排程，避免时间到了还差几十度。

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-10-19 19:02:55 +08:00
parent d8942defd1
commit 55acdb638d
11 changed files with 311 additions and 30 deletions
@@ -93,7 +93,7 @@ ROS 相对 XY，`keep_z`。硬件在 MoveL 期间不灌关节指令。D435 这
 `tool10120938` 的 TCP 长度（0.226587 m，2026-10-16 下午 RDK 重读）加上 `final_gap_mm` 之上，不按旧 tool1 的 0.223 m。
 贴面之后就停在这个法兰高度，不抬回 `hole_hover_z_m`。没有接触传感器。
 `stage2.hole_entry_mm`（现为 1）> 0 时，插孔目标改成规尖在工件上表面以下这么深（孔口凹槽），再转 A7；为 0 时仍停在 `final_gap_mm`。进孔超过 8 mm（板厚 12 mm）直接拒绝。2026-10-19 12:20 写成 4 mm 时，臂在面下 1.6 mm 被挡住。
-然后只转左臂 A7（厂家 `SendJointPosition` 的第 7 轴，模式 `NRT_JOINT_POSITION`）：先逆时针转到软件限位内侧 `extreme_margin_deg`，再顺时针转到另一侧。顺时针到位记为「首轮咬合结束」。接着 `run_gripper.sh --arm <臂号> --cli pos 400` 把平行爪只张开到 400（不全开），A7 逆时针回到限位，`--cli close` 夹住规，再顺时针转到限位。前六轴不动，不改法兰 Z。顺时针是进孔，逆时针是出孔。夹爪命令失败或状态没到，就停在当前关节。
+然后只转左臂 A7（厂家 `SendJointPosition` 的第 7 轴，模式 `NRT_JOINT_POSITION`）：先逆时针转到软件限位内侧 `extreme_margin_deg`，再顺时针转到另一侧。顺时针到位记为「首轮咬合结束」。接着 `run_gripper.sh --arm <臂号> --cli pos` 加上 `unwind_open_permille`（现为 400，不全开），再用一条厂家 `MoveL` 只抬 TCP 的 Z `unwind_lift_mm`（现为 3，即 0.3 cm；XY 和姿态不动，不拆成关节小跳）。这条和随后的下降都带 `zoneRadius=ZFine`、`targetTolerLevel=1`：厂家默认 `Z50` 和等级 3 会在还差约 0.5 mm 时就报到位，下降和记住的上抬对不上，合爪就不会发。A7 逆时针回到限位。请求速度是接口上限 0.90 rad/s；两端速度为 0 的样条峰速约是平均的 1.5 倍，所以整段按平均约 34 °/s 排程，避免时间到了还差几十度。然后下降刚才实测的那一段（不另写一个下降量），`--cli close`。合爪目标是位置 0；固件在位置先到时写 grip=1（到位），力先到才写 grip=2（夹住）。手指已经在合拢端就继续，还停在张开位则不转。再按 `extreme_speed_deg_s` 顺时针转到限位。前六轴不动。顺时针是进孔，逆时针是出孔。夹爪或这一段 Z 对不上，就停在当前关节。
 只测 D435 这一段位移时加 `--d435-only`：不交 D405、不进二阶段，并且要求先停 `run_real.sh`。

 ```bash