diff --git a/devices/grippers/dh_pgea/manifest.yaml b/devices/grippers/dh_pgea/manifest.yaml index b8af9f2..20fd971 100644 --- a/devices/grippers/dh_pgea/manifest.yaml +++ b/devices/grippers/dh_pgea/manifest.yaml @@ -60,8 +60,10 @@ models: stroke_mm: 30.0 notes: > rotate 是通止规装配 S08 循环的核心原语，必须可独立调用且可随时中止。 - 单次转角默认 180°（配置项 system.screw_step_deg），因为不确定基于工具系 - 能转多少度是极限。 + 单次转角由 site.yaml 的 gauge_assembly.screw_step_deg 决定（出厂 120°）； + 它**不是**随手取的：棘轮一个来回要 2×step 的关节行程，必须 + 2×step + 2×余量 ≤ 关节7 行程（Flexiv 各机型都是 ±175° ⇒ 350° ⇒ step ≤ 167°）。 + 填 180° 会必然贴限位（360° > 350°），见 docs/FRAMEWORK_RULES.md R2.10。 # --- 配置项声明 ------------------------------------------------------------ config_schema: diff --git a/devices/sim/gazebo/README.md b/devices/sim/gazebo/README.md index c96058f..00f7abf 100644 --- a/devices/sim/gazebo/README.md +++ b/devices/sim/gazebo/README.md @@ -70,6 +70,12 @@ python3 scripts/check_sim_gripper.py # ★ 夹爪的几何验收（开 机械臂应当**在重力下保持站住**。若它瘫下去或消失，说明描述有问题 —— 见 [坑 1](#坑-1官方-flexiv_description-直接喂-gazebo-会被-sdformat-拒掉)。 +> ⚠️ `check_sim_scene.py` 是**跑前**检查（"现在这一轮能不能开始"），不是跑后检查： +> 一轮装配跑完之后场景**必然**是脏的 —— 放手时抓取锁释放、规掉在台面上， +> 落点可能磕到工件。所以跑完再查会报红，这是正常的。下一轮 +> `run_gauge_assembly.sh --sim` 开头的上料脚本会把 `scene.yaml` 里**所有**物体 +> 复位回名义位姿（`sim_load_gauge.py::_reset_scene_objects`），届时就干净了。 + **这几条 `check_*.py` 才是验收标准，"有图像 / 服务有返回"证明不了什么。** 它们做的判断分别是： @@ -532,6 +538,88 @@ python3 -m pytest src/arm_sim_gazebo/test/test_truth_holes.py -q --- +### 坑 14：上一轮把工件推走了 → 下一轮 S04 报"一个视觉目标点都没收到" + +现象（`/tmp/pipeline_stage1.log`）： + +``` +[sim_truth_perception-1] [ERROR] 仿真物体 'workpiece' 已经不在 scene.yaml 说的地方了 —— **停止发真值点** + 名义 (x,y,z)=(0.5200,0.0000,0.4000) yaw=0.0° + 实际 (x,y,z)=(0.4923,-0.0717,0.4000) yaw=8.3° + 偏差 |Δp|=76.9mm、姿态差 8.3°（容差 2.0mm / 2.0°） +``` + +然后下一轮 `gauge_assembly` 报「一阶段 150s 没锁定：**一个视觉目标点都没收到**」， +看起来像"真值节点没起来 / 场景名读不出来"。 + +**这条不是 bug，是两条正确行为叠在一起**： + +1. 真值感知是**纯几何**的 —— 它按 `scene.yaml` 的名义位姿算目标点与孔位。 + 物体一挪，算出来的就是假测量，而假测量**不会报错**：现象会是 + 「S05 两孔间距 169mm（真值 58mm）」或「S07 一路下压不接触」， + 看起来像检测/标定/阈值坏了。所以 `sim_truth_perception` 有漂移看门狗。 + 它**不是一条线，是两条**（2026-09-24 改，判据是纯函数 `pose_deviation()`， + 有单测；看门狗本身会自检"话题一帧都没收到"）： + * `pose_info.drift_tol_m`（2mm）→ **告警**，但**照发点**：真值几何已经跟着 + **实测**位姿算了（见下面 §5.1），2mm 只是"这场戏跟 scene.yaml 写的不一样了"； + * `pose_info.drift_stop_m`（20mm）→ **停发**：到这个量级说明工件掉了/被搬走了， + 场景前提没了，再发点等于指挥机械臂去装一个不存在的工件。 +2. 谁把工件推走的：上一轮 S07 判到接触的那一刻臂上顶着几十牛顿 + （位置控制的臂撞硬面**不会**在"刚碰到"停住），若卸载没生效，S08 再一转， + 工件就被推走/推歪了。所以第 1 条其实是在**替你报告真因**。 + +**修法**：把场景搬回去。`scripts/sim_load_gauge.py` 开头现在会复位 +`scene.yaml` 里**所有**物体（不止通止规）—— 所以正常情况下重跑一次就恢复正常， +不必重启仿真。若复位失败（比如工件卡在爪里），看门狗会继续拒发点， +那时**重启仿真**（`bash scripts/run_sim_gz.sh`）让它按场景重新 spawn。 + +排查顺序（按物理可信度）： + +``` +① 看 sim_truth_perception 的日志有没有"不在 scene.yaml 说的地方了" + ①a 偏差 ~2mm 级 → 只是告警，点照发，不用管（这是落地/蠕动的常态） + ①b 偏差 >20mm → 场景前提没了：复位（重跑 sim_load_gauge.py）或重启仿真； + 顺手查"谁推的它"（S07 的卸载有没有生效：日志里那行 + "接触后卸载：回退 X mm … 残余 Y N"） +② 没有 → 才轮到查"真值节点起没起来 / 场景名对不对" +``` + +### 5.1 真值口径：**几何按实测位姿算**，名义位姿只用来报"物体被搬走了" + +（2026-09-24 定；实现 `arm_sim_gazebo/sim_live_pose.py`，谁要谁订阅。） + +真值路径最早是"按 `scene.yaml` 的名义位姿算目标点与孔位"，也就是**假设物体没动过**。 +实测这个假设站不住，而且偏差是**安静**的： + +| 现象 | 真因 | +|------|------| +| S05 算出的两孔间距 169mm（真值 58mm） | 工件掉进 90mm 通槽、落到地面（z≈0.6m），而真值仍按名义位姿把孔投到像素上 | +| S07 一路下压不接触 | 孔位差 2~3mm ⇒ 规尖（φ6）对不上孔（φ7，单边 0.5mm 余量），压在工件面上而不是进孔 | + +而 2~3mm 是**天天都有**的：物体是"在名义位姿上方 spawn、再落下"的，落地就偏 +2.6mm；工件骑在台面通槽上静置十几分钟还会再挪 1.9mm。 + +所以现在的口径是： + +1. **算几何用实测位姿**（`/world//pose/info` → `LiveScenePose`）。 + 真值感知的目标点、二阶段定位的孔位都走这一条。这也更接近真机 —— + 真机上的视觉也是"量到哪就是哪"，不会拿图纸坐标去算。 +2. **名义位姿只做两件事**：判"物体还在不在该在的地方"（上面那两条线）， + 和开局复位。 +3. **开局复位**（`pose_info.reset_on_start: true`）：只在节点启动后 20s 窗口内、 + 只做一次，把被盯的物体 set_pose 回名义位姿 —— 专门消掉 spawn 落地的 + 2~3mm。运行中的漂移**必须报错**，不能悄悄搬回去（那会抹掉"谁把它撞歪了" + 的现场，还可能把工件从爪里拽走）。实现见 + `sim_truth_perception._maybe_startup_reset` 与 `sim_pose_service.set_model_pose_yaw`。 + +``` +⚠️ 复位/搬运用的是 Gazebo 的 `set_pose` 服务，命令行调用时 `--reqtype` 必须写 + `ignition.msgs.Pose`（写 `gz.msgs.Pose` 会报 "Unable to create request"，且不说是哪错了）。 + 实现只有一份：`arm_sim_gazebo/sim_pose_service.py`。 +``` + +--- + ## 6. 现状与进度 ### 已就绪（端到端验证过） diff --git a/devices/sim/gazebo/config/grasp_lock.yaml b/devices/sim/gazebo/config/grasp_lock.yaml new file mode 100644 index 0000000..15fe14f --- /dev/null +++ b/devices/sim/gazebo/config/grasp_lock.yaml @@ -0,0 +1,68 @@ +# ============================================================================= +# devices/sim/gazebo/config/grasp_lock.yaml +# +# 仿真"抓取锁"（grasp lock）的口径。**只对仿真有意义**，真机没有任何对应物 —— +# 真机靠夹爪的摩擦夹持，本来就不会掉。 +# +# ----------------------------------------------------------------------------- +# 它解决什么问题（2026-09-24 一整轮排查换来的） +# ----------------------------------------------------------------------------- +# 仿真里的夹持力不是"设"出来的，而是**位置误差 × 接触刚度**（见 gauge/model.sdf +# 里 kp 那段）。规站着不动时够用；机械臂一动就不行： +# +# S01 绕工具轴快转 150°、pre_lift/粗定位是 60~100mm 的连续轨迹 —— +# 这两步里规在指间打滑。实测工具长度 0.243m → 0.285m、规歪 62°， +# S07 随后拿到的几何全错，现象是"S07 判不到接触"或"规掉了(E1)"。 +# +# 真机不会这样：握力 46N、µ≈0.8、规 0.265kg —— 摩擦余量两个数量级。 +# 所以这把锁补的是 **Gazebo 缺的那块物理**（刚性接触没有静摩擦迟滞、也不模拟 +# 指垫橡胶的贴合），不是绕过物理：规与指垫的接触对仍在，触觉与腕部 FT 的读数 +# 依旧是真实接触力。 +# +# ----------------------------------------------------------------------------- +# ⚠️ 一处对不上就**静默失效**（不报错，只是规不跟着走） +# ----------------------------------------------------------------------------- +# 本文件的名字必须与下面三处一致： +# ① gauge/model.sdf 里 DetachableJoint 插件的 / +# ② gauge/model.sdf 里插件的 / +# ③ 真名（Gazebo 里实际存在的连杆名）—— URDF 里与固定关节相连的连杆会被 +# sdformat **合并（lump）**，名字会变：手指只剩 sim_left_finger_mount。 +# 查真名的办法： +# ros2 topic echo /sim/world_pose_info ← 看全部 child_frame_id +# gazebo_sim.launch.py 启动时会读 gauge 的 SDF 核对 ① ②，不一致就直接报出来。 +# ----------------------------------------------------------------------------- + +# 总开关。关掉它仿真照样跑，只是回到"靠摩擦夹持"，也就是上面那串毛病。 +enabled: true + +# 被夹住的物体（scene.yaml 里的物体名 = Gazebo 里的模型名）。 +object: "gauge" +# 该模型里被夹的那根连杆。 +object_link: "body" + +# 机器人侧：模型名 = launch 里 spawn 的 -name；连杆名必须是 Gazebo 的**真名**。 +# +# ⚠️ 必须锁到**不会跟着张爪动的**连杆上 —— 默认是 `sim_link7`（法兰）。 +# 为什么（2026-09-24 实测，一条假 E1 的真因）： +# 起初锁到 `sim_left_finger_mount`（左手指）。夹住的规被**刚性**锁在这根 +# 手指上，而 S08 棘轮每轮都要"松爪 → 回转 → 夹紧"：张爪时手指往外走 +# 40mm，被锁住的规却还插在孔里（6mm 销 / 7mm 方孔）——两处约束打架， +# 物理解算把规**从孔里拽出来**（实测规尖跳 +17.5mm、偏摆 64°）， +# 然后在回转那一步掉到地上。日志只看得到"通止规掉了(E1)"。 +# `sim_grav_base_link`（夹爪本体）在 sdformat 里被**合并**进了 `sim_link7` +# （见 /sim/world_pose_info 里的 `sim_link7_fixed_joint_lump__...`）， +# 所以锚点就写 `sim_link7`：张爪不影响它，松爪/夹紧都不会带偏规。 +# `sim_grasp_lock.load_grasp_lock()` 会拒绝含 `finger` 的连杆名（带这段原因）。 +robot_model: "rizon_sim" +robot_link: "sim_link7" + +# 触发话题（gz 侧，由本模块用 ign topic 发一个空消息触发）。 +# 名字必须与 gauge/model.sdf 里插件的 attach_topic/detach_topic 一致。 +attach_topic: "/sim/grasp_lock/attach" +detach_topic: "/sim/grasp_lock/detach" +# 插件状态回执（会 attach/detach 成功时各发一条）。用它来判断"锁到底有没有生效"， +# 而不是靠"看规掉没掉"——后者要等故障发生才知道。 +state_topic: "/sim/grasp_lock/state" + +# 发一次触发消息后等多久 [ms]（ign topic 是短命进程，给足启动时间）。 +publish_timeout_ms: 3000 diff --git a/devices/sim/gazebo/config/scene.yaml b/devices/sim/gazebo/config/scene.yaml index 5d81a67..f62bce4 100644 --- a/devices/sim/gazebo/config/scene.yaml +++ b/devices/sim/gazebo/config/scene.yaml @@ -99,6 +99,58 @@ gates_px: insert_ring_radius: [6, 30] hole_spacing: [90, 700] +# --------------------------------------------------------------------------- +# 世界模型位姿回读 —— 仿真里唯一能发现"物体被撞走/掉下去了"的手段 +# --------------------------------------------------------------------------- +# 为什么需要它（2026-09-24 花了一整轮排查换来的，值得完整读一遍）： +# `perception.source=truth` 是**纯几何**的：它按下面 objects 里的**名义位姿** +# 把孔投到像素上，**从不问 Gazebo 物体现在到底在哪**。 +# 于是工件一旦被碰走，"真值"就开始撒谎，而且每一处现象都指向别的原因： +# +# 实测：工件掉进台面通槽、落到地面（z≈0），而 scene.yaml 仍写着 z=0.40。 +# S05 拿"名义孔位"的像素去查**真实**深度图 → 孔位处看到的是槽底/地面 +# （0.6m，而工件面其实只有 0.21m）→ 三角化出 k1/k2 间距 169mm +# （真值 58mm）→ 被间距门槛丢掉 → 精拍连丢 9 拍 → S05 超时 ABORT。 +# 现场现象像"检测坏了 / 标定坏了 / 阈值调高了"，全都不是。 +# 真正的信息只有一句："**工件现在不在 scene.yaml 说的位置**。" +# +# Gazebo 自己会把每个模型的实际位姿发在 /world//pose/info +# （gz.msgs.Pose_V），桥成 tf2_msgs/TFMessage 之后，漂移监视器 +# （sim_truth_perception）就能把"名义位姿 vs 实际位姿"直接对比出来并**指名报错**。 +# +# 话题名里的 world 名必须与 worlds/assembly.world 的文件名一致（`assembly`）。 +pose_info: + gz_topic: "/world/assembly/pose/info" + ros_topic: "/sim/world_pose_info" + # 类型固定这两条：ros_gz_bridge 原生支持 Pose_V ↔ TFMessage（实测确认， + # 见 ros_gz_sim_demos 的 tf_bridge.launch.py）。写错类型不会有任何报错， + # 只会"桥接器起来了但一个消息都没有" —— 所以别改。 + ros_type: "tf2_msgs/msg/TFMessage" + gz_type: "gz.msgs.Pose_V" + # 漂移判据：**两个**阈值，别合成一个（2026-09-24 的教训）。 + # + # drift_tol_m（2mm）= **告警**线：真值几何已经跟着**实测**位姿走 + # （见 arm_sim_gazebo/sim_live_pose.py），所以 2mm 不再意味着"几何错了"， + # 只意味着"这个场景跟 scene.yaml 写的不一样了"。而 2~3mm 是**天天都有**的： + # spawn 落地就偏 2.6mm，工件骑在通槽上静置十几分钟还会再挪 1.9mm。 + # 2mm 的来历：真值投影在相机到工件 0.17m 处，fx≈447 ⇒ 1mm 位姿误差 ≈ 2.6px， + # 而 S05 的稳定判据是 2px —— 2mm 是"开始影响二阶段"的量级。 + # yaw 取 2°：两孔间距 58mm，绕中心转 2° 会让孔横移 1mm（约 2.6px），同量级。 + drift_tol_m: 0.002 + drift_tol_deg: 2.0 + # drift_stop_m 才是**停发**线：到这个量级说明工件掉了/被搬走了，场景前提没了 + # （再发点等于指挥机械臂去装一个不存在的工件）。20mm 的来历：工件一半宽 70mm、 + # 掉下台面是 400mm 量级，而"顺手碰一下"是几毫米 —— 20mm 两边都分得开。 + drift_stop_m: 0.02 + # 启动复位：开局把被盯的物体 set_pose 回名义位姿（仿真专有，默认关，这里开）。 + # 为什么还要它：上面两条线是"容差"，这一条是**消除源头** —— + # 物体是"在名义位姿上方 spawn、再落下来"的，落地那 2.6mm 偏差纯属 spawn + # 的落点问题（实测 set_pose 回名义位姿后它就停在那儿不动）。 + # 只在开局 20s 窗口内、且只做一次 —— 运行中的漂移必须报错，不能悄悄搬回去 + # （那会抹掉"谁把它撞歪了"的现场，还可能把工件从爪里拽走）。 + # 实现见 sim_truth_perception._maybe_startup_reset（含完整理由）。 + reset_on_start: true + # --------------------------------------------------------------------------- # 场景物体 # --------------------------------------------------------------------------- diff --git a/devices/sim/gazebo/config/sim_tool_geometry.yaml b/devices/sim/gazebo/config/sim_tool_geometry.yaml new file mode 100644 index 0000000..1b7914d --- /dev/null +++ b/devices/sim/gazebo/config/sim_tool_geometry.yaml @@ -0,0 +1,12 @@ +# ⚠ 本文件由 scripts/sim_load_gauge.py 自动生成（每次仿真上料都覆盖）。 +# 它是**这次装夹**实测到的工具几何，run_gauge_assembly.sh --sim 会读它 +# 并作为启动参数覆盖 site.yaml 的 gauge_assembly.tool_tip_below_flange_m。 +# 为什么必须这样：仿真里规会在指间下滑到落座位置，下滑量随装夹变化 +# （实测见过 243mm / 254mm 两种）。写成固定值迟早会算错悬停高度。 +# 真机没有这个文件（现场装夹后把实测值写进 site.yaml）。 +flange_z_m: 0.763 +measured_at: '2026-09-24T19:40:08' +model: gauge +tip_z_m: 0.5198 +tool_tip_below_flange_m: 0.2432 +world: assembly diff --git a/devices/sim/gazebo/config/wrist_ft.yaml b/devices/sim/gazebo/config/wrist_ft.yaml new file mode 100644 index 0000000..5938db7 --- /dev/null +++ b/devices/sim/gazebo/config/wrist_ft.yaml @@ -0,0 +1,50 @@ +# ============================================================================= +# devices/sim/gazebo/config/wrist_ft.yaml +# +# 仿真腕部力扭矩传感器（wrist force/torque）的话题与口径。 +# sim_wrist_ft.py、gazebo_sim.launch.py 从这里读；改这里不用重编译。 +# +# 真机的对应物：Flexiv 的 flexiv_robot_states_broadcaster 自动发 +# //ft_sensor_wrench geometry_msgs/msg/WrenchStamped（法兰系） +# 不需要任何配置 —— 它在真机 bringup 里无条件启动。 +# +# ----------------------------------------------------------------------------- +# 为什么仿真与真机能共用一套判据（这与触觉**相反**） +# ----------------------------------------------------------------------------- +# 手指触觉（tactile.yaml）两边单位不同，所以阈值必须各配一套；腕部 FT 不同： +# +# 真机 ft_sensor_wrench 单位就是牛顿，坐标系是法兰系。 +# 仿真 force_torque 传感器单位也是牛顿，挂 joint7 取 child(link7) 系， +# 而 link7 的 z 轴 = 工具轴（工具竖直朝下时即竖直方向）。 +# +# 于是两边的"z 分量"都是**轴向力，单位牛顿** —— 公式一致，只有具体阈值 +# 可以按现场标定分开配（见 site.yaml 的 gauge_assembly.contact_axial_delta）。 +# +# ⚠️ 与触觉一样，这也是一处"选错不报错"的口径：口径错了只会表现为 +# "判据不触发"或"一碰就炸"，看起来像算法问题。 +# ----------------------------------------------------------------------------- + +# gz 侧话题名。**必须与应用层按 sn 推出来的名字一致**： +# 应用层算 f"/{sn}/ft_sensor_wrench"，仿真臂 sn = "sim" +# ⇒ /sim/ft_sensor_wrench +# 改 sn（config/site.yaml 的 robot.arms[sim]）时必须同步改这里。 +# 也同时写死在 arm.sim.sensors.xacro 的 wrist_ft_topic 默认值里 —— 两处 +# 必须一致，check_sim_wrist_ft.py 会核对 xacro 展开结果里有没有这个话题。 +topic: "/sim/ft_sensor_wrench" + +# 挂载关节。改这里必须同步改 arm.sim.sensors.xacro 里的 。 +# 为什么是 joint7 见 xacro 里那段注释（不能挂 fixed 关节：物理引擎不把它当 +# 独立关节求解，传感器永远没输出且不报错）。 +joint: "sim_joint7" + +# 期望消息类型（ROS 侧）。gz.msgs.Wrench → WrenchStamped 由 ros_gz_bridge +# 直接支持（实测确认，无需中间适配器节点）。 +# +# 为什么是 WrenchStamped 而不是 Wrench：真机发的就是 WrenchStamped。 +# 类型一致，应用层才能不加分支地两边通用。 +ros_type: "geometry_msgs/msg/WrenchStamped" +gz_type: "gz.msgs.Wrench" + +# 传感器输出率 [Hz]。与真机 broadcaster 对齐（100Hz）。 +# 下游按"数据够不够新"判活，率太低会被误判成掉线。 +publish_hz: 100.0 diff --git a/devices/sim/gazebo/models/gauge/model.sdf b/devices/sim/gazebo/models/gauge/model.sdf index 8095060..f9243ee 100644 --- a/devices/sim/gazebo/models/gauge/model.sdf +++ b/devices/sim/gazebo/models/gauge/model.sdf @@ -118,13 +118,24 @@ + 摩擦给 0.8：真机夹爪指垫是橡胶/聚氨酯，对钢件的 µ 在 0.6~1.0 之间。 + 这个数**直接决定"能不能把轴向力传给手指"**：能传的上限 = µ·N + （单指），握力 N≈46N → 0.8 时两指合计 ≈74N，0.4 时只有 ≈37N。 + + 为什么原来是 0.4、为什么必须改（2026-09-24）： + 台肩一落面，接触刚度 5e4 N/m 意味着**过压 0.5mm 就是 25N**。µ=0.4 + 时两指上限 ≈37N，看着够；但一步步进就是 0.5mm，下一步就 50N > 37N + → 规在爪里打滑，力就再也传不到指垫触觉上（读数停在 0，而它是对的）。 + 把 µ 改成接近真实的 0.8，连同"改用腕部 FT 当判据"，两条一起才成立： + - 腕部 FT 在摩擦上游，打滑也测得准（首选判据）； + - 摩擦接近真实值，才不会出现"规被顶得往上溜 20mm 顶到掌心"的 + 仿真特有现象（那会让 S07 在 25mm 处顶死、报假原因）。 + 给太低（<0.2）还会让规在指间打滑自转，现象看起来像"夹爪没夹紧"。 --> 0 0 0.0555 0 0 0 0.0150.045 - 0.40.4 + 0.80.8 5e45e20.010.0005 @@ -189,5 +200,53 @@ + + + + body + rizon_sim + sim_link7 + /sim/grasp_lock/attach + /sim/grasp_lock/detach + /sim/grasp_lock/state + + diff --git a/devices/sim/gazebo/urdf/arm.sim.sensors.xacro b/devices/sim/gazebo/urdf/arm.sim.sensors.xacro index 458c855..f9c15f4 100644 --- a/devices/sim/gazebo/urdf/arm.sim.sensors.xacro +++ b/devices/sim/gazebo/urdf/arm.sim.sensors.xacro @@ -41,6 +41,8 @@ sensors_file:='' d405_enable:=true tactile_enable:=true + wrist_ft_enable:=true + wrist_ft_topic:='/sim/ft_sensor_wrench' d405_x:=0.0 d405_y:=-0.06 d405_z:=0.05 @@ -313,6 +315,56 @@ + + + + + true + + 100 + ${wrist_ft_topic} + + child + parent_to_child + + + + + diff --git a/devices/sim/gazebo/worlds/assembly.world b/devices/sim/gazebo/worlds/assembly.world index c952aeb..c03dff9 100644 --- a/devices/sim/gazebo/worlds/assembly.world +++ b/devices/sim/gazebo/worlds/assembly.world @@ -116,7 +116,7 @@ 0 0 0.375 0 0 0 - 0 -0.3225 0 0 0 0 - 0.8 0.555 0.05 + 0 -0.309 0 0 0 0 + 0.8 0.582 0.05 0.80.8 @@ -153,8 +172,8 @@ - 0 0.3225 0 0 0 0 - 0.8 0.555 0.05 + 0 0.309 0 0 0 0 + 0.8 0.582 0.05 0.80.8 @@ -170,16 +189,16 @@ - 0 -0.3225 0 0 0 0 - 0.8 0.555 0.05 + 0 -0.309 0 0 0 0 + 0.8 0.582 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 0.309 0 0 0 0 + 0.8 0.582 0.05 0.45 0.4 0.35 1 0.55 0.5 0.42 1 diff --git a/docs/FRAMEWORK_RULES.md b/docs/FRAMEWORK_RULES.md index ead4245..7ad5a19 100644 --- a/docs/FRAMEWORK_RULES.md +++ b/docs/FRAMEWORK_RULES.md @@ -198,6 +198,88 @@ Fortress 恒发 `fx=fy=277, cx=160, cy=120`（它的默认 60° 视场 + 半分 物体的落座微动是正常的。卡到 `1e-6` 只会得到永远为真的告警，人就会把检查关掉。 默认 5mm 起步（见 `sim_scene.pose_mismatches`）。 +### R2.8 工具长这类"装夹完才知道"的几何：仿真必须**实测并落盘**，不许写死标称值 + +悬停高度 = 工件表面 + **工具长** + 间隙。工具长写小几毫米，二阶段悬停时 +规尖就停在工件**里面** —— 后面 S05 的每条残差、每个告警都是假象 +（见 §8 的"残差恒定 11mm"那一条）。 + +仿真里这个值**装夹完才会定**：合爪后规会在指间下滑到落座位置，下滑量随装夹 +变化（实测见过 243 / 254mm，差 11mm）。所以口径固定为三级，不许砍： + + 1. 上料脚本（`scripts/sim_load_gauge.py`）合爪后**实测**规尖相对法兰的位置； + 2. 落盘 `devices/sim/gazebo/config/sim_tool_geometry.yaml`（含时间戳/世界名/模型名）； + 3. `run_gauge_assembly.sh --sim` 读它，作为**启动参数** + `tool_tip_below_flange_m:=` 覆盖 `site.yaml` 的标称值。 + +优先级与其余参数一致（R1.1：命令行 > `site.yaml` > 包内默认），真机不传这个 +参数、走 `site.yaml` 的人工实测值。`site.yaml` 里那个数**只是兜底**，别拿它 +当"这次的真实工具长"。下游拿到工具长后一律过 `gauge_fsm.hover_z_guard()` +（会检查悬停高度是否把规尖留在工件里，修不了就明确报错，不静默改）。 + +### R2.9 "目标角"和"增量"是两个口径，混用必出错 —— 一律用 `ratchet_delta_rad` 折算 + +关节角是**周期量**：`+338.9°` 和 `−21.1°` 是同一个位置，但 `338.9 − 158.9` +和 `−21.1 − 158.9` 差了整整一圈（一个"走 180°"、一个"倒着走 180°"）。 + +棘轮（S08 拧入 / S09 拧出）里两种口径长得很像 —— 上游给的是**目标角**， +下游要的是**增量**。混用不会报错，只会算出"行程不够/足够"的反结论，然后 +把"让余量"退到限位外面，最后报一句与真因无关的"偏差过大"（§8 那一条）。 + +规矩： + - 任何"目标 − 当前"都必须过 `gauge_fsm.ratchet_delta_rad(target, q_now)`， + 它把差折进 `(−180°, +180°]`，同一个目标 + 同一个位置只有一个增量； + - **转向要显式给**：`ratchet_delta_rad(target, q_now, direction)`，`+1` 正转、 + `-1` 反转。步长贴着 180° 时"谁更短"取决于 0.1° 级的读数差 —— 调用方按 + 自己读到的 `q=+162.0` 算出目标 `−18.0`，棘轮晚 0.1° 读到 `+162.1`， + `−180.1°` 折一圈就成了 `+179.9°`，**方向翻了**，于是棘轮朝螺纹反方向走、 + 贴限位、让余量、重试，净位移 0 ⇒ ABORT，而现场什么都没坏（§8 那一条）。 + 棘轮的转向是**工艺意图**（S08 顺时针拧入、S09 逆时针拧出），不能交给 + 数值巧合决定； + - 传参名必须写清口径（`target_rad` / `delta_rad`），不许叫 `angle`； + - 有单测钉住（`test_gauge_fsm.py`：越限目标的折算、让余量后落在限位内、 + `+338.9°` 与 `−21.1°` 增量相同、180° 刀口不许翻向、给了方向一定落回目标）。 + +### R2.10 棘轮的步长受**关节行程**约束：`2×step + 2×余量 ≤ 行程`，且累计是整数圈 + +棘轮一个来回（慢转 + 回转）要 `2×step` 的行程，两头还要留余量 —— 贴死限位时 +"让余量"**腾不出任何空间**（腾一步还是 `2×step`）。所以配 `180°` 的步长给 +Rizon4/Rizon10 的关节 7（±175° ⇒ 行程 350°）是**必然失败**的：`2×180 = 360 > 350`。 + +实测症状（2026-09-24）：第 1、2 轮正常，第 3 轮开始"贴限位 → 让余量 → 还是贴 +限位"，真因（配置）藏在最后一行，白烧 3 轮节拍。更隐蔽的是**每轮回转都比指令 +少走 4~5°**（速度/加减速限制），这点亏欠只能靠余量吃 —— 余量给 0 时关节会在 +7~8 轮后累计偏出限位、再也转不回来。 + +规矩： + - 步长 ≤ `(行程 − 2×余量) / 2`（Rizon4/10 关节7 ⇒ ≤167°，出厂取 **120°**）； + - `screw_max_cycles × screw_step_deg` 必须是 **360° 的整数倍**，否则规停在 + 螺纹中间、S09 也就少拧出几分之一圈； + - 判据写成纯函数 `gauge_fsm.ratchet_travel_ok(step, limits, margin)`， + **S08 开头**就拿 `/robot_description` 里的限位算一次，不通过直接 ABORT + 并打印"该改哪个键、改成多少"； + - 出厂值有回归锚点：`test_gauge_config.py` 拿 `flexiv_description` 里**每台 + 支持臂**的真实限位去算，以后加一台行程更小的臂会自动报红。 + +### R2.11 工具还在孔里时**只许竖直退**，横向动作必须等它出来 + +刚拧出的规**仍然插在孔里**（真机只是沿螺纹退回几个毫米；仿真里**没有螺纹几何**， +"拧出"只让规在孔里转，深度一点没变）。这时任何带横向分量的动作都会把规**拽**在 +孔壁上 —— 实测抓取锁被拽断、规掉在台面上。 + +**它在这里不报错**，两分钟后表现为"下一孔 S07 下探 35mm 判不到接触"（工具上已经 +没规了），日志指不到真因（R5.1 的静默失效）。实测那一次的几何差得极小：规尖比 +工件面低 30mm，而换孔的斜线只往上抬了 29mm —— 差 1mm 就是"拽出孔"和"干净退出"的 +分界。所以必须是**先后有序的两个动作**，不能靠斜线一起走。 + +规矩： + - 拧出（S09）的**成功出口之前**先把工具提到**跟踪高**（`_track_z()` = + 孔悬停高 + 50mm，流程里"已离开工件"的既有定义），之后再走 XY； + - 提出是**纯 Z**（`hold_xy=True`），横向分量一律非法（`test_gauge_exit_lift.py` + 钉住结构顺序 + 目标高度 + "已经高了就不动"）； + - 提出失败只告警不失败：那一刻孔里已经没有要保护的规，硬失败会把一次成功的 + 装配变成失败；后面的换孔/复位仍会走到安全位。 + --- ## 3. 触觉与力 @@ -362,6 +444,32 @@ conda 激活不会往 `LD_LIBRARY_PATH` 加东西，而 gz 的插件搜索路径 - `--verbose` 要把**原始物理量**（指尖坐标）打出来，判错时能回看； - 结论不对时，先怀疑判据，再怀疑被测对象 —— 上面两条都是先怀疑了代码。 +### R4.13 仿真与真机的隔离**不许**依赖"环境里恰好没设某变量" + +`scripts/sim_env.sh` 里原本写着： + +```bash +export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-77}" # ← 错 +``` + +看起来是"尊重调用者已有设置"，实际把隔离交给了运气。实测（2026-09-24）： +本机 shell 里就 export 着 `ROS_DOMAIN_ID=0`（**真机默认就是这个**），于是 +`:-77` 当场取到 0 —— 仿真栈和真机落在**同一个 DDS domain** 上， +仿真的运动指令可能被真机控制器订阅到。而现场只会看到"脚本打印了 77、 +`echo $ROS_DOMAIN_ID` 是 0"这种自相矛盾的输出。 + +**规则**：这类"安全隔离项"必须**强制**设成自己的值，要覆盖就用**专用名字** +（`SIM_ROS_DOMAIN_ID`）显式覆盖，并且把"我改掉了你的值"喊出来。同类的还有 +`ROS_LOCALHOST_ONLY`、`CONDA_PREFIX`、代理变量。 + +```bash +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}，已强制改为 77 …" >&2 + export ROS_DOMAIN_ID=77 +else export ROS_DOMAIN_ID=77; fi +``` + --- ## 5. 代码与测试 @@ -422,6 +530,20 @@ diff src/arm_application/arm_application/<文件> \ 不一致时的现象是"我明明改了，行为却和没改一样"，而且**没有任何提示** —— 最容易被误判成"我的改动没效果"。 +**实测代价（2026-09-24）**：修好 verticalize 的静默跳过后忘了重编 `install_sim/`， +又白跑一轮 —— 两轮的日志**一字不差**，看起来像"这个改法根本无效"，于是开始怀疑 +改错了地方。判别只要一条命令： + +```bash +grep -c "<你刚写进去的注释关键字>" \ + install_sim/<包>/lib/python3*/site-packages/<包>/<文件> # 0 = 旧拷贝 +``` + +**同日第二次撞上同一条（所以它值得写两遍）**：给 `sim_truth_perception` 加启动复位后 +跑测试，`pytest` 直接报 `ImportError: cannot import name 'RESET_GIVE_UP'` —— +源码里明明有。因为测试 import 的是 `install_sim/` 里那份**旧拷贝**。 +所以"改了源码 → 先 `bash scripts/build_sim.sh` → 再跑测试/仿真"，顺序不能反。 + ### R5.8 同一份数学/解析实现只许有一份，并加**守卫测试** 四元数换算曾经在 `pipeline` / `stage2` / `stage3` / `visual_servo` 四个 launch 里 @@ -663,8 +785,21 @@ launch 会一直挂着、终端不回提示符。要显式挂 `OnProcessExit → | 每次 pytest 十几条红，没人看测试结果了 | 环境缺 `onnx`，用例直接失败而不是 `skip` | R5.10 | | `site.yaml` 里改了参数，行为一点没变 | 那个键**没接线**（配置表里有、代码里从没读过） | R1.2 | | 改了源码，仿真里行为一点没变，也没有任何提示 | 仿真 `install_sim/` 是**拷贝**（不能 `--symlink-install`），没重编就跑 | R5.7 | +| `pytest` 报 `ImportError: cannot import name ...`，而源码里明明有 | 同一件事的另一副面孔：测试导入的是 `install_sim/` 里的旧拷贝 | R5.7 | +| S05 带着 6° 倾角去压孔 → 撞件/E2，日志只说"规划停滞" | `verticalize` 的纯旋转 hop 用**平移**做进度判据（dist≈0 ⇒ planned<0 ⇒ 整段跳过） | R5.1、R5.3 | +| S07 判到接触、也"卸载成功"，S08 一转却掉件(E1) | 卸载回退量与 `_goto` 容差同量级 ⇒ 浮点回减后 `dist 行程，必然有一头贴死（"让余量"也腾不出空间）；真因在配置里 | R2.10 | +| S08 朝**反**方向转，然后贴限位、让余量、重试，净位移 0 ⇒ ABORT | 步长正好 180° 落在"最短路径"的刀口上：0.1° 级的读数差就能把 `−180.1°` 折成 `+179.9°`，棘轮倒着走 | R2.9 | +| 回转那一步每轮都比指令少走 4~5°，攒到 7~8 轮后转不回来 | 速度/加减速限制让"快回转"实际走不满整步；余量必须 ≥ 这点亏欠，否则关节均位逐轮外漂 | R2.10 | +| 第一孔装得好好的，第二孔的 S07 下探 35mm 判不到接触 | 第一孔拧出后规**还插在孔里**，换孔的斜线把它从孔里**拽**出来 → 锁被拽断、规掉在台面（"工具上已经没规了"）；提出与横move必须**先后有序** | R2.11 | --- diff --git a/docs/GAUGE_ASSEMBLY.md b/docs/GAUGE_ASSEMBLY.md index c8c9fbc..8dcbf10 100644 --- a/docs/GAUGE_ASSEMBLY.md +++ b/docs/GAUGE_ASSEMBLY.md @@ -160,6 +160,7 @@ ros2 topic pub --once /gauge/reload_ok std_msgs/msg/Bool "{data: true}" | S07 接触判据 | 阈值**未在真机上标定**（仿真里做过通路验证）。⚠️ 见下面「S07 为什么在仿真里过不去」 | | S08/S09 力矩到位判据 | 同上，属现场标定项 | | 真机全流程 | **未跑过**（本仓库当前无真机）。干跑 / 仿真 / 单测已过，见 §7 | +| 仿真全流程 | ✅ **已跑通**（S00→S11、两个孔、DONE，连着两次；基线见 §7.1） | ### S07 为什么在仿真里过不去（2026-09-24 查到根因，别再往阈值上调） @@ -171,6 +172,11 @@ ros2 topic pub --once /gauge/reload_ok std_msgs/msg/Bool "{data: true}" 25→35mm 下探预算、台面开 90mm 通槽让贯通孔真能穿过 —— 都做了，都该做 （确实是独立的 bug），但**没解决 S07**。真实因果链是： +> 补记（2026-09-24 晚）：那道通槽后来从 90mm **收窄到 36mm** —— 90mm 比工件 +> 还宽（工件 y 向只有 100mm），工件会掉进槽里，而真值感知又按名义位姿报孔位， +> 于是 S05 三角化出 169mm 的假孔距。见 `devices/sim/gazebo/worlds/assembly.world` +> 里 `work_table` 的注释与 `scripts/check_sim_scene.py` 的承压宽度检查。 + | # | 实测到的 | 日志证据 | |---|---------|---------| | 1 | 悬停时规尖已在工件面下 **23mm** | `悬停时尖端离面 -23.0mm`（工具几何自检） | @@ -197,17 +203,61 @@ ros2 topic pub --once /gauge/reload_ok std_msgs/msg/Bool "{data: true}" 3. 仍用指垫触觉，但阈值要按 **µ·N 反推**（≈23N 量级），且必须把夹持段 摩擦系数与握力一起标进 `site.yaml`。**最不推荐**：换了规/换了指垫就得重标。 -**下一步（不要再对着触觉阈值试）**：实现第 1 条 —— S07 判据改读腕部 FT 的 -轴向分量，触觉只作兜底。 +**已落地（2026-09-24 当天实现并在仿真里跑通到 S07 判接触）**： ---- +S07 的**首选判据已改成腕部 FT 轴向突变**（`gauge_fsm.wrench_touch`），触觉只作 +兜底。真机用 Flexiv 的 `/＜sn＞/ft_sensor_wrench`、仿真用挂在 `joint7` 的 +`force_torque` 传感器（`devices/sim/gazebo/config/wrist_ft.yaml`），两边都是 +**牛顿 + 法兰/link7 系**，所以判据公式完全一致。 + +实测（仿真，k1 孔）： + +``` +S07 基线：切向 (-1.5, -22.5)、法向 53.9；腕部轴向 -21.82N（活，话题 /sim/ft_sensor_wrench） +S07 工具几何：悬停法兰 z=0.6320、工具尖端在法兰下方 245.0mm → 尖端 z=0.3870 +S07 接触（腕部 FT）：轴向 +48.37N 相对基线 -21.82N 超过 +8.0N（下压 5.39mm） +``` + +下压 5.39mm 与几何自检算出来的"台肩还差 5.0mm 落面"吻合到亚毫米 —— +这既证明判据有效，也证明**工具几何配置是对的**。 + +**改判据之后暴露出的两个"静默假通过"**（都已修，见 `FRAMEWORK_RULES.md` §8）： + +1. **卸载回退一步没走，却报"已放干净"**。判到接触的那一刻臂上顶着的是 + "出力上限"量级的力（实测 ~70N，与阈值无关），必须先回退 `contact_relief_mm` + 再交给 S08。但回退量 0.5mm 与 `_goto` 的到位容差写成了**同一个量级**， + 浮点回减后 `dist < tol` 当场成立 ⇒ `_goto` 一步没走就返回 True； + 而残余力判据比的是"回退前 vs 回退后"两个**相同**的读数 ⇒ 报"残余 0.0N"。 + 结果 S08 带着 70N 去转（夹持摩擦上限 µ·N 两指合计 ≈73N）→ 掉件(E1)。 + 现在：容差由 `gauge_fsm.relief_tol_m(回退量)` 推导（**保证严格小于回退量**， + 有单测），残余力按**基线**算而不是按回退前后之差算。 +2. **S08 第 2 轮就报"通止规掉了"，爪里其实好好的**。掉件判据原来吃 + `GripperStatus.is_gripped` 的**瞬时翻转**，而它是**推断值**（"指令到合拢侧 + + 实际开口被顶住"）；S08 会在"合到某开口/全合"之间发指令，某一帧的指令落到与 + 实际开口同一档 ⇒ 翻 False ⇒ 假 E1，然后整条流程去等人工"放好通止规"。 + 现在判据改看**开口塌陷**（`GripperClient.dropped()`：夹住时的开口 − 当前开口 + ≥ 5mm）—— 真掉件是两指合到底，几十毫米的变化，不会被指令切换带偏。 +3. **S09 第一步就 ABORT，报"期望 +338.9°、实测 +26.0°"**，而现场什么都没坏。 + 棘轮把上游给的**目标角（绝对值）**当**增量**用了：关节在 `+158.9°`、目标 + `+338.9°`（故意越限，好让下一步有余量）被算成"要转 338.9°"，于是"让余量" + 退到 `−180°`（比限位 `−175°` 还低）⇒ 回旋校验不过 ⇒ ABORT。 + 现在全部过 `gauge_fsm.ratchet_delta_rad()` 折算（见 `FRAMEWORK_RULES.md` R2.9）。 +4. **悬停高度按 `site.yaml` 的工具长算，S05 还是把工件推走**（迭代用满、残差 + 恒定 ~11mm，日志却让你查手眼/相机）。真因是仿真里规合爪后**在指间下滑**到 + 落座位置，实际工具长比标称长十几毫米 ⇒ 悬停时规尖已经插进工件 ⇒ 闭环追的是 + "被自己推着走的工件"。现在：上料脚本**实测**工具长并落盘 + `devices/sim/gazebo/config/sim_tool_geometry.yaml`，`run_gauge_assembly.sh --sim` + 当启动参数覆盖标称值（`site.yaml` 里只是兜底）；S05 另有 `drag_verdict` + 看门狗，工件被推走立刻停并明确报 code=6，不再让你去查手眼。 + + 250|--- ## 7. 验收命令（都是真跑过的） ```bash # 1) 纯逻辑层（不需要机器人） PYTHONPATH=src/arm_application python3 -m pytest src/arm_application/test -q -# 仿真环境：349 passed, 14 skipped（缺 onnx 的用例跳过，不伪装成失败） +# 仿真环境：454 passed, 14 skipped（缺 onnx 的用例跳过，不伪装成失败） # 2) 干跑全流程：S00→S01→…→S11→DONE，脚本退出码 0，结论 ok=true bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 --sim --dry-run @@ -217,6 +267,27 @@ bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 --sim --dry-run bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 --sim ``` +### 7.1 仿真全流程第一次真跑通（2026-09-24，留作基线） + +``` +S00_boot → S01_home_extreme → S02_grip_check → S04_track → pre_lift → stage1_track +→ S05_snap → hover → snap_coarse_0 → snap_fine_0 → align_move_0 → snap_fine_1 +→ align_move_1 → snap_fine_2 → aligned（k1 残差 0.05mm）→ descend_above_hole → hold +→ S06_hover_k1 → S07_contact（腕部 FT +213N 判接触，下压 31.6mm；卸载回 0N） +→ S08_screw_in_k1（12 轮）→ S09_unscrew_k1 → 竖直提出 Z 0.627→0.707 +→ S10_to_k2 → S06_hover_k2 → S07_contact → S08_screw_in_k2（12 轮） +→ S09_unscrew_k2 → 竖直提出 Z 0.638→0.707 → S11_reset --pose_ok--> DONE + +通止规装配结束：state=DONE ok=True 已装 2/2 孔 掉件重放 0 次 +``` + +**连着跑了两次都是 DONE**（第二次 `24` 次棘轮 = 12×2 孔，`relocations=0`）， +说明场景复位是干净的、不残留上一轮的状态。落盘结论： +`/tmp/gauge_assembly_sim.json`（`ok=true`、`holes_done=2/2`、`residual_mm≈0.02`、 +k1/k2 孔距 58mm —— 之前那个 169mm 是工件掉进 90mm 通槽后的假孔距）。 + +跑之前**必须**先编：`bash scripts/build_sim.sh`（坑 2）。 + > 退出码是**脚本**给的：脚本读节点落盘的结论文件（`ros2 launch` 自己的退出码 > 恒为 0，见 R6.9）。 @@ -251,6 +322,43 @@ bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 --sim 5. **测试红一片时，人就不看测试了。** 仿真环境没装 `onnx`（只有真机 conda 有）， 14 个用例常年报红。现在缺依赖的用例 `skip`（`importorskip`）， 让"红"只表示真出问题。 +6. **棘轮步长 180° 配 ±175° 的关节 7：必然贴限位。**（2026-09-24，S08 第 3 轮 ABORT） + 棘轮一个来回（慢转 + 回转）要 `2×step` 的行程：`2×180 = 360°`，而关节 7 的 + 行程只有 `350°`（±175°）—— 关节**必然**有一头贴死，而"让余量"也腾不出空间 + （腾一步还是 360°）。日志表现是"贴限位 → 让余量 → 还是贴限位"， + **真因（配置）藏在最后一行**，白烧 3 轮节拍才炸。 + + 更隐蔽的一半：每轮回转实测只走 175.7 / 174.8°（速度/加减速限制），**每轮亏 + 4~5°**；这点亏欠只能靠余量吃，余量给 0 时关节均位会在 7~8 轮后累计外漂、 + 再也转不回来。 + + 现在的口径（`FRAMEWORK_RULES.md` R2.10）：`2×step + 2×余量 ≤ 行程`， + 出厂取 `screw_step_deg: 120`（12 轮 × 120° = 1440° = 4 整圈，仍是 360° 的 + 整数倍）；S08 开头用 `/robot_description` 里的**真实限位**算一次 + （`gauge_fsm.ratchet_travel_ok`），不通过直接 ABORT 并打印"改哪个键、改成多少"。 + 出厂值本身有回归锚点：`test_shipped_site_yaml_ratchet_step_fits_every_supported_arm` + 会拿 `flexiv_description` 里每台支持臂的 joint7 限位去算。 +7. **棘轮的"转向"不能交给"谁更短"。**（同日，S08 第 3 轮 ABORT 的**另一半**） + 棘轮原来是"目标角 → 最近一圈的增量"。步长正好 180° 时，最近的那条路取决于 + 0.1° 级的读数差：调用方按自己读到的 `q=+162.0` 算出目标 `−18.0°`， + 而棘轮晚一点读到 `+162.1`，`−180.1°` 折一圈就成了 `+179.9°` —— **方向翻了**。 + 于是棘轮朝螺纹反方向走，接着贴限位、让余量、重试，**净位移 0** ⇒ ABORT， + 而现场什么都没坏（正是"最不该失败的一步失败"）。 + + 棘轮的转向是**工艺意图**（S08 顺时针拧入、S09 逆时针拧出），不能由数值巧合 + 决定：`ratchet_delta_rad(target, q_now, direction=±1)`，四个原子步都显式给方向。 +8. **拧出后不竖直退出孔口，横向动作会把规拽出来。**（2026-09-24，k1 装完、k2 报废） + k1 的 S08/S09 走完（规"拧出"了），S10 直接斜着换到 k2 —— 而规**还插在 k1 的 + 孔里**（仿真里没有螺纹几何，"拧出"只是让规在孔里转；真机也只退回几个毫米）。 + 横move把规从孔里**拽**出来：抓取锁被拽断、规掉在台面上。 + + 最坑的是**它在这里不报错**：两分钟后表现为"k2 的 S07 下探 35mm 判不到接触"， + 因为工具上已经没有规了。几何差得极小 —— 规尖比工件面低 30mm、斜线只抬了 29mm。 + + 现在的口径（`FRAMEWORK_RULES.md` R2.11）：S09 成功出口之前先**纯 Z** 提到 + **跟踪高**（悬停 + 50mm，与 S04 的 `pre_lift` 同一个口径），再做任何横向动作； + `test_gauge_exit_lift.py` 钉住"顺序 + 高度 + 已高就不动"，并且提出失败只告警 + （那一刻孔里已没有要保护的规，硬失败会把成功的装配变成失败）。 --- diff --git a/scripts/build_sim.sh b/scripts/build_sim.sh index d635351..528e0c4 100644 --- a/scripts/build_sim.sh +++ b/scripts/build_sim.sh @@ -77,6 +77,29 @@ if [[ "${NO_SKIP}" -eq 0 ]]; then ARGS+=(--packages-skip "${SKIP[@]}") fi +# --- 先做语法自检（colcon **不**编译 .py）------------------------------------ +# colcon 对 Python 包只做拷贝：语法错了它照样"构建成功"，而报错会出现在 +# 很久以后的 `ros2 run` / `pytest` 收集阶段，且长这样： +# File "install_sim/.../sim_truth_perception.py", line 441 +# 看上去像"安装产物坏了"，其实根因在源码里（实测过一个中文字符串里混了 +# ASCII 引号）。在这里提前炸，能把"源码错"和"安装产物错"分开。 +echo "检查 Python 源码语法（colcon 不编译 .py，语法错要到运行时才炸）…" +python3 - <<'PY' +import pathlib +import sys + +bad = [] +for p in sorted(pathlib.Path("src").rglob("*.py")): + try: + compile(p.read_text(encoding="utf-8"), str(p), "exec") + except SyntaxError as e: # noqa: PERF203 + bad.append(f" {p}:{e.lineno}: {e.msg}") +if bad: + print("源码有语法错误，先修再构建：", file=sys.stderr) + print("\n".join(bad), file=sys.stderr) + raise SystemExit(1) +PY + colcon --log-base log_sim "${ARGS[@]}" 2>&1 \ | grep -vE 'anaconda-auth|conda-content-trust|Unknown distribution option|warnings\.warn' diff --git a/scripts/check_sim_scene.py b/scripts/check_sim_scene.py index 4d4f46b..f5a8e1d 100644 --- a/scripts/check_sim_scene.py +++ b/scripts/check_sim_scene.py @@ -329,6 +329,134 @@ def check_d435_visibility(scene, root) -> tuple[list[str], str]: return probs, info +def check_table_slot(scene, root) -> tuple[list[str], str]: + """台面通槽 vs 工件：既要"规的通端过得去"，又要"工件掉不下去"。 + + 两个方向都会出事，而且都是**静默**的（2026-09-24 各栽过一次）： + + a) 槽开得不够 **窄**（< 规的通端）：规下压到 12mm 就顶在台面上， + S07 一路压到上限都等不到台肩落面 → 报"没触发接触判据"， + 看起来像传感器/阈值坏了。 + b) 槽开得**太宽**：工件跨在槽上时承压面太小，一有扰动就翻进槽里、 + 落到地面 —— 于是 `perception.source=truth` 开始撒谎：孔位按名义位姿 + 投到像素上，而孔位正下方已经不是工件（是槽底/地面）， + 深度反投影把 58mm 孔距算成 169mm，S05 连丢 9 拍超时。 + + 所以这里同时查两件事：孔位必须落在**开口**内，承压宽度必须**够**。 + """ + import xml.etree.ElementTree as ET + + from arm_sim_gazebo.sim_scene import rings_local, world_boxes + from arm_sim_gazebo.sim_site import resolve_world + + world_path, _ = resolve_world("", root) + try: + tree = ET.parse(world_path) + except (OSError, ET.ParseError) as exc: + return [f"读不出 world（{world_path}）：{exc}"], "" + table = None + for m in tree.getroot().iter("model"): + if m.get("name") == "work_table": + table = m + break + if table is None: + return [f"{world_path} 里没有 —— 台面被改名了？"], "" + + def _pose_xyz(node) -> tuple[float, float, float]: + p = node.find("pose") + if p is None or not (p.text or "").strip(): + return (0.0, 0.0, 0.0) + v = [float(x) for x in p.text.split()[:3]] + while len(v) < 3: + v.append(0.0) + return (v[0], v[1], v[2]) + + my = _pose_xyz(table)[1] + spans: list[tuple[float, float]] = [] + for link in table.findall("link"): + ly = my + _pose_xyz(link)[1] + for col in link.findall("collision"): + box = col.find("geometry/box/size") + if box is None or not (box.text or "").strip(): + continue + cy = ly + _pose_xyz(col)[1] + half = float(box.text.split()[1]) / 2.0 + spans.append((cy - half, cy + half)) + if not spans: + return ["work_table 里读不到任何 box 碰撞体 —— 台面几何解析不出来"], "" + spans.sort() + + b = scene.table_bounds + # 开口 = 台面范围内的"没被碰撞体盖住"的 y 区间。 + gaps: list[tuple[float, float]] = [] + cur = b.y_min + for lo, hi in spans: + if lo > cur + 1e-9: + gaps.append((cur, min(lo, b.y_max))) + cur = max(cur, hi) + if cur < b.y_max - 1e-9: + gaps.append((cur, b.y_max)) + gaps = [(lo, hi) for lo, hi in gaps if hi - lo > 1e-6] + + part = next((o for o in scene.objects if o.name == scene.operating.part_object), None) + if part is None: + return [f"场景里没有 {scene.operating.part_object!r}"], "" + boxes = world_boxes(scene, part, root) + if not boxes: + return ["读不出工件的碰撞体"], "" + wy0 = min(float(v) for bx in boxes for v in (bx[2], bx[3])) + wy1 = max(float(v) for bx in boxes for v in (bx[2], bx[3])) + + # 承压宽度 = 工件在 y 方向上被台面碰撞体盖住的长度（区间求交后累加）。 + supported = 0.0 + for lo, hi in spans: + supported += max(0.0, min(hi, wy1) - max(lo, wy0)) + + rings = rings_local(scene, part, root) + hole_ys: list[float] = [] + for r in rings: + c, s = math.cos(float(part.yaw)), math.sin(float(part.yaw)) + hole_ys.append(float(part.y) + s * float(r.x) + c * float(r.y)) + + probs: list[str] = [] + info = ( + f"台面沿 y 的支撑区间 {[(round(lo, 4), round(hi, 4)) for lo, hi in spans]}；" + f"开口 {[(round(lo, 4), round(hi, 4)) for lo, hi in gaps]}；" + f"工件 y∈[{wy0:.4f},{wy1:.4f}] 承压 {supported * 1000:.1f}mm" + ) + + # (a) 孔位必须落在开口里 —— 否则规的通端顶在台面上，S07 永远等不到台肩落面。 + for i, hy in enumerate(hole_ys): + ok = any(lo < hy < hi for lo, hi in gaps) + if not ok: + probs.append( + f"第 {i + 1} 个孔（世界 y={hy:+.4f}）**不在台面开口内** —— " + f"规的通端会顶在台面上。\n" + f" 后果：S07 下压 12mm 就到底，一路到上限都等不到台肩落面，" + f"报「没触发接触判据」，看起来像传感器/阈值坏了。\n" + f" 修法：把开口挪到孔位下方（worlds/assembly.world 的 work_table " + f"碰撞体 y 区间），或改工件摆放。\n" + f" 当前开口：{[(round(lo, 4), round(hi, 4)) for lo, hi in gaps]}" + ) + + # (b) 承压宽度必须够 —— 否则工件会翻进槽里掉下去，而真值感知会继续按 + # 名义位姿算，把 58mm 孔距算成 169mm（见 truth_holes.HoleTruth 的说明）。 + min_support = 0.020 + if supported < min_support: + probs.append( + f"工件跨在台面开口上的**承压宽度只有 {supported * 1000:.1f}mm**" + f"（要求 ≥ {min_support * 1000:.0f}mm）。\n" + f" 后果：板一有扰动就翻进槽里、落到地面；而 " + f"perception.source=truth 仍按 scene.yaml 的**名义位姿**投孔 → " + f"孔位下方已经不是工件而是槽底/地面 →\n" + f" 深度反投影把 58mm 孔距算成 169mm → S05 每拍都丢、最后超时。" + f"现象看起来是「检测坏了」，根因却是**工件掉了**。\n" + f" 修法：把开口收窄（改 worlds/assembly.world 里 work_table 碰撞体的 " + f"y 尺寸/位姿，开口只要够规的通端过就行，不需要覆盖两个孔所在的整个 y 带）。" + ) + return probs, info + + def check_gazebo_poses(scene, root) -> tuple[list[str], str]: """查 Gazebo 里物体的实际位姿，与 scene.yaml 对照。 @@ -441,7 +569,17 @@ def main() -> int: if p: print("\n".join(f" ✘ {x}" for x in p)) - print("\n[5] 粗定位可见性（工件是否完整落在手外相机画面内）") + print("\n[5] 台面通槽 vs 工件（规过得去 + 工件掉不下去）") + p, info = check_table_slot(scene, root) + problems += p + if info: + print(f" {info}") + if p: + print("\n".join(f" ✘ {x}" for x in p)) + else: + print(" ✓ 孔位都在开口内，且承压宽度够") + + print("\n[6] 粗定位可见性（工件是否完整落在手外相机画面内）") p, info = check_d435_visibility(scene, root) problems += p if info: @@ -452,7 +590,7 @@ def main() -> int: print(" ✓ 完整可见") if not args.no_poses: - print("\n[6] Gazebo 实际位姿核对") + print("\n[7] Gazebo 实际位姿核对") p, info = check_gazebo_poses(scene, root) problems += p if info: diff --git a/scripts/lib/export_site_env.py b/scripts/lib/export_site_env.py index f69606d..e6352f1 100644 --- a/scripts/lib/export_site_env.py +++ b/scripts/lib/export_site_env.py @@ -82,6 +82,11 @@ _SIDE_WORDS = { _ARM_SECTION_KEYS = ( "camera", "gripper", "paths", "servo", "handeye", "go_home", "yolo", "perception", + # gauge_assembly：几何类参数是**按台子**的 —— 同一个数在仿真与真机上不同。 + # 例：workpiece_surface_z_m 仿真 0.412（臂站地面、台面 0.40m），真机 0.012。 + # 不登记就会**静默失效**：site.yaml 里写了 robots[sim].gauge_assembly， + # 合出来还是全局值，S07 的几何自检算错 0.4m 却不报错。 + "gauge_assembly", ) diff --git a/scripts/run_gauge_assembly.sh b/scripts/run_gauge_assembly.sh index c7be3b9..54f56c9 100755 --- a/scripts/run_gauge_assembly.sh +++ b/scripts/run_gauge_assembly.sh @@ -136,6 +136,23 @@ if [[ "${USE_SIM}" == true ]]; then echo " python3 scripts/sim_load_gauge.py --dry-run # 只看算出来的位姿" >&2 exit 1 fi + # 工具长**用实测值**，不用 site.yaml 的标称值（2026-09-24 的教训，完整说明见 + # sim_load_gauge._write_measured_geometry）：仿真里规会在指间下滑到落座位置， + # 而下滑量随装夹变化（实测见过 243 / 254mm）。工具长写小了，二阶段悬停就会把 + # 规尖停在工件**里面** —— 接着 S05 的横向对准变成拿规尖拨工件（工件被推走、 + # 残差恒不收敛），日志却报"检查手眼/相机安装"；写大了则 S07 提前判"台肩落面"。 + # 真机没有这个文件（现场装夹后把实测值写进 site.yaml），所以这条只在仿真里走。 + GEOM_FILE="${ROOT}/devices/sim/gazebo/config/sim_tool_geometry.yaml" + if [[ -f "${GEOM_FILE}" ]]; then + TOOL_LEN="$(python3 -c 'import sys,yaml;print(yaml.safe_load(open(sys.argv[1],encoding="utf-8"))["tool_tip_below_flange_m"])' "${GEOM_FILE}")" + if [[ -n "${TOOL_LEN}" ]]; then + ARGS+=(tool_tip_below_flange_m:="${TOOL_LEN}") + echo "=== 工具长用**实测值** ${TOOL_LEN}m（${GEOM_FILE#"${ROOT}/"}）===" + fi + else + echo "⚠ 没有 ${GEOM_FILE#"${ROOT}/"}：工具长退回 site.yaml 的标称值。" >&2 + echo " 上料脚本每次都会重写它 —— 没有这个文件说明上料没走完（或没落盘）。" >&2 + fi fi echo "=== launch gauge_assembly（自检不过会自动报缺什么） ===" diff --git a/scripts/sim_env.sh b/scripts/sim_env.sh index a123869..8809d90 100644 --- a/scripts/sim_env.sh +++ b/scripts/sim_env.sh @@ -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 里设 diff --git a/scripts/sim_load_gauge.py b/scripts/sim_load_gauge.py index 47492d1..76b8ab9 100644 --- a/scripts/sim_load_gauge.py +++ b/scripts/sim_load_gauge.py @@ -91,12 +91,78 @@ GAUGE_GRIP_DIA_MM = 30.0 #: gz 报得出 Contacts、夹爪也能读到接触，但规就是托不住、一路滑下去。 GAUGE_GRASP_CENTER_M = 0.043 +#: 标定结果的合理性上限 [mm]：|开口 0 时的指间间距| 超过它就判标定无效。 +#: 指节是连杆机构、间距与开口斜率实测 1.000，所以这个量应是个小偏置； +#: 大得离谱只有一个原因 —— 合爪时手指被台面/工件挡住了（量到的是被挡的值）。 +#: 实测（2026-09-24）：home 位姿下量出 −10.15mm，扫描区间整体偏 10mm， +#: 17 档全掉，白扫 80 秒。 +CAL_SANITY_MM = 6.0 + # 台面高度（scene.yaml 的 table_top_z）：用来判断"够不够空间把规塞进去"。 FALLBACK_TABLE_Z = 0.40 # 触觉"夹住了"的判据：contact 位或 |fnormal| 超过这个噪声地板。 # 空载时仿真触觉的 fnormal 实测 ~1e-5（见 config/tactile.yaml 的基线说明）。 TACTILE_NOISE_FLOOR = 0.05 +#: 合爪后规在指间**允许**的下滑量 [m]。超过就判"这次装夹和上次不一样"。 +#: +#: 为什么是 3mm：下滑量会直接加在工具长上（site.yaml 的 +#: ``gauge_assembly.tool_tip_below_flange_m``）。而流程现在**读实测值** +#:（见 :func:`_write_measured_geometry`），所以下滑本身不再是错误 —— +#: 它只是"这次的工具长和上次不同"，改由实测值带着走。 +#: 超过这个量仍然要**说出来**：说明装夹方式不稳定，值得人看一眼。 +#: 实测（2026-09-24）：同一个仿真台先后测到 254.4mm 与 243mm（差 11mm）。 +DRIFT_SLIP_MAX_M = 0.003 + + +def _write_measured_geometry( + path: Path, + *, + below: float, + flange_z: float, + tip_z: float, + world: str, + model: str, +) -> Optional[Path]: + """把本次实测的工具几何落盘（供 ``run_gauge_assembly.sh --sim`` 读）。 + + **为什么要有这个文件**（2026-09-24 一整轮排查换来的）： + + 仿真里规在指间会下滑到落座位置（合爪时规被两指挤住，会一直滑到 + 它的台阶卡在指腹上为止）。下滑量**随装夹变化** —— 实测见过 243mm + 与 254mm 两种，差 11mm。而 site.yaml 里那个值决定两件事： + · 二阶段悬停高度（写小了 → 悬停时规尖已经插进工件 → S05 的横向 + 对准变成拿规尖拨工件，工件被推走、残差恒不收敛）； + · S07 的下探预算（写小了 → 台肩还没落面就用完预算）。 + 两个症状都离真因（几何）很远，所以**不许**让它靠"人手抄一个数字" + 来保持一致：上料脚本测出来就落盘，流程读它。 + + 写入失败**不抛异常**（返回 ``None``）：上料这件事本身已经成功了， + 调用方只需要把"退回标称值"这件事喊出来。 + """ + try: + data = { + "tool_tip_below_flange_m": round(float(below), 4), + "measured_at": time.strftime("%Y-%m-%dT%H:%M:%S"), + "world": world, + "model": model, + "flange_z_m": round(float(flange_z), 4), + "tip_z_m": round(float(tip_z), 4), + } + header = ( + "# ⚠ 本文件由 scripts/sim_load_gauge.py 自动生成（每次仿真上料都覆盖）。\n" + "# 它是**这次装夹**实测到的工具几何，run_gauge_assembly.sh --sim 会读它\n" + "# 并作为启动参数覆盖 site.yaml 的 gauge_assembly.tool_tip_below_flange_m。\n" + "# 为什么必须这样：仿真里规会在指间下滑到落座位置，下滑量随装夹变化\n" + "# （实测见过 243mm / 254mm 两种）。写成固定值迟早会算错悬停高度。\n" + "# 真机没有这个文件（现场装夹后把实测值写进 site.yaml）。\n" + ) + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text(header + yaml.safe_dump(data, allow_unicode=True), encoding="utf-8") + return path + except Exception: # noqa: BLE001 - 见 docstring：失败只退回标称值 + return None + def _load_yaml(path: Path) -> dict: with path.open(encoding="utf-8") as fh: @@ -194,6 +260,59 @@ def gauge_target_pose( return tip, q_gauge +def _reset_scene_objects(scene_cfg, world: str, spin_for) -> None: + """把 ``scene.yaml`` 里**所有**物体放回名义位姿（幂等）。 + + 为什么不止复位通止规（2026-09-24 实测踩到，代价是白跑一轮）： + 上一轮 S08 带着 ~70N 的轴向力转了一圈，把**工件**推走了 —— + 实测 |Δp|=76.9mm、yaw 偏 8.3°。而真值感知是**纯几何**的：它按 + ``scene.yaml`` 的名义位姿算孔位/目标点，物体一挪，算出来的就是假测量。 + 幸好有漂移看门狗（``sim_truth_perception``）当场停下不发点，否则现象会是 + 「S05 两孔间距 169mm」或「S07 一路下压不接触」，看起来像检测坏了。 + + 但看门狗只会"停下来"，它**不会把工件搬回去** —— 于是下一轮跑不了， + 报的还是那句「一个视觉目标点都没收到」，看起来像感知节点根本没起来。 + 复位这一下，让"重跑"真的从干净状态开始，不必重启整个仿真。 + + 顺序：先全部搬回名义位姿，再等物理稳一下。逐条打印结果 —— + 哪一件没复位成功必须看得见（它会以"真值不发点"的形式在下游爆出来）。 + """ + objs = [o for o in (scene_cfg.get("objects") or []) if o.get("pose")] + if not objs: + print("⚠ scene.yaml 里没有带 pose 的物体，跳过场景复位。") + return + failed: List[str] = [] + for obj in objs: + name = str(obj.get("name") or "") + if not name: + continue + p = obj.get("pose") or {} + try: + x = float(p.get("x", 0.0)) + y = float(p.get("y", 0.0)) + z = float(p.get("z", 0.0)) + yaw = float(p.get("yaw", 0.0)) + except (TypeError, ValueError) as exc: + print(f"⚠ {name}: pose 字段不是数字（{exc}），跳过。") + failed.append(name) + continue + quat = (0.0, 0.0, math.sin(yaw / 2.0), math.cos(yaw / 2.0)) + ok, out = _set_gauge_pose(world, name, (x, y, z), quat) + if ok: + print(f"=== 复位: {name} → ({x:.3f}, {y:.3f}, {z:.3f}) yaw={math.degrees(yaw):.1f}° ===") + else: + print(f"⚠ 复位失败: {name}：{out[:120]}") + failed.append(name) + if failed: + print(f"⚠ 以下物体没能复位：{', '.join(failed)}\n" + f" 后果：真值感知会（正确地）拒绝发点，S04 收不到目标点、报" + f"「一个视觉目标点都没收到」。\n" + f" 真修法：重启仿真（bash scripts/run_sim_gz.sh）让它按场景重新 spawn。") + # 让物理静一下再往下走：连着瞬移多个物体会让接触求解器抖几下。 + if objs: + spin_for(1.5) + + def _run(cmd: List[str], timeout: float = 8.0) -> Tuple[int, str]: """跑一条外部命令，返回 (退出码, 合并输出)。不抛异常（调用方自己判）。""" try: @@ -208,34 +327,13 @@ def _run(cmd: List[str], timeout: float = 8.0) -> Tuple[int, str]: def _set_gauge_pose(world: str, name: str, pos, quat) -> Tuple[bool, str]: """用 Gazebo 的世界服务把模型搬到指定位姿。 - 这里用 ``ign service`` 而不是另起一个 ROS 桥：仿真的场景加载本来就是 - 用命令行 `ros_gz_sim create` 做的（见 arm_sim_gazebo/sim_scene.py）， - 口径一致、少一个需要维护的话题/服务桥。 - - 注意 ``--reqtype`` 必须写 ``ignition.msgs.Pose``（本环境实测：写 - ``gz.msgs.Pose`` 会报 "Unable to create request"，一个字母的差别、 - 报错信息完全没提原因）。 + 实现**只有一份**：``arm_sim_gazebo.sim_pose_service.set_model_pose`` + （那个 `--reqtype` 必须写 ``ignition.msgs.Pose`` 的坑也写在那边）。 + 真值感知的启动复位走的是同一个函数。 """ - req = ( - f'name: "{name}", ' - f"position: {{x: {pos[0]:.6f}, y: {pos[1]:.6f}, z: {pos[2]:.6f}}}, " - "orientation: {" - f"x: {quat[0]:.8f}, y: {quat[1]:.8f}, z: {quat[2]:.8f}, w: {quat[3]:.8f}" - "}" - ) - code, out = _run( - [ - "ign", "service", - "-s", f"/world/{world}/set_pose", - "--reqtype", "ignition.msgs.Pose", - "--reptype", "ignition.msgs.Boolean", - "--timeout", "3000", - "--req", req, - ], - timeout=10.0, - ) - ok = code == 0 and "data: true" in out - return ok, out.strip() + from arm_sim_gazebo.sim_pose_service import set_model_pose + + return set_model_pose(world, name, pos, quat) def _gauge_pose_now(name: str) -> Optional[Tuple[float, float, float]]: @@ -977,6 +1075,64 @@ def _ros_side(args) -> int: # noqa: C901 - 线性流程，拆开反而更难读 print("=== --place-only：不动夹爪，只按当前姿态摆位 ===") else: print("=== 开爪（保证两指之间能让规进去） ===") + + # ---- 0. 归位：先把规放回规座（幂等，避免上一次的残留污染这一次）-------- + # 为什么必须做：本脚本是要**反复重跑**的（调试时一分钟一次）。上一次跑完规 + # 可能还卡在指间/落在台面槽里，而后面两步都会被"指间有东西"毁掉： + # · 标定指间间距：规卡在指间时量到的是**规的外径**（实测 28.29mm 而不是 + # 20mm），于是 gap_cal 凭空多出 8mm，整个扫描区间偏移 → 一处都夹不到； + # · 摆位：规已经在指间时再瞬移一次，落点是在旧位姿上叠的。 + # 先把它放回 scene.yaml 写的位置再开始，行为就与"刚启动仿真"完全一致。 + if not args.place_only: + # 先解开可能残留的抓取锁：上锁之后规是被**固定关节**钉在手指上的， + # 这时瞬移回规座会被关节立刻拉回来（实测：瞬移瞬间 z 变 0.36 又弹回）， + # 而手指合到底也只能到"规的外径"（实测 28.29mm 而不是 20mm）—— + # 于是标定读到的正是规径，gap_cal 凭空多 8mm。 + try: + from arm_sim_gazebo.sim_grasp_lock import load_grasp_lock, publish_lock_event + + _spec = load_grasp_lock(root=ROOT) + if _spec.enabled and publish_lock_event(_spec, False): + print("=== 归位前先解锁（清掉上一次可能残留的抓取锁） ===") + spin_for(0.5) + except Exception: # noqa: BLE001 - 锁是可选补救措施，读不到就跳过 + pass + + nom = None + for obj in (scene_cfg.get("objects") or []): + if str(obj.get("name")) == args.model: + p = obj.get("pose") or {} + try: + nom = ( + float(p.get("x", 0.0)), + float(p.get("y", 0.0)), + float(p.get("z", 0.0)), + float(p.get("yaw", 0.0)), + ) + except (TypeError, ValueError): + nom = None + break + if nom is None: + print(f"⚠ scene.yaml 里没有 {args.model} 的名义位姿，跳过「归位」这一步。") + else: + yaw = nom[3] + quat = (0.0, 0.0, math.sin(yaw / 2.0), math.cos(yaw / 2.0)) + ok, out = _set_gauge_pose(args.world, args.model, nom[:3], quat) + if ok: + print(f"=== 归位：把 {args.model} 放回规座 " + f"({nom[0]:.3f}, {nom[1]:.3f}, {nom[2]:.3f}) ===") + wait_tf(tcp_frames["left"], timeout_s=1.0) # 顺便让物理先稳一下 + spin_for(1.5) + else: + print(f"⚠ 归位失败（{args.model} 可能还在指间）：{out[:120]}") + print(" 后果：下面的标定会量到规的外径，扫描区间整体偏移。") + # 顺带把**其它**场景物体也复位（最要紧的是工件）。 + # 为什么：上一轮 S08 若带着大轴向力转了一圈，工件会被推走，而真值感知 + # 按名义位姿算孔位 —— 看门狗会（正确地）停发点，于是下一轮报 + # "一个视觉目标点都没收到"，看起来像感知节点没起来。 + # 详见 _reset_scene_objects 的说明。 + _reset_scene_objects(scene_cfg, args.world, spin_for) + if not args.place_only: if not gripper_call(cli_open, "open"): print("✗ /gripper/open 调用超时") @@ -1015,6 +1171,24 @@ def _ros_side(args) -> int: # noqa: C901 - 线性流程，拆开反而更难读 dz = pts["left"][2] - pts["right"][2] return (dx * dx + dy * dy + dz * dz) ** 0.5 * 1000.0 + # ---- 1.5 摆到"竖直上料位姿"（工具轴朝下 + 离台面够高） --------------- + # ⚠️ 这一步必须在**标定之前**（见下面对 gap_cal 的合理性检查）： + # 标定要在"手指前方是空的"前提下量，而零位/ home 位姿又低又斜， + # 合爪时手指很容易顶在台面或工件上 —— 那时量到的间距是**被挡住的值**， + # 于是 gap_cal 凭空差出十几毫米，整个扫描区间都跟着错，一处都夹不到。 + # 实测（2026-09-24）：home 位姿下标定出"开口 20mm 时间距 9.85mm"， + # 扫了 17 档全掉，白跑 80 秒。 + # 不摆的话：仿真零位工具轴是**水平**的，水平夹一根钢规会被重力抽出去 + #（实测合爪后漂移 857mm，直接掉回规座）；home 位姿又斜又低，放不下。 + # 这里用 IK 把两指 TCP 中点送到台面上方的空旷处、工具轴朝下。 + if not args.place_only and not args.no_pose: + if not _go_vertical_load_pose( + node, buf, spin_for, wait_tf, tcp_frames, args, + ): + print("✗ 摆竖直上料位姿失败。可以用 --no-pose 跳过（自己先把臂摆好）。") + rclpy.shutdown() + return 2 + if not args.place_only: if not gripper_call(cli_move, "move", position=args.cal_aperture_mm): print("✗ /gripper/move 调用超时（标定用）") @@ -1029,18 +1203,19 @@ def _ros_side(args) -> int: # noqa: C901 - 线性流程，拆开反而更难读 print(f" 开口 {args.cal_aperture_mm:.2f}mm → 指间间距 {gap_at_aperture:.2f}mm") print(f" ⇒ 开口 0（全闭合）时间距 {gap_cal_mm:.2f}mm，" f"间距 = 开口 + {gap_cal_mm:.2f}mm") - - # ---- 1.5 摆到"竖直上料位姿"（工具轴朝下 + 离台面够高） --------------- - # 不摆的话：仿真零位工具轴是**水平**的，水平夹一根钢规会被重力抽出去 - #（实测合爪后漂移 857mm，直接掉回规座）；home 位姿又斜又低，放不下。 - # 这里用 IK 把两指 TCP 中点送到台面上方的空旷处、工具轴朝下。 - if not args.place_only and not args.no_pose: - if not _go_vertical_load_pose( - node, buf, spin_for, wait_tf, tcp_frames, args, - ): - print("✗ 摆竖直上料位姿失败。可以用 --no-pose 跳过（自己先把臂摆好）。") - rclpy.shutdown() - return 2 + # 合理性检查：指节是连杆机构，间距与开口严格线性、斜率实测 = 1.000， + # 所以 gap_cal（开口=0 时的间距）应该是个**小量**（几毫米量级）。 + # 它大得离谱时唯一的原因是"手指被挡住了"：量到的是被挡的位置。 + # 这个检查为什么值得写：不查的话后面会白扫十几档、最后只说 + # "都没夹住"，还列一堆可能原因 —— 而真因是标定这一步就错了。 + if abs(gap_cal_mm) > CAL_SANITY_MM: + print(f"✗ 标定结果不可信：开口 0 时间距 {gap_cal_mm:.2f}mm" + f"（正常应在 ±{CAL_SANITY_MM:.0f}mm 内）。") + print(" 最可能：合爪时手指顶在台面/工件上（手指前方不是空的）。") + print(" 修法：先把臂摆到空旷处再上料 ——") + print(" bash scripts/go_home.sh 或用 --no-pose 自行摆位。") + rclpy.shutdown() + return 2 # ---- 1.8 摆位（纯瞬移）与"人手托着合爪" ------------------------------- # @@ -1264,8 +1439,14 @@ def _ros_side(args) -> int: # noqa: C901 - 线性流程，拆开反而更难读 "跳过位姿校验，只看触觉。") else: drift = math.dist(now_pos, tip_pos) - print(f"=== 校验：规还在不在手里 ===\n 位姿漂移 {drift * 1000:.1f}mm" - f"（合爪后被顶开/顶飞会远大于 20mm）") + print(f"=== 校验：规还在不在手里 ===") + print(f" 位姿漂移 {drift * 1000:.1f}mm。两种漂移**性质不同**，别混：") + print(" · > 20mm → 合爪时被顶飞 / 模型名或世界名不对（上料失败）") + print(" · 3~20mm → 规在指间**下滑**到了它的落座位置。这不是上料失败，" + "而是") + print(" site.yaml 的 tool_tip_below_flange_m 与实际装夹不符 ——") + print(" 下滑量会**直接加在工具长上**，写小了二阶段悬停就会把") + print(" 规尖停在工件里面（S05 横向对准于是拿规尖拨工件）") tac_ok = False for side in ("left", "right"): item = tac.get(side) @@ -1283,6 +1464,18 @@ def _ros_side(args) -> int: # noqa: C901 - 线性流程，拆开反而更难读 if drift is not None and drift > 0.02: problems.append(f"规没在位姿上（漂移 {drift * 1000:.1f}mm）" "：可能合爪时被顶飞、或模型名/世界名不对") + elif drift is not None and drift > DRIFT_SLIP_MAX_M: + # 3~20mm：规落到了指间的落座位置。**不是**上料失败 —— 它是"这次的 + # 工具长和 site.yaml 里那个值不一样"，而工具长稍后会实测并落盘， + # 流程读的是实测值（见 _write_measured_geometry）。所以这里只"说出来"： + # 下滑量偏大说明装夹不稳定，值得人看一眼。 + print(f" ⚠ 规在指间下滑了 {drift * 1000:.1f}mm 才落座 —— 这次的工具长与" + f"site.yaml 的标称值差了这么多（不是上料失败）：") + print(" 下滑量会直接加在工具长上。稍后的「实测工具几何」会把它测出来并" + "落盘成") + print(" sim_tool_geometry.yaml，流程（--sim）会读它覆盖标称值 —— 所以" + "能继续跑。") + print(" 但下滑量偏大说明装夹不稳，值得确认一下规是不是卡在指腹上了。") if not tac_ok: problems.append("两指触觉都没读到接触：可能没合上（看上面开口）、或规不在两指之间") if problems: @@ -1296,6 +1489,112 @@ def _ros_side(args) -> int: # noqa: C901 - 线性流程，拆开反而更难读 print("✓ 通止规已装进夹爪（位姿贴合 + 触觉有接触）。可以跑装配了：") print(" bash scripts/run_gauge_assembly.sh --sim --holes 1") + + # ---- 4. 上"抓取锁"（仿真专用）------------------------------------------ + # 为什么由**这里**上锁，而不是等夹爪适配器自己判： + # 适配器的 is_gripped 判据是"指令到全闭合、实际被工件顶住"（真机 DH 没有 + # 夹持状态字，只能这么推）。而本脚本是**按直径精确合到某个开口**的 + # （例如 24.9mm 卡住 φ30 的规），指令与实际一致 —— 于是适配器**判不出** + # "夹住了"，锁也就不会触发。 + # 本脚本是"装夹"这件事的执行者，它自己知道刚刚校验过（位姿贴合 + 触觉 + # 有接触），所以由它上锁最准，也最好解释。 + # 解锁不需要在这里做：夹爪适配器在收到 open / 开口变大的 move 时会解锁 + #（见 sim_gripper_adapter._release_grasp_lock）。 + try: + from arm_sim_gazebo.sim_grasp_lock import load_grasp_lock, publish_lock_event + + _spec = load_grasp_lock(root=ROOT) + if _spec.enabled and _spec.object == args.model: + if publish_lock_event(_spec, True): + print(f"✓ 抓取锁已上：{_spec.object}.{_spec.object_link} 固定到 " + f"{_spec.robot_model}.{_spec.robot_link}") + print(" （仿真专用补丁：Gazebo 的刚性接触给不出真实指垫的静摩擦，" + "不上锁时规会在连续轨迹里打滑 —— 真机 46N 握力 + µ≈0.8 本来就不掉）") + else: + print("⚠ 抓取锁没上成（详见日志）——机械臂连续运动时规可能打滑。") + elif _spec.enabled: + print(f"⚠ 抓取锁配的对象是 {_spec.object}，与 --model {args.model} 不一致，跳过上锁。") + except Exception as exc: # noqa: BLE001 - 锁是补救措施，读不到配置不该拦住上料 + print(f"⚠ 读抓取锁配置失败，跳过上锁：{exc}") + + # ---- 7. 实测工具几何（给 site.yaml 的 tool_tip_below_flange_m 用）---- + # 为什么必须**实测**而不是推算：这个值决定 S07 的悬停/下探预算（见 + # gauge_fsm.descend_needed_m），差 10mm 就会让"台肩落面"永远到不了。 + # 推算要凑三层偏移（TCP 在法兰下方多少 + 规尖在 TCP 下方多少），而 + # 中间任何一层的定义（比如 tcp 帧是零偏移还是带偏置）都可能与假设不同。 + # 实测只有一次减法：法兰 z − 规尖 z。真机同法（夹住规后直接读 TF）。 + got_e2 = wait_tf(args.ee_frame, timeout_s=3.0) + tip_now = _gauge_pose_now(name) + if got_e2 is not None and tip_now is not None: + p_ee = got_e2[0] + below = float(p_ee[2]) - float(tip_now[2]) + print("\n=== 实测工具几何（抄进 config/site.yaml 的 gauge_assembly）===") + print(f" 法兰 z={p_ee[2]:.4f} 规尖 z={tip_now[2]:.4f}" + f" → **tool_tip_below_flange_m: {below:.3f}**") + if below <= 0.0: + print(" ⚠ 规尖不在法兰下方 —— 多半是规掉了或者没夹在指间，" + "别把这个值抄进配置。") + else: + # ⚠️ 这里以前算的是「当前法兰 z − 工具长」，也就是**上料时臂停在哪**， + # 与悬停高度毫无关系 —— 实测它印出"工件面 z=0.412 → 尖端 z=0.5201"， + # 看着很安全，而真正决定"会不会把规捅进工件"的是 site.yaml 里 + # servo.hole_hover_z_m（当时 0.632 ⇒ 尖端 0.387 = 面**下** 25mm）。 + # 现在按配置的悬停高度算，并且复用 gauge_fsm 的判据（一份实现）。 + try: + from arm_application.gauge_fsm import hover_z_guard + + site = _load_yaml(ROOT / "config" / "site.yaml") + rob = next( + (r for r in (site.get("robot") or {}).get("robots") or [] + if str(r.get("sn")) == str(getattr(args, "arm", "sim"))), + {}, + ) + ga = dict(site.get("gauge_assembly") or {}) + ga.update(rob.get("gauge_assembly") or {}) + servo = dict(site.get("servo") or {}) + servo.update(rob.get("servo") or {}) + surface = float(ga.get("workpiece_surface_z_m") or 0.0) + hover = float(servo.get("hole_hover_z_m") or 0.0) + # 目标间隙与二阶段"最后轻降"用的是**同一个**配置（hole_detect.stage2 + # .final_gap_mm）：都是"规尖悬在工件面上方多少"。两处各读一份迟早 + # 会打架，而症状是"上料脚本说安全、stage2 说危险"。 + st2 = dict((site.get("hole_detect") or {}).get("stage2") or {}) + gap = float(st2.get("final_gap_mm") or 2.0) / 1000.0 + fixed, why = hover_z_guard(hover, surface, below, gap) + print(f" 用它核二阶段悬停高：servo.hole_hover_z_m={hover:.3f}、" + f"工件上表面 z={surface:.3f}") + if why: + print(" ⚠ **悬停时规尖会捅进工件**（S05 的横向对准会拖着工件走）：") + print(f" {why}") + else: + print(f" → 悬停时规尖离工件面 " + f"{(hover - below - surface) * 1000:+.1f}mm（安全）") + except Exception as exc: # noqa: BLE001 - 这一项只是提示，别拦住上料 + print(f" （悬停高度自检跳过：{exc}）") + + # ---- 把实测值落盘，交给流程当启动参数 -------------------------- + # 为什么不能只"打印出来让人抄进 site.yaml"（2026-09-24 实测的教训）： + # 仿真里规会在指间下滑到落座位置，而**下滑量随装夹变化** —— + # 同一个仿真台先后测到过 0.243 和 0.254（差 11mm）。写死哪个值， + # 下一次都可能算错悬停高度，而错误现象是"S05 残差恒不收敛" + # （看起来像手眼坏了）。所以：**测量 → 落盘 → 流程读它**。 + # 真机走不到这里（真机由人装夹、同法读 TF，但由现场把值写进 site.yaml）。 + geom_path = _write_measured_geometry( + ROOT / "devices" / "sim" / "gazebo" / "config" / "sim_tool_geometry.yaml", + below=below, + flange_z=float(p_ee[2]), + tip_z=float(tip_now[2]), + world=args.world, + model=name, + ) + if geom_path is None: + print(" ⚠ 实测值没能落盘 —— 流程会退回 site.yaml 的标称值，" + "悬停/下探预算可能差几毫米到十几毫米。") + else: + print(f" 已落盘：{geom_path.relative_to(ROOT)}" + f"（run_gauge_assembly.sh --sim 会把它当启动参数覆盖 site.yaml）") + else: + print("\n（读不到法兰/规尖的位姿，跳过工具几何实测）") rclpy.shutdown() return 0 diff --git a/src/arm_application/arm_application/gauge_assembly_node.py b/src/arm_application/arm_application/gauge_assembly_node.py index 46075d7..db9b6f7 100644 --- a/src/arm_application/arm_application/gauge_assembly_node.py +++ b/src/arm_application/arm_application/gauge_assembly_node.py @@ -94,6 +94,9 @@ from arm_application.gauge_config import ( ) from arm_application.gauge_fsm import ( DESCEND_STALL_STEPS, + ROT_AXIS, + ROT_LIMIT, + ROT_SHORT, Event, FsmError, GaugeContext, @@ -109,9 +112,15 @@ from arm_application.gauge_fsm import ( pixel_stable, points_stable, ratchet_cycles, + ratchet_delta_rad, + ratchet_travel_ok, + relief_tol_m, + rotation_verdict, screw_stop, tangential_jump, tool_clearance_m, + wrench_live, + wrench_touch, ) from arm_application.gauge_gripper import ( CONTRACT_STATUS_TOPIC, @@ -139,9 +148,26 @@ from arm_application.stage2_locate_node import _bgr_copy, _pt as _joint_point #: 允许的"绕轴验证"误差 [deg]。超过就说明这根关节轴不是工具 Z 轴， #: 后面所有以"转多少度"为单位的动作都不成立 —— 必须报错而不是继续。 AXIS_VERIFY_TOL_DEG = 5.0 +#: 棘轮转腕时距离关节限位保留的余量 [deg]。 +#: 3° 不够用：实测轨迹会**自己**少走 ~5°（比如命令 180° 只到位 175.1°， +#: 而关节7 的限位正好是 175.06° —— 两者叠在一起，判断"是被限位截住还是 +#: 没走完"就分不清了）。留 8° 让这两种情况都还没有贴住。 +RATCHET_LIMIT_MARGIN_DEG = 8.0 #: 限制"跟踪丢失 → 粗定位 → 跟踪"的来回次数，防死循环。 MAX_TRACK_RETRIES = 3 +#: ``stage2_locate`` 返回码 → 人话。为什么要这张表：日志里只有 ``code=6`` 时， +#: 现场只能去翻源码；而每个码对应的**下一步动作完全不同**（垂直化/粗定位/ +#: 检测/对准/工件被推走），写在这里就不用来回翻了。 +_LOCATE_CODE_HINT: Dict[int, str] = { + 2: "（垂直化或悬停失败：工具轴没竖直、或悬停高度到不了）", + 3: "（粗定位失败：视野里没框到工件 —— 看相机/光照/权重）", + 4: "（k1/k2 解不出来：孔被挡、曝光不对、或孔根本没在视野里）", + 5: "（对准迭代用满仍未收敛：检查手眼/相机安装，或放宽 align_tol_m）", + 6: "（工件被工具推走：不是手眼/检测的问题，是悬停时工具尖端已经进了工件" + " —— 查 gauge_assembly.tool_tip_below_flange_m 与 servo.hole_hover_z_m）", +} + class GaugeAbort(RuntimeError): """阶段内部判定要中止。带上 exit code，方便脚本区分原因。""" @@ -158,6 +184,11 @@ class GripperClient: （真机今天是 Trigger，契约/仿真 是 GripperExecute），应用层不需要知道。 """ + #: 判"掉件"的开口塌陷量 [mm]：被夹住的工件掉了以后，两指会一路合到底， + #: 开口从"工件宽度"塌下去几十毫米。为什么不用 ``is_gripped`` 瞬时值 —— 见 + #: :meth:`dropped` 的说明（那是个推断值，会被 S08 的指令切换抖出假 E1）。 + DROP_COLLAPSE_MM = 5.0 + def __init__(self, node: Any, backend: str, status_topic: str) -> None: self._node = node self.backend = backend @@ -166,6 +197,10 @@ class GripperClient: self._last_gripped: Optional[bool] = None self._legacy_grip_state: Optional[int] = None self._ever_gripped = False + #: 最近一帧的开口 [mm]（契约后端才有）。掉件判据看它。 + self._last_aperture: Optional[float] = None + #: "夹住那一刻"的开口 [mm]。掉件判据是相对它的**塌陷量**，不是绝对值。 + self._grip_aperture: Optional[float] = None if backend == "contract": self._open_cli = node.create_client(GripperExecute, "/gripper/open") @@ -178,9 +213,15 @@ class GripperClient: # -- 状态回调 --------------------------------------------------------- def _on_contract(self, msg: GripperStatus) -> None: + pos = float(getattr(msg, "position", float("nan"))) + if math.isfinite(pos): + self._last_aperture = pos self._last_gripped = bool(msg.is_gripped) - if self._last_gripped: - self._ever_gripped = True + # ⚠️ 这里**故意不**记"夹住时的开口"。曾经记过，代价是一个假 E1： + # 本后端的 is_gripped 是**推断值**，夹爪**全开**时也会闪成 True + # （实测 99.29mm，见 dropped 的说明），于是基准被记成 99.29mm、 + # 塌陷量算成 77mm ⇒ "通止规掉了(E1)"，而规好好地在爪里。 + # 基准只由 :meth:`adopt_current_grip` 在**明确知道夹住了**的时候记。 def _on_legacy(self, msg: Float32MultiArray) -> None: fields = legacy_states_to_fields(list(msg.data)) @@ -191,6 +232,8 @@ class GripperClient: if got is not None: self._last_gripped = got if got: + # 老后端只有状态字、没有开口，掉件判据本来就吃状态字（见 dropped）， + # 所以这里记"曾经夹住过"是对的。 self._ever_gripped = True # -- 查询 ------------------------------------------------------------- @@ -205,15 +248,109 @@ class GripperClient: """``True``/``False``/``None``（后端给不出明确答案）。""" return self._last_gripped + def aperture(self) -> Optional[float]: + """最近一帧的**开口** [mm]（只有契约后端的 ``position`` 有；没有返回 None）。""" + return self._last_aperture + + def grip_aperture(self) -> Optional[float]: + """第一次判到"夹住"时的开口 [mm]（掉件判据的基准，见 :meth:`dropped`）。""" + return self._grip_aperture + + def collapse_mm(self) -> Optional[float]: + """相对夹住时的**开口塌陷量** [mm]（正 = 比夹住时更小）。拿不到返回 None。 + + S08/S09 每轮都要"松爪 → 回转 → 夹紧"，日志里必须看得到这个数： + 它一路变大就说明每轮夹紧都比上一轮更往里挤，或者规在往下滑。 + """ + if self._grip_aperture is None or self._last_aperture is None: + return None + return float(self._grip_aperture) - float(self._last_aperture) + + # -- 开口的"等稳定"与"记基准" ------------------------------------------ + def settle(self, *, settle_s: float = 1.0, tol_mm: float = 0.05) -> Optional[float]: + """等开口**稳定**下来，返回稳定后的开口 [mm]（拿不到读数返回 None）。 + + 为什么必须有这个（2026-09-24 用日志抓到的）： + ``close``/``open`` 服务**返回时动作还没做完**（JTC 轨迹是异步的）。 + 立刻读开口会读到**松手前的旧值**——实测 S08 夹紧后立刻读，读到的还是 + 全开 99.93mm，于是日志与基准全错（"夹住时 99.29mm"）。谁要在动作之后 + 判断开口，都必须先过这里。 + """ + spin = getattr(self._node, "_spin", None) + deadline = time.monotonic() + max(0.0, settle_s) + prev = self._last_aperture + stable = 0 + while True: + if spin is not None: + spin(0.05) + else: # pragma: no cover - 只有测试桩走这里 + time.sleep(0.05) + now = self._last_aperture + if now is not None and prev is not None and abs(float(now) - float(prev)) <= tol_mm: + stable += 1 + if stable >= 2: + return float(now) + else: + stable = 0 + prev = now + if time.monotonic() >= deadline: + return None if now is None else float(now) + + def adopt_current_grip(self, why: str, *, settle_s: float = 1.0) -> Optional[float]: + """把**当前**开口记为"夹住时的开口"（掉件判据的基准），并记"曾经夹住过"。 + + 只在**明确知道爪里夹着规**的时刻调用（S00 自检通过后、以及每一次 + 确认夹紧之后）——它是掉件判据唯一的参照，随手记会让判据失去意义 + （见 :meth:`dropped` 与 :meth:`_on_contract` 的说明）。 + """ + ap = self.settle(settle_s=settle_s) + if ap is None: + # 拿不到开口（老固件/别的夹爪）也要记"曾经夹住过"：否则 dropped() + # 会永远返回 False，那比"会抖的旧判据"更糟（真掉件也看不见）。 + self._ever_gripped = True + self._node.get_logger().warn( + f"拿不到开口读数（{why}）—— 掉件判据退回状态字，会抖但不会漏" + ) + return None + self._grip_aperture = float(ap) + self._ever_gripped = True + self._node.get_logger().info(f"夹持基准：开口 {ap:.2f}mm（{why}）") + return float(ap) + def dropped(self) -> bool: """E1 掉件：**曾经夹住过**，现在报"掉落"。 装配前爪里本来就空，那时状态字也可能是"掉落/无物"，直接判会一开场就 报 E1（``is_dropped_from_state`` 里解释了这一点）。 + + **契约后端的判据是"开口塌陷"，不是 ``is_gripped`` 的瞬时翻转** + （2026-09-24 实测踩到，值得完整读）： + + 仿真里 ``is_gripped`` 是**推断值**（"指令到了合拢侧 + 实际开口比指令 + 大出容差"）。S08 会在"合到某个开口 / 全合"之间发指令 —— 只要某一帧 + 的指令落到与实际开口同一档，``is_gripped`` 就翻成 False。于是 + **S08 第 2/12 轮直接报 E1**（爪里其实好好的，是判据在抖）， + 然后整条流程去等人工"放好通止规"，而仿真里根本没人能做这件事。 + 更坏的是这个推断值在**全开**时也会闪成 True（实测 99.29mm）—— + 拿它当基准就会得到"塌陷 77mm"的假 E1。所以基准只由 + :meth:`adopt_current_grip` 明确记录（见 :meth:`_on_contract`）。 + + 真正"掉了"的物理表现是**两指合到底**：开口从"工件宽度"塌到几毫米， + 是几十毫米的变化，不会被指令切换带偏。所以判据用 + ``夹住时的开口 − 当前开口 ≥ DROP_COLLAPSE_MM``。 """ + if not self._ever_gripped: + return False if self.backend == "legacy" and self._legacy_grip_state is not None: return is_dropped_from_state(self._legacy_grip_state, self._ever_gripped) - return self._ever_gripped and self._last_gripped is False + if self._grip_aperture is None or self._last_aperture is None: + # 没有开口读数（老固件/别的夹爪不给 position）：退回旧判据。 + # 它确实会抖，但比"永远判不了掉件"好 —— 而且这种情况本身会 + # 在 S00 的自检里被 `position` 相关检查点到。 + return self._last_gripped is False + return ( + float(self._grip_aperture) - float(self._last_aperture) + ) >= self.DROP_COLLAPSE_MM # -- 动作 ------------------------------------------------------------- def _call( @@ -310,12 +447,31 @@ class GaugeAssembly(AssemblyPipeline): self.declare_parameter("result_file", "/tmp/gauge_assembly_result.json") site = load_site_dict() + # robots[].gauge_assembly 覆盖全局段 —— 与 servo / paths / perception 同一套规矩。 + # 为什么必须有这条（2026-09-24）：几何类参数是**按台子**的。 + # `workpiece_surface_z_m` 在仿真里是 0.412（仿真臂站在地面、台面 0.40m 高）， + # 真机是 0.012（基座与台面同高）。写死全局值的话，S07 的 + # "悬停时工具尖端离面多高"自检会算错 —— 而现象是"一路下压判不到接触"， + # 看起来像力阈值标歪了，排查会从阈值开始走偏。 + _robot = self._robot_dict(site) + if isinstance(_robot.get("gauge_assembly"), dict): + site = dict(site) + site["gauge_assembly"] = deep_merge( + site.get("gauge_assembly") or {}, _robot["gauge_assembly"] + ) overrides: Dict[str, Any] = {} if bool(self.get_parameter("dry_run").value): overrides["dry_run"] = True holes = int(self.get_parameter("holes_total").value) if holes > 0: overrides["holes_total"] = holes + # 工具长：仿真里由启动脚本传**实测值**（上料脚本刚量的），真机不传（走 site）。 + # 为什么必须能覆盖：这个值决定悬停高度与 S07 下探预算，而仿真里规会在指间 + # 下滑到落座位置、下滑量随装夹变化（实测见过 243 / 254mm，差 11mm）。 + # 差 11mm 的后果见 gauge_fsm.hover_z_guard / drag_verdict 的说明。 + tool_len = float(self.get_parameter("tool_tip_below_flange_m").value) + if tool_len > 0.0: + overrides["tool_tip_below_flange_m"] = tool_len try: self.cfg: GaugeConfig = from_site(site, overrides=overrides or None) except GaugeConfigError as exc: @@ -906,6 +1062,13 @@ class GaugeAssembly(AssemblyPipeline): return Event.BOOT_OK self._publish_gauge_result({"ok": False, "state": "S00", "failures": blockers}) return Event.BOOT_FAIL + # 记"夹住时的开口"（掉件判据的基准）。**放在这里而不是等到 S08**： + # · 此刻爪里确实夹着规（仿真上料脚本刚装好，真机是人工装好才开始跑）； + # · 基准必须是**明确知道夹住**时记的 —— 本后端的 is_gripped 是全开时 + # 都会闪 True 的推断值（实测 99.29mm），拿它当基准会造出假 E1 + # （见 GripperClient.dropped）。 + if self.grip is not None: + self.grip.adopt_current_grip("S00 自检通过：爪里已装好通止规") return Event.BOOT_OK # ===================================================================== @@ -1087,20 +1250,25 @@ class GaugeAssembly(AssemblyPipeline): f"{tag}: 目标 {delta_deg:+.1f}°，实测绕工具 Z {got:+.1f}°" f"（转轴偏差 {mis:.1f}°，阈值 {AXIS_VERIFY_TOL_DEG:.1f}°）" ) - if abs(got) < 3.0 and abs(delta_deg) > 10.0: - # 完全没动：最可能是撞上关节极限（用户要求的"转到极限角"就是这种情形） + verdict = rotation_verdict(delta_deg, got, mis, AXIS_VERIFY_TOL_DEG) + if verdict == ROT_LIMIT: + # 完全没动、或只走了一部分：都是**关节限位**把这段截住了 + # （2026-09-24 实测：棘轮回转每轮顶到上限 ±175°，于是下一步反向 + # 只剩 ~16° 余量）。交给调用方让余量重试（见 _ratchet_step）； + # 调用方不处理时 FSM 会按未定义转移报错，不会静默走下去。 self.get_logger().warn( - f"{tag}: 指令 {delta_deg:+.1f}° 但几乎没动 —— 判为已贴关节极限" + f"{tag}: 期望 {delta_deg:+.1f}°、实测 {got:+.1f}°" + f"（差 {abs(delta_deg) - abs(got):.1f}°）—— 判为**贴到关节限位**" ) return Event.JOINT_LIMIT - if mis > AXIS_VERIFY_TOL_DEG: + if verdict == ROT_AXIS: self.get_logger().error( f"{tag}: 转轴偏差 {mis:.1f}° 超过阈值 —— 关节7 轴与工具 Z 不共线，" "以『度』为单位的动作（S01/S08/S09）都不可信，中止。" "先确认工具系是否垂直向下、有没有装转接件" ) return Event.ABORT - if abs(abs(got) - abs(delta_deg)) > max(AXIS_VERIFY_TOL_DEG, 0.1 * abs(delta_deg)): + if verdict == ROT_SHORT: self.get_logger().error( f"{tag}: 期望 {delta_deg:+.1f}°、实测 {got:+.1f}°，偏差过大，中止" ) @@ -1108,6 +1276,146 @@ class GaugeAssembly(AssemblyPipeline): self._axis_verified = True return fsm_event + def _ratchet_step( + self, target_rad: float, *, tag: str, fsm_event: Event, speed_deg_s: Optional[float], + direction: int = 0, + ) -> Event: + """棘轮的一步"转腕"：行程不够时**张开爪让出余量**再转。 + + :param target_rad: 关节 7 的**目标角（绝对值）**，与 + :meth:`_rotate_joint_and_verify` 同一口径（那边算的就是 + ``target_rad - q_now``）。**不是增量。** + :param direction: 这一步的**工艺转向**：``+1`` 正转、``-1`` 反转、 + ``0`` 交给"最短路径"。棘轮的四步一律显式给：步长接近 180° 时 + "最短路径"会被 0.1° 的读数差翻方向（详见 + :func:`arm_application.gauge_fsm.ratchet_delta_rad`）。 + + ⚠️ 为什么要把口径写在这里（2026-09-24 栽过一次，S09 直接 ABORT）： + 调用方（S08/S09）给的本来就是绝对值（``q[-1] ± screw_step_rad``）， + 而本函数此前把它当**增量**用（``prewind(want_deg=abs(deg(target)))``、 + 让余量时又按 ``q - target`` 算新目标）。于是 S09 第一步被算成 + "要转 338.9°"（其实是目标 338.9°、增量只有 180°）⇒ 误判"行程不够" + ⇒ 让余量那一步算到限位**外面**（``158.9 − 338.9 = −180°``，比 + −175° 的限位还低 5°）⇒ 报"期望 −338.9°、实测 +26.0°，偏差过大" + ⇒ 整条流程在**最不该失败的一步** ABORT（规在爪里好好的、什么坏事都没发生）。 + 两个函数各用一种口径，编译器不会说话，日志还会把"目标"和"增量"都印成 + 一个数 —— 所以口径写进参数名和 docstring。 + + 为什么需要"让余量"这一步： + 棘轮要求"一正一反各 180°"，而关节 7 的行程只有 ±175°（3.0543 rad）。 + 拧入（S08）的**回转**那一步每轮都正好顶到上限（实测 +175.1°）， + 于是 S09 要做的**反方向**第一步只剩 ~16° 余量，被限位截住。 + 而这一轮其实什么坏事都没发生。 + + 为什么"让余量"是免费的： + 指间**张开**时规在打滑（这正是棘轮成立的原理），所以手腕空转既不会 + 把已经拧进去的螺纹转回去，也不会漏掉进度 —— 只是把行程重新分配。 + + 让多少：整步（不去算亏欠了多少）。反正张开状态下这一步不产生任何装配 + 效果，整步最省事也最不容易算错；重试时的那一步才是"用力"的那一步。 + + 两条路：**先算**（限位已知时，提前让余量，干净）＋ **兜底**（真被限位 + 截住时再让，覆盖"限位没读到 / 轨迹自己没走完"这两种情况）。 + + 两条路**最后都汇到同一次让余量**：只有一次，避免"先算发现不够、 + 让完还不够、再让一次"这种把行程越绕越少的做法。 + """ + limits = self._wait_joint_limits() + q = None + if limits is not None: + q, _Tee = self._wait_state(3.0) + if q is None: + return Event.TIMEOUT + # 目标角可能落在限位**外面**（这正是棘轮要解决的问题），先折成"最近一圈" + # 的增量（gauge_fsm.ratchet_delta_rad，含完整说明与单测）： + # 关节在 +158.9°、目标 +338.9° ⇒ 增量 +180°（而不是 +338.9°）。 + # **方向按工艺意图给**：步长贴着 180° 时"最短路径"会被读数差翻向（R2.10）。 + delta_rad = ratchet_delta_rad(target_rad, float(q[-1]), direction) + why = "" + if limits is not None and abs(math.degrees(delta_rad)) > 1e-6: + plan = prewind( + q_now_rad=float(q[-1]), want_deg=abs(math.degrees(delta_rad)), + direction=1 if delta_rad > 0 else -1, + limits=limits, margin_deg=RATCHET_LIMIT_MARGIN_DEG, + ) + if plan.clamped: + why = ( + f"行程不够（关节7 在 {math.degrees(float(q[-1])):+.1f}°，" + f"这一步要转 {math.degrees(delta_rad):+.1f}°" + f"（目标 {math.degrees(float(target_rad)):+.1f}°），限位 " + f"{math.degrees(limits[0]):+.0f}~{math.degrees(limits[1]):+.0f}°）" + ) + if not why: + ev = self._rotate_joint_and_verify( + target_rad, tag=tag, fsm_event=fsm_event, speed_deg_s=speed_deg_s + ) + if ev is not Event.JOINT_LIMIT: + return ev + why = "轨迹被关节限位截住（算着够、实际没走完）" + return self._ratchet_rewind_then( + delta_rad, tag=tag, fsm_event=fsm_event, speed_deg_s=speed_deg_s, why=why, + ) + + def _ratchet_rewind_then( + self, delta_rad: float, *, tag: str, fsm_event: Event, + speed_deg_s: Optional[float], why: str, + ) -> Event: + """张开爪 → 反向让一步 → 夹紧 → 转原来那一步。 + + :param delta_rad: **要转的增量**（不是目标角，见 :meth:`_ratchet_step`）。 + 让余量时按它反着退一步，重试时再按它前进 —— 两次都以**当时的** + 关节角为基准重新算目标，所以让完余量后关节会回到原处、工具净转 + ``delta_rad``。把"目标"固定成一开始那个绝对值是错的：让完余量之后 + 那个目标已经在另一圈上了（差 360°）。 + """ + if self.grip is None: + self.get_logger().error(f"{tag}: {why}，但没有夹爪客户端，无法让余量 → 中止") + return Event.ABORT + self.get_logger().info( + f"{tag}: {why} —— 张开爪让出一步（此刻规在打滑，不会动到螺纹）再转" + ) + if not self.grip.open(): + self.get_logger().error(f"{tag}: 需要张爪让余量，但开爪失败 → 中止") + return Event.ABORT + time.sleep(self.cfg.screw_release_wait_s) + q, _Tee = self._wait_state(3.0) + if q is None: + return Event.TIMEOUT + ev = self._rotate_joint_and_verify( + float(q[-1]) - delta_rad, tag=f"{tag}_rewind", + fsm_event=fsm_event, speed_deg_s=speed_deg_s, + ) + if ev is not fsm_event: + self.get_logger().error( + f"{tag}: 让余量那一步也没走开（{ev}）—— 关节7 的行程真用尽了，中止。\n" + f" 排查：site.yaml 的 gauge_assembly.screw_step_deg 太接近关节行程了" + f"（Rizon4 的关节7 是 ±175°），棘轮一步要 {math.degrees(abs(delta_rad)):.0f}°，" + f"留给'回转'的余量本来就不够。\n" + f" 改法：把 screw_step_deg 调小（例如 120°），并保证 " + f"screw_max_cycles × screw_step_deg 仍是 **360° 的整数倍** —— " + f"累计必须转整数圈，否则规会停在螺纹中间（S09 拧出也就少转几分之一圈）。" + ) + return Event.ABORT + if not self.grip.close(): + self.get_logger().error(f"{tag}: 让完余量后夹紧失败 → 中止") + return Event.ABORT + # 重试目标**按让完余量后的位置重算**：让余量退了一步，原来那个绝对目标 + # 现在差了 360°（正好一整圈），照原样下发会让关节反向绕一圈回去。 + q2, _Tee2 = self._wait_state(3.0) + if q2 is None: + return Event.TIMEOUT + ev = self._rotate_joint_and_verify( + float(q2[-1]) + delta_rad, tag=f"{tag}_retry", + fsm_event=fsm_event, speed_deg_s=speed_deg_s, + ) + if ev is not fsm_event: + self.get_logger().error( + f"{tag}: 让完余量重试仍然没走到位（{ev}）—— 中止" + f"（不是限位问题，先查是不是有东西卡住/螺纹咬死）" + ) + return Event.ABORT + return fsm_event + # ===================================================================== # S02 夹持确认（预留） # ===================================================================== @@ -1301,7 +1609,9 @@ class GaugeAssembly(AssemblyPipeline): return Event.TIMEOUT code, q, Tee, final = self._run_locate(q, Tee) if code != 0 or not final: - self.get_logger().error(f"S05 精拍定位失败 code={code}") + self.get_logger().error( + f"S05 精拍定位失败 code={code}{_LOCATE_CODE_HINT.get(code, '')}" + ) return Event.TIMEOUT self._snap_final = dict(final) self._snap_pose = Tee.copy() @@ -1440,6 +1750,21 @@ class GaugeAssembly(AssemblyPipeline): return (0.0, 0.0) return (float(getattr(best, "fshearx", 0.0)), float(getattr(best, "fsheary", 0.0))) + def _wrench_axial(self) -> Optional[float]: + """当前腕部 FT 的轴向力 [N]；一帧都没收到过时返回 None。 + + 轴向 = ``force.z``。这个口径对真机与仿真**都成立**： + 真机 ``//ft_sensor_wrench`` 是法兰系，z 是法兰轴； + 仿真传感器挂在 joint7 上取 child(link7) 系，z 也是工具轴。 + 工具锁垂直向下时，两者都等于"竖直方向的力"。 + """ + return getattr(self, "_ft_fz", None) + + def _wrench_age(self) -> Optional[float]: + """腕部 FT 最近一帧的年龄 [s]；一帧都没收到过时返回 None。""" + t = getattr(self, "_t_ft", None) + return None if t is None else (time.monotonic() - float(t)) + def _step_contact(self) -> Event: self._state("S07_contact") if self.cfg.dry_run: @@ -1452,8 +1777,16 @@ class GaugeAssembly(AssemblyPipeline): # 基线必须在**还没碰工件**的时候测。S06 结束时悬停在工件上方， # 正好是测基线的时刻（带载测会把阈值抬飞）。 ref = self._baseline(samples=8) + # 腕部 FT 的轴向基线：与触觉基线同一时刻采（这时还没碰工件）。 + # 腕部 FT 在夹持摩擦的**上游**，所以它是 S07 的首选判据；触觉只是兜底 + #（通止规靠夹持摩擦抱住，轴向力传不到指垫，见 gauge_fsm.wrench_touch）。 + ft_ref = self._wrench_axial() + ft_age = self._wrench_age() + ft_alive = wrench_live(ft_age) self.get_logger().info( f"S07 基线：切向 ({ref[0]:.1f}, {ref[1]:.1f})、法向 {self._fn_rest:.1f}" + f"；腕部轴向 {ft_ref if ft_ref is not None else float('nan'):+.2f}N" + f"（{'活' if ft_alive else '**没有数据**'}，话题 {self.ft_wrench_topic}）" ) z0 = float(Tee[2, 3]) @@ -1565,23 +1898,75 @@ class GaugeAssembly(AssemblyPipeline): z_prev = z_now if steps % 5 == 0: now_dbg = self._shear_axes() + ax_dbg = self._wrench_axial() self.get_logger().info( f"S07 已下压 {abs(z_now - z0) * 1000:.2f}mm" f"（{steps}/{budget} 步）切向 ({now_dbg[0]:.1f}, {now_dbg[1]:.1f})" + f" 腕部轴向 " + f"{ax_dbg - ft_ref if ax_dbg is not None else float('nan'):+.1f}N" ) + + # ---- 判据一（首选）：腕部 FT 轴向突变 -------------------------- + # 腕部 FT 在夹持摩擦的**上游**（工件→规→夹爪→法兰→腕部关节）， + # 所以工具在爪里打不打滑都测得准。触觉做不到这点： + # 轴向力要传过"指垫↔规"的摩擦面，上限只有 µ·N≈23N，而台肩落面 + # 一步就产生 25N —— 必然打滑，读数停在 0（而它是对的）。 + # 完整因果链见 gauge_fsm.wrench_touch 与 docs/GAUGE_ASSEMBLY.md §6。 + ft_now = self._wrench_axial() + if ft_alive and ft_now is not None and ft_ref is not None: + if wrench_touch(ft_now, ft_ref, self.cfg.contact_axial_delta_n): + self.get_logger().info( + f"S07 接触（腕部 FT）：轴向 {ft_now:+.2f}N 相对基线 " + f"{ft_ref:+.2f}N 超过 +{self.cfg.contact_axial_delta_n:.1f}N" + f"（下压 {abs(float(Tee[2, 3]) - z0) * 1000:.2f}mm）" + ) + # 必须先卸载再交给 S08 —— 判到接触的这一刻臂上顶着的是**出力上限** + # 那个量级的力（实测 200N+，与阈值无关），而规的轴向传力上限 + # 只有 µ·N≈73N。见 _relieve_after_contact 的完整说明。 + self._relieve_after_contact(q, Tee, "腕部 FT", ft_ref) + return Event.CONTACT_OK + elif steps == 1: + # 只报一次，别每步刷屏。降级到触觉兜底时阈值口径要按现场重新标定。 + self.get_logger().warn( + f"S07 腕部 FT 没有数据（话题 {self.ft_wrench_topic}，" + f"年龄 {ft_age if ft_age is not None else float('nan'):.1f}s）——" + "降级为只看触觉切向突变。\n" + " ⚠️ 触觉是**靠夹持摩擦**传力的，轴向力上限只有 µ·N（≈23N 量级），" + "台肩落面那一下可能传不过去、读数停在 0。\n" + " 真机上查：真机 bringup 无条件起 flexiv_robot_states_broadcaster，" + "话题是 //ft_sensor_wrench；收不到就是话题名/网络的问题。\n" + " 仿真上查：devices/sim/gazebo/config/wrist_ft.yaml 的 topic 与" + " arm.sim.sensors.xacro 的 wrist_ft_topic 必须一致（都是 " + "/sim/ft_sensor_wrench）。" + ) + try: fn, _fn_max, _shear, _contact = self._tac_fresh(self.tac_timeout) except Exception as exc: # noqa: BLE001 - self.get_logger().error(f"S07 触觉断流，无法判断接触：{exc}") - self._retract(q, Tee, dz_m=0.01) - return Event.TIMEOUT + # 腕部 FT 活着时，触觉断流不影响判接触（两个来源互相独立）。 + # 只有两个来源都没了才真的判不了。 + if not ft_alive: + self.get_logger().error( + f"S07 腕部 FT 与触觉**都没有**数据，无法判断接触：{exc}\n" + " 这不是阈值问题，是数据源问题：先让其中一个活过来。" + ) + self._retract(q, Tee, dz_m=0.01) + return Event.TIMEOUT + self.get_logger().warn( + f"S07 触觉断流（{exc}），但腕部 FT 活着，继续下压" + ) + fn, _fn_max, _shear, _contact = 0.0, 0.0, (0.0, 0.0), False + + # ---- 判据二（兜底）：触觉切向突变 ------------------------------ now = self._shear_axes() if tangential_jump(now, ref, self.cfg.contact_tangential_delta): self.get_logger().info( - f"S07 接触：切向 {now[0]:.1f}/{now[1]:.1f} 相对基线 " + f"S07 接触（触觉切向）：切向 {now[0]:.1f}/{now[1]:.1f} 相对基线 " f"{ref[0]:.1f}/{ref[1]:.1f} 超过 +{self.cfg.contact_tangential_delta:.0f}" f"（法向 {fn:.1f}，下压 {abs(float(Tee[2, 3]) - z0) * 1000:.2f}mm）" ) + # 与腕部 FT 那条同规矩：判到接触时臂上顶着的力远大于阈值，必须先卸载。 + self._relieve_after_contact(q, Tee, "触觉切向", ft_ref) return Event.CONTACT_OK if fn - self._fn_rest > self.cfg.contact_safe_delta: self.get_logger().error( @@ -1590,14 +1975,132 @@ class GaugeAssembly(AssemblyPipeline): ) self._retract(q, Tee, dz_m=0.01) return Event.FORCE_OVER_LIMIT + # 腕部 FT 的独立安全上限：它测的是**真实轴向力**，所以这里能给出 + # 一个物理量级的兜底（触觉那个 contact_safe_delta 是 au，量纲都对不上）。 + # 取触觉安全上限的同样口径：一旦超过就说明"压得太狠了"，与"接触了没" + # 无关 —— 接触判据应该早就返回了，能走到这里说明判据失灵。 + if ft_alive and ft_now is not None and ft_ref is not None: + if abs(ft_now - ft_ref) > self.cfg.contact_safe_delta: + self.get_logger().error( + f"S07 腕部轴向力超安全上限：{ft_now:+.2f}N - 基线 {ft_ref:+.2f}N " + f"= {abs(ft_now - ft_ref):.1f}N > +{self.cfg.contact_safe_delta:.0f}N，" + "抬起并中止。\n" + " 能走到这里说明接触判据没生效（那么大的力早该判到接触）——" + "先查 contact_axial_delta_n 是不是被调得太大了。" + ) + self._retract(q, Tee, dz_m=0.01) + return Event.FORCE_OVER_LIMIT self.get_logger().error( f"S07 下探 {max_depth_m * 1000:.1f}mm 都没触发接触判据。\n" - " 常见原因：① 切向阈值太大（先看上面打印的基线/实测值再调）；" - "② 孔位偏了，规擦着孔壁下去但没接触面；③ 触觉通道不对" + " 排查顺序（按物理可信度，别从阈值开始调）：\n" + " ① **先看上面每 5 步打印的「腕部轴向」**：它一直≈0 → 是数据源/几何问题，\n" + " 不是阈值问题（传感器没数据、悬停几何错、孔位偏了没碰到面）；\n" + " ② 腕部轴向有涨但没到阈值 → 才轮到调 contact_axial_delta_n；\n" + " ③ 只有触觉兜底时：轴向力要经夹持摩擦传递，上限 µ·N≈23N，\n" + " 台肩落面那一下可能传不过来 —— 这时调阈值是白费，要修的是夹持。" ) self._retract(q, Tee, dz_m=0.01) return Event.TIMEOUT + def _relieve_after_contact( + self, + q: np.ndarray, + Tee: np.ndarray, + why: str, + axial_ref: Optional[float] = None, + ) -> None: + """判到接触之后的**卸载回退**：把下压量放掉，再交给 S08。 + + 为什么必须有这一步（2026-09-24 用一整轮排查换来的，值得完整读）： + + 位置控制的臂撞上硬面时，**不会**在"刚碰到"的地方停住 —— 它会一直顶到 + 自己的出力上限。仿真实测：指令 0.5mm 的一步压在工件上，腕部 FT 直接跳到 + **+244N**，而臂这一步只走了 0.09mm（日志里那句"S07 单步未完全到位"）。 + 也就是说"判到接触"的那一刻，臂上是顶着一个**大得多的力**的， + 这个力跟 contact_axial_delta_n 没关系（阈值只决定"什么时候承认接触"）。 + + 而通止规是**靠夹持摩擦**抱住的：轴向上能传的力上限 = µ·N，两指合计 + 约 73N（µ=0.8、握力 46N）。244N 远超它 ⇒ 规被轴向力直接顶出爪外。 + 本次就是这样：S07 判到接触 → 进 S08 一转 → 规掉到地上 → 报 E1， + 而 E1 的日志只说"通止规掉了"，看不到"是顶着 244N 进去的"。 + + 所以这里回退 contact_relief_mm，把力放回基线附近；S08 只带着"规自身的 + 重量 + 一点点残余"进孔 —— 那才是螺纹/棘轮真正需要的预压。 + + 回退量刻意**不做闭环**（不"退到力小了为止"）：判据闭环会在"接触刚度很大" + 时来回震荡，而这里要的只是"把下压量放掉"。放不干净就报出来（见下）， + 让现场去改夹持（那是机械问题，不是参数问题）。 + """ + dz = float(self.cfg.contact_relief_mm) / 1000.0 + ft_before = self._wrench_axial() + if dz <= 0.0: + self.get_logger().warn( + f"S07 判到接触（{why}）但 contact_relief_mm=0 —— **不卸载**。\n" + " 位置控制的臂此刻顶着的力可能远大于判据阈值（实测 200N+），" + "而规靠夹持摩擦抱住、轴向上限只有 µ·N（≈73N）。" + "S08 一转就可能掉件（E1），且日志里看不到真因。" + ) + return + target = np.array([float(Tee[0, 3]), float(Tee[1, 3]), float(Tee[2, 3]) + dz]) + # ⚠️ 到位容差**必须远小于回退量**，不能写死一个与它同量级的常数。 + # 实测（2026-09-24）：回退 0.50mm 而 tol_m 也写 0.0005 ⇒ 目标 z 是 + # `Tee[2,3] + 0.0005`，浮点回减后 dist 会比 0.0005 略小 ⇒ `_goto` 的 + # 入口判据 `dist < tol_m` 当场成立、**一步没走就 return True**。 + # 于是"卸载"变成一次空操作，而下面的残余力判据比的是"回退前 vs 回退后" + # 两个一模一样的读数 ⇒ 报"残余 0.0N，已放干净"（**假通过**）。 + # 后果：S08 带着 70N 的轴向力去转 → 掉件(E1)，日志里只有"规掉了"。 + # 规则本身抽成 gauge_fsm.relief_tol_m 并有单测钉住（tol 必须 < 回退量）。 + tol = relief_tol_m(dz) + ok = self._goto( + q, Tee, target, verticalize=True, tol_m=tol, + tol_tilt=self.vert_tol_rad * 2.5, tag="S07_relief", + v=min(0.02, self.cfg.contact_speed_m_s), max_hops=3, + settle_s=max(0.1, self.settle3 * 0.5), min_dur=0.3, + )[0] + time.sleep(0.2) + ft_after = self._wrench_axial() + msg = ( + f"S07 接触后卸载：回退 {dz * 1000:.2f}mm，腕部轴向 " + f"{ft_before if ft_before is not None else float('nan'):+.1f}N → " + f"{ft_after if ft_after is not None else float('nan'):+.1f}N" + ) + if not ok: + self.get_logger().warn( + msg + "\n ⚠ 卸载回退**没走到位**（上面有 _goto 的告警）。" + "残余力判据因此不可信 —— 这一路压进去的力仍留在规上。" + ) + if ft_after is not None and ft_before is not None: + # 残余按**基线**（还没碰工件时的轴向力）算，不能按"回退前后之差"算： + # 回退本身要是没生效，前后读数相同 ⇒ 差值 0 ⇒ 假报"放干净"。 + # 基线是"完全没有接触"的状态，这才是"卸载干不干净"的参照。 + residual = ( + abs(float(ft_after) - float(axial_ref)) + if axial_ref is not None + else abs(float(ft_after) - float(ft_before)) + ) + ref_txt = ( + f"基线 {float(axial_ref):+.1f}N" if axial_ref is not None else "无基线" + ) + if residual > float(self.cfg.contact_hold_n): + self.get_logger().warn( + msg + f"（残余 {residual:.1f}N（相对{ref_txt}）" + f" > {self.cfg.contact_hold_n:.1f}N）\n" + " ⚠ 卸载没放干净：这一路压进去的力**超过了夹持摩擦能承受的量**" + "（上限 µ·N，两指合计 ≈73N）。\n" + " 接着 S08 一转，规会从爪里被推出去（现象是「通止规掉了」E1，" + "看不到真因）。\n" + " 两条真修法：① 降峰值 —— 调小 contact_step_mm（步长越小峰值越小）" + "或加大 contact_relief_mm；② 加握力 —— 改夹爪合拢力度/指垫摩擦。\n" + " **不要**去调 contact_axial_delta_n：它只管「什么时候承认接触」，" + "管不了「判到的时候已经顶了多大」。" + ) + else: + self.get_logger().info( + msg + f"（残余 {residual:.1f}N（相对{ref_txt}），已放干净）" + ) + else: + self.get_logger().info(msg + "（腕部 FT 没数据，无法确认卸载效果）") + def _baseline(self, samples: int) -> Tuple[float, float]: """静止基线的两路切向均值（顺手把法向基线记到 ``self._fn_rest``）。 @@ -1633,6 +2136,29 @@ class GaugeAssembly(AssemblyPipeline): # ===================================================================== # S08 旋转装配循环（棘轮） # ===================================================================== + def _log_regrip(self, tag: str) -> None: + """把"这一轮夹紧后的开口"打出来（含相对夹住时的塌陷量）。 + + 为什么每轮都要打（2026-09-24 实测）： + S08/S09 每轮都是"慢转 → 松爪 → 回转 → 夹紧"，而 `close` 在仿真里 + 是"位置控制走到全合 0mm" —— **真机走到力上限就停，仿真会一路挤进规里**。 + 只报一句"通止规掉了(E1)"时，人分不出"真掉了"和"被挤小了"。 + 把开口与塌陷量打出来一眼就能分辨： + · 塌陷量逐轮变大、开口还在十几毫米 → 是**被挤**（规还在指间） + · 开口塌到接近 0（两指合到底） → 是**真掉了** + """ + if self.grip is None: + return + ap = self.grip.settle(settle_s=1.0) # 服务返回时动作还没做完，等它停 + if ap is None: + return + gp = self.grip.grip_aperture() + col = self.grip.collapse_mm() + detail = "" + if gp is not None and col is not None: + detail = f"（夹住时 {gp:.2f}mm，塌陷 {col:+.2f}mm）" + self.get_logger().info(f"{tag}：开口 {ap:.2f}mm{detail}") + def _step_screw_in(self) -> Event: hole = self._hole_name() self._state(f"S08_screw_in_{hole}") @@ -1649,6 +2175,20 @@ class GaugeAssembly(AssemblyPipeline): f"停止判据「切向 >+{self.cfg.screw_shear_delta:.0f} " f"且法向 >+{self.cfg.screw_normal_delta:.0f}」" ) + # 步长 vs 关节行程：**在这里算一次**，不要等第 3 轮贴限位再炸。 + # 2026-09-24 实测：180° 的步长配 ±175° 的关节 7，第 3 轮 ABORT， + # 日志是一长串"贴限位 → 让余量 → 还是贴限位"，真因（配置）藏在最后。 + travel = ratchet_travel_ok( + self.cfg.screw_step_deg, self._wait_joint_limits(), RATCHET_LIMIT_MARGIN_DEG + ) + self.get_logger().info( + f"S08 行程核算：一步棘轮要 {travel.need_deg:.0f}°，" + f"关节7 行程 {travel.travel_deg:.0f}°" + f"（最大可用步长 {travel.max_step_deg:.0f}°）—— {'放得下' if travel.ok else '放不下'}" + ) + if not travel.ok: + self.get_logger().error(f"S08 配置自检不通过（先改配置，别硬跑）：{travel.why}") + return Event.ABORT for cycle in range(self.cfg.screw_max_cycles): q, Tee = self._wait_state(5.0) if q is None or Tee is None: @@ -1663,9 +2203,10 @@ class GaugeAssembly(AssemblyPipeline): # --- 四步原子动作，顺序固定（慢转 → 松爪 → 快回转 → 夹紧）--- # 慢转是**顺时针**：与 S01 的逆时针预绕相反，这样预绕行程就是 # 这一整段拧入的回转空间（S01 不预绕，转两圈就贴限位）。 - ev = self._rotate_joint_and_verify( + ev = self._ratchet_step( float(q[-1]) - self.cfg.screw_step_rad, tag=f"S08_slow_{cycle}", fsm_event=Event.SCREW_DONE, speed_deg_s=self.cfg.screw_speed_deg_s, + direction=-1, # 顺时针拧入（与 S01 的预绕相反） ) if ev is not Event.SCREW_DONE: return ev @@ -1678,15 +2219,17 @@ class GaugeAssembly(AssemblyPipeline): q, Tee = self._wait_state(3.0) if q is None: return Event.TIMEOUT - ev = self._rotate_joint_and_verify( + ev = self._ratchet_step( float(q[-1]) + self.cfg.screw_step_rad, tag=f"S08_back_{cycle}", fsm_event=Event.SCREW_DONE, speed_deg_s=self.cfg.screw_back_speed_deg_s, + direction=+1, # 回转：与慢转正好相反 ) if ev is not Event.SCREW_DONE: return ev if not self.grip.close(): self.get_logger().error("S08 夹紧失败 —— 下一轮慢转会打滑磨螺纹") return Event.ABORT + self._log_regrip(f"S08 第 {cycle + 1} 轮夹紧后") else: self.get_logger().error("S08 没有夹爪客户端，无法松爪/夹紧，中止") return Event.ABORT @@ -1715,9 +2258,17 @@ class GaugeAssembly(AssemblyPipeline): self._progress(f"{hole} 拧入到位（{cycle + 1} 轮）") return Event.SCREW_STOP if self.grip is not None and self.grip.dropped(): + ap = self.grip.aperture() + gp = self.grip.grip_aperture() self.get_logger().error( "S08 通止规掉了（E1）—— 回 S01 流程：张开爪、提示人工放好，" - "确认后继续（按看板：该孔记为完成，换下一个孔）" + "确认后继续（按看板：该孔记为完成，换下一个孔）\n" + f" 开口：夹住时 {gp if gp is None else f'{gp:.2f}mm'}" + f" → 现在 {ap if ap is None else f'{ap:.2f}mm'}" + f"（判据：塌陷 ≥ {GripperClient.DROP_COLLAPSE_MM:.1f}mm）\n" + " 分辨真掉还是被挤：开口塌到接近 0 = 两指合到底（真掉）；\n" + " 开口还有十几毫米、只是比夹住时小了几毫米 = 被**挤**" + "（仿真位置控制的「全合」不知道「挡住就停」，见 GripperClient.dropped）" ) self._e1_pending = True return Event.GAUGE_DROPPED @@ -1748,9 +2299,10 @@ class GaugeAssembly(AssemblyPipeline): if q is None: return Event.TIMEOUT # 反向：慢转是逆时针（与 S08 相反），空手回转是顺时针 - ev = self._rotate_joint_and_verify( + ev = self._ratchet_step( float(q[-1]) + self.cfg.screw_step_rad, tag=f"S09_slow_{cycle}", fsm_event=Event.UNSCREW_DONE, speed_deg_s=self.cfg.screw_speed_deg_s, + direction=+1, # 逆时针拧出 ) if ev is not Event.UNSCREW_DONE: return ev @@ -1762,17 +2314,64 @@ class GaugeAssembly(AssemblyPipeline): q, Tee = self._wait_state(3.0) if q is None: return Event.TIMEOUT - ev = self._rotate_joint_and_verify( + ev = self._ratchet_step( float(q[-1]) - self.cfg.screw_step_rad, tag=f"S09_back_{cycle}", fsm_event=Event.UNSCREW_DONE, speed_deg_s=self.cfg.screw_back_speed_deg_s, + direction=-1, # 回转：与慢转相反 ) if ev is not Event.UNSCREW_DONE: return ev if not self.grip.close(): return Event.ABORT + self._log_regrip(f"S09 第 {cycle + 1} 轮夹紧后") + self._lift_clear_of_hole(hole) self._progress(f"{hole} 已拧出") return Event.UNSCREW_DONE + def _lift_clear_of_hole(self, hole: str) -> None: + """拧出后**先竖直把规提出孔**，之后才允许任何横向动作。 + + **为什么必须有这一步**（2026-09-24 仿真实测，两次栽在同一个地方）： + 1. 仿真里**没有螺纹几何**：S09 的"拧出"只是让规在孔里转，规**仍然插在 + 孔里**（真机会沿螺纹退出来几个毫米，也没退出多少）。 + 2. 于是紧跟着的横向动作（S10 换孔 / S11 回 home）会把规从孔里**拽** + 出来 —— 实测抓取锁被拽断、规掉在台面上，而下游只表现为 + "下一孔 S07 下探 35mm 判不到接触"（工具上已经没规了）， + 日志完全指不到真因。 + 3. 真机也怕这个动作：螺纹刚脱开时横向别住最容易啃伤螺纹。标准做法就是 + **先竖直退出来，再走 XY**。 + + 高度提到**跟踪高**（``_track_z()`` = 孔悬停高 + 50mm）而不是写死毫米数： + 跟踪高是这条流程里"工具已离开工件"的既有定义（``_ensure_clearance`` + 也用它），拿它当判据不用再维护第二套口径。实测：不提出时规尖比工件面 + **低 30mm**，而 S10 的斜线只往上抬了 29mm —— 差 1mm 就是"拽出孔"和 + "干净退出"的分界，所以必须是**先后有序**的两个动作，不能靠斜线一起走。 + + 失败只告警（不失败）：这一刻孔里已经没有规要保护了，硬失败会把一次成功 + 的装配变成失败；后面的 S10/S11 仍会走到安全位。 + """ + try: + q, Tee = self._wait_state(3.0) + if q is None or Tee is None: + return + tz = self._track_z() + z = float(Tee[2, 3]) + if z >= tz - 0.008: + return + self.get_logger().info( + f"S09 {hole} 拧出后竖直提出：Z {z:.3f} → {tz:.3f}m" + "（先离开孔口，再做横向动作）" + ) + _, q2, _Tee2 = self._goto( + q, Tee, + np.array([float(Tee[0, 3]), float(Tee[1, 3]), tz]), + verticalize=True, tol_m=0.003, tol_tilt=self.vert_tol_rad * 2.0, + tag=f"S09_exit_{hole}", v=self.move_speed, max_hops=8, + hold_xy=True, settle_s=0.25, + ) + except Exception as exc: # noqa: BLE001 + self.get_logger().warn(f"S09 {hole} 提出孔口失败（继续走 S10/S11）：{exc}") + # ===================================================================== # S10 换孔 / S11 复位 # ===================================================================== diff --git a/src/arm_application/arm_application/gauge_config.py b/src/arm_application/arm_application/gauge_config.py index b8cfac6..5bc0bea 100644 --- a/src/arm_application/arm_application/gauge_config.py +++ b/src/arm_application/arm_application/gauge_config.py @@ -53,6 +53,9 @@ _UNIT_SUFFIX: Dict[str, str] = { "px": "_px", "pct": "_pct", "au": "_delta", + # 腕部 FT 的**真牛顿**。与 au 分开一个后缀是刻意的：两个口径混用不会报错， + # 只会"判据不触发"或"一碰就炸"，看起来像算法问题（见 site.yaml 的注释）。 + "N": "_n", } #: 显式无量纲的键（布尔、计数、比例、枚举）。**必须在这里列出来** —— @@ -99,6 +102,9 @@ _TYPES: Dict[str, type] = { "tool_tip_below_flange_m": float, "workpiece_surface_z_m": float, "contact_tangential_delta": float, + "contact_axial_delta_n": float, + "contact_relief_mm": float, + "contact_hold_n": float, "contact_safe_delta": float, "screw_step_deg": float, "screw_speed_deg_s": float, @@ -142,6 +148,12 @@ _UNITS: Dict[str, str] = { "tool_tip_below_flange_m": "m", "workpiece_surface_z_m": "m", "contact_tangential_delta": "au", + # 腕部 FT 是**真牛顿**（与触觉的 au 不是一个口径，所以键名也不带 taut 前缀） + "contact_axial_delta_n": "N", + # 卸载回退是**长度**（与 step_mm 同一把尺） + "contact_relief_mm": "mm", + # 残余轴向力是**牛顿**（腕部 FT 口径，不是触觉的 au） + "contact_hold_n": "N", "contact_safe_delta": "au", "screw_step_deg": "deg", "screw_speed_deg_s": "deg/s", @@ -209,9 +221,42 @@ DEFAULTS: Dict[str, Any] = { "tool_tip_below_flange_m": 0.243, "workpiece_surface_z_m": 0.0, # 0 = 未知，运行期用 stage2 实测孔位兜底 "contact_tangential_delta": 300.0, + # 腕部 FT 的轴向力阈值 [N]。**这是 S07 的首选判据**，触觉只是兜底。 + # + # 为什么默认是 8N 这么小：这是"接触了没有"的判据，不是"压到位了没有"。 + # 台肩一落面，接触刚度 5e4 N/m 意味着过压 0.1mm 就有 5N、0.2mm 有 10N —— + # 所以 8N 对应约 0.16mm 的过压，**在打滑之前**就能判到（打滑上限由 + # µ·N 决定：µ=0.25、N≈46N → 两指合计 ~23N ≈ 0.46mm）。留了 3 倍余量。 + # 现场标定法：空载慢慢压到台肩落面，看日志里"腕部轴向"从基线涨到多少， + # 取那个值的 1/3 作为阈值（要能区分"擦到孔壁"和"台肩落面"）。 + "contact_axial_delta_n": 8.0, + # 接触到之后的**卸载回退** [mm]。 + # + # 为什么必须有这一步（2026-09-24 一整轮排查换来的）： + # 位置控制的臂在"撞上硬面"时会把力顶到自己的**出力上限**才停住 —— + # 仿真实测：一步 0.5mm 的指令压在工件上，腕部 FT 读数直接跳到 **+244N**， + # 而臂这一步根本没走完（"实测只走了 +0.09mm"）。这不是仿真特有的： + # 接触刚度越高、步长越大，峰值力就越大，而位置环**不限制**它。 + # + # 规是**靠夹持摩擦**抱住的，轴向能传的力上限是 µ·N（两指合计 ~73N@µ=0.8）。 + # 244N 远超这个上限 ⇒ 规被轴向力**顶出爪外**（本次就是：S08 一转， + # 规直接掉到地上，报 E1）。所以"判到接触"之后不能就那样顶着， + # 必须先把下压量放掉，再交给 S08 拧入。 + # + # 0.5mm 的来历：判到接触时的**真实**过压量通常只有 0.0x mm（刚度 5e4 N/m + # 时，10N 对应 0.2mm），回退 0.5mm 足够把力放干净，而规尖仍在孔里 + # （通端穿板 18mm），不会脱出。 + "contact_relief_mm": 0.5, + # 卸载后允许的残余轴向力 [N]。 + # 超了不失败（可能只是回退量不够），但要**大声报**：它说明"夹持摩擦力不够 + # 承受这次接触"，接着 S08 一转就会掉件。调法只有两条 —— 降峰值（调小 + # contact_step_mm / contact_relief_mm）或加握力（改夹爪合拢力度/指垫）。 + "contact_hold_n": 5.0, "contact_safe_delta": 2000.0, # S08 - "screw_step_deg": 180.0, + # 步长必须满足 2×step + 2×余量 ≤ 关节7 行程（Rizon4/10 ±175° ⇒ step ≤ 167°）： + # 180° 会必然贴限位（棘轮一个来回要 360° > 350°），实测第 3 轮 ABORT。 + "screw_step_deg": 120.0, "screw_speed_deg_s": 30.0, "screw_back_speed_deg_s": 120.0, "screw_max_cycles": 12, @@ -259,6 +304,9 @@ class GaugeConfig: tool_tip_below_flange_m: float workpiece_surface_z_m: float contact_tangential_delta: float + contact_axial_delta_n: float + contact_relief_mm: float + contact_hold_n: float contact_safe_delta: float screw_step_deg: float screw_speed_deg_s: float @@ -310,8 +358,14 @@ class GaugeConfig: # 悬停高度**不在这里**：它取自 servo.hole_hover_z_m（stage2 那一层）， # 这里只印本段的容差 —— 一段配置里出现"另一个段的键"会让人改错地方。 f" S06 对位容差 {self.align_tol_mm:.1f}mm（悬停高取自 servo.hole_hover_z_m）", - f" S07 切向突变阈值 +{self.contact_tangential_delta:.0f}（两通道任一，现场标定），" + f" S07 腕部轴向突变阈值 +{self.contact_axial_delta_n:.0f}N（**首选判据**，" + "腕部 FT 在夹持摩擦的上游，工具打滑也测得准）," + f"切向突变阈值 +{self.contact_tangential_delta:.0f}au（触觉兜底，两通道任一，现场标定），" f"安全上限 +{self.contact_safe_delta:.0f}，最多下探 {self.contact_max_depth_mm:.0f}mm", + f" S07 接触后卸载：回退 {self.contact_relief_mm:.2f}mm，" + f"残余轴向力要求 ≤{self.contact_hold_n:.1f}N" + "（位置控制的臂撞上硬面会把力顶到出力上限再停住，实测能到 200N+；" + "规靠夹持摩擦抱住，轴向上限只有 µ·N≈73N，顶着不放 S08 一转就掉件）", f" S07 工具几何：尖端在法兰下方 {self.tool_tip_below_flange_m * 1000:.1f}mm，" f"工件上表面 z={self.workpiece_surface_z_m:.3f}m" "（启动时会按这两个值 + servo.hole_hover_z_m 算「悬停时工具到底入没入工件」，" @@ -457,6 +511,20 @@ def _sanity(cfg: GaugeConfig) -> None: ) if cfg.snap_tol_px <= 0.0: raise GaugeConfigError(f"gauge_assembly.snap_tol_px 必须为正，收到 {cfg.snap_tol_px}") + if cfg.contact_relief_mm < 0.0: + raise GaugeConfigError( + f"gauge_assembly.contact_relief_mm 不能为负（0 = 接触到之后不卸载），" + f"收到 {cfg.contact_relief_mm}。\n" + " 它为什么值得一条检查：位置控制的臂撞上硬面会把轴向下压顶到**出力上限**" + "才停住（实测 200N+），而规靠夹持摩擦抱住、轴向上限只有 µ·N（≈73N）。" + "卸载这一步省掉，S08 一转就掉件（报 E1）—— 而那时的日志只会说" + "「通止规掉了」，看不到真因。" + ) + if cfg.contact_hold_n <= 0.0: + raise GaugeConfigError( + f"gauge_assembly.contact_hold_n 必须为正（卸载后的残余轴向力上限 [N]），" + f"收到 {cfg.contact_hold_n}。0 会让「卸载干不干净」永远判不过。" + ) if cfg.snap_tol_mm <= 0.0: raise GaugeConfigError(f"gauge_assembly.snap_tol_mm 必须为正，收到 {cfg.snap_tol_mm}") if cfg.snap_stable_feature not in SNAP_STABLE_FEATURES: diff --git a/src/arm_application/arm_application/gauge_fsm.py b/src/arm_application/arm_application/gauge_fsm.py index 2218e82..7081429 100644 --- a/src/arm_application/arm_application/gauge_fsm.py +++ b/src/arm_application/arm_application/gauge_fsm.py @@ -358,6 +358,54 @@ def tangential_jump( return any(abs(n - r) >= delta for n, r in zip(shear_now, shear_ref)) +def wrench_touch( + axial_now: float, axial_ref: float, delta: float +) -> bool: + """S07（**首选判据**）：腕部 FT 的轴向力相对基线突变 → 判为接触到工件。 + + 为什么这是首选、而不是手指触觉（2026-09-24 用一整轮排查换来的结论）： + + 通止规是**靠夹持摩擦抱住**的工具。轴向力要传到手指，必须过"指垫↔规"的 + 摩擦面，而摩擦能传的上限是 µ·N： + 夹持段 µ=0.25、实测握力 N≈46N → 单指 ~11.5N、两指 ~23N + 可台肩落面那一下，接触刚度 5e4 N/m 意味着**过压 0.5mm 就是 25N** —— + 一步的步进量就足以让规在爪里打滑。**于是手指触觉物理上读不到那个力** + （读数停在 0，而它是对的），阈值怎么调都不对：调小被噪声骗，调大永远 + 达不到。当时确实是照"阈值调高了"去改的，白费一轮。 + + 腕部 FT 在这条力路的**上游**：工件→规→夹爪→法兰→腕部关节。 + 工具打不打滑都要经过它，所以它测得到真实的轴向力。 + + 判据用**绝对增量**而不是带符号压入量，理由与 ``tangential_jump`` 相同： + 传感器坐标系的正负号方向取决于安装，赌错了就是"整段流程静默失效" + （下发正常、日志正常、就是停不住）。取绝对值只会更早停一点点， + 这个方向的错是安全的 —— 与 ``stage2_locate_node._contact_detected`` + 的既有口径也一致（那里也是 ``abs(fz - baseline)``）。 + + :param axial_now: 当前轴向力读数 [N]。 + :param axial_ref: 静止（还没碰工件）时的轴向基线 [N]。 + :param delta: 触发阈值（相对基线的增量）[N]。 + """ + return abs(float(axial_now) - float(axial_ref)) >= float(delta) + + +#: S07 腕部 FT 判据需要多"新"的数据才算活着 [s]。 +#: 真机 broadcaster 与仿真传感器都是 100Hz，0.5s 相当于容许掉 50 帧 —— +#: 只用来排除"根本没在发"，不是用来做实时性判据的。 +WRENCH_MAX_AGE_S: float = 0.5 + + +def wrench_live(age_s: Optional[float]) -> bool: + """腕部 FT 是否有活的数据（够新）。 + + ``age_s`` 为 None 表示**一帧都没收到过**（话题名写错、传感器没起来）。 + 注意"收到过但数值为 0"**是活的** —— 静止时轴向力本来就接近 0。 + 这个区别很关键：拿数值大小判活，会让"安静的传感器"被误判成掉线， + 而那正是 2026-09-24 在触觉上踩过的同一个坑（见 stage3 的 ``live_tactile``）。 + """ + return age_s is not None and float(age_s) <= WRENCH_MAX_AGE_S + + #: S07 单步下压给 ``_goto`` 的到位容差 = 步长的这个比例。 #: **必须 < 1，而且只能小不能大**：``_goto`` 的语义是"已经落在容差里就直接返回、 #: 一点都不动"。容差 ≥ 步长时，每一次"往下走 0.5mm"的请求都被判成"已在容差内" @@ -368,10 +416,34 @@ DESCEND_TOL_FRAC: float = 0.4 DESCEND_TOL_FLOOR_M: float = 2.0e-5 #: 判定"这一步真的往下走了"的位移下限，占步长的比例。 DESCEND_PROGRESS_FRAC: float = 0.2 +#: 卸载回退的到位容差占**回退量**的比例（见 :func:`relief_tol_m`）。 +RELIEF_TOL_FRAC: float = 0.2 #: 允许连续多少步毫无位移后中止（防"没接触也一直转"的静默自旋）。 DESCEND_STALL_STEPS: int = 5 +def relief_tol_m(dz_m: float) -> float: + """S07 判到接触后**卸载回退**该给 ``_goto`` 的到位容差 [m]。 + + 为什么单独立一条（2026-09-24 实测踩到，代价是一轮假通过）： + + 回退量 0.50mm，而 ``_goto`` 的容差被写死成 0.0005 m（**同量级**）。 + 目标 z 是 ``Tee[2,3] + 0.0005``，浮点回减后 ``dist`` 会比 0.0005 略小 + ⇒ ``_goto`` 的入口判据 ``dist < tol_m`` 当场成立 ⇒ **一步没走就返回 True**。 + 于是"卸载"变成空操作，而残余力判据比的是"回退前 vs 回退后"两个**相同**的 + 读数 ⇒ 报"残余 0.0N，已放干净"（假通过）。 + 接着 S08 带着 ~70N 的轴向力去转（夹持摩擦上限 µ·N 两指合计 ≈73N） + → 掉件(E1)，而 E1 的日志只说"规掉了"。 + + 返回 ``回退量的 20%``，再夹到 ``[DESCEND_TOL_FLOOR_M, 回退量的一半]`` —— + 后者保证**容差永远严格小于回退量**，`_goto` 不可能在入口就判"已到位"。 + """ + dz = float(dz_m) + if dz <= 0.0: + raise ValueError(f"卸载回退量必须为正，收到 {dz_m}") + return min(max(DESCEND_TOL_FLOOR_M, dz * RELIEF_TOL_FRAC), dz * 0.5) + + def descend_tol_m(step_m: float) -> float: """S07 单步下压该给 ``_goto`` 的到位容差 [m]。 @@ -441,6 +513,181 @@ def tool_clearance_m( return tip_z - float(surface_z_m) +def hover_z_for_tool_clearance( + surface_z_m: float, + tool_tip_below_flange_m: float, + clearance_m: float, +) -> float: + """让工具尖端停在工件面上方 ``clearance_m`` 所需的**法兰**高度 [m]。 + + 就是 ``tool_clearance_m`` 的逆运算 —— 写成一个函数是为了让"该用多高"这个 + 结论只有一个来源（下面 ``hover_z_guard`` 与调用方都用它，不各推一遍）。 + """ + return float(surface_z_m) + float(tool_tip_below_flange_m) + float(clearance_m) + + +def hover_z_guard( + hover_z_m: float, + surface_z_m: float, + tool_tip_below_flange_m: float, + want_clearance_m: float, +) -> Tuple[float, str]: + """二阶段悬停高度的自检 → ``(该用的悬停高度, 说明; 不用改时说明是空串)``。 + + **为什么必须有这条**（2026-09-24 实测栽在这上面）： + + 悬停高度是**按相机**定的（腕部相机要离工件面 ~0.17m 才看得清孔）， + 而工具（夹爪 + 通止规）可能比相机长得多 —— 仿真里实测 + ``tool_tip_below_flange_m=0.245``，而悬停算的是 + ``0.412 + 0.17 + 0.05 = 0.632`` ⇒ 工具尖端在工件上表面**下面 25mm**。 + + 后果不在 S07（S07 有自己的几何自检），而在**更早的 S05**： + "悬停"这一步本身就把工具捅进工件里了，于是 S05 的横向对准等于 + 拿规尖去拨工件 —— 实测把工件从 x=0.520 拖到 0.4284（**92mm**）。 + 而真值孔位是按工件的**实测**位姿算的（``sim_live_pose``，这是对的）， + 工件一走，孔位就跟着工具走 ⇒ 残差恒定在 13mm、迭代 9 次永不收敛 + （日志：``[fine 3..8] 残差=13.25/13.19/…/11.79mm``）。 + 现象看起来像"手眼标定坏了 / 检测不认孔"。 + + 真机同参数同结果：这不是仿真专有，只要"悬停按相机定、工具比相机长"就会这样。 + + 判据取"明显低于目标间隙"而不是"一毫米都不许低"：配置的悬停与实测的 + 工件面 z 都有几毫米的不确定度，拿 0 当界会把正常配置判成错的。 + """ + clear_m = tool_clearance_m(hover_z_m, tool_tip_below_flange_m, surface_z_m) + if clear_m >= float(want_clearance_m) - 0.001: + return float(hover_z_m), "" + need_z = hover_z_for_tool_clearance(surface_z_m, tool_tip_below_flange_m, want_clearance_m) + why = ( + f"悬停高度会让工具尖端钻进工件：法兰 {float(hover_z_m):.4f} − 工具长 " + f"{float(tool_tip_below_flange_m) * 1000:.1f}mm = 尖端 " + f"{float(hover_z_m) - float(tool_tip_below_flange_m):.4f}，工件上表面 " + f"{float(surface_z_m):.4f} → 尖端在面{'下' if clear_m < 0 else '上仅'} " + f"{abs(clear_m) * 1000:.1f}mm（要求 ≥ {float(want_clearance_m) * 1000:.1f}mm）。\n" + f" 悬停高度通常是按**相机**视距定的，而工具比相机长 —— 两者冲突时必须" + f"先保证不碰工件。\n" + f" 已按几何抬到 {need_z:.4f}（= {float(surface_z_m):.3f} 上表面 + " + f"{float(tool_tip_below_flange_m) * 1000:.0f}mm 工具 + " + f"{float(want_clearance_m) * 1000:.1f}mm 间隙）。\n" + f" 要固定这个值：改 site.yaml 的 robots[].servo.hole_hover_z_m = " + f"{need_z:.3f}（以及同组件的 ee_z_m ≥ 它 +0.05）。" + ) + return need_z, why + + +#: S05 精对位时判"工件被工具推走"的两级阈值 [m]。 +# +# 为什么需要这条判据（2026-09-24 又栽了一次，见下面 drag_verdict 的说明）： +# S05 的对准是"看着孔走"的闭环，而孔位是**工件上**的。工具一旦碰到工件， +# 闭环追的就不再是孔，而是"自己推着走的工件" —— 残差恒定（本轮 11mm）、 +# 迭代用满，日志报的是"检查手眼/相机安装"，与真因（工具比相机长、 +# 悬停时已经插进工件）差着十万八千里。 +# 取 3mm：工件是刚体，两拍之间它整体挪动 >3mm 只可能是被推/被撞（正常加工 +# 过程里它一动不动）。同一对孔的两拍测量噪声在 0.2mm 量级，离阈值很远。 +SNAP_DRAG_TOL_M: float = 0.003 +#: 单拍就这么大 ⇒ 不必等第二拍确认（刚体不会自己"挪回去"）。 +SNAP_DRAG_HARD_M: float = 0.006 + + +#: "已经到位"的容差：避免 ±1e-9 的浮点噪声变成"再转一整圈"。 +_RATCHET_FULL_TURN_EPS: float = 1e-9 + + +def ratchet_delta_rad(target_rad: float, q_now_rad: float, direction: int = 0) -> float: + """"目标角 − 当前角"，折成**一次**能转到的增量 [rad]。 + + **为什么要有这个函数**（2026-09-24 实测：棘轮把整条流程 ABORT 在 S09 第一步）： + + S08/S09 给棘轮的是**关节 7 的目标角（绝对值）**（``q[-1] ± step``）， + 而棘轮此前把它当**增量**用。于是"关节在 +158.9°、目标 +338.9°"被算成 + "要转 338.9°"（实际增量只有 180°）⇒ 误判行程不够 ⇒ 让余量那一步算到 + 限位外面（``158.9 − 338.9 = −180°``，比 −175° 还低）⇒ 报 + "期望 −338.9°、实测 +26.0°，偏差过大" ⇒ ABORT。现场什么坏事都没发生。 + + 差值必须折一圈：目标常常**故意**超出限位（这正是要"让余量"的原因）， + ``+338.9°`` 与 ``−21.1°`` 是同一个关节位置。 + + **为什么还要 direction**（2026-09-24 又一次实测：S08 第 3 轮 ABORT）： + + 不给方向时这里取"最短的那条路"，而名义步长**正好是 180°** 时， + 哪条路更短取决于 0.1° 级的读数差：调用方按自己读到的 ``q`` 算出目标 + ``−18.0°``，棘轮再用**稍晚一点**读到的 ``q=+162.1°`` 去算差值， + ``−180.1°`` 折一圈就成了 ``+179.9°`` —— 方向**翻了个个儿**。 + 于是棘轮朝螺纹的反方向走，接着贴限位、让余量、重试，净位移 0 ⇒ ABORT。 + + 棘轮的转向是**工艺意图**（S08 顺时针拧入、S09 逆时针拧出），不能交给 + "谁更短"这种数值巧合决定，所以由调用方明确给出 ``direction``。 + + 注：``direction=0``（默认）保留旧的"最短路径"语义，给"目标本来就明确 + 在某一侧"的调用方用；棘轮的四个原子步一律显式给方向。 + + :param target_rad: 目标角（绝对值，可以是限位外的角度）。 + :param q_now_rad: 当前关节角。 + :param direction: 想要的转向：``+1`` 正转、``-1`` 反转、``0`` 取最短路径。 + :returns: 增量 [rad]。``direction=0`` 时落在 ``(−π, π]``；给了方向时落在 + ``[0, 2π)``（正转）或 ``(−2π, 0]``（反转）——**一定**沿指定方向， + 且是能落到目标角的最小那一步。**同一个目标、同一个当前位置**只会有一个 + 增量 —— 棘轮的"让余量"与"重试"都按它算，两次不会互相矛盾。 + """ + base = float(target_rad) - float(q_now_rad) + two_pi = 2.0 * math.pi + if direction == 0: + return math.remainder(base, two_pi) + # 先折到 [0, 2π)，再按想要的转向取那一侧（与目标角差正好一整圈，是同一个位置）。 + pos = math.fmod(base, two_pi) + if pos < 0.0: + pos += two_pi + if pos >= two_pi - _RATCHET_FULL_TURN_EPS: + pos = 0.0 # 已经（几乎）在目标上：不要为了凑方向白转一整圈 + if direction > 0: + return pos + return pos - two_pi if pos > 0.0 else 0.0 + + + +def drag_verdict( + mid_now: Tuple[float, float], + mid_prev: Optional[Tuple[float, float]], + mid_prev2: Optional[Tuple[float, float]] = None, + *, + tol_m: float = SNAP_DRAG_TOL_M, + hard_m: float = SNAP_DRAG_HARD_M, +) -> Tuple[bool, float]: + """两孔中点是否在两拍之间"整体挪动" → ``(是否判定为被推走, 本拍位移 m)``。 + + **为什么看"中点"而不是看目标孔**：目标孔是**追**的对象，工具一动它就该动 + （对准本意如此），看它分不出"正常对准"和"推着工件走"。而两孔**中点**在 + 对准过程中不该动 —— 它只有工件整体平移时才会动。所以中点位移干净地对应 + "工件被搬动了"。 + + **为什么看"两孔中点"而不是单孔**：单孔在相机里会因分辨率/遮挡抖动，且 + 目标孔本来就在被逼近；中点是两者的平均，噪声更小、语义更明确。 + + :param mid_now: 本拍两孔中点 (x, y)[m]（基座系）。 + :param mid_prev: 上一拍中点；``None`` = 还没有上一拍（第一拍不判）。 + :param mid_prev2: 上上拍中点。用来确认"位移是持续的"（单拍可能是误检）。 + :returns: ``(判定, 本拍位移 m)``。 + """ + if mid_prev is None: + return False, 0.0 + dx = float(mid_now[0]) - float(mid_prev[0]) + dy = float(mid_now[1]) - float(mid_prev[1]) + d1 = math.hypot(dx, dy) + if d1 >= float(hard_m): + return True, d1 + if d1 < float(tol_m) or mid_prev2 is None: + return False, d1 + px = float(mid_prev[0]) - float(mid_prev2[0]) + py = float(mid_prev[1]) - float(mid_prev2[1]) + d2 = math.hypot(px, py) + # 第二拍确认：上一拍也挪了（≥ 阈值的一半），且方向一致（点积 > 0）。 + # 为什么允许方向只"大致一致"：工件被推着走时前后两拍的推动量本来就不同 + # （工件一让开，工具就少推一点），拿夹角严格相等会漏判。 + if d2 >= float(tol_m) * 0.5 and (dx * px + dy * py) > 0.0: + return True, d1 + return False, d1 + + def descend_needed_m( flange_z_m: float, tool_tip_below_flange_m: float, @@ -512,6 +759,115 @@ def ratchet_cycles(total_deg: float, step_deg: float) -> int: return int(math.ceil(total_deg / step_deg)) +@dataclass(frozen=True) +class RatchetTravel: + """棘轮步长与关节行程的**可用性核算**（S08 开头自检，别等到第 3 轮才炸）。""" + + ok: bool + step_deg: float # 配置的一步转角 + travel_deg: float # 关节行程（上限 − 下限） + need_deg: float # 一步棘轮真正需要的行程 + max_step_deg: float # 行程允许的最大步长 + why: str # 不 ok 时给人看的说明（ok 时为空串） + + +def ratchet_travel_ok( + step_deg: float, limits_rad: Optional[Tuple[float, float]], margin_deg: float +) -> RatchetTravel: + """一步棘轮 ``step_deg`` 放得进关节行程吗？ + + **为什么需要**：2026-09-24 实测，``screw_step_deg=180`` 配 Rizon4 的 + 关节 7（±175°，行程 350°）在第 3 轮 ABORT —— 慢转那一步要转 180°、 + 回转那一步还要 180°，两次加起来 360° > 350°，关节**必然**有一头贴限位； + 更糟的是"让余量"也腾不出地方（要腾就得往反向再让一整步，还是 360°）。 + 日志里那一长串"贴限位 → 让余量 → 还是贴限位"就是这么来的。 + + 判据取 ``2×step + 2×margin ≤ 行程``： + - 2×step：棘轮一个来回（慢转 + 回转）要的行程； + - 2×margin：两头都要留余量 —— 贴死限位再让余量时**一点空间都没有**， + 而实测每轮回转还会比指令少走 4~5°（速度/加减速限制）， + 这点亏欠要靠余量吃，否则 7~8 轮后累计偏出限位、再也转不回来。 + + :param limits_rad: ``(lower, upper)`` [rad]；``None`` = 拿不到限位，放行 + （返回 ``ok=True``，理由写清楚：真机靠驱动器报 JOINT_LIMIT 兜底）。 + """ + need = 2.0 * float(step_deg) + 2.0 * float(margin_deg) + if limits_rad is None: + return RatchetTravel( + ok=True, step_deg=float(step_deg), travel_deg=float("inf"), + need_deg=need, max_step_deg=float("inf"), + why="", + ) + travel = math.degrees(float(limits_rad[1]) - float(limits_rad[0])) + max_step = 0.5 * (travel - 2.0 * float(margin_deg)) + if need <= travel: + return RatchetTravel( + ok=True, step_deg=float(step_deg), travel_deg=travel, + need_deg=need, max_step_deg=max_step, why="", + ) + # 给个能直接照着改的步长：往整十度取整（现场习惯填 120/150/180）， + # 但不许把步长压到 30° 以下 —— 那样拧进/拧出的轮数会多到失控。 + suggest = max(30.0, math.floor(max_step / 10.0) * 10.0) + why = ( + f"棘轮一步 {step_deg:.0f}° 放不进关节 7 的行程：一步棘轮要 " + f"2×{step_deg:.0f}° + 2×{margin_deg:.0f}°(余量) = {need:.0f}°，" + f"而行程只有 {travel:.0f}°（上限 {math.degrees(limits_rad[1]):.0f}° − " + f"下限 {math.degrees(limits_rad[0]):.0f}°）。\n" + f" 改法：site.yaml 的 gauge_assembly.screw_step_deg 改到 " + f"{max_step:.0f}° 以内（例如 {suggest:.0f}°），并保证 " + f"screw_max_cycles × screw_step_deg 仍是 **360° 的整数倍**（" + f"累计必须转整数圈，否则规停在螺纹中间、S09 也就少拧出几分之一圈）。" + ) + return RatchetTravel( + ok=False, step_deg=float(step_deg), travel_deg=travel, + need_deg=need, max_step_deg=max_step, why=why, + ) + + +#: ``rotation_verdict`` 的结论。 +ROT_OK = "ok" +ROT_LIMIT = "limit" # 转了一部分、方向对、但没走完 ⇒ 最可能是贴到关节限位 +ROT_AXIS = "axis" # 转轴与工具 Z 不共线 ⇒ 以「度」为单位的一切动作都不成立 +ROT_SHORT = "short" # 其它偏差（方向不对/过冲）⇒ 真有问题，该停 + + +def rotation_verdict( + delta_deg: float, + got_deg: float, + mis_deg: float, + tol_deg: float, +) -> str: + """绕工具 Z 转 ``delta_deg``、实测转 ``got_deg`` 的验收判据。 + + **纯函数**：这条判据 2026-09-24 把整条 S09 判死过一次 + （"期望 +180.0°、实测 +16.4°，偏差过大" → ABORT），而真因只是 + **关节7 的行程被前面的棘轮回转顶到了上限**（±175°，而棘轮一步就要 180°）。 + 判错两个方向都贵： + * 把"贴限位"判成"short" ⇒ 白扔一整轮（还可能把规撞歪）； + * 把"真有问题"判成"limit" ⇒ 该停的没停，继续拧。 + + 还有一条**曾经漏判**的：只比绝对值时，"让它 +180、它 -170"会算成 + 10° 误差、落在 10% 容差里 ⇒ 判成功，而实际上一个孔都没拧。所以方向必须比。 + 所以它必须能脱离节点被钉住。 + """ + if abs(got_deg) < 3.0 and abs(delta_deg) > 10.0: + return ROT_LIMIT # 几乎没动：与下面的"只走了一部分"同因不同余量 + if mis_deg > tol_deg: + return ROT_AXIS + if abs(got_deg) >= 3.0 and abs(delta_deg) >= 3.0 and got_deg * delta_deg < 0: + # 方向反了。这条以前**漏判**：老判据只比绝对值 + # （``abs(abs(got) - abs(delta))``），所以"让它 +180、它 -170"会算 10° 误差 + # 落在 10% 容差里 → 判成功，而实际上一个孔都没拧。 + return ROT_SHORT + slack = max(tol_deg, 0.1 * abs(delta_deg)) + short = abs(delta_deg) - abs(got_deg) + if short > slack: + return ROT_LIMIT # 方向对、只是没走完 ⇒ 被硬止点截住 + if short < -slack: + return ROT_SHORT # 过冲：转多了，同样不该当成到位 + return ROT_OK + + def is_joint_limit_error(text: str) -> bool: """S01：把驱动/规划报的字符串认成"撞到关节极限"。 diff --git a/src/arm_application/arm_application/stage3_search_node.py b/src/arm_application/arm_application/stage3_search_node.py index 28867dc..1b7c119 100644 --- a/src/arm_application/arm_application/stage3_search_node.py +++ b/src/arm_application/arm_application/stage3_search_node.py @@ -247,7 +247,9 @@ class Stage3Search(Stage2Locate): sn_us = self.prefix.rstrip("_").replace("-", "_") self.ft_topic = str(s3.get("ft_topic") or f"/{sn_us}/flexiv_robot_states") if self.ft_watchdog and RobotStates is not None: - self.create_subscription(RobotStates, self.ft_topic, self._on_ft, 10) + self.create_subscription( + RobotStates, self.ft_topic, self._on_robot_states, 10 + ) self._state_pub = self.create_publisher(String, "/stage3/state", 10) self._result_pub = self.create_publisher(String, "/stage3/result", 10) @@ -271,7 +273,18 @@ class Stage3Search(Stage2Locate): def _on_tac_r(self, msg: Any) -> None: self._tac_r = (time.monotonic(), msg) - def _on_ft(self, msg: Any) -> None: + def _on_robot_states(self, msg: Any) -> None: + """缓存 ``flexiv_msgs/RobotStates``（腕部力看门狗的输入）。 + + ⚠️ 这个方法名**必须**与 ``Stage2Locate`` 里那个腕部 FT 的回调区分开。 + 2026-09-24 踩到：两者原先都叫 ``_on_ft``，子类的定义**静默覆盖**了父类的， + 而两个订阅都在 ``__init__`` 里建好了 —— 于是： + Stage2Locate 建的 WrenchStamped 订阅 → 实际进了这个 RobotStates 缓存； + ``_ft_fz`` 永远不写 → S07 打"腕部 FT 没有数据"（而话题数据是好的）。 + 真机上更阴：两个话题都存在，``self._ft`` 里 RobotStates 与 WrenchStamped + 交替被覆盖 → ``_ft_mag()`` 时有时无 → 看门狗时灵时不灵。 + 这类"同名覆盖 + 不报错"的坑，只要名字区分开就根除。 + """ self._ft = (time.monotonic(), msg) def _tac_fresh(self, timeout_s: float = 1.0) -> Tuple[float, float, float, bool]: diff --git a/src/arm_application/test/test_gauge_config.py b/src/arm_application/test/test_gauge_config.py index 27d49c9..a1f7792 100644 --- a/src/arm_application/test/test_gauge_config.py +++ b/src/arm_application/test/test_gauge_config.py @@ -27,7 +27,11 @@ from arm_application.gauge_config import ( missing_entries, unit_violations, ) -from arm_application.gauge_fsm import descend_iter_budget, descend_tol_m +from arm_application.gauge_fsm import ( + descend_iter_budget, + descend_tol_m, + ratchet_travel_ok, +) # ============================================================================= @@ -185,7 +189,7 @@ def test_describe_prints_effective_values_and_marks_calibration_needed(): text = "\n".join(cfg.describe()) # 实际生效的值（不是"我读了 site.yaml"） assert "S01 预绕 150.0°" in text - assert "S08 棘轮 180°×12 轮 = 累计 2160°" in text + assert "S08 棘轮 120°×12 轮 = 累计 1440°" in text # 必须提示这些阈值要现场标定，否则默认值会被当成"调好的参数" assert "现场标定" in text # 稳定判据的口径要写出来，避免下次有人改成"相邻帧之差" @@ -265,3 +269,50 @@ def test_shipped_site_yaml_s07_step_cannot_deadlock(): assert descend_iter_budget(depth_m, step_m) >= depth_m / step_m # 顺带钉住"出厂配置的派生值印得出来"（describe 里会打印容差） assert "S07" in "\n".join(cfg.describe()) + + +# ============================================================================= +# 出厂配置的 S08：棘轮步长必须放得进**支持的所有臂**的关节7 行程 +# ============================================================================= +def test_shipped_site_yaml_ratchet_step_fits_every_supported_arm(): + """**回归锚点**（2026-09-24 在仿真里踩到：S08 第 3 轮 ABORT）。 + + `screw_step_deg: 180` 配关节 7 的 ±175°（行程 350°）时，棘轮一个来回要 + 2×180°=360° > 350° —— 关节**必然**有一头贴限位，"让余量"也腾不出空间 + （腾一步还是 360°）。日志表现是一长串"贴限位 → 让余量 → 还是贴限位"， + 真因（配置）藏在最后一行，白烧 3 轮节拍。 + + 这条测试盯**出厂 site.yaml 的真实取值**，并且拿 `flexiv_description` 里 + **每台支持臂**的 joint7 限位去算（不是写死 350°）：以后加一台行程更小的 + 臂，这里会自动报红。余量取 `gauge_assembly_node.RATCHET_LIMIT_MARGIN_DEG` + 的同一个口径 —— 两处不一致时，测试会红，而不是现场崩。 + """ + import math + + import yaml + + root = Path(__file__).resolve().parents[3] + doc = yaml.safe_load((root / "config" / "site.yaml").read_text(encoding="utf-8")) + step = float(doc["gauge_assembly"]["screw_step_deg"]) + + margin = 8.0 # 与 gauge_assembly_node.RATCHET_LIMIT_MARGIN_DEG 同口径 + from arm_application.gauge_assembly_node import RATCHET_LIMIT_MARGIN_DEG + + assert margin == RATCHET_LIMIT_MARGIN_DEG, "余量口径变了，测试要跟着改" + + limit_files = sorted((root / "src" / "flexiv_description" / "config").glob("*/joint_limits.yaml")) + assert limit_files, "找不到任何臂的 joint_limits.yaml —— 路径变了？" + for path in limit_files: + doc_l = yaml.safe_load(path.read_text(encoding="utf-8")) or {} + joint7 = (doc_l.get("joint_limits") or {}).get("joint7") or doc_l.get("joint7") + assert joint7, f"{path} 里没有 joint7 限位" + limits = (float(joint7["lower"]), float(joint7["upper"])) + verdict = ratchet_travel_ok(step, limits, margin) + assert verdict.ok, f"{path.name}: 出厂配置的棘轮步长放不下 —— {verdict.why}" + assert verdict.max_step_deg > 0.0 + + # 累计转角必须是整数圈：否则规停在螺纹中间，S09 拧出也少转几分之一圈 + cycles = int(doc["gauge_assembly"]["screw_max_cycles"]) + assert math.isclose(step * cycles % 360.0, 0.0, abs_tol=1e-9), ( + f"screw_step_deg×screw_max_cycles = {step * cycles}° 不是 360° 的整数倍" + ) diff --git a/src/arm_application/test/test_gauge_exit_lift.py b/src/arm_application/test/test_gauge_exit_lift.py new file mode 100644 index 0000000..bb276c7 --- /dev/null +++ b/src/arm_application/test/test_gauge_exit_lift.py @@ -0,0 +1,182 @@ +"""S09 拧出后**先竖直提出孔口**：结构上有序，行为上提对高度。 + +为什么单独钉住（2026-09-24 仿真实测，两次栽在同一个地方） +-------------------------------------------------------- +仿真里**没有螺纹几何**：S09 的"拧出"只让规在孔里转，规**仍然插在孔里** +（真机也只是沿螺纹退回几个毫米）。于是紧跟着的横向动作 —— S10 换孔 / S11 回 home +—— 会把规从孔里**拽**出来：实测抓取锁被拽断、规掉在台面上。 + +最坑的是**它不在这里报错**：两分钟后下游只表现为"下一孔 S07 下探 35mm 判不到 +接触"（工具上已经没规了），日志指不到真因（规则 R5.1 的静默失效）。 + +所以这里有两条断言，缺一不可： +* **结构**：``_step_unscrew`` 在"成功返回"之前必须调用提出动作 —— 顺序错了 + （比如把提出挪到 S10 里）就等于没做，因为横向动作已经发生了； +* **行为**：提出目标是**跟踪高**（``_track_z()`` = 悬停高 + 50mm），不是写死的 + 毫米数；已经在跟踪高上时不动（否则每次换孔白等 1 秒多）。 +""" + +from __future__ import annotations + +import ast +import pathlib +from typing import Any, List + +import numpy as np +import pytest + +from arm_application.gauge_assembly_node import GaugeAssembly + +NODE_SRC = ( + pathlib.Path(__file__).resolve().parents[1] + / "arm_application" / "gauge_assembly_node.py" +) + + +# ============================================================================= +# 结构：提出动作必须在 S09 的"成功出口"之前 +# ============================================================================= +def _node_ast() -> ast.Module: + return ast.parse(NODE_SRC.read_text(encoding="utf-8")) + + +def _find_method(name: str) -> ast.FunctionDef: + for node in ast.walk(_node_ast()): + if isinstance(node, ast.FunctionDef) and node.name == name: + return node + raise AssertionError(f"节点里找不到方法 {name}") + + +def _call_lines(func: ast.AST, attr: str) -> List[int]: + out: List[int] = [] + for node in ast.walk(func): + if isinstance(node, ast.Call): + f = node.func + if isinstance(f, ast.Attribute) and f.attr == attr: + out.append(node.lineno) + return out + + +def test_unscrew_lifts_out_of_the_hole_before_succeeding(): + """拧出成功后必须先提出孔口，再返回 ``UNSCREW_DONE``。""" + fn = _find_method("_step_unscrew") + lifts = _call_lines(fn, "_lift_clear_of_hole") + assert lifts, ( + "S09 结束时没有竖直提出孔口 —— S10/S11 的横向动作会把规从孔里拽出来" + "（实测：锁被拽断、规掉在台面上，下游表现为『S07 判不到接触』）" + ) + exits = [ + n.lineno + for n in ast.walk(fn) + if isinstance(n, ast.Return) + and isinstance(n.value, ast.Attribute) + and n.value.attr == "UNSCREW_DONE" + ] + assert exits, "S09 没有成功出口？" + assert min(lifts) < max(exits), ( + "提出孔口必须在**成功返回之前**：顺序反了等于没做（横向动作已经发生）" + ) + + +def test_lift_helper_exists_and_uses_the_track_height_idiom(): + """提出高度用 ``_track_z()``（悬停高 + 50mm），不写死毫米数。""" + fn = _find_method("_lift_clear_of_hole") + assert _call_lines(fn, "_track_z"), ( + "提出高度必须用 _track_z()：这是流程里『工具已离开工件』的既有定义，" + "写死毫米数会多出一套没人维护的口径" + ) + assert _call_lines(fn, "_goto"), "提出是一次真实运动（_goto），不是改个变量" + src = ast.get_source_segment(NODE_SRC.read_text(encoding="utf-8"), fn) or "" + assert "hold_xy" in src, "提出必须是**纯 Z**：横向分量会把规别在孔壁上" + + +# ============================================================================= +# 行为：提对高度 / 已在高度上不动 / 拿不到状态不炸 +# ============================================================================= +class _StubLogger: + def __init__(self, sink: List[str]) -> None: + self._sink = sink + + def info(self, msg: str) -> None: + self._sink.append(msg) + + def warn(self, msg: str) -> None: + self._sink.append(msg) + + +class _StubAssembly: + """``_lift_clear_of_hole`` 只用这几样东西；测试里不建任何 ROS 实体。""" + + move_speed = 0.05 + vert_tol_rad = 0.02 + + def __init__(self, z: Any, track_z: float = 0.707, raise_on_goto: bool = False) -> None: + self.logs: List[str] = [] + self.moves: List[Any] = [] + self._z = z + self._tz = float(track_z) + self._raise = bool(raise_on_goto) + + def _wait_state(self, _timeout: float) -> Any: + if self._z is None: + return None, None + Tee = np.eye(4) + Tee[0, 3] = 0.5130 + Tee[1, 3] = -0.0020 + Tee[2, 3] = float(self._z) + return np.zeros(7), Tee + + def _track_z(self) -> float: + return self._tz + + def get_logger(self) -> Any: + return _StubLogger(self.logs) + + def _goto(self, q: Any, Tee: Any, target: Any, **kw: Any) -> Any: + if self._raise: + raise RuntimeError("轨迹被拒") + self.moves.append((np.array(target, dtype=float), kw)) + return True, q, Tee + + +def _stub(z: Any, track_z: float = 0.707, raise_on_goto: bool = False) -> _StubAssembly: + return _StubAssembly(z, track_z, raise_on_goto) + + +def test_deep_in_the_hole_lifts_to_track_height(): + """规还在孔里（实测规尖比工件面低 30mm）→ 提到跟踪高，纯 Z。""" + st = _stub(0.628) + GaugeAssembly._lift_clear_of_hole(st, "k1") # type: ignore[arg-type] + + assert len(st.moves) == 1, "应当恰好走一次提出" + target, kw = st.moves[0] + assert target[0] == pytest.approx(0.5130) and target[1] == pytest.approx(-0.0020), ( + "提出必须是**纯 Z**：XY 一动就是把规别在孔壁上" + ) + assert target[2] == pytest.approx(0.707), "目标高度就是跟踪高（悬停 + 50mm）" + assert kw.get("hold_xy") is True + assert kw.get("tag") == "S09_exit_k1" + assert any("竖直提出" in m for m in st.logs), "日志要能回看这次提出（不然失败时查不到）" + + +def test_already_high_enough_does_not_move(): + """已经比跟踪高还高 → 一步都不走（否则每次换孔白等一秒多）。""" + st = _stub(0.80) + GaugeAssembly._lift_clear_of_hole(st, "k1") # type: ignore[arg-type] + assert st.moves == [] + assert st.logs == [] + + +def test_no_joint_state_is_not_fatal(): + """拿不到状态只告警：这一刻孔里已经没有规要保护，硬失败会把成功的装配变成失败。""" + st = _stub(None) + GaugeAssembly._lift_clear_of_hole(st, "k1") # type: ignore[arg-type] + assert st.moves == [] + assert st.logs == [], "拿不到状态连日志都不用刷（静默跳过即可）" + + +def test_goto_exception_is_swallowed_with_a_warning(): + """提出失败（轨迹被拒）只告警，流程继续 —— 后面 S10/S11 仍会走到安全位。""" + bad = _stub(0.628, raise_on_goto=True) + GaugeAssembly._lift_clear_of_hole(bad, "k1") # type: ignore[arg-type] + assert any("提出孔口失败" in m for m in bad.logs) diff --git a/src/arm_application/test/test_gauge_fsm.py b/src/arm_application/test/test_gauge_fsm.py index 0e2f9da..57c97da 100644 --- a/src/arm_application/test/test_gauge_fsm.py +++ b/src/arm_application/test/test_gauge_fsm.py @@ -25,21 +25,34 @@ from arm_application.gauge_fsm import ( State, DESCEND_BUDGET_FLOOR_M, GAUGE_SHOULDER_ABOVE_TIP_M, + WRENCH_MAX_AGE_S, descend_budget_m, descend_iter_budget, descend_needed_m, descend_progress, descend_tol_m, + relief_tol_m, tool_clearance_m, + wrench_live, + wrench_touch, is_joint_limit_error, pixel_stable, points_stable, + hover_z_guard, + hover_z_for_tool_clearance, ratchet_cycles, resolve_next, + rotation_verdict, screw_stop, tangential_jump, + drag_verdict, + ratchet_delta_rad, + ratchet_travel_ok, + SNAP_DRAG_TOL_M, ) +from arm_application.gauge_config import DEFAULTS + # ============================================================================= # 主线：k1 → k2 → 复位 @@ -456,6 +469,34 @@ def test_descend_tol_rejects_nonpositive_step(): descend_tol_m(-0.0005) +def test_relief_tolerance_is_strictly_smaller_than_the_relief_move(): + """卸载回退的容差必须**严格小于**回退量（与下压步长同一条纪律）。 + + 2026-09-24 实测踩到：回退 0.50mm 而容差被写死 0.0005 m（同量级）， + 目标 z 的浮点回减让 ``dist`` 比 0.0005 略小 ⇒ ``_goto`` 入口就判"已到位" + ⇒ 卸载成为空操作，而残余力判据比的是两个相同读数 ⇒ 假报"已放干净"。 + 之后 S08 带着 ~70N 轴向力去转（夹持摩擦上限 ≈73N）→ 掉件(E1)。 + """ + for dz_mm in (0.05, 0.2, 0.5, 1.0, 3.0): + dz = dz_mm / 1000.0 + tol = relief_tol_m(dz) + assert 0.0 < tol < dz, f"回退 {dz_mm}mm 的容差 {tol * 1000:.4f}mm 不小于回退量" + + +def test_relief_tolerance_has_a_floor_but_never_swallows_the_move(): + """回退量极小时容差退不下去，但**仍不许**大于等于回退量。""" + dz = 1e-5 + tol = relief_tol_m(dz) + assert 0.0 < tol <= dz * 0.5 + + +def test_relief_tolerance_rejects_nonpositive_move(): + with pytest.raises(ValueError): + relief_tol_m(0.0) + with pytest.raises(ValueError): + relief_tol_m(-0.0005) + + def test_descend_iter_budget_covers_the_whole_depth(): """步数上限必须够走完整个下探深度（否则会在"还没到底"时被上限截断）。""" step_m, depth_m = 0.0005, 0.025 @@ -519,8 +560,6 @@ def test_shipped_budget_covers_shipped_gauge_and_hover(): 以后谁把 contact_max_depth_mm 调回去、或把 tool_tip_below_flange_m 改大 到不匹配，这里就会红。 """ - from arm_application.gauge_config import DEFAULTS - budget_m = DEFAULTS["contact_max_depth_mm"] / 1000.0 tip_below = DEFAULTS["tool_tip_below_flange_m"] # 出厂配置里 workpiece_surface_z_m = 0（运行期用 stage2 实测孔位兜底）， @@ -532,6 +571,57 @@ def test_shipped_budget_covers_shipped_gauge_and_hover(): ) +def test_wrench_touch_is_sign_agnostic(): + """腕部 FT 判据取**绝对增量**，不赌正负号。 + + 正负号方向取决于传感器安装（真机法兰系、仿真 link7 系，压下去是 +z 还是 -z + 都可能）。赌错了就是"整段流程静默失效"：下发正常、日志正常、就是停不住。 + 取绝对值只会更早停一点点，这个方向的错是安全的 —— 与既有 + stage2_locate_node._contact_detected 的口径（abs(fz - baseline)）也一致。 + """ + assert wrench_touch(20.0, 0.0, 8.0) + assert wrench_touch(-20.0, 0.0, 8.0) + # 基线本身不是 0（带载）时同样只看增量 + assert wrench_touch(54.0, 46.0, 8.0) + assert wrench_touch(38.0, 46.0, 8.0) + + +def test_wrench_touch_below_threshold_does_not_fire(): + assert not wrench_touch(7.9, 0.0, 8.0) + assert not wrench_touch(46.0, 46.0, 8.0) + # 边界：等于阈值算触发（与 tangential_jump 的 >= 口径一致） + assert wrench_touch(8.0, 0.0, 8.0) + + +def test_wrench_touch_threshold_is_reachable_before_grip_slips(): + """阈值的量级必须**远低于**夹持摩擦能传的上限，否则等于设了个永远达不到的数。 + + 这是"用 300 当阈值"那个 bug 的一般化回归：夹持段 µ=0.25、实测握力 ≈46N + ⇒ 两指合计能传约 µ·N ≈ 23N。接触刚度 5e4 N/m，所以 8N 阈值 ≈ 0.16mm + 过压就判到，而打滑要到 ≈0.46mm —— 留了约 3 倍余量。 + """ + mu, grip_n = 0.25, 46.0 + slip_limit_n = mu * grip_n * 2 # 两指 + assert DEFAULTS["contact_axial_delta_n"] < slip_limit_n / 2, ( + f"阈值 {DEFAULTS['contact_axial_delta_n']}N 太接近夹持摩擦上限 " + f"{slip_limit_n:.1f}N —— 打滑前可能来不及判到" + ) + + +def test_wrench_live_uses_freshness_not_magnitude(): + """判活看"够不够新"，**不看数值大小**。 + + 静止时轴向力本来就接近 0。拿数值判活会把"安静的传感器"误判成掉线 —— + 这正是 2026-09-24 在触觉上踩过的同一个坑（判据从 |fnormal|>1.0 改成 + "够新 + connected"）。所以这里专门断言 age 有值但数值为 0 → 活的。 + """ + assert wrench_live(0.0) + assert wrench_live(WRENCH_MAX_AGE_S) + assert not wrench_live(WRENCH_MAX_AGE_S + 1e-6) + # None = 一帧都没收到过（话题名错/传感器没起来）→ 不是活的 + assert not wrench_live(None) + + def test_descend_iter_budget_zero_depth_means_no_iterations(): assert descend_iter_budget(0.0, 0.0005) == 0 with pytest.raises(ValueError): @@ -546,3 +636,279 @@ def test_descend_progress_only_counts_real_downward_motion(): assert not descend_progress(0.6316, 0.63159, step_m) # 只走了 0.01mm：被死区吃掉 assert not descend_progress(0.6316, 0.6316, step_m) # 完全没动（空转） assert not descend_progress(0.6316, 0.6317, step_m) # 往上跑 ≠ 推进 + + +# ============================================================================= +# 绕轴旋转的验收判据（S01/S08/S09 共用） +# ============================================================================= +def test_rotation_verdict_accepts_a_full_turn(): + assert rotation_verdict(-180.0, -179.7, 0.0, 5.0) == "ok" + assert rotation_verdict(+180.0, +175.1, 0.0, 5.0) == "ok" # 5° 之内算到位 + + +def test_rotation_verdict_calls_a_partial_turn_a_joint_limit(): + """2026-09-24 实测的那一条：**只转了 16.4°**（被关节7 的上限截住）。 + + 期望 +180、实测 +16.4：方向对、只是没走完 ⇒ 判"贴限位"（调用方张开爪 + 让出余量再转），**不是**"偏差过大"直接 ABORT —— 后者把一整轮白扔， + 而且日志看起来像"这台臂坏了"。 + """ + assert rotation_verdict(+180.0, +16.4, 0.1, 5.0) == "limit" + # 完全没动也是同一类（只是余量更少） + assert rotation_verdict(+180.0, +0.4, 0.0, 5.0) == "limit" + + +def test_rotation_verdict_still_stops_on_wrong_direction_or_overshoot(): + """判"贴限位"必须**卡住方向**：反向转/过冲都是真问题，不能拿去让余量重试。""" + assert rotation_verdict(+180.0, -170.0, 0.0, 5.0) == "short" # 转反了 + assert rotation_verdict(+180.0, +260.0, 0.0, 5.0) == "short" # 过冲 + # 小幅指令（≤10°）几乎没动时**不算**"完全没动"那条特例，于是落进 + # "方向对但没走完" ⇒ 同样判 limit。这是有意的：让余量那一步是免费的 + # （张开爪时规在打滑），宁可白让一次，也别把"其实贴住了"判成"臂坏了"。 + assert rotation_verdict(+8.0, +0.2, 0.0, 5.0) == "limit" + + +def test_rotation_verdict_axis_misalignment_beats_everything(): + """转轴不共线（>5°）时，任何角度数字都没有意义 —— 必须先报它。""" + assert rotation_verdict(+180.0, +179.0, 6.0, 5.0) == "axis" + assert rotation_verdict(+180.0, +16.4, 6.0, 5.0) == "axis" + + +def test_ratchet_step_of_180_never_fits_the_rizon_joint7_range(): + """棘轮一步 180° 在 Rizon4 的关节7 行程（±175°）里**永远**放不下。 + + 这是 S09 中止的物理根源，也解释了为什么转腕前要先让余量： + 贴着上限时反向那一步只剩十几度（实测 16.4°）。 + """ + from arm_application.gauge_kin import prewind + + lim = (-3.0543, 3.0543) # Rizon4 关节7：±175° + # 贴在上限（棘轮回转每轮都停在这儿）→ 想再往 + 转：放不下，必须让余量 + plan = prewind(q_now_rad=math.radians(171.0), want_deg=180.0, direction=+1, + limits=lim, margin_deg=8.0) + assert plan.clamped + # 同一个位置往 - 转：放得下（这正是"让余量"那一步） + plan = prewind(q_now_rad=math.radians(171.0), want_deg=180.0, direction=-1, + limits=lim, margin_deg=8.0) + assert not plan.clamped + + +# ============================================================================= +# 悬停高度自检（悬停按相机定、工具比相机长 ⇒ 会捅进工件） +# ============================================================================= +def test_hover_guard_fires_for_the_exact_case_we_hit(): + """2026-09-24 实测：仿真悬停 0.632、工具长 0.245、工件面 0.412。 + + 尖端 0.632−0.245=0.387，比工件面低 25mm ⇒ 悬停这一步就把工具捅进去了， + 紧接着 S05 的横向对准把工件拖走 92mm。这里必须报"要改"并给出几何值。 + """ + fixed, why = hover_z_guard(0.632, 0.412, 0.245, 0.002) + assert why, "这种悬停必须被判为不安全" + assert fixed == pytest.approx(0.412 + 0.245 + 0.002) + assert "钻进工件" in why + assert "hole_hover_z_m = 0.659" in why # 要能直接照着改配置 + + +def test_hover_guard_is_silent_when_the_tool_stays_clear(): + """几何对的配置一个字都不许说（否则每次启动都刷一堆无用告警）。""" + fixed, why = hover_z_guard(0.659, 0.412, 0.245, 0.002) + assert why == "" + assert fixed == pytest.approx(0.659) + # 更高也照过（悬停高一点只是拍得远，不算错） + assert hover_z_guard(0.700, 0.412, 0.245, 0.002)[1] == "" + + +def test_hover_guard_uses_want_clearance_when_configuring(): + """修正值必须落在"刚好 want_clearance"的位置，而不是随手抬一点。""" + fixed, _ = hover_z_guard(0.50, 0.412, 0.245, 0.005) + assert fixed == pytest.approx(0.412 + 0.245 + 0.005) + assert hover_z_for_tool_clearance(0.412, 0.245, 0.005) == pytest.approx(fixed) + + +def test_hover_guard_tolerates_millimetre_level_shortfall(): + """夹在目标间隙附近（差不到 1mm）不算错：工件面 z 本身就有几毫米不确定度。""" + assert hover_z_guard(0.6580, 0.412, 0.245, 0.002)[1] == "" # 差 1.0mm 之内 + assert hover_z_guard(0.6560, 0.412, 0.245, 0.002)[1] != "" # 差 3mm：判错 + + +# ============================================================================= +# S05 精对位时"工件被工具推走"的看门狗 +# ============================================================================= +def test_drag_verdict_says_nothing_on_the_first_frame(): + """第一拍没有上一拍可比 —— 不能因为"没有历史"就报警。""" + assert drag_verdict((0.49, 0.0), None)[0] is False + + +def test_drag_verdict_ignores_measurement_jitter(): + """正常对准时两孔中点不动（动的是目标孔，不是中点）—— 亚毫米抖动不算。""" + ok, d = drag_verdict((0.4905, 0.0002), (0.4900, 0.0000), (0.4899, 0.0000)) + assert ok is False + assert d == pytest.approx(math.hypot(0.0005, 0.0002), abs=1e-9) + + +def test_drag_verdict_fires_on_one_big_jump(): + """单拍就挪了 8mm：刚体不会自己挪回去，不必等第二拍确认。""" + ok, d = drag_verdict((0.4820, 0.0), (0.4900, 0.0)) + assert ok is True + assert d == pytest.approx(0.008) + + +def test_drag_verdict_needs_two_frames_when_it_is_only_3_plus_mm(): + """只大过 3mm 一点点：要求上一拍也朝同方向挪过，避免一次误检就中止流程。""" + mid = (0.4860, 0.0) + prev = (0.4900, 0.0) + # 上一拍没动（d2 = 0）⇒ 只当噪声 + assert drag_verdict(mid, prev, (0.4901, 0.0))[0] is False + # 上一拍也挪了 4mm、方向一致 ⇒ 判被推走 + assert drag_verdict(mid, prev, (0.4940, 0.0))[0] is True + # 上一拍也挪了 4mm，但方向**相反**（工件抖动/误检）⇒ 不判 + assert drag_verdict(mid, prev, (0.4820, 0.0))[0] is False + + +def test_drag_verdict_matches_what_we_actually_saw_in_gazebo(): + """2026-09-24 实测那一轮的真实数字：工件每拍被推 11.8mm（残差恒 11mm）。 + + 这条是回归测试：当时的现场没有任何判据报警，日志只说"检查手眼/相机安装"， + 于是一路追到迭代用满。现在同样的数列必须在**第一拍**就判出来。 + """ + ks = [0.4910, 0.4910, 0.4792, 0.4674, 0.4566, 0.4458, 0.4350, 0.4239, 0.4121] + mids = [(k - 0.029, 0.0) for k in ks] # 中点 = k1 左移两孔间距的一半 + flags = [ + drag_verdict(mids[i], mids[i - 1] if i >= 1 else None, + mids[i - 2] if i >= 2 else None)[0] + for i in range(len(mids)) + ] + assert flags == [False, False, True, True, True, True, True, True, True] + assert SNAP_DRAG_TOL_M < 0.0118 + + +# ============================================================================= +# 棘轮：目标角（绝对值）→ 增量。2026-09-24 S09 第一步 ABORT 的根因 +# ============================================================================= +def test_ratchet_delta_wraps_a_target_that_is_past_the_limit(): + """实测那一笔：关节在 +158.9°、目标 +338.9° ⇒ 增量是 +180°（不是 +338.9°）。 + + 差值的折算是整个棘轮的地基：把它当成 338.9° 会让"行程够不够"算错、 + 让余量那一步算到限位外面，最后报一句与真因无关的"偏差过大"。 + """ + d = ratchet_delta_rad(math.radians(338.9), math.radians(158.9)) + assert math.degrees(d) == pytest.approx(180.0, abs=1e-6) + + +def test_ratchet_rewind_target_stays_inside_the_limit(): + """让余量那一步必须落在限位内 —— 这正是当时失败的地方（−180° < −175°）。""" + lo, up = math.radians(-175.0), math.radians(175.0) + q_now = math.radians(158.9) + delta = ratchet_delta_rad(math.radians(338.9), q_now) + rewind = q_now - delta # 让出整步 + assert lo < rewind < up + assert math.degrees(rewind) == pytest.approx(-21.1, abs=1e-6) + # 让完余量再前进整步 ⇒ 关节回到原处、工具净转 +180° + retry = rewind + delta + assert math.degrees(retry) == pytest.approx(158.9, abs=1e-6) + + +def test_ratchet_delta_is_the_same_number_for_both_wrapped_aims(): + """+338.9° 与 −21.1° 是同一个关节位置（差一整圈）—— 增量必须一致。""" + q_now = math.radians(158.9) + a = ratchet_delta_rad(math.radians(338.9), q_now) + b = ratchet_delta_rad(math.radians(-21.1), q_now) + assert a == pytest.approx(b, abs=1e-9) + + +def test_ratchet_delta_covers_a_plain_back_and_forth_cycle(): + """正常的棘轮一步（不贴限位）：从 +150° 拧 −180°、再回转 +180°。""" + q = math.radians(150.0) + d = ratchet_delta_rad(math.radians(-30.0), q) + assert math.degrees(d) == pytest.approx(-180.0, abs=1e-6) + d2 = ratchet_delta_rad(math.radians(150.0), q + d) + assert math.degrees(d2) == pytest.approx(180.0, abs=1e-6) + + +# ============================================================================= +# 棘轮的**转向**必须由工艺意图决定，不能交给"谁更短" +# 2026-09-24 实测：S08 第 3 轮 ABORT（步长 180° 的刀口） +# ============================================================================= +def test_ratchet_180_degree_step_flips_direction_on_a_tenth_of_a_degree_read(): + """**实测那一笔**：调用方读到 q=+162.0 ⇒ 目标 −18.0；棘轮晚 0.1° 读到 + q=+162.1 ⇒ "最短路径"给出 **+179.9°**（方向反了）。 + + 180° 正好落在 ±180° 的刀口上：0.1° 的读数差就能决定"往前"还是"往后"。 + 结果是棘轮朝螺纹反方向走 → 贴限位 → 让余量 → 重试 → **净位移 0** ⇒ ABORT， + 而现场什么都没坏。所以转向必须由调用方按工艺意图给定。 + """ + d_naive = ratchet_delta_rad(math.radians(-18.0), math.radians(162.1)) + assert math.degrees(d_naive) == pytest.approx(+179.9, abs=0.05) # 旧口径：翻向 + d_screw = ratchet_delta_rad(math.radians(-18.0), math.radians(162.1), -1) + assert math.degrees(d_screw) == pytest.approx(-180.1, abs=0.05), "拧入必须往 − 转" + d_back = ratchet_delta_rad(math.radians(342.0), math.radians(162.1), +1) + assert math.degrees(d_back) == pytest.approx(+179.9, abs=0.05), "回转必须往 + 转" + + +def test_directed_delta_always_lands_on_the_target_and_keeps_the_sign(): + """给了方向：增量符号一定等于指定方向，且转过去**就是**目标角（差整圈）。""" + q = math.radians(162.1) + for want_sign, target_deg in ((+1, 342.0), (+1, -18.0), (-1, -18.0), (-1, 342.0)): + d = ratchet_delta_rad(math.radians(target_deg), q, want_sign) + assert d != 0.0 + assert math.copysign(1.0, d) == want_sign, (want_sign, target_deg, math.degrees(d)) + # 转过去与目标角**同一个位置**（差整数圈） + turns = (q + d - math.radians(target_deg)) / (2.0 * math.pi) + assert abs(turns - round(turns)) < 1e-9 + # 且是那一侧**最小**的一步（再多转一整圈就不是"最小"了） + assert abs(d) <= 2.0 * math.pi + 1e-9 + + +def test_directed_delta_does_not_waste_a_full_turn_when_already_there(): + """已经在目标角上时不许"为了凑方向"白转一整圈 —— 那是 1 秒多的空转。""" + q = math.radians(30.0) + assert ratchet_delta_rad(q, q, +1) == 0.0 + assert ratchet_delta_rad(q, q, -1) == 0.0 + # 差一点点也算"就在那儿"（1e-12 rad 量级）而不是 359.999…° + assert abs(ratchet_delta_rad(q, q - 1e-12, +1)) < 1e-9 + + +def test_plain_shortest_path_is_still_available_for_non_ratchet_callers(): + """``direction=0`` 保留"最短路径"语义（给目标本来就明确在某一侧的调用方）。""" + d = ratchet_delta_rad(math.radians(338.9), math.radians(158.9)) + assert math.degrees(d) == pytest.approx(180.0, abs=1e-6) + assert math.degrees(ratchet_delta_rad(math.radians(10.0), math.radians(350.0))) == pytest.approx(20.0) + + +# ============================================================================= +# 棘轮步长 vs 关节行程：S08 开头就要判，不能等第 3 轮贴限位 +# ============================================================================= +def test_ratchet_travel_rejects_180_on_the_rizon_joint7(): + """180° 的步长在 ±175° 的关节 7 上**永远**放不下（实测第 3 轮 ABORT）。 + + 一个来回要 2×180° = 360°，比行程 350° 还大 —— 无论怎么让余量都腾不出 + 空间，所以这不是"调调就好"，是**配置错**，必须在 S08 开头就拦下来。 + """ + lim = (-3.0543, 3.0543) # Rizon4/10 关节7：±175° + v = ratchet_travel_ok(180.0, lim, 8.0) + assert not v.ok + assert v.need_deg == pytest.approx(376.0) + assert v.travel_deg == pytest.approx(350.0, abs=0.1) + assert v.max_step_deg == pytest.approx(167.0, abs=0.1) + assert "screw_step_deg" in v.why, "报错要直接指出该改哪个键" + assert "screw_max_cycles" in v.why, "还要提醒累计必须是整数圈" + + +def test_ratchet_travel_accepts_the_shipped_step_and_margin(): + """出厂值（120°）必须过 —— 否则 S08 一进门就 ABORT，仿真永远跑不完。""" + from arm_application.gauge_config import DEFAULTS + + lim = (-3.0543, 3.0543) + v = ratchet_travel_ok(DEFAULTS["screw_step_deg"], lim, 8.0) + assert v.ok, v.why + # 建议值必须落在上限内，否则"照着改"会再撞一次 + assert v.max_step_deg >= DEFAULTS["screw_step_deg"] + + +def test_ratchet_travel_passes_when_limits_are_unknown_but_says_so(): + """拿不到限位不能猜着拦：放行，理由交给驱动器报 JOINT_LIMIT 兜底。""" + v = ratchet_travel_ok(180.0, None, 8.0) + assert v.ok + assert v.travel_deg == float("inf") + assert v.max_step_deg == float("inf") + assert v.why == "" diff --git a/src/arm_application/test/test_gauge_gripper_client.py b/src/arm_application/test/test_gauge_gripper_client.py new file mode 100644 index 0000000..a3b5700 --- /dev/null +++ b/src/arm_application/test/test_gauge_gripper_client.py @@ -0,0 +1,151 @@ +"""``GripperClient.dropped()``：E1 掉件判据只认**开口塌陷**，不认状态位抖动。 + +为什么值得单独钉住（2026-09-24 实测踩到）： + 仿真里 ``GripperStatus.is_gripped`` 是**推断值**（"指令到了合拢侧 + 实际开口 + 比指令大出容差"）。S08 拧规时会在"合到某个开口 / 全合"之间发指令，只要某一帧 + 的指令落到与实际开口同一档，``is_gripped`` 就翻成 False —— 于是 + **S08 第 2/12 轮报「通止规掉了」**（爪里其实好好的），整条流程转去等人工 + "放好通止规"，而仿真里没有这个人。 + + 判据改成"夹住时的开口 − 当前开口 ≥ 5mm"以后，这种抖动不再报掉件；而真的掉了 + （两指合到底、开口塌几十毫米）仍然会报。 +""" + +from __future__ import annotations + +from typing import Any, List + +from arm_msgs.msg import GripperStatus + + +class _StubNode: + """``GripperClient`` 只用到这三个方法；测试里不建任何 ROS 实体。""" + + def __init__(self) -> None: + self.warnings: List[str] = [] + + def create_client(self, srv_type: Any, name: str) -> None: + return None + + def create_subscription(self, msg_type: Any, topic: str, cb: Any, qos: Any) -> None: + self._cb = cb + return None + + def get_logger(self) -> Any: + return _StubLogger(self.warnings) + + +class _StubLogger: + def __init__(self, sink: List[str]) -> None: + self._sink = sink + + def info(self, msg: str) -> None: + self._sink.append(msg) + + def warn(self, msg: str) -> None: + self._sink.append(msg) + + def error(self, msg: str) -> None: + self._sink.append(msg) + + +def _status(is_gripped: bool, position: float) -> GripperStatus: + msg = GripperStatus() + msg.is_gripped = bool(is_gripped) + msg.position = float(position) + return msg + + +def _client(): + from arm_application.gauge_assembly_node import GripperClient + + node = _StubNode() + return GripperClient(node, "contract", "/gripper/status"), node + + +def test_no_drop_before_ever_gripped(): + """装配前爪里本来就空：状态位说什么都不算掉件（否则一开场就 E1）。""" + client, _ = _client() + client._on_contract(_status(False, 0.0)) + assert client.dropped() is False + + +def test_a_wide_open_flap_does_not_poison_the_baseline(): + """**本次踩到的那个假 E1 的真因**：全开时 is_gripped 也闪 True（实测 99.29mm）。 + + 以前"第一次判到夹住"就记基准 ⇒ 基准被记成 99.29mm ⇒ 塌陷量算成 77mm ⇒ + 报"通止规掉了"，而规好好地在爪里、真实开口一直是 21.9mm。 + 现在基准只由明确调用 adopt_current_grip 记录：状态位怎么闪都不动它。 + """ + client, _ = _client() + client._on_contract(_status(True, 99.29)) # 全开时的假 GRIPPING + assert client._grip_aperture is None, "状态位不许写基准" + assert client.dropped() is False + client._on_contract(_status(True, 21.9)) # 真正夹住规之后 + client.adopt_current_grip("测试：确认夹住") + assert client._grip_aperture == 21.9 + client._on_contract(_status(False, 21.8)) + assert client.dropped() is False, "开口没塌就不是掉件" + + +def test_is_gripped_flap_is_not_a_drop(): + """**本次踩到的那个假 E1**：指令切换让 is_gripped 翻 False，开口没塌。""" + client, _ = _client() + client._on_contract(_status(True, 24.9)) # 夹住 φ30 的规，开口停在 24.9mm + client.adopt_current_grip("测试：确认夹住") + assert client.dropped() is False + client._on_contract(_status(False, 24.8)) # 指令落到同一档 → 推断值翻 False + assert client.dropped() is False, "开口没塌就不是掉件" + + +def test_real_drop_is_detected_by_aperture_collapse(): + """真的掉了：两指一路合到底，开口塌到几毫米。""" + client, _ = _client() + client._on_contract(_status(True, 24.9)) + client.adopt_current_grip("测试：确认夹住") + client._on_contract(_status(False, 0.2)) + assert client.dropped() is True + + +def test_aperture_jitter_within_the_threshold_is_not_a_drop(): + """实测抖动是亚毫米量级；阈值 5mm 不该被它触发。""" + client, _ = _client() + client._on_contract(_status(True, 24.9)) + client.adopt_current_grip("测试：确认夹住") + client._on_contract(_status(False, 24.9 - 4.9)) + assert client.dropped() is False + + +def test_without_aperture_reading_falls_back_to_the_old_criterion(): + """拿不到 position（老固件/别的夹爪）时退回旧判据：会抖，但不至于"永远判不了掉件"。""" + client, _ = _client() + client._on_contract(_status(True, float("nan"))) + assert client.adopt_current_grip("测试：确认夹住") is None + assert client._grip_aperture is None + client._on_contract(_status(False, float("nan"))) + assert client.dropped() is True, "没有开口读数时不能变成「永远不报掉件」" + + +def test_settle_waits_for_the_motion_to_finish(): + """``settle`` 不能读"动作还没做完"的旧值。 + + 实测：``close`` 服务返回时手指还在动，立刻读开口读到的是**全开 99.93mm** —— + 于是"第 1 轮夹紧后：开口 99.93mm（夹住时 99.29mm）"这种荒唐日志。 + ``settle`` 要一直等到连续两帧一样才认。 + """ + client, _ = _client() + client._on_contract(_status(True, 99.93)) + assert client.settle(settle_s=0.2) == 99.93 + # 动作进行中：值一直在变 ⇒ 不到稳定就不返回（这里只验证它会返回**最终**值） + client._on_contract(_status(False, 30.0)) + assert client.settle(settle_s=0.2) == 30.0 + + +def test_adopt_current_grip_records_what_is_in_the_jaws_now(): + """基准 = "确认夹住那一刻"的开口；之后状态位再翻也不改参照。""" + client, _ = _client() + client._on_contract(_status(True, 21.9)) + assert client.adopt_current_grip("测试") == 21.9 + client._on_contract(_status(False, 21.4)) + assert client.grip_aperture() == 21.9 + assert client.collapse_mm() == 0.5 diff --git a/src/arm_application/test/test_node_handler_collisions.py b/src/arm_application/test/test_node_handler_collisions.py new file mode 100644 index 0000000..eb384e9 --- /dev/null +++ b/src/arm_application/test/test_node_handler_collisions.py @@ -0,0 +1,190 @@ +"""回归：节点类之间的**回调方法重名**必须被拦住。 + +为什么值得一条专门的测试（2026-09-24 踩到的真实故障，代价是两轮排查方向全错）： + + Stage2Locate.__init__ 里建了 + create_subscription(WrenchStamped, ft_wrench_topic, self._on_ft, ...) + Stage3Search（子类）里定义了 + def _on_ft(self, msg): self._ft = (time.monotonic(), msg) + + Python 的方法解析顺序让**子类覆盖父类**，而两个订阅都已经在 __init__ 里 + 建好了。于是那条 WrenchStamped 订阅的回调实际是子类那个 —— 它把消息塞进 + stage3 的 RobotStates 缓存，而父类的 `_ft_fz` 永远不被赋值。 + + 现象："S07 腕部 FT 没有数据（话题 /sim/ft_sensor_wrench）"， + 而在 shell 里 `ros2 topic echo` 那条话题**数据完全正常**、 + QoS 也完全兼容（实测 BEST_EFFORT 与 RELIABLE 订阅者都收得到）。 + 于是排查方向被引到"话题名写错 / QoS 不匹配 / 桥接没起来"上，全部走偏。 + + 真机上更阴：两个话题都存在，`self._ft` 里 RobotStates 与 WrenchStamped + 交替覆盖 → `_ft_mag()` 时有时无 → 腕部力看门狗时灵时不灵（偶发，最难查）。 + +规则：**整条继承链上，`_on_*` 形式的方法名必须唯一 —— 除非子类显式 +``super().<同名>(...)`` 委托回父类**。后者是"扩展"而不是"遮蔽"，是允许的 +（``GaugeAssembly._on_color`` 就属于这种：先 ``super()._on_color(msg)`` 存帧， +再补记"收到时刻"）。判定标准就是有没有那一次委托调用 —— +没有委托的同名覆盖，一定会让父类的那份状态**永远不更新**，而且不报任何错。 + +命名空间不够用就改名字（`_on_wrench` / `_on_robot_states` 比 `_on_ft` 好， +后者里那个 "ft" 到底指腕部六维力还是手指触觉，本来就含糊）—— +不要靠"反正签名不同"来赌，Python 不检查签名。 + +为什么用 AST 读源码而不是 import 后反射： + 这几个节点模块 import 了 ROS 生成的消息包（``arm_msgs`` 等）， + 在没有 source 过工作空间的 pytest 环境里 import 不进来。 + 而这条规则是**纯源码层面**的，不需要真的构造节点。 + 这也是本仓库既有的做法（见 ``test_gauge_step_events.py``）。 +""" + +from __future__ import annotations + +import ast +import pathlib +from typing import Dict, List + +_APP = pathlib.Path(__file__).resolve().parents[1] / "arm_application" + +#: 继承链上的源文件。 +_CHAIN_FILES = ( + "stage2_locate_node.py", + "stage3_search_node.py", + "pipeline_node.py", + "gauge_assembly_node.py", +) + +#: 从哪个类开始往上走（装配流程真正实例化的那个）。 +_LEAF = "GaugeAssembly" + + +class _Cls: + """一个类的：基类名、本类定义的 `_on_*` 方法（名字 → AST 节点）。""" + + def __init__(self, name: str, bases: List[str], handlers: Dict[str, ast.AST]) -> None: + self.name = name + self.bases = bases + self.handlers = handlers + + +def _classes_in(path: pathlib.Path) -> Dict[str, _Cls]: + tree = ast.parse(path.read_text(encoding="utf-8")) + out: Dict[str, _Cls] = {} + for node in tree.body: + if not isinstance(node, ast.ClassDef): + continue + bases = [b.id for b in node.bases if isinstance(b, ast.Name)] + handlers = { + item.name: item + for item in node.body + if isinstance(item, (ast.FunctionDef, ast.AsyncFunctionDef)) + and item.name.startswith("_on_") + } + out[node.name] = _Cls(node.name, bases, handlers) + return out + + +def _table() -> Dict[str, _Cls]: + table: Dict[str, _Cls] = {} + for name in _CHAIN_FILES: + table.update(_classes_in(_APP / name)) + return table + + +def _chain() -> List[str]: + """从 ``_LEAF`` 沿基类往上，返回类名列表（子类在前）。""" + table = _table() + order: List[str] = [] + cur = _LEAF + while cur and cur in table: + order.append(cur) + bases = table[cur].bases + cur = bases[0] if bases else "" + assert len(order) >= 3, f"继承链没解析出来（只找到 {order}）—— 改过类名？" + return order + + +def _delegates_to_super(func: ast.AST, name: str) -> bool: + """``func`` 的方法体里有没有 ``super().(...)`` 这一次委托调用。""" + for node in ast.walk(func): + if not isinstance(node, ast.Call): + continue + callee = node.func + if not (isinstance(callee, ast.Attribute) and callee.attr == name): + continue + owner = callee.value + if ( + isinstance(owner, ast.Call) + and isinstance(owner.func, ast.Name) + and owner.func.id == "super" + ): + return True + return False + + +def test_leaf_class_is_reachable_from_every_chain_file(): + """先确认这条测试自己在看对文件 —— 文件改名/移动时别静默变成空跑。""" + chain = _chain() + assert _LEAF in chain + for expected in ("Stage2Locate", "Stage3Search", "AssemblyPipeline"): + assert expected in chain, f"继承链里没有 {expected}（链 = {chain}）" + + +def test_no_silent_callback_shadowing_across_inheritance_chain(): + """同名 `_on_*` 只允许两种情形：唯一，或子类显式 ``super()`` 委托。 + + 挂了怎么修（二选一）： + a) 在子类那个方法体里加 ``super().<同名>(msg)`` —— 表示"我是扩展"； + b) 改掉其中一个的名字（带来源，如 ``_on_wrench`` / ``_on_robot_states``）。 + 别什么都不做：父类那份状态会永远不更新，而**不会有任何报错**。 + """ + table = _table() + owner: Dict[str, str] = {} + problems: List[str] = [] + + # _chain() 是"子类在前"，所以后出现的才是父类。 + for cls_name in _chain(): + cls = table.get(cls_name) + if cls is None: + continue + for handler in sorted(cls.handlers): + owner.setdefault(handler, cls_name) + + # 再逐个查：凡是有多个类定义了的，子类必须委托。 + for handler, owner_cls in owner.items(): + definers = [c for c in _chain() if c in table and handler in table[c].handlers] + if len(definers) < 2: + continue + for cls_name in definers: + if cls_name == definers[-1]: + continue # 链最靠上的那个（基类）不需要委托 + cls = table[cls_name] + if _delegates_to_super(cls.handlers[handler], handler): + continue + problems.append( + f"{handler}: 定义在 {', '.join(definers)} —— {cls_name} 覆盖了" + f"{owner_cls} 的同名回调，但方法体里没有 super().{handler}(...) 委托。\n" + f" 实际生效的只有 {cls_name} 那个，父类那份状态永远不会更新。" + ) + + assert not problems, ( + "回调被静默遮蔽。症状是「某路数据永远收不到」而话题本身完全正常，" + "极难从现象倒推到这里：\n " + "\n ".join(problems) + ) + + +def test_wrench_and_robot_states_callbacks_are_not_the_same_method(): + """专门钉住这一对名字 —— 它们是最容易被合并成 `_on_ft` 的两个。 + + 腕部 FT（WrenchStamped，100Hz，法兰系）与整机状态（RobotStates，含外部 + 力/力矩）是两个**不同的**话题、不同的消息类型，缓存也必须是两个字段： + ``_ft_fz``（轴向力标量）与 ``_ft``（整帧 RobotStates）。 + """ + table = _table() + s2 = set(table["Stage2Locate"].handlers) + s3 = set(table["Stage3Search"].handlers) + + assert "_on_wrench" in s2, ( + "Stage2Locate 里腕部 FT 的回调应叫 _on_wrench（不能再叫 _on_ft：" + "会被 Stage3Search 的同名方法遮蔽）" + ) + assert "_on_robot_states" in s3, "Stage3Search 里整机状态的回调应叫 _on_robot_states" + assert not (s2 & s3), f"两个类还有重名的回调: {sorted(s2 & s3)}" diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/robot_description.py b/src/arm_sim_gazebo/arm_sim_gazebo/robot_description.py index 0b3faef..7e3bf54 100644 --- a/src/arm_sim_gazebo/arm_sim_gazebo/robot_description.py +++ b/src/arm_sim_gazebo/arm_sim_gazebo/robot_description.py @@ -566,6 +566,7 @@ def prepare_urdf( rizon_type: str = "Rizon4", arm_prefix: str = "sim", robot_sn: str = "", + wrist_ft_topic: str = "", ) -> Tuple[str, NormalizeReport]: """展开 + 归一化，返回 (URDF 文本, 归一化报告)。 @@ -576,6 +577,10 @@ def prepare_urdf( （SDF 里只存了插件名和参数文件路径，接口声明不在 SDF 里，实测确认）。 所以若 RSP 发的是**未归一化**的那份，控制器注册的关节名就会和 Gazebo 里的 对不上，症状是 "Joint 'sim_joint1' not found in the robot model"。 + + :param wrist_ft_topic: 腕部 FT 的 gz 话题名。**从 wrist_ft.yaml 传进来**， + 而不是让 xacro 里再写一份 —— 话题名有三处要对上（配置、URDF、 + 应用层按 sn 推出的名字），两处写死迟早漂移。空 = 用 xacro 的默认值。 """ root = repo_root() xacro_path = Path(xacro_path) if xacro_path else default_xacro(root) @@ -590,16 +595,18 @@ def prepare_urdf( f"缺了会导致 Gazebo 报 'Failed to load system plugin'。" ) - urdf = expand_xacro( - xacro_path, - { - "controllers_file": str(Path(controllers_file).resolve()), - "load_gripper": "true" if load_gripper else "false", - "with_sensors": "true" if with_sensors else "false", - "rizon_type": rizon_type, - "arm_prefix": arm_prefix, - "robot_sn": robot_sn, - }, - ) + xacro_args = { + "controllers_file": str(Path(controllers_file).resolve()), + "load_gripper": "true" if load_gripper else "false", + "with_sensors": "true" if with_sensors else "false", + "rizon_type": rizon_type, + "arm_prefix": arm_prefix, + "robot_sn": robot_sn, + } + # 只在给了值时才覆盖，否则保持 xacro 里那个有注释、能单独跑 xacro 的默认值。 + if wrist_ft_topic: + xacro_args["wrist_ft_topic"] = wrist_ft_topic + + urdf = expand_xacro(xacro_path, xacro_args) normalized, report = normalize_urdf(urdf) return normalized, report diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/sim_grasp_lock.py b/src/arm_sim_gazebo/arm_sim_gazebo/sim_grasp_lock.py new file mode 100644 index 0000000..6c62abd --- /dev/null +++ b/src/arm_sim_gazebo/arm_sim_gazebo/sim_grasp_lock.py @@ -0,0 +1,329 @@ +"""仿真"抓取锁"（grasp lock）：让夹爪夹住的物体在仿真里真的不会滑掉。 + +============================================================================= +它在链路上的位置 +============================================================================= + 真机：夹爪 46N 握力 + 指垫摩擦 → 工件被"抱住"，机械臂怎么动都不掉。 + 仿真：夹持力 = 位置误差 × 接触刚度（见 gauge/model.sdf 的 kp 说明）。 + 规站着不动时够用；一旦 S01 绕工具轴快转 150°、或 pre_lift 走 60~100mm + 的连续轨迹，规就在指间打滑（实测工具长度 0.243m → 0.285m、歪 62°）， + S07 随后拿到的几何全错 —— 现象是"判不到接触"或"规掉了(E1)"。 + + 本模块做的唯一一件事：**在夹爪确认夹住时，用 DetachableJoint 插件把物体 + 与手指用一个固定关节锁起来；夹爪张开时解锁。** + + 这不是"作弊绕过物理"：刚性接触求解器给不出静摩擦迟滞，也不模拟指垫橡胶 + 的贴合，而真实夹持恰恰靠这两件事。锁补的就是这一块。物体与指垫之间的 + **接触对仍然存在**，所以触觉、腕部 FT 的读数照旧是真实的接触力。 + +============================================================================= +为什么用 `ign topic` 命令行而不是 ros_gz_bridge +============================================================================= + 触发消息是 `ignition.msgs.Empty`。bridging 一个"空消息"要走 + ros_gz_bridge 的 Empty 映射，而这条链路上**没有任何东西需要它出现在 ROS + 侧** —— 唯一的目的就是把 gz 的那个插件叫一下。多接一个 bridge 就多一个 + "配错了不报错"的点（bridge 类型名对不上时它只是安静地什么也不桥）。 + + 代价是每次触发起一个短命进程（几十毫秒）。触发频率是"每次夹/放一次"， + 不是每个控制周期 —— 这个代价可以忽略。 + +============================================================================= +静默失效的四个点（改配置时全都要对上） +============================================================================= + ① = 机器人 spawn 的 -name（launch 里 rizon_sim） + ② = Gazebo 里**真实存在**的连杆名（URDF 里与固定关节相连的 + 连杆会被 sdformat 合并，手指只剩 sim_left_finger_mount） + ③ attach/detach 话题名与 gauge/model.sdf 里的插件一致 + ④ 插件确实写进了 gauge 的 SDF + + ①②③④ 全都不会自己报错：锁不触发时，现象只是"规莫名掉件"。 + 所以 :func:`check_grasp_lock_sdf` 把它变成启动时的一条告警/报错。 +""" + +from __future__ import annotations + +import subprocess +from dataclasses import dataclass +from pathlib import Path +from typing import List, Optional + +import yaml + +#: 相对仓库根的默认配置路径。 +DEFAULT_GRASP_LOCK_REL = "devices/sim/gazebo/config/grasp_lock.yaml" + +#: 被 lock 的物体模型 SDF 的默认位置（用来做静态核对）。 +DEFAULT_OBJECT_SDF_TMPL = "devices/sim/gazebo/models/{object}/model.sdf" + + +class GraspLockError(RuntimeError): + """抓取锁配置缺失或写错。消息里必须给出修法（现场就靠这句话）。""" + + +@dataclass(frozen=True) +class GraspLockSpec: + """抓取锁的一份完整描述。""" + + enabled: bool + object: str + object_link: str + robot_model: str + robot_link: str + attach_topic: str + detach_topic: str + state_topic: str + publish_timeout_ms: int + + def describe(self) -> str: + """给启动日志用的一句话。""" + if not self.enabled: + return "抓取锁：关闭（仿真靠摩擦夹持，机械臂快速运动时可能滑件）" + return ( + f"抓取锁：开 → {self.object}.{self.object_link} 锁到 " + f"{self.robot_model}.{self.robot_link}" + f"（attach={self.attach_topic} detach={self.detach_topic}）" + ) + + +def load_grasp_lock( + path: Optional[Path] = None, root: Optional[Path] = None +) -> GraspLockSpec: + """读抓取锁配置。找不到就返回一份"关闭"的默认值（仿真仍能跑）。 + + **为什么文件缺失不报错**：抓取锁是仿真里的**补救措施**，不是必需品 —— + 关掉它只是回到"靠摩擦夹持"，多数轻动作仍然能跑。让仿真因为一个可选补救 + 措施起不来，是拿主流程给可选项陪葬。 + 但**文件存在而字段写错**时必须报错：那说明有人动过它、而且写错了。 + """ + if path is None: + base = root if root is not None else Path.cwd() + path = Path(base) / DEFAULT_GRASP_LOCK_REL + path = Path(path) + + if not path.is_file(): + return GraspLockSpec( + enabled=False, + object="gauge", + object_link="body", + robot_model="", + robot_link="", + attach_topic="/sim/grasp_lock/attach", + detach_topic="/sim/grasp_lock/detach", + state_topic="/sim/grasp_lock/state", + publish_timeout_ms=3000, + ) + + try: + doc = yaml.safe_load(path.read_text(encoding="utf-8")) or {} + except yaml.YAMLError as exc: + raise GraspLockError( + f"抓取锁配置 YAML 解析失败: {path}\n 原因: {exc}" + ) from exc + + if not isinstance(doc, dict): + raise GraspLockError(f"{path} 的顶层应该是映射（key: value 形式）。") + + enabled = bool(doc.get("enabled", True)) + + def _text(key: str, default: str = "") -> str: + return str(doc.get(key, default) or "").strip() + + spec = GraspLockSpec( + enabled=enabled, + object=_text("object", "gauge") or "gauge", + object_link=_text("object_link", "body") or "body", + robot_model=_text("robot_model"), + robot_link=_text("robot_link"), + attach_topic=_text("attach_topic", "/sim/grasp_lock/attach"), + detach_topic=_text("detach_topic", "/sim/grasp_lock/detach"), + state_topic=_text("state_topic", "/sim/grasp_lock/state"), + publish_timeout_ms=int(doc.get("publish_timeout_ms", 3000)), + ) + + if not enabled: + return spec + + missing = [ + k + for k in ("robot_model", "robot_link", "attach_topic", "detach_topic") + if not getattr(spec, k) + ] + if missing: + raise GraspLockError( + f"{path} 开着（enabled: true）但缺字段: {', '.join(missing)}。\n" + f" robot_model 必须等于 launch 里 spawn 机器人的 -name（当前 rizon_sim）；\n" + f" robot_link 必须是 Gazebo 里的**真名**（与固定关节相连的连杆会被\n" + f" sdformat 合并，手指只剩 sim_left_finger_mount）。真名查法：\n" + f" ros2 topic echo /sim/world_pose_info ← 看全部 child_frame_id\n" + f" 这两个名字写错的后果是**静默失效**：锁不触发、规照样滑掉，\n" + f" 而日志里只看得到「规掉了」。" + ) + if spec.attach_topic == spec.detach_topic: + raise GraspLockError( + f"{path} 的 attach_topic 与 detach_topic 相同（{spec.attach_topic}）——\n" + f" 那样夹住和放开会走同一条消息，锁的状态完全随机。两个必须不同。" + ) + # 锚点不能是"会跟着张爪动的"那根手指 —— 这是 2026-09-24 用一条假 E1 换来的。 + if "finger" in spec.robot_link.lower(): + raise GraspLockError( + f"{path} 的 robot_link 是 {spec.robot_link} —— **手指**。\n" + f" 被锁的工件会被刚性固定在手指上，而手指会随指令张/合：\n" + f" · S08 棘轮每轮都要\"松爪 → 回转 → 夹紧\"，张爪时手指外移几十毫米，\n" + f" 而工件（规）还插在孔里 —— 两处约束打架，物理解算会把工件\n" + f" **从孔里拽出来**（实测：规尖跳 +17.5mm、偏摆 64°），下一步掉地。\n" + f" 日志里只看得到「通止规掉了(E1)」，看不出真因。\n" + f" 修法：填**不跟着张爪动**的连杆。仿真是法兰 `sim_link7`\n" + f" （夹爪本体 sim_grav_base_link 在 sdformat 里被合并进它了）。\n" + f" 真名查法：ros2 topic echo /sim/world_pose_info ← 列全部 child_frame_id" + ) + if spec.publish_timeout_ms <= 0: + raise GraspLockError( + f"{path} 的 publish_timeout_ms 必须为正，收到 {spec.publish_timeout_ms}" + ) + return spec + + +def bridge_target_links(spec: GraspLockSpec) -> List[str]: + """把"锁的两端"整理成一条便于核对/记录的清单（给日志与自检用）。""" + return [f"{spec.object}.{spec.object_link}", f"{spec.robot_model}.{spec.robot_link}"] + + +def check_grasp_lock_sdf( + sdf_text: str, spec: GraspLockSpec, sdf_label: str = "" +) -> List[str]: + """静态核对：物体 SDF 里的 DetachableJoint 插件是否与本配置一致。 + + 为什么值得一查（与腕部 FT 的 check 同一个理由）： + 插件参数写错时 Gazebo **什么都不说** —— 插件照样加载（只要 child_model + 找得到），assess 话题照样发，只是关节连不上。现象只有一个： + "规莫名其妙掉了"，而那要等到 S07/S08 才显形、且看不到真因。 + 这里在**启动时**把它变成一句话。 + + 返回告警列表（空 = 没问题）；enabled=false 时永远返回空。 + """ + if not spec.enabled: + return [] + where = f"（{sdf_label}）" if sdf_label else "" + warnings: List[str] = [] + if not sdf_text: + return [f"读不到物体 SDF{where}，无法核对抓取锁插件"] + + if "DetachableJoint" not in sdf_text: + warnings.append( + f"物体 SDF{where} 里没有 DetachableJoint 插件 —— 抓取锁不会生效。\n" + f" 配置说 enabled: true，但模型里没有插件时**不会报错**，\n" + f" 只是夹住的物体依旧会滑（现象＝「规莫名掉了」，看不到真因）。\n" + f" 修法：把插件块补进 {spec.object}/model.sdf，参数照 grasp_lock.yaml。" + ) + return warnings + + if spec.robot_model and f"{spec.robot_model}" not in sdf_text: + warnings.append( + f"插件里的 不是 {spec.robot_model} —— " + f"它必须等于 launch 里 spawn 机器人的 -name。\n" + f" 对不上时 DetachableJoint 在 Configure 阶段直接失败，" + f"插件被卸载、{spec.state_topic} 也不会出现（这是唯一的线索）。" + ) + + if spec.robot_link and f"{spec.robot_link}" not in sdf_text: + warnings.append( + f"插件里的 不是 {spec.robot_link}。\n" + f" 注意这里要填 Gazebo 的**真名**：URDF 里与固定关节相连的连杆会被\n" + f" sdformat 合并（lump），例如手指只剩 sim_left_finger_mount。\n" + f" 名字写错时插件照样加载、attach 照样「成功」，但关节连到不存在的\n" + f" 连杆上 → 锁不生效。真名查法：ros2 topic echo /sim/world_pose_info" + ) + + for topic, key in ((spec.attach_topic, "attach_topic"), (spec.detach_topic, "detach_topic")): + if topic and f"<{key}>{topic}" not in sdf_text: + warnings.append( + f"插件里的 <{key}> 不是 {topic}。\n" + f" 两侧话题名不一致时，锁的触发消息发到一个没人听的地址上 ——\n" + f" 不报错，只是永远不锁。" + ) + + return warnings + + +def _ign_topic_cmd(exe: str, topic: str, timeout_ms: int) -> List[str]: + """拼一条 `ign topic` 发布命令（空消息）。 + + ⚠️ ``-d``（duration 秒）不是可选的装饰 —— 少了它**大概率丢消息**： + CLI 建立 publisher 与网络里发现订阅者需要一点时间，一条 fire-and-forget + 的消息在发现完成前就发完了。曾经写成 ``-w <秒>``（以为它等订阅者）， + 而这个版本的 ``ign topic`` **没有 -w** —— 参数被当成多余的位置参数： + "The following arguments were not expected: 3" + 于是"上锁/解锁"永远失败，而现象和"锁没生效"完全一样（规照样掉）。 + """ + return [ + exe, + "topic", + "-t", topic, + "-m", "ignition.msgs.Empty", + "-p", "", + "-d", f"{max(0.5, timeout_ms / 1000.0):.2f}", + ] + + +def _find_ign_exe(explicit: Optional[str] = None) -> Optional[str]: + """找到 ign / gz 命令行。找不到返回 None（调用方决定是告警还是忽略）。""" + import shutil + + if explicit: + return explicit + for name in ("ign", "gz"): + found = shutil.which(name) + if found: + return found + return None + + +def publish_lock_event( + spec: GraspLockSpec, + attach: bool, + exe: Optional[str] = None, + logger=None, +) -> bool: + """发一条"上锁/解锁"消息。返回是否成功（失败只记日志，不抛）。 + + **为什么不抛异常**：这不是主流程的一步 —— 它是仿真里的补救措施。 + 发不出去时正确行为是"继续跑，但明确告警"，而不是把整条装配流程打断： + 现场宁可看到"锁没生效、规可能滑"的提示，也不要看到"因为锁发不出去， + 装配根本没做"。 + """ + if not spec.enabled: + return False + topic = spec.attach_topic if attach else spec.detach_topic + what = "上锁" if attach else "解锁" + exe_path = _find_ign_exe(exe) + if exe_path is None: + if logger is not None: + logger.warn( + f"抓取锁{what}失败：找不到 ign/gz 命令行（PATH 里没有）。\n" + f" 后果：夹住的物体在机械臂连续运动时可能滑掉（现象＝「规掉了」）。" + ) + return False + + cmd = _ign_topic_cmd(exe_path, topic, spec.publish_timeout_ms) + try: + proc = subprocess.run( + cmd, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + timeout=max(2.0, spec.publish_timeout_ms / 1000.0 + 2.0), + ) + except (OSError, subprocess.SubprocessError) as exc: + if logger is not None: + logger.warn(f"抓取锁{what}失败（{topic}）：{exc}") + return False + + if proc.returncode != 0: + out = (proc.stdout or b"").decode("utf-8", "replace").strip() + if logger is not None: + logger.warn( + f"抓取锁{what}失败（{topic}，退出码 {proc.returncode}）：{out[:200]}" + ) + return False + if logger is not None: + logger.info(f"抓取锁{what}：{topic}") + return True diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/sim_gripper_adapter.py b/src/arm_sim_gazebo/arm_sim_gazebo/sim_gripper_adapter.py index 5a77a59..4b77c55 100644 --- a/src/arm_sim_gazebo/arm_sim_gazebo/sim_gripper_adapter.py +++ b/src/arm_sim_gazebo/arm_sim_gazebo/sim_gripper_adapter.py @@ -63,6 +63,14 @@ RViz/TF 与 Gazebo 就一致（详见 urdf_normalize.py 里那段说明）。 - 但**反馈绝不能取它**，否则 status 永远报"开口 0 mm"而夹爪明明开着。 适配器改用"与主关节直接相连、|multiplier| 最大的那个从动关节"反推开口 （见 GripperLayout._pick_feedback / aperture_from_feedback）。 + +5) **仿真里的夹持靠不住，需要一把"抓取锁"**。仿真的夹持力不是设出来的， + 而是位置误差 × 接触刚度：规站着不动够用，机械臂一走连续轨迹（S01 快转 + 150°、pre_lift 60~100mm）就在指间打滑 —— 实测工具长度 0.243m → 0.285m、 + 规歪 62°，S07 随后拿到的几何全错。真机不会（46N 握力 + µ≈0.8 + 0.27kg）。 + 所以本节点在**确认夹住之后**用 DetachableJoint 把物体固定到手指上， + **指令要求张开时**解锁（详见 sim_grasp_lock.py；为什么不拿 is_gripped + 当解锁条件，见 :meth:`_sync_grasp_lock`）。 """ from __future__ import annotations @@ -77,6 +85,7 @@ from arm_msgs.msg import GripperStatus from arm_msgs.srv import GripperExecute from sensor_msgs.msg import JointState +from .sim_grasp_lock import GraspLockSpec, publish_lock_event from .sim_gripper import ( DEFAULT_CONTROLLER, DEFAULT_CONTROLLERS_REL, @@ -142,6 +151,13 @@ class SimGripperAdapter(Node): self.declare_parameter("tolerance_mm", DEFAULT_GRASP_TOLERANCE_MM) # 夹住判据容差（毫米开口） self.declare_parameter("default_speed_pct", 50.0) self.declare_parameter("default_force_pct", 20.0) + # 仿真抓取锁（见 sim_grasp_lock.py）。默认**关**：参数化之后，launch 才是 + # 唯一决定它开不开的地方 —— 单跑本节点做调试时不该莫名其妙去动 Gazebo。 + self.declare_parameter("grasp_lock_enabled", False) + self.declare_parameter("grasp_lock_object", "gauge") + self.declare_parameter("grasp_lock_attach_topic", "/sim/grasp_lock/attach") + self.declare_parameter("grasp_lock_detach_topic", "/sim/grasp_lock/detach") + self.declare_parameter("grasp_lock_timeout_ms", 3000) controller = str(self.get_parameter("controller").value or DEFAULT_CONTROLLER) raw_ctrl = str(self.get_parameter("controllers_file").value or "") @@ -214,6 +230,34 @@ class SimGripperAdapter(Node): # 先把夹爪摆到默认全开，免得机器人一出场就是捏着的状态。 self._publish_target(self._target) + + # 抓取锁：规格从参数来（launch 从 grasp_lock.yaml 派生），状态用三态： + # None = 不知道 / False = 已解锁 / True = 已上锁 + # 之所以要 None 而不是 bool + 计数器：发布失败时要能**重试**， + # 而"发布失败"恰恰表现为"发出去没人听"（话题名错、进程起不来）， + # 只有把"未确认"单独表示出来才可能重试。 + self._lock = GraspLockSpec( + enabled=bool(self.get_parameter("grasp_lock_enabled").value), + object=str(self.get_parameter("grasp_lock_object").value or "gauge"), + object_link="", + robot_model="", + robot_link="", + attach_topic=str(self.get_parameter("grasp_lock_attach_topic").value or ""), + detach_topic=str(self.get_parameter("grasp_lock_detach_topic").value or ""), + state_topic="", + publish_timeout_ms=int(self.get_parameter("grasp_lock_timeout_ms").value), + ) + self._locked: Optional[bool] = None + if self._lock.enabled: + self.get_logger().info( + f"抓取锁已启用：夹住后把 {self._lock.object} 用固定关节锁到手指上，" + f"张开时解锁（attach={self._lock.attach_topic}）。\n" + " 它补的是 Gazebo 缺的那块物理（刚性接触没有静摩擦迟滞、也不模拟" + "指垫贴合）——\n" + " 真机 46N 握力 + µ≈0.8 本来就夹得住，仿真的位置控制夹持则会在" + "连续运动时打滑。" + ) + self.create_timer(0.1, self._tick_status) # --- 参数与描述 ------------------------------------------------------- @@ -311,7 +355,43 @@ class SimGripperAdapter(Node): return msg def _tick_status(self) -> None: - self._pub_status.publish(self._status_msg()) + msg = self._status_msg() + self._pub_status.publish(msg) + self._sync_grasp_lock(bool(msg.is_gripped)) + + # --- 抓取锁（仿真专用）------------------------------------------------- + def _sync_grasp_lock(self, gripped: bool) -> None: + """夹持状态 → 抓取锁。只在"确认夹住"时上锁，绝不自动解锁。 + + **为什么不在这里因为"判到没夹住"而解锁**： + 本适配器的 ``is_gripped`` 是**推断值**（指令到合拢位 + 实际开口被顶住）， + 抓取锁要解决的恰恰是"规在指间打了滑"的情形 —— 打滑会让触觉接触 + 闪断、``is_gripped`` 跟着抖。若拿它当解锁条件，就会得到 + "一抖就解锁 → 规真掉下去 → 更读不到夹持"的自毁循环。 + 解锁只认**指令**（open / 开口变大的 move），那才是松手的真实原因。 + """ + spec = self._lock + if not spec.enabled or not gripped or self._locked is True: + return + if publish_lock_event(spec, True, logger=self.get_logger()): + self._locked = True + self.get_logger().info( + f"抓取锁：已把 {spec.object} 锁到手指上（此后机械臂怎么动都不会滑）。" + ) + else: + # 失败不抛异常（锁只是补救措施），但要留痕：现象是"规掉了"而看不到真因。 + self._locked = None + + def _release_grasp_lock(self, why: str) -> None: + """松开抓取锁。只在**指令明确要张开**时调用。""" + spec = self._lock + if not spec.enabled or self._locked is False: + return + if publish_lock_event(spec, False, logger=self.get_logger()): + self._locked = False + self.get_logger().info(f"抓取锁：已解锁（{why}）") + else: + self._locked = None # --- 服务 ------------------------------------------------------------- def _make_handler(self, verb: str): @@ -346,6 +426,7 @@ class SimGripperAdapter(Node): cmd = request.command try: if verb == "open": + self._release_grasp_lock("收到 open 指令") self._move_to_aperture(self.layout.stroke_mm) note = f"全开（{self.layout.stroke_mm:.1f} mm）" elif verb == "close": @@ -354,6 +435,16 @@ class SimGripperAdapter(Node): elif verb == "move": want = float(cmd.position) clamped = self.layout.is_clamped(want) + # 「开口变大」才叫松手。判据用**实测开口**而不是上一次指令： + # 指令可以是"先夹到 30 再挪到 32"这种，比指令会误判成开爪。 + state = self._fresh_state() + now_mm = ( + self.layout.aperture_from_feedback(state) + if state is not None + else None + ) + if now_mm is not None and want > now_mm + self._tolerance_mm: + self._release_grasp_lock(f"开口 {now_mm:.1f} → {want:.1f} mm") self._move_to_aperture(want) note = f"开口 → {want:.2f} mm" if clamped: diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/sim_live_pose.py b/src/arm_sim_gazebo/arm_sim_gazebo/sim_live_pose.py new file mode 100644 index 0000000..1147661 --- /dev/null +++ b/src/arm_sim_gazebo/arm_sim_gazebo/sim_live_pose.py @@ -0,0 +1,159 @@ +#!/usr/bin/env python3 +"""仿真里"物体**现在到底在哪**"的唯一来源：``/world//pose/info``。 + +为什么要有这个模块（2026-09-24 的教训，别再让真值按名义位姿算） +---------------------------------------------------------------- +真值路径（``perception.source=truth``）最初是**按 scene.yaml 的名义位姿**算目标点 +与孔位的 —— 也就是它假设"物体没动过"。实测这个假设站不住： + +1. 物体是"在名义位姿上方 spawn、再落下来"的，**落地就偏 2~3mm**（实测 2.6mm）； +2. 工件骑在台面那 36mm 通槽上，静置中还会缓慢蠕动（实测十几分钟里又挪了 1.9mm）。 + +而 2mm 的位姿误差在 S05 的像素上约 5px，在 S06 对孔上就是**规尖（φ6）进不了 +φ7 的孔** —— 现象是"S07 压到工件面、但孔没进去"，接着 S08 一转就掉件。 +现场看起来像"柔顺参数不对/夹持力不够"，其实几何从一开始就指错了两毫米。 + +所以：**真值就该用实测位姿**，名义位姿只用来做"物体是不是被搬走了/掉下去了"的 +告警。这件事被两个地方需要（真值感知的目标点、二阶段定位的孔位）， +所以实现只放这一份（R5.8）。 + +⚠️ 实测位姿是 Gazebo 的**真值**（不是测量），所以"按实测位姿算"比"按配置算" + 更接近真机上的视觉：真机也是量到哪就是哪。 +""" + +from __future__ import annotations + +import math +from dataclasses import replace +from typing import Any, Dict, Iterable, Mapping, Optional, Tuple + +from arm_sim_gazebo import sim_scene + +#: 位姿（名 → ``(x, y, z, yaw)``，单位米/弧度）。缺的物体不在字典里。 +LivePoses = Dict[str, Tuple[float, float, float, float]] + + +def yaw_from_quat_xyzw(quat: Iterable[float]) -> float: + """四元数 ``(x, y, z, w)`` → 绕 Z 的 yaw [rad]。 + + 四元数→矩阵**不在这里实现**：复用全仓库唯一那份 ``handeye_lib.quat_xyzw_to_R`` + （它有回环测试钉着）。重复实现的下场见 ``sim_truth_perception`` 的模块 docstring。 + """ + from arm_application.handeye_lib import quat_xyzw_to_R + + R = quat_xyzw_to_R(list(quat)) + return math.atan2(float(R[1, 0]), float(R[0, 0])) + + +def poses_from_tf(msg: Any) -> LivePoses: + """``/world//pose/info``（``tf2_msgs/TFMessage``）→ 名 → ``(x,y,z,yaw)``。 + + 每个模型的 ``child_frame_id`` 就是模型名（ros_gz_bridge 的约定）。 + + **纯函数**（只要求对象有 ``transforms``，不依赖 ROS 类型）：这条解析错的表现是 + "实测位姿永远是空的"，于是真值退回名义位姿 —— 又是那个两毫米的错， + 没有任何一处日志会提。所以它必须能脱离节点被钉住。 + """ + out: LivePoses = {} + for tr in getattr(msg, "transforms", None) or []: + name = str(getattr(tr, "child_frame_id", "") or "").strip().lstrip("/") + if not name: + continue + t = tr.transform.translation + q = tr.transform.rotation + out[name] = ( + float(t.x), + float(t.y), + float(t.z), + yaw_from_quat_xyzw((float(q.x), float(q.y), float(q.z), float(q.w))), + ) + return out + + +def spec_with_live_poses( + spec: sim_scene.SceneSpec, + poses: Mapping[str, Tuple[float, float, float, float]], + names: Optional[Iterable[str]] = None, +) -> sim_scene.SceneSpec: + """把场景**副本**里物体的位姿换成实测值；查不到实测位姿的物体保持原样。 + + 为什么只换位姿、不换别的：``SceneSpec`` 里的几何（模型形状、孔环位置、 + 台面）都是配置里的事实，只有"物体摆在哪"是会变的。 + + ``names`` 给定时只换这几个（真值只依赖工件，没必要动别的）。 + 返回**副本**（``dataclasses.replace``）：调用方别把"实测位姿"写回配置， + 否则名义位姿就永远消失了，而它正是判断"有没有被搬走"的基准。 + """ + want = None if names is None else {str(n) for n in names} + out = [] + changed = 0 + for obj in spec.objects: + p = poses.get(obj.name) + if p is None or (want is not None and obj.name not in want): + out.append(obj) + continue + out.append(replace(obj, x=float(p[0]), y=float(p[1]), z=float(p[2]), yaw=float(p[3]))) + changed += 1 + if changed == 0: + return spec + return replace(spec, objects=out) + + +class LiveScenePose: + """订阅世界模型位姿，随时给出"物体现在在哪"。仿真专有。 + + 用法（两个节点都一样，所以放在这里而不是各写一份）:: + + live = LiveScenePose(self, topic) # topic 来自 scene.yaml 的 pose_info + spec_live = live.apply(spec, [obj_name]) # 换上位姿的副本 + if live.ready: ... # 收到过帧没有 + + 没配话题 / 没收到帧时 ``apply`` 原样返回 —— 调用方仍能用名义位姿干活， + 但必须**说清楚**（``LiveScenePose.why_not``），否则"退回名义位姿"这件事 + 又会变成没有人知道的静默行为。 + """ + + def __init__(self, node: Any, topic: str, qos_depth: int = 10) -> None: + self._node = node + self._topic = str(topic or "").strip() + self._poses: LivePoses = {} + self._seen = 0 + self._sub = None + if not self._topic: + return + from tf2_msgs.msg import TFMessage + + self._sub = node.create_subscription(TFMessage, self._topic, self._on_msg, qos_depth) + + def _on_msg(self, msg: Any) -> None: + got = poses_from_tf(msg) + if got: + self._seen += 1 + self._poses.update(got) + + @property + def topic(self) -> str: + return self._topic + + @property + def ready(self) -> bool: + """收到过至少一帧（一帧都没有说明话题名/桥接有问题，不是"物体没动"）。""" + return self._seen > 0 + + def pose(self, name: str) -> Optional[Tuple[float, float, float, float]]: + return self._poses.get(name) + + def apply( + self, + spec: sim_scene.SceneSpec, + names: Optional[Iterable[str]] = None, + ) -> sim_scene.SceneSpec: + return spec_with_live_poses(spec, self._poses, names) + + def why_not(self, name: str) -> str: + """给"为什么还在用名义位姿"一个能直接贴进日志的句子。""" + if not self._topic: + return "scene.yaml 的 pose_info 未配（没有世界模型位姿话题）" + if not self.ready: + return f"话题 {self._topic} 一帧都没收到（桥接没起来？话题名/世界名对不上？）" + return f"实测位姿里没有物体 {name!r}（模型名与 scene.yaml 的 name 不一致？）" diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/sim_pose_service.py b/src/arm_sim_gazebo/arm_sim_gazebo/sim_pose_service.py new file mode 100644 index 0000000..80528ad --- /dev/null +++ b/src/arm_sim_gazebo/arm_sim_gazebo/sim_pose_service.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 +"""把 Gazebo 里的模型**搬到指定位姿**（`/world//set_pose`）。 + +谁在用 +------ +* ``scripts/sim_load_gauge.py``：加载通止规前把场景物体复位； +* ``arm_sim_gazebo/sim_truth_perception.py``：启动时把真值依赖的工件复位 + （见那里的 `_maybe_startup_reset`）。 + +**实现只有这一份**：这个请求字符串以前抄过一遍，而它有一个很坑的细节 +（下面 `--reqtype` 的注释），第二份抄错的人不会知道。 + +为什么走命令行 `ign service` 而不是 ROS 服务桥 +-------------------------------------------- +仿真的场景加载本来就是用命令行 `ros_gz_sim create` 做的（见 `sim_scene.py`）， +口径一致、少一个需要维护的服务桥；而且 `ros_gz_bridge` 对**服务**的桥接需要 +额外配置，桥不上时的表现是"服务一直在、调用永远超时"。 +""" + +from __future__ import annotations + +import math +import re +import subprocess +from typing import Optional, Sequence, Tuple + +#: Gazebo 侧的服务等待超时 [ms]。 +SERVICE_TIMEOUT_MS = 3000 +#: 命令行整体超时 [s]（要大于上面那个，否则会被外面先掐掉）。 +CMD_TIMEOUT_S = 10.0 + + +def world_from_gz_topic(topic: str) -> Optional[str]: + """从 ``/world//pose/info`` 里取出 world 名；取不到返回 ``None``。 + + world 名必须与 ``worlds/assembly.world`` 的文件名一致，而 scene.yaml 里 + 已经写着那条话题了 —— 所以**不要**再单独配一个 world 名参数： + 两份写法迟早不一致，而症状是"服务调用超时"（看起来像 Gazebo 没起来）。 + """ + m = re.fullmatch(r"/world/([^/]+)/pose(?:/info)?/?", (topic or "").strip()) + return m.group(1) if m else None + + +def yaw_to_quat(yaw_rad: float) -> Tuple[float, float, float, float]: + """绕 Z 的 yaw [rad] → 四元数 ``(x, y, z, w)``。 + + 只支持绕 Z：场景里的物体位姿本来就是"平面位姿 + yaw"（``SceneObject`` + 也只有 yaw）—— 这样写反而不会被"顺手支持一下全姿态"拖进来一堆没测过的路径。 + """ + return (0.0, 0.0, math.sin(float(yaw_rad) / 2.0), math.cos(float(yaw_rad) / 2.0)) + + +def _run(cmd: Sequence[str], timeout: float = CMD_TIMEOUT_S) -> Tuple[int, str]: + """跑一条外部命令，返回 ``(退出码, 合并输出)``。**不抛异常**（调用方自己判）。""" + try: + p = subprocess.run(list(cmd), capture_output=True, text=True, timeout=timeout, check=False) + return p.returncode, (p.stdout or "") + (p.stderr or "") + except FileNotFoundError: + return 127, f"找不到可执行文件：{cmd[0]}" + except subprocess.TimeoutExpired: + return 124, f"命令超时（{timeout}s）：{' '.join(cmd)}" + + +def set_model_pose( + world: str, + name: str, + pos: Sequence[float], + quat: Sequence[float], + timeout_s: float = CMD_TIMEOUT_S, +) -> Tuple[bool, str]: + """把模型 ``name`` 搬到世界位姿 ``pos`` / ``quat``（四元数 xyzw）。 + + 返回 ``(成功?, 输出)``。**失败只返回 False，不抛异常**：调用方拿到的场景 + 可能只是"少了一个可选物体"，因为一个失败的复位让整个流程起不来不值得。 + + 注意 ``--reqtype`` 必须写 ``ignition.msgs.Pose``：本环境实测写 + ``gz.msgs.Pose`` 会报 "Unable to create request"，一个字母的差别、 + 报错信息完全没提原因。 + """ + req = ( + f'name: "{name}", ' + f"position: {{x: {float(pos[0]):.6f}, y: {float(pos[1]):.6f}, z: {float(pos[2]):.6f}}}, " + "orientation: {" + f"x: {float(quat[0]):.8f}, y: {float(quat[1]):.8f}, " + f"z: {float(quat[2]):.8f}, w: {float(quat[3]):.8f}" + "}" + ) + code, out = _run( + [ + "ign", "service", + "-s", f"/world/{world}/set_pose", + "--reqtype", "ignition.msgs.Pose", + "--reptype", "ignition.msgs.Boolean", + "--timeout", str(int(SERVICE_TIMEOUT_MS)), + "--req", req, + ], + timeout=timeout_s, + ) + ok = code == 0 and "data: true" in out + return ok, out.strip() + + +def set_model_pose_yaw( + world: str, + name: str, + pos: Sequence[float], + yaw_rad: float, + timeout_s: float = CMD_TIMEOUT_S, +) -> Tuple[bool, str]: + """``set_model_pose`` 的平面版：只有 yaw。""" + return set_model_pose(world, name, pos, yaw_to_quat(yaw_rad), timeout_s) diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/sim_scene.py b/src/arm_sim_gazebo/arm_sim_gazebo/sim_scene.py index fd2d20b..516fb8d 100644 --- a/src/arm_sim_gazebo/arm_sim_gazebo/sim_scene.py +++ b/src/arm_sim_gazebo/arm_sim_gazebo/sim_scene.py @@ -94,6 +94,36 @@ class Operating: part_object: str = "workpiece" +@dataclass(frozen=True) +class PoseInfo: + """Gazebo 世界模型位姿回读的桥接参数 + "物体被撞走"的判据。 + + 为什么要有它：``perception.source=truth`` 是**纯几何**的 —— 它按本文件的 + 名义位姿算孔位，**不去问 Gazebo 物体现在到底在哪**。物体一旦被碰走， + "真值"就开始撒谎，而每一处现象都指向别的原因（详见 scene.yaml 里 + ``pose_info`` 那段注释）。这个结构就是让仿真能把"名义 vs 实际"对上。 + """ + + gz_topic: str + ros_topic: str + ros_type: str = "tf2_msgs/msg/TFMessage" + gz_type: str = "gz.msgs.Pose_V" + #: 部件相对名义位姿的平移超过它 [m] 就算"被搬走了" → 告警（**但仍然发点**， + #: 因为真值几何已经跟着实测位姿走，见 sim_live_pose）。 + drift_tol_m: float = 0.002 + #: 同上，姿态 [deg]。 + drift_tol_deg: float = 2.0 + #: 平移超过它才**停发**真值点 [m]。这个量级说明物体掉了/被搬走了， + #: 场景前提没了（装配没有意义）。2mm 那个量级天天都有（spawn 落地 + + #: 工件在通槽上蠕动），拿它当停发线会把监视器变成"每轮都误报的假故障"。 + drift_stop_m: float = 0.02 + #: 启动时把被盯的物体**实际搬回**名义位姿（仿真专有，默认关）。 + #: 为什么需要、为什么只在启动窗口内做，见 ``sim_truth_perception`` 的 + #: ``_maybe_startup_reset``（一句话：spawn 落地会偏 2~3mm，那不是"被撞走"， + #: 但看门狗分不出来；而运行中的漂移必须报错、不能悄悄搬回去）。 + reset_on_start: bool = False + + @dataclass class SceneSpec: """scene.yaml 的解析结果。""" @@ -105,6 +135,8 @@ class SceneSpec: objects: List[SceneObject] = field(default_factory=list) # 特征名 → (下限, 上限) 像素门槛。没写 = 不设门槛（只报告不判定）。 gates_px: Dict[str, Tuple[float, float]] = field(default_factory=dict) + # 世界模型位姿回读。**没写 = 不做漂移监视**（老场景文件仍然能读）。 + pose_info: Optional[PoseInfo] = None source: Optional[Path] = None @@ -229,10 +261,69 @@ def load_scene(path: Optional[Path] = None, root: Optional[Path] = None) -> Scen operating=operating, objects=objects, gates_px=_parse_gates(p, doc.get("gates_px")), + pose_info=_parse_pose_info(p, doc.get("pose_info")), source=p, ) +def _parse_pose_info(path: Path, raw: object) -> Optional[PoseInfo]: + """解析 pose_info。**没写就返回 None**（= 不做漂移监视）。 + + 与 gates_px 不同，这一段是**可选**的：老场景文件没有它也必须能读。 + 但只要写了，就必须写全 —— 少了话题名会得到一个"订阅了不存在的话题、 + 永远没有数据"的监视器，那比没有监视更糟（看起来"监视器没报警"）。 + """ + if raw is None: + return None + if not isinstance(raw, dict): + raise SceneError( + f"{path} 的 pose_info 不是映射。期望形如：\n" + f" pose_info:\n" + f" gz_topic: /world/assembly/pose/info\n" + f" ros_topic: /sim/world_pose_info" + ) + missing = [k for k in ("gz_topic", "ros_topic") if not str(raw.get(k, "")).strip()] + if missing: + raise SceneError( + f"{path} 的 pose_info 缺字段 {missing}。\n" + f" 这两个话题名必须都写：gz_topic 是 Gazebo 侧的 " + f"/world//pose/info，ros_topic 是桥过来之后的名字。" + ) + try: + tol_m = float(raw.get("drift_tol_m", 0.002)) + tol_deg = float(raw.get("drift_tol_deg", 2.0)) + stop_m = float(raw.get("drift_stop_m", 0.02)) + except (TypeError, ValueError) as exc: + raise SceneError(f"{path} 的 pose_info 容差不是数字: {exc}") from exc + if stop_m <= tol_m: + # 停发线必须**明显大于**告警线，否则"告警"永远和"停发"一起出现， + # 而现实里 2~3mm 的偏差天天有（spawn 落地 + 工件在通槽上蠕动）—— + # 那会让每一轮开局都被假故障拦死，现场学会忽略告警，监视器就废了。 + raise SceneError( + f"{path} 的 pose_info.drift_stop_m={stop_m} 必须大于 drift_tol_m={tol_m}。\n" + f" drift_tol_m 是**告警**线（几何已按实测算，不影响发点），" + f"drift_stop_m 是**停发**线（物体掉了/被搬走了）。" + ) + reset = raw.get("reset_on_start", False) + if not isinstance(reset, bool): + # 不做 `bool(reset)` 强转：YAML 里 `reset_on_start: "false"` 是**真值**， + # 而这类错误的表现是"开关关了却没关"，排查时没人会去怀疑引号。 + raise SceneError( + f"{path} 的 pose_info.reset_on_start 不是布尔量（收到 {reset!r}）。\n" + f" 写 true / false；不要加引号（\"false\" 在 YAML 里是真值）。" + ) + return PoseInfo( + gz_topic=str(raw["gz_topic"]).strip(), + ros_topic=str(raw["ros_topic"]).strip(), + ros_type=str(raw.get("ros_type") or "tf2_msgs/msg/TFMessage").strip(), + gz_type=str(raw.get("gz_type") or "gz.msgs.Pose_V").strip(), + drift_tol_m=tol_m, + drift_tol_deg=tol_deg, + drift_stop_m=stop_m, + reset_on_start=reset, + ) + + def _parse_gates(path: Path, raw: object) -> Dict[str, Tuple[float, float]]: """解析 gates_px。写成 {名字: [下限, 上限]}。 diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/sim_truth_perception.py b/src/arm_sim_gazebo/arm_sim_gazebo/sim_truth_perception.py index 25002b2..dc396b1 100644 --- a/src/arm_sim_gazebo/arm_sim_gazebo/sim_truth_perception.py +++ b/src/arm_sim_gazebo/arm_sim_gazebo/sim_truth_perception.py @@ -35,6 +35,24 @@ torch/ultralytics，而且这台机器**离线**装不上，于是 perception_no * 目标在相机背后 / 相机查不到 TF 时**不发假点**（和 perception_node 的 "没有可用深度就不发点"同一条规矩）。宁可让伺服看门狗超时，也不给它一个 幻影目标 —— 后者会让机械臂朝错误方向走，而且看起来像"标定坏了"。 +* **物体被撞走了就不再发点**。真值感知的整个前提是"物体就在 scene.yaml 写的位置"， + 前提破了，算出来的就是**假测量**。所以本节点自己订阅 ``/world//pose/info`` + （scene.yaml 的 ``pose_info`` 段）盯着部件的实际位姿，偏离名义位姿超过容差就停下 + 并**指名报错**。 + 为什么必须由这一侧盯（2026-09-24 的教训）：``perception.source=truth`` 是纯几何的， + 它从不问 Gazebo 物体现在在哪。实测工件掉进台面通槽、落到地面之后，"真值"开始撒谎， + 而现象全都指向别处（二阶段报"两孔间距 169mm 不像 k1/k2"、S07 一路下压不接触）， + 看起来像检测/标定/阈值坏了 —— 真正的原因只有一个，却没有任何一处日志说出来。 + **谁假设的，谁负责查。** +* **开局那一次例外**（scene.yaml 的 ``pose_info.reset_on_start``）：物体是"在名义位姿 + 上方 spawn、再落下来"的，落稳后实测偏 2.6mm（越过 2mm 容差）—— 那不是"被撞走"， + 但看门狗分不出来，于是每轮开局都可能被这条假故障拦死。所以在**开局窗口内** + （``STARTUP_RESET_WINDOW_S``）把它 set_pose 回名义位姿，只做一次； + 过了窗口再漂就是运行中被撞，必须报错停下、**绝不**搬（见 + ``startup_reset_decision`` 与 ``_maybe_startup_reset``）。 +* 不自己算四元数→旋转矩阵（复用 ``handeye_lib.quat_xyzw_to_R``）。 + 这段换算以前在 4 个 launch 文件里各抄了一遍、错了 3 处，症状是 + "总能到附近、永远差一点"。数学实现只能有一份。 * 不自己算四元数→旋转矩阵（复用 ``handeye_lib.quat_xyzw_to_R``）。 这段换算以前在 4 个 launch 文件里各抄了一遍、错了 3 处，症状是 "总能到附近、永远差一点"。数学实现只能有一份。 @@ -45,7 +63,7 @@ from __future__ import annotations import math from dataclasses import dataclass from pathlib import Path -from typing import Optional, Sequence, Tuple +from typing import Dict, Optional, Sequence, Tuple import numpy as np import rclpy @@ -54,8 +72,9 @@ from rclpy.node import Node from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy from rclpy.time import Time from tf2_ros import Buffer, TransformListener +from tf2_msgs.msg import TFMessage -from arm_sim_gazebo import sim_scene +from arm_sim_gazebo import sim_live_pose, sim_pose_service, sim_scene #: 与 perception_node / servo_node 一致：点话题用 BEST_EFFORT + KEEP_LAST(5)。 #: 匹配不上 QoS 的后果是"话题在、消息发了、订阅方一条也收不到"， @@ -69,6 +88,12 @@ POINT_QOS = QoSProfile( #: 目标点离相机光心太近（或跑到背后）就不发：z<=0 在相机系里是"背后"。 MIN_CAM_Z_M = 0.01 +#: 启动复位的**时间窗口** [s]：只在这段时间内把工件搬回名义位姿。 +#: 过了这段时间还漂 = 运行中被撞走/被压走，那时**绝不能**悄悄搬回去 +#: （机器人可能正握着它，搬回去等于把现场证据抹掉并让真值开始撒谎）， +#: 必须照旧报错停下。整个理由见 ``_maybe_startup_reset``。 +STARTUP_RESET_WINDOW_S = 20.0 + class TruthError(RuntimeError): """真值目标解不出来（场景 / 模型 / 参数有问题）。 @@ -78,6 +103,42 @@ class TruthError(RuntimeError): """ +#: ``startup_reset_decision`` 的决定：不动 / 再等等 / 现在搬 / 放弃。 +RESET_SKIP = "skip" +RESET_WAIT = "wait" +RESET_NOW = "reset" +RESET_GIVE_UP = "give_up" + + +def startup_reset_decision( + now_s: float, + deadline_s: Optional[float], + window_s: float, + pose_known: bool, + any_drift: bool, +) -> Tuple[str, Optional[float]]: + """开局"要不要把物体搬回名义位姿"的判据 → ``(决定, 新的截止时刻)``。 + + **纯函数**：写错的表现两头都很糟 —— + * 判成"运行中也搬"：把"谁把它撞歪了"的现场抹掉，甚至把工件从爪里拽走， + 现象会变成"孔位对不上/下压不接触"，比原故障更难查； + * 判成"永远等"：开局那 2.6mm 落地偏差每轮都把真值点拦死， + S04 报「一个视觉目标点都没收到」，看起来像真值节点根本没起来。 + + 所以它必须能脱离节点被钉住（``test_sim_truth_perception.py``）。 + """ + dl = float(deadline_s) if deadline_s is not None else float(now_s) + float(window_s) + if now_s > dl: + return RESET_GIVE_UP, dl + if not pose_known: + # 还没收到 pose/info 帧：此时"有没有漂移"根本无从判断， + # 等下去（窗口到点会自己变成 give_up）。 + return RESET_WAIT, dl + if not any_drift: + return RESET_SKIP, dl + return RESET_NOW, dl + + @dataclass(frozen=True) class TruthTarget: """真值目标：一个**世界系**点，以及它是怎么算出来的（给人看）。""" @@ -190,6 +251,56 @@ def matrix_from_transform(tr) -> np.ndarray: return T +def pose_deviation( + obj: sim_scene.SceneObject, + p: Sequence[float], + quat: Sequence[float], +) -> Tuple[float, float, float]: + """实际位姿相对名义位姿的偏差 → ``(平移 [m], 姿态夹角 [deg], 实测 yaw [rad])``。 + + **纯函数**：这条判据搞错了的表现是"监视器静默失声" —— + 要么永远不报（物体明明被撞走了却没人说），要么天天误报（现场学会忽略它）。 + 两种情况都比没有监视更糟，所以它必须能脱离节点被钉住。 + """ + from arm_application.handeye_lib import quat_xyzw_to_R + + d = math.dist((float(obj.x), float(obj.y), float(obj.z)), [float(v) for v in p[:3]]) + R_now = quat_xyzw_to_R([float(v) for v in quat]) + yaw_now = math.atan2(float(R_now[1, 0]), float(R_now[0, 0])) + # 姿态用**整段旋转差**的夹角，不只比 yaw：板被碰一下往往是"翘起来夹着转了一点"， + # 只比 yaw 会漏掉翘起，而翘起恰恰最影响孔的成像位置。 + c, s = math.cos(float(obj.yaw)), math.sin(float(obj.yaw)) + R_ref = np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]]) + cosang = (float(np.trace(R_ref.T @ R_now)) - 1.0) / 2.0 + ang = math.degrees(math.acos(max(-1.0, min(1.0, cosang)))) + return (d, ang, yaw_now) + + +def pose_drift( + obj: sim_scene.SceneObject, + p: Sequence[float], + quat: Sequence[float], + tol_m: float, + tol_deg: float, +) -> Optional[str]: + """偏差超过容差时的**说明文字**；在容差内返回 ``None``（= 没漂移）。 + + 给日志/告警用的人话版；数值判据在 :func:`pose_deviation`（同一个实现， + 免得两处各算一遍距离）。 + """ + d, ang, yaw_now = pose_deviation(obj, p, quat) + if d <= tol_m and ang <= tol_deg: + return None + return ( + f"名义 (x,y,z)=({obj.x:.4f},{obj.y:.4f},{obj.z:.4f}) " + f"yaw={math.degrees(float(obj.yaw)):.1f}°\n" + f" 实际 (x,y,z)=({p[0]:.4f},{p[1]:.4f},{p[2]:.4f}) " + f"yaw={math.degrees(yaw_now):.1f}°\n" + f" 偏差 |Δp|={d * 1000:.1f}mm、姿态差 {ang:.1f}°" + f"（容差 {tol_m * 1000:.1f}mm / {tol_deg:.1f}°）" + ) + + class SimTruthPerception(Node): """把真值目标点按固定频率发到 ``visual_servo/filtered_point``。""" @@ -214,9 +325,15 @@ class SimTruthPerception(Node): self.declare_parameter("publish_hz", 20.0) """发布频率：不必跟着相机帧率，伺服 100Hz 控制会自己插值/外推。""" + self.declare_parameter("pose_info_topic", "") + """世界模型位姿话题（``tf2_msgs/TFMessage``）。 + + 空 = 用 scene.yaml 的 ``pose_info.ros_topic``；场景里也没配 = 真值**按名义 + 位姿**算（几何会偏，且没有"物体被搬走"的告警）。""" + scene_file = str(self.get_parameter("scene_file").value or "").strip() self._scene_file = Path(scene_file) if scene_file else None - offset = self.get_parameter("offset_m").value or [0.0, 0.0, 0.0] + self._offset = offset = self.get_parameter("offset_m").value or [0.0, 0.0, 0.0] # 长度的校验在 target_point_world 里（纯函数，有测试钉着），这里不重复。 self._camera_frame = str(self.get_parameter("camera_frame").value or "").strip() if not self._camera_frame: @@ -231,6 +348,9 @@ class SimTruthPerception(Node): # 目标点在启动时算一次就够：场景是静态的（工件被推动后位置会变， # 但那种情况下"真值"本身也就失效了 —— 此时应该重跑 gen_sim_handeye # 或重启，而不是悄悄跟着一个被撞歪的位姿走）。 + # ⚠️ 这句话在 2026-09-24 之前只是**注释**：工件真的被碰走了，没有任何人 + # 发现（详见 scene.yaml 的 pose_info 段）。现在由 _setup_drift_watch + # 把它变成**运行时的判据**：一旦对上不，立刻停发并指名报错。 self._spec, self._target = load_truth_target( self._scene_file, str(self.get_parameter("target_object").value or ""), @@ -244,6 +364,17 @@ class SimTruthPerception(Node): TransformListener(self._tf, self, spin_thread=True) self._sent = 0 self._warned_tf = 0 + self._warned_drift = 0 + self._t_start: Optional[float] = None + # 目标点按**实测**位姿算（见模块 docstring 与 sim_live_pose）， + # 位姿没变就不重算（算一次要读模型 SDF，20Hz 下不能每拍都读）。 + self._target_live: Optional[TruthTarget] = None + self._target_key: Optional[Tuple[float, ...]] = None + self._warned_nominal = 0 + # 启动复位（scene.yaml 的 pose_info.reset_on_start）：见 _maybe_startup_reset。 + self._reset_done = False + self._reset_deadline: Optional[float] = None + self._setup_drift_watch() hz = max(1.0, min(100.0, float(self.get_parameter("publish_hz").value))) self.create_timer(1.0 / hz, self._tick) @@ -260,7 +391,254 @@ class SimTruthPerception(Node): ) # ------------------------------------------------------------------ + # 漂移监视：真值感知的"前提自检" + # ------------------------------------------------------------------ + def _setup_drift_watch(self) -> None: + """订阅世界模型位姿，盯住"被真值当作几何基准"的物体现在在哪。 + + 两件事一起做，因为它们是同一份数据： + * **量测**：给真值目标/孔位提供**实测**位姿（``sim_live_pose``）—— + 这才是真值该用的几何（见模块 docstring）； + * **告警**：实测位姿相对 ``scene.yaml`` 的名义位姿偏了多少。 + 超过 ``drift_tol_m`` **告警但继续发**（几何已经跟着实测走， + 没有幻影目标）；超过 ``drift_stop_m`` 才**停发**并指名报错 + （那个量级说明物体掉了/被搬走了，场景前提没了）。 + + 为什么告警与停发要分开（2026-09-24 实测）： + 物体 spawn 落地就偏 2.6mm、骑在通槽上还会缓慢再挪 1.9mm —— + 这个量级**天天都会出现**，而它正好压在"2mm"这条线上。用同一条阈值 + 既停发又告警，结果是"每轮开局都可能被假故障拦死"， + 而现场的正确反应是"忽略这条告警" —— 监视器一旦被学会忽略就废了。 + + 只盯**工件**：它是二阶段孔位与伺服目标的几何基准。 + 通止规刻意不盯 —— 它本来就要被夹走、被搬动，那正是流程的一部分。 + """ + pi = self._spec.pose_info + topic = str(self.get_parameter("pose_info_topic").value or "").strip() + if not topic and pi is not None: + topic = pi.ros_topic + # 只盯工件：它是二阶段孔位与伺服目标的几何基准。 + want = self._spec.operating.part_object + self._nominal_obj = next((o for o in self._spec.objects if o.name == want), None) + self._tol_m = float(pi.drift_tol_m if pi is not None else 0.002) + self._tol_deg = float(pi.drift_tol_deg if pi is not None else 2.0) + self._stop_m = float(getattr(pi, "drift_stop_m", 0.02) if pi is not None else 0.02) + # 启动复位要用 world 名（服务路径里），而 world 名从 gz_topic 推出来 —— + # 不另配一个参数：两份写法迟早不一致，症状是"服务调用超时"。 + self._reset_world = ( + sim_pose_service.world_from_gz_topic(pi.gz_topic) if pi is not None else None + ) + self._reset_on_start = bool(pi.reset_on_start) if pi is not None else False + self._live = sim_live_pose.LiveScenePose(self, topic) + + if self._nominal_obj is None: + self.get_logger().warn( + f"scene.yaml 的 operating.part_object={want!r} 不在 objects 里 —— " + f"实测位姿无从取得（只能按名义位姿算，几何会偏）。" + ) + if not topic: + self.get_logger().info( + "没有世界模型位姿话题（scene.yaml 的 pose_info 未配）—— " + "真值将**按名义位姿**算目标点与孔位（物体一动就偏，实测偏 2~3mm " + "就会让规尖进不了孔），而且没有「物体被搬走」的告警。" + ) + return + self.get_logger().info( + f"已订阅 {topic}：真值目标按**实测**位姿算；与该物体在 scene.yaml 的" + f"名义位姿相差超过 {self._tol_m * 1000:.1f}mm / {self._tol_deg:.1f}° 告警，" + f"超过 {self._stop_m * 1000:.0f}mm 停发。" + ) + if self._reset_on_start: + if self._reset_world is None: + self.get_logger().error( + f"pose_info.reset_on_start=true，但从 gz_topic " + f"{pi.gz_topic!r} 里认不出 world 名 —— **启动复位不会执行**。\n" + f" 期望形如 /world//pose/info，world 名 = " + f"worlds/assembly.world 的文件名。\n" + f" 后果：spawn 落地的 2~3mm 偏差会被当成" + f"「场景被动过」，告警一直刷。" + ) + else: + self.get_logger().info( + f"启动复位已开：开局（{STARTUP_RESET_WINDOW_S:.0f}s 内）会把被盯的物体" + f"搬回名义位姿（set_pose /world/{self._reset_world}/set_pose）。" + ) + + # ------------------------------------------------------------------ + # 实测位姿 → 目标点 + # ------------------------------------------------------------------ + def _live_deviation(self) -> Optional[Tuple[float, float, str]]: + """实测位姿相对名义位姿的偏差 ``(d [m], ang [deg], 说明)``；没有实测返回 None。""" + obj = getattr(self, "_nominal_obj", None) + if obj is None: + return None + p = self._live.pose(obj.name) + if p is None: + return None + d, ang, _ = pose_deviation(obj, p[:3], sim_pose_service.yaw_to_quat(p[3])) + reason = ( + pose_drift(obj, p[:3], sim_pose_service.yaw_to_quat(p[3]), self._tol_m, self._tol_deg) + or "" + ) + return (d, ang, reason) + + def _current_target(self) -> Tuple[TruthTarget, bool]: + """按实测位姿算目标点（位姿没变就复用）。返回 ``(目标, 是否按实测算的)``。 + + 实测位姿拿不到时**退回名义位姿**并把这个事实报给调用方 —— + 静默退回是这条链上最贵的错（几何偏两毫米、没有任何日志提起）。 + """ + obj = getattr(self, "_nominal_obj", None) + p = self._live.pose(obj.name) if obj is not None else None + if obj is None or p is None: + return self._target, False + key = tuple(round(float(v), 7) for v in p) + if self._target_key == key and self._target_live is not None: + return self._target_live, True + spec_live = self._live.apply(self._spec, [obj.name]) + live_obj = pick_object(spec_live, obj.name) + t = target_point_world(spec_live, live_obj, self._offset) + self._target_key = key + self._target_live = t + return t, True + + # ------------------------------------------------------------------ + # 启动复位：把"落地的 2~3mm"与"被撞走"分开 + # ------------------------------------------------------------------ + def _maybe_startup_reset(self) -> None: + """开局把被盯的物体**实际搬到**名义位姿（只做一次，且有时间窗）。 + + 为什么需要（2026-09-24 实测，别再当玄学）： + 物体是"在名义位姿上方 spawn、再落下来"的，落稳后实测偏 **2.6mm** + （(0.5175, 0.0008) vs 名义 (0.5200, 0.0000)），正好越过 2mm 容差 —— + 它不是"被撞走"，但看门狗**分不出来**，于是每一轮开局都可能被这条 + 假故障拦死：S04 等不到目标点、报「一个视觉目标点都没收到」。 + 而把它 set_pose 回名义位姿之后，它**停在那儿不动**（实测）—— + 说明名义位姿本身就是稳定平衡点，差的只是 spawn 那一下的落点。 + + 为什么只在窗口内做、且**只做一次**： + 运行中漂移的唯一合法解释是"被机器人撞了/压了"。那时悄悄搬回去等于 + 抹掉现场证据（而现象会变成"孔位对不上/下压不接触"，更难查）， + 还可能把工件从爪里拽出来。所以过了窗口就照旧报错停下。 + + 判据本身在纯函数 ``startup_reset_decision`` 里（有单测钉着）， + 这里只负责"按决定去调服务、并把话说明白"。 + """ + if self._reset_done or not self._reset_on_start or self._reset_world is None: + return + now = self.get_clock().now().nanoseconds * 1e-9 + dev = self._live_deviation() + decision, deadline = startup_reset_decision( + now_s=now, + deadline_s=self._reset_deadline, + window_s=STARTUP_RESET_WINDOW_S, + pose_known=self._live.ready, + any_drift=bool(dev is not None and (dev[2] or dev[0] > self._tol_m)), + ) + self._reset_deadline = deadline + if decision == "wait": + return + if decision == "skip": + self._reset_done = True # 本来就在位（或还没量到足够信息）：不必搬 + return + if decision == "give_up": + # 窗口内没成功（比如 set_pose 服务不通）→ 放弃并说清楚， + # 之后就是告警/停发的正常行为（真值几何已经跟着实测算，不影响发点）。 + self._reset_done = True + self.get_logger().warn( + f"启动复位窗口（{STARTUP_RESET_WINDOW_S:.0f}s）内没能把物体搬回名义位姿 —— " + f"放弃复位；真值几何按实测位姿算（不受影响），" + f"但告警会一直刷「与名义位姿不一致」。" + ) + return + + self._reset_done = True + obj = getattr(self, "_nominal_obj", None) + if obj is None: + return + reason = (dev[2] if dev else "") or "" + ok, out = sim_pose_service.set_model_pose_yaw( + self._reset_world, obj.name, (obj.x, obj.y, obj.z), float(obj.yaw) + ) + if ok: + self.get_logger().warn( + f"启动复位：{obj.name!r} 不在名义位姿" + f"（{reason.splitlines()[-1].strip() if reason else ''}）" + f"—— 已 set_pose 回 ({obj.x:.4f},{obj.y:.4f},{obj.z:.4f}) " + f"yaw={math.degrees(float(obj.yaw)):.1f}°。\n" + f" 这是**开局**的正常现象（spawn 落地会偏 2~3mm），不是故障；" + f"复位后实测位姿等于名义位姿。" + ) + else: + self.get_logger().error( + f"启动复位失败：{obj.name!r} 没能 set_pose 回名义位姿 —— {out[:200]}\n" + f" 影响有限：真值几何按实测位姿算，仍能发点；但告警会一直刷。" + ) + def _tick(self) -> None: + # 前提自检的**活性检查**：订阅了话题却一帧都没收到，等于没有监视 —— + # 而那正是"以为有保护、实际裸奔"。所以这里必须把这种情况说出来。 + if self._live.topic and not self._live.ready: + if self._t_start is None: + self._t_start = self.get_clock().now().nanoseconds * 1e-9 + elif self.get_clock().now().nanoseconds * 1e-9 - self._t_start > 8.0: + self.get_logger().warn( + "盯物体位姿的话题**一帧都没收到** —— 真值只能退回**名义位姿**" + "（几何会偏，实测偏 2~3mm 就能让规尖进不了孔）。\n" + " 常见原因：gazebo_sim.launch.py 里的 pose_info_bridge 没起来，" + "或 scene.yaml 的 pose_info.gz_topic 与 world 名对不上\n" + " （必须等于 /world//pose/info，world 名 = " + "worlds/assembly.world 的文件名）。", + throttle_duration_sec=10.0, + ) + # 先给"开局落地偏差"一次纠偏机会（见 _maybe_startup_reset）。 + self._maybe_startup_reset() + + dev = self._live_deviation() + if dev is None and self._nominal_obj is not None and self._live.ready: + # 话题有帧，就是没有这个模型 → 名字对不上，说出来。 + self.get_logger().warn( + f"实测位姿里没有物体 {self._nominal_obj.name!r}：" + f"{self._live.why_not(self._nominal_obj.name)}", + throttle_duration_sec=10.0, + ) + elif dev is not None: + d, ang, reason = dev + if d > self._stop_m: + # 这个量级说明物体掉了/被搬走了：场景前提没了，**不发点**。 + self._warned_drift += 1 + if self._warned_drift in (1, 5, 30) or self._warned_drift % 300 == 0: + self.get_logger().error( + f"仿真物体 {self._nominal_obj.name!r} 已经不在 scene.yaml 说的地方了 —— " + f"**停止发真值点**（偏了 {d * 1000:.0f}mm，超过 " + f"{self._stop_m * 1000:.0f}mm 这条线）。\n" + f" {reason}\n" + f" ⚠ 为什么停：物体掉了/被搬走之后，装配就没有意义了 ——" + f"再发点等于指挥机械臂去装一个不存在的工件。\n" + f" 修法：把 {self._nominal_obj.name!r} 放回 scene.yaml 的位置" + f"（真机现场就改 scene.yaml），或重启仿真让它按场景重新 spawn。\n" + f" 排查提示：台面沿 X 有一道 36mm 通槽（|y| ≤ 18mm）；" + f"工件若跨槽时被压翻就会掉下去。" + ) + return + if reason: + # 2~3mm 这个量级**天天都有**（spawn 落地 + 通槽上缓慢蠕动）， + # 而几何已经跟着实测走，所以只告警、**继续发点**。 + self.get_logger().warn( + f"物体 {self._nominal_obj.name!r} 与 scene.yaml 的名义位姿不一致 —— " + f"真值几何已按**实测**位姿算，不受影响；只是提醒你场景被动过。\n" + f" {reason}\n" + f" （超过 {self._stop_m * 1000:.0f}mm 才停发）", + throttle_duration_sec=30.0, + ) + + target, live = self._current_target() + if not live and self._warned_nominal < 3: + self._warned_nominal += 1 + self.get_logger().warn( + f"真值目标暂时按**名义位姿**算（{self._live.why_not(target.object_name)}）—— " + f"物体一动几何就偏，实测偏 2~3mm 就会让规尖进不了孔。" + ) try: tr = self._tf.lookup_transform(self._camera_frame, self._world_frame, Time()) except Exception as exc: # noqa: BLE001 —— tf2 的异常类型很杂，统一当"还没有" @@ -274,14 +652,14 @@ class SimTruthPerception(Node): throttle_duration_sec=5.0, ) return - p = apply_transform(matrix_from_transform(tr), self._target.world_xyz) + p = apply_transform(matrix_from_transform(tr), target.world_xyz) if p[2] < MIN_CAM_Z_M: # 目标在相机背后或贴到镜头上：不发！发出去伺服会朝反方向走。 self.get_logger().warn( f"真值目标不在相机前方（相机系 z={p[2] * 1000:.0f}mm < " f"{MIN_CAM_Z_M * 1000:.0f}mm）—— 不发点。" - f"世界系点 ({self._target.world_xyz[0]:.3f}, " - f"{self._target.world_xyz[1]:.3f}, {self._target.world_xyz[2]:.3f})" + f"世界系点 ({target.world_xyz[0]:.3f}, " + f"{target.world_xyz[1]:.3f}, {target.world_xyz[2]:.3f})" f" 不在 {self._camera_frame} 视野里；先让臂摆到能看到它的位置。", throttle_duration_sec=5.0, ) diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/sim_wrist_ft.py b/src/arm_sim_gazebo/arm_sim_gazebo/sim_wrist_ft.py new file mode 100644 index 0000000..2acb2ee --- /dev/null +++ b/src/arm_sim_gazebo/arm_sim_gazebo/sim_wrist_ft.py @@ -0,0 +1,159 @@ +"""仿真腕部力扭矩（wrist force/torque）的配置与桥接规则。 + +这一层只做两件事，别的都不做： + + 1. 从 ``devices/sim/gazebo/config/wrist_ft.yaml`` 读话题与关节名 + 2. 生成 ros_gz_bridge 的参数（gz 消息 → ROS 消息） + +**为什么不需要适配器节点**（与触觉不同）： + 触觉必须有 ``sim_tactile_adapter``，因为要做的语义工作很多 —— 轴向映射、 + 基线相减、单位换算、接触超时、把 ``Contacts`` 折算成 contact 标志。 + 腕部 FT 不需要任何这些：Gazebo 的 force_torque 直接给的就是**牛顿**， + 坐标系是 ``child`` 连杆系（工具竖直时 z 即轴向），而 ros_gz_bridge + **原生支持** gz.msgs.Wrench → geometry_msgs/msg/WrenchStamped（实测确认）。 + 于是仿真的输出与真机 ``//ft_sensor_wrench`` **同名同类型同单位**， + 应用层一行分支都不用写。 + + 少一层就少一个"配错了不报错"的地方，所以这里刻意不加节点。 + +与真机的对应： + 真机 //ft_sensor_wrench geometry_msgs/WrenchStamped（法兰系，牛顿） + 仿真 /sim/ft_sensor_wrench geometry_msgs/WrenchStamped（link7 系，牛顿） + ← 名字由 site.yaml 的 robot.arms[sim].sn 决定 +""" + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +from typing import Any, Dict, List, Optional + +import yaml + +#: 相对仓库根的默认配置路径。 +DEFAULT_WRIST_FT_REL = "devices/sim/gazebo/config/wrist_ft.yaml" + + +class WristFtConfigError(RuntimeError): + """腕部 FT 配置缺失或写错。消息里必须给出修法（现场就靠这句话）。""" + + +@dataclass(frozen=True) +class WristFtSpec: + """腕部 FT 的一份完整描述。""" + + topic: str + joint: str + ros_type: str + gz_type: str + publish_hz: float + + +def load_wrist_ft( + path: Optional[Path] = None, root: Optional[Path] = None +) -> WristFtSpec: + """读腕部 FT 配置。找不到或写错时抛 WristFtConfigError，消息里给出修法。""" + if path is None: + base = root if root is not None else Path.cwd() + path = Path(base) / DEFAULT_WRIST_FT_REL + path = Path(path) + + if not path.is_file(): + raise WristFtConfigError( + f"腕部 FT 配置不存在: {path}\n" + f" 它声明了仿真腕部力扭矩的话题名与挂载关节（桥接从它派生）。\n" + f" 缺了它，S07 的接触判据在仿真里永远没有数据源。\n" + f" 确认仓库根下存在 {DEFAULT_WRIST_FT_REL}。" + ) + + try: + doc = yaml.safe_load(path.read_text(encoding="utf-8")) or {} + except yaml.YAMLError as exc: + raise WristFtConfigError( + f"腕部 FT 配置 YAML 解析失败: {path}\n 原因: {exc}" + ) from exc + + if not isinstance(doc, dict): + raise WristFtConfigError(f"{path} 的顶层应该是映射（key: value 形式）。") + + missing = [ + k for k in ("topic", "joint", "ros_type", "gz_type") if not str(doc.get(k, "")).strip() + ] + if missing: + raise WristFtConfigError( + f"{path} 缺字段: {', '.join(missing)}。\n" + f" topic 必须与 arm.sim.sensors.xacro 里 wrist_ft_topic 的默认值\n" + f" 完全一致，也要与应用层按 sn 推出的名字一致 —— 对不上时桥接器只是\n" + f" 订阅了一个不存在的话题，不报错，而 S07 会一直判不到接触。" + ) + + try: + hz = float(doc.get("publish_hz", 100.0)) + except (TypeError, ValueError) as exc: + raise WristFtConfigError( + f"{path} 的 publish_hz 不是数字: {doc.get('publish_hz')!r}" + ) from exc + if hz <= 0.0: + raise WristFtConfigError(f"{path} 的 publish_hz 必须为正，收到 {hz}") + + return WristFtSpec( + topic=str(doc["topic"]).strip(), + joint=str(doc["joint"]).strip(), + ros_type=str(doc["ros_type"]).strip(), + gz_type=str(doc["gz_type"]).strip(), + publish_hz=hz, + ) + + +def bridge_args(spec: WristFtSpec) -> List[str]: + """生成 ros_gz_bridge parameter_bridge 的话题规则。 + + 只做**传输**：gz 类型 → ROS 类型，话题名两侧保持同名。 + 这条规则的终点就是应用层真正订阅的那个话题，所以话题名必须一次写对 —— + 桥接器没有"改名"能力，写错只会得到"话题上没数据"。 + """ + return [f"{spec.topic}@{spec.ros_type}[{spec.gz_type}"] + + +def check_wrist_ft_sensor( + urdf_text: str, spec: WristFtSpec +) -> List[str]: + """描述级静态检查：URDF 里到底有没有那个腕部 FT 传感器。 + + 为什么需要（与 ``check_tactile_collisions`` 同一个理由）： + Gazebo 对**挂错关节**的传感器不报错 —— 传感器会被静默丢弃， + 话题永远没有数据，现象是"S07 判不到接触"。光看 launch 日志完全看不出来 + （桥接器和 Gazebo 都说一切正常）。这里在启动时就把它指出来。 + + 返回告警列表（空 = 没问题）；**不抛异常** —— 缺了它仿真仍能跑， + 只是 S07 的判据要退回触觉兜底，那是降级不是崩溃。 + """ + warnings: List[str] = [] + if not urdf_text: + return ["URDF 文本为空，无法核对腕部 FT 传感器"] + + if f'reference="{spec.joint}"' not in urdf_text: + near = [ln for ln in urdf_text.splitlines() if "gazebo reference=" in ln and "joint" in ln] + warnings.append( + f"URDF 里没有 —— " + f"腕部 FT 传感器挂错关节了，它会被 Gazebo 静默丢弃、话题永远没数据。\n" + f" 期望挂在 {spec.joint}（最后一个 revolute 关节：它下游挂着整个腕部以下" + f" 的载荷，这才是工程意义上的腕部 FT）。\n" + f" 注意不能挂 fixed 关节（如 flange_joint）——物理引擎不把它当独立关节" + f" 求解，传感器永远没输出且不报错。\n" + f" 当前 URDF 里的 关节有: {', '.join(sorted(set(near))) or '（无）'}" + ) + + if spec.topic not in urdf_text: + warnings.append( + f"URDF 里找不到腕部 FT 的话题 {spec.topic} —— 传感器的话题名与配置不一致。\n" + f" 两侧必须一致：config 的 topic、arm.sim.sensors.xacro 的 wrist_ft_topic、\n" + f" 以及应用层按 sn 推出的 /{{sn}}/ft_sensor_wrench（仿真 sn = sim）。" + ) + + if "force_torque" not in urdf_text: + warnings.append( + "URDF 里没有任何 type=\"force_torque\" 传感器 —— 腕部 FT 不会被创建。" + ) + + return warnings diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/truth_holes.py b/src/arm_sim_gazebo/arm_sim_gazebo/truth_holes.py index 80a422a..a8b9a23 100644 --- a/src/arm_sim_gazebo/arm_sim_gazebo/truth_holes.py +++ b/src/arm_sim_gazebo/arm_sim_gazebo/truth_holes.py @@ -59,6 +59,20 @@ class HoleTruth: #: 两孔的像素半径（环的物理半径按透视缩放；Hough 路径给的是它自己的估计， #: 真值路径给它一个**可信的**值 —— 上下游只把它当"尺度参考"用） hole_radii: List[float] = field(default_factory=list) + #: 两孔**环心的世界坐标**（米），与 ``hole_points`` 一一对应、同序。 + #: + #: 为什么真值路径要把三维也带出来（2026-09-24 的教训）： + #: 原来只给像素，调用方（stage2）再拿**深度图**反投影回三维。仿真里 + #: 那一步是错的，而且错法很隐蔽：孔是**贯通孔**，孔位正下方就是台面 + #: 通槽 —— 深度相机在孔位看到的是**槽底/地面**（实测 0.62m）， + #: 而工件面其实只有 0.21m。拿 0.21m 的像素尺度去乘 0.62m 的深度， + #: 三角化出的 k1/k2 间距变成 169mm（真值 58mm），被间距门槛丢掉。 + #: 真值路径**本来就知道**孔在世界系的精确位置，没有理由再绕一圈深度图。 + hole_world: List[Tuple[float, float, float]] = field(default_factory=list) + #: 工件**上表面中心**的世界坐标（粗居中用；同样是"真值就该直接用"）。 + part_center_world: Optional[Tuple[float, float, float]] = None + #: 工件上表面的世界 z（= 二阶段的对孔基准面）。 + part_top_z: Optional[float] = None #: 工件在画面里占的框（孔检测的 ROI 用；也是"工件中心"的来源） bbox: Tuple[int, int, int, int] = (0, 0, 0, 0) #: 给人看的说明（进了日志才知道这个数是哪来的） @@ -154,9 +168,11 @@ def locate_holes( pts: List[Tuple[float, float]] = [] rad: List[float] = [] + holes_world: List[Tuple[float, float, float]] = [] for ring in rings: wx, wy = _object_world_xy(obj, ring.x, ring.y) - got = project_point(T_cam_world, intr, (wx, wy, float(obj.z) + ring.z)) + wz = float(obj.z) + float(ring.z) + got = project_point(T_cam_world, intr, (wx, wy, wz)) if got is None: return HoleTruth( ok=False, @@ -165,6 +181,7 @@ def locate_holes( u, v, z = got pts.append((u, v)) rad.append(float(intr[0]) * float(ring.radius) / z) + holes_world.append((wx, wy, wz)) # k1/k2 的左右（上下）规则与 Hough 路径一致：水平对按 x，竖直对按 y。 (p, q) = pts @@ -173,11 +190,24 @@ def locate_holes( if pts[0][key] > pts[1][key]: pts = [pts[1], pts[0]] rad = [rad[1], rad[0]] + holes_world = [holes_world[1], holes_world[0]] - bbox = object_bbox_px(T_cam_world, intr, sim_scene.world_boxes(spec, obj, root), image_shape) + boxes = sim_scene.world_boxes(spec, obj, root) + bbox = object_bbox_px(T_cam_world, intr, boxes, image_shape) if bbox is None: return HoleTruth(ok=False, why="工件整体投影不出画面框（角点都在相机背后）") + # 工件上表面中心与上表面 z：从各碰撞体的世界 AABB 取（AABB 是轴对齐的， + # 只有 yaw 的物体上它正好等于"外接矩形"，对"粗居中"这个用途是合适的）。 + part_center: Optional[Tuple[float, float, float]] = None + part_top: Optional[float] = None + if boxes: + xs = [float(v) for b in boxes for v in (b[0], b[1])] + ys = [float(v) for b in boxes for v in (b[2], b[3])] + zs = [float(v) for b in boxes for v in (b[4], b[5])] + part_top = max(zs) + part_center = (0.5 * (min(xs) + max(xs)), 0.5 * (min(ys) + max(ys)), part_top) + dist_px = math.hypot(pts[0][0] - pts[1][0], pts[0][1] - pts[1][1]) note = ( f"{obj.name}（模型 {obj.model}） 环心距 {dist_px:.1f}px " @@ -188,6 +218,9 @@ def locate_holes( why="", hole_points=[(float(pts[0][0]), float(pts[0][1])), (float(pts[1][0]), float(pts[1][1]))], hole_radii=[float(rad[0]), float(rad[1])], + hole_world=holes_world, + part_center_world=part_center, + part_top_z=part_top, bbox=bbox, note=note, ) diff --git a/src/arm_sim_gazebo/launch/gazebo_sim.launch.py b/src/arm_sim_gazebo/launch/gazebo_sim.launch.py index d814490..ceb3cb5 100644 --- a/src/arm_sim_gazebo/launch/gazebo_sim.launch.py +++ b/src/arm_sim_gazebo/launch/gazebo_sim.launch.py @@ -103,11 +103,57 @@ def _prepare(context, *args, **kwargs): world_path, _world_origin = resolve_world(world, root, fallback=default_world(root)) world_path = str(world_path) + # 腕部 FT 的话题名先读出来，喂给 xacro —— 这个话题名有三处要对上 + # （配置、URDF 里的 、应用层按 sn 推出的 /{sn}/ft_sensor_wrench）， + # 让配置当唯一源头，xacro 不再自己写一份。 + from arm_sim_gazebo.sim_wrist_ft import ( + WristFtConfigError, + bridge_args as wrist_ft_bridge_args, + check_wrist_ft_sensor, + ) + from arm_sim_gazebo.sim_wrist_ft import load_wrist_ft + + try: + wft = load_wrist_ft(root=root) + except WristFtConfigError as exc: + raise RuntimeError(f"\n[gazebo_sim] 仿真腕部 FT 配置有问题：\n{exc}\n") from exc + + # 抓取锁：仿真里唯一能保证"夹住的东西不会在机械臂连续运动时滑掉"的机制， + # 说明见 devices/sim/gazebo/config/grasp_lock.yaml 与 sim_grasp_lock.py。 + from arm_sim_gazebo.sim_grasp_lock import ( + DEFAULT_OBJECT_SDF_TMPL, + GraspLockError, + check_grasp_lock_sdf, + ) + from arm_sim_gazebo.sim_grasp_lock import load_grasp_lock + + try: + glock = load_grasp_lock(root=root) + except GraspLockError as exc: + raise RuntimeError( + f"\n[gazebo_sim] 仿真抓取锁配置有问题：\n{exc}\n" + f" （只想先跑起来的话，把 grasp_lock.yaml 的 enabled 改成 false）\n" + ) from exc + + # 静态核对：物体 SDF 里到底有没有那个插件、参数对不对得上。 + # 差一处就**静默失效**（现象只是"规莫名掉了"），所以必须在这里说清楚。 + glock_warnings: List[str] = [] + if glock.enabled: + _obj_sdf = root / DEFAULT_OBJECT_SDF_TMPL.format(object=glock.object) + try: + _sdf_text = _obj_sdf.read_text(encoding="utf-8") + except OSError: + _sdf_text = "" + glock_warnings = check_grasp_lock_sdf( + _sdf_text, glock, sdf_label=str(_obj_sdf) + ) + urdf, report = prepare_urdf( xacro_path=default_xacro(root), controllers_file=default_controllers(root), load_gripper=load_gripper != "false", with_sensors=with_sensors != "false", + wrist_ft_topic=wft.topic, ) wname = world_name(world_path) missing_base = check_base_plugins(Path(world_path)) @@ -120,7 +166,10 @@ def _prepare(context, *args, **kwargs): LogInfo(msg=f"[gazebo_sim] 仓库根: {root}"), LogInfo(msg=f"[gazebo_sim] 世界: {world_path} (world 名: {wname})"), LogInfo(msg=f"[gazebo_sim] URDF 归一化: {report.summary()}"), + LogInfo(msg=f"[gazebo_sim] {glock.describe()}"), ] + for _w in glock_warnings: + actions.append(LogInfo(msg=f"[gazebo_sim] ⚠ 抓取锁检查: {_w}")) if missing_base: actions.append( LogInfo( @@ -191,6 +240,43 @@ def _prepare(context, *args, **kwargs): ) ) + # --- 3b) 世界模型位姿回读：物体被撞走时唯一能指名报错的数据源 ------------ + # 为什么值得一条独立桥接：`perception.source=truth` 是**纯几何**的， + # 它按 scene.yaml 的**名义位姿**算孔位，从不问 Gazebo 物体现在在哪。 + # 工件被碰走（实测：掉进台面通槽落到地面）之后，"真值"开始撒谎， + # 而现象全都指向别处（S05 报"两孔间距 169mm"、S07 一路下压不接触）。 + # 这一条把 gz 的"每个模型实际位姿"搬进 ROS，让 sim_truth_perception + # 能把它和 scene.yaml 对上、指名报"工件不在它该在的地方"。 + # 具体参数（话题名/类型/漂移容差）在 scene.yaml 的 pose_info 段。 + from arm_sim_gazebo.sim_scene import load_scene as _load_scene + + try: + _spec = _load_scene(None, root) + except Exception as exc: # noqa: BLE001 —— 场景读不出来要说清是场景的问题 + raise RuntimeError(f"\n[gazebo_sim] 场景配置有问题：\n{exc}\n") from exc + if _spec.pose_info is not None: + _pi = _spec.pose_info + actions.append( + Node( + package="ros_gz_bridge", + executable="parameter_bridge", + name="pose_info_bridge", + output="screen", + arguments=[f"{_pi.gz_topic}@{_pi.ros_type}[{_pi.gz_type}"], + # gz 的话题名固定是 /world//pose/info（命名空间由 gz 决定， + # 应用侧改不了），ros_topic 只是"到 ROS 这一侧叫什么"。 + # ros_gz_bridge 没有改名能力，所以这里用 remap 把名字对到 + # scene.yaml 声明的那一个 —— 两侧对不上的表现是 + # "桥接器起来了但一个消息都没有"，那是个假故障。 + remappings=[(_pi.gz_topic, _pi.ros_topic)], + ) + ) + actions.append( + LogInfo( + msg=f"[gazebo_sim] 世界模型位姿回读: {_pi.gz_topic} → {_pi.ros_topic}" + ) + ) + # --- 4) 相机：桥接 → 适配器 → （世界相机）补 TF -------------------------- # 只在挂了传感器时才做。--no-sensors 时跳过是必要的：适配器会去订阅 # 永远不存在的 gz 话题，它**不会报错**，只是静默没输出 —— 而"相机没图" @@ -357,6 +443,26 @@ def _prepare(context, *args, **kwargs): ) ) + # 4e) 腕部 FT：只需桥接，**不需要适配器** ------------------------------ + # 与触觉不同：触觉要轴向映射/基线/单位换算/contact 折算，所以有适配器； + # 腕部 FT 的 gz 输出本来就是牛顿、坐标系就是 child(link7) 系，而 + # ros_gz_bridge **原生支持** gz.msgs.Wrench → WrenchStamped（实测确认）。 + # 于是仿真产出的就是真机同名的 //ft_sensor_wrench（同名同类型同单位）， + # 应用层一行分支都不用写。少一层节点就少一个"配错了不报错"的地方。 + if with_sensors != "false": + for _w in check_wrist_ft_sensor(urdf, wft): + actions.append(LogInfo(msg=f"[gazebo_sim] ⚠ 腕部 FT 检查: {_w}")) + + actions.append( + Node( + package="ros_gz_bridge", + executable="parameter_bridge", + name="wrist_ft_bridge", + output="screen", + arguments=wrist_ft_bridge_args(wft), + ) + ) + # --- 5) spawn 机器人 ----------------------------------------------------- # -world 必须给对：world 名写错时 Gazebo **不报错**，只是实体没被创建， # 表现成"launch 一切正常但 RViz 里什么都没有"。 @@ -533,6 +639,14 @@ def _prepare(context, *args, **kwargs): "mimics": mimics, "robot_description": urdf, "namespace": "/gripper", + # 抓取锁（仿真专用，见 sim_grasp_lock.py）： + # 夹住后把物体固定到手指上，避免"位置控制夹持 + + # 连续轨迹"导致的打滑掉件。 + "grasp_lock_enabled": bool(glock.enabled), + "grasp_lock_object": str(glock.object), + "grasp_lock_attach_topic": str(glock.attach_topic), + "grasp_lock_detach_topic": str(glock.detach_topic), + "grasp_lock_timeout_ms": int(glock.publish_timeout_ms), }, {"use_sim_time": True}, ], diff --git a/src/arm_sim_gazebo/test/test_sim_grasp_lock.py b/src/arm_sim_gazebo/test/test_sim_grasp_lock.py new file mode 100644 index 0000000..f21ff4e --- /dev/null +++ b/src/arm_sim_gazebo/test/test_sim_grasp_lock.py @@ -0,0 +1,240 @@ +"""sim_grasp_lock.py 的测试：配置读取、与物体 SDF 的静态核对、触发命令拼装。 + +为什么这个模块值得单独一套测试：它管的**全是静默失效**。 +抓取锁不生效时 Gazebo 什么都不说 —— 插件没写进模型、child_model 名字不对、 +话题名两侧不一致，全都表现为同一个现象："规莫名其妙掉了"（要等到 S07/S08、 +而且日志里看不到真因）。所以这里把"能不能发现"逐条钉住。 + +三条重点（都是真实踩到的）： + * 插件参数与配置**必须两侧一致**，而名字有好几个（child_model/child_link/ + attach_topic/detach_topic），漏一个就是静默失效 —— 所以 check 要逐项查， + 而不是只查"插件在不在"。 + * child_link 必须是 Gazebo 的**真名**：URDF 里与固定关节相连的连杆会被 + sdformat 合并（lump），手指那样的名字在 Gazebo 里根本不存在。 + * 配置文件缺失**不该报错**（锁是可选的补救措施），但文件在、字段写错 + 必须报错（那说明有人动过它并且写错了）。 + +全部不依赖 ROS、不依赖 Gazebo。 +""" + +from __future__ import annotations + +import pytest +import yaml + +from arm_sim_gazebo.sim_grasp_lock import ( + DEFAULT_GRASP_LOCK_REL, + GraspLockError, + GraspLockSpec, + bridge_target_links, + check_grasp_lock_sdf, + load_grasp_lock, +) + + +# --------------------------------------------------------------------------- +# 配置读取 +# --------------------------------------------------------------------------- +def _write(tmp_path, doc) -> "object": + p = tmp_path / "grasp_lock.yaml" + p.write_text(yaml.safe_dump(doc, allow_unicode=True), encoding="utf-8") + return p + + +def _ok_doc() -> dict: + return { + "enabled": True, + "object": "gauge", + "object_link": "body", + "robot_model": "rizon_sim", + "robot_link": "sim_left_finger_mount", + "attach_topic": "/sim/grasp_lock/attach", + "detach_topic": "/sim/grasp_lock/detach", + "state_topic": "/sim/grasp_lock/state", + "publish_timeout_ms": 3000, + } + + +def test_repo_config_loads_and_describes(): + """仓库里真实的那份配置必须能读、且 describe() 不空。""" + spec = load_grasp_lock(root=__import__("pathlib").Path(__file__).resolve().parents[3]) + assert spec.enabled in (True, False) + assert spec.object + assert "抓取锁" in spec.describe() + if spec.enabled: + assert spec.robot_model and spec.robot_link + assert spec.attach_topic != spec.detach_topic + + +def test_missing_file_is_disabled_not_error(tmp_path): + """文件不在 → 返回"关闭"，而不是抛。锁是可选的补救措施，不该拦住仿真。""" + spec = load_grasp_lock(tmp_path / "nope.yaml") + assert spec.enabled is False + + +def test_enabled_requires_names(tmp_path): + """开着但缺 robot_model/robot_link → 必须抛，且消息里说清后果。""" + doc = _ok_doc() + doc.pop("robot_link") + with pytest.raises(GraspLockError) as ei: + load_grasp_lock(_write(tmp_path, doc)) + msg = str(ei.value) + assert "robot_link" in msg + # 必须解释"写错会静默失效"，否则现场只会去改阈值 + assert "静默失效" in msg + + +def test_attach_and_detach_must_differ(tmp_path): + """两个话题相同 = 上锁解锁走同一条消息 → 状态随机。必须拦住。""" + doc = _ok_doc() + doc["detach_topic"] = doc["attach_topic"] + with pytest.raises(GraspLockError) as ei: + load_grasp_lock(_write(tmp_path, doc)) + assert "attach_topic" in str(ei.value) + + +def test_finger_anchor_is_rejected_and_explains_the_yank(tmp_path): + """锚点不许是**手指**：张爪时会把被锁的工件从孔里拽出来（2026-09-24 实测）。 + + 症状只有一句"通止规掉了(E1)"，看不到真因 —— 所以必须在**读配置时**就拦住， + 并且把"为什么"和"改成什么"写进消息里。 + """ + doc = _ok_doc() + doc["robot_link"] = "sim_left_finger_mount" + with pytest.raises(GraspLockError) as ei: + load_grasp_lock(_write(tmp_path, doc)) + msg = str(ei.value) + assert "sim_left_finger_mount" in msg + assert "sim_link7" in msg, "必须给出正确的锚点，否则现场还得自己猜" + assert "拽" in msg or "拉出" in msg, "必须说清物理后果（工件被拽出孔）" + + +def test_bad_yaml_reports_path(tmp_path): + """YAML 语法错 → 消息里带路径（现场一眼知道改哪个文件）。""" + p = tmp_path / "grasp_lock.yaml" + p.write_text("enabled: [true\n", encoding="utf-8") + with pytest.raises(GraspLockError) as ei: + load_grasp_lock(p) + assert "grasp_lock.yaml" in str(ei.value) + + +def test_disabled_skips_required_fields(tmp_path): + """enabled: false 时不该因为缺字段而报错 —— 关了就是关了。""" + doc = _ok_doc() + doc["enabled"] = False + doc.pop("robot_model") + doc.pop("robot_link") + spec = load_grasp_lock(_write(tmp_path, doc)) + assert spec.enabled is False + + +# --------------------------------------------------------------------------- +# 与物体 SDF 的核对 +# --------------------------------------------------------------------------- +def _sdf(**over) -> str: + """一份"长得像"插件块的 SDF 文本（只关心被查的那几个字段）。""" + vals = { + "child_model": "rizon_sim", + "child_link": "sim_left_finger_mount", + "attach_topic": "/sim/grasp_lock/attach", + "detach_topic": "/sim/grasp_lock/detach", + } + vals.update(over) + return ( + "\n" + "\n" + f" {vals['child_model']}\n" + f" {vals['child_link']}\n" + f" {vals['attach_topic']}\n" + f" {vals['detach_topic']}\n" + "" + ) + + +def _spec(**over) -> GraspLockSpec: + base = dict( + enabled=True, + object="gauge", + object_link="body", + robot_model="rizon_sim", + robot_link="sim_left_finger_mount", + attach_topic="/sim/grasp_lock/attach", + detach_topic="/sim/grasp_lock/detach", + state_topic="/sim/grasp_lock/state", + publish_timeout_ms=3000, + ) + base.update(over) + return GraspLockSpec(**base) + + +def test_consistent_sdf_has_no_warning(): + assert check_grasp_lock_sdf(_sdf(), _spec()) == [] + + +def test_missing_plugin_is_reported(): + """模型里没有插件 → 必须报，而且要说清"不会报错，只是规照样掉"。""" + warns = check_grasp_lock_sdf("", _spec()) + assert len(warns) == 1 + assert "DetachableJoint" in warns[0] + assert "不会报错" in warns[0] + + +def test_wrong_child_model_is_reported(): + """child_model 与 spawn 的 -name 不一致 → 插件 Configure 直接失败。""" + warns = check_grasp_lock_sdf(_sdf(child_model="rizon"), _spec()) + assert any("" in w for w in warns) + + +def test_wrong_child_link_is_reported_and_explains_lumping(): + """child_link 用了 URDF 名（Gazebo 里被 lump 掉的真名不存在）→ 必须报。""" + warns = check_grasp_lock_sdf(_sdf(child_link="sim_left_finger"), _spec()) + msg = "\n".join(warns) + assert "" in msg + # 解释"要填真名、真名怎么查"，否则现场只会照抄 URDF + assert "合并" in msg and "world_pose_info" in msg + + +def test_topic_mismatch_is_reported(): + warns = check_grasp_lock_sdf(_sdf(attach_topic="/attachable/attach"), _spec()) + assert any("attach_topic" in w for w in warns) + + +def test_disabled_lock_never_warns(): + """关掉时不该报 —— 没插件是预期状态，不该刷告警让人忽略真告警。""" + assert check_grasp_lock_sdf("", _spec(enabled=False)) == [] + + +def test_empty_sdf_is_reported(): + warns = check_grasp_lock_sdf("", _spec()) + assert warns and "读不到" in warns[0] + + +def test_target_links_lists_both_ends(): + links = bridge_target_links(_spec()) + assert links == ["gauge.body", "rizon_sim.sim_left_finger_mount"] + + +def test_default_rel_path_matches_repo_layout(): + """默认路径必须真的是仓库里那份（改了目录而忘了改常量 = 锁永远静默关闭）。""" + assert DEFAULT_GRASP_LOCK_REL.endswith("config/grasp_lock.yaml") + + +def test_topic_cmd_publishes_with_duration_not_wait(): + """`ign topic` 的参数必须带 -d（duration），**不能**带 -w。 + + 2026-09-24 踩到：写成了 ``-w <秒>``（以为它"等订阅者"），而这个版本的 + ign topic 没有 -w —— 参数变成多余的位置参数： + "The following arguments were not expected: 3" + 于是上锁/解锁永远失败，而现象和"锁根本没生效"一模一样（规照样掉）， + 只看日志根本分不出来。这里把命令行钉死。 + """ + from arm_sim_gazebo.sim_grasp_lock import _ign_topic_cmd + + cmd = _ign_topic_cmd("ign", "/sim/grasp_lock/attach", 3000) + assert "-d" in cmd + assert "-w" not in cmd + assert cmd[cmd.index("-t") + 1] == "/sim/grasp_lock/attach" + assert cmd[cmd.index("-m") + 1] == "ignition.msgs.Empty" + # duration 必须是正的浮点秒数 + assert float(cmd[cmd.index("-d") + 1]) > 0.0 diff --git a/src/arm_sim_gazebo/test/test_sim_pose_service.py b/src/arm_sim_gazebo/test/test_sim_pose_service.py new file mode 100644 index 0000000..dec5027 --- /dev/null +++ b/src/arm_sim_gazebo/test/test_sim_pose_service.py @@ -0,0 +1,90 @@ +"""sim_pose_service.py 的测试：把模型 set_pose 到指定位姿的那一层。 + +为什么这层要单独测：它唯一的实现细节（`--reqtype ignition.msgs.Pose`） +**错一个字母就只是"服务调用失败"**，而报错信息完全不提原因；而调用方 +（场景加载脚本、真值感知的启动复位）都把它当"可选步骤"处理 —— +于是它坏掉的表现是"物体没被复位"，没有任何一处日志指名它。 + +这里只测**纯函数**（world 名解析、yaw→四元数）：真正调 `ign` 需要 Gazebo， +放在 `test_check_sim_scene.py` 那种"要环境"的地方没意义（离线机器跑不了）。 +""" + +from __future__ import annotations + +import math + +import pytest + +from arm_sim_gazebo.sim_pose_service import ( + set_model_pose, + set_model_pose_yaw, + world_from_gz_topic, + yaw_to_quat, +) + + +@pytest.mark.parametrize( + "topic,want", + [ + ("/world/assembly/pose/info", "assembly"), + ("/world/assembly/pose", "assembly"), + (" /world/assembly/pose/info ", "assembly"), + ("/world/assembly/pose/info/", "assembly"), + ("/world/sim_room/pose/info", "sim_room"), + ], +) +def test_world_from_gz_topic(topic, want): + assert world_from_gz_topic(topic) == want + + +@pytest.mark.parametrize( + "topic", + ["", None, "assembly", "/world/pose/info", "/world/assembly/other", "world/assembly/pose/info"], +) +def test_world_from_gz_topic_rejects(topic): + """认不出来必须返回 None（调用方据此**跳过**复位并说清楚），不能猜。 + + 猜错的后果是"服务名拼错了"，而 `ign service` 对不存在的服务只会超时 —— + 现象就变成"复位偶发失败"，没人会连到 world 名上。 + """ + assert world_from_gz_topic(topic) is None + + +def test_yaw_to_quat_matches_rotation(): + for yaw in (0.0, 0.3, -1.2, math.pi): + x, y, z, w = yaw_to_quat(yaw) + # 绕 Z 的单位四元数 → 旋转矩阵，第一列必须是 (cos yaw, sin yaw, 0) + c, s = math.cos(yaw), math.sin(yaw) + R = [ + [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)], + [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)], + ] + assert R[0][0] == pytest.approx(c, abs=1e-12) + assert R[1][0] == pytest.approx(s, abs=1e-12) + assert R[0][1] == pytest.approx(-s, abs=1e-12) + assert (x, y) == (0.0, 0.0) + + +def test_set_model_pose_yaw_is_the_planar_wrapper(monkeypatch): + """平面版必须是同一个实现（不是抄一份）：这里钉住它转发的内容一样。""" + seen = [] + + def fake(world, name, pos, quat, timeout_s=0.0): # noqa: ANN001 + seen.append((world, name, tuple(pos), tuple(quat))) + return True, "data: true" + + monkeypatch.setattr("arm_sim_gazebo.sim_pose_service.set_model_pose", fake) + ok, _ = set_model_pose_yaw("assembly", "workpiece", (0.52, 0.0, 0.40), math.pi / 2) + assert ok + assert seen == [("assembly", "workpiece", (0.52, 0.0, 0.40), yaw_to_quat(math.pi / 2))] + + +def test_set_model_pose_reports_failure_without_raising(): + """失败只返回 False + 输出：调用方要能"降级继续"，不能因为复位失败就崩。 + + 这里用一个不存在的 world（服务不存在）走真命令行路径 —— 只要环境里有 + `ign` 就会走到"超时/失败"，没有 `ign` 则返回 127，两种都必须是 (False, 原因)。 + """ + ok, out = set_model_pose("no_such_world_xyz", "nope", (0, 0, 0), (0, 0, 0, 1), timeout_s=2.0) + assert ok is False + assert out # 必须带原因，不能返回空字符串 diff --git a/src/arm_sim_gazebo/test/test_sim_scene.py b/src/arm_sim_gazebo/test/test_sim_scene.py index 99f05bb..d070f90 100644 --- a/src/arm_sim_gazebo/test/test_sim_scene.py +++ b/src/arm_sim_gazebo/test/test_sim_scene.py @@ -318,6 +318,127 @@ def test_load_scene_records_source_path(tmp_path): assert load_scene(root=tmp_path).source == p +# --------------------------------------------------------------------------- +# pose_info（世界模型位姿回读）—— 可选段，写错必须当场拦住 +# --------------------------------------------------------------------------- +def test_load_scene_without_pose_info_is_fine(tmp_path): + """**老场景文件没有这一段也必须能读**：它是可选的，缺了只是不做漂移监视。""" + _write_scene(tmp_path, [_obj()]) + assert load_scene(root=tmp_path).pose_info is None + + +def test_load_scene_parses_pose_info(tmp_path): + _write_scene(tmp_path, [_obj()]) + p = tmp_path / "devices/sim/gazebo/config/scene.yaml" + doc = yaml.safe_load(p.read_text(encoding="utf-8")) + doc["pose_info"] = { + "gz_topic": "/world/assembly/pose/info", + "ros_topic": "/sim/world_pose_info", + } + p.write_text(yaml.safe_dump(doc, allow_unicode=True), encoding="utf-8") + pi = load_scene(root=tmp_path).pose_info + assert pi is not None + assert pi.gz_topic == "/world/assembly/pose/info" + assert pi.ros_topic == "/sim/world_pose_info" + # 类型有默认值：ros_gz_bridge 只认这一对，改了就"桥接器起来但没消息" + assert pi.ros_type == "tf2_msgs/msg/TFMessage" + assert pi.gz_type == "gz.msgs.Pose_V" + assert (pi.drift_tol_m, pi.drift_tol_deg) == (0.002, 2.0) + + +def test_load_scene_pose_info_missing_topic_raises(tmp_path): + """只写一半话题名 = 订阅一个不存在的话题 = **永远不会报警**的监视器。 + + 那比没有监视更糟（人会以为有保护），所以必须当场报错，而不是取默认值。 + """ + _write_scene(tmp_path, [_obj()]) + p = tmp_path / "devices/sim/gazebo/config/scene.yaml" + doc = yaml.safe_load(p.read_text(encoding="utf-8")) + doc["pose_info"] = {"gz_topic": "/world/assembly/pose/info"} + p.write_text(yaml.safe_dump(doc, allow_unicode=True), encoding="utf-8") + with pytest.raises(SceneError) as e: + load_scene(root=tmp_path) + assert "ros_topic" in str(e.value) + + +def test_load_scene_pose_info_not_mapping_raises(tmp_path): + _write_scene(tmp_path, [_obj()]) + p = tmp_path / "devices/sim/gazebo/config/scene.yaml" + doc = yaml.safe_load(p.read_text(encoding="utf-8")) + doc["pose_info"] = ["/world/assembly/pose/info"] + p.write_text(yaml.safe_dump(doc, allow_unicode=True), encoding="utf-8") + with pytest.raises(SceneError) as e: + load_scene(root=tmp_path) + assert "pose_info 不是映射" in str(e.value) + + +def test_pose_info_reset_on_start_defaults_off(tmp_path): + """**默认不搬物体**：老场景文件不写这一项时，行为必须与以前完全一样。 + + 这个开关是"开局把物体 set_pose 回名义位姿"，默认开的话等于替所有老场景 + 改了语义（而且是在运行中搬物体），风险远大于收益。 + """ + _write_scene(tmp_path, [_obj()]) + p = tmp_path / "devices/sim/gazebo/config/scene.yaml" + doc = yaml.safe_load(p.read_text(encoding="utf-8")) + doc["pose_info"] = { + "gz_topic": "/world/assembly/pose/info", + "ros_topic": "/sim/world_pose_info", + } + p.write_text(yaml.safe_dump(doc, allow_unicode=True), encoding="utf-8") + assert load_scene(root=tmp_path).pose_info.reset_on_start is False + + +def test_pose_info_reset_on_start_must_be_bool(tmp_path): + """`reset_on_start: "false"` 在 YAML 里是**真值** —— 必须当场拦，不能强转。 + + 这类错误的表现是"开关关了却没关"：排查时没人会想到去怀疑引号。 + """ + _write_scene(tmp_path, [_obj()]) + p = tmp_path / "devices/sim/gazebo/config/scene.yaml" + doc = yaml.safe_load(p.read_text(encoding="utf-8")) + doc["pose_info"] = { + "gz_topic": "/world/assembly/pose/info", + "ros_topic": "/sim/world_pose_info", + "reset_on_start": "false", + } + p.write_text(yaml.safe_dump(doc, allow_unicode=True), encoding="utf-8") + with pytest.raises(SceneError) as e: + load_scene(root=tmp_path) + assert "reset_on_start" in str(e.value) + + +def test_shipped_scene_has_reset_on_start(tmp_path): + """仓库自带场景**开着**启动复位：不开的话开局 2.6mm 落地偏差会拦住真值点。 + + 实测数据与完整理由写在 scene.yaml 那段注释和 + ``sim_truth_perception._maybe_startup_reset`` 里。 + """ + from arm_sim_gazebo.sim_scene import repo_root + + spec = load_scene(None, repo_root()) + assert spec.pose_info is not None + assert spec.pose_info.reset_on_start is True + + +def test_shipped_scene_world_name_matches_the_world_file(): + """scene.yaml 里 pose_info.gz_topic 的 world 名必须与真实 world 文件名一致。 + + 不一致的后果是**静默的**：桥接器订阅一个不存在的话题，永远没有消息， + 漂移监视形同不存在 —— 而它正是为了防"静默失效"才加的。 + 这条测试是那两个名字之间唯一的联系。 + """ + from arm_sim_gazebo.sim_scene import repo_root + + root = repo_root() + spec = load_scene(None, root) + assert spec.pose_info is not None, "仓库自带的 scene.yaml 应当配了 pose_info" + worlds = sorted((root / "devices/sim/gazebo/worlds").glob("*.world")) + assert worlds, "worlds 目录里一个 .world 都没有？" + world = worlds[0].stem + assert spec.pose_info.gz_topic == f"/world/{world}/pose/info" + + # --------------------------------------------------------------------------- # Bounds # --------------------------------------------------------------------------- diff --git a/src/arm_sim_gazebo/test/test_sim_truth_perception.py b/src/arm_sim_gazebo/test/test_sim_truth_perception.py index d873875..f899bd8 100644 --- a/src/arm_sim_gazebo/test/test_sim_truth_perception.py +++ b/src/arm_sim_gazebo/test/test_sim_truth_perception.py @@ -30,11 +30,18 @@ import pytest from arm_sim_gazebo import sim_scene from arm_sim_gazebo.sim_truth_perception import ( MIN_CAM_Z_M, + RESET_GIVE_UP, + RESET_NOW, + RESET_SKIP, + RESET_WAIT, + STARTUP_RESET_WINDOW_S, TruthError, apply_transform, load_truth_target, matrix_from_transform, pick_object, + pose_drift, + startup_reset_decision, target_point_world, ) @@ -253,3 +260,105 @@ def test_offset_length_is_checked_not_guessed(tmp_path): def test_camera_backside_guard_is_positive(): """背后判据用 z<=0，且阈值是个正数（写成 0 会把贴镜头的点也发出去）。""" assert MIN_CAM_Z_M > 0.0 + + +# --------------------------------------------------------------------------- +# 漂移判据（真值感知的"前提自检"） +# --------------------------------------------------------------------------- +def _obj_at(x=0.52, y=0.0, z=0.40, yaw=0.0): + return sim_scene.SceneObject(name="workpiece", model="workpiece", x=x, y=y, z=z, yaw=yaw) + + +def _quat_z(yaw_deg: float): + h = math.radians(yaw_deg) / 2.0 + return (0.0, 0.0, math.sin(h), math.cos(h)) + + +def test_pose_drift_quiet_when_the_part_is_where_the_scene_says(): + """在位 → 必须**完全安静**。误报会让现场学会忽略这个监视，比没有更糟。""" + assert pose_drift(_obj_at(), (0.52, 0.0, 0.40), _quat_z(0.0), 0.002, 2.0) is None + # 容差边界内也算安静（1mm / 1°） + assert pose_drift(_obj_at(), (0.521, 0.0, 0.40), _quat_z(1.0), 0.002, 2.0) is None + + +def test_pose_drift_reports_the_case_we_actually_hit(): + """2026-09-24 的真实故障：工件掉进台面通槽、落到地面（z≈0）还转了半圈。 + + 这一拍必须报，而且要把**名义/实际/偏差**都写出来 —— 现场看到这句话就该 + 立刻去检查工件，而不是去调检测阈值。 + """ + msg = pose_drift(_obj_at(), (0.626, -0.034, 0.0), _quat_z(-54.0), 0.002, 2.0) + assert msg is not None + assert "名义" in msg and "实际" in msg + # 掉下去约 0.4m（连同 XY 偏移合计 415mm），这个量级必须一眼看到 + assert "415.2mm" in msg + assert "-54.0°" in msg + + +def test_pose_drift_sees_tilt_not_just_yaw(): + """姿态差用**整段旋转差**：板被碰一下常常是"翘起来夹着转一点"。 + + 只比 yaw 会漏掉纯翘起 —— 而翘起同样把孔在图上挪走。 + 这里绕 x 轴翘 5°、yaw 不变：必须报。 + """ + h = math.radians(5.0) / 2.0 + quat_tilt = (math.sin(h), 0.0, 0.0, math.cos(h)) + assert pose_drift(_obj_at(), (0.52, 0.0, 0.40), quat_tilt, 0.002, 2.0) is not None + # 而只翘 1°（在 2° 容差内）应当安静 + h1 = math.radians(1.0) / 2.0 + assert pose_drift( + _obj_at(), (0.52, 0.0, 0.40), (math.sin(h1), 0.0, 0.0, math.cos(h1)), 0.002, 2.0 + ) is None + + +def test_pose_drift_measures_a_part_with_nonzero_yaw(): + """名义 yaw 不是 0 时（现场摆歪了）也要比得准：转回同一角度 = 安静。""" + obj = _obj_at(yaw=math.radians(30.0)) + assert pose_drift(obj, (0.52, 0.0, 0.40), _quat_z(30.0), 0.002, 2.0) is None + assert pose_drift(obj, (0.52, 0.0, 0.40), _quat_z(0.0), 0.002, 2.0) is not None + + +# --------------------------------------------------------------------------- +# 启动复位判据（把"spawn 落地的 2~3mm"和"被撞走"分开） +# --------------------------------------------------------------------------- +def test_startup_reset_only_fires_inside_the_window(): + """**运行中绝不搬物体**：过了窗口一律 give_up，交给漂移看门狗报错。 + + 这是这一整套里最要紧的一条：在运行中悄悄把物体搬回去，等于把 + "谁把它撞歪了"的证据抹掉，还可能把工件从爪里拽走。 + """ + d, _ = startup_reset_decision( + now_s=100.0, deadline_s=20.0, window_s=20.0, pose_known=True, any_drift=True + ) + assert d == RESET_GIVE_UP + + +def test_startup_reset_waits_until_it_knows_the_pose(): + """还没收到 pose/info 帧就不能判断"要不要搬" —— 必须等，不能瞎搬。""" + d, _ = startup_reset_decision( + now_s=1.0, deadline_s=None, window_s=20.0, pose_known=False, any_drift=True + ) + assert d == RESET_WAIT + + +def test_startup_reset_is_silent_when_the_part_is_in_place(): + """物体本来就在名义位姿 → 什么都不做（没有漂移就没有理由碰它）。""" + d, _ = startup_reset_decision( + now_s=1.0, deadline_s=None, window_s=20.0, pose_known=True, any_drift=False + ) + assert d == RESET_SKIP + + +def test_startup_reset_fires_for_the_settle_case_we_actually_hit(): + """2026-09-24 的真实开局：落稳后偏 2.6mm（越过 2mm 容差）→ 搬回名义位姿。""" + d, dl = startup_reset_decision( + now_s=1.0, deadline_s=None, window_s=20.0, pose_known=True, any_drift=True + ) + assert d == RESET_NOW + assert dl == pytest.approx(21.0) # 截止时刻 = 首次判定时刻 + 窗口 + + +def test_startup_reset_window_is_bounded_and_positive(): + """窗口必须有界：`None`/0 会让"要不要在运行中搬物体"变成不可控。""" + assert STARTUP_RESET_WINDOW_S > 0.0 + assert STARTUP_RESET_WINDOW_S <= 120.0 diff --git a/src/arm_sim_gazebo/test/test_truth_holes.py b/src/arm_sim_gazebo/test/test_truth_holes.py index 892f599..8247bce 100644 --- a/src/arm_sim_gazebo/test/test_truth_holes.py +++ b/src/arm_sim_gazebo/test/test_truth_holes.py @@ -293,3 +293,65 @@ def test_object_bbox_px_clips_to_image(): x1, y1, x2, y2 = bbox assert 0 <= x1 <= x2 <= IMAGE[1] - 1 assert 0 <= y1 <= y2 <= IMAGE[0] - 1 + + +# ------------------------------------------------- 真值三维（2026-09-24 新增） +def test_locate_holes_also_returns_exact_world_geometry(tmp_path): + """真值必须**同时**给三维 —— 调用方不该再去反投影。 + + 为什么钉这条：仿真里孔是贯通孔、孔位正下方就是台面通槽，深度相机在孔位 + 看到的是槽底/地面（0.62m）而非工件面（0.21m）。stage2 原先就拿那些像素去 + 查深度图反投影，于是 58mm 的孔距算成 169mm、被间距门槛丢掉， + 现象是"S05 每拍都丢、最后超时"，看起来像检测坏了。 + 真值本来就精确知道孔在世界系哪儿 —— 这条测试保证它一直带出来。 + """ + spec = _build_repo(tmp_path) + res = locate_holes(_cam_looking_down((0.50, 0.0, 0.632)), INTR, IMAGE, + spec=spec, root=tmp_path) + assert res.ok, res.why + assert len(res.hole_world) == 2 + # 环的局部 z=0.010，物体 z=0.40 → 世界 z=0.410 + for (x, y, z) in res.hole_world: + assert z == pytest.approx(0.410, abs=1e-9) + assert y == pytest.approx(0.0, abs=1e-9) + # 世界系孔距必须**正好**是 58mm（不像像素那样有透视缩放） + d = math.dist(res.hole_world[0], res.hole_world[1]) + assert d == pytest.approx(2 * RING_X, rel=1e-9) + # 上表面中心与上表面 z：板厚 12mm，底面 z=0.40 + assert res.part_top_z == pytest.approx(0.412, abs=1e-9) + assert res.part_center_world is not None + assert res.part_center_world[2] == pytest.approx(0.412, abs=1e-9) + assert res.part_center_world[:2] == pytest.approx((0.50, 0.0), abs=1e-9) + + +def test_world_geometry_follows_yaw(tmp_path): + """物体带 yaw 时，真值三维必须跟着转 —— 否则"真值"是错的。 + + 只比像素会漏掉这类错：像素上转 90° 的两孔与没转的两孔**看起来都合理**， + 但世界坐标差了一整条孔距。 + """ + _build_repo(tmp_path) + cfg = tmp_path / "devices/sim/gazebo/config/scene.yaml" + doc = yaml.safe_load(cfg.read_text(encoding="utf-8")) + doc["objects"][0]["pose"]["yaw"] = math.pi / 2 + cfg.write_text(yaml.safe_dump(doc, allow_unicode=True), encoding="utf-8") + spec = sim_scene.load_scene(root=tmp_path) + res = locate_holes(_cam_looking_down((0.50, 0.0, 0.632)), INTR, IMAGE, + spec=spec, root=tmp_path) + assert res.ok, res.why + # 两个孔都转到 y 方向：|Δy| = 58mm，|Δx| ≈ 0 + dx = abs(res.hole_world[0][0] - res.hole_world[1][0]) + dy = abs(res.hole_world[0][1] - res.hole_world[1][1]) + assert dx == pytest.approx(0.0, abs=1e-9) + assert dy == pytest.approx(2 * RING_X, rel=1e-9) + + +def test_locate_holes_without_two_rings_has_no_world_geometry(tmp_path): + """失败时不能留下半份三维 —— 那样调用方会拿着空列表去当坐标用。""" + spec = _build_repo(tmp_path, rings=((0.0, 0.0),)) + res = locate_holes(_cam_looking_down((0.50, 0.0, 0.632)), INTR, IMAGE, + spec=spec, root=tmp_path) + assert not res.ok + assert res.hole_world == [] + assert res.part_center_world is None + assert res.part_top_z is None