贴面后不再抬回悬高；MoveL 切回关节后不再把整段位移当前进速度。
搜孔开局会抬到悬高再落回 hole_hover_z_m，螺纹还没咬合臂就离开孔口。没有接触传感器，到位只认 final_gap 算出的法兰高度，贴面后停在这个 Z。09:25 厂家 MoveL 切回 NRT_JOINT_POSITION 的第一拍把整段关节差算成 1.686 rad/s，前瞻再推 0.07 rad，光心从 5 mm 冲到 276 mm 后急停。切回后钉住结束瞬间的关节角并清零速度，JTC 还指着出发前的关节时不放行。 Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
+40
-41
@@ -23,7 +23,7 @@ robot:
|
||||
# 本臂装了 Orisys 视触觉（只有左臂有）。脚本靠这一项决定要不要拉触觉服务，
|
||||
# 而不是靠"SN 等于某个具体值"这种写死的判断（换台臂就会失效）。
|
||||
tactile: true
|
||||
# 2026-10-12 示教的工具 TCP（tool + 1012 + 0938）。眼在手上 AX=XB 用这份 TCP。
|
||||
# 2026-10-16 下午 RDK 重读 Studio tool10120938（用户再次更新 TCP）。
|
||||
tool_name: "tool10120938"
|
||||
camera:
|
||||
name: d405_left # 话题 /d405_left/... 与 91 分开
|
||||
@@ -45,17 +45,17 @@ robot:
|
||||
# （status 里的 init=need_init 只说明"绝对位置标定没做"，不影响夹持）
|
||||
# 想让开机必初始化（无人值守的上电流程）就显式改 true，并确认爪里是空的。
|
||||
auto_init: false
|
||||
named_poses_file: "src/arm_control/config/named_poses_89.yaml"
|
||||
named_poses_file: "/home/cc/ros2/src/arm_control/config/named_poses_89.yaml"
|
||||
paths:
|
||||
handeye_file: "handeye/results/89_eye_in_hand.yaml"
|
||||
handeye_waypoints: "handeye/poses/89_eye_in_hand.yaml"
|
||||
handeye_workdir: "handeye/tmp/89"
|
||||
handeye_file: "/home/cc/ros2/handeye/results/89_eye_in_hand.yaml"
|
||||
handeye_waypoints: "/home/cc/ros2/handeye/poses/89_eye_in_hand.yaml"
|
||||
handeye_workdir: "/home/cc/ros2/handeye/tmp/89"
|
||||
handeye:
|
||||
# Studio tcp_location（法兰系，米 + qwxyz）。与 rdk Tool.params 必须一致。
|
||||
# 眼在手上：AX=XB 的手是这份 TCP，再折成 cam→法兰写 T_4x4。
|
||||
# 眼在手外：AX=XB 的手同样是这份 TCP（板夹在工具上）；T_4x4 仍是 cam→基座，不折。
|
||||
gripper_tool: "tool10120938"
|
||||
gripper_tcp_xyz: [-0.0005, 0.0067, 0.2245]
|
||||
gripper_tcp_xyz: [0.000537, -0.000264, 0.226587]
|
||||
gripper_tcp_qwxyz: [1.0, 0.0, 0.0, 0.0]
|
||||
# 左臂跟踪高度（只在 --arm 89 时生效，改 91 的 servo 不影响这里）
|
||||
# 锚点 `&servo_left`：下面的"仿真台"条目直接引用它 —— 仿真建的就是左臂，
|
||||
@@ -163,9 +163,11 @@ robot:
|
||||
# 落在本文件下方注释记录的合理区间 0.204~0.233 内。
|
||||
# 复核：python3 scripts/sense_tool_length.py --sn Rizon4-063989
|
||||
# python3 scripts/lib/real_geometry_check.py --sn Rizon4-063989
|
||||
# 2026-10-12：下臂安全距离改按 Studio 工具 tool10120938 的 TCP +Z。
|
||||
# 上面 0.223 是旧 tool1（夹爪 TCP 0.147 + 规露出 0.076），这把工具不再用。
|
||||
tool_tip_below_flange_m: 0.2245
|
||||
# 2026-10-16 下午：RDK Tool.params("tool10120938").tcp_location
|
||||
# [x,y,z,qw,qx,qy,qz] = [0.000537, -0.000264, 0.226587, 1, 0, 0, 0]
|
||||
# 示教器拟合误差：平均 0.58 / 最大 0.99 / 最小 0.29 mm。
|
||||
# 下臂安全距离用 TCP +Z。上午 0.226274；再往前 0.2245 是 10-12；0.223 是 tool1。
|
||||
tool_tip_below_flange_m: 0.226587
|
||||
# >>> 仿真那台臂（Gazebo）也登记成 robots[] 里的一台 <<<
|
||||
# 为什么必须登记：应用是按"当前臂"的名字去查 frame / joint / 相机话题的
|
||||
#（`sim_env.sh` 注释里说的"仿真与真机共用同一套 site.yaml"）。仿真的 TF 与关节
|
||||
@@ -195,7 +197,7 @@ robot:
|
||||
# 为什么必须给文件而不能留空：visual_servo 的 servo_node 需要
|
||||
# T_flange_cam 矩阵做像素→笛卡尔换算；留空它就直接拒绝启动
|
||||
#（"手眼标定结果缺失"），S04 跟踪起不来。
|
||||
handeye_file: "handeye/results/sim_eye_in_hand.yaml"
|
||||
handeye_file: "/home/cc/ros2/handeye/results/sim_eye_in_hand.yaml"
|
||||
# 运动参数：**大部分**与 89 共用（锚点），但**高度类**必须按仿真台的几何改写。
|
||||
#
|
||||
# 为什么不能直接共用：真机是"机械臂装在台面同一高度"，所以"法兰相对基座"
|
||||
@@ -289,11 +291,11 @@ robot:
|
||||
port: "/dev/ttyUSB1"
|
||||
stroke_mm: 30.0
|
||||
auto_init: false
|
||||
named_poses_file: "src/arm_control/config/named_poses_91.yaml"
|
||||
named_poses_file: "/home/cc/ros2/src/arm_control/config/named_poses_91.yaml"
|
||||
paths:
|
||||
handeye_file: "handeye/results/91_eye_in_hand.yaml"
|
||||
handeye_waypoints: "handeye/poses/91_eye_in_hand.yaml"
|
||||
handeye_workdir: "handeye/tmp/91"
|
||||
handeye_file: "/home/cc/ros2/handeye/results/91_eye_in_hand.yaml"
|
||||
handeye_waypoints: "/home/cc/ros2/handeye/poses/91_eye_in_hand.yaml"
|
||||
handeye_workdir: "/home/cc/ros2/handeye/tmp/91"
|
||||
# >>> 右臂跟踪高度：只改这里，89 仍用上面那段 <<<
|
||||
# 法兰 Z；RGI 比平行爪高，通常要比 89 略抬一点。改完重启：
|
||||
# bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 91 --control
|
||||
@@ -314,7 +316,7 @@ robot:
|
||||
|
||||
paths:
|
||||
# >>> YOLO 权重（标准放法，不必改代码）<<<
|
||||
# 1) 把 .onnx / .pt 放到 models/yolo/
|
||||
# 1) 把 .onnx / .pt 放到 /home/cc/ros2/models/yolo/
|
||||
# 2) 这一行改成那个绝对路径（写 .onnx 即可；TensorRT 会找同名 .engine）
|
||||
# 3) backend=tensorrt 时必须先导出引擎（只需做一次）：
|
||||
# bash ~/ros2/scripts/weights.sh convert <源文件> --name <名字> --group yolo
|
||||
@@ -328,18 +330,18 @@ paths:
|
||||
# ultralytics 解不了 pose 的 TensorRT engine，运行时走
|
||||
# arm_application/pose_detector.py 的自解码（见该模块头）。
|
||||
# 仍然用 .engine（实测 ~2.6 ms/帧；onnx 只有 CPU provider，约 27 ms）。
|
||||
yolo_weights: "models/yolo/gongjian0929.onnx"
|
||||
yolo_weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
|
||||
# 换模型：把 models/yolo/ 下别的 .onnx / .pt 绝对路径填进来即可。
|
||||
# 未在用的模型已移出仓库（体积大且干扰 `weights.sh list`），要恢复见 models/README.md。
|
||||
# 入孔模型（backend=yolo 时把孔当框）。two_stage 用下面的工件权重。
|
||||
# pose 模型两处共用同一份 —— 它既能框工件，又直接给 k1/k2 关键点。
|
||||
# 查悬空引用：bash ~/ros2/scripts/weights.sh doctor
|
||||
hole_weights: "models/yolo/gongjian0929.onnx"
|
||||
hole_weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
|
||||
# 下面是缺省（未匹配到 robots[] 时）。真机以 robots[].paths 为准：89 / 91 各一份。
|
||||
handeye_file: "handeye/results/89_eye_in_hand.yaml"
|
||||
handeye_waypoints: "handeye/poses/89_eye_in_hand.yaml"
|
||||
handeye_calibrate: "handeye/calibrate.yaml"
|
||||
handeye_workdir: "handeye/tmp/89"
|
||||
handeye_file: "/home/cc/ros2/handeye/results/89_eye_in_hand.yaml"
|
||||
handeye_waypoints: "/home/cc/ros2/handeye/poses/89_eye_in_hand.yaml"
|
||||
handeye_calibrate: "/home/cc/ros2/handeye/calibrate.yaml"
|
||||
handeye_workdir: "/home/cc/ros2/handeye/tmp/89"
|
||||
# 空=默认 DDS(SHM)；勿强制 UDP-only，否则相机预览收不到 /workpiece/detection
|
||||
fastdds_profile: ""
|
||||
|
||||
@@ -579,9 +581,8 @@ servo:
|
||||
lock_hold_sec: 0.7 # 旧死区驻留（混合模式不再用它进二阶段）
|
||||
# 混合模式必填。交回 D405 的等待不再用它，见 d435_handback_sec。
|
||||
lock_still_sec: 3.0
|
||||
# 进程内 MoveL 到位后，法兰 XY 连续停稳这么久才交回 D405。
|
||||
# 旧 3s 是杀栈冷启动的垫时间；不杀栈后只留机械停稳。晃了把数加大，不许再杀栈。
|
||||
d435_handback_sec: 0.5
|
||||
# 厂家 MoveL 结束后，法兰连续这么久没再动，才打开 D405 框心。
|
||||
d435_handback_sec: 2.0
|
||||
# 框心对准并且法兰停稳这么久，才精拍一次小孔关键点、进对孔。实时点到此为止。
|
||||
wrist_align_sec: 0.5
|
||||
lock_still_move_m: 0.015 # 15 mm；超过就重计时，避免"晃一下刚好满 3 秒"
|
||||
@@ -663,10 +664,10 @@ gripper:
|
||||
hole_detect:
|
||||
backend: "two_stage" # two_stage | yolo | hough
|
||||
# backend=yolo 时用。
|
||||
weights: "models/yolo/gongjian0929.onnx"
|
||||
weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
|
||||
# 二阶段工件检测权重。pose 模型既框工件、又直接给 k1/k2 关键点，
|
||||
# 所以和上面的跟踪权重是同一份。
|
||||
workpiece_weights: "models/yolo/gongjian0929.onnx"
|
||||
workpiece_weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
|
||||
# pose 关键点优先：检测器给了 k1/k2 就直接用，不用再找圆。
|
||||
# 关键点不可信（低于 keypoint_min_conf / 两点几乎重合）时自动回落到
|
||||
# 亮环 + Hough 那套老路径。
|
||||
@@ -674,7 +675,7 @@ hole_detect:
|
||||
keypoint_min_conf: 0.30
|
||||
keypoint_min_dist_px: 15.0
|
||||
keypoint_hole_radius_px: 12.0 # 关键点没有半径；反投影时按这个半径取孔周深度环
|
||||
config: "src/arm_application/config/hole_detect.yaml"
|
||||
config: "/home/cc/ros2/src/arm_application/config/hole_detect.yaml"
|
||||
conf: 0.25
|
||||
iou: 0.7
|
||||
imgsz: 640
|
||||
@@ -752,7 +753,7 @@ hole_detect:
|
||||
# bbox=(0,142,97,222) 反投影出盒外的 (0.280,-0.747)（偏差 211.5mm），旧逻辑
|
||||
# **不判误检、只把目标截断到盒边**，臂照样横移 56mm 离开工件 ⇒ 精定位再也
|
||||
# 看不到 k1/k2 ⇒ exit=4。判据改成"盒外/偏移过大 ⇒ 判误检、原地不动"。
|
||||
coarse_max_offset_m: 0.12 # 目标离夹爪轴线超过此值就不像粗居中量 [m]
|
||||
coarse_max_offset_m: 0.08 # 目标离夹爪轴线超过此值就不像粗居中量 [m]；0.12 放行了 2026-10-16 的 110mm 回撤
|
||||
coarse_border_px: 2 # bbox 贴边（被画面切）即弃用 [px]；见 _bbox_on_border
|
||||
# —— 柔顺运动（夹爪持规，防急加速/急停/过冲把规震脱）：速度放慢 + 平滑刹车 ——
|
||||
align_speed: 0.014 # 对准段笛卡尔速度 [m/s]（原0.03，放缓防冲）
|
||||
@@ -771,7 +772,7 @@ hole_detect:
|
||||
# 时法兰 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。
|
||||
# 左臂89：悬停间隙约 71.4mm（0.320−0.226587−0.022）⇒ 观测下降约 56.4mm。
|
||||
final_descent_speed: 0.005 # 最后轻降速度 [m/s]（很慢，柔顺）
|
||||
# ✅ 2026-10-08 定稿：下降量**只由几何决定**（法兰 FK − 工具长 − 工件面 − final_gap），
|
||||
# 深度读数只做交叉核对、不再参与任何决策。两段真机事故方向相反、都指向这条：
|
||||
@@ -784,7 +785,7 @@ hole_detect:
|
||||
# ⇒ 读数往**小**错一次就永远到不了目标。
|
||||
# 同一套读数两个方向各错一次、幅度 150mm 量级、随位姿乱跳 —— 它对"能降多少"
|
||||
# 和"该不该停"都没有发言权。现在它只在偏差 >10mm 时喊一句"深度测距与几何不符"。
|
||||
# 现行几何：悬停 0.320 − 工具长 0.2245 − 工件面 0.022 = 73.5mm 间隙 ⇒ 降 71.5mm 到 2mm。
|
||||
# 现行几何：悬停 0.320 − 工具长 0.226587 − 工件面 0.022 = 71.4mm 间隙 ⇒ 降 69.4mm 到 2mm。
|
||||
# 三道**物理/几何**保险：descent_budget_m（配置上限）+ 腕部 FT 接触检测（停+退 1mm）
|
||||
# + 任一步 _goto 走不到位就停。
|
||||
# 缺几何（工具长/工件面没配）时**拒绝下降**并报错（保持悬停），绝不拿测距去赌。
|
||||
@@ -796,9 +797,11 @@ hole_detect:
|
||||
max_depth_m: 0.60
|
||||
linger_sec: 8.0 # 完成后继续发布结果/叠图的时长
|
||||
|
||||
# ===== 旋转装配仍读这里的力阈值（stage3_search_node）=====
|
||||
# 搜孔轻压已取消：不带 --screw 的入口会直接拒绝，整体流程也不再进入这一步。
|
||||
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw
|
||||
# ===== 三阶段柔顺搜孔（stage3_search_node，Orisys 触觉 + 力闭环螺旋）=====
|
||||
# 前置：二阶段已完成，臂悬停在目标孔上方。先 run_tactile.sh --cli 看力读数量级再调阈值。
|
||||
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 # 搜孔+落座
|
||||
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 0 # 只搜孔定心不插入
|
||||
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0 # 螺纹规旋入
|
||||
stage3:
|
||||
# —— 力阈值全是「相对静止均值的增量」（Orisys 读数是形变任意单位且基线会漂移，
|
||||
# 节点启动时自动测 1.2s 静止均值 μ0/方差 σ0，看日志「触觉基线/派生阈值」两行）——
|
||||
@@ -825,12 +828,9 @@ hole_detect:
|
||||
drop_frames: 2 # 跌落需持续的帧数（防毛刺）
|
||||
center_scan: false # 入孔后十字微扫；默认关（0.8mm 扫会把已入孔的规刮偏）
|
||||
center_scan_mm: 0.3
|
||||
insert_depth_mm: 0.0 # 默认只搜孔+轻压，不拧入。旋转装配不读这项
|
||||
insert_depth_mm: 0.0 # 默认只搜孔+轻压，不拧入；拧入阶段再用 --depth
|
||||
insert_retry: 3
|
||||
screw_pitch_mm: 0.0 # 搜孔轻压不旋。旋转装配不读这项
|
||||
# 控制页「旋转装配」单独用。数值就是手册里的 M6 旋入：深度 6 mm、螺距 1.0 mm/圈。
|
||||
screw_stage_depth_mm: 6.0
|
||||
screw_stage_pitch_mm: 1.0
|
||||
screw_pitch_mm: 0.0 # 螺纹通止规螺距：>0 右旋 / <0 左旋 / 0=不旋（光面规）
|
||||
search_speed: 0.03 # 搜孔笛卡尔速度 [m/s]
|
||||
settle_sec: 0.25 # 每个微步后的静止时间
|
||||
tactile_timeout_s: 3.0 # 触觉话题断流多久中止
|
||||
@@ -1086,7 +1086,7 @@ tactile:
|
||||
# 传感器型号。必须与 devices/sensors/orisys/models/ 下的子目录名一致。
|
||||
model: "dd03"
|
||||
# 型号配置随 SDK 一起放：devices/sensors/orisys/models/<型号>/config.json
|
||||
config_file: "devices/sensors/orisys/models/dd03/config.json"
|
||||
config_file: "/home/cc/ros2/devices/sensors/orisys/models/dd03/config.json"
|
||||
# 用 by-path 稳定路径（插哪个 USB 口就认哪个口，不受 video 编号漂移影响）。
|
||||
# 当前：3.4.1=左指 3.4.2=右指；若左右反了，互换两行即可。
|
||||
left_video: "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:3.4.1:1.0-video-index0"
|
||||
@@ -1113,8 +1113,7 @@ go_home:
|
||||
|
||||
network:
|
||||
# 2026-09-29 改线：两台臂 → 交换机 → USB-C 千兆网卡（ASIX AX88179）
|
||||
# MAC 6c:1f:f7:d0:6f:17。内核名会在 eth0 和 enx6c1ff7d06f17 之间变，
|
||||
# NetworkManager 按 MAC 绑定，不按名字。
|
||||
# MAC 6c:1f:f7:d0:6f:17。内核现在叫 eth0（不再用 enx6c1ff7d06f17）。
|
||||
# 本机 192.168.2.10 ↔ 063989(左89)=.101 / 063991(右91)=.100
|
||||
# NM 配置名 robot-usb-lan（静态 .10/24，不当默认路由）。
|
||||
# 说明：enp4s0 仍是办公网（192.168.1.x）；两台臂已不在 enp4s0 上。
|
||||
|
||||
Reference in New Issue
Block a user