arm_dev
d32e6ae53f
一、场景物料（新增）
devices/sim/gazebo/models/workpiece/ 带三个孔的铝板。碰撞体是"带方孔的平板"，
视觉额外加两个 φ14/φ7 亮环 —— 因为真机的 HoughCircles 就是靠这两个亮环找孔，
只按碰撞体建视觉会让"孔检测"在仿真里永远失败，而那看起来像算法问题。
devices/sim/gazebo/models/gauge/ 通止规：通端/套环/握柄/止端四段，各段异色
便于肉眼判断方向。接触刚度刻意比台面软 —— 位置控制器会"刚性压进去"，
用默认刚度会被弹飞（注释里写了原因，不是随手抄的参数）。
devices/sim/gazebo/models/gauge_stand/ 规座（static）。规直接站在台面上会立刻倒，
加个方孔底座把它立住。
三个模型都是 model.sdf + model.config，原点在**底面中心**，单位米。
二、场景配置与校验（新增）
config/scene.yaml 物体不再写死在 world 里，改为 scene.yaml + 启动时动态 spawn。
理由：world 是"环境"（地面/台面/相机/系统插件），场景是"这一批活做什么"。
动 world 的风险面太大 —— 它只要丢一个系统声明，所有模型都 spawn 不进去（见旧坑 2）。
含 operating（相机到工件的距离等作业条件）与 gates_px（像素门槛）。
sim_scene.py 摆放校验：从 SDF **逐碰撞体**解析 AABB（不抄尺寸，避免漂移），
检查原点在底面 / 不穿台面 / 不出界 / 物体之间不嵌入。
重叠判定必须逐碰撞体三维判，不能拿总包围盒相交 ——
正常装配里本来就有嵌套（规立在规座方孔里），粗判会误报，
而误报的后果是人把检查整个关掉，之后真重叠也抓不到。
另有成像几何：把亮环/孔间距按相机距离换算成像素，与门槛比对。
scripts/check_sim_scene.py 验收工具（退出码可用）：摆放 + 门槛漂移 + 成像几何 +
悬停高度 vs 工作空间 + 手外相机是否**完整**看得见工件 + Gazebo 实际位姿。
三、修掉两处静默失效（都是"看起来一切正常"）
1) parse_spawned_poses 一直是**死代码**：它按 "x: N / y: N / z: N" 解析，
而 Fortress 实际输出是方括号两行（XYZ / RPY）。对不上 → 永远返回空 →
调用方按设计"降级成跳过位姿核对" → 位姿检查**从来没跑过**，
而日志只说"跳过"，看起来像正常降级。
现在两种格式都支持，并用真实抓下来的输出样本做回归测试。
教训：宽容降级会把"没检查"伪装成"检查通过"。
2) stop_sim_gz.sh 的模式表里漏了 robot_state_publisher（而注释里写着它有覆盖）：
脚本报"已停止"，它却还在发上一份 /robot_description 与 TF，
下一次启动就会"改了描述却不生效"。
顺带修掉同源的两个问题：awk 用 -v 传模式导致它匹配到自己、每次复查都多报
一个空命令行的幽灵 PID；以及"杀的时候"和"复查的时候"两份扫描实现会各自漂移。
现在共用同一个 scan_pids()，模式改用环境变量传递，并在杀完之后**再扫一遍**：
有残留就报出来并非零退出 —— 下次再有漏项，不再静默。
实测：--list 列出全套 9 个进程；停止后 rc=0 且 pgrep 干净。
四、相机：D405 分辨率对齐真机
仿真的 D405 此前渲染 640x480，而真机跑 848x480（fx 337 而非 447）。后果是
58mm 的两孔间距成像只剩 85px，低于 hole_distance_min=90 ——
孔检测在仿真里根本不可能通过，而现象看起来像"算法不行"。
已把 xacro 与 cameras.yaml 一起改成 848x480，并把启动时的比对从"只比视场"
扩成"视场 + 图幅"（sim_cameras.check_camera_consistency），
不一致时说明它对像素门槛的影响。
五、文档
devices/sim/gazebo/README.md 重写为中文手册：使用方法 / 建模要求 / 导入格式 /
配置方式（含"改哪里才真的生效"的接线表）/ 11 条实测坑（每条都有"怎么发现"）。
manifest.yaml 的进度、assets、description、坑清单与本轮状态对齐。
scripts/README.md 补两个验收工具与停止脚本；根 README 补仿真入口与手册链接。
cameras.yaml 里一处过期的函数名（check_fov_consistency）修正。
六、测试
新增 test_sim_scene.py（79 条，此前 sim_scene.py 完全没有单测 —— 这正是
位姿解析死掉却无人发现的原因）与 test_check_sim_scene.py（21 条），
含三条回归：world_boxes 的偏移碰撞体不能被对称化放大（假重叠）、
center_bore 不能被 _bore 后缀吃掉、Fortress 位姿输出格式。
全量：src/arm_sim_gazebo + arm_interfaces + arm_application 共 515 通过。
同时附上 config/site.example.yaml 的重新生成（此前 site.yaml 的注释改动没同步，
它的 parity 测试是红的）。
Co-authored-by: Cursor <cursoragent@cursor.com>
505 lines
26 KiB
YAML
505 lines
26 KiB
YAML
# =============================================================================
|
||
# site.example.yaml — 新环境部署模板
|
||
#
|
||
# ！本文件由 scripts/gen_site_example.py 从 config/site.yaml 自动生成，请勿手改。
|
||
# 要改模板内容，请改 config/site.yaml（它是唯一权威）后重跑：
|
||
# python3 scripts/gen_site_example.py
|
||
# 或在改完 site.yaml 后跑一次以确认模板同步：
|
||
# python3 scripts/gen_site_example.py --check
|
||
#
|
||
# 用法：复制本文件为 config/site.yaml，然后把所有 <占位符> 换成现场实际值。
|
||
# 现场值包括：机械臂 SN / IP、相机序列号、夹爪串口、传感器 USB 路径。
|
||
#
|
||
# 说明：site.yaml 里的路径出现 /home/USER 是因为生成时匿名化了用户名；
|
||
# 部署时请改成实际用户的 home 目录。
|
||
# =============================================================================
|
||
|
||
# =============================================================================
|
||
# site.yaml — 本机唯一配置入口（不进 git；从 site.example.yaml 复制而来）
|
||
# 路径：~/ros2/config/site.yaml
|
||
#
|
||
# 89 与 91 的手眼 / 腕部相机 / 夹爪 / 点位互相独立，禁止混用。
|
||
# 切换机械臂：改 robot.sn，或所有终端加同一 --arm 89|91。
|
||
# =============================================================================
|
||
|
||
robot:
|
||
# >>> 切换机械臂：改成 robots 里某一台的完整 SN <<<
|
||
sn: "Rizon4-XXXXXX"
|
||
# Studio Tools 名（一字不差）。空=不切换。查：bash ~/ros2/scripts/rdk_switch_tool.sh --list
|
||
tool_name: ""
|
||
# 每台登记 SN + IP，以及该臂自己的相机/夹爪/手眼/点位（覆盖下面的全局段）。
|
||
robots:
|
||
- sn: "Rizon4-XXXXXX"
|
||
arm: left
|
||
ip: "192.168.2.XXX"
|
||
pc_ip: "192.168.2.XXX"
|
||
iface: "enp4s0"
|
||
label: "左臂89"
|
||
camera:
|
||
name: d405_left # 话题 /d405_left/... 与 91 分开
|
||
serial: "<序列号>" # 左腕 D405（一直在用的那台）
|
||
auto_exposure: true
|
||
ae_limit_us: 4000
|
||
emitter_enabled: false
|
||
gripper:
|
||
model: "PGEA-100-20-W"
|
||
sn: "<序列号>"
|
||
port: "/dev/ttyUSB0"
|
||
stroke_mm: 20.0
|
||
named_poses_file: "/home/USER/ros2/src/arm_control/config/named_poses_89.yaml"
|
||
paths:
|
||
handeye_file: "/home/USER/ros2/handeye/results/89_eye_in_hand.yaml"
|
||
handeye_waypoints: "/home/USER/ros2/handeye/poses/89_eye_in_hand.yaml"
|
||
handeye_workdir: "/home/USER/ros2/handeye/tmp/89"
|
||
# 左臂跟踪高度（只在 --arm 89 时生效，改 91 的 servo 不影响这里）
|
||
servo:
|
||
ee_z_m: 0.35 # 跟踪锁高：法兰相对基座 Z [m]
|
||
min_ee_z_m: 0.20
|
||
safe_z_m: 0.45
|
||
z_min_m: 0.22
|
||
xy_track: hover # 法兰(夹爪中心) XY 压在工件上，不是画面中心
|
||
ee_vertical_down: true # 法兰 +Z 朝世界 −Z
|
||
hover_offset_ee_xy_m: [0.0, 0.0] # 爪心相对法兰 XY；平行爪同轴保持 0
|
||
# >>> 二阶段孔定位悬停锁高：只改这里，91 用下面那段 <<<
|
||
# 法兰 Z [m]；要兼顾：够低拍清 k1/k2 螺纹孔，够高通止规尖不碰工件。
|
||
# 调试：bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only
|
||
hole_hover_z_m: 0.23 # ← 二阶段悬停高度调这一行
|
||
- sn: "Rizon4-XXXXXX"
|
||
arm: right
|
||
ip: "192.168.2.XXX"
|
||
pc_ip: "192.168.2.XXX"
|
||
iface: "enp4s0"
|
||
label: "右臂91"
|
||
camera:
|
||
name: d405_right # 话题 /d405_right/... 禁止和 89 的 /d405_left 混用
|
||
serial: "<序列号>" # 右腕新 D405
|
||
auto_exposure: true
|
||
ae_limit_us: 0 # 0=不封顶，让固件自己调亮
|
||
emitter_enabled: false
|
||
laser_power: 0
|
||
gripper:
|
||
model: "RGI-100-30-O-S-M1" # 旋转爪：run_gripper.sh --arm 91
|
||
sn: ""
|
||
port: "/dev/ttyUSB1"
|
||
stroke_mm: 30.0
|
||
auto_init: false
|
||
named_poses_file: "/home/USER/ros2/src/arm_control/config/named_poses_91.yaml"
|
||
paths:
|
||
handeye_file: "/home/USER/ros2/handeye/results/91_eye_in_hand.yaml"
|
||
handeye_waypoints: "/home/USER/ros2/handeye/poses/91_eye_in_hand.yaml"
|
||
handeye_workdir: "/home/USER/ros2/handeye/tmp/91"
|
||
# >>> 右臂跟踪高度：只改这里，89 仍用上面那段 <<<
|
||
# 法兰 Z；RGI 比平行爪高，通常要比 89 略抬一点。改完重启：
|
||
# bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 91 --control
|
||
servo:
|
||
ee_z_m: 0.25 # 跟踪锁高：法兰相对基座 Z [m] ← 日常调这一行
|
||
min_ee_z_m: 0.20
|
||
safe_z_m: 0.45
|
||
z_min_m: 0.22
|
||
xy_track: hover # 用法兰系手眼 T：夹爪中心(法兰原点)压在工件正上方
|
||
ee_vertical_down: true # 法兰/RGI 轴线朝下；相机有 29° 倾角，绿框不必对准十字
|
||
hover_offset_ee_xy_m: [0.0, 0.0] # RGI 与法兰同轴；爪心偏了再改这两项 [m]
|
||
# 二阶段孔定位悬停锁高（91 装了传感器后同样可用）
|
||
hole_hover_z_m: 0.22
|
||
# --control：先只降到 ee_z_m（XY 不动），到位后才 hover，用手眼把爪心压到工件正上方
|
||
|
||
paths:
|
||
# >>> YOLO 权重（标准放法，不必改代码）<<<
|
||
# 1) 把 .onnx / .pt 放到 /home/USER/ros2/models/yolo/
|
||
# 2) 这一行改成那个绝对路径（写 .onnx 即可；TensorRT 会找同名 .engine）
|
||
# 3) backend=tensorrt 时必须先导出引擎（只需做一次）：
|
||
# bash ~/ros2/scripts/export_yolo_trt.sh /home/USER/ros2/models/yolo/detect_best_0.onnx 640
|
||
# 4) yolo.imgsz 必须等于 ONNX 输入边长（当前 640）
|
||
# 5) 改权重/imgsz 后重启：bash ~/ros2/scripts/run_visual_servo_rt.sh [--control]
|
||
# 6) 换完确认标签没丢：bash ~/ros2/scripts/weights.sh doctor --check-engines
|
||
# 当前：detect_best_0.onnx（2026-09-04 从 ~/下载 移入，640 输入）
|
||
yolo_weights: "/home/USER/ros2/models/yolo/detect_best_0.onnx"
|
||
# 换模型：把 models/yolo/ 下别的 .onnx / .pt 绝对路径填进来即可。
|
||
# 未在用的模型已移出仓库（体积大且干扰 `weights.sh list`），要恢复见 models/README.md。
|
||
# 入孔 YOLO（backend=yolo 时把孔当框）。two_stage 用下面的工件权重。
|
||
# 原先指向 hole_insert/best.pt —— 那个文件不存在（悬空引用）。这里改成实际存在的
|
||
# detect_best_1.onnx，这样 backend: yolo 至少是能跑的。查悬空引用：
|
||
# bash ~/ros2/scripts/weights.sh doctor
|
||
hole_weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx"
|
||
# 下面是缺省（未匹配到 robots[] 时）。真机以 robots[].paths 为准：89 / 91 各一份。
|
||
handeye_file: "/home/USER/ros2/handeye/results/89_eye_in_hand.yaml"
|
||
handeye_waypoints: "/home/USER/ros2/handeye/poses/89_eye_in_hand.yaml"
|
||
handeye_calibrate: "/home/USER/ros2/handeye/calibrate.yaml"
|
||
handeye_workdir: "/home/USER/ros2/handeye/tmp/89"
|
||
# 空=默认 DDS(SHM)；勿强制 UDP-only，否则相机预览收不到 /workpiece/detection
|
||
fastdds_profile: ""
|
||
|
||
handeye:
|
||
mode: "eye_in_hand"
|
||
# >>> 标定板尺寸（内角点数 + 方格边长 m）；改这里即可 <<<
|
||
# 例：A5 板 12×17 方格 → 内角点 11×16，方格 11 mm
|
||
board_cols: 8 # 内角点列数 (方格列数 - 1)
|
||
board_rows: 11 # 内角点行数 (方格行数 - 1)
|
||
square_size_m: 0.015 # 方格边长 [m]
|
||
# 运动更顺滑：慢速 + 到位等静止 + 零速 pin 后再拍照
|
||
velocity_scale: 0.05
|
||
move_time_sec: 8.0
|
||
settle_sec: 1.5
|
||
settle_qd_max: 0.015 # |qd|_max 低于此值才算静
|
||
settle_stable_sec: 0.6
|
||
settle_timeout_sec: 8.0
|
||
pin_holds: 2
|
||
pin_hold_sec: 0.45
|
||
skip_unstable_poses: true # 某点看不到棋盘则跳过，不整段中止
|
||
# 点位：paths.handeye_waypoints
|
||
|
||
camera:
|
||
name: "d405"
|
||
serial: "<序列号>" # 全局缺省=左腕；真机以 robots[].camera.serial 为准
|
||
# USB3 推荐 848x480@30；USB2 自动降到 640x480@15，深度/右目仍开
|
||
width: 848
|
||
height: 480 # 改宽高/帧率必须重启相机：bash ~/ros2/scripts/run_camera_d405.sh --preview
|
||
fps: 30
|
||
enable_depth: true
|
||
enable_infra: true # 同一 SDK 进程开左右红外；失败则自动退回彩图+深度
|
||
preview: true
|
||
preview_stereo: true # 预览同时显示 彩图 | 深度 | 左目 | 右目（不另开相机、不抢 USB）
|
||
overlay_hold_sec: 0.10 # 预览只留很短；长残留会看起来像绿框不跟
|
||
use_site_resolution: true
|
||
auto_exposure: true
|
||
ae_limit_us: 4000 # D405 腕部近距；91 在 robots[] 里更严
|
||
emitter_enabled: false # 腕部关红外投射，否则彩图发白
|
||
laser_power: 0
|
||
|
||
# 现场三台 RealSense（按序列号钉死，禁止按「第一台 D405」乱开）
|
||
cameras:
|
||
d405_left:
|
||
model: d405
|
||
name: d405_left
|
||
serial: "<序列号>"
|
||
role: left_wrist
|
||
arm: left
|
||
label: 左腕D405（89 平行夹爪）
|
||
width: 848 # 相机分辨率以本段为准（全局 camera: 段为旧路径，P2 收敛）
|
||
height: 480
|
||
emitter_enabled: false # 腕部必须关投射器，红外点阵会污染跟踪图像
|
||
d405_right:
|
||
model: d405
|
||
name: d405_right
|
||
serial: "<序列号>"
|
||
role: right_wrist
|
||
arm: right
|
||
label: 右腕D405（91 旋转夹爪）
|
||
width: 848
|
||
height: 480
|
||
emitter_enabled: false
|
||
d435:
|
||
model: d435
|
||
serial: "<序列号>"
|
||
role: overhead_eye_to_hand
|
||
label: 双臂中间手外D435
|
||
name: d435
|
||
width: 1280
|
||
height: 720
|
||
fps: 30
|
||
auto_exposure: true
|
||
ae_limit_us: 8000
|
||
emitter_enabled: true # 顶视深度可用投射器；只看彩图可改 false
|
||
|
||
|
||
yolo:
|
||
conf: 0.10 # 检测器门槛：略低于 min_conf，让运动模糊框也能进跟踪
|
||
iou: 0.45
|
||
imgsz: 640 # 必须等于所用 ONNX 的输入边长（当前 detect_best_0 = 640）
|
||
device: "0"
|
||
half_precision: true
|
||
backend: "tensorrt" # 强制 .engine；没有则启动失败。导出：bash ~/ros2/scripts/export_yolo_trt.sh
|
||
mp_infer: true # YOLO 独立进程，不和 ROS 抢 GIL
|
||
min_conf: 0.12 # 新锁门槛；已锁定的框可略低于此
|
||
vision_hz: 30.0
|
||
infer_max_width: 640 # 进 GPU 进程前缩到 640，减小进程间拷图，避免队列堵
|
||
box_ema_alpha: 1.00 # 绿框用预测补偿后的框，不再 EMA
|
||
miss_clear_frames: 10 # ~0.33 s @30 Hz；期间绿框按像素速度滑行
|
||
overlay_hold_on_miss: true # 滑行失败时仍续发上一框
|
||
hold_point_on_miss: false # 眼在手上：丢检续发旧点会让臂追错位置
|
||
miss_hold_point_frames: 1
|
||
jump_px: 400.0
|
||
jump_z_m: 0.10
|
||
z_ref_m: 0.22
|
||
min_depth_m: 0.04
|
||
max_depth_m: 0.55
|
||
work_z_max_m: 0.50
|
||
meas_blend: 0.85 # 3D 跟手；抖则略降
|
||
z_alpha_min: 0.10
|
||
z_alpha_max: 0.40
|
||
z_alpha_gain: 1.0
|
||
camera_frame: "d405_color_optical_frame"
|
||
object_frame: "tracked_object"
|
||
depth_topic: "/d405/aligned_depth_to_color/image_raw"
|
||
feature_mode: "plane" # plane=框内近表面立体点（工件本体）；box=单目射线
|
||
plane_inlier_m: 0.008 # 拟合平面内点阈值 [m]
|
||
plane_band_m: 0.06 # 近表面深度带宽 [m]
|
||
|
||
servo:
|
||
control_hz: 100.0
|
||
# --- 基座坐标系下的末端约束（法兰，单位 m）---
|
||
ee_x_m: 0.25
|
||
ee_y_m: 0.00
|
||
ee_z_m: 0.35 # 全局缺省锁高；真机以 robots[].servo.ee_z_m 为准（89/91 分开调）
|
||
ee_hold_xy: false # false=跟踪工件 XY；true=锁死 ee_x/ee_y
|
||
xy_track: "hover" # hover=法兰悬停在目标正上方（任意锁高） lookat=画面中心对准目标
|
||
use_static_handeye_tf: false # 只用 handeye_result.yaml，避免 TF 树抢父子帧
|
||
ee_vertical_down: true
|
||
# --- 启动回初始位（已停用：--control 不再调用 goto_ee_plane）---
|
||
safe_z_m: 0.45 # >>> 防撞安全高度：先纯 Z 抬到这里再回 home，避免扫到料框/壁
|
||
home_pose_name: "home" # 非夕初始点位（/set_named_pose：home | ready | inspect | pos_left）
|
||
home_pose_speed: 0.30 # 回初始位的速度比例 (0,1]，越小越稳
|
||
ee_hold_kp: 2.0
|
||
ee_w_max: 0.60
|
||
z_ref_m: 0.22
|
||
z_min_m: 0.22
|
||
approach_xy_gate_m: 0.040 # --control 先降到锁高，|ee_z-ee_z_m| 小于此值才开始 XY hover
|
||
v_approach_z: 0.025
|
||
watchdog_timeout_sec: 1.2
|
||
lost_timeout_sec: 1.2
|
||
stale_hold_sec: 0.35 # 丢检空窗内用基座 KF 预测继续跟（自运动安全），超过才钉住
|
||
max_extrapolate_sec: 0.35 # 预测时间上限（图像延迟 + 前瞻）
|
||
predict_enable: true # 基座系恒速预测，补视觉延迟
|
||
predict_sec: 0.16 # 额外前瞻 [s]
|
||
predict_v_max: 0.80 # 工件速度上限 [m/s]，防乱飞
|
||
predict_jump_m: 0.08 # 基座 XY 跳变大于此则重锁速度
|
||
kp_xy: 4.20 # 很跟手：v = kp × 误差，再经加速度整形；抖则降回 3.5
|
||
ki_xy: 0.00
|
||
kd_xy: 0.20
|
||
kp_z: 0.80
|
||
ki_z: 0.00
|
||
kd_z: 0.35
|
||
i_sep_pos_m: 0.008
|
||
deadzone_m: 0.003 # 3 mm；收紧提高对准精度。若抖动调回 0.006
|
||
deadzone_z_m: 0.012
|
||
lock_hold_sec: 0.7 # 死区驻留多久才发 /visual_servo/locked（串联 0.6s 已够）
|
||
e_xy_clip_m: 0.200 # 大误差仍按 v_max 走，靠 a_max 加速
|
||
v_max_xy: 0.45 # 法兰 XY 上限 450 mm/s
|
||
v_max_z: 0.04
|
||
min_ee_z_m: 0.20
|
||
workspace_xyz_min: [0.08, -0.55, 0.18] # 法兰工作盒；盒外只允许往回走，不再卡死 no traj
|
||
workspace_xyz_max: [0.85, 0.55, 0.65]
|
||
a_max: 3.00 # 加速度上限（真正作用在 hover 指令上）
|
||
vel_lpf_alpha: 0.80 # 速度低通：越大越跟手（1=不滤波）
|
||
dq_window_size: 1 # 1=不做滑窗平均，去掉 ~20ms 滞后
|
||
traj_stream_hz: 50.0 # 轨迹点流率：JTC open_loop+样条，吃得起
|
||
exec_horizon_sec: 0.08 # 每个轨迹点的执行窗口
|
||
max_qdot: 2.00 # 关节速度上限 [rad/s]
|
||
min_cmd_delta_rad: 0.001
|
||
point_topic: "visual_servo/filtered_point"
|
||
traj_topic: "rizon_arm_controller/joint_trajectory"
|
||
|
||
# 大寰夹爪：官方 Modbus RTU，USB–RS485 插 IPC，不走 Studio/RDK。
|
||
# 左 89 PGEA → /dev/ttyUSB0；右 91 RGI → /dev/ttyUSB1。
|
||
# bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli open|close|pos
|
||
# bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli open|close|rotate 90
|
||
# bash ~/ros2/scripts/run_dual_gripper.sh
|
||
# ROS：/gripper/open|close（PGEA 单爪）或 /dual_gripper_driver/{left,right}/execute
|
||
gripper:
|
||
backend: "usb" # usb=工控机官方Modbus rdk=Studio设备（不用）
|
||
vendor: "dh"
|
||
model: "PGEA-100-20-W"
|
||
sn: "<序列号>" # 铭牌，不进协议
|
||
device_name: "GripperDahuanModbus"
|
||
port: "/dev/ttyUSB0" # 本机 MACROSILICON/pl2303
|
||
baud: 115200 # 官方默认 115200 8N1
|
||
slave_id: 1 # 官方默认从站 1
|
||
rs485_rts: "" # 空=不拉RTS；scan 若 OK 在 tx/tx_inv 再填
|
||
stroke_mm: 20.0
|
||
# --- 开合 / 力 / 速（大寰官方寄存器，超出会被驱动钳位）---
|
||
force_pct: 20 # 夹持力 区间 20–100 %（低于 20 会被钳到 20；PGEA-100 约 30–100 N）
|
||
speed_pct: 50 # 开合速度 区间 1–100 %
|
||
open_position: 1000 # 张开目标 区间 0–1000 ‰（1000=全开）
|
||
close_position: 0 # 闭合目标 区间 0–1000 ‰（0=全合；留缝改 50–200）
|
||
auto_init: true
|
||
state_hz: 20.0
|
||
# 双爪共用参数（run_dual_gripper.sh / load_from_site）
|
||
parallel:
|
||
stroke_min: 0.0
|
||
stroke_max: 20.0
|
||
default_speed: 50
|
||
default_force: 20
|
||
force_min: 20
|
||
force_max: 100
|
||
auto_initialize: true
|
||
rotary:
|
||
auto_initialize: false
|
||
angle_min: -32767.0
|
||
angle_max: 32767.0
|
||
max_step_deg: 30000.0
|
||
finger_open_per_mille: 1000
|
||
finger_close_per_mille: 0
|
||
default_speed: 50
|
||
default_torque: 50
|
||
default_finger_speed: 50
|
||
default_finger_force: 50
|
||
torque_min: 20
|
||
torque_max: 100
|
||
# RDK 回退字段（backend: rdk 才用）
|
||
force_n: 50.0
|
||
vel_mps: 0.05
|
||
|
||
# 入孔检测。算法参数写在这里（覆盖包内 hole_detect.yaml 默认值）。
|
||
# bash ~/ros2/scripts/run_hole_detect.sh --arm 89 --preview
|
||
hole_detect:
|
||
backend: "two_stage" # two_stage | yolo | hough
|
||
# backend=yolo 时用。原为 hole_insert/best.pt（文件不存在），已改为实际存在的模型。
|
||
weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx"
|
||
# 二阶段工件检测权重（YOLO 先框出工件，再 Hough 找 k1/k2 螺纹孔）
|
||
workpiece_weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx"
|
||
config: "/home/USER/ros2/src/arm_application/config/hole_detect.yaml"
|
||
conf: 0.25
|
||
iou: 0.7
|
||
imgsz: 640
|
||
center_gate_enable: true
|
||
center_max_offset_ratio_x: 0.15
|
||
center_max_offset_ratio_y: 0.18
|
||
center_gate_draw: true
|
||
refine_hole_centers: true
|
||
bbox_padding: 80
|
||
gaussian_ksize: 5
|
||
gaussian_sigma: 1.5
|
||
hough_dp: 1.2
|
||
hough_min_dist: 50
|
||
hough_param1: 100
|
||
hough_param2: 26
|
||
hough_min_radius: 6 # 悬停较高时螺纹孔半径只有几像素
|
||
hough_max_radius: 30
|
||
min_radius: 6
|
||
max_radius: 30
|
||
hole_distance_min: 90 # 悬停 0.23m 两螺纹孔约 170px；过小会配到中心孔
|
||
hole_distance_max: 700
|
||
expected_holes: 2
|
||
min_hole_sep_px: 20
|
||
max_hole_sep_px: 2000
|
||
stabilize_hold_frames: 15
|
||
stabilize_ema_alpha: 0.35
|
||
stabilize_max_jump: 60
|
||
default_depth_m: 0.10
|
||
camera_intrinsics:
|
||
fx: 425.0
|
||
fy: 425.0
|
||
cx: 640.0
|
||
cy: 360.0
|
||
# ===== 二阶段一次性拍照定位（stage2_locate_node，不做实时推理）=====
|
||
# bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 # 完整流程
|
||
# bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only # 只悬停调高度
|
||
stage2:
|
||
target_hole: "k1" # 对准目标孔：k1=图像左孔 / k2=右孔（见 k1_label）
|
||
k1_label: "left" # k1 是画面里的左孔；现场相反就改 right
|
||
align_tol_m: 0.0007 # 对准到位判据 [m]
|
||
align_max_iters: 8 # 拍照→修正→复拍 迭代上限
|
||
coarse_max_moves: 2 # 工件粗居中（bbox 中心）移动上限
|
||
# —— 柔顺运动（夹爪持规，防急加速/急停/过冲把规震脱）：速度放慢 + 平滑刹车 ——
|
||
align_speed: 0.014 # 对准段笛卡尔速度 [m/s]（原0.03，放缓防冲）
|
||
move_speed: 0.022 # 升降/粗居中段速度 [m/s]（原0.05，放缓防震规）
|
||
final_gap_mm: 2.0 # 对准后轻降目标：规尖悬在孔面上方这么高 [mm]（不接触；安全下限1.2mm自动停）
|
||
final_descent_speed: 0.005 # 最后轻降速度 [m/s]（很慢，柔顺）
|
||
final_descent_enable: true # 对准后是否轻降到 final_gap_mm（false=停在悬停高）
|
||
verticalize_tol_deg: 0.8 # 垂直朝下判据 [deg]
|
||
settle_sec: 0.45 # 到位后静止多久再拍照
|
||
snap_frames: 5 # 每次精拍取中值的帧数
|
||
min_depth_m: 0.06 # 有效深度范围（防孔底/桌面拉偏 XY）
|
||
max_depth_m: 0.60
|
||
linger_sec: 8.0 # 完成后继续发布结果/叠图的时长
|
||
|
||
# ===== 三阶段柔顺搜孔（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，看日志「触觉基线/派生阈值」两行）——
|
||
touch_delta: 250.0 # 触面判定：fnormal > μ0 + max(4σ0, 本值)
|
||
press_delta: 200.0 # 轻触目标按压量（触面阈值之上再加这么多）
|
||
max_press_delta: 900.0 # 螺旋中允许的最大按压（超过→力闭环抬起）
|
||
safe_press_delta: 2000.0 # 安全上限：fnormal > μ0+本值 立即抬升中止
|
||
shear_delta: 600.0 # 切向拖拽保护：shear > 静止切向均值+本值 → 贴面时微抬 0.15mm（Z 钉在触面）
|
||
insert_press_delta: 1200.0 # 插入力上限（触面阈值之上；超过回退微抖重试）
|
||
kp_z_mm_per_unit: 0.001 # Z 力闭环增益：每单位读数偏差调整的毫米数
|
||
approach_step_mm: 0.8 # 触面前每步下降量（驱动死区已修，0.5mm 微步可跟踪；小步=触面冲击小）
|
||
enter_probe_mm: 2.0 # 触面深于此值→疑似直接入孔，四向微探验证后跳过螺旋
|
||
mouth_lift_mm: 2.0 # 直接入孔后抬到孔口倒角段再定心（深插横探会别死孔壁）
|
||
baseline_min: 8.0 # 触觉基线健康窗下限（<此值=几乎全 0，tare/断流）
|
||
baseline_max: 1500.0 # 上限（>此值=近饱和，接触余量不足）——出窗即中止并提示重启触觉
|
||
approach_ft_touch_n: 6.0 # approach 腕部 |F| 旁路触面判据（标定牛顿；自由空气≈2-3N）
|
||
approach_max_mm: 60.0 # 从当前位姿最多下降这么多（找不到面就中止；悬停被抬高后仍有余量）
|
||
spiral_pitch_mm: 1.0 # 螺旋螺距（< 通止规直径，保证重叠覆盖）
|
||
spiral_step_mm: 0.5 # 沿螺旋的弧长步长
|
||
spiral_r_max_mm: 4.0 # 最大搜索半径（覆盖二阶段残差 1.5mm + 余量）
|
||
spiral_expand_max: 1 # 找不到孔时扩大半径的次数（残差≤1.8mm 时代码不再扩大）
|
||
spiral_expand_step_mm: 2.0
|
||
drop_ratio: 0.45 # 入孔判据：fnormal 相对接触均值跌落比例
|
||
drop_frames: 2 # 跌落需持续的帧数（防毛刺）
|
||
center_scan: false # 入孔后十字微扫；默认关（0.8mm 扫会把已入孔的规刮偏）
|
||
center_scan_mm: 0.3
|
||
insert_depth_mm: 0.0 # 默认只搜孔+轻压，不拧入；拧入阶段再用 --depth
|
||
insert_retry: 3
|
||
screw_pitch_mm: 0.0 # 螺纹通止规螺距：>0 右旋 / <0 左旋 / 0=不旋（光面规）
|
||
search_speed: 0.03 # 搜孔笛卡尔速度 [m/s]
|
||
settle_sec: 0.25 # 每个微步后的静止时间
|
||
tactile_timeout_s: 3.0 # 触觉话题断流多久中止
|
||
ft_watchdog: true # 订阅 /<sn>/flexiv_robot_states 做腕部力看门狗
|
||
ft_safe_n: 35.0 # 腕部 |F| 安全上限（真牛顿，标定过的；连续3拍超限才中止）
|
||
linger_sec: 8.0 # 完成后继续发布结果的时长
|
||
|
||
# Orisys 视触觉（夹爪左右指摄像机，不是腕部 D405）。SDK 单独 pip 安装。
|
||
# bash ~/ros2/scripts/run_tactile.sh --cli
|
||
# bash ~/ros2/scripts/run_tactile.sh
|
||
tactile:
|
||
backend: "orisys" # orisys | mock
|
||
# 传感器型号。必须与 devices/sensors/orisys/models/ 下的子目录名一致。
|
||
model: "dd03"
|
||
# 型号配置随 SDK 一起放：devices/sensors/orisys/models/<型号>/config.json
|
||
config_file: "/home/USER/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:X.Y.Z:1.0-video-index0"
|
||
right_video: "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:X.Y.Z:1.0-video-index0"
|
||
cuda: false # 两路进程抢 GPU 上下文会互相拖慢；CPU 解算仅 ~9ms/帧，足够
|
||
isstitch: true
|
||
infer_backend: "cpu" # cupy | opencl | cpu；双路并行用 cpu 最稳
|
||
baseline_seconds: 2.0
|
||
filter_alpha: 0.25
|
||
motion_threshold: 0.0
|
||
target_fps: 30.0
|
||
publish_hz: 20.0
|
||
# 视触觉图像与力矢量场的发布频率。0 = 关闭（默认）。
|
||
# 三力走 30fps 的轻量队列；图像每帧几百 KB，单独走一条 maxsize=1 的队列，
|
||
# 打开它不会影响三力的时序。需要看形变图/矢量场时设成 5~10 即可。
|
||
image_hz: 0.0
|
||
rotation_config: "" # 左右共用；有分指文件则用下面两项
|
||
left_rotation: "" # 例：orisys SDK 的 rotation_config_0.json
|
||
right_rotation: ""
|
||
|
||
go_home:
|
||
pose: "home"
|
||
speed: 0.25
|
||
|
||
network:
|
||
# User1=enp4s0 本机 <PC_IP> ↔ <左臂SN>=.101 / <右臂SN>=.100
|
||
prefer_iface: "enp4s0"
|
||
|
||
# 仿真平台（当前 Gazebo 尚未接线，详见 devices/sim/gazebo/README.md）
|
||
# 设计约束：仿真与真机共用同一套参数（servo.* / hole_detect.stage2.*），
|
||
# 只有物理引擎自身的参数放在这里，避免「仿真调好的值搬到真机不生效」。
|
||
sim:
|
||
# 这一段由 arm_sim_gazebo.sim_site 真正读取（此前是"声明了但没人读"，
|
||
# 改了毫无反应也不报错）。三个键的接线状况**不一样**，务必看清：
|
||
#
|
||
# world ✔ 已接线。优先级：命令行 --world > 这里 > 内置默认。
|
||
# 相对路径按**仓库根**解析。
|
||
# physics_enable ✘ 未接线。世界文件里的重力/步长是写死的。若这里写的值
|
||
# 与世界的实际状态不符，启动时会打印告警（不再静默）。
|
||
# joint_prefix ✘ 未接线。前缀由 urdf/arm.sim.urdf.xacro 的 arm_prefix
|
||
# 决定，且 config/controllers.yaml 里硬编码了 sim_joint1..7。
|
||
# 写了个不同的值会告警。要真改，三处必须一起改。
|
||
#
|
||
# 另外注意：`run_sim.sh` 走的仍是 mock 驱动（假数据 + RViz），不是物理仿真；
|
||
# 物理仿真入口是 `scripts/run_sim_gz.sh`（Gazebo）。两者互不影响。
|
||
platform: "gazebo"
|
||
world: "devices/sim/gazebo/worlds/assembly.world"
|
||
physics_enable: true
|
||
joint_prefix: "sim_" # 仿真关节前缀，必须与真实臂前缀区分
|