orisys/ros_contorl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
18 Commits 2 Branches 0 Tags
83c7a897315805686cefa7250fde1d407173ac63
Commit Graph

18 Commits

This Branch
This Branch
All Branches
Author SHA1 Message Date
arm_dev 83c7a89731 P5c-5 仿真夹爪可开合：Gazebo 不支持 mimic，改为显式驱动全部 7 个关节 
三条独立的坑，前两条都在"看起来完全正常"的表象下静默失效：

1) 夹爪在仿真里是死的（关节级）
   夹爪只有 finger_width_joint 是真关节（effort=80），其余 6 个手指关节
   全是 mimic、effort="0"（上游语义：靠机械耦合跟随，不由电机驱动）。
   而 sdformat 转 SDF 时**直接丢弃 <mimic>**（实测生成 SDF 里 "mimic"
   出现 0 次），gz-sim 也没有任何 mimic 实现，SDF 规范里就没有这个元素。
   于是 7 个关节在 Gazebo 里互相独立、谁都不跟随谁。

2) 补了 effort 才有用（力度）
   gz_ros2_control 的 position 接口是"位置误差 → 力"
   （实测 position_proportional_gain=0.1），力被 effort 上限夹到 0 →
   永远出不了力。症状极隐蔽：接口 [claimed]、控制器 active、话题有订阅、
   命令也发得出去，但手指一动不动，只是自由地被重力拽着漂。
   对照实验：同一套链路下 sim_joint1 收到 0.2 rad 后精确到位（0.19998），
   所以链路本身没问题，差别只在这 6 个关节的 effort=0。

3) 描述与 Gazebo 的事实不一致（语义）
   robot_state_publisher 支持 mimic，会用主关节的值覆盖从动关节；而 Gazebo
   里它们是独立被驱动的。实测当前 RSP 没有应用 mimic（TF 跟随真实值），
   所以碰巧一致 —— 但这是实现细节，它一变 TF 就静默退回闭合位。

改动：
  - ros2_control 增加夹爪 7 个关节的接口（含 load_gripper 开关）
  - controllers.yaml 增加 gripper_controller（JointGroupPositionController）
  - 启动器 spawn 它；并重命名归一化第三条修复
  - urdf_normalize 第三条修复：被 mimic 关节补 effort=80、删除 <mimic>
    并记录耦合关系；新增 mimic_relations() 供适配器在归一化前读取
    （信息只是搬家，不能丢）
  - 新增 6 项单测锁定上述行为

验收（实测）：命令 width 0→0.1，指尖间距 27.1mm → 127.0mm（约 100mm 行程，
与真机 GRAV 规格吻合），7 个关节实测值均命中命令值，TF 与之一致。

Co-authored-by: Cursor <cursoragent@cursor.com>
 2026-09-23 15:46:08 +08:00
arm_dev 02516fe567 P5c-5 触觉链路打通：接触传感器此前是"完全静默失效"，已定位并修复 
两条独立的静默坑，都不报错、不告警，只会"永远收不到数据"：

1) world 里没声明 contact-system 系统插件。
   gz-sim 的 <sensor type="contact"> 不由 ignition-sensors 库实现，而是由
   ignition-gazebo6-contact-system 插件实现 —— 所以环境里根本没有
   libignition-sensors6-contact.so（相机/IMU/lidar 的后端都在，唯独缺它），
   那不是包缺失。不声明插件的症状：话题不出现、日志空白。
   实测：加上插件后 /tactile/left/raw、/tactile/right/raw 立刻出现。

2) 接触传感器的三个必备写法，错一个就静默失效：
   - <contact><collision> 必须写 URDF→SDF 自动生成的 collision 名；
     留空 <contact/> 会被补成 SDF 规范的默认值 __default__（不存在的
     collision）。实测生成名：
       sim_left_finger_mount_fixed_joint_lump__sim_left_finger_tip_collision
   - 话题必须写在 <contact><topic> 里，写在外层 <sensor><topic> 无效，
     gz 会静默改用默认 scoped 话题名。
   - 传感器必须挂在有碰撞体的 link 上（指尖），不是另建的空 link。

两处均已按实测结果修正，并把"为什么这么写"写进文件注释。

Co-authored-by: Cursor <cursoragent@cursor.com>
 2026-09-23 15:29:09 +08:00
arm_dev 47ad9d9a8b P5c-4 相机出图联调：桥接 d405/d435 到 ROS，并修掉两处静默的几何错误 
相机链路（devices/sim/gazebo/config/cameras.yaml 是唯一真相）
  - sim_cameras.py        纯逻辑：话题映射 / 深度换算 / 内参 / 视场一致性比对
  - sim_camera_adapter.py 改光学系 frame_id、32FC1 米 → 16UC1 毫米、覆盖内参
  - gazebo_sim.launch.py  桥接 + 适配器 + 世界相机的静态 TF（位姿从 world 解析）

