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)
This commit is contained in:
arm_dev
2026-09-23 11:17:12 +08:00
parent 6edb8d39ab
commit 883ed6b0b9
39 changed files with 4295 additions and 245 deletions
+43 -2
View File
@@ -24,29 +24,55 @@ description: >
# 重要:上层算法依赖的数据契约
outputs:
- topic: tangential_1
ros_topic: "/tactile/{left|right}/status"
field: fshearx
msg: arm_msgs/TactileStatus
type: scalar
unit: 任意单位(非牛顿)
notes: 基线会漂移,所有阈值必须按「相对静止均值的增量」使用
- topic: tangential_2
ros_topic: "/tactile/{left|right}/status"
field: fsheary
msg: arm_msgs/TactileStatus
type: scalar
unit: 任意单位(非牛顿)
notes: 与 tangential_1 正交。通止规装配需要确认哪一路对应哪个方向,见 README
- topic: normal
ros_topic: "/tactile/{left|right}/status"
field: fnormal
msg: arm_msgs/TactileStatus
type: scalar
unit: 任意单位(非牛顿)
notes: 法向力。拧紧判定需要「法向力突增」这一条件
- topic: depth
ros_topic: "/tactile/{left|right}/image"
msg: sensor_msgs/Image
type: image
unit: null
notes: 接触面深度图
notes: >
接触面深度 / 形变图。编码随 SDK 返回值而定:float32 深度图是 32FC1,
uint8 形变图是 mono8(image_utils 自动判断,判不出来就跳过并告警一次)。
需要把 site.yaml 的 tactile.image_hz 设为 >0 才会发布 —— 默认 0(关闭),
因为图像每帧几百 KB,不该让它挤占三力那条 30fps 的时序。
- topic: force_vector
ros_topic: "/tactile/{left|right}/vector"
msg: sensor_msgs/Image
type: image
unit: null
notes: 接触点力矢量可视化
notes: >
接触点力矢量场。每像素 2 个 float32(dx, dy),编码 32FC2 —— 这是 ROS 里
表达矢量场的标准方式。
Orisys 的绑定里没有一个明确叫 "force vector map" 的方法,所以实现按语义
依次尝试 compute_flow / compute_hhd / compute_deformation,取第一个返回
二维以上数组的。全都拿不到就不发这个话题(宁可没有,也不发一张假图)。
同样需要 tactile.image_hz > 0。
# --- 厂商物料放置要求 ------------------------------------------------------
assets:
@@ -121,6 +147,21 @@ config_schema:
default: 20.0
doc: 数据发布率。
- key: image_hz
site_path: tactile.image_hz
type: float
unit: Hz
required: false
default: 0.0
doc: >
视触觉图像(深度/形变图,话题 /tactile/<side>/image)与力矢量场
(话题 /tactile/<side>/vector)的发布频率。0 = 关闭。
默认关是因为图像每帧几百 KB,跨进程序列化 + ROS 发布的开销远大于三力;
三力的 30fps 是接触判据的基础,不该被动到。图像走一条独立的
maxsize=1 队列,满了丢旧留新,所以打开它也不会拖累三力。
人工观察 / 调试设 5~10 即可,不必跟 target_fps 一样高。
- key: cuda
site_path: tactile.cuda
type: bool