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
parent 6edb8d39ab
commit 883ed6b0b9
39 changed files with 4295 additions and 245 deletions
@@ -1,24 +1,41 @@
 #!/usr/bin/env python3
-"""RDK Free-Drive bridge (must run under rdk_py310 / Python 3.10).
+"""RDK 控制桥（必须在 rdk_py310 / Python 3.10 下运行）。

-Why soft-impedance drag
-----------------------
-Under AUTO (Remote) the Studio Free Drive button is unavailable (remote owns
-the robot). RDK FloatingJoint/FloatingCartesian often need an extra license.
-So we default to soft NRT_JOINT_IMPEDANCE: low stiffness + target tracks
-measured joints → arm can be pushed by hand while Remote stays on.
+为什么需要这个进程
+------------------
+非夕的 `flexivrdk` Python 绑定与 ROS 环境的 Python 3.11 ABI 不兼容，且 RDK 对
+同一台机器人是**独占访问**：多个进程同时连同一个 SN 会失败。所以框架里所有
+RDK 操作都收敛到本进程，由它串行化，业务节点通过 JSON 行协议远程调用。

-IMPORTANT: flexivrdk.Robot is NOT thread-safe. All robot API calls (including
-the drag loop) share one lock. Joint reads during drag use a cache so Enter
-does not race Stop()/SendJointPosition.
+这也让 `arm_driver_flexiv` 不必 import flexivrdk，符合
+`docs/INTERFACES.md` 的约束（业务节点不得 import flexivrdk）。