本轮修掉两个**静默**的几何错误（都能出图、都能查到 TF，但点位是错的）

  1. gz 发的 camera_info 是错的：恒为 fx=fy=277, cx=160, cy=120
     （等于它的默认 60° 视场 + 半分辨率主点），而渲染用的是配置的 87° 视场。
     照抄它做三维反投影，点会偏半米量级；单目预览完全正常。
     用已知地面平面做最小二乘反解，得 fy=337.2 / cy=239.5，与
     640/(2·tan(87°/2))=337.2、480/2=240 吻合（残差中位 0.2mm）。
     改为由 sim_cameras.intrinsics() 按 cameras.yaml 的视场重算并覆盖。
     代价：cameras.yaml 的 horizontal_fov 必须与 SDF 一致，启动时自动比对告警。

  2. assembly.world 里 D435 的 rpy 写成 (0, 50°, 180°)，
     而 URDF 相机的视线是**本体 X 轴**、rpy 复合顺序又是 Rz·Ry·Rx，
     于是"先俯 50° 再偏航 180°"把视线翻到了背面：相机背对台面、只看地面。
     它不报错，而且几何自洽（点确实落在真实地面上），只查数值发现不了。
     改为 (0, 50°, 0)，并加回归测试锁住。

新增验收工具 + 测试
  - scripts/check_sim_cameras.py  反投影深度、要求命中地面/台面才算通过。
    "有图像"不是验收标准；实测 d435 台面 11.9%/地面 75.3%、d405 台面 37.8%。
  - 97 项单测（新增 sim_cameras 22 项、sim_site 16 项）

顺带修掉一处"改了配置静默无效"
  - sim.world 此前硬编码在 robot_description._WORLD_REL，而 site.yaml 与
    manifest.yaml 都宣称改它就生效。现在真的读了：
    优先级 --world > site.yaml:sim.world > 内置默认（相对路径按仓库根解析）。
  - physics_enable / joint_prefix 仍未接线，但若其值与实际状态冲突，
    启动时会告警（sim_site.unwired_warnings），不再静默忽略。

文档
  - devices/sim/gazebo/README.md 重写：原文还写着"仿真不是 Gazebo"、
    相机插件用 libgazebo_ros_camera.so，与实现早已不符。现分三部分：
    哪个"仿真"是真的 / 现状与缺什么 / 五条静默失效的坑及怎么验。

Co-authored-by: Cursor <cursoragent@cursor.com>
 2026-09-23 15:19:16 +08:00
arm_dev f67f59f383 handeye/calibrate.yaml 精简为纯算法参数，移除站点相关副本 
原先这里各留了一份站点相关值的副本，与 poses/ 里的实际数据并不一致：

    calibrate.yaml 内嵌示教点   13 点，2026-08-28
    poses/89_eye_in_hand.yaml   12 点，2026-09-02

只是恰好被覆盖顺序压住（run_handeye.sh 把 poses 文件作为后一个
--params-file，且用 -p 显式传 joint_prefix / output_dir / result_file），
所以当前行为是对的。但留着就是给"改了脚本顺序 / 直连节点"埋雷：会静默用上
旧数据集，而症状只是"标定结果变差"，几乎不可能联想到这份副本。
这也是 SN 的又一处分散暴露面。

已移除 joint_prefix / output_dir / num_poses / waypoint_positions，
保留算法参数（mode、board_*、稳定判据、velocity_scale 等），并在原位写清
这四个值各自由谁提供。节点侧对它们本就有默认值，删掉不会报参数缺失；
但 waypoint_positions 的节点默认值是全 0，所以**直连节点是跑不了的**，
必须经 run_handeye.sh —— 这一点已在注释里说明。

验证：bash scripts/run_handeye.sh show 全部路径正常解析
      （calibrate / poses / result / workdir 均正确，num_poses 读自 poses yaml）

Co-authored-by: Cursor <cursoragent@cursor.com>
 2026-09-23 14:52:32 +08:00
arm_dev 01940f54e7 P5c-3 仿真启动器跑通：Gazebo + spawn + 两个控制器 active 
端到端验收（headless，本机无显示器）：
    ign 加载 Physics/UserCommands/SceneBroadcaster/Sensors + gz_ros2_control
    机械臂 spawn 进 assembly 世界，7 个 sim_joint 全在 /joint_states
    joint_state_broadcaster 与 rizon_arm_controller 双双 active
    world → sim_tcp 可查，且末端位姿 3 秒内不变（重力下站住）
    网格解析报错 0 条

新增：
  scripts/run_sim_gz.sh                  此前**不存在**，却被 install_sim_env.sh
                                         与 build_sim.sh 的提示语引用
  arm_sim_gazebo/robot_description.py    展开 + 归一化，launch 与 CLI 共用一份
  arm_sim_gazebo/gazebo_sim_launcher.py  薄壳：准备描述后转发给 launch 文件
  launch/gazebo_sim.launch.py            起 gz → 发归一化 URDF → spawn → 拉控制器
  test/test_robot_description.py         含"仓库里这份 world 必须过守卫"的测试

接线过程中实测踩到并修掉的三个坑，共同特征是**每一步看起来都成功**：

