真机落地前的仿真补齐:夹持/腕力/真值位姿,让仿真能验证 S07–S09

这批是 09-24 那轮仿真工作的收尾,一直没提交。内容按目的分四块:

1) 仿真真能「夹住」工件
   sim_grasp_lock:给规与夹爪之间加接触锁(默认关,run_gauge_assembly --sim 才开)。
   没有它时仿真里规只靠摩擦,S08 棘轮一转就滑出指间,于是把「仿真不真」误读成
   「力判据不对」。新增 test_sim_grasp_lock.py。

2) 腕部力传感器进仿真
   sim_wrist_ft(+ config/wrist_ft.yaml):S07 的首选判据是腕部 FT 的轴向突变,
   而仿真里以前根本没有腕部力,S07 只能退到触觉兜底,两套判据从没在同一场景下比过。
   现在两条通道都在,接触判定可对照。

3) 真值位姿可查(不再靠猜)
   sim_live_pose / sim_pose_service:把 Gazebo 里刚体的实测位姿发布出来,
   stage2 的真值路径按**实测**位姿算孔位,而不是按场景文件里的标称位姿。
   顺带把真值孔位的推导(truth_holes)独立出来并补测。

4) 工具几何成为可配置项
   config/sim_tool_geometry.yaml + stage2 的 tool_tip_below_flange_m 启动参数:
   仿真里规会在指间下滑,工具长每次装夹都不同(实测见过 243/254mm),
   悬停高度与 S07 下探预算必须按实测值算,不能再写死。

应用层同时跟进:gauge_fsm / gauge_config 把「悬停时工具尖端入没入工件」的判据
(hover_z_guard)与拖件判定(drag_verdict)收成纯函数并补测;stage3 搜孔、
site 导出、run_gauge_assembly 与若干文档同步。
新增 test_gauge_exit_lift / test_gauge_gripper_client / test_node_handler_collisions。

测试:全仓库 1085 passed。
This commit is contained in:
arm_dev
2026-09-28 10:17:06 +08:00
parent 1b4d6168d9
commit c1cd862eea
41 changed files with 5327 additions and 115 deletions
+16 -1
View File
@@ -87,7 +87,22 @@ export YOLO_AUTOINSTALL=false
export ROS2_WS="${ROOT}"
# 仿真单独一个 DDS domain:避免与正在跑的真机节点互相抢话题/污染 TF。
# 真机用的是默认 domain,两者天然隔离。
export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-77}"
#
# ⚠️ 这里**强制** 77,不是 `${ROS_DOMAIN_ID:-77}`(2026-09-24 修的)。
# 写法上"尊重已有值"看起来更礼貌,实际会把隔离**交给运气**:实测本机 shell 里
# 就 export 着 ROS_DOMAIN_ID=0(真机默认),于是 `${ROS_DOMAIN_ID:-77}` 当场取到 0,
# 仿真栈和真机落在**同一个 domain** 上 —— 仿真的运动指令可能被真机控制器订阅到。
# 这是最坏的一类错误(动真机),而现场只会看到"脚本打印了 77、实际是 0"。
# 要换 domain 请显式设 SIM_ROS_DOMAIN_ID(名字里带 SIM,不会被真机环境的 export 撞上)。
if [[ -n "${SIM_ROS_DOMAIN_ID:-}" ]]; then
export ROS_DOMAIN_ID="${SIM_ROS_DOMAIN_ID}"
elif [[ "${ROS_DOMAIN_ID:-77}" != "77" ]]; then
echo "[sim_env] ⚠ 环境里已有 ROS_DOMAIN_ID=${ROS_DOMAIN_ID}(真机通常用 0)——" >&2
echo "[sim_env] 已**强制**改成 77:隔离不能靠「环境里恰好没设」。要另指定请设 SIM_ROS_DOMAIN_ID。" >&2
export ROS_DOMAIN_ID=77
else
export ROS_DOMAIN_ID=77
fi
# =============================================================================
# Gazebo 的插件与资源搜索路径 —— 必须在这里设,不能只在 launch 里设