 arm_dev
|
c1cd862eea
|
真机落地前的仿真补齐：夹持/腕力/真值位姿，让仿真能验证 S07–S09
这批是 09-24 那轮仿真工作的收尾，一直没提交。内容按目的分四块：
1) 仿真真能「夹住」工件
sim_grasp_lock：给规与夹爪之间加接触锁（默认关，run_gauge_assembly --sim 才开）。
没有它时仿真里规只靠摩擦，S08 棘轮一转就滑出指间，于是把「仿真不真」误读成
「力判据不对」。新增 test_sim_grasp_lock.py。
2) 腕部力传感器进仿真
sim_wrist_ft（+ config/wrist_ft.yaml）：S07 的首选判据是腕部 FT 的轴向突变，
而仿真里以前根本没有腕部力，S07 只能退到触觉兜底，两套判据从没在同一场景下比过。
现在两条通道都在，接触判定可对照。
3) 真值位姿可查（不再靠猜）
sim_live_pose / sim_pose_service：把 Gazebo 里刚体的实测位姿发布出来，
stage2 的真值路径按**实测**位姿算孔位，而不是按场景文件里的标称位姿。
顺带把真值孔位的推导（truth_holes）独立出来并补测。
4) 工具几何成为可配置项
config/sim_tool_geometry.yaml + stage2 的 tool_tip_below_flange_m 启动参数：
仿真里规会在指间下滑，工具长每次装夹都不同（实测见过 243/254mm），
悬停高度与 S07 下探预算必须按实测值算，不能再写死。
应用层同时跟进：gauge_fsm / gauge_config 把「悬停时工具尖端入没入工件」的判据
（hover_z_guard）与拖件判定（drag_verdict）收成纯函数并补测；stage3 搜孔、
site 导出、run_gauge_assembly 与若干文档同步。
新增 test_gauge_exit_lift / test_gauge_gripper_client / test_node_handler_collisions。
测试：全仓库 1085 passed。
|
2026-09-28 10:17:06 +08:00 |
|
|
arm_dev
|
b29339e522
|
仿真感知：真值孔位 + 真值相机 + 补 /home，让流程不改代码就能跑仿真
三块补齐，目标是"同一个流程节点，仿真/真机都不改代码"：
1. 真值孔位（truth_holes.py）：把 scene.yaml 与模型 SDF 里已知的孔几何
直接投影到相机像素坐标，替代 HoughCircles。perception.source 选：
yolo / truth / external —— truth 就走这条路。
为什么不用 Hough：仿真里孔是"方孔 + 贴片亮环"的近似（SDF 没有布尔运算），
靠图像门槛去凑 radus 区间很脆；而孔位本来就是场景里已知量，直接投影
既准又稳，还能当真实检测算法的对照基线。
2. 真值相机（sim_truth_perception.py）：同理，直接算物体位姿，供
visual_servo/filtered_point 用，绕开 ONNX。
3. 补 /home 与 set_named_pose 服务（sim_home_service.py）：真机上这两个
由 Flexiv 驱动提供，仿真里没有 —— 缺了 S01 直接超时。
4. sim_tactile_adapter 容错导入 ros_gz_interfaces：那一路只是"接触点"，
力本身走 geometry_msgs/Wrench。typesupport 加载不了时退化成
contact 恒 False + centroid 恒 0，三力判据（S07 用的）不受影响 ——
宁可用退化模式跑，也不要整个适配器起不来。
5. repo_root 解析（已装包要能找回源码仓库根，见 test_repo_root.py 的用例）
与 use_sim_time 向子 launch 的传递。
另：scripts/gen_sim_handeye.py 把 URDF 真值导出手眼文件（仿真不标定）。
Co-authored-by: Cursor <cursoragent@cursor.com>
|
2026-09-24 14:43:13 +08:00 |
|
|
arm_dev
|
d32e6ae53f
|
P5c-6 场景物料落地 + Gazebo 中文手册：工件/通止规模型、场景校验、两处静默失效修复
一、场景物料（新增）
devices/sim/gazebo/models/workpiece/ 带三个孔的铝板。碰撞体是"带方孔的平板"，
视觉额外加两个 φ14/φ7 亮环 —— 因为真机的 HoughCircles 就是靠这两个亮环找孔，
只按碰撞体建视觉会让"孔检测"在仿真里永远失败，而那看起来像算法问题。
devices/sim/gazebo/models/gauge/ 通止规：通端/套环/握柄/止端四段，各段异色
便于肉眼判断方向。接触刚度刻意比台面软 —— 位置控制器会"刚性压进去"，
用默认刚度会被弹飞（注释里写了原因，不是随手抄的参数）。
devices/sim/gazebo/models/gauge_stand/ 规座（static）。规直接站在台面上会立刻倒，
加个方孔底座把它立住。
三个模型都是 model.sdf + model.config，原点在**底面中心**，单位米。
二、场景配置与校验（新增）
config/scene.yaml 物体不再写死在 world 里，改为 scene.yaml + 启动时动态 spawn。
理由：world 是"环境"（地面/台面/相机/系统插件），场景是"这一批活做什么"。
动 world 的风险面太大 —— 它只要丢一个系统声明，所有模型都 spawn 不进去（见旧坑 2）。
含 operating（相机到工件的距离等作业条件）与 gates_px（像素门槛）。
sim_scene.py 摆放校验：从 SDF **逐碰撞体**解析 AABB（不抄尺寸，避免漂移），
检查原点在底面 / 不穿台面 / 不出界 / 物体之间不嵌入。
重叠判定必须逐碰撞体三维判，不能拿总包围盒相交 ——
正常装配里本来就有嵌套（规立在规座方孔里），粗判会误报，
而误报的后果是人把检查整个关掉，之后真重叠也抓不到。
另有成像几何：把亮环/孔间距按相机距离换算成像素，与门槛比对。
scripts/check_sim_scene.py 验收工具（退出码可用）：摆放 + 门槛漂移 + 成像几何 +
悬停高度 vs 工作空间 + 手外相机是否**完整**看得见工件 + Gazebo 实际位姿。
三、修掉两处静默失效（都是"看起来一切正常"）
1) parse_spawned_poses 一直是**死代码**：它按 "x: N / y: N / z: N" 解析，
而 Fortress 实际输出是方括号两行（XYZ / RPY）。对不上 → 永远返回空 →
调用方按设计"降级成跳过位姿核对" → 位姿检查**从来没跑过**，
而日志只说"跳过"，看起来像正常降级。
现在两种格式都支持，并用真实抓下来的输出样本做回归测试。
教训：宽容降级会把"没检查"伪装成"检查通过"。
2) stop_sim_gz.sh 的模式表里漏了 robot_state_publisher（而注释里写着它有覆盖）：
脚本报"已停止"，它却还在发上一份 /robot_description 与 TF，
下一次启动就会"改了描述却不生效"。
顺带修掉同源的两个问题：awk 用 -v 传模式导致它匹配到自己、每次复查都多报
一个空命令行的幽灵 PID；以及"杀的时候"和"复查的时候"两份扫描实现会各自漂移。
现在共用同一个 scan_pids()，模式改用环境变量传递，并在杀完之后**再扫一遍**：
有残留就报出来并非零退出 —— 下次再有漏项，不再静默。
实测：--list 列出全套 9 个进程；停止后 rc=0 且 pgrep 干净。
四、相机：D405 分辨率对齐真机
仿真的 D405 此前渲染 640x480，而真机跑 848x480（fx 337 而非 447）。后果是
58mm 的两孔间距成像只剩 85px，低于 hole_distance_min=90 ——
孔检测在仿真里根本不可能通过，而现象看起来像"算法不行"。
已把 xacro 与 cameras.yaml 一起改成 848x480，并把启动时的比对从"只比视场"
扩成"视场 + 图幅"（sim_cameras.check_camera_consistency），
不一致时说明它对像素门槛的影响。
五、文档
devices/sim/gazebo/README.md 重写为中文手册：使用方法 / 建模要求 / 导入格式 /
配置方式（含"改哪里才真的生效"的接线表）/ 11 条实测坑（每条都有"怎么发现"）。
manifest.yaml 的进度、assets、description、坑清单与本轮状态对齐。
scripts/README.md 补两个验收工具与停止脚本；根 README 补仿真入口与手册链接。
cameras.yaml 里一处过期的函数名（check_fov_consistency）修正。
六、测试
新增 test_sim_scene.py（79 条，此前 sim_scene.py 完全没有单测 —— 这正是
位姿解析死掉却无人发现的原因）与 test_check_sim_scene.py（21 条），
含三条回归：world_boxes 的偏移碰撞体不能被对称化放大（假重叠）、
center_bore 不能被 _bore 后缀吃掉、Fortress 位姿输出格式。
全量：src/arm_sim_gazebo + arm_interfaces + arm_application 共 515 通过。
同时附上 config/site.example.yaml 的重新生成（此前 site.yaml 的注释改动没同步，
它的 parity 测试是红的）。
Co-authored-by: Cursor <cursoragent@cursor.com>
|
2026-09-23 17:13:00 +08:00 |
|
|
arm_dev
|
01315773e7
|
P5c-5 触觉适配器落地：Gazebo 关节力 → arm_msgs/TactileStatus
链路：指尖关节 force_torque（持续发布）→ ros_gz_bridge → sim_tactile_adapter
→ /tactile/{left,right}/status。字段口径与真机 tactile_node 完全同构：
20Hz、frame_id=<side>_tactile、fnormal clamp 到非负、所有力都是"相对静止
基线的增量"。所以 compliant_descent 与 stage3_search_node 一行不改就能在
仿真里跑 —— 这是"同一套接口、换后端"契约的一部分。
新增 sim_tactile.py 把三件纯数据变换独立出来（轴向投影、桥接参数、基线
跟踪），因为这三处的错误全都**不会报错**，只会让力的语义静默错位：
轴向映射反了照样"有力、会响应、松开归零"，基线把接触吃掉了则表现为
"明明夹住了 fnormal 恒为 0"。抽出来才有单测可钉（新增 25 条）。
已实测验收（关闭夹爪夹静态薄板，60×120×80mm）：
空载 fnormal ~1e-8、contact=false（基线相减到位）
夹住 contact=true、fnormal=714.7/102.5，切向同时起变化
松开 contact 立刻回 false（靠 contact_timeout_s，因为 gz 的接触消息
是事件式的，不接触时一条都不发），力回落到 ~1e-8
启动期 check_tactile_collisions 无告警
check_tactile_collisions 顺带被负向单测逼出两处"会被静默骗过"的判据：
sdformat 对缺失的 contact/topic 会补 __default_topic__、对缺失的 collision
会补 __default__，所以只判"空"是抓不到的 —— 必须连占位值一起判。
force_scale 口径写入 config/tactile.yaml：仿真输出 = Gazebo 牛顿 × force_scale。
要共用真机那套 250/600 阈值必须先标定它，否则要么永不触发要么一碰就炸
（实测位置控制器硬压可到几百，量级与真机完全不同）。
Co-authored-by: Cursor <cursoragent@cursor.com>
|
2026-09-23 16:32:56 +08:00 |
|
|
arm_dev
|
768da34cf4
|
P5c-5 触觉三向力落地：改用力指关节 force_torque 传感器（接触消息根本没有力）
此前方案是"从接触传感器拿力"，实测证明**行不通**：
gz.msgs.Contacts 虽然 schema 里有 wrench/force 字段，但 Fortress 的
Contact 系统从不填充它们 —— 一条接触消息全文只有 header + 碰撞体名 +
接触点坐标，连 normal/depth 都是空的；该系统的二进制里连一个 "wrench"
字符串都搜不到。所以那不是配置问题，是设计上只有位置。
改为：
- 三向力 ← <sensor type="force_torque"> 挂在**指尖关节**上
（转动关节物理上要承受三向约束力，读到的即整条手指链的力）
- 是否接触 ← 保留接触传感器（几何接触，比力阈值准）
- URDF 无"关节传感器"语法，用 sdformat 扩展 <gazebo reference="关节名">
表达；已验证转换后传感器确实落在 <joint> 内
踩坑（与 contact-system 同一个坑的第二次）：
world 里声明了任何 <plugin> 后 gz 不再加载默认系统列表，所以
forcetorque-system 必须显式声明，否则 /tactile/*/wrench 话题**不出现**、
日志一片空白。补上后立即出现。
已实测确认：
- wrench 话题 ~100Hz **持续发布**（无接触也发零）—— 这点关键，接触传感器
只在接触时发消息，若适配器只靠它驱动，下游会误判"话题断流"
- 静止读数 = 远端三个 link 重量 2.94mN，物理自洽
- 轴向：静止重量落在 child x（竖直），child z = 世界 +x，故法向 = child y；
符号与 force_scale 一起作为可配置标定项
Co-authored-by: Cursor <cursoragent@cursor.com>
|
2026-09-23 16:00:52 +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
|
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 |
|