arm_dev
7af9e3d4b6
搜孔开局会抬到悬高再落回 hole_hover_z_m，螺纹还没咬合臂就离开孔口。没有接触传感器，到位只认 final_gap 算出的法兰高度，贴面后停在这个 Z。09:25 厂家 MoveL 切回 NRT_JOINT_POSITION 的第一拍把整段关节差算成 1.686 rad/s，前瞻再推 0.07 rad，光心从 5 mm 冲到 276 mm 后急停。切回后钉住结束瞬间的关节角并清零速度，JTC 还指着出发前的关节时不放行。 Co-authored-by: Cursor <cursoragent@cursor.com>
194 lines
11 KiB
Markdown
194 lines
11 KiB
Markdown
# 入孔检测
|
||
|
||
只做视觉定位（像素孔心），**不包含**装配插入运动 / HMI。跟踪仍用 `run_visual_servo_rt.sh`。
|
||
|
||
日常改 **`config/site.yaml` → `hole_detect:`**（Hough、居中门控、间距都会覆盖包内 yaml）。
|
||
`src/arm_application/config/hole_detect.yaml` 只是缺省值。
|
||
|
||
源码：
|
||
|
||
| 文件 | 作用 |
|
||
|------|------|
|
||
| `src/arm_application/arm_application/hole_detector.py` | 统一入口：`yolo` / `hough` / `two_stage` |
|
||
| `src/arm_application/arm_application/hole_detect_hough.py` | YOLO 工件框 + HoughCircles 左右孔 |
|
||
| `src/arm_application/arm_application/hole_detect_node.py` | 订当前臂 D405 彩图 |
|
||
| `src/arm_application/config/hole_detect.yaml` | `two_stage` 参数 |
|
||
| `src/arm_application/config/hole_insert.yaml` | `yolo`/`hough` 参数 |
|
||
| `models/hole_insert/` | 孔模型权重（二进制不进 git） |
|
||
|
||
## 跑
|
||
|
||
先开该臂相机，再开检测：
|
||
|
||
```bash
|
||
bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
|
||
bash ~/ros2/scripts/run_hole_detect.sh --arm 89 --preview
|
||
```
|
||
|
||
91 把 `--arm` 换成 `91`（话题 `/d405_right/...`）。
|
||
|
||
`site.yaml`：
|
||
|
||
```yaml
|
||
hole_detect:
|
||
backend: "two_stage" # two_stage | yolo | hough
|
||
weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx"
|
||
workpiece_weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx" # 空 = paths.yolo_weights
|
||
conf: 0.25
|
||
hough_param2: 26
|
||
hole_distance_min: 150
|
||
```
|
||
|
||
- `two_stage`：用工件 YOLO 找 bbox，再在 ROI 里 Hough 找左右螺纹孔（原 `camera_test/hole_detect.py`）。
|
||
- `yolo`：模型框直接当孔（原 `hole_insert` HoleDetector）。
|
||
- `hough`：整幅图圆检测，不用权重。
|
||
|
||
权重放到 `~/ros2/models/hole_insert/`（当前 `detect_best_1.onnx`，二进制不进 git，布局见 `models/README.md`）。`two_stage` 的工件模型默认就是跟踪用的 `paths.yolo_weights`（`hole_detect.workpiece_weights` 可覆盖）。
|
||
|
||
## ROS
|
||
|
||
| 话题 | 类型 |
|
||
|------|------|
|
||
| `/hole_detect/result` | `arm_msgs/msg/HoleDetections` |
|
||
| `/hole_detect/holes` | `std_msgs/Float32MultiArray` `[ok, n, u,v,r,conf, …]` |
|
||
| `/hole_detect/overlay` | `sensor_msgs/Image` 画框画孔 |
|
||
|
||
改 `hole_detect.yaml` 的 Hough / 居中门控后重启 `run_hole_detect.sh`。`two_stage` 会做时序稳帧（`stabilize_*`）。
|
||
|
||
## 三阶段串联（推荐日常入口）
|
||
|
||
一条命令跑完：**视觉跟踪锁定 → 拍照对孔 → 搜孔并轻轻按压**。默认**不拧入**
|
||
（拧入是后面单独阶段）。三个阶段在同一个节点里交接，相机 / 触觉 / 手眼 TF 只起一次。
|
||
|
||
```bash
|
||
# 终端 1 —— 控制栈（一直开着）
|
||
bash ~/ros2/scripts/run_real.sh --arm 89
|
||
|
||
# 终端 2 —— 相机预览（一直开着，看跟踪画面）
|
||
bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
|
||
|
||
# 终端 3 —— 串联（搜到孔后轻压保持，不拧）
|
||
bash ~/ros2/scripts/run_all.sh --arm 89 # 跟踪 + 对孔 + 搜孔轻压
|
||
bash ~/ros2/scripts/run_all.sh --arm 89 --hole k2 # 换 k2
|
||
bash ~/ros2/scripts/run_all.sh --arm 89 --skip-stage1 # 臂已在工件上方，跳过跟踪
|
||
# 若这一次就要拧入（一般留给后续阶段）：
|
||
bash ~/ros2/scripts/run_all.sh --arm 89 --depth 6 --pitch 1.0
|
||
```
|
||
|
||
`run_pipeline.sh` 是 `run_all.sh` 的别名。
|
||
|
||
**跑之前确认**：夹爪夹着通止规；工件在相机大概能看见的位置（一阶段会自己找）。
|
||
左指触觉如果掉线会自动降级成单指。
|
||
|
||
盯这几行就知道跑到哪了：
|
||
|
||
1. `[pipeline] === stage1_track ===` → 等跟踪锁定
|
||
2. `一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段`
|
||
3. `[pipeline] === stage2_locate ===` → 降到孔悬停高，拍照对准
|
||
4. `[pipeline] === stage3_search ===` → `触面` → `★ 入孔` → `轻压到位`（不拧入）
|
||
|
||
急停：终端 3 Ctrl-C。搜孔中会先抬 5mm。失败时臂保持当前位姿，看
|
||
`/tmp/pipeline_stage1.log`、`/tmp/stage2_result.json`、`/tmp/stage3_result.json`。
|
||
|
||
高度接口：`robots[89].servo.ee_z_m`（跟踪锁高）、
|
||
`robots[89].servo.hole_hover_z_m`（拍照悬停高）。
|
||
|
||
分阶段单独跑仍用 `run_visual_servo_rt.sh`、`run_stage2_locate.sh`、
|
||
`run_stage3_search.sh`，下面两节。
|
||
|
||
## 二阶段一次性拍照定位（stage2，与一阶段跟踪分开跑）
|
||
|
||
先悬停垂直朝下 → 拍一张照 → `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 封顶）。
|
||
|
||
现行实验 `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 定稿）。通止规夹在爪里，相对法兰轴线
|
||
> 可以整体偏出若干毫米；旧写法把**法兰**对到孔上，规尖就偏着压在孔沿上，轻降一压
|
||
> 腕力蹦到 63N。现在每一帧同时解「规尖基座 XY」和「孔基座 XY」——两者走同一条手眼
|
||
> 链，链路误差与装夹变动一起抵消。规尖偏移是**刚体量**，量准一份（`tip_calib_n` 拍
|
||
> 一致）就冻结。
|
||
> 真机验证（arm 89，独立重测那一帧）：规尖到 k1 **0.34mm**、到 k2 61.15mm、
|
||
> 反投影工具长 222mm（与 site 的 223mm 一致），偏移冻结值 (2.6, −1.0)mm。
|
||
> 相关开关在 `site.yaml → hole_detect.stage2:`（`align_tip_vision` / `tip_*`）。
|
||
> 量不到规尖偏移时二阶段**判失败**（exit 8），不会拿"法兰对上了"冒充成功。
|
||
|
||
```bash
|
||
# 前置：T1 run_real.sh --arm 89；T3 run_camera_d405.sh --arm 89 --preview
|
||
bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 # 完整流程（自动拉起触觉服务，仅左臂）
|
||
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 在同名段）。
|
||
工件权重：`site.yaml → hole_detect.workpiece_weights`（默认 `models/hole_insert/detect_best_1.onnx`）。
|
||
其余二阶段参数：`site.yaml → hole_detect.stage2:`（对准容差、迭代次数、垂直判据等）。
|
||
|
||
| 话题 | 类型 |
|
||
|------|------|
|
||
| `/stage2/state` | `std_msgs/String` 阶段状态机 |
|
||
| `/stage2/result` | `std_msgs/String` JSON：k1/k2 基座坐标、残差、悬停高 |
|
||
| `/stage2/overlay` | `sensor_msgs/Image` 标注图（同时落盘 /tmp/stage2_overlay.png） |
|
||
|
||
## 三阶段柔顺搜孔（stage3，Orisys 触觉 + 力闭环螺旋）
|
||
|
||
前置：二阶段刚跑完（臂悬停在目标孔上方、垂直朝下、夹持通止规）。流程：
|
||
|
||
1. **verify** — 垂直度核验；Orisys 双指触觉话题判活（connected=true 才算数），
|
||
并自动测 ~1.2s **静止基线 μ0/σ0**（日志打印「触觉基线/派生阈值」两行）
|
||
2. **approach** — 1.5mm 小步下降，`fnormal > μ0 + max(4σ0, touch_delta)` 判定触面，记录 `z_contact`
|
||
3. **seat** — Z 力闭环建立轻触（fn ≈ 触面阈值 + `press_delta`）
|
||
4. **spiral** — 阿基米德螺旋搜孔：XY 沿螺旋走（螺距 `spiral_pitch_mm`，保证轨迹重叠
|
||
覆盖），Z 由力闭环维持轻触；`fnormal` 相对接触均值跌落 `drop_ratio` 持续
|
||
`drop_frames` 帧 → **尖端入孔**。找不到则每次扩大 `spiral_expand_step_mm` 再搜
|
||
5. **center** — 入孔点十字微扫（±`center_scan_mm`），剪切力最小点 = 孔心
|
||
6. **insert** — 力限下压 `insert_depth_mm`（超限回退微抖重试 `insert_retry` 次）；
|
||
`screw_pitch_mm ≠ 0` 时按螺距同步绕世界 Z 旋入（**螺纹通止规**：>0 右旋 / <0 左旋；
|
||
光面规保持 0）。`insert_depth_mm: 0` = 只搜孔定心不插入
|
||
7. **hold** — 锁定关节，发布结果
|
||
|
||
```bash
|
||
# 前置：T1 run_real.sh --arm 89；二阶段已完成（臂保持不动）
|
||
bash ~/ros2/scripts/run_stage3_search.sh --arm 89 # 搜孔+落座
|
||
bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 0 # 只搜孔定心，不插入
|
||
bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0 # M6 螺纹规旋入
|
||
bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --r-max 5 # 临时改搜索半径
|
||
bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --resnap # 搜前用 D405 复拍对准一次
|
||
```
|
||
|
||
**力阈值是「相对静止均值的增量」，不用标定牛顿**：Orisys 读数是形变任意单位且
|
||
夹持基线会漂移，节点每次启动自动测基线后派生全部阈值。要调灵敏度就改
|
||
`site.yaml → hole_detect.stage3:` 的 `touch_delta / press_delta / safe_press_delta /
|
||
shear_delta`（日志里能看到实际派生值）。
|
||
|
||
安全（任一触发立即抬 5mm 中止）：Orisys `fnormal > μ0 + safe_press_delta`；腕部 F/T
|
||
`|F| > ft_safe_n`（订 `/<sn>/flexiv_robot_states`，订不到只警告）；触觉断流 >
|
||
`tactile_timeout_s`。切向力超 `μ0shear + shear_delta` 自动抬 0.3mm 减拖。
|
||
|
||
| 话题 | 类型 |
|
||
|------|------|
|
||
| `/stage3/state` | `std_msgs/String` 阶段状态机 |
|
||
| `/stage3/result` | `std_msgs/String` JSON：孔心基座坐标、触面 Z、插入深度、峰值力 |
|
||
|
||
结果同时落盘 `/tmp/stage3_result.json`。退出码：0 成功；3 未触面；4 搜孔失败；
|
||
5 插入受阻；6 安全中止；7 触觉断流。
|