1) urdf_normalize 在 Python 路径下静默失效（我自己引入的，最严重）
   xacro 的输出格式在不同环境里不一样：真机环境的 CLI 缩进成多行，而仿真环境
   走 xacro.process_file().toxml() 时全挤在一行。归一化的正则要求标签之间必须有
   \n，于是**仿真路径下它什么都没做**（报告"包裹 0 处"），而 CLI 路径下正常 ——
   之前那次"已修好"的验证走的就是 CLI，所以看起来过了。
   后果是 Gazebo 会丢掉整条机械臂，正是这个归一化本来要防的事。
   已改成对空白格式零假设，并加回归测试锁住。

2) world 里声明了 <plugin> 会把默认系统挤掉
   只要 world 声明任何一个 <plugin>，Gazebo 就不再加载 server.config 的
   Physics/UserCommands/SceneBroadcaster。缺 UserCommands →
   /world/<名>/create 服务不存在 → ros_gz_sim create 调服务超时后回退发 topic、
   没人收 → **模型从未进入 Gazebo**，而 create 节点仍打印
   "OK creation of entity"，控制器则永远卡在 "waiting for service"。
   已在这三个系统在 assembly.world 里显式声明，并加纯文本守卫
   check_base_plugins()，让"加了插件忘了补基础系统"在启动前就被指出。

3) gz 的插件与资源搜索路径缺 $CONDA_PREFIX/lib
   gz_ros2_control 报 "Failed to load system plugin
   [libgz_ros2_control-system.so] : couldn't find shared library"，而 ldd 全齐、
   库就在 $CONDA_PREFIX/lib 下。原因是 gz 的 SystemLoader 先在搜索路径里找文件，
   找不到就直接放弃，不去 dlopen 裸文件名（所以 conda 二进制的 RPATH 兜不住）；
   而 ros_gz_sim 只把 LD_LIBRARY_PATH 拼进搜索路径，conda 激活并不往那里加。
   同一原因导致 mesh 的 model:// 也解析不了（视觉报错很吵，但更要紧的是
   **碰撞网格也加载不出来**，会让后续装配的接触判据失效）。
   由于 gz_sim.launch.py 用 `from os import environ` 在生成 launch 描述时就读
   进程环境，这些变量**必须在 ros2 launch 之前**设好，因此放在 sim_env.sh ——
   在 launch 文件里用 SetEnvironmentVariable 补是无效的，会被它自己算的那份
   字典覆盖。

Co-authored-by: Cursor <cursoragent@cursor.com>
 2026-09-23 14:48:16 +08:00
arm_dev e9da641160 README 与 handeye：把"什么该入库"的规则和仓库实际状态对齐 
子代理同步文档时发现 README 的"要提交/不要提交"清单漏了 handeye/，顺着一查发现
规则写的是"不要提交含序列号的文件、克隆后必须重新标定"，但仓库里
handeye/poses/ 与 handeye/results/ 一直是**被跟踪**的，而且
results/89_eye_in_hand.yaml 的文件头就写着 Rizon4-063989 —— 正是那个让
config/site.yaml 被排除的序列号，从另一个门漏了出来。规则与实际不符。

澄清后的规则（已写进 README）：按"配置 vs 资产"区分，而不是按"是否含序列号"
    配置属于站点 → 不入库（config/site.yaml、handeye/tmp/）
    标定成果属于资产 → 入库（handeye/poses/、handeye/results/）
理由是手眼标定要上真机示教十几分钟，是费真机时间换来的；入库才有一份可回滚的
备份。若按"含序列号就排除"一刀切，代价是误删后必须重新上真机采点。

同时把结果文件头里的完整序列号去掉（只留臂号 89/91）—— 臂号本来就唯一标识了
是哪台机器，完整 SN 是多余的暴露面。

另：handeye/README.md 补充"tmp 是按臂号分目录"的后果说明，并点出直接启节点
（绕过 run_handeye.sh）会退回共用的 handeye/tmp —— 那种情况下两条臂的采点会
互相覆盖，而症状表现为"标定结果突然变差"，很难联想到目录冲突。

Co-authored-by: Cursor <cursoragent@cursor.com>
 2026-09-23 14:27:43 +08:00
arm_dev 6626d18020 文档修正：handeye 的 tmp 目录是按臂号分的，不是共用一个 
handeye/README.md 里把 tmp/ 画成了扁平的一层，并写"采点数据写在 handeye/tmp/"。
实际生效的取值来自 site.yaml 的 handeye_workdir，是 **按臂号分目录**的
（handeye/tmp/89、handeye/tmp/91）。

这不是措辞问题：两条臂若真的共用一个工作目录，先标定的采点会被后标定的覆盖，
而症状表现为"标定结果突然变得很差"，几乎不会有人联想到是目录冲突。
所以顺带把这一点和它的后果一起写清楚。

Co-authored-by: Cursor <cursoragent@cursor.com>
 2026-09-23 14:20:42 +08:00
arm_dev 25635e21f8 P5c-2：仿真世界 + 机器人描述 + 控制器配置，并解决两处官方描述与 Gazebo 的不兼容 
这一步解决的是"官方 Flexiv 描述能不能喂给 Gazebo"的问题。结论是**不能直接用**，
有两处不兼容，且都只有 Gazebo 会暴露 —— RViz/MoveIt 完全正常，所以此前从未发现。

