贴面后不再抬回悬高；MoveL 切回关节后不再把整段位移当前进速度。
搜孔开局会抬到悬高再落回 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>
This commit is contained in:
arm_dev
+17
-11
@@ -34,7 +34,7 @@
|
||||
|
||||
### 1.3 一次完整任务分几步
|
||||
|
||||
代码里跟踪和对孔串成一条命令。搜孔轻压已取消，`run_all.sh` 不会再进入那一步，也不能用参数打开。旋入是单独的 `run_stage3_search.sh --screw`。
|
||||
代码里用 **stage1 / stage2 / stage3** 三段串起来，一条命令跑完：
|
||||
|
||||
```
|
||||
run_all.sh
|
||||
@@ -46,13 +46,18 @@ run_all.sh
|
||||
│ YOLO 找工件外框 → Hough 找左右两个孔心 →
|
||||
│ 深度 + 内参 + 手眼 → 换算出孔的基座坐标 →
|
||||
│ 移动夹爪轴线到目标孔正上方 → 复拍验证 → 锁定
|
||||
│
|
||||
└─ stage3_search 柔顺搜孔：力闭环下的螺旋搜索，直到规的尖端"掉进"孔里，
|
||||
再十字微扫定心，最后在力限内下压（可选按螺距旋入）
|
||||
```
|
||||
|
||||
跟踪和对孔在同一个进程里交接。盯日志：
|
||||
三段之间**在同一个进程里交接**，所以相机、触觉、手眼 TF 只初始化一次。
|
||||
盯日志就能知道跑到哪了 —— 三行关键输出：
|
||||
|
||||
```
|
||||
[pipeline] === stage1_track === → 在等跟踪锁定
|
||||
[pipeline] === stage2_locate === → 在拍照对孔
|
||||
[pipeline] === stage3_search === → 触面 → ★ 入孔 → 轻压到位
|
||||
```
|
||||
|
||||
日常入口（三个终端，**必须同一个 `--arm`**）：
|
||||
@@ -60,7 +65,7 @@ run_all.sh
|
||||
```bash
|
||||
bash ~/ros2/scripts/run_real.sh --arm 89 # T1 控制栈，一直开着
|
||||
bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview # T2 相机预览，一直开着
|
||||
bash ~/ros2/scripts/run_all.sh --arm 89 # T3 跟踪 + 对孔（不含搜孔、不含旋入）
|
||||
bash ~/ros2/scripts/run_all.sh --arm 89 # T3 三段串联（默认不拧入）
|
||||
bash ~/ros2/scripts/run_all.sh --arm 89 --init-z 0.35 --hover-z 0.34
|
||||
```
|
||||
|
||||
@@ -68,15 +73,15 @@ bash ~/ros2/scripts/run_all.sh --arm 89 --init-z 0.35 --hover-z 0.34
|
||||
`D405_MISS_BEFORE_D435_S`（0.5 s）没有 conf≥`perception.wrist_handoff_conf`（0.5）
|
||||
的目标，才用顶视 D435 **拍一张**，把点用眼在手外 T 变到基座（不再乘法兰）。
|
||||
这一段运动是进程内厂家 `MoveL`（不杀 `run_real.sh`）：当前厂家 `tcp_pose` 加
|
||||
ROS 相对 XY，`keep_z`。硬件在 MoveL 期间不灌关节指令。到位后法兰 XY 连续停稳
|
||||
`servo.d435_handback_sec`（0.5 s）才交回 D405。赶路期间不下发 D405（两套控制互抢会晃）。
|
||||
停稳之后改回 D405，跟踪点是工件框的矩形中心，不追 k0。
|
||||
ROS 相对 XY，`keep_z`。硬件在 MoveL 期间不灌关节指令。D435 这一点变到基座系后，只下发厂家 `MoveL`。赶路期间 D405 推理开着，
|
||||
控制点截断。法兰连续停稳 `servo.d435_handback_sec`（2 s）后才打开 D405，
|
||||
追工件框的矩形中心。框心对准并停稳后才精拍 k1/k2、下臂。
|
||||
启动高度用 `--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`。
|
||||
框心对准连续 `servo.wrist_align_sec`（0.5 s）才 `/visual_servo/locked`，然后才精拍下臂。
|
||||
本分支（`exp/hole-keypoint-once`）二阶段是**单次精拍**：悬停拍一次 k1/k2，
|
||||
把框心到目标孔一次走完（步长上限 40mm）。现行实验 `hole_once_lock_xy_descend: true`：
|
||||
走完后不补拍，一条厂家 MoveL 收到贴面。`once_ff_look_per_m: 0` 时 MoveL 只降 Z
|
||||
@@ -84,7 +89,8 @@ ROS 相对 XY，`keep_z`。硬件在 MoveL 期间不灌关节指令。到位后
|
||||
不是 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。
|
||||
`tool10120938` 的 TCP 长度（0.226587 m，2026-10-16 下午 RDK 重读）加上 `final_gap_mm` 之上，不按旧 tool1 的 0.223 m。
|
||||
贴面之后就停在这个法兰高度，不抬回 `hole_hover_z_m`。没有接触传感器，到位只认 `final_gap_mm`，不另设一个 Z。
|
||||
只测 D435 这一段位移时加 `--d435-only`：不交 D405、不进二阶段，并且要求先停 `run_real.sh`。
|
||||
|
||||
```bash
|
||||
@@ -107,8 +113,8 @@ bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 --control
|
||||
|
||||
| 能力 | 状态 | 怎么自己确认 |
|
||||
|------|------|--------------|
|
||||
| 跟踪 → 对孔 | ✅ 真机跑通过。搜孔轻压已取消，这条命令不再下压搜孔 | `run_all.sh --arm 89` |
|
||||
| 按螺距旋入（螺纹规） | ✅ 单独阶段。不带 `--screw` 的旧搜孔命令会直接拒绝 | `run_stage3_search.sh --arm 89 --screw` |
|
||||
| 三段串联（跟踪 → 对孔 → 搜孔轻压） | ✅ 真机跑通 | `run_all.sh --arm 89` |
|
||||
| 按螺距旋入（螺纹规） | ✅ 有参数，`--depth`/`--pitch` 控制 | `run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0` |
|
||||
| Orisys 双指触觉（法向 + 两路切向） | ⚠️ 能读，但**轴向映射尚未实测确认** | 见 [`TACTILE.md`](TACTILE.md) |
|
||||
| Orisys 的深度图 / 力矢量图 | ⚠️ 消息结构与话题已在，真机值待确认 | 同上 |
|
||||
| D435 手外相机（一阶段一次性 + S03） | ⚠️ 89 已按 `tool10120938` 重标（24 视角，10-09 旧结果在 `results/backup/`）；S03 粗定位真机未对过 | `handeye/results/89_d435_eye_to_hand.yaml` |
|
||||
@@ -505,7 +511,7 @@ bash scripts/run_unit_tests.sh -k gauge # 只跑名字带 gauge 的
|
||||
| **89 / 91** | 左臂 / 右臂的现场编号，**各有独立的手眼、相机、夹爪、锁高，禁止混用** |
|
||||
| **法兰** | 机械臂末端安装面；手眼 `T_4x4` 仍是相机→法兰。左臂 89 现行按 Studio `tool10120938` 的 TCP 求解，再折回法兰 |
|
||||
| **eye-in-hand / eye-to-hand** | 相机装在手上 / 装在手上之外（D405 在手上，D435 在外部） |
|
||||
| **stage1/2** | 跟踪 / 对孔，见 1.3。搜孔轻压已取消 |
|
||||
| **stage1/2/3** | 跟踪 / 对孔 / 搜孔，见 1.3 |
|
||||
| **S03 / S04 / S07 / S08** | 整体装配任务书里的阶段编号（S03 粗定位、S04 跟踪、S07 柔顺下压、S08 夹爪旋转循环）。代码里对应的是 stage1/2/3 的拆分实现，**没有一一对应**，别硬套 |
|
||||
| **基线** | 触觉静止时的读数均值。力的语义是"相对基线的增量"，不是绝对牛顿值 |
|
||||
| **R1.1 / M1 …** | [`FRAMEWORK_RULES.md`](FRAMEWORK_RULES.md) 里的规则编号，文档和提交信息里直接引用 |
|
||||
|
||||
+15
-14
@@ -57,7 +57,8 @@ hole_detect:
|
||||
|
||||
## 三阶段串联（推荐日常入口）
|
||||
|
||||
一条命令跑完：**视觉跟踪锁定 → 拍照对孔**。搜孔轻压已取消，这条命令不再下压，也不能用参数重新打开。旋入是后面的单独命令。
|
||||
一条命令跑完：**视觉跟踪锁定 → 拍照对孔 → 搜孔并轻轻按压**。默认**不拧入**
|
||||
（拧入是后面单独阶段）。三个阶段在同一个节点里交接，相机 / 触觉 / 手眼 TF 只起一次。
|
||||
|
||||
```bash
|
||||
# 终端 1 —— 控制栈（一直开着）
|
||||
@@ -66,12 +67,12 @@ 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 # 跟踪 + 对孔
|
||||
# 终端 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_stage3_search.sh --arm 89 --screw
|
||||
# 若这一次就要拧入（一般留给后续阶段）：
|
||||
bash ~/ros2/scripts/run_all.sh --arm 89 --depth 6 --pitch 1.0
|
||||
```
|
||||
|
||||
`run_pipeline.sh` 是 `run_all.sh` 的别名。
|
||||
@@ -84,9 +85,9 @@ bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw
|
||||
1. `[pipeline] === stage1_track ===` → 等跟踪锁定
|
||||
2. `一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段`
|
||||
3. `[pipeline] === stage2_locate ===` → 降到孔悬停高，拍照对准
|
||||
对孔结束后臂停在当前高度。搜孔轻压不再接着跑。
|
||||
4. `[pipeline] === stage3_search ===` → `触面` → `★ 入孔` → `轻压到位`（不拧入）
|
||||
|
||||
急停：终端 3 Ctrl-C。失败时臂保持当前位姿，看
|
||||
急停：终端 3 Ctrl-C。搜孔中会先抬 5mm。失败时臂保持当前位姿，看
|
||||
`/tmp/pipeline_stage1.log`、`/tmp/stage2_result.json`、`/tmp/stage3_result.json`。
|
||||
|
||||
高度接口：`robots[89].servo.ee_z_m`（跟踪锁高）、
|
||||
@@ -148,10 +149,7 @@ bash ~/ros2/scripts/run_all.sh --arm 89 --final-gap 8 # 锁 XY，规
|
||||
| `/stage2/result` | `std_msgs/String` JSON：k1/k2 基座坐标、残差、悬停高 |
|
||||
| `/stage2/overlay` | `sensor_msgs/Image` 标注图（同时落盘 /tmp/stage2_overlay.png） |
|
||||
|
||||
## 旋转装配（原三阶段节点；搜孔轻压入口已取消）
|
||||
|
||||
不带 `--screw` 时 `run_stage3_search.sh` 直接退出，不会触面，也不会螺旋搜孔。
|
||||
下面是这个节点在旋入时仍会走的力控步骤，不是一条还能单独打开的搜孔命令。
|
||||
## 三阶段柔顺搜孔（stage3，Orisys 触觉 + 力闭环螺旋）
|
||||
|
||||
前置：二阶段刚跑完（臂悬停在目标孔上方、垂直朝下、夹持通止规）。流程：
|
||||
|
||||
@@ -169,9 +167,12 @@ bash ~/ros2/scripts/run_all.sh --arm 89 --final-gap 8 # 锁 XY，规
|
||||
7. **hold** — 锁定关节，发布结果
|
||||
|
||||
```bash
|
||||
# 前置：T1 run_real.sh --arm 89；对孔已完成（臂保持不动）
|
||||
# 不带 --screw 会直接退出，不会搜孔。
|
||||
bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw
|
||||
# 前置：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 读数是形变任意单位且
|
||||
|
||||
Reference in New Issue
Block a user