-Line protocol (JSON on stdout only; RDK logs → stderr):
-  {"cmd":"ping"}
-  {"cmd":"connect","sn":"Rizon4-063989"}
-  {"cmd":"drag_on","mode":"soft"}   # soft | floating
-  {"cmd":"drag_off"}
-  {"cmd":"joints"}
-  {"cmd":"quit"}
+软拖拽为什么用 NRT_JOINT_IMPEDANCE
+----------------------------------
+AUTO(Remote) 模式下示教器上的 Free Drive 按钮不可用（控制权在 Remote）。
+RDK 的 FloatingJoint/FloatingCartesian 通常需要额外授权。
+所以默认用低刚度 NRT_JOINT_IMPEDANCE：目标跟踪实测关节角 + 低刚度，
+于是手臂可以用手推动，同时 Remote 保持开启。
+
+线程安全
+--------
+`flexivrdk.Robot` 不是线程安全的。所有 robot API 调用（含拖拽循环）共用同一
+把锁 `rlock`。拖拽期间的关节读取走缓存，避免 Enter 与 Stop() 竞争。
+
+行协议（JSON 只写 stdout；RDK 自身日志走 stderr）
+-------------------------------------------------
+    {"cmd":"ping"}
+    {"cmd":"connect","sn":"Rizon4-063989","iface":"enp4s0"}
+    {"cmd":"enable"}                              # 使能 + 清故障（幂等）
+    {"cmd":"clear_fault"}                         # 显式清故障并使能
+    {"cmd":"switch_tool","name":"tool1"}          # 切工具；name 空=只查询
+    {"cmd":"status"}                              # operational/fault/mode/tool
+    {"cmd":"drag_on","mode":"soft"}               # soft | floating
+    {"cmd":"drag_off"}
+    {"cmd":"joints"}
+    {"cmd":"quit"}
+
+每个命令的应答都是单行 JSON，含 ``ok`` 字段；失败时另有 ``error``。
 """

 from __future__ import annotations
@@ -27,8 +44,23 @@ import json
 import sys
 import threading
 import time
+from pathlib import Path
 from typing import List, Optional

+# 公共 RDK 操作统一在 scripts/lib/rdk_helpers.py，避免与 rdk_switch_tool.py
+# 各写一份导致行为漂移。
+sys.path.insert(0, str(Path(__file__).resolve().parent / 'lib'))
+from rdk_helpers import (  # noqa: E402
+    clear_fault as rdk_clear_fault,
+    ensure_idle,
+    ensure_operational,
+    resolve_tool_name,
+    single_arm_groups,
+    switch_tool,
+    tool_name,
+    tool_params,
+)
+
 _JSON_OUT = sys.stdout
 sys.stdout = sys.stderr

@@ -131,29 +163,8 @@ class SoftDrag:


 def _ensure_operational(robot) -> Optional[str]:
-    try:
-        if robot.fault():
-            robot.ClearFault()
-            time.sleep(0.5)
-        if not robot.operational():
-            for fn in ('ServoOn', 'Enable'):
-                if hasattr(robot, fn):
-                    try:
-                        getattr(robot, fn)()
-                        break
-                    except Exception:
-                        pass
-            t0 = time.monotonic()
-            while not robot.operational() and time.monotonic() - t0 < 10:
-                time.sleep(0.2)
-        if not robot.operational():
-            return (
-                'Robot not operational (fault/E-Stop?). '
-                'Clear fault on pendant, release E-Stop, AUTO Remote.'
-            )
-    except Exception as exc:
-        return str(exc)
-    return None
+    """保留旧名以兼容既有调用点；实现已统一到 rdk_helpers.ensure_operational。"""
+    return ensure_operational(robot)


 def _idle_async(robot, lock: threading.Lock, flexivrdk, timeout: float = 4.0) -> bool:
@@ -308,6 +319,105 @@ def main() -> int:
                cached_q = q
                reply({'ok': True, 'q': q, 'drag': drag_on})

+            elif cmd == 'enable':
+                if robot is None:
+                    reply({'ok': False, 'error': '尚未 connect'})
+                    continue
+                err = ensure_operational(robot)
+                reply({
+                    'ok': err is None,
+                    'error': err,
+                    'operational': bool(robot.operational()),
+                })
+
+            elif cmd == 'clear_fault':
+                if robot is None:
+                    reply({'ok': False, 'error': '尚未 connect'})
+                    continue
+                err = rdk_clear_fault(robot)
+                if err:
+                    reply({'ok': False, 'error': err})
+                    continue
+                # 清故障后顺手使能，否则调用方还得再发一条 enable。
+                err2 = ensure_operational(robot)
+                reply({
+                    'ok': err2 is None,
+                    'error': err2,
+                    'fault': bool(robot.fault()),
+                    'operational': bool(robot.operational()),
+                })
+
+            elif cmd == 'switch_tool':
+                if robot is None:
+                    reply({'ok': False, 'error': '尚未 connect'})
+                    continue
+                want = str(msg.get('name', '')).strip()
+
+                # 切换工具要求 IDLE；这一步是同步的，因为切换本身不能被打断。
+                err = ensure_idle(robot, flexivrdk.Mode, timeout_s=12.0)
+                if err:
+                    reply({'ok': False, 'error': err})
+                    continue
+
+                try:
+                    with rlock:
+                        tool = flexivrdk.Tool(robot)
+                        groups = single_arm_groups(robot)
+                        available = [str(n) for n in (tool.list() or [])]
+                        anchor = groups[0] if groups else None
+
+                        if not want:
+                            # 纯查询：不切换，把当前工具与参数报回去。
+                            params = tool_params(tool, anchor)
+                            reply({
+                                'ok': True,
+                                'switched': False,
+                                'current': tool_name(tool, anchor),
+                                'available': available,
+                                **params,
+                            })
+                            continue
+
+                        target = resolve_tool_name(tool, want)
+                        already = all(tool_name(tool, g) == target for g in groups)
+                        if not already:
+                            for g in groups:
+                                switch_tool(tool, g, target)
+                            time.sleep(0.3)
+
+                        params = tool_params(tool, anchor)
+                        reply({
+                            'ok': True,
+                            'switched': not already,
+                            'already': already,
+                            'current': tool_name(tool, anchor),
+                            'target': target,
+                            'available': available,
+                            **params,
+                        })
+                except Exception as exc:  # noqa: BLE001
+                    reply({'ok': False, 'error': str(exc)})
+
+            elif cmd == 'status':
+                if robot is None:
+                    reply({'ok': False, 'connected': False, 'error': '尚未 connect'})
+                    continue
+                info = {'ok': True, 'connected': True, 'drag': drag_on}
+                try:
+                    info['operational'] = bool(robot.operational())
+                    info['fault'] = bool(robot.fault())
+                    info['mode'] = str(robot.mode())
+                except Exception as exc:  # noqa: BLE001
+                    info['error'] = str(exc)
+                try:
+                    with rlock:
+                        tool = flexivrdk.Tool(robot)
+                        groups = single_arm_groups(robot)
+                        info['tool'] = tool_name(tool, groups[0] if groups else None)
+                except Exception:
+                    info['tool'] = ''
+                reply(info)
+
            elif cmd == 'quit':
                if soft is not None:
                    soft.stop()