问题一：7 轴本体的质量/惯量是"孤儿标签"
    rizon_arm.xacro 调 get_inertias 时没写 <inertial> 包裹，而那个宏只吐
    <origin>/<mass>/<inertia> 三个碎片。RViz/MoveIt 忽略不认识的子元素所以看着
    没事；sdformat 判定 "link has no <inertial> block"，然后把 **link 和它的父
    关节一起丢掉**。症状极具误导性：机械臂凭空消失或只剩夹爪，报错只有一句
    "link[sim_link1] is not modeled in sdf"。

问题二：夹爪有零质量 link，导致 sdformat 惯性叠加出 NaN
    grav_macro.xacro 给六个纯坐标系 dummy link 写了 mass=0、惯量全 0。
    sdformat 对 fixed 关节会把子 link 惯性叠加到父 link，叠加要除以总质量 →
    质量 0 就出 NaN。而报错指向的是**父** link：
        Pose values must be finite. link[sim_left_finger_mount]/inertial/pose
    完全看不出根因在子 link 的质量上。

修法：启动时归一化（不走改上游文件的路，因为上游由 fetch_flexiv.sh 获取、改了会被
覆盖；而且换别的臂时大概率有别的坑，在启动时统一过一道更可持续）
    - 孤儿三件套 → 只重新分组已有元素，一个数字都不改
    - 零质量 → 补 1e-4 kg（与夹爪其他配件同量级，0.1g 对动力学无影响），
               惯量全 0 时补极小对角值避免张量退化
    不发明物理数据，也不掩盖真问题：上游真没给质量时这里什么都不会包。
    16 项单测，含"已有 <inertial> 不能被二次包裹"（早期用全局正则踩过，
    sdformat 报 "Inertial element must have a mass element"）与幂等性。

新增/改动
- devices/sim/gazebo/urdf/：arm.sim.urdf.xacro（主）、.ros2_control（gz 硬件）、
  .gazebo（系统插件/材质/接触刚度）、.sensors（D405 + 双指触觉）
- devices/sim/gazebo/worlds/assembly.world：1ms 步长 + 150 次接触迭代（力信号
  要干净才谈得上"切向力突变"判据）、工作台、world 级渲染插件、D435
- devices/sim/gazebo/config/controllers.yaml：控制器名 rizon_arm_controller
  **故意与真机一致** —— 隔离的是关节/TF 层（sim_ 前缀），不是应用层依赖的接口
- src/arm_sim_gazebo/：新包，含 urdf_normalize（库 + CLI）与单测

一处设计纠正：D435 从 URDF 挪进 world。原本挂在 URDF 的 world link 上，结果
整台相机转 SDF 时凭空消失（sdformat 丢弃无质量根 link 时连带丢了整棵子树，
且退出码 0、不报错）。眼在手外的相机物理上就属于场景，放 world 才对；
渲染插件也一并归 world 级，避免"插件在模型里、相机在世界里"的错配。

验证（不是只看退出码）
- 带夹爪全量：sdformat 退出码 0、0 报错、15 link / 15 joint / 26 frame
- 传感器 d405_rgbd + tactile_left/right 均在；控制器插件与参数路径正确
- 世界文件 sdformat 通过
- 包在 ros_gz_sim 环境构建通过，CLI 实跑结果与库调用一致
- 未注册 gazebo_sim_launcher 入口点：模块还没写，注册了构建能过但一运行就
  ImportError，不如老实缺着（manifest 里如实标注为未实现）

注：实测确认 gz_ros2_control 的硬件/接口声明**不在 SDF 里**，插件是从
/robot_description 话题读 URDF 的 <ros2_control> —— 所以启动器必须先起
robot_state_publisher 且发布归一化后的同一份 URDF，否则控制器注册的关节名
会和 Gazebo 里的对不上。这条已写进 P5c-3 的实现要求。

Co-authored-by: Cursor <cursoragent@cursor.com>
 2026-09-23 14:19:39 +08:00
arm_dev d93dab31c9 P4d：文档同步 —— 修掉 P3 手眼路径迁移后遗留的过期引用，并指向新的 weights 工具 
P3 把 handeye 相关文件从 src/arm_application/config/ 迁到了仓库根的 handeye/，
但文档还写着老路径。这类"代码改了、文档没跟"的偏差最麻烦：新人照着文档敲
命令会直接报文件不存在，然后开始怀疑人生。

同步内容
- docs/HANDEYE.md / WORKSPACE.md / SITE_CONFIG.md / ARCHITECTURE.md：
  手眼结果与示教点位改指 handeye/results|poses/<臂号>_<模式>.yaml，
  临时目录改指 handeye/tmp/<臂号>/，并补上 --mode / --points / --auto-solve。
- docs/INTERFACES.md / TRACKING.md / docs/HOLE_DETECT.md / README.md：
  权重那一行从 export_yolo_trt.sh 改为 weights.sh，并注明前者已是兼容转发壳。
- docs/HOLE_DETECT.md：hole_weights / workpiece_weights 的实际取值改为
  models/hole_insert/detect_best_1.onnx（原文档写的是不存在的 best.pt）。
- scripts/merge_handeye_poses.py：只更新文件头注释里的示例路径。

