orisys/ros_contorl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
56 Commits 2 Branches 0 Tags
37b85aa09bf4aeaf642626682a8d10d7e75e2e73
Commit Graph

56 Commits

This Branch
This Branch
All Branches
Author SHA1 Message Date
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