arm_dev
6edb8d39ab
在开始'设备契约层 + 目录重构 + 通止规装配'改造之前，先钉住当前可运行状态， 使后续任何删除/重构都可回退。 本基线包含： - 现有 ROS 2 工作区全部源码（arm_application / arm_control / arm_bringup 等） - 官方 flexiv_ros2 栈（含 flexiv_hardware / flexiv_controllers 等符号链接） - 设备契约层 src/arm_interfaces（新增，46 项单测通过） - devices/ 设备选型与品牌清单（新增） - scripts/gen_site_example.py 与由它生成的 config/site.example.yaml 按 .gitignore 排除：build/ / install/ / log/ / config/site.yaml / 模型权重二进制。
165 lines
8.2 KiB
Markdown
165 lines
8.2 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/best.pt"
|
||
workpiece_weights: "" # 空 = 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/best.pt`。`two_stage` 的工件模型默认就是跟踪用的 `paths.yolo_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 孔心 →
|
||
深度（孔外工作面中值）+ 内参 + 手眼换算基座坐标 → 移动夹爪/通止规轴线到目标孔正上方 →
|
||
复拍验证（≤`align_max_iters` 次，收敛判据 `align_tol_m`）→ 锁定保持。
|
||
|
||
```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
|
||
```
|
||
|
||
悬停锁高接口：`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 触觉断流。
|