多处改为指向 models/README.md 与 handeye/README.md，避免同一套规则在
七八个文件里各写一遍、然后各自漂移。

验证：rg 扫 docs/ README.md，已无 handeye_poses_/handeye_result_/
/tmp/handeye 之类过期路径残留。

Co-authored-by: Cursor <cursoragent@cursor.com>
 2026-09-23 13:52:28 +08:00
arm_dev a89250d05d P5c-1：把 Gazebo 仿真环境真正装起来（独立 conda 环境），并固化安装/构建流程 
在此之前，仓库里所有 Gazebo 相关的东西都是"声明"而非"事实"：
arm.gazebo.xacro 零引用、stack.launch.py 的 isaac 分支只打印日志、
没有 world 文件、devices/sim/gazebo 下的目录全是空占位。
更关键的是：**这台机器上根本没装 Gazebo**，而 ROS 在 conda 里，没有 /opt/ros，
apt 的 ros-humble-gazebo-* 装了也不会被 conda 的 ROS 认到。

环境（与真机环境物理隔离）
- 新建 conda 环境 ros_gz_sim，**不碰能跑真机的 ros_flexiv**。
  仿真要往环境里塞 Gazebo 及 protobuf/ignition/渲染一整条链，
  和真机环境混装随时可能把它弄坏，代价远大于仿真带来的便利。
- 频道组合照抄 ros_flexiv 的安装记录（robostack-staging + conda-forge），
  那是本机已经证明可用的搭配。

两个实测踩到的坑（都写进了脚本注释，避免下次重踩）
1. empy 版本：conda 默认给 empy 4.x，而 Humble 的 rosidl_adapter 用的是 3.x 的
   API。症状是构建自定义消息包时报 `_interpreter.shutdown()` 的 AttributeError，
   报错点在 rosidl_adapter 内部，完全看不出跟 empy 有关。必须钉 empy=3.3.4。
2. conda 激活是**堆叠**的：同一 shell 里从 ros_gz_sim 切到 ros_flexiv 后，
   `which python` 指向 ros_flexiv，但 `python -m pip` 却解析到 ros_gz_sim 的 pip，
   导致 colcon --symlink-install 报 "option --editable not recognized"。
   这个报错和真实原因毫无关系，极易误判成"环境被搞坏了"。
   现在 scripts/lib/conda_switch.sh 会在切换前退到 base，两个 env 脚本共用。

仿真用独立构建产物
- build_sim/ + install_sim/（scripts/build_sim.sh）。工作区只有一份源码，
  若仿真也写 install/，就会覆盖真机的构建产物。
- 排除 11 个真机专属包（flexiv_hardware 需要 Flexiv RDK C++ SDK，根本装不上；
  另有真机驱动/GPIO/MoveIt 配置/真机 bringup），每个都在脚本里写了理由。

已验证（不是只看退出码）
- Gazebo Fortress 6.16 无头启动正常，物理初始化完成
- /clock 桥接到 ROS 成功收到仿真时间；ros_gz_sim create 成功往世界里建物
- 真机构建 18 包通过、233 项测试通过，build/ install/ 时间戳未变（没被覆盖）
- 仿真侧 7 包构建通过；两环境互切后各自都能正常构建

注：Humble 配的是 Gazebo Fortress（ignition-gazebo6，命令行 `ign gazebo`），
不是现代的 gz-sim —— 后者要 libprotobuf>=6.31，与 RoboStack Humble 钉住的
5.29.3 解不开依赖。Fortress 属于新 Gazebo 家族（非已 EOL 的 Classic），
且是 Humble 的官方配对版本。

Co-authored-by: Cursor <cursoragent@cursor.com>
 2026-09-23 13:52:04 +08:00
arm_dev d289168dd0 P4s：模型审计 —— 移出零引用模型，修掉代码里三处悬空 best.pt 
先回答"这些模型到底有没有人用"，用全仓库引用扫描判定，不靠猜：

  在用   yolo/detect_best_0.onnx + .engine   ← paths.yolo_weights
  在用   hole_insert/detect_best_1.onnx      ← hole_detect.weights / workpiece_weights
  零引用 detect_hc90（pt/onnx/engine）       ← 只剩注释里的历史说明
  零引用 detect_yolo26_gongjian.pt
  零引用 detect_best（onnx/engine）          ← 仅代码兜底候选，而那个候选是 .pt，不存在

所以 detect_hc90 不需要修复，是不用的东西：连同另两个一起移出仓库。
注意**二进制权重被 gitignore**，删掉就找不回来了，因此是移出到
~/ros2-models-unused-20260923/ 而非 rm。搬回 models/ 即可被 list 重新发现。

顺手挖出并修掉三处悬空引用（和之前 site.yaml 里那个 best.pt 同一类问题）：
- hole_detector.weights_from_site：兜底路径写死 models/hole_insert/best.pt（不存在）
- hole_detector._default_model_path：三个候选路径**全都不存在**，函数却
  `return candidates[0]`，于是永远返回不存在的 best.pt；报错路径看着很合理，
  极易被误判成"文件被误删"。现改为以 site.yaml 为准，再退到规范模型。
- config/hole_insert.yaml 顶部注释同样写的是 best.pt

