diff --git a/config/site.example.yaml b/config/site.example.yaml index f9287cd..3e228c0 100644 --- a/config/site.example.yaml +++ b/config/site.example.yaml @@ -55,7 +55,9 @@ robot: 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: + # 锚点 `&servo_left`:下面的"仿真台"条目直接引用它 —— 仿真建的就是左臂, + # 悬停/锁高必须与 89 **一模一样**,抄两份迟早漂移。 + servo: &servo_left ee_z_m: 0.35 # 跟踪锁高:法兰相对基座 Z [m] min_ee_z_m: 0.20 safe_z_m: 0.45 @@ -67,6 +69,67 @@ robot: # 法兰 Z [m];要兼顾:够低拍清 k1/k2 螺纹孔,够高通止规尖不碰工件。 # 调试:bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only hole_hover_z_m: 0.23 # ← 二阶段悬停高度调这一行 + # >>> 仿真那台臂(Gazebo)也登记成 robots[] 里的一台 <<< + # 为什么必须登记:应用是按"当前臂"的名字去查 frame / joint / 相机话题的 + #(`sim_env.sh` 注释里说的"仿真与真机共用同一套 site.yaml")。仿真的 TF 与关节 + # 名带 `sim_` 前缀(与真机区分,避免仿真数据冒充真机),所以它需要自己的一台。 + # 登记之后,**所有阶段(stage2/stage3/装配…)都不改一行代码**就能跑仿真, + # 只要 `--arm sim`(`scripts/run_gauge_assembly.sh --sim` 会自动选它)。 + # + # 真机上永远不会选到它:只有显式 `--arm sim` 才会,而没有任何真机脚本会这么写。 + - sn: "sim" + arm: sim + label: "仿真台" + # 仿真建模的就是左臂,触觉也只有左指(由 sim_tactile_adapter 发) + tactile: true + camera: + # 话题 /sim_d405/... :与真机的 /d405_left 分开。为什么不用同一个名字: + # 同域里两个来源会互相覆盖,"台上这台到底是谁"就说不清了。 + # 名字里的 d405 与 URDF 里的模型名一致 → 光学系 sim_d405_color_optical_frame + # 正好等于 `{camera.name}_color_optical_frame`,所以应用查 TF 不用特判。 + name: sim_d405 + # 仿真不需要手眼标定文件:腕部相机在 URDF 里就装在法兰上, + # 而真机那份标定文件描述的是"相机相对法兰的实际安装偏差", + # 套到仿真上会把相机搬到一个**错误**的位置(投影全错)。 + # 留空 → 启动脚本不发静态 TF(见 gauge_assembly.launch.py 的提示)。 + paths: + # 仿真不"标定",而是把 URDF 里的真值**导出**成同格式的文件: + # scripts/gen_sim_handeye.py(run_gauge_assembly.sh --sim 会自动跑)。 + # 为什么必须给文件而不能留空:visual_servo 的 servo_node 需要 + # T_flange_cam 矩阵做像素→笛卡尔换算;留空它就直接拒绝启动 + #("手眼标定结果缺失"),S04 跟踪起不来。 + handeye_file: "/home/USER/ros2/handeye/results/sim_eye_in_hand.yaml" + # 运动参数:**大部分**与 89 共用(锚点),但**高度类**必须按仿真台的几何改写。 + # + # 为什么不能直接共用:真机是"机械臂装在台面同一高度",所以"法兰相对基座" + # 就等于"法兰相对台面"(hole_hover_z_m=0.23 就是这么来的)。 + # 仿真里机械臂站在**地面**、台面 0.40m 高、工件上表面 0.412m —— 一切 Z + # 都比真机大 0.412。照抄真实值会让伺服把法兰送到 0.28m,也就是**撞进台面**。 + # + # 推导(与 scene.yaml 的 operating 段同源,scripts/check_sim_scene.py 会核对): + # 相机到工件 0.17(像素门槛反推,不能改) + # + 相机比法兰低 0.05(URDF 里 d405_z=0.05) + # + 工件上表面 0.412 + # ⇒ 二阶段悬停法兰高 = 0.632 + # 一阶段跟踪高 = 悬停 + 0.05(通止规离工件)= 0.682 + # 所以工作盒上界必须 ≥ 0.682 —— 真机那套上界 0.65 不够,S04 一跟踪就会被 + # "超出工作盒"截断(现象是"越跟踪越偏",很难想到是配置差 3cm)。 + servo: + <<: *servo_left + ee_z_m: 0.68 # 一阶段跟踪锁高(法兰,世界系) + hole_hover_z_m: 0.632 # 二阶段悬停高:= 0.412 + 0.17 + 0.05 + safe_z_m: 0.75 # 安全抬升/复位高度(高于跟踪高) + z_ref_m: 0.632 # 没有测量时的兜底 Z:绝不能是 0.22(会撞工件) + z_min_m: 0.55 # 伺服硬下限:0.55 = 工件上表面 + 0.14 + min_ee_z_m: 0.55 + workspace_xyz_min: [0.08, -0.55, 0.55] + workspace_xyz_max: [0.85, 0.55, 0.75] + # 仿真不跑推理:感知结果由 sim_truth_perception 按已知位姿直接给出。 + # 原因:ros_gz_sim 环境里没有 torch/ultralytics,且本机离线装不上 —— + # perception_node 一启动就死,表现为"一阶段永远不锁定"。 + # 详见 devices/sim/gazebo/README.md「仿真里哪些模块用真值替代」。 + perception: + source: "truth" - sn: "Rizon4-XXXXXX" arm: right ip: "192.168.2.XXX" @@ -191,6 +254,33 @@ cameras: width: 848 height: 480 emitter_enabled: false + # >>> 仿真那两台(arm: sim)<<< + # 为什么仿真也要在这里登记:应用是"按当前臂"去认相机的 —— 腕部相机取 + # robots[].camera.name,手外相机取这里 role=overhead* 且 arm 与当前臂一致的 + # 那一条(见 gauge_assembly_node._d435_ns)。名字必须与 + # devices/sim/gazebo/config/cameras.yaml 的 ros_ns 一模一样。 + # 名字不带 d405_left/d435 那样的真机名:同域里两个来源会互相覆盖, + # 而 eye-to-hand 标定文件是按相机名命名的 —— 重名会让"跑一次仿真把真机标定盖掉"。 + sim_d405: + model: d405 + name: sim_d405 + role: sim_wrist + arm: sim + label: 仿真腕部D405(= 左臂位置) + width: 848 + height: 480 + emitter_enabled: false + sim_d435: + model: d435 + name: sim_d435 + role: overhead_eye_to_hand + arm: sim + label: 仿真手外D435(世界固定) + width: 640 + height: 480 + fps: 30 + emitter_enabled: true + d435: model: d435 serial: "<序列号>" @@ -451,6 +541,34 @@ hole_detect: # bash ~/ros2/scripts/run_tactile.sh --cli # bash ~/ros2/scripts/run_tactile.sh # ============================================================================= +# 感知来源:伺服用的"视觉测量"由谁产生 +# +# yolo 本体 perception_node(YOLO/ONNX,需要 torch/ultralytics)—— 真机 +# truth 仿真真值:**整条视觉链都由场景几何算出**,不跑任何推理 +# · 一阶段目标点:arm_sim_gazebo 的 sim_truth_perception +# · 二阶段孔位 :arm_sim_gazebo 的 truth_holes(投影 *_ring) +# external 由外部节点按**同一话题**(visual_servo/filtered_point)提供, +# 本框架不起感知进程(二阶段仍走检测器) +# +# ⚠️ 别和 yolo.backend 混了:那个是"**怎么**推理"(tensorrt/onnx), +# 这个是"**谁**给出测量"。 +# +# 为什么需要这个开关:仿真环境(ros_gz_sim conda)没有 torch/ultralytics, +# 离线也装不上,perception_node 一起就死;但仿真里工件位姿本来就是已知的, +# 没必要也没法做推理。切到 truth 后,流程能在仿真里验"控制与接口"那一半, +# 而**感知那一半留给真机**——这条边界写在 devices/sim/gazebo/README.md 的 +# 「仿真里哪些模块用真值替代」一节。 +# ============================================================================= +perception: + source: "yolo" + truth: + # 真值目标物体。留空 = 场景文件里的 operating.part_object(推荐留空: + # "哪个是工件"是场景的知识,换场景不该改这里)。 + object: "" + # 目标点相对物体**顶面中心**的偏移 [m],在物体局部系里(会跟着 yaw 转)。 + # 例:把目标从板中心挪到 k1 孔 → [0.0, -0.029, 0.0] + offset_m: [0.0, 0.0, 0.0] +# ============================================================================= # 通止规装配(看板 S00–S11):bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 # # 这一段全是**会动的量**,也是现场最常改的地方。改完不必改代码: @@ -492,7 +610,11 @@ gauge_assembly: # 前提是手外相机做过 eye-to-hand 标定:handeye/results/<相机>_eye_to_hand.yaml。 # 没有那个文件时 S03 会明确报"缺哪个文件、怎么生成"并回 S04,不做假装成功的反投影。 coarse_enabled: true - coarse_camera: "d435" # 手外相机命名空间(话题 /d435/...) + # 手外相机名:**以 cameras 段里 role=overhead* 且 arm 与本臂一致的那条为准** + #(就是 cameras 注册表,见 gauge_assembly_node._d435_ns)。这一项只在 + # "注册表里没有手外相机"时当兜底;两者不一致会在 S00 打告警并以注册表为准。 + # 所以真机/仿真各跑各的时候**不用**改这里 —— 曾经写死 d435 让仿真去查真机的相机。 + coarse_camera: "d435" coarse_hover_m: 0.25 # 粗定位后悬停在工件上方多高 [m](可调) coarse_conf: 0.35 # 工件检测置信度阈值(0~1) coarse_timeout_s: 20.0 # 等手外相机出图的最长秒数(相机刚起来要一两秒) @@ -502,7 +624,14 @@ gauge_assembly: # 跟踪抖动是零均值的,相邻帧之差会在画面明明在晃时判成稳定,拍出糊图。 snap_stable_s: 1.0 # 连续观察多久 [s] snap_tol_px: 2.0 # 允许的抖动半径 [px](越小越严,越难满) + snap_tol_mm: 0.4 # 同上,但特征=法兰位移时的容差 [mm] snap_min_frames: 6 # 至少采到多少帧才敢说"稳定" + # 用哪个特征判"画面稳定了": + # auto = 按 perception.source 自动选(真机 yolo→bbox;仿真真值/外部→motion) + # bbox = 工件检测框中心(像素)。需要检测器真的出框:仿真里合成图过不了 + # 真机训练出来的模型(实测最高分 0.017),选它会让 S05 空转到超时 + # motion = 法兰位移(mm)。相机装在法兰上,法兰不动画面就不动 —— 不依赖模型 + snap_stable_feature: "auto" # ---- S06 正悬停(悬停高度不在这里:见 servo.hole_hover_z_m)---- align_tol_mm: 1.0 # 对位残差容差 [mm];超了回 S05 重拍(不是硬往下走) @@ -511,7 +640,13 @@ gauge_assembly: # ---- S07 柔顺接触:步进下压,切向力突变即停 ---- contact_speed_m_s: 0.02 # 每步下压速度 [m/s];这一步必须慢,快了就是一次冲击 contact_step_mm: 0.5 # 每步下压量 [mm](越大越快,越小越不会过冲) - contact_max_depth_mm: 25.0 # 最多下探多少 [mm];到了还没接触判失败(不会一直压) + contact_max_depth_mm: 35.0 # 最多下探多少 [mm];到了还没接触判失败(不会一直压) + # 为什么是 35 而不是 25:判"到底了"的是**台肩落到工件 + # 上表面**那一下,而台肩在规尖上方 30mm(通端长 30, + # 见 models/gauge/model.sdf)。工件板厚 12mm,所以要 + # 下压 30mm 台肩才落面 —— 25mm 的上限根本到不了, + # 只会报"下探 25mm 都没触发接触判据"然后 ABORT。 + # 35 = 30(台肩落面)+ 5(对位残差/工件厚度公差余量)。 contact_tangential_delta: 300.0 # 接触判据:两路切向任一相对基线增量超过它 [任意单位] contact_safe_delta: 2000.0 # 法向安全上限 [任意单位]:超了立刻抬起并中止(E2) # 必须 > contact_tangential_delta,否则自检就报矛盾 diff --git a/devices/sim/gazebo/worlds/assembly.world b/devices/sim/gazebo/worlds/assembly.world index 35e9926..c952aeb 100644 --- a/devices/sim/gazebo/worlds/assembly.world +++ b/devices/sim/gazebo/worlds/assembly.world @@ -115,8 +115,29 @@ 0.55 0 0 0 0 0 0 0 0.375 0 0 0 - - 0.8 1.2 0.05 + + + 0 -0.3225 0 0 0 0 + 0.8 0.555 0.05 0.80.8 @@ -131,19 +152,56 @@ - - 0.8 1.2 0.05 + + 0 0.3225 0 0 0 0 + 0.8 0.555 0.05 + + + 0.80.8 + + + + 1e7 + 1e4 + 0.01 + 0.001 + + + + + + 0 -0.3225 0 0 0 0 + 0.8 0.555 0.05 + + 0.45 0.4 0.35 1 + 0.55 0.5 0.42 1 + + + + 0 0.3225 0 0 0 0 + 0.8 0.555 0.05 0.45 0.4 0.35 1 0.55 0.5 0.42 1 - + 0 0 0.175 0 0 0 - - 0.7 1.1 0.35 + + 0 -0.325 0 0 0 0 + 0.7 0.45 0.35 + + 0.3 0.3 0.3 1 + 0.35 0.35 0.35 1 + + + + 0 0.325 0 0 0 0 + 0.7 0.45 0.35 0.3 0.3 0.3 1 0.35 0.35 0.35 1