diff --git a/config/site.example.yaml b/config/site.example.yaml index 848298b..34db6c4 100644 --- a/config/site.example.yaml +++ b/config/site.example.yaml @@ -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。 diff --git a/config/site.yaml b/config/site.yaml index 792d5c3..2be5d83 100644 --- a/config/site.yaml +++ b/config/site.yaml @@ -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。 diff --git a/docs/HANDBOOK.md b/docs/HANDBOOK.md index bcc81a5..9da9c49 100644 --- a/docs/HANDBOOK.md +++ b/docs/HANDBOOK.md @@ -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。 diff --git a/docs/HOLE_DETECT.md b/docs/HOLE_DETECT.md index 284268a..fba4c2a 100644 --- a/docs/HOLE_DETECT.md +++ b/docs/HOLE_DETECT.md @@ -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 在同名段）。 diff --git a/handeye/poses/89_eye_to_hand.yaml b/handeye/poses/89_eye_to_hand.yaml index b2261ea..7620bbd 100644 --- a/handeye/poses/89_eye_to_hand.yaml +++ b/handeye/poses/89_eye_to_hand.yaml @@ -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, ] diff --git a/scripts/run_all.sh b/scripts/run_all.sh index 389fb32..647484f 100755 --- a/scripts/run_all.sh +++ b/scripts/run_all.sh @@ -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[@]}" diff --git a/scripts/run_stage2_locate.sh b/scripts/run_stage2_locate.sh index 8642d92..1a940c7 100755 --- a/scripts/run_stage2_locate.sh +++ b/scripts/run_stage2_locate.sh @@ -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[@]}" diff --git a/src/arm_application/arm_application/overhead_oneshot.py b/src/arm_application/arm_application/overhead_oneshot.py index d838b0f..fa1d3b1 100644 --- a/src/arm_application/arm_application/overhead_oneshot.py +++ b/src/arm_application/arm_application/overhead_oneshot.py @@ -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) diff --git a/src/arm_application/arm_application/servo_node.py b/src/arm_application/arm_application/servo_node.py index afcab20..4e49b88 100644 --- a/src/arm_application/arm_application/servo_node.py +++ b/src/arm_application/arm_application/servo_node.py @@ -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) diff --git a/src/arm_application/arm_application/stage2_locate_node.py b/src/arm_application/arm_application/stage2_locate_node.py index 01531c4..670bacb 100644 --- a/src/arm_application/arm_application/stage2_locate_node.py +++ b/src/arm_application/arm_application/stage2_locate_node.py @@ -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,8 +251,14 @@ 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（不接触） - self.final_gap_m = float(stage2.get("final_gap_mm") or 1.5) / 1000.0 + # 最后轻降到孔面上方 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）高一截，对偏了也不会把规压上孔沿。 self.observe_gap_m = float(stage2.get("observe_gap_mm") or 15.0) / 1000.0 @@ -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) - 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 到位，不把这次当对准成功" + 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="走后核验" + ) + if not final.get("residual_remeasured"): + self.get_logger().error( + f"走后核验没复测到。开环指令残差 {commanded_mm:.2f}mm，" + "不是 hop 到位，不把这次当对准成功" + ) + final["ok"] = False + self.get_logger().info( + f"单次精拍悬停走完：指令 {commanded_mm:.2f}mm → " + f"复测 {final.get('residual_mm')}mm；下降后还要补交平面漂的那几毫米" ) - final["ok"] = False self._state("aligned") - self.get_logger().info( - f"单次精拍悬停走完：指令 {commanded_mm:.2f}mm → " - f"复测 {final.get('residual_mm')}mm；下降后还要补交平面漂的那几毫米" - ) 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="降后核验") diff --git a/src/arm_application/launch/pipeline.launch.py b/src/arm_application/launch/pipeline.launch.py index ca6b77a..ea18aff 100644 --- a/src/arm_application/launch/pipeline.launch.py +++ b/src/arm_application/launch/pipeline.launch.py @@ -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), ] diff --git a/src/arm_application/launch/stage2_locate.launch.py b/src/arm_application/launch/stage2_locate.launch.py index fcdb997..f7cdc5b 100644 --- a/src/arm_application/launch/stage2_locate.launch.py +++ b/src/arm_application/launch/stage2_locate.launch.py @@ -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), ] diff --git a/src/arm_application/test/test_overhead_oneshot.py b/src/arm_application/test/test_overhead_oneshot.py index 730b965..5d18dc6 100644 --- a/src/arm_application/test/test_overhead_oneshot.py +++ b/src/arm_application/test/test_overhead_oneshot.py @@ -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 diff --git a/src/arm_application/test/test_overhead_track.py b/src/arm_application/test/test_overhead_track.py index 3278f73..be72bba 100644 --- a/src/arm_application/test/test_overhead_track.py +++ b/src/arm_application/test/test_overhead_track.py @@ -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) # 负向：跟踪外参不能被换成顶视文件 diff --git a/src/arm_application/test/test_stage2_coarse_guard.py b/src/arm_application/test/test_stage2_coarse_guard.py index 7a2ef41..d8f24c5 100644 --- a/src/arm_application/test/test_stage2_coarse_guard.py +++ b/src/arm_application/test/test_stage2_coarse_guard.py @@ -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(): diff --git a/src/arm_application/test/test_stage2_descent_clamp.py b/src/arm_application/test/test_stage2_descent_clamp.py index 11faa10..843111a 100644 --- a/src/arm_application/test/test_stage2_descent_clamp.py +++ b/src/arm_application/test/test_stage2_descent_clamp.py @@ -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