orisys/ros_contorl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files
config/site.example.yaml
+8 -4
View File
@@ -592,8 +592,8 @@ servo:
lock_hold_sec: 0.7 # 旧死区驻留（混合模式不再用它进二阶段）
# 混合模式必填。交回 D405 的等待不再用它，见 d435_handback_sec。
lock_still_sec: 3.0
# MoveL 后杀栈冷启动回来，再叠这么久才交回 D405。流畅度先不管，停稳优先。
d435_handback_sec: 3.0
# 进程内 MoveL 到位后，法兰 XY 连续停稳这么久才交回 D405。
d435_handback_sec: 0.5
# 框心对准并且法兰停稳这么久，才精拍一次小孔关键点、进对孔。实时点到此为止。
wrist_align_sec: 0.5
lock_still_move_m: 0.015 # 15 mm；超过就重计时，避免"晃一下刚好满 3 秒"
@@ -770,9 +770,13 @@ hole_detect:
align_speed: 0.014 # 对准段笛卡尔速度 [m/s]（原0.03，放缓防冲）
move_speed: 0.022 # 升降/粗居中段速度 [m/s]（原0.05，放缓防震规）
hole_visual_only: true
# 本分支实验：true=悬停拍一次并走完到孔，下降后再补 XY；main 保存点应保持 false。
# 本分支实验：true=悬停拍一次并走完到孔；main 保存点应保持 false。
hole_keypoint_once: true
final_gap_mm: 2.0 # 对准后贴面轻降目标：规尖悬在孔面上方这么高 [mm]（不接触）
# true=悬停走完后一条厂家 MoveL 贴面。对照观察高补交时改 false。
hole_once_lock_xy_descend: true
# 0=笛卡尔锁 XY 只降 Z。k>0 才沿 -look 加前馈。
once_ff_look_per_m: 0.0
final_gap_mm: 2.0 # 对准后贴面目标：规尖离面 [mm]。CLI --final-gap 覆盖。
observe_gap_mm: 15.0 # --no-descent 时的观测下降：只降到离面这么高 [mm]，
# 方便看规尖偏没偏。必须 ≥ final_gap_mm。
# 左臂89：悬停间隙约 75mm（0.320−0.223−0.022）⇒ 大约下降 60mm。
config/site.yaml
+11 -4
View File
@@ -579,8 +579,9 @@ servo:
lock_hold_sec: 0.7 # 旧死区驻留（混合模式不再用它进二阶段）
# 混合模式必填。交回 D405 的等待不再用它，见 d435_handback_sec。
lock_still_sec: 3.0
# MoveL 后杀栈冷启动回来，再叠这么久才交回 D405。流畅度先不管，停稳优先。
d435_handback_sec: 3.0
# 进程内 MoveL 到位后，法兰 XY 连续停稳这么久才交回 D405。
# 旧 3s 是杀栈冷启动的垫时间；不杀栈后只留机械停稳。晃了把数加大，不许再杀栈。
d435_handback_sec: 0.5
# 框心对准并且法兰停稳这么久，才精拍一次小孔关键点、进对孔。实时点到此为止。
wrist_align_sec: 0.5
lock_still_move_m: 0.015 # 15 mm；超过就重计时，避免"晃一下刚好满 3 秒"
@@ -759,9 +760,15 @@ hole_detect:
# 对小孔关键点：只用像素射线交到 workpiece_surface_z_m，不读深度图。
hole_visual_only: true
# 本分支实验（exp/hole-keypoint-once）：true=悬停拍一次，框心→k1 一次走完
# （40mm 上限），下降后再补交平面漂的 XY。main 保存点是 false（多走多拍）。
# （40mm 上限）。main 保存点是 false（多走多拍）。
hole_keypoint_once: true
final_gap_mm: 2.0 # 对准后贴面轻降目标：规尖悬在孔面上方这么高 [mm]（不接触）
# true=悬停走完后一条厂家 MoveL 贴面（不补拍）。对照 37b85aa 改回 false。
hole_once_lock_xy_descend: true
# 贴面相对悬停：沿 -look 走 k×剩余下降 [m/m]。0=笛卡尔锁 XY 只降 Z。
# 0.018 在框心改平面交点后两条链路都 +1.3mm X 过补，插不进。
once_ff_look_per_m: 0.0
final_gap_mm: 2.0 # 对准后贴面目标：规尖离面 [mm]（不接触）。笛卡尔锁 XY
# 时法兰 Z = 工具长 + 工件面 + 本值。CLI --final-gap 覆盖。
observe_gap_mm: 15.0 # --no-descent 时的观测下降：只降到离面这么高 [mm]，
# 方便看规尖偏没偏。必须 ≥ final_gap_mm。
# 左臂89：悬停间隙约 73.5mm（0.320−0.2245−0.022）⇒ 观测下降约 58mm。
docs/HANDBOOK.md
+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。
docs/HOLE_DETECT.md
+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 在同名段）。
handeye/poses/89_eye_to_hand.yaml
+22 -28
View File
@@ -7,54 +7,48 @@
/**:
ros__parameters:
num_poses: 24
num_poses: 21
waypoint_positions: [
# pose 0
-1.241563, -0.359631, 0.346153, 2.172565, -0.179027, 0.929920, -0.778876,
-0.798667, -0.611079, 0.054912, 2.246988, 0.049199, 1.389219, 0.196135,
# pose 1
-1.388832, -0.554698, 0.416075, 1.897617, -0.358341, 0.827495, -0.762302,
-0.787304, -0.531383, 0.240108, 2.183440, 0.367026, 1.370311, 0.472489,
# pose 2
-1.436048, -0.573046, 0.461762, 1.870967, -0.387696, 0.813090, -0.762917,
-1.615896, -0.687070, 1.187600, 2.243693, -0.162789, 1.650045, 1.026121,
# pose 3
-1.614491, -0.519365, 0.693991, 2.156193, -0.499113, 0.968762, 0.073287,
-1.787431, -0.668004, 1.408195, 2.025431, -0.064718, 0.565415, 1.324635,
# pose 4
-2.020087, -0.869088, 1.059166, 2.155478, -1.198473, 0.740567, 0.478840,
-2.024343, -0.863793, 1.349640, 2.359469, -0.720629, 0.084467, 0.821412,
# pose 5
-2.082145, -1.122210, 1.203011, 1.950272, -1.269847, 0.434138, -0.641975,
-1.884100, -1.117584, 1.183093, 2.127937, -1.258013, 0.151934, 0.985555,
# pose 6
-1.983153, -0.886082, 0.999329, 2.324821, -1.272773, 0.754127, 0.313613,
-1.866465, -1.194137, 1.143263, 1.872811, -1.320028, 0.280719, 0.777663,
# pose 7
-2.021479, -0.886609, 1.055679, 2.337716, -1.282773, 0.669319, -0.028355,
-1.729402, -1.139685, 1.127574, 1.913544, -1.329630, -0.194162, 1.383834,
# pose 8
-2.022792, -0.947211, 1.118194, 2.127275, -1.211995, 0.527528, -0.169900,
-1.611937, -1.158158, 1.064049, 1.918313, -1.480676, -0.392994, 1.833894,
# pose 9
-2.004766, -1.065820, 1.183799, 1.901084, -1.217488, 0.380285, -0.312733,
-2.252978, -1.387014, 1.686186, 2.101298, -1.333349, -0.111994, 0.036817,
# pose 10
-1.942863, -1.219617, 1.256733, 1.653113, -1.222302, 0.261818, -0.420706,
-2.179722, -1.297659, 1.497171, 2.167675, -1.272158, 0.100548, 0.673603,
# pose 11
-2.039558, -1.082441, 1.189833, 1.996967, -1.241252, 0.521591, 0.526563,
-1.921897, -1.543024, 1.000863, 1.897273, -1.455972, 1.437388, -0.140968,
# pose 12
-2.098074, -1.006236, 1.538183, 2.118382, -0.737422, 0.019903, 0.648802,
-1.884059, -1.063347, 1.439248, 2.253200, -1.164963, -0.423306, 0.907197,
# pose 13
-2.039609, -1.159222, 1.488501, 1.908579, -1.168793, -0.140451, 0.034889,
-1.987022, -1.235459, 1.354108, 2.370862, -1.354044, 0.065378, 0.923079,
# pose 14
-1.880783, -1.104601, 1.579382, 2.036427, -0.665049, 0.254105, 1.518504,
-1.770869, -1.333861, 1.241996, 1.800723, -1.395819, -0.255590, 1.389660,
# pose 15
-2.015530, -1.535468, 1.711220, 1.836908, -1.707450, -0.400234, -0.708683,
-1.761759, -1.369860, 1.213137, 1.791639, -1.467956, -0.170537, 1.473557,
# pose 16
-1.997672, -1.626362, 1.529443, 2.200206, -1.291013, 0.398664, 1.954208,
-1.504294, -1.346227, 1.206550, 1.660186, -1.320782, -0.393676, 1.657215,
# pose 17
-1.981875, -1.620651, 1.545443, 2.373140, -1.167365, 0.396487, 2.012547,
-1.846546, -1.476930, 0.946255, 1.910615, -2.435429, 1.535168, 2.151544,
# pose 18
-2.179415, -1.511968, 1.578832, 2.401706, -1.326480, -0.051353, 1.137181,
-1.700067, -1.452760, 0.909035, 1.758222, -2.103930, 1.445249, 2.174743,
# pose 19
-2.173463, -1.530413, 1.575534, 2.173673, -1.289673, -0.090117, 0.867214,
-2.173713, -0.983647, 1.399849, 2.384939, -1.379509, -0.216471, 0.253561,
# pose 20
-2.096314, -1.569352, 1.579820, 1.942057, -1.458598, -0.057003, 0.572953,
# pose 21
-2.129376, -1.566787, 1.903585, 1.962858, -1.454137, -0.335990, -0.148441,
# pose 22
-2.020280, -1.527165, 1.887130, 1.728957, -1.491867, -0.370585, -0.277940,
# pose 23
-2.140817, -1.607357, 1.617236, 2.153850, -1.571057, -0.147540, 0.770535,
-2.160187, -0.967904, 1.376288, 2.451713, -1.393524, -0.212278, 0.473606,
]
scripts/run_all.sh
+11 -2
View File
@@ -23,6 +23,7 @@
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-descent # 对准后只降到观测高度（默认离面 15mm）
# bash ~/ros2/scripts/run_all.sh --arm 89 --init-z 0.35 # 启动后先到这个法兰高度 [m]
# bash ~/ros2/scripts/run_all.sh --arm 89 --hover-z 0.34 # 对孔悬停法兰高度 [m]，不安全会拒绝
# bash ~/ros2/scripts/run_all.sh --arm 89 --final-gap 8 # 下臂锁 XY，规尖停在离面 8mm（site 默认 2）
# bash ~/ros2/scripts/run_all.sh --arm 89 --d435-only # 拍一张 K0，厂家 MoveL 把工具尖 XY 对准它（先停 run_real.sh）
#
# 前置（另开一个终端，先跑起来）：
@@ -49,6 +50,7 @@ NO_DESCENT=false
D435_ONLY=false
INIT_Z=""
HOVER_Z=""
FINAL_GAP=""
# --sn/--arm 必须先解析：load_site_config.sh 靠 ROBOT_SN 决定加载哪套机器人配置
# shellcheck disable=SC1091
@@ -71,15 +73,21 @@ while [[ $# -gt 0 ]]; do
--d435-only) D435_ONLY=true; SKIP_STAGE2=true; SKIP_STAGE3=true; shift ;;
--init-z) INIT_Z="${2:?--init-z 需要法兰高度，单位米}"; shift 2 ;;
--hover-z) HOVER_Z="${2:?--hover-z 需要法兰高度，单位米}"; shift 2 ;;
--final-gap) FINAL_GAP="${2:?--final-gap 需要规尖离面间隙，单位毫米}"; shift 2 ;;
--no-preview) PIPELINE_PREVIEW=false; shift ;;
--camera-preview) CAMERA_PREVIEW=true; shift ;;
--no-camera) WITH_CAMERA=false; shift ;;
--no-tactile) WITH_TACTILE=false; shift ;;
-h|--help) sed -n '2,31p' "$0"; exit 0 ;;
-h|--help) sed -n '2,32p' "$0"; exit 0 ;;
*) echo "Unknown: $1" >&2; exit 1 ;;
esac
done
if [[ -n "${FINAL_GAP}" && "${NO_DESCENT}" == true ]]; then
echo "ERROR: 同时写了 --final-gap 和 --no-descent，不知道听谁的。" >&2
exit 1
fi
# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"
# shellcheck disable=SC1091
@@ -311,9 +319,10 @@ ARGS=(
)
[[ -n "${INIT_Z}" ]] && ARGS+=("init_z_m:=${INIT_Z}")
[[ -n "${HOVER_Z}" ]] && ARGS+=("hover_z_m:=${HOVER_Z}")
[[ -n "${FINAL_GAP}" ]] && ARGS+=("final_gap_mm:=${FINAL_GAP}")
[[ -n "${HOLE}" ]] && ARGS+=("target_hole:=${HOLE}")
[[ -n "${DEPTH}" ]] && ARGS+=("insert_depth_mm:=${DEPTH}")
[[ -n "${PITCH}" ]] && ARGS+=("screw_pitch_mm:=${PITCH}")
echo "=== launch pipeline hole=${HOLE:-site} depth=${DEPTH:-site} pitch=${PITCH:-site} init_z=${INIT_Z:-site} hover_z=${HOVER_Z:-site} skip1=${SKIP_STAGE1} skip2=${SKIP_STAGE2} skip3=${SKIP_STAGE3} no_descent=${NO_DESCENT} d435_only=${D435_ONLY} ==="
echo "=== launch pipeline hole=${HOLE:-site} depth=${DEPTH:-site} pitch=${PITCH:-site} init_z=${INIT_Z:-site} hover_z=${HOVER_Z:-site} final_gap=${FINAL_GAP:-site} skip1=${SKIP_STAGE1} skip2=${SKIP_STAGE2} skip3=${SKIP_STAGE3} no_descent=${NO_DESCENT} d435_only=${D435_ONLY} ==="
exec ros2 launch arm_application pipeline.launch.py "${ARGS[@]}"
scripts/run_stage2_locate.sh
+6 -2
View File
@@ -12,6 +12,7 @@
# bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hole k2 # 对准 k2
# bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only # 只垂直+悬停锁定（调高度）
# bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-z 0.28 # 临时改悬停高（不改 yaml）
# bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --final-gap 8 # 下臂锁 XY，规尖离面 8mm
# bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --no-tactile # 不起触觉服务
#
# 悬停锁高固定接口：config/site.yaml → robots[89].servo.hole_hover_z_m
@@ -21,6 +22,7 @@ ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
HOVER_ONLY=false
HOLE=""
HOVER_Z=""
FINAL_GAP=""
WITH_TACTILE=true
# shellcheck disable=SC1091
@@ -33,8 +35,9 @@ while [[ $# -gt 0 ]]; do
--hover-only) HOVER_ONLY=true; shift ;;
--hole) HOLE="${2:?}"; shift 2 ;;
--hover-z) HOVER_Z="${2:?}"; shift 2 ;;
--final-gap) FINAL_GAP="${2:?--final-gap 需要规尖离面间隙，单位毫米}"; shift 2 ;;
--no-tactile) WITH_TACTILE=false; shift ;;
-h|--help) sed -n '2,22p' "$0"; exit 0 ;;
-h|--help) sed -n '2,18p' "$0"; exit 0 ;;
*) echo "Unknown: $1" >&2; exit 1 ;;
esac
done
@@ -137,5 +140,6 @@ echo "=== 确认：左臂夹爪已夹持通止规，通止规轴线与法兰轴
ARGS=(hover_only:="${HOVER_ONLY}")
if [[ -n "${HOLE}" ]]; then ARGS+=(target_hole:="${HOLE}"); fi
if [[ -n "${HOVER_Z}" ]]; then ARGS+=(hover_z_m:="${HOVER_Z}"); fi
echo "=== launch stage2_locate hover_only=${HOVER_ONLY} hole=${HOLE:-site} hover_z=${HOVER_Z:-site} ==="
if [[ -n "${FINAL_GAP}" ]]; then ARGS+=(final_gap_mm:="${FINAL_GAP}"); fi
echo "=== launch stage2_locate hover_only=${HOVER_ONLY} hole=${HOLE:-site} hover_z=${HOVER_Z:-site} final_gap=${FINAL_GAP:-site} ==="
exec ros2 launch arm_application stage2_locate.launch.py "${ARGS[@]}"
src/arm_application/arm_application/overhead_oneshot.py
+149 -3
View File
@@ -154,9 +154,9 @@ def d405_commands_arm(
) -> bool:
"""D405 这一帧的点能不能下发给左臂。
推理一直开着。正在用 D435 那一个点时，杀栈 MoveL 冷启动结束并且停稳之前不下发。
停稳从控制栈回来之后才计，用 ``d435_handback_sec`` 和
``lock_still_move_m``。赶路时 XY 已经进 15 mm 也不算到位。
推理一直开着。正在用 D435 那一个点时，进程内 MoveL 结束并且停稳之前不下发。
停稳用 ``d435_handback_sec`` 和 ``lock_still_move_m``。赶路时 XY 已经进 15 mm
也不算到位。不许在 MoveL 期间下发 D405（两套控制互抢会晃）。
``move_only``：只测 D435 位移，D405 点一律不下发（到位之后也不交）。
"""
if move_only:
@@ -541,6 +541,112 @@ HOLE_PAIR_TRIM_MAX_M = 0.070
# 悬停 → 观察高（约 15mm）单步上限。左臂间隙约 58mm，20mm × 3 步走完。
# 观察高 → 贴面不再走这段，改斜线一步（XY 补偿 + 剩余 Z）。
ONCE_APPROACH_STEP_M = 0.020
# 悬停已对上时 ΔXY 必须为 0。site ``once_ff_look_per_m`` 同源。
# 2026-10-14 下午：框心改平面交点后两条链路下臂都仍 +1.3mm X 过补插不进。
# k=0.018 是深度框心时代的对冲，不是下降几何。笛卡尔 MoveL 只降 Z。
ONCE_FF_LOOK_PER_M = 0.0
ONCE_FF_MOVEL_VEL_MPS = 0.05
def once_look_ff_xy(look_xy, descent_m: float, k: float):
"""视线前馈 ΔXY [m]。k≤0 是笛卡尔锁 XY：返回 (0,0)，不是 None。
k>0 时沿 -look 走 k×下降。下降≤0 / 视线退化 → None。
"""
if float(descent_m) <= 1e-9:
return None
if float(k) <= 1e-9:
return np.array([0.0, 0.0], dtype=float)
look = np.asarray(look_xy, dtype=float).reshape(2)
n = float(np.linalg.norm(look))
if n < 1e-6 or not np.all(np.isfinite(look)):
return None
look = look / n
dxy = -look * float(k) * float(descent_m)
if not np.all(np.isfinite(dxy)):
return None
return dxy
def once_ff_tcp_xyz(
flange_xy,
tip_off_xy,
look_xy,
descent_m: float,
k: float,
surface_z_m: float,
gap_m: float,
):
"""规尖（厂家 MoveL 的 TCP）目标 [m]。Z 是面 + 间隙，不是法兰。"""
dxy = once_look_ff_xy(look_xy, descent_m, k)
if dxy is None:
return None
off = np.asarray(tip_off_xy, dtype=float).reshape(2)
fl = np.asarray(flange_xy, dtype=float).reshape(2)
if not np.all(np.isfinite(off)) or not np.all(np.isfinite(fl)):
return None
tcp = np.array(
[
float(fl[0]) + float(off[0]) + float(dxy[0]),
float(fl[1]) + float(off[1]) + float(dxy[1]),
float(surface_z_m) + float(gap_m),
],
dtype=float,
)
if not np.all(np.isfinite(tcp)):
return None
return tcp, dxy
def d435_movel_from_tcp(tcp_xyz, flange_xy, k0_xy):
"""D435 粗定位 MoveL 目标 [m]：当前厂家 TCP + (K0−法兰) 的 ROS XY。
keep_z 由硬件用当前 TCP 的 Z 覆盖。ROS 绝对 K0 不能当 WORLD 目标
（2026-10-14 贴面 MoveL 已踩过）。缺数 / 非有限 → None。
"""
try:
t = np.asarray(tcp_xyz, dtype=float).reshape(3)
fl = np.asarray(flange_xy, dtype=float).reshape(2)
k0 = np.asarray(k0_xy, dtype=float).reshape(2)
except (TypeError, ValueError):
return None
if t.size != 3 or fl.size != 2 or k0.size != 2:
return None
if not np.all(np.isfinite(t)) or not np.all(np.isfinite(fl)) or not np.all(np.isfinite(k0)):
return None
out = np.array(
[float(t[0]) + float(k0[0]) - float(fl[0]),
float(t[1]) + float(k0[1]) - float(fl[1]),
float(t[2])],
dtype=float,
)
if not np.all(np.isfinite(out)):
return None
return out
def once_ff_movel_from_tcp(tcp_xyz, dxy, descent_m: float):
"""厂家 MoveL 目标 = 当前 Studio TCP + (ΔXY, −下降) [m]。
2026-10-14：把 ROS 规尖绝对坐标发给 MoveL，k=0.018 与 0.035 都落到
WORLD (0.595, −0.468, 0.024)，法兰实走约 (+9, −8)mm。WORLD 原点与
ROS 基座不是同一点，绝对坐标不能用。相对位移才跟 k 有关。
"""
if float(descent_m) <= 1e-9:
return None
t = np.asarray(tcp_xyz, dtype=float).reshape(3)
d = np.asarray(dxy, dtype=float).reshape(2)
if t.size != 3 or d.size != 2:
return None
if not np.all(np.isfinite(t)) or not np.all(np.isfinite(d)):
return None
out = np.array(
[float(t[0]) + float(d[0]), float(t[1]) + float(d[1]), float(t[2]) - float(descent_m)],
dtype=float,
)
if not np.all(np.isfinite(out)):
return None
return out
def oneshot_align_step_m(err_m: float) -> float:
@@ -570,6 +676,46 @@ def flange_z_for_gap_m(tip_len_m: float, surface_z_m: float, gap_m: float) -> fl
return float(tip_len_m) + float(surface_z_m) + float(gap_m)
def final_gap_refusal(
gap_m: float,
tool_len_m: float,
surface_z_m: float,
hover_z_m: float,
z_min_m: float,
z_max_m: float,
) -> str:
"""笛卡尔锁 XY 的贴面间隙能不能用。空串表示可以；非空就是拒绝原因。
参数是规尖离面 [m]，不是法兰 Z：法兰 = 工具长 + 工件面 + 间隙，只在这里算一次。
"""
gap = float(gap_m)
if not np.isfinite(gap):
return "贴面间隙不是有限数，拒绝开跑"
if gap <= 1e-9:
return (
f"贴面间隙 {gap * 1000:.1f}mm ≤ 0，规尖会进工件/贴面接触，拒绝开跑"
)
tool = float(tool_len_m)
surf = float(surface_z_m)
if tool <= 0.0 or surf <= 0.0:
return "写了贴面间隙，但工具长度或工件面未知，无法确认尖端在面之上，拒绝开跑"
hover_clear = float(hover_z_m) - tool - surf
if gap > hover_clear + 1e-6:
return (
f"贴面间隙 {gap * 1000:.1f}mm 比悬停离面 {hover_clear * 1000:.1f}mm 还高，"
"锁 XY 只会抬臂，拒绝开跑"
)
flange = flange_z_for_gap_m(tool, surf, gap)
z_lo = float(z_min_m)
z_hi = float(z_max_m)
if flange < z_lo - 1e-6 or flange > z_hi + 1e-6:
return (
f"贴面法兰 Z {flange:.3f}m（规尖离面 {gap * 1000:.1f}mm）"
f"超出工作盒 Z [{z_lo:.3f}, {z_hi:.3f}]，拒绝开跑"
)
return ""
def decompose_base_xy(delta_xy, T_cam_to_base: np.ndarray):
"""把基座 XY 残差拆成沿视线 / 沿画面右 [m]。投影退化返回 None。"""
axes = cam_xy_bias_axes(T_cam_to_base)
src/arm_application/arm_application/servo_node.py
+93 -34
View File
@@ -14,12 +14,11 @@ from __future__ import annotations
import os
import math
import subprocess
import sys
import threading
import time
from collections import deque
from typing import Deque, List, Optional
from typing import Deque, List, Optional, Tuple
import numpy as np
import rclpy
@@ -35,6 +34,7 @@ from rclpy.qos import (
HistoryPolicy,
QoSProfile,
ReliabilityPolicy,
qos_profile_sensor_data,
qos_profile_system_default,
)
from sensor_msgs.msg import JointState
@@ -52,11 +52,11 @@ from arm_application.handeye_lib import cam_point_to_base, cam_to_base_T
from arm_application.overhead_oneshot import (
D435_NEAR_CRUISE_MPS,
D435_SETTLE_S,
D435_VENDOR_MOVEL_ACC_MPS2,
D435_VENDOR_MOVEL_VEL_MPS,
StillLock,
at_lock_height,
d405_commands_arm,
d435_movel_from_tcp,
flange_tilt_rad,
hold_translation_vz,
hybrid_cruise_vxy,
@@ -88,6 +88,11 @@ from arm_application.site_io import (
from arm_application.ros_node import TolerantNode
try:
from flexiv_msgs.msg import RobotStates
except ImportError: # pragma: no cover
RobotStates = None # type: ignore
DEFAULT_JOINTS = [
"joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7",
@@ -219,7 +224,8 @@ class ServoNode(TolerantNode):
self.declare_parameter("lock_hold_sec", 2.0)
# D405 跟住后法兰静止多久进二阶段。哨兵 -1 = 没从 site 接入。
# lock_still_sec 仍须从 site 接上（缺了就报错）。
# 交回 D405：MoveL 结束后杀栈冷启动，再叠 d435_handback_sec（现场 3 s）。
# 交回 D405：进程内 MoveL 到位后，再叠 d435_handback_sec（法兰 XY 停稳）。
# 旧杀栈冷启动曾把这 3s 垫在控制器重启后面；不杀栈后只留机械停稳。
self.declare_parameter("lock_still_sec", -1.0)
self.declare_parameter("d435_handback_sec", -1.0)
self.declare_parameter("wrist_align_sec", -1.0)
@@ -511,6 +517,24 @@ class ServoNode(TolerantNode):
10,
callback_group=self._control_cbg,
)
# 厂家 tcp_pose（WORLD）。D435 MoveL 必须在这上面加 ROS 相对位移。
self._tcp_world: Optional[Tuple[float, float, float]] = None
self._t_tcp: Optional[float] = None
prefix_ns = str(self.get_parameter("joint_prefix").value or "").rstrip("_")
self.robot_states_topic = f"/{prefix_ns.replace('-', '_')}/flexiv_robot_states"
if RobotStates is not None:
self.create_subscription(
RobotStates,
self.robot_states_topic,
self._on_servo_robot_states,
qos_profile_sensor_data,
callback_group=self._state_cbg,
)
else:
self.get_logger().error(
"没有 flexiv_msgs.RobotStates，D435 粗定位不能读厂家 tcp_pose，"
"不许退回杀栈 MoveL"
)
cmd_qos = QoSProfile(
history=HistoryPolicy.KEEP_LAST,
@@ -606,6 +630,23 @@ class ServoNode(TolerantNode):
def _on_js(self, msg: JointState) -> None:
self._last_js = msg
def _on_servo_robot_states(self, msg) -> None:
"""缓存厂家 ``tcp_pose``（WORLD）。名字避开其它节点的 ``_on_robot_states``。"""
p = msg.tcp_pose.pose.position
xyz = (float(p.x), float(p.y), float(p.z))
if not all(math.isfinite(v) for v in xyz):
return
self._tcp_world = xyz
self._t_tcp = time.monotonic()
def _fresh_tcp_world(self):
"""最近 0.5s 的厂家 TCP [m]。过期/没有 → None，不许用旧值凑。"""
if self._tcp_world is None or self._t_tcp is None:
return None
if time.monotonic() - float(self._t_tcp) > 0.5:
return None
return self._tcp_world
def _on_wrist_conf(self, msg: Float32) -> None:
c = float(msg.data)
with self._lock:
@@ -1545,12 +1586,17 @@ class ServoNode(TolerantNode):
return False, ""
def _d435_vendor_movel(self) -> bool:
"""D435 这一点：停真机栈 → 厂家 MoveL → 冷启动控制栈。
"""D435 这一点：进程内厂家 MoveL，不杀控制栈。
线速度 / 加速度走 ``D435_VENDOR_MOVEL_VEL_MPS``、``D435_VENDOR_MOVEL_ACC_MPS2``，
交给 SDK 的 ``vel`` / ``acc``。杀栈再拉起大约多几秒，RDK 独占，
D405 的点在整段 busy 期间都不会下发。停稳计时从冷启动回来之后才开始。
目标 = 当前厂家 TCP + (K0−法兰) 的 ROS XY，``keep_z``。硬件在 MoveL
期间不灌关节指令，D405 被 ``d435_commanding`` 挡住。不许退回杀栈
（那会冷启动十几秒，而且两套控制会互抢晃臂）。
"""
lib = ros2_root() / "scripts" / "lib"
if str(lib) not in sys.path:
sys.path.insert(0, str(lib))
from flexiv_movel_ipc import write_request
with self._lock:
goal = None if self._p_feat_base is None else np.asarray(
self._p_feat_base, dtype=float
@@ -1558,38 +1604,51 @@ class ServoNode(TolerantNode):
if goal is None or not np.all(np.isfinite(goal)):
self.get_logger().error("没有 D435 基座点，厂家 MoveL 不下发")
return False
arm = str(os.environ.get("SITE_ROBOT_ARM") or "89").strip() or "89"
script = ros2_root() / "scripts" / "rdk_release_movel.py"
if not script.is_file():
self.get_logger().error(f"找不到 {script}")
tcp_now = self._fresh_tcp_world()
if tcp_now is None:
self.get_logger().error(
"没有厂家 tcp_pose（话题 "
f"{getattr(self, 'robot_states_topic', '?')} 不新鲜），"
"拒绝杀栈凑 MoveL"
)
return False
cmd = [
sys.executable,
str(script),
"--xyz",
f"{float(goal[0]):.6f}",
f"{float(goal[1]):.6f}",
f"{float(goal[2]):.6f}",
"--arm",
arm,
"--vel",
f"{float(D435_VENDOR_MOVEL_VEL_MPS):.3f}",
"--acc",
f"{float(D435_VENDOR_MOVEL_ACC_MPS2):.3f}",
]
T_ee = self._lookup_T(self.base_frame, self.ee_frame)
if T_ee is None:
self.get_logger().error("没有法兰 TF，D435 相对位移算不出，不下发")
return False
tgt = d435_movel_from_tcp(tcp_now, T_ee[:2, 3], goal[:2])
if tgt is None:
self.get_logger().error("D435 厂家 TCP 加上相对位移算不出，不下发")
return False
seq = int(time.monotonic() * 1000.0) % 1_000_000_000
write_request(
seq,
float(tgt[0]),
float(tgt[1]),
float(tgt[2]),
True,
float(D435_VENDOR_MOVEL_VEL_MPS),
)
self.get_logger().info(
f"D435 厂家 MoveL XY=({goal[0]:.3f},{goal[1]:.3f}) "
f"vel={D435_VENDOR_MOVEL_VEL_MPS*1000:.0f}mm/s "
f"acc={D435_VENDOR_MOVEL_ACC_MPS2:.1f}m/s² "
"—— 先停控制栈，MoveL 后再冷启动；期间不下发 D405"
f"D435 进程内 MoveL XY=({goal[0]:.3f},{goal[1]:.3f}) "
f"厂家TCP现=({tcp_now[0]:.4f},{tcp_now[1]:.4f},{tcp_now[2]:.4f}) "
f"MoveL=({tgt[0]:.4f},{tgt[1]:.4f},keep_z) "
f"vel={D435_VENDOR_MOVEL_VEL_MPS * 1000:.0f}mm/s "
"—— 不杀栈；期间不下发 D405"
)
t0 = time.monotonic()
got = subprocess.run(cmd, check=False)
acked, _why = self._wait_movel_file(seq, 2.0, want_done=False)
if not acked:
self.get_logger().error(
"厂家 MoveL 2s 内没 ack —— 硬件没接 /tmp/flexiv_movel_req。"
"真机环境重建 install/ 后再跑，不许改走杀栈假装成功"
)
return False
ok, detail = self._wait_movel_file(seq, 90.0, want_done=True)
dt = time.monotonic() - t0
ok = got.returncode == 0
self.get_logger().info(
f"rdk_release_movel {'完成' if ok else '失败'}，耗时 {dt:.2f}s"
+ (f" 退出码 {got.returncode}" if not ok else "")
f"D435 进程内 MoveL {'完成' if ok else '失败'}，耗时 {dt:.2f}s"
+ (f" {detail}" if detail else "")
)
return bool(ok)
src/arm_application/arm_application/stage2_locate_node.py
+333 -10
View File
@@ -10,6 +10,7 @@
4. fine — 拍照，two_stage（YOLO+HoughCircles）解 k1/k2 孔心像素
5. align — 像素射线交工件面 + 内参 + 手眼，把规尖移到目标孔正上方。
``hole_keypoint_once`` 时只走一次，核验拍只记账；
``hole_once_lock_xy_descend`` 时按视线前馈一条 MoveL 贴面；
否则复拍迭代到 align_tol_m
6. hold — 锁定关节，发布 /stage2/result(JSON) + /tmp/stage2_result.json
@@ -22,6 +23,7 @@ from __future__ import annotations
import json
import math
import os
import sys
import time
from pathlib import Path
from typing import Any, Dict, List, Optional, Tuple
@@ -53,12 +55,18 @@ from arm_application.path_profile import (
)
from arm_application.overhead_oneshot import (
ONCE_APPROACH_STEP_M,
ONCE_FF_LOOK_PER_M,
ONCE_FF_MOVEL_VEL_MPS,
ONCE_TRIM_MAX_STEP_M,
ONCE_TRIM_REFUSE_M,
cam_xy_bias_axes,
decompose_base_xy,
flange_z_for_gap_m,
final_gap_refusal,
hole_pair_span_m,
hole_pair_span_ok,
once_ff_movel_from_tcp,
once_ff_tcp_xyz,
oneshot_align_step_m,
pixel_on_base_plane,
)
@@ -67,9 +75,14 @@ from arm_application.gauge_boot import perception_source_of
from arm_application.gauge_fsm import drag_verdict, resolve_hover_z, wrench_all_zero
from arm_application.hole_detect_hough import detect_holes
from arm_application.hole_detector import config_from_site, weights_from_site
from arm_application.site_io import deep_merge, load_site_dict
from arm_application.site_io import deep_merge, load_site_dict, ros2_root
from arm_application.ros_node import TolerantNode
try:
from flexiv_msgs.msg import RobotStates
except ImportError: # pragma: no cover
RobotStates = None # type: ignore
def contact_armed_gap_m(observe_gap_m: float, final_gap_m: float = 0.002) -> float:
"""轻降力接触门槛对应的几何间隙。离面更远时腕部力会漂几牛，不能当碰到。
@@ -152,6 +165,9 @@ class Stage2Locate(TolerantNode):
self.declare_parameter("hover_only", False)
self.declare_parameter("target_hole", "") # 空字符串 → 用 site 的 stage2.target_hole
self.declare_parameter("hover_z_m", -1.0) # 大于 0 → 覆盖 site 里的值
# 贴面规尖离面 [mm]。≥0 → 覆盖 site ``final_gap_mm``。-1=用 site。
# 笛卡尔锁 XY 时法兰 Z = 工具长 + 工件面 + 间隙，XY 不动。
self.declare_parameter("final_gap_mm", -1.0)
# 工具尖端在法兰下方多少 [m]；大于 0 → 覆盖 site。
# 仿真里由 run_gauge_assembly.sh --sim 传**实测值**（每次上料都可能不同），
# 真机不传（走 site.yaml）。悬停高度与 S07 下探预算都按它算。
@@ -235,7 +251,13 @@ class Stage2Locate(TolerantNode):
# k1/k2，而流程会把它当"孔检测失败"，现场根本想不到是粗居中把臂带走的。
self.coarse_max_offset_m = float(stage2.get("coarse_max_offset_m") or 0.12)
self.coarse_border_px = int(stage2.get("coarse_border_px") or 2)
# 最后轻降到孔面上方 final_gap_mm（不接触）
# 最后轻降到孔面上方 final_gap_mm（不接触）。启动参数 ≥0 覆盖 site。
self._final_gap_refused = ""
_gap_override_mm = float(self.get_parameter("final_gap_mm").value)
self.final_gap_user_set = _gap_override_mm >= 0.0
if self.final_gap_user_set:
self.final_gap_m = _gap_override_mm / 1000.0
else:
self.final_gap_m = float(stage2.get("final_gap_mm") or 1.5) / 1000.0
# 观测下降：对准后降到这个间隙就停，方便看规尖是不是对着孔。
# 比 final_gap（贴面 2mm）高一截，对偏了也不会把规压上孔沿。
@@ -269,6 +291,16 @@ class Stage2Locate(TolerantNode):
# 本分支实验：悬停拍一次 k1/k2，走一次，复拍只记账不改 XY。
# 键必须接线——写了 true 却仍多走多拍，等于没做实验。
self.hole_keypoint_once = _as_bool(stage2.get("hole_keypoint_once"), False)
# 悬停走完后一条厂家 MoveL 收到贴面。k=0：笛卡尔锁 XY 只降 Z。
# 缺键 = 仍走观察高补交（37b85aa）。
self.hole_once_lock_xy_descend = _as_bool(
stage2.get("hole_once_lock_xy_descend"), False
)
_ff = stage2.get("once_ff_look_per_m")
if _ff is None:
self.once_ff_look_per_m = float(ONCE_FF_LOOK_PER_M)
else:
self.once_ff_look_per_m = float(_ff)
self._ws_clipped = False
self._hole_snaps: List[Dict[str, Any]] = []
if self.hover_z_user_set and (_tool_len <= 0.0 or _surface_z <= 0.0):
@@ -314,6 +346,28 @@ class Stage2Locate(TolerantNode):
f"final_descent_enable 被启动参数覆盖为 {self.final_descent_enable} "
f"（site 里是 {stage2.get('final_descent_enable')}）—— 只在调试时这么用"
)
if self.final_gap_user_set and not self.final_descent_enable:
self._final_gap_refused = (
"同时写了 --final-gap 和 --no-descent，不知道听谁的，拒绝开跑"
)
self.get_logger().error(self._final_gap_refused)
elif self.final_gap_user_set and (_tool_len <= 0.0 or _surface_z <= 0.0):
self._final_gap_refused = (
"写了 --final-gap，但工具长度或工件面未知，无法确认尖端在面之上，拒绝执行"
)
self.get_logger().error(self._final_gap_refused)
elif _tool_len > 0.0 and _surface_z > 0.0:
_why_gap = final_gap_refusal(
self.final_gap_m,
_tool_len,
_surface_z,
self.hover_z,
float(self.ws_min[2]),
float(self.ws_max[2]),
)
if _why_gap:
self._final_gap_refused = _why_gap
self.get_logger().error(_why_gap)
self.vert_tol_rad = math.radians(float(stage2.get("verticalize_tol_deg") or 0.8))
self.settle_sec = float(stage2.get("settle_sec") or 0.45)
self.snap_frames = int(stage2.get("snap_frames") or 5)
@@ -398,6 +452,23 @@ class Stage2Locate(TolerantNode):
self.create_subscription(
WrenchStamped, self.ft_wrench_topic, self._on_wrench, qos_profile_sensor_data
)
# 厂家 tcp_pose（WORLD）。视线前馈 MoveL 必须在这上面加相对位移，
# 不许拿 ROS 法兰+规尖当绝对目标。回调名避开 stage3 的 `_on_robot_states`。
self._tcp_world: Optional[Tuple[float, float, float]] = None
self._t_tcp: Optional[float] = None
self.robot_states_topic = f"/{robot_ns}/flexiv_robot_states"
if RobotStates is not None:
self.create_subscription(
RobotStates,
self.robot_states_topic,
self._on_stage2_robot_states,
qos_profile_sensor_data,
)
elif self.hole_once_lock_xy_descend:
self.get_logger().error(
"没有 flexiv_msgs.RobotStates，视线前馈不能读厂家 tcp_pose，"
"不许退回 ROS 绝对坐标"
)
self._state_pub = self.create_publisher(String, "/stage2/state", 10)
self._result_pub = self.create_publisher(String, "/stage2/result", 10)
@@ -415,10 +486,33 @@ class Stage2Locate(TolerantNode):
f" image={self.image_topic} depth={self.depth_topic}\n"
f" target={self.target_hole} (k1={self.k1_label}) tol={self.align_tol*1000:.1f}mm "
f"hover_only={self.hover_only} hole_visual_only={self.hole_visual_only} "
f"hole_keypoint_once={self.hole_keypoint_once}\n"
f"hole_keypoint_once={self.hole_keypoint_once} "
f"lock_xy_descend={self.hole_once_lock_xy_descend} "
f"ff_look={self.once_ff_look_per_m:.3f} "
f"final_gap={self.final_gap_m * 1000:.1f}mm"
+ (
f"→法兰Z={flange_z_for_gap_m(self.tool_tip_below_flange_m, self.surface_z_m, self.final_gap_m):.4f}m"
if self.tool_tip_below_flange_m > 0.0 and self.surface_z_m > 0.0
else ""
)
+ "\n"
f" 工具长={self.tool_tip_below_flange_m:.4f}m（法兰到尖，下臂不得低于面+间隙）"
)
if self.hole_keypoint_once:
if self.hole_keypoint_once and self.hole_once_lock_xy_descend:
k = float(self.once_ff_look_per_m)
if k <= 1e-9:
self.get_logger().warn(
"对孔方案=单次精拍·笛卡尔锁 XY（实验）。悬停走完后不补拍，"
"厂家 MoveL 只降 Z、ΔXY=0。k>0 会沿 -look 再加前馈。"
"对照 37b85aa 把 hole_once_lock_xy_descend 改回 false。"
)
else:
self.get_logger().warn(
"对孔方案=单次精拍·视线前馈斜线（实验）。悬停走完后不补拍，"
f"ΔXY=-look×{k:.3f}×剩余下降，一条 MoveL 到贴面。"
"对照 37b85aa 把 hole_once_lock_xy_descend 改回 false。"
)
elif self.hole_keypoint_once:
self.get_logger().warn(
"对孔方案=单次精拍（实验分支）。拍一次→走一次，复拍和下降后只记账不改 XY。"
"残差以核验拍为准，不许用 hop 到位冒充。看 /tmp/stage2_result.json 的 snaps。"
@@ -438,6 +532,23 @@ class Stage2Locate(TolerantNode):
def _on_info(self, msg: CameraInfo) -> None:
self._info = msg
def _on_stage2_robot_states(self, msg) -> None:
"""缓存厂家 ``tcp_pose``（WORLD）。名字必须与 stage3 ``_on_robot_states`` 分开。"""
p = msg.tcp_pose.pose.position
xyz = (float(p.x), float(p.y), float(p.z))
if not all(math.isfinite(v) for v in xyz):
return
self._tcp_world = xyz
self._t_tcp = time.monotonic()
def _fresh_tcp_world(self):
"""最近 0.5s 的厂家 TCP [m]。过期/没有 → None，不许用旧值凑。"""
if self._tcp_world is None or self._t_tcp is None:
return None
if time.monotonic() - float(self._t_tcp) > 0.5:
return None
return self._tcp_world
def _on_wrench(self, msg: WrenchStamped) -> None:
"""缓存腕部 FT 的**轴向力**（`geometry_msgs/WrenchStamped`，法兰系）。
@@ -1652,7 +1763,7 @@ class Stage2Locate(TolerantNode):
"""
# final_descent_enable：多走多拍降到贴面 final_gap（约 2mm）。
# 关掉时（--no-descent）仍做**观测下降**，停在 observe_gap（约 15mm）。
# 单次精拍由调用方传入 target_gap_m=观察高，贴面由随后斜线一步完成。
# 单次精拍补交路径传入观察高；锁 XY 实验传入贴面 final_gap。
if target_gap_m is None:
target_gap = (
self.final_gap_m if self.final_descent_enable else self.observe_gap_m
@@ -1817,7 +1928,12 @@ class Stage2Locate(TolerantNode):
"""把基座残差拆成视线 / 画面右，写入记录。投影退化时键仍在、值为 None。"""
rec = dict(rec)
rec["tag"] = tag
rec["scheme"] = "keypoint_once" if self.hole_keypoint_once else "multi_snap"
if self.hole_keypoint_once and getattr(self, "hole_once_lock_xy_descend", False):
rec["scheme"] = "keypoint_once_look_ff"
elif self.hole_keypoint_once:
rec["scheme"] = "keypoint_once"
else:
rec["scheme"] = "multi_snap"
rec["cam_frame"] = self.cam_frame
rec["image_topic"] = self.image_topic
err_vec = np.array(
@@ -1992,6 +2108,155 @@ class Stage2Locate(TolerantNode):
)
return q, Tee, True
def _vendor_movel_tcp(self, xyz, vel: float) -> bool:
"""进程内厂家 MoveL：TCP 目标，不杀控制栈。"""
lib = ros2_root() / "scripts" / "lib"
if str(lib) not in sys.path:
sys.path.insert(0, str(lib))
from flexiv_movel_ipc import read_ack, read_done, write_request
seq = int(time.monotonic() * 1000.0) % 1_000_000_000
write_request(
seq, float(xyz[0]), float(xyz[1]), float(xyz[2]), False, float(vel)
)
t0 = time.monotonic()
acked = False
while time.monotonic() - t0 < 90.0:
if not rclpy.ok():
return False
self._spin(0.02)
if not acked:
if read_ack() == seq:
acked = True
elif time.monotonic() - t0 > 2.0:
self.get_logger().error(
"厂家 MoveL 2s 内没 ack —— 硬件没接 /tmp/flexiv_movel_req。"
"真机环境重建 install/ 后再跑，不许改走关节折线假装成功"
)
return False
continue
got = read_done()
if got is not None and int(got[0]) == seq:
if not got[1]:
self.get_logger().error(f"厂家 MoveL 失败：{got[2]}")
return bool(got[1])
self.get_logger().error("厂家 MoveL 90s 内没有 done")
return False
def _once_ff_diagonal_to_gap(self, q, Tee, *, target_gap_m):
"""悬停点一条 MoveL 收到 ``target_gap_m``。k=0 只降 Z；k>0 再沿 -look 收 XY。"""
self._state("ff_diagonal_movel")
tip_len = float(self.tool_tip_below_flange_m)
surf = float(self.surface_z_m)
if tip_len <= 0.0 or surf <= 0.0:
self.get_logger().error("贴面下降缺工具长/工件面，停在悬停高")
return False, q, Tee
if self._tip_off is None:
self.get_logger().error("贴面下降需要悬停规尖偏移，没有就不许用法兰凑")
return False, q, Tee
k = float(self.once_ff_look_per_m)
look_xy = np.array([0.0, 0.0], dtype=float)
T_cam = self._lookup(self.base_frame, self.cam_frame)
if k > 1e-9:
if T_cam is None:
self.get_logger().error("没有 cam TF，视线轴算不出，不下发")
return False, q, Tee
axes = cam_xy_bias_axes(T_cam)
if axes is None:
self.get_logger().error("光轴水平投影退化，不下发")
return False, q, Tee
look_xy, _right = axes
elif T_cam is not None:
axes = cam_xy_bias_axes(T_cam)
if axes is not None:
look_xy, _right = axes
gap0 = float(Tee[2, 3]) - tip_len - surf
descent = gap0 - float(target_gap_m)
packed = once_ff_tcp_xyz(
[Tee[0, 3], Tee[1, 3]],
self._tip_off,
look_xy,
descent,
k,
surf,
target_gap_m,
)
if packed is None:
if descent <= 0.001:
self.get_logger().info("规尖已在目标间隙附近，无需斜线")
self.descent_final_gap_m = gap0
return True, q, Tee
self.get_logger().error("视线前馈算不出 ΔXY，停在悬停高")
return False, q, Tee
tcp, dxy = packed
n_xy = float(np.hypot(float(dxy[0]), float(dxy[1])))
if n_xy > ONCE_TRIM_REFUSE_M + 1e-9:
self.get_logger().error(
f"视线前馈 {n_xy * 1000:.1f}mm 超过 {ONCE_TRIM_REFUSE_M * 1000:.0f}mm，"
"不像交平面那几毫米，停在悬停高"
)
return False, q, Tee
z_flange = flange_z_for_gap_m(tip_len, surf, target_gap_m)
flange_tgt = np.array(
[
float(Tee[0, 3]) + float(dxy[0]),
float(Tee[1, 3]) + float(dxy[1]),
z_flange,
]
)
self._ws_clipped = False
self._clamp_ws(flange_tgt)
if self._ws_clipped:
self.get_logger().error(
"视线前馈目标被工作盒截断，不下发（不许把孔位改到盒沿）"
)
return False, q, Tee
self._spin(0.05)
tcp_now = self._fresh_tcp_world()
if tcp_now is None:
self.get_logger().error(
"没有厂家 tcp_pose（话题 "
f"{getattr(self, 'robot_states_topic', '?')} 不新鲜），"
"拒绝用 ROS 规尖绝对坐标发 MoveL"
)
return False, q, Tee
movel_xyz = once_ff_movel_from_tcp(tcp_now, dxy, descent)
if movel_xyz is None:
self.get_logger().error("厂家 TCP 加上相对位移算不出，停在悬停高")
return False, q, Tee
off_xy = (
(float(tcp_now[0]) - float(tcp[0]) + float(dxy[0])) * 1000.0,
(float(tcp_now[1]) - float(tcp[1]) + float(dxy[1])) * 1000.0,
)
scheme = "笛卡尔锁XY只降Z" if n_xy < 1e-6 else "视线前馈斜线"
self.get_logger().info(
f"{scheme}：look=({look_xy[0]:+.3f},{look_xy[1]:+.3f}) "
f"k={k:.3f} 降 {descent * 1000:.1f}mm → "
f"ΔXY=({dxy[0] * 1000:+.2f},{dxy[1] * 1000:+.2f})mm "
f"厂家TCP现=({tcp_now[0]:.4f},{tcp_now[1]:.4f},{tcp_now[2]:.4f}) "
f"MoveL=({movel_xyz[0]:.4f},{movel_xyz[1]:.4f},{movel_xyz[2]:.4f}) "
f"ROS尖目标=({tcp[0]:.4f},{tcp[1]:.4f},{tcp[2]:.4f}) "
f"WORLD相对ROS XY差≈({off_xy[0]:+.1f},{off_xy[1]:+.1f})mm "
f"vel={ONCE_FF_MOVEL_VEL_MPS * 1000:.0f}mm/s"
)
fl0 = (float(Tee[0, 3]), float(Tee[1, 3]), float(Tee[2, 3]))
ok = self._vendor_movel_tcp(movel_xyz, ONCE_FF_MOVEL_VEL_MPS)
q2, Tee2 = self._wait_state(2.0)
if q2 is not None and Tee2 is not None:
q, Tee = q2, Tee2
self._pin(q)
self.descent_final_gap_m = float(Tee[2, 3]) - tip_len - surf
dax = (float(Tee[0, 3]) - fl0[0]) * 1000.0
day = (float(Tee[1, 3]) - fl0[1]) * 1000.0
daz = (float(Tee[2, 3]) - fl0[2]) * 1000.0
self.get_logger().info(
f"贴面走后：法兰 ΔXYZ=({dax:+.2f},{day:+.2f},{daz:+.2f})mm "
f"指令 ΔXY=({dxy[0] * 1000:+.2f},{dxy[1] * 1000:+.2f})mm "
f"间隙 {self.descent_final_gap_m * 1000:.1f}mm "
f"{'到位' if ok else 'MoveL未完成'}"
)
return bool(ok), q, Tee
def _trim_xy_keep_hover_tip(self, q, Tee, final: Dict[str, Any], z_target_m=None):
"""观察高重交孔，规尖沿用悬停冻住的偏移。只许一跳、XY ≤3mm。
@@ -2470,6 +2735,9 @@ class Stage2Locate(TolerantNode):
if self._hover_refused:
self.get_logger().error(self._hover_refused)
return 2
if self._final_gap_refused:
self.get_logger().error(self._final_gap_refused)
return 2
if self.hover_z_user_set:
z_lo = float(self.ws_min[2])
@@ -2641,6 +2909,37 @@ class Stage2Locate(TolerantNode):
if truth is not None and truth.get("part_center_base"):
p_base = np.asarray(truth["part_center_base"][0], dtype=float)
src = "真值"
# 对孔走平面交点时，框心必须同一条射线∩面。深度框心沿 30° 视线
# 偏 1～2mm，符号随站位翻（D435 送到夹具位 vs 纯 D405 停在别处），
# 精拍再把这截当 K1.x−规尖.x 走掉，前馈又沿同轴加一次。
if p_base is None and self.hole_visual_only:
if float(self.surface_z_m) <= 0.0:
self.get_logger().error(
"粗居中要对孔同一平面，但 workpiece_surface_z_m 没有配，"
"拒绝用深度图凑框心"
)
break
T_cam = self._lookup(self.base_frame, self.cam_frame)
if T_cam is None:
self.get_logger().error("粗居中缺 cam TF，拒绝用深度图凑框心")
break
hit = pixel_on_base_plane(
float(bx),
float(by),
float(intr["fx"]),
float(intr["fy"]),
float(intr["cx"]),
float(intr["cy"]),
T_cam,
float(self.surface_z_m),
)
if hit is None:
self.get_logger().error(
"粗居中框心射线没交到工件面，拒绝用深度图凑"
)
break
p_base = hit
src = "像素交工件面"
if p_base is None:
z_wp = self._surface_depth(depth_m, bbox)
p_base = self._cam_to_base(self._px_to_cam(bx, by, z_wp, intr))
@@ -2931,21 +3230,32 @@ class Stage2Locate(TolerantNode):
final["align_trusted"] = False
final["fail_reason"] = "未量到规尖偏移，工具位置退化为法兰轴线"
q, Tee, _moved = self._oneshot_align_move(q, Tee, tgt_b, gxy, err)
final["commanded_residual_mm"] = commanded_mm
if self.hole_once_lock_xy_descend:
final["residual_remeasured"] = False
final["lock_xy_descend"] = True
if self._tip_off is not None:
final["ok"] = True
self.get_logger().warn(
f"视线前馈斜线：悬停指令 {commanded_mm:.2f}mm 已走，"
"不补拍。下一步按 k×下降收 XY，一条 MoveL 到贴面。"
"residual_mm 仍是走之前那一拍，不是贴面复测"
)
else:
q, Tee, final = self._observe_holes(
q, Tee, final, tag="走后核验"
)
final["commanded_residual_mm"] = commanded_mm
if not final.get("residual_remeasured"):
self.get_logger().error(
f"走后核验没复测到。开环指令残差 {commanded_mm:.2f}mm，"
"不是 hop 到位，不把这次当对准成功"
)
final["ok"] = False
self._state("aligned")
self.get_logger().info(
f"单次精拍悬停走完：指令 {commanded_mm:.2f}mm → "
f"复测 {final.get('residual_mm')}mm；下降后还要补交平面漂的那几毫米"
)
self._state("aligned")
break
if err <= self.align_tol:
@@ -3036,7 +3346,15 @@ class Stage2Locate(TolerantNode):
# ---- 4.5) 柔顺轻降到孔面上方 1-2mm（不接触）----
# 下降量由**几何**决定（法兰 FK − 工具长 − 工件面 − final_gap），深度读数只
# 做交叉核对，见 _descend_above_hole 的说明。
if self.hole_keypoint_once:
if self.hole_keypoint_once and self.hole_once_lock_xy_descend:
# 视线前馈斜线：不经过观察高、不补交。一条 MoveL 收到贴面。
lock_gap = (
self.final_gap_m if self.final_descent_enable else self.observe_gap_m
)
okd, q, Tee = self._once_ff_diagonal_to_gap(
q, Tee, target_gap_m=lock_gap
)
elif self.hole_keypoint_once:
# 先降到观察高（约 15mm）再交孔；贴面由补孔斜线一步完成，不再 4mm 切到 2mm。
okd, q, Tee = self._descend_above_hole(
q, Tee,
@@ -3047,7 +3365,12 @@ class Stage2Locate(TolerantNode):
okd, q, Tee = self._descend_above_hole(q, Tee)
if not okd:
self.get_logger().warn("轻降未完全到位，保持当前高度（安全优先）")
if self.hole_keypoint_once:
if self.hole_keypoint_once and self.hole_once_lock_xy_descend:
tip_len = float(self.tool_tip_below_flange_m)
surf = float(self.surface_z_m)
if tip_len > 0.0 and surf > 0.0:
self.descent_final_gap_m = float(Tee[2, 3]) - tip_len - surf
elif self.hole_keypoint_once:
# 悬停那一拍的基座 XY 不能带到观测高：前倾 30°，降 50mm 交点会漂约 4mm。
# 核验记账后补 XY；规尖沿用悬停冻结值，不在观测高度重测。
q, Tee, final = self._observe_holes(q, Tee, final, tag="降后核验")
src/arm_application/launch/pipeline.launch.py
+5 -1
View File
@@ -84,6 +84,7 @@ def _launch_setup(context, *args, **kwargs):
d435_only = _as_bool(LaunchConfiguration("d435_move_only").perform(context))
init_z = LaunchConfiguration("init_z_m").perform(context).strip()
hover_z = LaunchConfiguration("hover_z_m").perform(context).strip()
final_gap = LaunchConfiguration("final_gap_mm").perform(context).strip()
target_hole = LaunchConfiguration("target_hole").perform(context).strip()
depth = LaunchConfiguration("insert_depth_mm").perform(context).strip()
pitch = LaunchConfiguration("screw_pitch_mm").perform(context).strip()
@@ -125,6 +126,7 @@ def _launch_setup(context, *args, **kwargs):
"d435_move_only": d435_only,
"init_z_m": float(init_z) if init_z else -1.0,
"hover_z_m": float(hover_z) if hover_z else -1.0,
"final_gap_mm": float(final_gap) if final_gap else -1.0,
# -1=按 site 的 stage2.final_descent_enable；0=强制不降（调试）
"final_descent_enable": 0 if no_descent else -1,
}
@@ -136,7 +138,8 @@ def _launch_setup(context, *args, **kwargs):
LogInfo(msg=f"[pipeline] arm={robot.get('label') or robot.get('sn')} cam={cam}"),
LogInfo(
msg=f"[pipeline] hole={target_hole or 'site'} depth={depth or 'site'} "
f"pitch={pitch or 'site'} skip1={skip1} skip2={skip2} skip3={skip3}"
f"pitch={pitch or 'site'} final_gap={final_gap or 'site'} "
f"skip1={skip1} skip2={skip2} skip3={skip3}"
+ (" [d435_only：只测 D435 位移]" if d435_only else "")
+ (" [no_descent：只对准，不轻降]" if no_descent else "")
),
@@ -166,6 +169,7 @@ def generate_launch_description() -> LaunchDescription:
DeclareLaunchArgument("d435_move_only", default_value="false"),
DeclareLaunchArgument("init_z_m", default_value=""),
DeclareLaunchArgument("hover_z_m", default_value=""),
DeclareLaunchArgument("final_gap_mm", default_value=""),
DeclareLaunchArgument("preview", default_value="true"),
OpaqueFunction(function=_launch_setup),
]
src/arm_application/launch/stage2_locate.launch.py
+3
View File
@@ -75,6 +75,7 @@ def _launch_setup(context, *args, **kwargs):
preview = LaunchConfiguration("preview").perform(context).lower() in ("1", "true", "yes")
target_hole = LaunchConfiguration("target_hole").perform(context).strip()
hover_z = LaunchConfiguration("hover_z_m").perform(context).strip()
final_gap = LaunchConfiguration("final_gap_mm").perform(context).strip()
tx, ty, tz, qx, qy, qz, qw = _handeye_xyz_xyzw(handeye)
static_tf = Node(
@@ -101,6 +102,7 @@ def _launch_setup(context, *args, **kwargs):
"hover_only": hover_only,
"target_hole": target_hole,
"hover_z_m": float(hover_z) if hover_z else -1.0,
"final_gap_mm": float(final_gap) if final_gap else -1.0,
"preview": preview,
}
],
@@ -123,6 +125,7 @@ def generate_launch_description() -> LaunchDescription:
DeclareLaunchArgument("hover_only", default_value="false"),
DeclareLaunchArgument("target_hole", default_value=""),
DeclareLaunchArgument("hover_z_m", default_value=""),
DeclareLaunchArgument("final_gap_mm", default_value=""),
DeclareLaunchArgument("preview", default_value="true"),
OpaqueFunction(function=_launch_setup),
]
src/arm_application/test/test_overhead_oneshot.py
+147 -7
View File
@@ -60,7 +60,14 @@ from arm_application.overhead_oneshot import (
hole_pair_span_ok,
oneshot_align_step_m,
flange_z_for_gap_m,
final_gap_refusal,
d435_movel_from_tcp,
once_ff_movel_from_tcp,
once_ff_tcp_xyz,
once_look_ff_xy,
ONCE_APPROACH_STEP_M,
ONCE_FF_LOOK_PER_M,
ONCE_FF_MOVEL_VEL_MPS,
ONCE_TRIM_MAX_STEP_M,
ONCE_TRIM_REFUSE_M,
pixel_on_base_plane,
@@ -175,21 +182,44 @@ class TestHandoff:
).read_text(encoding="utf-8")
assert "pixel_on_base_plane(" in stage2
assert "hole_visual_only" in stage2
coarse = stage2[stage2.find("# ---- 3) 粗居中") : stage2.find("# ---- 4)")]
assert "pixel_on_base_plane" in coarse
assert "拒绝用深度图凑框心" in coarse
assert "_trim_xy_after_descent" in stage2
assert "align_max_iters" in stage2
assert "像素交工件面" in stage2
assert "det_depth = None if self.hole_visual_only else med_depth" in stage2
assert 'self.hole_keypoint_once = _as_bool(stage2.get("hole_keypoint_once")' in stage2
assert 'self.hole_once_lock_xy_descend = _as_bool(' in stage2
obs = stage2[stage2.find("def _observe_holes"): stage2.find("def _oneshot_align_move")]
assert "_goto(" not in obs, "核验拍不许改 XY"
assert "residual_remeasured" in obs
desc = stage2[stage2.find("okd, q, Tee = self._descend_above_hole"):]
desc = desc[:1800]
desc = stage2[stage2.find("# ---- 4.5)"):]
desc = desc[:2800]
assert "hole_once_lock_xy_descend" in desc
assert "if self.hole_keypoint_once:" in desc
assert 'tag="降后核验"' in desc
assert "_trim_xy_after_descent" in desc
assert "target_gap_m=self.observe_gap_m" in desc
assert "ONCE_APPROACH_STEP_M" in desc
lock_at = stage2.find("# ---- 4.5)")
assert lock_at > 0
lock_block = stage2[lock_at: stage2.find("elif self.hole_keypoint_once:", lock_at)]
assert "hole_once_lock_xy_descend" in lock_block
assert "lock_gap" in lock_block
assert "_once_ff_diagonal_to_gap" in lock_block
assert "self._descend_above_hole" not in lock_block
assert "_observe_holes" not in lock_block
assert "_trim_xy_after_descent" not in lock_block
assert "once_ff_tcp_xyz" in stage2
assert "once_ff_movel_from_tcp" in stage2
assert "_fresh_tcp_world" in stage2
assert "没有厂家 tcp_pose" in stage2
assert "write_request" in stage2
moved_at = stage2.find("q, Tee, _moved = self._oneshot_align_move")
moved_blk = stage2[moved_at: stage2.find("self._state(\"aligned\")", moved_at) + 80]
assert "hole_once_lock_xy_descend" in moved_blk
assert 'tag="走后核验"' in moved_blk
keep = stage2[
stage2.find("def _trim_xy_keep_hover_tip"): stage2.find("def _trim_xy_after_descent")
]
@@ -235,12 +265,15 @@ class TestHandoff:
assert abs(euler[0]) == pytest.approx(180.0, abs=1e-6)
assert abs(euler[1]) == pytest.approx(0.0, abs=1e-6)
assert abs(euler[2]) == pytest.approx(0.0, abs=1e-6)
assert "rdk_release_movel.py" in servo
movel = servo[servo.find("def _d435_vendor_movel"): servo.find("def _d435_oneshot_holds_locked")]
assert "rdk_release_movel.py" not in movel
assert "write_request" in movel
assert "d435_movel_from_tcp" in movel
assert "keep_z" in movel or "True" in movel
assert "拒绝杀栈凑 MoveL" in movel
assert "D435_VENDOR_MOVEL_VEL_MPS" in movel
assert "D435_VENDOR_MOVEL_ACC_MPS2" in movel
assert '"--acc"' in movel
assert '"0.05"' not in movel
assert "D435_VENDOR_MOVEL_ACC_MPS2" not in movel
assert '"--acc"' not in movel
assert D435_VENDOR_MOVEL_VEL_MPS == pytest.approx(0.3)
assert D435_VENDOR_MOVEL_ACC_MPS2 == pytest.approx(1.5)
rel = (
@@ -251,10 +284,14 @@ class TestHandoff:
assert f"default={D435_VENDOR_MOVEL_ACC_MPS2}" in rel
assert '"--acc"' in rel
assert 'prim["acc"]' in goto
tgt = d435_movel_from_tcp([0.580, -0.460, 0.096], [0.687, -0.113], [0.708, -0.168])
assert tgt is not None
np.testing.assert_allclose(tgt, [0.580 + 0.021, -0.460 - 0.055, 0.096], atol=1e-9)
assert d435_movel_from_tcp(None, [0.0, 0.0], [0.1, 0.1]) is None
assert "d435_handback_sec" in servo
assert "still_sec=max(0.0, self.d435_handback_sec)" in servo
assert "wrist_align_sec" in servo
# 杀栈 MoveL 还在走时不许把 vendor_done 提前置上，停稳计时也不许在这段路上累计。
# 进程内 MoveL 还在走时不许把 vendor_done 提前置上，停稳计时也不许在这段路上累计。
call_at = servo.find("ok = self._d435_vendor_movel()")
done_at = servo.find("self._d435_vendor_done = True", call_at)
assert call_at > 0 and done_at > call_at
@@ -274,6 +311,23 @@ class TestHandoff:
flat = np.eye(4)
flat[:3, 3] = [0.0, 0.0, 0.50]
assert pixel_on_base_plane(320, 240, 400, 400, 320, 240, flat, 0.022) is None
# 前倾 30°：同一主点，深度偏远的点比平面交点更 −X。爪站在深度点上时
# K1.x−规尖.x > 0；深度偏近则反号。这就是两套一阶段站位符号相反的几何。
ca, sa = float(np.cos(np.deg2rad(30))), float(np.sin(np.deg2rad(30)))
tilt = np.eye(4)
tilt[:3, :3] = np.array(
[[0.0, sa, -ca], [1.0, 0.0, 0.0], [0.0, -ca, -sa]], dtype=float
)
tilt[:3, 3] = [0.65, -0.50, 0.19]
plane_pt = pixel_on_base_plane(320, 240, 400, 400, 320, 240, tilt, 0.022)
assert plane_pt is not None
ray = tilt[:3, :3] @ np.array([0.0, 0.0, 1.0])
ray = ray / float(np.linalg.norm(ray))
far = plane_pt[:3] + 0.020 * ray
near = plane_pt[:3] - 0.020 * ray
assert far[0] < plane_pt[0] < near[0]
assert (plane_pt[0] - far[0]) > 0.0
assert (plane_pt[0] - near[0]) < 0.0
lib = (
__import__("pathlib").Path(__file__).resolve().parents[3] / "scripts" / "lib"
)
@@ -815,6 +869,10 @@ class TestHoleBiasAxes:
st2 = (site.get("hole_detect") or {}).get("stage2") or {}
assert st2.get("hole_keypoint_once") is True
assert st2.get("hole_visual_only") is True
assert st2.get("hole_once_lock_xy_descend") is True
assert st2.get("once_ff_look_per_m") == pytest.approx(ONCE_FF_LOOK_PER_M)
servo = (site.get("servo") or {})
assert servo.get("d435_handback_sec") == pytest.approx(0.5)
def test_oneshot_step_covers_half_hole_pair(self):
"""框心到 k1 约 29mm：必须整步走完，不许再截成 12mm。"""
@@ -836,3 +894,85 @@ class TestHoleBiasAxes:
assert ONCE_APPROACH_STEP_M == pytest.approx(0.020)
assert flange_z_for_gap_m(0.223, 0.022, 0.002) == pytest.approx(0.247)
assert flange_z_for_gap_m(0.223, 0.022, 0.015) == pytest.approx(0.260)
def test_final_gap_refusal_uses_tip_clearance_not_flange_z(self):
"""贴面参数是规尖离面。0 / 比悬停还高 / 法兰出盒必须红。"""
tool, surf, hover = 0.2245, 0.022, 0.320
z_lo, z_hi = 0.18, 0.65
assert final_gap_refusal(0.002, tool, surf, hover, z_lo, z_hi) == ""
assert final_gap_refusal(0.008, tool, surf, hover, z_lo, z_hi) == ""
assert "≤ 0" in final_gap_refusal(0.0, tool, surf, hover, z_lo, z_hi)
assert "未知" in final_gap_refusal(0.008, 0.0, surf, hover, z_lo, z_hi)
hover_clear = hover - tool - surf
why_up = final_gap_refusal(hover_clear + 0.005, tool, surf, hover, z_lo, z_hi)
assert "抬臂" in why_up
why_box = final_gap_refusal(0.008, tool, surf, hover, 0.40, z_hi)
assert "工作盒" in why_box
flange_8 = flange_z_for_gap_m(tool, surf, 0.008)
assert flange_8 == pytest.approx(0.2545)
assert flange_8 < hover
def test_final_gap_cli_overrides_site_and_conflicts_with_no_descent(self):
"""--final-gap 必须接到 launch/节点；和 --no-descent 一起写必须在脚本里红。"""
from pathlib import Path
root = Path(__file__).resolve().parents[1]
ws = root
while ws != ws.parent and not (ws / "scripts" / "run_all.sh").is_file():
ws = ws.parent
run_all = (ws / "scripts" / "run_all.sh").read_text(encoding="utf-8")
stage2 = (root / "arm_application" / "stage2_locate_node.py").read_text(
encoding="utf-8"
)
launch = (root / "launch" / "pipeline.launch.py").read_text(encoding="utf-8")
assert "--final-gap" in run_all
assert "final_gap_mm:=${FINAL_GAP}" in run_all
assert "同时写了 --final-gap 和 --no-descent" in run_all
assert 'self.declare_parameter("final_gap_mm"' in stage2
assert "final_gap_user_set" in stage2
assert "target_gap_m=lock_gap" in stage2
assert '"final_gap_mm": float(final_gap) if final_gap else -1.0' in launch
assert 'DeclareLaunchArgument("final_gap_mm"' in launch
def test_once_look_ff_is_minus_look_times_k_dz(self):
"""现行 k=0 → ΔXY=(0,0)。k=0.018、look=-X、降 71.5mm 才是 +1.287mm X。"""
k0 = float(ONCE_FF_LOOK_PER_M)
assert k0 == pytest.approx(0.0)
dxy0 = once_look_ff_xy([-1.0, 0.0], 0.0715, k0)
assert dxy0 is not None
np.testing.assert_allclose(dxy0, [0.0, 0.0], atol=1e-12)
np.testing.assert_allclose(
once_look_ff_xy([0.0, 0.0], 0.0715, 0.0), [0.0, 0.0], atol=1e-12
)
assert once_look_ff_xy([-1.0, 0.0], 0.0, 0.0) is None
k = 0.018
expect_x = k * 0.0715
dxy = once_look_ff_xy([-1.0, 0.0], 0.0715, k)
assert dxy is not None
np.testing.assert_allclose(dxy, [expect_x, 0.0], atol=1e-9)
assert expect_x == pytest.approx(0.001287, abs=1e-6)
assert once_look_ff_xy([0.0, 0.0], 0.0715, k) is None
assert once_look_ff_xy([-1.0, 0.0], 0.0, k) is None
packed = once_ff_tcp_xyz(
[0.50, -0.40],
[-0.001, -0.001],
[-1.0, 0.0],
0.0715,
k0,
0.022,
0.002,
)
assert packed is not None
tcp, dxy2 = packed
assert tcp[2] == pytest.approx(0.024)
assert tcp[2] < 0.18
np.testing.assert_allclose(tcp[:2], [0.50 - 0.001, -0.40 - 0.001])
np.testing.assert_allclose(dxy2, [0.0, 0.0], atol=1e-12)
assert ONCE_FF_MOVEL_VEL_MPS == pytest.approx(0.05)
dz_refuse = ONCE_TRIM_REFUSE_M / k + 0.01
assert once_look_ff_xy([-1.0, 0.0], dz_refuse, k) is not None
assert float(np.hypot(*once_look_ff_xy([-1.0, 0.0], dz_refuse, k))) > ONCE_TRIM_REFUSE_M
rel = once_ff_movel_from_tcp([0.580, -0.460, 0.096], [0.0, 0.0], 0.0715)
assert rel is not None
np.testing.assert_allclose(rel, [0.580, -0.460, 0.0245], atol=1e-9)
assert once_ff_movel_from_tcp([0.580, -0.460, 0.096], [0.00129, 0.0], 0.0) is None
src/arm_application/test/test_overhead_track.py
+1 -1
View File
@@ -238,7 +238,7 @@ class TestRealSiteBranchDefault:
assert s["handeye_mode"] == "eye_in_hand"
assert s["handeye_file"].endswith("89_eye_in_hand.yaml")
assert s["lock_still_sec"] == pytest.approx(3.0)
assert s["d435_handback_sec"] == pytest.approx(3.0)
assert s["d435_handback_sec"] == pytest.approx(0.5)
assert s["wrist_align_sec"] == pytest.approx(0.5)
assert s["lock_still_move_m"] == pytest.approx(0.015)
# 负向：跟踪外参不能被换成顶视文件
src/arm_application/test/test_stage2_coarse_guard.py
+5
View File
@@ -254,6 +254,11 @@ def test_coarse_loop_rejects_before_moving():
"又在咬孔检测器的 bbox 当粗居中目标了（真机那帧是整幅画面）"
)
assert "skip_coarse = True" in src, "'跳过粗居中'必须真的不移动"
coarse = src[src.find("# ---- 3) 粗居中") : src.find("# ---- 4)")]
assert "hole_visual_only" in coarse
assert "pixel_on_base_plane" in coarse
assert "拒绝用深度图凑框心" in coarse
assert 'src = "像素交工件面"' in coarse
def test_guard_reason_is_logged_with_both_numbers():
src/arm_application/test/test_stage2_descent_clamp.py
+194
View File
@@ -36,6 +36,7 @@ from __future__ import annotations
from typing import Any, List, Optional
import numpy as np
import pytest
from arm_application.overhead_oneshot import ONCE_APPROACH_STEP_M
from arm_application.stage2_locate_node import (
@@ -112,6 +113,7 @@ class _StubStage2:
self._contact_after = contact_after_steps
# 记录每一次实际下发的下降步长，用来核对累计行程
self.steps_mm: List[float] = []
self.goto_hold_xy: List[bool] = []
self.pins = 0
# --- 被测函数会调到的钩子 ---
@@ -134,10 +136,14 @@ class _StubStage2:
def _goto(self, _q: Any, Tee: Any, target: Any, **_kw: Any) -> Any:
Tee = np.array(Tee, dtype=float)
self.goto_hold_xy.append(bool(_kw.get("hold_xy")))
step_mm = (Tee[2, 3] - float(target[2])) * 1000.0
# 记录"下降步"（正数）；回退步（负数）不计入下降行程
if step_mm > 0.0:
self.steps_mm.append(step_mm)
if not _kw.get("hold_xy"):
Tee[0, 3] = float(target[0])
Tee[1, 3] = float(target[1])
Tee[2, 3] = float(target[2])
self._cur_z = float(target[2])
return True, np.zeros(7), Tee
@@ -290,6 +296,194 @@ def test_contact_not_armed_while_still_high_above_the_part():
assert contact_armed_by_gap(0.005, 0.015) is True
def test_lock_xy_descend_holds_xy_and_stops_at_final_gap():
"""锁 XY 实验：从悬停纯 Z 收到 2mm，XY 不许动，步长仍 20mm。"""
import rclpy
if not rclpy.ok():
rclpy.init()
st = _StubStage2()
Tee0 = np.eye(4)
Tee0[0, 3] = 0.512
Tee0[1, 3] = -0.401
Tee0[2, 3] = _HOVER_Z
ok, _q, Tee = Stage2Locate._descend_above_hole( # type: ignore[arg-type]
st, np.zeros(7), Tee0, target_gap_m=_FINAL_GAP, step_m=ONCE_APPROACH_STEP_M
)
assert ok is True
gap = float(Tee[2, 3]) - _TOOL_LEN - _SURFACE_Z
assert abs(gap - _FINAL_GAP) < 0.001, f"应停在贴面 2mm，实际 {gap * 1000:.1f}mm"
assert Tee[0, 3] == 0.512
assert Tee[1, 3] == -0.401
assert max(st.steps_mm) <= ONCE_APPROACH_STEP_M * 1000 + 1e-6
assert all(st.goto_hold_xy), "下臂每一步都必须 hold_xy"
def _look_neg_x_T() -> np.ndarray:
T = np.eye(4)
T[:3, :3] = np.array(
[[0.0, 0.0, -1.0], [1.0, 0.0, 0.0], [0.0, -1.0, 0.0]],
dtype=float,
)
return T
def test_once_ff_zero_k_is_cartesian_lock_xy_movel():
"""k=0 必须下发相对 MoveL，且 ΔXY=0、只降 Z。不许停在悬停高装成功。"""
import rclpy
if not rclpy.ok():
rclpy.init()
st = _ff_stub_ready()
st.once_ff_look_per_m = 0.0
st._lookup = lambda *_a, **_k: np.eye(4) # type: ignore[method-assign]
st._fresh_tcp_world = lambda: (0.580, -0.460, 0.096) # type: ignore[method-assign]
got: list = []
st._vendor_movel_tcp = ( # type: ignore[method-assign]
lambda xyz, _vel: got.append([float(xyz[0]), float(xyz[1]), float(xyz[2])]) or True
)
Tee = np.eye(4)
Tee[0, 3] = 0.595
Tee[1, 3] = -0.467
Tee[2, 3] = _HOVER_Z
ok, _q, _Tee1 = Stage2Locate._once_ff_diagonal_to_gap( # type: ignore[arg-type]
st, np.zeros(7), Tee, target_gap_m=_FINAL_GAP
)
assert ok is True
assert len(got) == 1
gap0 = _HOVER_Z - _TOOL_LEN - _SURFACE_Z
descent = gap0 - _FINAL_GAP
np.testing.assert_allclose(got[0], [0.580, -0.460, 0.096 - descent], atol=1e-9)
assert any("笛卡尔锁XY只降Z" in m for m in st.logs)
def _ff_stub_ready() -> _StubStage2:
st = _StubStage2()
st.once_ff_look_per_m = 0.0
st._tip_off = np.array([-0.001, -0.001], dtype=float)
st.cam_frame = "cam"
st.base_frame = "base"
st._ws_clipped = False
st._lookup = lambda *_a, **_k: _look_neg_x_T() # type: ignore[method-assign]
st._clamp_ws = lambda *_a, **_k: None # type: ignore[method-assign]
st._spin = lambda *_a, **_k: None # type: ignore[method-assign]
st._wait_state = lambda *_a, **_k: (None, None) # type: ignore[method-assign]
st.robot_states_topic = "/Rizon4_063989/flexiv_robot_states"
return st
def test_once_ff_refuses_without_tcp_pose():
"""没有厂家 tcp_pose 不许退回 ROS 绝对坐标。"""
import rclpy
if not rclpy.ok():
rclpy.init()
st = _ff_stub_ready()
st._fresh_tcp_world = lambda: None # type: ignore[method-assign]
called: list = []
st._vendor_movel_tcp = lambda *_a, **_k: called.append(1) or True # type: ignore[method-assign]
Tee = np.eye(4)
Tee[0, 3] = 0.595
Tee[1, 3] = -0.467
Tee[2, 3] = _HOVER_Z
ok, _q, Tee1 = Stage2Locate._once_ff_diagonal_to_gap( # type: ignore[arg-type]
st, np.zeros(7), Tee, target_gap_m=_FINAL_GAP
)
assert ok is False
assert called == []
assert Tee1[2, 3] == pytest.approx(_HOVER_Z)
assert any("没有厂家 tcp_pose" in m for m in st.logs)
def test_once_ff_movel_is_flexiv_tcp_plus_delta():
"""MoveL 必须是当前厂家 TCP + (ΔXY, −下降)，不是 ROS 规尖绝对坐标。"""
import rclpy
if not rclpy.ok():
rclpy.init()
st = _ff_stub_ready()
st._fresh_tcp_world = lambda: (0.580, -0.460, 0.096) # type: ignore[method-assign]
got: list = []
st._vendor_movel_tcp = ( # type: ignore[method-assign]
lambda xyz, _vel: got.append([float(xyz[0]), float(xyz[1]), float(xyz[2])]) or True
)
Tee = np.eye(4)
Tee[0, 3] = 0.595
Tee[1, 3] = -0.467
Tee[2, 3] = _HOVER_Z
ok, _q, _Tee1 = Stage2Locate._once_ff_diagonal_to_gap( # type: ignore[arg-type]
st, np.zeros(7), Tee, target_gap_m=_FINAL_GAP
)
assert ok is True
assert len(got) == 1
gap0 = _HOVER_Z - _TOOL_LEN - _SURFACE_Z
descent = gap0 - _FINAL_GAP
np.testing.assert_allclose(got[0], [0.580, -0.460, 0.096 - descent], atol=1e-9)
assert got[0][0] == pytest.approx(0.580)
assert not np.allclose(got[0][:2], [0.595 - 0.001, -0.467 - 0.001], atol=1e-4)
def test_once_ff_custom_final_gap_only_changes_z():
"""--final-gap 8mm：MoveL XY 仍为 0，下降量必须少 6mm，不许再走贴面 2mm。"""
import rclpy
if not rclpy.ok():
rclpy.init()
st = _ff_stub_ready()
st._fresh_tcp_world = lambda: (0.580, -0.460, 0.096) # type: ignore[method-assign]
got: list = []
st._vendor_movel_tcp = ( # type: ignore[method-assign]
lambda xyz, _vel: got.append([float(xyz[0]), float(xyz[1]), float(xyz[2])]) or True
)
Tee = np.eye(4)
Tee[0, 3] = 0.595
Tee[1, 3] = -0.467
Tee[2, 3] = _HOVER_Z
gap_custom = 0.008
ok, _q, _Tee1 = Stage2Locate._once_ff_diagonal_to_gap( # type: ignore[arg-type]
st, np.zeros(7), Tee, target_gap_m=gap_custom
)
assert ok is True
assert len(got) == 1
gap0 = _HOVER_Z - _TOOL_LEN - _SURFACE_Z
descent = gap0 - gap_custom
descent_2mm = gap0 - _FINAL_GAP
np.testing.assert_allclose(got[0], [0.580, -0.460, 0.096 - descent], atol=1e-9)
assert got[0][0] == pytest.approx(0.580)
assert got[0][1] == pytest.approx(-0.460)
assert descent == pytest.approx(descent_2mm - 0.006)
assert any("笛卡尔锁XY只降Z" in m for m in st.logs)
def test_once_ff_k_018_still_adds_look_xy():
"""有人把 k 改回 0.018 时，MoveL 必须真的加 +k×下降，不许再吞成绝对坐标。"""
import rclpy
if not rclpy.ok():
rclpy.init()
st = _ff_stub_ready()
st.once_ff_look_per_m = 0.018
st._fresh_tcp_world = lambda: (0.580, -0.460, 0.096) # type: ignore[method-assign]
got: list = []
st._vendor_movel_tcp = ( # type: ignore[method-assign]
lambda xyz, _vel: got.append([float(xyz[0]), float(xyz[1]), float(xyz[2])]) or True
)
Tee = np.eye(4)
Tee[0, 3] = 0.595
Tee[1, 3] = -0.467
Tee[2, 3] = _HOVER_Z
ok, _q, _Tee1 = Stage2Locate._once_ff_diagonal_to_gap( # type: ignore[arg-type]
st, np.zeros(7), Tee, target_gap_m=_FINAL_GAP
)
assert ok is True
assert len(got) == 1
gap0 = _HOVER_Z - _TOOL_LEN - _SURFACE_Z
descent = gap0 - _FINAL_GAP
dxy = 0.018 * descent
np.testing.assert_allclose(got[0], [0.580 + dxy, -0.460, 0.096 - descent], atol=1e-9)
assert any("视线前馈斜线" in m for m in st.logs)
def test_oneshot_approach_stops_at_observe_gap_with_larger_steps():
"""单次精拍第一段只降到 15mm，步长 20mm（约 3 步），不许 4mm 切到 2mm。"""
import rclpy