悬停走完后常数前馈会往 +X 过补约 1.3mm、插不进,贴面改成笛卡尔锁 XY、只降 Z 的厂家 MoveL;D435 交回也不再杀栈,只等法兰停稳 0.5 秒。

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
2026-10-15 10:23:52 +08:00
parent 37b85aa09b
commit 5c7966d8cd
16 changed files with 1024 additions and 117 deletions
+9 -5
View File
@@ -72,17 +72,21 @@ bash ~/ros2/scripts/run_all.sh --arm 89 --init-z 0.35 --hover-z 0.34
一阶段默认 **D405 实时推理**,D435 待命。YOLO-pose 这一路不关。连续
`D405_MISS_BEFORE_D435_S`(0.5 s)没有 conf≥`perception.wrist_handoff_conf`(0.5)
的目标,才用顶视 D435 **拍一张**,把点用眼在手外 T 变到基座(不再乘法兰)。
这一段运动是厂家 `MoveL`(SDK `vel` 0.3 m/s、`acc` 1.5 m/s²):脚本先停真机控制栈(感知不停),XY 对准该点,
工具尖高度保持当前,再冷启动 `run_real.sh --no-stop`。冷启动回来之后才开始计
`servo.d435_handback_sec`(3 s)。赶路和杀栈期间不下发 D405。
这一段运动是进程内厂家 `MoveL`(不杀 `run_real.sh`):当前厂家 `tcp_pose` 加
ROS 相对 XY,`keep_z`。硬件在 MoveL 期间不灌关节指令。到位后法兰 XY 连续停稳
`servo.d435_handback_sec`(0.5 s)才交回 D405。赶路期间不下发 D405(两套控制互抢会晃)。
停稳之后改回 D405,跟踪点是工件框的矩形中心,不追 k0。
启动高度用 `--init-z`(法兰 Z,米),对孔悬停用 `--hover-z`。两个都是法兰高度。
下臂贴面用 `--final-gap`(规尖离面,毫米;site 默认 `final_gap_mm` 2)。笛卡尔锁 XY
时只改 Z:法兰 = 工具长 + 工件面 + 间隙。不能和 `--no-descent` 一起写。
会让工具尖进入工件、或超出工作盒的值直接拒绝,不会改成另一个高度再往下走。
不写则一阶段锁高仍按 site,对孔悬停仍是 `hole_hover_z_m`(89 为 0.320)。
框心对准连续 `servo.wrist_align_sec`(0.5 s)→ `/visual_servo/locked`。
本分支(`exp/hole-keypoint-once`)二阶段是**单次精拍**:悬停拍一次 k1/k2,
把框心到目标孔一次走完(步长上限 40mm,不再截成 12mm),先降到观察高约 15mm 再交孔,斜线一步把 XY 补偿(≤3mm)和剩余 Z 收到贴面。轻降接触门控是 `final_gap+4mm`。
`hole_detect.stage2.hole_keypoint_once: true`。看 `/tmp/stage2_result.json` 的 `snaps`。
把框心到目标孔一次走完(步长上限 40mm)。现行实验 `hole_once_lock_xy_descend: true`:
走完后不补拍,一条厂家 MoveL 收到贴面。`once_ff_look_per_m: 0` 时 MoveL 只降 Z
(笛卡尔锁 XY);k>0 才沿 -look 加前馈。目标 = 当前厂家 `tcp_pose` + 相对位移,
不是 ROS 绝对坐标。保存点 `37b85aa` 是观察高 15mm 再斜线补 XY。看 `/tmp/stage2_result.json`。
`main` 保存点仍是多走多拍。
孔坐标把关键点像素交到 `workpiece_surface_z_m`,不读深度图。下臂停在
`tool10120938` 的 TCP 长度(0.2245 m)加上 `final_gap_mm` 之上,不按旧 tool1 的 0.223 m。
+16 -5
View File
@@ -100,13 +100,23 @@ bash ~/ros2/scripts/run_all.sh --arm 89 --depth 6 --pitch 1.0
先悬停垂直朝下 → 拍一张照 → `detect_best_1.onnx` 框工件 → Hough 解 k1/k2 孔心 →
像素射线交到 `workpiece_surface_z_m`(不读深度图)+ 内参 + 手眼换算基座坐标 →
移动**规尖**到目标孔正上方。
移动**规尖**到目标孔正上方。`hole_visual_only` 时粗居中框心也走同一条平面交点,
不用深度(30° 相机下深度框心会让 `K1.x−规尖.x` 随站位翻号)。
本分支 `hole_keypoint_once: true`:悬停拍一次,把框心到 k1 的半孔距(约 30mm)
一次走完(不再用多走多拍的 12mm 封顶),走后核验只记账。轻降先到观察高约 15mm
(单步 20mm,约 3 步),再交孔测补偿,斜线一步把 XY(≤3mm)和剩余 Z 收到 `final_gap_mm`。
规尖沿用悬停冻结值,不在近距重测。两孔间距掉出 50–70mm 或要补超过 8mm 则不动、
也不贴面。轻降力接触门控是 `final_gap_mm + 4mm`(贴面 2mm 时 = 6mm)。看
一次走完(不再用多走多拍的 12mm 封顶)。
现行实验 `hole_once_lock_xy_descend: true`:走完后不补拍,一条进程内厂家
MoveL 收到 `final_gap_mm`(规尖离面,毫米;`--final-gap` 覆盖,默认 2)。
`once_ff_look_per_m: 0` 时 ΔXY=0、只降 Z(笛卡尔锁 XY)。k>0 才
`ΔXY = -look × k × 剩余下降`。法兰 Z = 工具长 + 工件面 + 间隙,不许直接写法兰高度。MoveL 目标是**当前厂家 `tcp_pose` 加
相对位移**,不是 ROS 规尖绝对坐标。
2026-10-14:k=0.018 在框心改平面交点后,D435+D405 与单 D405 下臂都往 +X 过补
约 1.3mm、插不进——悬停 X 已对上,常数前馈是多余的。锁 XY 折线失败是关节 hop
漂 XY,不是笛卡尔 MoveL 锁 XY。对照 `37b85aa`:观察高约 15mm 再交孔斜线补 XY。
前馈超过 8mm 或工作盒会截断则不动。没有新鲜 `tcp_pose` 则不下发。
轻降力接触门控是 `final_gap_mm + 4mm`(贴面 2mm 时 = 6mm)。看
`/tmp/stage2_result.json` 的 `snaps[]`。`main` 保存点是多走多拍闭环。
> **对的是"规尖",不是"法兰轴线"**(2026-10-08 定稿)。通止规夹在爪里,相对法兰轴线
@@ -126,6 +136,7 @@ bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only # 只垂直+悬
bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hole k2 # 对准 k2(默认 k1)
bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-z 0.28 # 临时改悬停高,不动 yaml
bash ~/ros2/scripts/run_all.sh --arm 89 --no-descent # 只验证精对准、不轻降
bash ~/ros2/scripts/run_all.sh --arm 89 --final-gap 8 # 锁 XY,规尖停在离面 8mm
```
悬停锁高接口:`site.yaml → robots[89].servo.hole_hover_z_m`(91 在同名段)。