新增 test_hole_detector.py（8 项）锁住这个回归。测试刻意**不断言文件存在**
——二进制不进 git，新克隆的仓库里 models/ 是空的——只断言解析结果指向规范模型名。

体检从 4 处问题降到 0：bash scripts/weights.sh doctor --check-engines → "没有发现问题"

测试 233 项通过；18 个包构建通过。

Co-authored-by: Cursor <cursoragent@cursor.com>
 2026-09-23 13:08:11 +08:00
arm_dev 2b77ec127d P4：权重/标签模块 —— 旁挂标签文件 + PT→ONNX→TRT 转换工具 + 归档归位 
痛点：TensorRT 10.13 的 Python API 没有元数据读写，engine 一转就把类名丢了；
推理侧只能退化成 str(cid)（"0"、"1"），界面上看不出是什么类，属于静默失效。
同时旧的 export_yolo_trt.sh 只能手工搬文件、手工改 site.yaml、标签无处安放。

标签子系统（model_labels.py）
- 旁挂标签文件 <模型名>.labels.yaml，放在权重旁边，文本、可手改、进 git
  （二进制权重不进 git，标签是配置所以进）
- 读取优先级：旁挂文件 > ONNX 元数据 > 模型元数据 > 占位符 class_N
  占位符是故意的：缺标签时要一眼看出来，而不是悄悄退回 cid
- 解析器容忍多种写法：dict / list / 逗号串 / 逐行文本 / Python repr 串

布局与转换（weights_layout.py + weights.py + weights.sh）
- models/<分组>/<模型名>.{pt,onnx,engine} + 同名 .labels.yaml
- convert：PT→ONNX→TensorRT，支持自定义名字、自动归位、固化标签
- staging/：非标准格式先进这里，转换后源文件归档到 staging/done/
- list / labels / doctor / show 四个子命令
- doctor --check-engines 能查出 engine 与 TensorRT 版本不匹配（反序列化返回空）

推理侧接入
- yolo_detector / onnx_workpiece / hole_detect_hough 统一走 read_labels()
- 画框标签不再硬编码 "gongjian"，改用实际类名

顺手修掉的真实问题
- setup.py 撞上断掉的符号链接会让整包构建失败，报错还指向 build/ 里的幽灵文件
  （源文件被挪走后 build/ 里留下的断链被 os.walk 列出来）。加 _existing() 过滤。
- P3 遗漏：site.yaml / export_site_env.py / run_handeye.sh / handeye_teach.py /
  handeye_calibrate.py 里的 handeye 工作目录仍是 /tmp/handeye_calib*，
  与已迁到 handeye/ 的其余路径不一致。统一到 handeye/tmp/<臂号>。
- site.yaml 的 hole_weights / hole_detect.weights 原指向不存在的 best.pt，
  改为实际存在的 detect_best_1.onnx；active.pt 符号链接已废弃并清理。
- weights.sh labels 原先不接受 "分组/模型名" 写法，而 list 输出正是这个形式，
  照着敲会报"找不到模型"。补上，并让报错直接给出可复制的全名。

测试：225 项通过（新增 test_model_labels.py / test_weights_layout.py）。
真实 ONNX→engine 端到端转换已验过。

Co-authored-by: Cursor <cursoragent@cursor.com>
 2026-09-23 12:39:18 +08:00
arm_dev adc72955c7 P3c：teach --auto-solve —— 采完自动 merge 并接着标定，收尾手眼手动闭环 
新增 teach --auto-solve：采完点按 q 后自动 merge、重启相机、接着 calibrate，
不再需要手敲第二、三条命令。

但只有 --ros（示教器）模式能真正全程跑通，这一点如实处理而非强行自动：
标定必须通过 run_real.sh 的控制器驱动机械臂走点位，而 --rdk 软拖拽要求
RDK 独占，两者无法同时存在。这是硬件约束，脚本绕不过去。
所以 --rdk --auto-solve 会照常自动 merge，然后打印明确的两步指令，
并提示想要一条命令就改用示教器 Free Drive 模式。
硬跑一个必然失败的 calibrate 只会让人以为标定坏了。

顺带修正一处不可达代码：_need_camera / _need_files / _need_robot 内部用的是
exit（终止整个脚本）而非 return，所以 `if ! _need_camera; then echo ERROR; fi`
里的分支永远不会执行。改为直接调用，由它们自己打印原因并退出。

usage 里补上 --auto-solve 及其限制说明；handeye/README.md 第 5 节补
"一条命令跑完"小节，写明为什么 RDK 模式做不到。
usage() 的 sed 行号改为按首个非注释行自动计算，避免以后加注释又要同步。

测试：224 passed
 2026-09-23 11:55:08 +08:00
arm_dev e4f513392d P3b：手眼标定支持 --mode 模式选择与 --points 目标点数 
--mode eye_in_hand|eye_to_hand
- 切换读写的文件为 <臂号>_<模式>.yaml，两种模式的采点与结果互不覆盖、可共存
- 臂号从 Flexiv 序列号后两位取（Rizon4-063989 → 89），不做硬编码映射
- 非法模式立即报错退出。不静默退回默认值 —— 那会造成"以为标的是
  eye-to-hand、其实算的是 eye-in-hand"，是这类工具最难排查的错误
