 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
|
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
|
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 |
|