orisys/ros_contorl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
17 Commits 2 Branches 0 Tags
02516fe5678680b2fcbc9a09f65b479177406083
Commit Graph

5 Commits

Author SHA1 Message Date
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 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 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 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