- 求解侧本来就支持（calibrate_hand_eye(eye_to_hand=...) 对 gripper2base 求逆），
  这次把入口补上，让模式不再只存在于参数文件里

--points N（仅 teach）
- teach 新增 target_points 参数并实现计数：显示 [3/12] ... 还差 9 个点，
  采够时提示可以收工
- 刻意不强制关窗：实际采点常出现"某个角度还没覆盖"，强制退出要重开

usage 重写：路径改为 handeye/，补上全局选项与组合示例；
usage() 的 sed 行号随之同步（原 2,18p 已不覆盖新头部）。

handeye/README.md 补第 5 节：模式切换与点数的完整用法，
并说明为什么非法模式要报错而不是兜底。

测试：224 passed
 2026-09-23 11:48:11 +08:00
arm_dev a3a589b9e3 P3a：手眼标定集中到 handeye/，修复三套板尺寸矛盾与从未生效的稳定性参数 
新建 handeye/ 专用文件夹（用户要求：专用文件夹 + 使用说明）
- handeye/README.md：从小白视角讲清楚两种模式怎么选、板尺寸怎么量、
  完整流程、出问题先看哪里。以前这些知识散在 docs/ 和代码注释里。
- 结构：calibrate.yaml（参数）/ poses/（采点输入）/ results/（结果输出）/ tmp/（临时，不进 git）
- 文件用 git mv 迁移，历史保留

命名约定改为 <臂号>_<模式>.yaml
- 如 89_eye_in_hand.yaml、91_eye_to_hand.yaml
- 臂号 = Flexiv 序列号后两位；模式 = eye_in_hand | eye_to_hand
- 换模式 = 改 site.yaml 里 paths.handeye_file 一行，不动代码
- 迁移只需改 site.yaml（4 个 launch + servo_node 都经 paths 读取，一行没改）

修复：三套互相矛盾的标定板尺寸（真实 bug）
同一个"板子多大"分散在三处，且值各不相同：
  handeye_calibrate.py 默认 9x6 / 0.025
  handeye/calibrate.yaml   11x16 / 0.011
  site.yaml + 结果文件头     8x11 / 0.015   ← 实测的真实板子
谁生效取决于覆盖顺序。症状是"标定流程跑得很顺、不报错，但结果明显不对"，
极难排查。已全部统一为 8x11 / 0.015，README 里写明换板必须三处同改。

修复：site.yaml 里五项稳定性配置从未生效
run_handeye.sh 一直在传 settle_qd_max / settle_stable_sec / settle_timeout_sec /
pin_holds / pin_hold_sec，但 handeye_calibrate.py 从未声明这些参数 ——
ROS 2 会静默忽略未声明的覆盖参数，所以"等机械臂停稳再拍照"这件事其实没实现，
实际只有一次固定 settle_sec。现已声明并实现：
- _wait_settled()：用关节速度 |qd|_max 判定"真的停了"，比固定 sleep 可靠
  （负载/姿态不同刹车距离差别很大；等太短会在臂还动时拍照导致角点抖动）
- _pin_hold()：静止后再保持几拍，抑制到位后的微小回弹

新增：自动标定进度条
一条可视化进度条（[ 4/12] [████░░░░]  33.3%），无人值守的长流程能一眼看出还剩多久。

清理重复文件（内容逐字节比对确认后删除）
- config/handeye_result.yaml 与 _89 逐字节相同 → 删
- config/handeye_poses.yaml 与 _89 逐字节相同 → 删
迁移后 src/arm_application/config/ 下已无 handeye 文件。

测试：224 passed（原 198，新增 26）
新增 src/arm_application/test/test_handeye_lib.py —— 手眼模块此前零测试。
关键用例是合成数据往返：用人为设定的 X_true 反推出每个视图的
(gripper→base, target→cam)，再让求解器还原。坐标链方向/求逆/转置
只要有一处写错就必然失败。
- 真值恢复（旋转+平移，atol 1e-6）
- 单独验证平移精确（旋转对而平移错是最易漏的一类 bug）
- 结果必须是 det=+1 的正交阵（排除镜像解）
- 退化输入（全同位姿）不得悄悄给出"看起来能用"的结果
- 稳定性判据：窗口未满不判稳、丢帧后重新计数、抖动超阈不判稳
- 写盘再读回不漂移
 2026-09-23 11:46:03 +08:00
arm_dev 52c108de07 P2：修复 run_all.sh 缺失，消除等待超时陷阱，标注无消费者的配置 
修复：run_all.sh 不存在，但被三处依赖
- docs/HOLE_DETECT.md 有 4 处用法示例；
- pipeline.launch.py 文件头写着"相机 / 触觉由 scripts/run_all.sh 负责拉起一次"；
- run_pipeline.sh 唯一职责就是 exec 它。

也就是说文档在教用户跑一个不存在的脚本，而 run_pipeline.sh 执行必然失败。
按 pipeline.launch.py 的实际参数（skip_stage1/2/3、target_hole、insert_depth_mm、
screw_pitch_mm、preview、resnap）重建：先确认控制器 ACTIVE，再拉起相机与触觉
（都已就绪则跳过），最后 exec pipeline.launch.py。无硬件时会明确报错退出。

修复：判活循环反复撞超时，脚本看起来像卡死
服务/话题不存在时，`ros2 service call` / `ros2 topic echo` 每次都要耗满 timeout
才返回。原写法 30 次循环最坏等 165s（实测无 ROS 时白等 63s）。改成先
`ros2 service list` / `topic list` 探测，再决定要不要 call/echo。
- run_stage2_locate.sh、run_stage3_search.sh 同步修复（165s → ~0s）
- run_all.sh 一开始就按正确顺序写
- run_handeye.sh 原本就是对的（这次是向它对齐）

标注：site.yaml 的 sim 段没有任何消费者
它声明 platform: gazebo，但 Gazebo 尚未接线，改它不会有任何效果。
加注释说明真实现状，避免用户以为改了就生效。

澄清：camera 与 cameras 不是重复键，两者都在读
- camera（单数）= 当前激活相机的参数，被各 launch 与节点读
- cameras（复数）= 相机目录（型号/序列号），被 export_site_env.py 读
命名容易混淆，但删任一个都会坏的。

保留：fetch_isaac_flexiv.sh
一度按"0 引用"删除，随后发现 thirdparty/ 里确实存在 isaac_sim_ws 与
flexiv_sim_plugin，说明这条路线被真实使用过；且该脚本还负责 clone
flexiv_rdk（核心依赖）。已恢复，并在 scripts/README.md 标为"搁置未用"。

docs/scripts/README.md 补上 run_all.sh、rdk_drag_bridge.py、fetch_isaac_flexiv.sh，
并新增"排查脚本卡住时的常识"一节记录判活顺序。

测试：198 passed
 2026-09-23 11:25:19 +08:00
arm_dev 883ed6b0b9 P1：设备契约层收尾 + Flexiv 驱动适配器 + 触觉四路数据打通 
新增包
- arm_kinematics：从 arm_application 提取几何雅可比 + DLS 逆解（纯 numpy）。
  它被应用层和驱动层共用，留在应用层会让驱动反向依赖应用层。
  arm_application/geom_ik.py 改为 re-export shim，现有 import 一行未改。
- arm_driver_flexiv：非夕驱动适配器。只补 manifest 声明而现有节点缺失的接口
  （/home /enable /clear_fault /switch_tool /soft_drag /compliant_descent），
  已有能力不重复实现。RDK 访问经 rdk_py310 下的长驻桥，本包不 import flexivrdk。

新接口
- arm_msgs/SwitchTool.srv、arm_msgs/CompliantDescent.action
- 合并 capabilities 里语义重复的 set_tool / switch_tool

修复（都是只在真机上暴露、且难以定位的问题）
- rdk_client 会永久挂死：select 的"可读"不等于"有完整一行"，readline 遇到
  半行 JSON 会一直阻塞等换行符，超时机制完全失效。改用 os.read + 自己做行缓冲。
- CompliantDescent 行程少一步：descended + step > max 在浮点累加下会提前跳出，
  永远到不了标称行程。改用整数步数计数。
- CompliantDescentRunner 的 TF frame 退化成 _base_link：没继承节点已解析的 SN。
- resolve_orisys_config 仍指向旧路径；load_orisys_from_site 读的是旧键 config。

触觉四路数据打通
- 深度图 / 力矢量场原先被 tactile_worker 有意丢弃（注释写"图像不过队列"），
  从未发布到 ROS。新增 image_utils（数组→Image，编码自动判定）、独立的图像
  队列（maxsize=1 满则丢旧）与 /tactile/<side>/{image,vector}。
  默认关闭（tactile.image_hz=0），开启也不影响三力的 30fps 时序。
- mock 后端也会发图像，无硬件即可验证整条发布链路。

配置
- site.yaml 新增 tactile.image_hz；移除与 model 重复的旧键 tactile.config
- 型号配置从 src/arm_application/config/orisys/ 迁到
  devices/sensors/orisys/models/dd03/（随 SDK 一起放）

测试：198 个（原 139）
- arm_kinematics 24（雅可比/逆解的解析算例与不变量）
- arm_driver_flexiv 69（下压状态机 34 + RDK 协议层 35）
- arm_application 59（图像打包 36 + 矢量场探测 23）
 2026-09-23 11:17:12 +08:00
arm_dev 6edb8d39ab chore: 框架科学优化前的完整基线 
在开始'设备契约层 + 目录重构 + 通止规装配'改造之前，先钉住当前可运行状态，
使后续任何删除/重构都可回退。

本基线包含：
- 现有 ROS 2 工作区全部源码（arm_application / arm_control / arm_bringup 等）
- 官方 flexiv_ros2 栈（含 flexiv_hardware / flexiv_controllers 等符号链接）
- 设备契约层 src/arm_interfaces（新增，46 项单测通过）
- devices/ 设备选型与品牌清单（新增）
- scripts/gen_site_example.py 与由它生成的 config/site.example.yaml

按 .gitignore 排除：build/ / install/ / log/ / config/site.yaml / 模型权重二进制。
 2026-09-23 10:53:06 +08:00