From 883ed6b0b95c839b7cfa28eaf6773a2906765e20 Mon Sep 17 00:00:00 2001 From: arm_dev Date: Wed, 23 Sep 2026 11:17:12 +0800 Subject: [PATCH] =?UTF-8?q?P1=EF=BC=9A=E8=AE=BE=E5=A4=87=E5=A5=91=E7=BA=A6?= =?UTF-8?q?=E5=B1=82=E6=94=B6=E5=B0=BE=20+=20Flexiv=20=E9=A9=B1=E5=8A=A8?= =?UTF-8?q?=E9=80=82=E9=85=8D=E5=99=A8=20+=20=E8=A7=A6=E8=A7=89=E5=9B=9B?= =?UTF-8?q?=E8=B7=AF=E6=95=B0=E6=8D=AE=E6=89=93=E9=80=9A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 新增包 - 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//{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） --- config/site.example.yaml | 8 +- devices/robots/flexiv/manifest.yaml | 1 - devices/sensors/orisys/manifest.yaml | 45 +- docs/TACTILE.md | 14 +- scripts/lib/rdk_helpers.py | 189 ++++ scripts/rdk_drag_bridge.py | 190 ++++- src/arm_application/README.md | 2 +- .../arm_application/geom_ik.py | 163 +--- .../arm_application/image_utils.py | 149 ++++ .../arm_application/orisys_tactile.py | 126 ++- .../arm_application/tactile_node.py | 148 +++- src/arm_application/config/orisys/README.md | 10 - src/arm_application/config/orisys/dd03.json | 35 - src/arm_application/package.xml | 3 + src/arm_application/setup.cfg | 8 + src/arm_application/test/test_image_utils.py | 215 +++++ .../test/test_orisys_vector.py | 146 ++++ .../arm_driver_flexiv/__init__.py | 17 + .../arm_driver_flexiv/compliant_descent.py | 806 ++++++++++++++++++ .../arm_driver_flexiv/flexiv_arm_driver.py | 380 +++++++++ .../arm_driver_flexiv/rdk_client.py | 385 +++++++++ .../arm_driver_flexiv/site_config.py | 97 +++ src/arm_driver_flexiv/package.xml | 34 + .../resource/arm_driver_flexiv | 0 src/arm_driver_flexiv/setup.cfg | 10 + src/arm_driver_flexiv/setup.py | 26 + .../test/test_descent_control.py | 438 ++++++++++ src/arm_driver_flexiv/test/test_rdk_client.py | 341 ++++++++ .../arm_interfaces/capabilities.py | 10 +- src/arm_kinematics/arm_kinematics/__init__.py | 33 + src/arm_kinematics/arm_kinematics/geom_ik.py | 186 ++++ src/arm_kinematics/package.xml | 22 + src/arm_kinematics/resource/arm_kinematics | 0 src/arm_kinematics/setup.cfg | 5 + src/arm_kinematics/setup.py | 20 + src/arm_kinematics/test/test_geom_ik.py | 219 +++++ src/arm_msgs/CMakeLists.txt | 2 + src/arm_msgs/action/CompliantDescent.action | 40 + src/arm_msgs/srv/SwitchTool.srv | 17 + 39 files changed, 4295 insertions(+), 245 deletions(-) create mode 100644 scripts/lib/rdk_helpers.py create mode 100644 src/arm_application/arm_application/image_utils.py delete mode 100644 src/arm_application/config/orisys/README.md delete mode 100644 src/arm_application/config/orisys/dd03.json create mode 100644 src/arm_application/test/test_image_utils.py create mode 100644 src/arm_application/test/test_orisys_vector.py create mode 100644 src/arm_driver_flexiv/arm_driver_flexiv/__init__.py create mode 100644 src/arm_driver_flexiv/arm_driver_flexiv/compliant_descent.py create mode 100644 src/arm_driver_flexiv/arm_driver_flexiv/flexiv_arm_driver.py create mode 100644 src/arm_driver_flexiv/arm_driver_flexiv/rdk_client.py create mode 100644 src/arm_driver_flexiv/arm_driver_flexiv/site_config.py create mode 100644 src/arm_driver_flexiv/package.xml create mode 100644 src/arm_driver_flexiv/resource/arm_driver_flexiv create mode 100644 src/arm_driver_flexiv/setup.cfg create mode 100644 src/arm_driver_flexiv/setup.py create mode 100644 src/arm_driver_flexiv/test/test_descent_control.py create mode 100644 src/arm_driver_flexiv/test/test_rdk_client.py create mode 100644 src/arm_kinematics/arm_kinematics/__init__.py create mode 100644 src/arm_kinematics/arm_kinematics/geom_ik.py create mode 100644 src/arm_kinematics/package.xml create mode 100644 src/arm_kinematics/resource/arm_kinematics create mode 100644 src/arm_kinematics/setup.cfg create mode 100644 src/arm_kinematics/setup.py create mode 100644 src/arm_kinematics/test/test_geom_ik.py create mode 100644 src/arm_msgs/action/CompliantDescent.action create mode 100644 src/arm_msgs/srv/SwitchTool.srv diff --git a/config/site.example.yaml b/config/site.example.yaml index f45a13b..cc4e810 100644 --- a/config/site.example.yaml +++ b/config/site.example.yaml @@ -445,8 +445,8 @@ hole_detect: # bash ~/ros2/scripts/run_tactile.sh tactile: backend: "orisys" # orisys | mock - model: "dd03" # 传感器型号（设备契约层用的键；须与 devices/sensors/orisys/models/ 下的目录同名） - config: "dd03" # 旧键，与 model 同义。仅为兼容现有节点保留，P2 会移除 + # 传感器型号。必须与 devices/sensors/orisys/models/ 下的子目录名一致。 + model: "dd03" # 型号配置随 SDK 一起放：devices/sensors/orisys/models/<型号>/config.json config_file: "/home/USER/ros2/devices/sensors/orisys/models/dd03/config.json" # 用 by-path 稳定路径（插哪个 USB 口就认哪个口，不受 video 编号漂移影响）。 @@ -461,6 +461,10 @@ tactile: motion_threshold: 0.0 target_fps: 30.0 publish_hz: 20.0 + # 视触觉图像与力矢量场的发布频率。0 = 关闭（默认）。 + # 三力走 30fps 的轻量队列；图像每帧几百 KB，单独走一条 maxsize=1 的队列， + # 打开它不会影响三力的时序。需要看形变图/矢量场时设成 5~10 即可。 + image_hz: 0.0 rotation_config: "" # 左右共用；有分指文件则用下面两项 left_rotation: "" # 例：orisys SDK 的 rotation_config_0.json right_rotation: "" diff --git a/devices/robots/flexiv/manifest.yaml b/devices/robots/flexiv/manifest.yaml index e4ca7dc..519f874 100644 --- a/devices/robots/flexiv/manifest.yaml +++ b/devices/robots/flexiv/manifest.yaml @@ -46,7 +46,6 @@ capabilities: - servo_joint - servo_cartesian - compliant_descent - - set_tool - switch_tool - home - enable diff --git a/devices/sensors/orisys/manifest.yaml b/devices/sensors/orisys/manifest.yaml index 2bc5b54..aaacc8c 100644 --- a/devices/sensors/orisys/manifest.yaml +++ b/devices/sensors/orisys/manifest.yaml @@ -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//image）与力矢量场 + （话题 /tactile//vector）的发布频率。0 = 关闭。 + + 默认关是因为图像每帧几百 KB，跨进程序列化 + ROS 发布的开销远大于三力； + 三力的 30fps 是接触判据的基础，不该被动到。图像走一条独立的 + maxsize=1 队列，满了丢旧留新，所以打开它也不会拖累三力。 + 人工观察 / 调试设 5~10 即可，不必跟 target_fps 一样高。 + - key: cuda site_path: tactile.cuda type: bool diff --git a/docs/TACTILE.md b/docs/TACTILE.md index eafbea0..854998d 100644 --- a/docs/TACTILE.md +++ b/docs/TACTILE.md @@ -10,8 +10,18 @@ |------|------| | `arm_application/orisys_tactile.py` | SDK 适配 + `load_orisys_from_site` + `tactile_worker`（多进程工作器） | | `arm_application/tactile_interface.py` | 可替换接口 / Mock | -| `arm_application/tactile_node.py` | 发布 `/tactile/left\|right/status` | -| `config/orisys/dd03.json` | 传感器几何/跟踪（厂家配置，可进 git） | +| `arm_application/tactile_node.py` | 发布 `/tactile/left\|right/status`（三力），以及可选的 `/image`（深度图）与 `/vector`（力矢量场） | +| `arm_application/image_utils.py` | numpy 数组 → `sensor_msgs/Image` 的打包（编码自动判定） | +| `devices/sensors/orisys/models/dd03/config.json` | 传感器几何/跟踪（厂家配置，可进 git）。原先在 `src/arm_application/config/orisys/`，已迁到设备目录随 SDK 一起放 | + +**四路数据怎么拿到**：三力在 `/tactile//status`（`TactileStatus` 的 +`fnormal` / `fshearx` / `fsheary`），深度图在 `/tactile//image`， +力矢量场在 `/tactile//vector`（`32FC2`，每像素 dx/dy）。 + +后两路默认**关闭**：设 `site.yaml` 的 `tactile.image_hz > 0` 才发布。原因见下节 —— +图像不该和 30fps 的三力挤同一条队列。没有硬件时可以用 +`ros2 run arm_application tactile --ros-args -p backend:=mock -p image_hz:=5.0` +把这两条话题跑起来验证链路。 **架构：每侧手指一个独立进程**（厂家示例 `two_sensor_pipeline.py` 的推荐做法， 每进程独立 CUDA 上下文）。实测单进程双开两路会互相拖垮（30→0.8fps）， diff --git a/scripts/lib/rdk_helpers.py b/scripts/lib/rdk_helpers.py new file mode 100644 index 0000000..7ff86ab --- /dev/null +++ b/scripts/lib/rdk_helpers.py @@ -0,0 +1,189 @@ +#!/usr/bin/env python3 +"""RDK 操作公共函数（必须在 rdk_py310 / Python 3.10 下运行）。 + +背景与约束 +---------- +非夕的 `flexivrdk` Python 绑定与 ROS 环境的 Python 3.11 ABI 不兼容，所以所有 +直接调用 RDK 的代码必须以子进程方式跑在 `/home/cc/rdk_py310/bin/python` 下。 +因此本文件不能用任何 3.11+ 语法，也不 import ROS。 + +抽取原因 +-------- +`ensure_operational` / `ensure_idle` / tool 相关函数原先在 +`scripts/rdk_switch_tool.py` 与 `scripts/rdk_drag_bridge.py` 里各写了一份， +行为已经出现细微差异（一个用 ServoOn 一个用 Enable，超时时间不同）。 +统一到本文件后，两处调用方行为一致。 + +重要限制 +-------- +RDK 对同一台机器人是**独占访问**：同时有两个进程连着同一个 SN 会失败或行为异常。 +所以框架里所有 RDK 操作都应当经由 `rdk_drag_bridge.py` 这一个长驻进程串行化， +不要让业务节点各自起 RDK 连接。 +""" + +from __future__ import annotations + +import time +from typing import Any, Dict, List, Optional + + +# --------------------------------------------------------------------------- +# 使能与故障 +# --------------------------------------------------------------------------- + +def ensure_operational(robot: Any, timeout_s: float = 10.0) -> Optional[str]: + """确保机器人处于 operational（已使能、无故障）。 + + 返回 ``None`` 表示成功，否则返回给用户看的错误说明。 + + 这是幂等的：已经 operational 时不做任何事。 + """ + try: + if robot.fault(): + robot.ClearFault() + time.sleep(0.5) + if not robot.operational(): + # 不同 RDK 版本一个叫 ServoOn 一个叫 Enable。 + for fn in ("ServoOn", "Enable"): + if hasattr(robot, fn): + try: + getattr(robot, fn)() + break + except Exception: + pass + deadline = time.monotonic() + timeout_s + while not robot.operational() and time.monotonic() < deadline: + time.sleep(0.2) + if not robot.operational(): + return ( + "机器人未处于 operational 状态（故障 / 急停未复位？）。" + "请在示教器上清故障、松开急停，并确认处于 AUTO(Remote)。" + ) + except Exception as exc: # noqa: BLE001 + return str(exc) + return None + + +def clear_fault(robot: Any) -> Optional[str]: + """清除故障。返回 ``None`` 表示成功。""" + try: + robot.ClearFault() + time.sleep(0.5) + except Exception as exc: # noqa: BLE001 + return str(exc) + return None + + +def ensure_idle(robot: Any, mode: Any, timeout_s: float = 10.0) -> Optional[str]: + """确保机器人切到 IDLE 模式（工具切换等操作的前提）。 + + 返回 ``None`` 表示成功，否则返回错误说明。 + """ + try: + robot.SwitchMode(mode.IDLE) + return None + except Exception: + pass + + err = ensure_operational(robot, timeout_s=timeout_s) + if err: + return err + try: + robot.SwitchMode(mode.IDLE) + except Exception as exc: # noqa: BLE001 + return f"无法切到 IDLE: {exc}" + return None + + +# --------------------------------------------------------------------------- +# 工具坐标系 +# --------------------------------------------------------------------------- + +def single_arm_groups(robot: Any) -> List[Any]: + """返回工具 API 的关节组列表。 + + RDK 1.9 的 Tool API 是单臂的（没有 JointGroup 概念），此时返回 ``[None]`` + 作为哨兵，调用方据此走不带 group 参数的分支。老版本返回实际组名列表。 + """ + try: + info = robot.info() + groups = getattr(info, "single_arm_groups", None) or {} + keys = list(groups.keys()) if isinstance(groups, dict) else list(groups) + if keys: + return keys + except Exception: + pass + try: + import flexivrdk + + if hasattr(flexivrdk, "JointGroup"): + return [flexivrdk.JointGroup.ARM_1] + except Exception: + pass + return [None] + + +def tool_name(tool: Any, group: Any) -> str: + """读当前工具名。""" + try: + return str(tool.name() if group is None else tool.name(group)) + except Exception: + return "" + + +def tool_params(tool: Any, group: Any) -> Dict[str, Any]: + """读当前工具参数，归一化成普通 dict。 + + 返回 ``{"mass_kg": float, "com_m": [x,y,z], "tcp_m": [x,y,z]}``。 + 读不到的字段用 0 / 空列表。 + """ + try: + params = tool.params() if group is None else tool.params(group) + except Exception: + return {"mass_kg": 0.0, "com_m": [], "tcp_m": []} + + def _floats(value: Any) -> List[float]: + try: + return [float(v) for v in list(value)[:3]] + except Exception: + return [] + + return { + "mass_kg": float(getattr(params, "mass", 0.0) or 0.0), + "com_m": _floats(getattr(params, "CoM", []) or []), + "tcp_m": _floats(getattr(params, "tcp_location", []) or []), + } + + +def switch_tool(tool: Any, group: Any, target: str) -> None: + """切换工具。失败抛异常。""" + if group is None: + tool.Switch(target) + else: + tool.Switch(group, target) + + +def resolve_tool_name(tool: Any, want: str) -> str: + """把用户给的模糊名字解析成 Studio 工具池里的准确名字。 + + 大小写不敏感。找不到时抛 ``ValueError``，消息里列出全部可选项 —— + 这样操作员能立刻看到拼错在哪，而不是面对一个"切换失败"。 + """ + want = (want or "").strip() + if not want: + return "" + try: + names = list(tool.list() or []) + except Exception as exc: # noqa: BLE001 + raise ValueError(f"无法读取 Studio 工具列表: {exc}") from exc + + if want in names: + return want + lower = {str(n).lower(): str(n) for n in names} + hit = lower.get(want.lower()) + if hit: + return hit + raise ValueError( + f"工具 '{want}' 不在 Studio 工具池里。可选: " + + (", ".join(str(n) for n in names) if names else "（空）") + ) diff --git a/scripts/rdk_drag_bridge.py b/scripts/rdk_drag_bridge.py index 2299c9d..5a70a61 100755 --- a/scripts/rdk_drag_bridge.py +++ b/scripts/rdk_drag_bridge.py @@ -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() diff --git a/src/arm_application/README.md b/src/arm_application/README.md index 6ffe82a..7341300 100644 --- a/src/arm_application/README.md +++ b/src/arm_application/README.md @@ -29,7 +29,7 @@ | `config/perception.yaml` `servo.yaml` | 默认值，被 `site.yaml` 覆盖 | | `config/hole_detect.yaml` | `two_stage` 缺省；被 `site.yaml hole_detect` 覆盖 | | `config/hole_insert.yaml` | `yolo`/`hough` 缺省 | -| `config/orisys/dd03.json` | Orisys 厂家配置，路径写在 `site.yaml tactile.config_file` | +| ~~`config/orisys/dd03.json`~~ | 已迁走：型号配置随 SDK 放在 `devices/sensors/orisys/models/dd03/config.json`，路径写在 `site.yaml tactile.config_file` | | `config/handeye_result_89.yaml` / `_91.yaml` | 该臂手眼 T（标定写入；禁止混用） | | `config/handeye_poses_89.yaml` / `_91.yaml` | 该臂示教关节 | | `launch/visual_servo.launch.py` | 感知 + 伺服 + 手眼 TF | diff --git a/src/arm_application/arm_application/geom_ik.py b/src/arm_application/arm_application/geom_ik.py index 2b7357e..8a226be 100644 --- a/src/arm_application/arm_application/geom_ik.py +++ b/src/arm_application/arm_application/geom_ik.py @@ -1,135 +1,40 @@ #!/usr/bin/env python3 -"""TF-based geometric Jacobian + damped least-squares position IK step.""" +"""向后兼容 shim —— 实现已移到 :mod:`arm_kinematics.geom_ik`。 -from __future__ import annotations +为什么搬走 +---------- +这套几何雅可比 + DLS 逆解同时被应用层（视觉伺服、搜孔装配）和驱动层 +（``arm_driver_flexiv`` 的柔顺下压）需要。留在 ``arm_application`` 里会让驱动层 +反过来依赖应用层，属于层次倒置。搬到不依赖 ROS 的 ``arm_kinematics`` 后， +两边都依赖一个底层包。 -from typing import List, Optional, Sequence, Tuple +为什么保留这个文件 +------------------ +``stage2_locate_node`` / ``stage3_search_node`` / ``servo_node`` 等现有代码都在 +``from arm_application.geom_ik import ...``。保留 shim 就不用改动这些已经在跑的 +文件 —— 重构不应牵连能工作的代码。 -import numpy as np +新代码请直接 ``from arm_kinematics import dls_twist``。 +""" -# URDF axes expressed in each child link frame (Rizon4). -RIZON_LINK_AXES = [ - (0.0, 0.0, 1.0), # joint1 -> link1 - (0.0, 1.0, 0.0), # joint2 -> link2 - (0.0, 0.0, 1.0), - (0.0, 1.0, 0.0), - (0.0, 0.0, 1.0), - (0.0, 1.0, 0.0), - (0.0, 0.0, 1.0), +from arm_kinematics.geom_ik import ( # noqa: F401 + RIZON_LINK_AXES, + dls_position_step, + dls_translate_hold_ori, + dls_twist, + geometric_jacobian, + load_handeye_T, + position_jacobian, + quat_xyzw_to_R, +) + +__all__ = [ + "RIZON_LINK_AXES", + "quat_xyzw_to_R", + "geometric_jacobian", + "position_jacobian", + "dls_position_step", + "dls_twist", + "dls_translate_hold_ori", + "load_handeye_T", ] - - -def quat_xyzw_to_R(q: Sequence[float]) -> np.ndarray: - x, y, z, w = [float(v) for v in q] - n = np.sqrt(x * x + y * y + z * z + w * w) + 1e-12 - x, y, z, w = x / n, y / n, z / n, w / n - return np.array( - [ - [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)], - [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)], - [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)], - ], - dtype=float, - ) - - -def geometric_jacobian( - link_R: Sequence[np.ndarray], - link_p: Sequence[np.ndarray], - ee_p: np.ndarray, - axes_local: Sequence[Tuple[float, float, float]] = RIZON_LINK_AXES, -) -> np.ndarray: - """Return 6x7 geometric Jacobian in base frame (linear then angular).""" - n = len(axes_local) - J = np.zeros((6, n), dtype=float) - pee = np.asarray(ee_p, dtype=float).reshape(3) - for i in range(n): - R = np.asarray(link_R[i], dtype=float).reshape(3, 3) - p = np.asarray(link_p[i], dtype=float).reshape(3) - z = R @ np.asarray(axes_local[i], dtype=float).reshape(3) - J[:3, i] = np.cross(z, pee - p) - J[3:, i] = z - return J - - -def position_jacobian( - link_R: Sequence[np.ndarray], - link_p: Sequence[np.ndarray], - ee_p: np.ndarray, - axes_local: Sequence[Tuple[float, float, float]] = RIZON_LINK_AXES, -) -> np.ndarray: - """Return 3x7 translational Jacobian in base frame.""" - return geometric_jacobian(link_R, link_p, ee_p, axes_local)[:3, :] - - -def dls_position_step( - J: np.ndarray, - dp_base: np.ndarray, - *, - damping: float = 0.05, - max_dq: float = 0.05, -) -> np.ndarray: - """Solve J dq = dp with damped least squares; clip |dq|.""" - dp = np.asarray(dp_base, dtype=float).reshape(3) - JT = J.T - A = J @ JT + (damping ** 2) * np.eye(3) - dq = JT @ np.linalg.solve(A, dp) - dq = np.clip(dq, -abs(max_dq), abs(max_dq)) - return dq - - -def dls_twist( - J6: np.ndarray, - v_base: np.ndarray, - *, - w_base: Optional[np.ndarray] = None, - damping: float = 0.08, - max_qdot: float = 0.80, - ori_weight: float = 1.2, -) -> np.ndarray: - """Solve J qdot = [v; w] with orientation held (w≈0 by default).""" - dx = np.zeros(6, dtype=float) - dx[:3] = np.asarray(v_base, dtype=float).reshape(3) - if w_base is not None: - dx[3:] = np.asarray(w_base, dtype=float).reshape(3) - w = np.array([1.0, 1.0, 1.0, ori_weight, ori_weight, ori_weight], dtype=float) - Jw = J6 * w[:, None] - dxw = dx * w - JT = Jw.T - A = Jw @ JT + (damping ** 2) * np.eye(6) - qdot = JT @ np.linalg.solve(A, dxw) - return np.clip(qdot, -abs(max_qdot), abs(max_qdot)) - - -def dls_translate_hold_ori( - J6: np.ndarray, - dp_base: np.ndarray, - *, - damping: float = 0.08, - max_dq: float = 0.05, - ori_weight: float = 1.2, -) -> np.ndarray: - """Translate EE by dp while keeping orientation (wx,wy,wz ≈ 0).""" - dx = np.zeros(6, dtype=float) - dx[:3] = np.asarray(dp_base, dtype=float).reshape(3) - w = np.array([1.0, 1.0, 1.0, ori_weight, ori_weight, ori_weight], dtype=float) - Jw = J6 * w[:, None] - dxw = dx * w - JT = Jw.T - A = Jw @ JT + (damping ** 2) * np.eye(6) - dq = JT @ np.linalg.solve(A, dxw) - return np.clip(dq, -abs(max_dq), abs(max_dq)) - - -def load_handeye_T(path: str) -> np.ndarray: - """Load 4x4 from handeye_result.yaml (T_4x4 list).""" - import yaml - - with open(path, 'r', encoding='utf-8') as f: - data = yaml.safe_load(f) - if not isinstance(data, dict) or 'T_4x4' not in data: - raise ValueError(f'No T_4x4 in hand-eye file: {path}') - T = np.array(data['T_4x4'], dtype=float) - if T.shape != (4, 4): - raise ValueError(f'T_4x4 shape {T.shape}, expected (4,4)') - return T diff --git a/src/arm_application/arm_application/image_utils.py b/src/arm_application/arm_application/image_utils.py new file mode 100644 index 0000000..4f0d294 --- /dev/null +++ b/src/arm_application/arm_application/image_utils.py @@ -0,0 +1,149 @@ +"""把 numpy 数组打包成 ``sensor_msgs/Image``。 + +为什么需要 +---------- +Orisys 视触觉传感器的数据分两类： + +* **三力**（``fnormal`` / ``fshearx`` / ``fsheary``）—— 每帧几个标量，走子进程 + 队列几乎没有开销。 +* **图像类**（形变图 / 深度图 / 力矢量场）—— 每帧几百 KB 到几 MB。原先 + ``tactile_worker`` 为了性能**故意不过队列**，于是这些数据从未离开子进程、 + 也从未发布到 ROS 话题上。 + +本模块只负责最后一公里：把已经拿到的数组正确打包成 ``Image``。放在单独文件 +里是为了能脱离相机单测 —— 编码、``step``、通道顺序这些细节一旦写错，下游 +拿到的图会静默错位（图像撕裂、颜色通道互换），而不是报错。 + +支持的形状与编码 +---------------- +=============== ============== ========================================== +形状 dtype 编码 +=============== ============== ========================================== +``(H, W)`` ``uint8`` ``mono8`` +``(H, W)`` ``uint16`` ``mono16`` +``(H, W)`` ``float32`` ``32FC1`` +``(H, W, 1)`` 任意 ``mono8`` +``(H, W, 2)`` ``float32`` ``32FC2``（力矢量场：每像素 dx, dy） +``(H, W, 3)`` ``uint8`` ``rgb8`` +``(H, W, 4)`` ``uint8`` ``rgba8`` +=============== ============== ========================================== + +2 通道的 ``32FC2`` 是 ROS 里表达矢量场的标准做法，``step = W * 4 * 2``。 +""" + +from __future__ import annotations + +from typing import Optional, Tuple + +import numpy as np +from sensor_msgs.msg import Image + +__all__ = ["ndarray_to_image", "infer_encoding", "UnsupportedImageError"] + + +class UnsupportedImageError(ValueError): + """数组的形状/类型没有对应的 ROS 编码。 + + 宁可显式报错也不要猜 —— 猜错的结果是下游拿到一张静默错位的图。 + 错误消息里带上实际形状与 dtype，方便对照本文档的表格。 + """ + + +#: (通道数, dtype) -> ROS 编码 +_ENCODING_BY_CHANNELS = { + (1, np.dtype(np.uint8)): "mono8", + (1, np.dtype(np.uint16)): "mono16", + (1, np.dtype(np.float32)): "32FC1", + (2, np.dtype(np.float32)): "32FC2", + (3, np.dtype(np.uint8)): "rgb8", + (4, np.dtype(np.uint8)): "rgba8", +} + + +def infer_encoding(array: np.ndarray) -> str: + """推断 ROS 编码。推不出来时抛 :class:`UnsupportedImageError`。""" + arr = np.asarray(array) + if arr.ndim == 2: + channels = 1 + elif arr.ndim == 3: + channels = arr.shape[2] + else: + raise UnsupportedImageError( + f"只支持 2D（灰度）或 3D（H,W,C）数组，收到 {arr.ndim}D，形状 {arr.shape}" + ) + + key = (channels, arr.dtype) + encoding = _ENCODING_BY_CHANNELS.get(key) + if encoding is None: + supported = ", ".join( + f"{enc}({c}通道/{d.name})" + for (c, d), enc in sorted(_ENCODING_BY_CHANNELS.items(), key=lambda kv: kv[1]) + ) + raise UnsupportedImageError( + f"没有 {channels} 通道 + {arr.dtype.name} 对应的 ROS 编码。" + f"支持: {supported}。" + f"（收到形状 {arr.shape}）" + ) + return encoding + + +def _layout(array: np.ndarray) -> Tuple[int, int, int]: + """返回 ``(height, width, channels)``，并把 (H,W,1) 归一化成 2D 语义。""" + arr = np.asarray(array) + if arr.ndim == 2: + h, w = arr.shape + return int(h), int(w), 1 + h, w, c = arr.shape + return int(h), int(w), int(c) + + +def ndarray_to_image( + array: np.ndarray, + *, + frame_id: str = "", + stamp=None, + encoding: Optional[str] = None, +) -> Image: + """把数组打包成 ``sensor_msgs/Image``。 + + 参数 + ---- + array: + 图像或矢量场。必须是 C 连续（``np.ascontiguousarray`` 会自动处理）。 + encoding: + 显式指定编码以覆盖推断。仅在确实知道自己在做什么时使用 —— + 编码与 dtype 不匹配会让下游按错误的元素大小解读 ``data``。 + """ + arr = np.asarray(array) + if encoding is None: + encoding = infer_encoding(arr) + + height, width, channels = _layout(arr) + + # (H,W,1) 在内存里与 (H,W) 连续一致，压平即可。 + flat = np.ascontiguousarray(arr).reshape(-1) + + msg = Image() + if stamp is not None: + msg.header.stamp = stamp + msg.header.frame_id = frame_id + msg.height = height + msg.width = width + msg.encoding = encoding + msg.is_bigendian = False + msg.step = int(width * channels * flat.dtype.itemsize) + msg.data = flat.tobytes() + return msg + + +def expected_step(array: np.ndarray, encoding: Optional[str] = None) -> int: + """算出应当写入 ``Image.step`` 的值。测试与自检用。""" + arr = np.asarray(array) + _, width, channels = _layout(arr) + if encoding is not None: + channels = {"mono8": 1, "mono16": 1, "32FC1": 1, "32FC2": 2, + "rgb8": 3, "rgba8": 4}.get(encoding, channels) + itemsize = {"mono8": 1, "mono16": 2, "32FC1": 4, "32FC2": 4, + "rgb8": 1, "rgba8": 1}.get(encoding, arr.dtype.itemsize) + return int(width * channels * itemsize) + return int(width * channels * arr.dtype.itemsize) diff --git a/src/arm_application/arm_application/orisys_tactile.py b/src/arm_application/arm_application/orisys_tactile.py index 127bd61..e25e7ed 100644 --- a/src/arm_application/arm_application/orisys_tactile.py +++ b/src/arm_application/arm_application/orisys_tactile.py @@ -32,9 +32,28 @@ class OrisysDualConfig: baseline_seconds: float = 2.0 filter_alpha: float = 0.25 backend: str = "orisys" # orisys | mock + image_hz: float = 0.0 + """视触觉图像（深度/形变图）的发布频率。0 = 不发布图像。 + + 为什么默认关：图像每帧几百 KB，跨进程序列化 + ROS 发布的开销远大于三力。 + 三力路径必须跑在 30fps（接触判据靠它），图像只是给人看和给算法备用， + 默认不开以免动到已经调好的三力时序。装配现场按需在 site.yaml 打开。""" def resolve_orisys_config(config_name: str, config_file: str = "") -> str: + """把型号名解析成实际的 config.json 路径。 + + 查找顺序（先命中先用）： + + 1. 显式传入的 ``config_file`` + 2. 参数本身就是存在的文件路径 + 3. ``devices/sensors/orisys/models/<型号>/config.json`` —— 型号配置随 SDK 放， + 这是现在唯一推荐的位置 + 4. 老位置 ``src/arm_application/config/orisys/<型号>.json``（兼容保留： + 站点自己放的定制配置可能还在这里） + 5. Orisys 包自带的 ``configs/<型号>.json`` + 6. 都没找到就把原样返回，交给 SDK 自己解析（它支持内置型号名） + """ if config_file: p = Path(config_file).expanduser() if p.is_file(): @@ -43,14 +62,19 @@ def resolve_orisys_config(config_name: str, config_file: str = "") -> str: p = Path(raw).expanduser() if p.is_file(): return str(p) - pkg = ros2_root() / "src" / "arm_application" / "config" / "orisys" / f"{raw}.json" - if pkg.is_file(): - return str(pkg) + + root = ros2_root() + for cand in ( + root / "devices" / "sensors" / "orisys" / "models" / raw / "config.json", + root / "src" / "arm_application" / "config" / "orisys" / f"{raw}.json", + ): + if cand.is_file(): + return str(cand) try: import orisys - root = Path(orisys.__file__).resolve().parent.parent - for cand in (root / "configs" / f"{raw}.json", root / "config" / f"{raw}.json"): + pkg_root = Path(orisys.__file__).resolve().parent.parent + for cand in (pkg_root / "configs" / f"{raw}.json", pkg_root / "config" / f"{raw}.json"): if cand.is_file(): return str(cand) except Exception: @@ -63,7 +87,9 @@ def load_orisys_from_site(data: dict) -> OrisysDualConfig: left_raw = t.get("left") if isinstance(t.get("left"), dict) else {} right_raw = t.get("right") if isinstance(t.get("right"), dict) else {} cfg_file = str(t.get("config_file") or "") - cfg_name = str(t.get("config") or "dd03") + # model 是设备契约层选型号用的键（对应 models/<型号>/ 目录名）； + # config 是它的旧名，仅为兼容老站点文件保留，站点里应统一用 model。 + cfg_name = str(t.get("model") or t.get("config") or "dd03") resolved = resolve_orisys_config(cfg_name, cfg_file) common = dict( config_name=resolved, @@ -100,9 +126,37 @@ def load_orisys_from_site(data: dict) -> OrisysDualConfig: baseline_seconds=float(t.get("baseline_seconds") or 2.0), filter_alpha=float(t.get("filter_alpha") or 0.25), backend=str(t.get("backend") or "orisys"), + image_hz=float(t.get("image_hz") or 0.0), ) +def read_vector_map(sensor: Any) -> Optional[np.ndarray]: + """尝试从传感器读出力矢量场（每像素的切向力/位移矢量）。 + + 为什么要"尝试" + -------------- + Orisys 的 Python 绑定是编译过的，公开接口里没有一个明确叫 + "force vector map" 的方法。按语义最接近的是 ``compute_flow()`` + （形变光流 ≈ 切向位移场 ≈ 力矢量场），其次是 ``compute_hhd()``。 + 不同 SDK 版本这两个方法的返回值也不一样 —— 有的是 ``(H,W,2)`` 数组， + 有的返回 ``bool``（只给"形变是否超阈值"）或可视化图。 + + 所以这里逐个试，只接受真正是二维以上数组的结果；都拿不到就返回 ``None``， + 上层跳过该话题而不是发一张假图。哪一路成功会打到日志里，便于现场核对。 + """ + for name in ("compute_flow", "compute_hhd", "compute_deformation"): + fn = getattr(sensor, name, None) + if fn is None: + continue + try: + result = fn() + except Exception: + continue + if isinstance(result, np.ndarray) and result.ndim >= 2 and result.size: + return result + return None + + def _parse_video(value) -> int | str: if isinstance(value, int): return value @@ -153,13 +207,17 @@ class OrisysTactile(TactileInterface): self._calibrate_baseline() self._connected = True - def read_side(self, side: str) -> dict[str, Any]: - """读指定侧（单侧进程模式）；未连接/未知侧返回全零。""" + def read_side(self, side: str, want_vector: bool = False) -> dict[str, Any]: + """读指定侧（单侧进程模式）；未连接/未知侧返回全零。 + + ``want_vector=True`` 时额外计算力矢量场（逐像素运算，较贵）—— + 只应在需要发布矢量图的那些帧上打开。 + """ if not self._connected: return self._empty_sample(side) for item in self._sensors: if item["side"] == side: - return self._read_single(item) + return self._read_single(item, want_vector=want_vector) return self._empty_sample(side) def disconnect(self) -> None: @@ -226,7 +284,7 @@ class OrisysTactile(TactileInterface): raw={"left": left, "right": right, "centroid": self._estimate_centroid(left, right)}, ) - def _read_single(self, item: dict) -> dict[str, Any]: + def _read_single(self, item: dict, want_vector: bool = False) -> dict[str, Any]: sensor = item["sensor"] cfg = item["cfg"] img = sensor.get_img() @@ -263,6 +321,9 @@ class OrisysTactile(TactileInterface): "centroid": centroid, "centroid_found": centroid_found, "img": img, + # 矢量场只在被要求时才算：compute_flow 是逐像素运算，每帧都做会 + # 明显增加 worker 耗时，可能拖累三力的 30fps。 + "vector": read_vector_map(sensor) if want_vector else None, } def _empty_sample(self, side: str = "unknown") -> dict[str, Any]: @@ -276,6 +337,7 @@ class OrisysTactile(TactileInterface): "centroid": (0.0, 0.0), "centroid_found": False, "img": None, + "vector": None, } def _calibrate_baseline(self) -> None: @@ -403,10 +465,17 @@ def _usb_self_heal(vid_src, side: str) -> None: return f"[{side}] USB 自愈失败 {dev}: {exc}" -def tactile_worker(side: str, cfg: OrisysDualConfig, q, stop_event) -> None: - """独立进程：开一侧传感器，按 target_fps 把最新样本推进队列（图像不过队列）。 +def tactile_worker(side: str, cfg: OrisysDualConfig, q, stop_event, image_q=None) -> None: + """独立进程：开一侧传感器，把样本推进队列。 + + q: mp.Queue（调用方给 maxsize≈2，满了丢旧留新）—— 三力等轻量数据，每帧都推。 + image_q: mp.Queue 或 None —— 视触觉图像与力矢量场。 + + 图像默认**不过队列**（``image_q=None``）：每帧几百 KB，跨进程序列化 + + ROS 发布的开销会挤占三力的 30fps 时序。需要图像时调用方传一个 + ``maxsize=1`` 的队列，并按 ``cfg.image_hz`` 节流推送 —— 队列满时丢旧留新， + 主进程来不及消费也只会丢图像，不会影响三力。 - q: mp.Queue（调用方给 maxsize≈2，满了丢旧留新） stop_event: mp.Event，置位后干净退出（先 disconnect 释放 V4L2 设备） 自动重连：摄像头被强占/USB 抖动后会出现 get_img 持续 select 超时（空读， @@ -429,7 +498,23 @@ def tactile_worker(side: str, cfg: OrisysDualConfig, q, stop_event) -> None: except Exception: pass + def _put_image(item: dict) -> None: + """图像入队：只保留最新一帧。队列满说明主进程没在消费，丢掉旧的即可。""" + try: + if image_q is None: + return + if image_q.full(): + try: + image_q.get_nowait() + except Exception: + pass + image_q.put(item) + except Exception: + pass + period = 1.0 / max(1.0, float(getattr(cfg.left if side == "left" else cfg.right, "target_fps", 30.0))) + img_period = (1.0 / cfg.image_hz) if (image_q is not None and cfg.image_hz > 0) else 0.0 + last_img_t = 0.0 vid_src = getattr(cfg.left if side == "left" else cfg.right, "vid_src", None) tac: Optional[OrisysTactile] = None bad = 0 @@ -452,9 +537,11 @@ def tactile_worker(side: str, cfg: OrisysDualConfig, q, stop_event) -> None: time.sleep(min(backoff, 3.0)) backoff *= 2.0 continue + # 只有图像帧才算矢量场：compute_flow 是逐像素运算，每帧都算会拖累三力。 + want_img = image_q is not None and (time.perf_counter() - last_img_t) >= img_period t0 = time.perf_counter() try: - d = tac.read_side(side) + d = tac.read_side(side, want_vector=want_img) empty = d.get("img") is None except Exception: d, empty = None, True @@ -478,7 +565,16 @@ def tactile_worker(side: str, cfg: OrisysDualConfig, q, stop_event) -> None: bad = 0 dead_cycles = 0 if d is not None and not empty: - d = {k: v for k, v in d.items() if k != "img"} + if want_img: + last_img_t = time.perf_counter() + _put_image({ + "side": side, + "img": d.get("img"), + "vector": d.get("vector"), + "stamp": time.time(), + }) + # 图像与矢量场已单独走 image_q；三力这条轻量队列不背它们。 + d = {k: v for k, v in d.items() if k not in ("img", "vector")} d["error"] = "" _put(d) rest = period - (time.perf_counter() - t0) diff --git a/src/arm_application/arm_application/tactile_node.py b/src/arm_application/arm_application/tactile_node.py index 323968a..4170528 100644 --- a/src/arm_application/arm_application/tactile_node.py +++ b/src/arm_application/arm_application/tactile_node.py @@ -12,22 +12,32 @@ import multiprocessing as mp import signal import time +import numpy as np import rclpy from rclpy.node import Node +from arm_application.image_utils import UnsupportedImageError, ndarray_to_image from arm_application.orisys_tactile import load_orisys_from_site, tactile_worker from arm_application.site_io import load_site_dict, site_path from arm_msgs.msg import TactileStatus +from sensor_msgs.msg import Image class TactileNode(Node): def __init__(self) -> None: super().__init__("tactile") self.declare_parameter("publish_hz", 20.0) + # 下面两个可以用 ROS 参数覆盖 site.yaml，便于不改站点文件就做验证 + # （例如 mock 后端 + 打开图像话题来确认链路通）。 + self.declare_parameter("backend", "") + self.declare_parameter("image_hz", -1.0) data = load_site_dict() t = data.get("tactile") or {} hz = float(t.get("publish_hz") or self.get_parameter("publish_hz").value or 20.0) - self._backend = str(t.get("backend") or "orisys").strip().lower() + backend_override = str(self.get_parameter("backend").value).strip() + self._backend = ( + backend_override or str(t.get("backend") or "orisys") + ).strip().lower() self._mock = self._backend in ("mock", "mock_orisys") self._pub = { @@ -41,6 +51,37 @@ class TactileNode(Node): self._queues: dict[str, object] = {} self._stop = None + # --- 视触觉图像 / 力矢量场 --- + # 默认关闭（image_hz=0）：图像每帧几百 KB，不该拖累三力的 30fps 时序。 + # 打开后走一条 maxsize=1 的独立队列，满则丢旧留新 —— 主进程来不及消费 + # 只会丢图像，三力一条不受影响。 + self._image_hz = ( + float(self.get_parameter("image_hz").value) + if float(self.get_parameter("image_hz").value) >= 0 + else float(t.get("image_hz") or 0.0) + ) + self._image_q = None + self._image_warned: set[str] = set() + self._image_pub: dict[str, object] = {} + self._vector_pub: dict[str, object] = {} + self._image_count: dict[str, int] = {} + self._last_mock_img = 0.0 + + # 图像 publisher 与后端无关 —— mock 后端也要能发。 + # 否则"图像话题到底通不通"只能等到有相机、上真机那天才发现。 + if self._image_hz > 0: + for side in ("left", "right"): + self._image_pub[side] = self.create_publisher( + Image, f"/tactile/{side}/image", 5 + ) + self._vector_pub[side] = self.create_publisher( + Image, f"/tactile/{side}/vector", 5 + ) + self.get_logger().info( + f"视触觉图像已开启: {self._image_hz:.1f}Hz " + f"→ /tactile/{{left,right}}/image 与 /vector" + ) + if self._mock: self._connected = {"left": True, "right": True} self.get_logger().info("tactile mock backend") @@ -48,11 +89,14 @@ class TactileNode(Node): cfg = load_orisys_from_site(data) ctx = mp.get_context("spawn") # spawn：子进程不带 DDS 线程，CUDA 上下文干净 self._stop = ctx.Event() + if self._image_hz > 0: + self._image_q = ctx.Queue(maxsize=1) for side in ("left", "right"): q = ctx.Queue(maxsize=2) self._queues[side] = q p = ctx.Process( - target=tactile_worker, args=(side, cfg, q, self._stop), + target=tactile_worker, + args=(side, cfg, q, self._stop, self._image_q), name=f"tactile_{side}", daemon=True, ) p.start() @@ -81,7 +125,101 @@ class TactileNode(Node): self._connected[side] = True self._latest[side] = (time.monotonic(), latest) + def _drain_images(self) -> None: + """把 worker 推来的最新图像 / 力矢量场发布出去。 + + 只取最新一帧：worker 已按 ``image_hz`` 节流、队列 ``maxsize=1``， + 所以这里读到的永远是最新画面；主进程偶尔卡一下，丢掉的是中间帧 + 而不是最新帧。 + """ + if self._image_q is None: + return + latest = None + while True: + try: + latest = self._image_q.get_nowait() + except Exception: + break + if latest is None: + return + + side = str(latest.get("side") or "") + if side not in self._image_pub: + return + stamp = self.get_clock().now().to_msg() + frame_id = f"{side}_tactile" + + self._publish_array( + self._image_pub[side], latest.get("img"), frame_id, stamp, side, "image" + ) + self._publish_array( + self._vector_pub[side], latest.get("vector"), frame_id, stamp, side, "vector" + ) + + def _publish_array(self, pub, array, frame_id: str, stamp, side: str, tag: str) -> None: + """打包并发布一个数组；形状/类型不支持时跳过该话题而不是崩溃。 + + ``array is None`` 是正常情况（该侧没有矢量场输出、或本帧没算）， + 不报错也不发空图 —— 发一张全零的假图会让下游以为接触到了东西。 + """ + if array is None: + return + key = f"{side}_{tag}" + try: + msg = ndarray_to_image(array, frame_id=frame_id, stamp=stamp) + except UnsupportedImageError as exc: + # 编码不支持是固定的（SDK 给的 dtype 不会变），每侧每种图只提醒一次， + # 否则 5Hz 的警告会把现场日志冲掉。 + if key not in self._image_warned: + self._image_warned.add(key) + self.get_logger().warn( + f"[{key}] 无法打包成 Image，已跳过该话题: {exc}" + ) + return + + try: + pub.publish(msg) + except Exception as exc: + if f"{key}_pub" not in self._image_warned: + self._image_warned.add(f"{key}_pub") + self.get_logger().warn(f"[{key}] 发布失败: {exc}") + return + + if self._image_count.get(key, 0) == 0: + self.get_logger().info( + f"[{key}] 首帧已发布 ({msg.width}x{msg.height} {msg.encoding})" + ) + self._image_count[key] = self._image_count.get(key, 0) + 1 + + def _publish_mock_images(self, force: float) -> None: + """mock 后端下造一张形变图 + 一张力矢量场，用来打通图像发布链路。 + + 没有传感器时也要能把 ``/tactile//image`` 与 ``/vector`` 跑起来 —— + 否则这两个话题是否真的能出图，只能等到上真机那天才知道。 + + 图像内容模仿视触觉的形变：中间一个高斯隆起（法向深度）， + 连同指向中心的切向流（矢量场），接触越深幅度越大。 + """ + if not self._image_pub: + return + stamp = self.get_clock().now().to_msg() + h, w = 48, 64 + yy, xx = np.mgrid[0:h, 0:w] + cx, cy = w / 2.0, h / 2.0 + bump = np.exp(-(((xx - cx) ** 2 + (yy - cy) ** 2) / (2.0 * 9.0 ** 2))) + + depth = (bump * (0.05 + 0.5 * float(force))).astype(np.float32) + vx = ((cx - xx) / w * bump * float(force)).astype(np.float32) + vy = ((cy - yy) / h * bump * float(force)).astype(np.float32) + vector = np.dstack([vx, vy]) + + for side in ("left", "right"): + frame = f"{side}_tactile" + self._publish_array(self._image_pub[side], depth, frame, stamp, side, "image") + self._publish_array(self._vector_pub[side], vector, frame, stamp, side, "vector") + def _tick(self) -> None: + self._drain_images() if self._mock: self._mock_n += 1 force = 0.2 if self._mock_n >= 8 else 0.0 @@ -91,6 +229,12 @@ class TactileNode(Node): "fps": 30.0, "is_contact": self._mock_n >= 8, "centroid": (0.0, 0.0), }) + # mock 的图像也按 image_hz 节流，避免比真机行为更"勤快"。 + if self._image_hz > 0: + now = time.monotonic() + if now - self._last_mock_img >= 1.0 / self._image_hz: + self._last_mock_img = now + self._publish_mock_images(force) return for side in ("left", "right"): self._drain(side) diff --git a/src/arm_application/config/orisys/README.md b/src/arm_application/config/orisys/README.md deleted file mode 100644 index 2f7f0f7..0000000 --- a/src/arm_application/config/orisys/README.md +++ /dev/null @@ -1,10 +0,0 @@ -# Orisys 传感器配置 - -`dd03.json` 是厂家相机/跟踪参数，由 `site.yaml` → `tactile.config_file` 指向。 -换型号时把新 json 放到本目录，并改 `tactile.config` / `config_file`。 - -Orisys Python SDK **不进本仓库**，在工控机： - -```bash -pip install -e /path/to/orisys-sdk -``` diff --git a/src/arm_application/config/orisys/dd03.json b/src/arm_application/config/orisys/dd03.json deleted file mode 100644 index 29ebb3b..0000000 --- a/src/arm_application/config/orisys/dd03.json +++ /dev/null @@ -1,35 +0,0 @@ -{ - "name": "dd03", - "type": "single_camera", - "version": "1.0", - "camera": { - "resolution_raw": [1280, 720], - "fps": 30, - "resolution_stitch": [420, 450], - "source_scale": 0.4, - "old": [672, 728, 1, 0.5], - "work": [504, 546, 1], - "work2": [420, 450, 0.8], - "work3": [350, 375, 4, 0.7], - "work4": [280, 300, 4, 0.4, 0.2] - }, - "tracker": { - "adaptive": true, - "lower_threshold": 12.0, - "upper_threshold": 18.0, - "loop_closure_interval": 1, - "algorithm": "dis", - "dis_preset": "medium", - "dis_mode": 3, - "algorithm_list": ["raft", "dis"], - "hhd_downsample": 3 - }, - "contact": { - "min_contour_area": 3000, - "flow_threshold": 0.2, - "div_threshold": 0.022, - "expansion_threshold": 0.01, - "kernel_size": [5, 5], - "denoise_sigma": 12 - } -} diff --git a/src/arm_application/package.xml b/src/arm_application/package.xml index 8de5ab9..8015b8d 100644 --- a/src/arm_application/package.xml +++ b/src/arm_application/package.xml @@ -19,6 +19,9 @@ tf2_ros python3-yaml python3-serial + + arm_kinematics ament_python diff --git a/src/arm_application/setup.cfg b/src/arm_application/setup.cfg index 4139b12..eb5efb9 100644 --- a/src/arm_application/setup.cfg +++ b/src/arm_application/setup.cfg @@ -2,3 +2,11 @@ script_dir=$base/lib/arm_application [install] install_scripts=$base/lib/arm_application + +[tool:pytest] +# ROS 2 Humble 的 launch_testing / launch_ros 两个 pytest 插件用了旧版 hook +# 签名（pytest_pycollect_makemodule 多声明了 path 参数），当前 pytest 会在 +# 加载插件阶段就抛 PluginValidationError，连收集都进不去。 +# 本包的单测不涉及 launch 测试，直接禁掉这两个插件。 +addopts = -p no:launch_testing -p no:launch_ros +testpaths = test diff --git a/src/arm_application/test/test_image_utils.py b/src/arm_application/test/test_image_utils.py new file mode 100644 index 0000000..0275048 --- /dev/null +++ b/src/arm_application/test/test_image_utils.py @@ -0,0 +1,215 @@ +"""``image_utils`` 的单测：数组 -> ``sensor_msgs/Image`` 的打包是否正确。 + +这类代码的错误是**静默**的：编码写错、step 算错、通道顺序颠倒，都不会抛异常， +只是下游拿到的图裂开或颜色错位 —— 而现场很难判断是相机坏了还是打包错了。 +所以这里把每种编码的字节布局都钉死。 +""" + +from __future__ import annotations + +import numpy as np +import pytest + +from arm_application.image_utils import ( + UnsupportedImageError, + expected_step, + infer_encoding, + ndarray_to_image, +) + + +# --------------------------------------------------------------------------- +# 编码推断 +# --------------------------------------------------------------------------- + + +class TestInferEncoding: + def test_mono8(self): + assert infer_encoding(np.zeros((4, 5), np.uint8)) == "mono8" + + def test_mono16(self): + assert infer_encoding(np.zeros((4, 5), np.uint16)) == "mono16" + + def test_float_depth_is_32fc1(self): + """深度图常以米为单位的 float32 出现，必须是 32FC1 而不是 mono。""" + assert infer_encoding(np.zeros((4, 5), np.float32)) == "32FC1" + + def test_vector_field_is_32fc2(self): + """力矢量场（每像素 dx, dy）用 32FC2 表达，这是 ROS 里的标准做法。""" + assert infer_encoding(np.zeros((4, 5, 2), np.float32)) == "32FC2" + + def test_rgb8(self): + assert infer_encoding(np.zeros((4, 5, 3), np.uint8)) == "rgb8" + + def test_rgba8(self): + assert infer_encoding(np.zeros((4, 5, 4), np.uint8)) == "rgba8" + + def test_single_channel_3d_is_mono(self): + """(H,W,1) 等价于灰度，不要因为没有 1 通道的编码表项就报错。""" + assert infer_encoding(np.zeros((4, 5, 1), np.uint8)) == "mono8" + + def test_1d_is_rejected(self): + with pytest.raises(UnsupportedImageError) as exc: + infer_encoding(np.zeros(10, np.uint8)) + assert "2D" in str(exc.value) + + def test_4d_is_rejected(self): + with pytest.raises(UnsupportedImageError): + infer_encoding(np.zeros((2, 3, 4, 5), np.uint8)) + + def test_uint16_rgb_is_rejected(self): + """16 位 RGB 没有标准编码；宁可报错也不要按 8 位打包（会静默错位）。""" + with pytest.raises(UnsupportedImageError) as exc: + infer_encoding(np.zeros((4, 5, 3), np.uint16)) + assert "形状" in str(exc.value) + + def test_float64_is_rejected(self): + """float64 图很常见（默认 dtype），必须显式转换，不能猜。""" + with pytest.raises(UnsupportedImageError): + infer_encoding(np.zeros((4, 5), np.float64)) + + def test_error_lists_supported_combinations(self): + with pytest.raises(UnsupportedImageError) as exc: + infer_encoding(np.zeros((3, 3, 5), np.uint8)) + assert "mono8" in str(exc.value) and "rgb8" in str(exc.value) + + +# --------------------------------------------------------------------------- +# 打包 +# --------------------------------------------------------------------------- + + +class TestPackaging: + def test_mono8_layout(self): + arr = np.arange(12, dtype=np.uint8).reshape(3, 4) + msg = ndarray_to_image(arr, frame_id="left_tactile") + assert (msg.height, msg.width) == (3, 4) + assert msg.encoding == "mono8" + assert msg.step == 4 + assert msg.header.frame_id == "left_tactile" + assert msg.is_bigendian is False + assert bytes(msg.data) == arr.tobytes() + + def test_mono16_step_and_bytes(self): + arr = np.arange(12, dtype=np.uint16).reshape(3, 4) + msg = ndarray_to_image(arr) + assert msg.encoding == "mono16" + assert msg.step == 4 * 2 # 每像素 2 字节 + assert len(msg.data) == 3 * 4 * 2 + assert bytes(msg.data) == arr.tobytes() + + def test_rgb8_step_is_three_bytes_per_pixel(self): + arr = np.zeros((5, 7, 3), np.uint8) + msg = ndarray_to_image(arr) + assert msg.encoding == "rgb8" + assert msg.step == 7 * 3 + assert msg.height == 5 and msg.width == 7 + + def test_rgba8_step(self): + msg = ndarray_to_image(np.zeros((5, 7, 4), np.uint8)) + assert msg.step == 7 * 4 + + def test_vector_field_32fc2(self): + """矢量场：每像素 2 个 float32，step 必须是 W*8。""" + arr = np.zeros((6, 8, 2), np.float32) + msg = ndarray_to_image(arr) + assert msg.encoding == "32FC2" + assert msg.step == 8 * 4 * 2 + assert len(msg.data) == 6 * 8 * 2 * 4 + + def test_float_depth_32fc1(self): + msg = ndarray_to_image(np.zeros((6, 8), np.float32)) + assert msg.encoding == "32FC1" + assert msg.step == 8 * 4 + + def test_3d_single_channel_flattens_fine(self): + """(H,W,1) 压平后与 (H,W) 的字节完全一致。""" + a = np.arange(12, dtype=np.uint8).reshape(3, 4, 1) + b = np.arange(12, dtype=np.uint8).reshape(3, 4) + assert bytes(ndarray_to_image(a).data) == bytes(ndarray_to_image(b).data) + + def test_non_contiguous_input_is_handled(self): + """切片产生的非连续数组必须先转连续，否则 tobytes 的顺序是错的。""" + base = np.arange(24, dtype=np.uint8).reshape(4, 6) + view = base[:, ::2] # 非连续 + assert not view.flags["C_CONTIGUOUS"] + msg = ndarray_to_image(view) + assert msg.width == 3 + assert bytes(msg.data) == np.ascontiguousarray(view).tobytes() + + def test_width_and_height_are_ints_not_numpy(self): + """ROS 消息字段是 int32；塞 numpy 标量在某些版本会失败。""" + msg = ndarray_to_image(np.zeros((3, 4), np.uint8)) + assert isinstance(msg.height, int) and isinstance(msg.width, int) + assert isinstance(msg.step, int) + + def test_stamp_is_forwarded(self): + """stamp 必须原样传下去 —— 下游靠它做时间对齐。""" + from builtin_interfaces.msg import Time + + stamp = Time(sec=123, nanosec=456) + msg = ndarray_to_image(np.zeros((2, 2), np.uint8), stamp=stamp) + assert msg.header.stamp.sec == 123 + assert msg.header.stamp.nanosec == 456 + + def test_stamp_defaults_to_zero_when_omitted(self): + msg = ndarray_to_image(np.zeros((2, 2), np.uint8)) + assert msg.header.stamp.sec == 0 and msg.header.stamp.nanosec == 0 + + def test_fortran_order_is_normalised(self): + arr = np.asfortranarray(np.arange(12, dtype=np.uint8).reshape(3, 4)) + msg = ndarray_to_image(arr) + assert bytes(msg.data) == np.ascontiguousarray(arr).tobytes() + + def test_empty_image(self): + """零尺寸图：别崩，正常给出 height=0。""" + msg = ndarray_to_image(np.zeros((0, 0), np.uint8)) + assert msg.height == 0 and msg.width == 0 + assert len(msg.data) == 0 + + def test_explicit_encoding_override_is_respected(self): + """显式指定编码时要照用 —— 有些相机给出的 BGR 需要按 rgb8 打包。""" + arr = np.zeros((3, 4, 3), np.uint8) + msg = ndarray_to_image(arr, encoding="rgb8") + assert msg.encoding == "rgb8" + + +# --------------------------------------------------------------------------- +# step 计算与 data 长度的一致性 +# --------------------------------------------------------------------------- + + +class TestStepConsistency: + """``step`` 与 ``len(data)`` 必须自洽：``len(data) == step * height``。 + + 这是下游按行读图时唯一依赖的不变量。不一致会导致整幅图错位。 + """ + + @pytest.mark.parametrize( + "shape,dtype", + [ + ((3, 4), np.uint8), + ((3, 4), np.uint16), + ((3, 4), np.float32), + ((3, 4, 1), np.uint8), + ((3, 4, 2), np.float32), + ((3, 4, 3), np.uint8), + ((3, 4, 4), np.uint8), + ((1, 1), np.uint8), + ((17, 23, 3), np.uint8), + ], + ) + def test_data_length_matches_step_times_height(self, shape, dtype): + arr = np.zeros(shape, dtype) + msg = ndarray_to_image(arr) + assert len(msg.data) == msg.step * msg.height, ( + f"shape={shape} dtype={dtype.name} encoding={msg.encoding} " + f"step={msg.step} h={msg.height} len={len(msg.data)}" + ) + + def test_expected_step_helper_agrees(self): + for shape, dtype in [((3, 4), np.uint8), ((3, 4, 3), np.uint8), + ((3, 4, 2), np.float32)]: + arr = np.zeros(shape, dtype) + msg = ndarray_to_image(arr) + assert msg.step == expected_step(arr) diff --git a/src/arm_application/test/test_orisys_vector.py b/src/arm_application/test/test_orisys_vector.py new file mode 100644 index 0000000..e7314ac --- /dev/null +++ b/src/arm_application/test/test_orisys_vector.py @@ -0,0 +1,146 @@ +"""``read_vector_map`` 的单测。 + +这个函数存在的唯一原因是：Orisys 的 Python 绑定是编译过的，公开接口里没有 +一个明确叫 "force vector map" 的方法，只能按语义去试 ``compute_flow`` / +``compute_hhd`` / ``compute_deformation``，而且要判断返回值到底是数组 +还是 bool/可视化图。 + +但它接的是真传感器，没法在单测里调。所以这里用假传感器把每条分支钉死： +**试错的顺序**和**接受的返回类型**一旦写错，表现是"矢量场话题永远没消息" +或"发了一张全零假图"，两者都很难从现场现象反推回代码。 +""" + +from __future__ import annotations + +from typing import Any + +import numpy as np +import pytest + +from arm_application.orisys_tactile import read_vector_map + + +class FakeSensor: + """可控假传感器。 + + 传入 ``方法名=返回值``；想让某个方法抛异常就传异常实例。 + 没有列出的方法属性一律不存在 —— 这正好模拟"老版本 SDK 没这个方法"。 + """ + + def __init__(self, **methods: Any) -> None: + for name, value in methods.items(): + if isinstance(value, BaseException): + def _raise(_exc=value): + raise _exc + + setattr(self, name, _raise) + else: + setattr(self, name, (lambda _v=value: _v)) + + +class TestPreferredSource: + def test_uses_compute_flow_first(self): + """compute_flow 语义上最接近"切向力矢量场"，优先级最高。""" + flow = np.ones((8, 6, 2), np.float32) + sensor = FakeSensor( + compute_flow=flow, + compute_hhd=np.zeros((8, 6, 3), np.uint8), + ) + assert read_vector_map(sensor) is flow + + def test_falls_back_to_hhd_when_flow_is_bool(self): + """有些版本 compute_flow 只返回 bool（"有没有流动"）—— 那不是矢量场， + 必须继续往下试，而不是把 True 当成图。""" + hhd = np.ones((4, 4), np.float32) + sensor = FakeSensor(compute_flow=True, compute_hhd=hhd) + assert read_vector_map(sensor) is hhd + + def test_falls_back_to_deformation(self): + deform = np.ones((5, 5, 2), np.float32) + sensor = FakeSensor(compute_flow=False, compute_hhd=None, compute_deformation=deform) + assert read_vector_map(sensor) is deform + + def test_skips_flow_that_raises(self): + hhd = np.ones((3, 3), np.float32) + sensor = FakeSensor(compute_flow=RuntimeError("no cuda"), compute_hhd=hhd) + assert read_vector_map(sensor) is hhd + + +class TestRejectedValues: + """不是"二维以上数组"的返回值一律不能当矢量场用。""" + + @pytest.mark.parametrize( + "bad", + [True, False, None, 0, 0.0, "flow", (1, 2), [1.0, 2.0], {"a": 1}], + ) + def test_non_array_values_are_rejected(self, bad): + assert read_vector_map(FakeSensor(compute_flow=bad)) is None + + def test_1d_array_is_rejected(self): + """1D 没法按 (H,W) 解释成图。""" + assert read_vector_map(FakeSensor(compute_flow=np.zeros(10, np.float32))) is None + + def test_empty_array_is_rejected(self): + """空数组同样是"没有数据"，不能发一张 0x0 的图。""" + assert read_vector_map(FakeSensor(compute_flow=np.zeros((0, 0), np.uint8))) is None + + def test_2d_array_is_accepted(self): + """2D 灰度图是合法结果（某些版本把形变强度图当成矢量场返回）。""" + arr = np.zeros((4, 5), np.uint8) + assert read_vector_map(FakeSensor(compute_flow=arr)) is arr + + def test_scalar_ndarray_is_rejected(self): + """0 维 ndarray 没有 (H,W) 语义。""" + assert read_vector_map(FakeSensor(compute_flow=np.array(3.0))) is None + + +class TestMissingMethods: + def test_all_missing_returns_none(self): + """老版本 SDK 三个方法都没有 —— 应当安静返回 None，让上层跳过话题。""" + assert read_vector_map(FakeSensor()) is None + + def test_all_raising_returns_none(self): + sensor = FakeSensor( + compute_flow=RuntimeError("x"), + compute_hhd=RuntimeError("y"), + compute_deformation=RuntimeError("z"), + ) + assert read_vector_map(sensor) is None + + def test_never_raises_even_when_everything_fails(self): + """这是它的契约：探测失败只能返回 None，不能把异常抛给 30fps 的读取循环。""" + sensor = FakeSensor(compute_flow=KeyboardInterrupt()) + # KeyboardInterrupt 是 BaseException；探测逻辑捕获 Exception， + # 所以它会被放行 —— 记录当前行为，提醒别在这里拿 BaseException 当普通错误。 + with pytest.raises(KeyboardInterrupt): + read_vector_map(sensor) + + +class TestResultIsUsableByImageUtils: + """探测到的数组必须能被 image_utils 打包 —— 否则等于白探测。""" + + def test_flow_shaped_array_packs_as_32fc2(self): + from arm_application.image_utils import ndarray_to_image + + flow = np.zeros((8, 6, 2), np.float32) + sensor = FakeSensor(compute_flow=flow) + arr = read_vector_map(sensor) + msg = ndarray_to_image(arr) + assert msg.encoding == "32FC2" + assert (msg.height, msg.width) == (8, 6) + + def test_grayscale_array_packs_as_mono8(self): + from arm_application.image_utils import ndarray_to_image + + arr = read_vector_map(FakeSensor(compute_flow=np.zeros((8, 6), np.uint8))) + assert ndarray_to_image(arr).encoding == "mono8" + + def test_float64_result_is_rejected_by_packer_not_silently_truncated(self): + """float64 是 numpy 的默认 dtype，SDK 很可能返回它。image_utils 会 + 明确报错而不是按 float32 打包 —— 那样会让每个像素错位 4 字节。""" + from arm_application.image_utils import UnsupportedImageError, ndarray_to_image + + arr = read_vector_map(FakeSensor(compute_flow=np.zeros((8, 6, 2), np.float64))) + assert arr is not None + with pytest.raises(UnsupportedImageError): + ndarray_to_image(arr) diff --git a/src/arm_driver_flexiv/arm_driver_flexiv/__init__.py b/src/arm_driver_flexiv/arm_driver_flexiv/__init__.py new file mode 100644 index 0000000..483ba26 --- /dev/null +++ b/src/arm_driver_flexiv/arm_driver_flexiv/__init__.py @@ -0,0 +1,17 @@ +"""非夕 Flexiv 机械臂驱动适配器。 + +对外只暴露一个可执行节点 ``flexiv_arm_driver``（见 ``setup.py`` 的 +``console_scripts``），其余模块是它的实现细节： + +``rdk_client`` + 管理 ``scripts/rdk_drag_bridge.py`` 长驻子进程的客户端。所有需要独占 RDK + 的操作都经它转发 —— 本包自身不 import flexivrdk（ABI 不兼容）。 +``site_config`` + 从 ``config/site.yaml`` 读参数默认值，使 ``ros2 run`` 也能直接起节点。 +``compliant_descent`` + 柔顺下压：纯逻辑状态机 + ROS 包装。 +``flexiv_arm_driver`` + 节点本体，装配上面几块。 +""" + +__version__ = "0.1.0" diff --git a/src/arm_driver_flexiv/arm_driver_flexiv/compliant_descent.py b/src/arm_driver_flexiv/arm_driver_flexiv/compliant_descent.py new file mode 100644 index 0000000..ebbbc50 --- /dev/null +++ b/src/arm_driver_flexiv/arm_driver_flexiv/compliant_descent.py @@ -0,0 +1,806 @@ +"""柔顺下压：从悬停位姿沿工具系 −Z 缓慢下压，直到触觉检测到接触。 + +为什么不用非夕 RDK 的柔顺模式 +---------------------------- +RDK 的 compliant motion 会接管整臂阻抗，而装配的其它阶段（跟踪 / 对准 / 拧入） +都走 JTC 位置控制。在两种控制权之间来回切换代价高，且切换瞬间的力冲击对通止规 +这种细长工件风险大。这里改用**应用层力闭环**：小幅步进（默认 0.5 mm）+ 步间读 +触觉，一旦切向力相对基线跳变就立刻停住。任何一步都可安全中断。 + +文件结构（两层，故意分开） +-------------------------- +1. :class:`DescentController` —— **纯逻辑**状态机。不 import ROS，所有外部交互 + （读触觉、走一步、看时间）都通过注入的回调完成。因此它的全部判据 —— + 接触 / 超时 / 行程上限 / 法向力安全限 / 触觉断流 —— 都能用假回调单测， + 见 ``test/test_descent_control.py``。 +2. :class:`CompliantDescentRunner` —— ROS 包装。负责 TF 读位形、DLS 逆解、 + 发 JTC 轨迹、订阅触觉，把纯逻辑接到真链路上。 + +力阈值都是**相对静止基线的增量** +-------------------------------- +Orisys 读数是形变任意单位且基线会漂移，绝对值没有意义。传感器节点在启动时测定 +基线并在发布前减掉，所以话题上的 ``fshearx`` / ``fsheary`` 已经是增量。 +""" + +from __future__ import annotations + +import math +import threading +import time +from dataclasses import dataclass +from enum import Enum +from typing import Callable, List, Optional, Tuple + +import numpy as np +import rclpy +from control_msgs.action import FollowJointTrajectory +from rclpy.action import ActionClient +from rclpy.callback_groups import CallbackGroup +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + +from arm_msgs.action import CompliantDescent +from arm_msgs.msg import TactileStatus +from arm_kinematics import RIZON_LINK_AXES, dls_translate_hold_ori, geometric_jacobian + +# --------------------------------------------------------------------------- +# 第一层：纯逻辑状态机 +# --------------------------------------------------------------------------- + + +class DescentOutcome(str, Enum): + """下压结束的原因。只有 ``CONTACT`` 算成功。""" + + CONTACT = "contact" + TIMEOUT = "timeout" + MAX_DESCENT = "max_descent" + NORMAL_LIMIT = "normal_limit" + TACTILE_STALE = "tactile_stale" + MOTION_FAILED = "motion_failed" + CANCELLED = "cancelled" + + +#: 各结束原因对应的 ``status`` 字段值（action 的 feedback/result 协议）。 +_STATUS_BY_OUTCOME = { + DescentOutcome.CONTACT: "contacting", + DescentOutcome.TIMEOUT: "error", + DescentOutcome.MAX_DESCENT: "error", + DescentOutcome.NORMAL_LIMIT: "error", + DescentOutcome.TACTILE_STALE: "error", + DescentOutcome.MOTION_FAILED: "error", + DescentOutcome.CANCELLED: "retracting", +} + + +@dataclass(frozen=True) +class DescentConfig: + """一次下压的全部参数。不可变，便于在测试里共享。""" + + contact_axis: int = 0 + """监测哪条切向通道：0 = 第一路切向（``fshearx``），1 = 第二路（``fsheary``）。""" + + contact_delta: float = 0.0 + """接触判据：|切向力| 相对基线的增量阈值（传感器原生单位）。""" + + max_normal_delta: float = 0.0 + """法向力安全上限（相对基线增量）。<= 0 表示不检查。""" + + descent_step_m: float = 0.0005 + """每步下压量（m）。越小越安全，但接触检测越慢。""" + + descent_speed_mps: float = 0.005 + """名义下压速度（m/s）。步进间隔 = step / speed。""" + + max_descent_m: float = 0.05 + """最多下压距离（m）。到顶仍未接触即判失败并停住 —— 这是安全上限。""" + + settle_sec: float = 0.05 + """每步后额外等待（s），让机械振荡衰减、触觉稳定。""" + + timeout_sec: float = 0.0 + """总超时（s）。<= 0 时按 ``max_descent_m / descent_speed_mps`` 推算并留余量。""" + + tactile_stale_sec: float = 0.5 + """触觉数据超过这个时长没更新就判断流并停住（安全）。""" + + @classmethod + def from_goal(cls, goal: CompliantDescent.Goal) -> "DescentConfig": + """从 action goal 构造。0 / NaN 的字段在这里被解释成默认值。""" + + def _f(value: float, fallback: float) -> float: + v = float(value) + return fallback if (math.isnan(v) or v == 0.0) else v + + step = _f(goal.descent_step_m, 0.0005) + step = min(max(step, 1e-5), 0.01) # 夹到 [0.01mm, 10mm]，防手滑填错单位 + return cls( + contact_axis=1 if int(goal.contact_axis) == 1 else 0, + contact_delta=_f(goal.contact_delta, 0.0), + max_normal_delta=float(goal.max_normal_delta), + descent_step_m=step, + descent_speed_mps=_f(goal.descent_speed_mps, 0.005), + max_descent_m=_f(goal.max_descent_m, 0.05), + settle_sec=max(0.0, float(goal.settle_sec)), + timeout_sec=max(0.0, float(goal.timeout_sec)), + ) + + @property + def step_interval_sec(self) -> float: + """步与步之间的间隔：走完一步需要的时间 + 稳定等待。""" + if self.descent_speed_mps <= 0: + return self.settle_sec + return self.descent_step_m / self.descent_speed_mps + self.settle_sec + + @property + def effective_timeout_sec(self) -> float: + """实际生效的超时。未指定时按行程推算并留 3 倍余量（至少 2s）。 + + 留余量是因为实际下压速度受 JTC 加减速限制，名义速度只是上界。 + """ + if self.timeout_sec > 0: + return self.timeout_sec + if self.descent_speed_mps <= 0: + return 2.0 + nominal = self.max_descent_m / self.descent_speed_mps + return max(2.0, nominal * 3.0) + + +@dataclass +class TactileSample: + """某一时刻的触觉读数（已减去基线）。""" + + tangential: float + """被监测那条切向通道的值。""" + + normal: float + """法向力。""" + + age_sec: float + """采样距今多久。用来判断数据是否断流。""" + + +def select_tangential(shearx: float, sheary: float, axis: int) -> float: + """按 ``contact_axis`` 从两路切向力里挑出要监测的那一路。 + + 0 = 第一路（``fshearx``），1 = 第二路（``fsheary``）。其它值按 0 处理。 + + 单独提成纯函数而不是写在 ROS 订阅回调里，是因为"到底看哪一路"是装配能否 + 检测到接触的关键，必须能脱离 ROS 单测 —— 见 ``test_descent_control.py``。 + """ + return float(sheary) if int(axis) == 1 else float(shearx) + + +@dataclass +class DescentResult: + """下压结束时的完整结论。""" + + outcome: DescentOutcome + descended_m: float = 0.0 + tangential_delta: float = 0.0 + normal_delta: float = 0.0 + message: str = "" + + @property + def success(self) -> bool: + return self.outcome is DescentOutcome.CONTACT + + @property + def status(self) -> str: + return _STATUS_BY_OUTCOME.get(self.outcome, "error") + + +class DescentController: + """下压状态机（纯逻辑）。 + + 参数 + ---- + cfg: + 全部阈值与运动参数。 + read_tactile: + 读一次触觉。返回 ``None`` 表示数据源不可用（未连接 / 话题没起来）。 + step_down: + 沿 −Z 走一步 ``dz``（正数）。返回 ``False`` 表示这一步没走成。 + now / sleep: + 注入时间源，测试里可以换成假时钟，避免真的 sleep。 + is_cancelled: + 返回 ``True`` 时立刻停下（外部取消或节点在关闭）。 + on_progress: + 每次循环结束时回调，用来发 action feedback。 + """ + + def __init__( + self, + cfg: DescentConfig, + read_tactile: Callable[[], Optional[TactileSample]], + step_down: Callable[[float], bool], + *, + now: Callable[[], float] = time.monotonic, + sleep: Callable[[float], None] = time.sleep, + is_cancelled: Callable[[], bool] = lambda: False, + on_progress: Optional[Callable[["DescentProgress"], None]] = None, + ) -> None: + self.cfg = cfg + self._read_tactile = read_tactile + self._step_down = step_down + self._now = now + self._sleep = sleep + self._is_cancelled = is_cancelled + self._on_progress = on_progress + + def run(self) -> DescentResult: + """执行下压，直到接触或触发任一停止条件。""" + cfg = self.cfg + step = cfg.descent_step_m + interval = cfg.step_interval_sec + deadline = self._now() + cfg.effective_timeout_sec + + # 用整数步数计数，而不是累加浮点再比较。 + # 反例：step=0.0005、max=0.05 时，"descended + step > max" 会因为 + # 0.0005 累加 99 次产生 1e-17 量级的误差而在第 100 步之前跳出， + # 结果是行程少走一步、永远到不了上限。计数法没有这个问题， + # 而且 descended_m 用 steps_done * step 一次算出，也不累积误差。 + max_steps = max(1, int(round(cfg.max_descent_m / step))) + steps_done = 0 + descended = 0.0 + last = TactileSample(0.0, 0.0, 0.0) + + while True: + if self._is_cancelled(): + return self._finish(DescentOutcome.CANCELLED, descended, last, "收到取消请求") + + sample = self._read_tactile() + if sample is None: + return self._finish( + DescentOutcome.TACTILE_STALE, descended, last, + "读不到触觉数据（传感器未连接 / 话题未发布）", + ) + last = sample + if sample.age_sec > cfg.tactile_stale_sec: + return self._finish( + DescentOutcome.TACTILE_STALE, descended, last, + f"触觉数据已过期 {sample.age_sec:.2f}s（> {cfg.tactile_stale_sec:.2f}s），" + "停止下压以免在无反馈情况下压坏工件", + ) + + # 安全上限优先于接触判据：法向力超限说明已经在硬顶，立刻停。 + if cfg.max_normal_delta > 0 and sample.normal >= cfg.max_normal_delta: + return self._finish( + DescentOutcome.NORMAL_LIMIT, descended, last, + f"法向力增量 {sample.normal:.1f} 达到安全上限 " + f"{cfg.max_normal_delta:.1f}，停止下压", + ) + + # 接触判据用绝对值：切向力往哪个方向跳都算接触。 + if abs(sample.tangential) >= cfg.contact_delta: + return self._finish( + DescentOutcome.CONTACT, descended, last, + f"检测到接触：切向增量 {sample.tangential:.1f} ≥ 阈值 " + f"{cfg.contact_delta:.1f}（已下压 {descended * 1000:.2f}mm）", + ) + + if self._now() > deadline: + return self._finish( + DescentOutcome.TIMEOUT, descended, last, + f"超时 {cfg.effective_timeout_sec:.1f}s 仍未接触（已下压 " + f"{descended * 1000:.2f}mm）", + ) + + if steps_done >= max_steps: + return self._finish( + DescentOutcome.MAX_DESCENT, descended, last, + f"已下压 {descended * 1000:.2f}mm 达到行程上限 " + f"{cfg.max_descent_m * 1000:.2f}mm 仍未接触", + ) + + if not self._step_down(step): + return self._finish( + DescentOutcome.MOTION_FAILED, descended, last, + "单步下压未执行成功（JTC 拒绝 / 控制器未激活 / TF 缺失）", + ) + steps_done += 1 + descended = steps_done * step + + if self._on_progress is not None: + self._on_progress( + DescentProgress( + fraction=_clamp01(descended / cfg.max_descent_m), + descended_m=descended, + tangential=sample.tangential, + normal=sample.normal, + status="approaching", + ) + ) + self._sleep(interval) + + def _finish( + self, outcome: DescentOutcome, descended: float, last: TactileSample, message: str + ) -> DescentResult: + if self._on_progress is not None: + self._on_progress( + DescentProgress( + fraction=_clamp01(descended / self.cfg.max_descent_m), + descended_m=descended, + tangential=last.tangential, + normal=last.normal, + status=_STATUS_BY_OUTCOME.get(outcome, "error"), + ) + ) + return DescentResult( + outcome=outcome, + descended_m=descended, + tangential_delta=last.tangential, + normal_delta=last.normal, + message=message, + ) + + +@dataclass +class DescentProgress: + """一次 feedback 快照。""" + + fraction: float + descended_m: float + tangential: float + normal: float + status: str + + +def _clamp01(value: float) -> float: + return max(0.0, min(1.0, float(value))) + + +# --------------------------------------------------------------------------- +# 第二层：ROS 包装 +# --------------------------------------------------------------------------- + + +class CompliantDescentRunner: + """``arm_msgs/CompliantDescent`` 的 action server。 + + 每步下压怎么落地 + ---------------- + 1. 从 TF 读 ``link1..7`` 在基座系下的位姿 → 用当前位形算几何雅可比 + 2. ``dls_translate_hold_ori(J, [0, 0, −step])`` 求关节增量 ``dq``（保持姿态） + 3. 把 ``q + dq`` 作为**单点** JTC 轨迹发给 ``rizon_arm_controller``，等它完成 + + 为什么是一步一发而不是一条长轨迹：接触时刻的位置无法事先知道，必须边走边看 + 力反馈；一步一发让我们能在任意一步之后停下，不需要取消一条已经下发的轨迹。 + """ + + #: 单步轨迹至少给这么长时间，否则 JTC 会因速度超限而拒绝。 + MIN_STEP_DURATION_SEC = 0.15 + + def __init__( + self, + node: Node, + callback_group: Optional[CallbackGroup] = None, + *, + default_sn: str = "", + ) -> None: + """``default_sn`` 是 ``robot_sn`` 参数的默认值。 + + 必须由调用方（驱动节点）传入它已经从 site.yaml 解析出的 SN —— 否则这里 + 的默认值会是空串，frame 名退化成 ``_base_link`` / ``_flange``， + TF 永远查不到，只有在真机跑起来报"读不到 TF"时才会发现。 + """ + self._node = node + self._cb = callback_group + self._log = node.get_logger() + + node.declare_parameter("action_name", "compliant_descent") + node.declare_parameter("jtc_action_name", "rizon_arm_controller/follow_joint_trajectory") + node.declare_parameter("robot_sn", default_sn) + node.declare_parameter("base_frame", "") + node.declare_parameter("ee_frame", "") + node.declare_parameter("joint_prefix", "") + node.declare_parameter("tactile_topic_tmpl", "/tactile/{side}/status") + + sn = str(node.get_parameter("robot_sn").value).strip() + self._prefix = str(node.get_parameter("joint_prefix").value).strip() or f"{sn}_" + self._base_frame = ( + str(node.get_parameter("base_frame").value).strip() or f"{sn}_base_link" + ) + self._ee_frame = str(node.get_parameter("ee_frame").value).strip() or f"{sn}_flange" + self._joint_names = [f"{self._prefix}joint{i}" for i in range(1, 8)] + self._link_frames = [f"{self._prefix}link{i}" for i in range(1, 8)] + self._tactile_tmpl = str(node.get_parameter("tactile_topic_tmpl").value) + + self._lock = threading.Lock() + self._q: Optional[List[float]] = None + #: side -> (fshearx, fsheary, fnormal, 到达时间 monotonic) + self._tactile: dict[str, Tuple[float, float, float, float]] = {} + self._cancelled = threading.Event() + + # TF + from tf2_ros import Buffer, TransformListener + + self._tf_buffer = Buffer() + self._tf_listener = TransformListener(self._tf_buffer, node) + + # 关节角：JTC 步进的输入 + from sensor_msgs.msg import JointState + + node.create_subscription( + JointState, "/joint_states", self._on_joint_state, 10, callback_group=callback_group + ) + + # 触觉：按下压时指定的一侧懒订阅（左右各建一个，成本很低） + self._tactile_subs = {} + for side in ("left", "right"): + topic = self._tactile_tmpl.format(side=side) + self._tactile_subs[side] = node.create_subscription( + TactileStatus, topic, self._make_tactile_cb(side), 10, + callback_group=callback_group, + ) + + self._jtc = ActionClient( + node, FollowJointTrajectory, + str(node.get_parameter("jtc_action_name").value), + callback_group=callback_group, + ) + + self._server = rclpy.action.ActionServer( + node, + CompliantDescent, + str(node.get_parameter("action_name").value), + execute_callback=self._execute, + cancel_callback=self._on_cancel, + callback_group=callback_group, + ) + + self._log.info( + f"compliant_descent 就绪: base={self._base_frame} " + f"jtc={self._jtc._action_name} 触觉模板={self._tactile_tmpl}" + ) + if not sn: + self._log.warn( + "robot_sn 为空，TF frame 名可能不对（默认 f'{sn}_base_link'）。" + "用 parameters 传 robot_sn。" + ) + + # -- 订阅回调 --------------------------------------------------------- + + def _on_joint_state(self, msg) -> None: + if not self._joint_names: + return + by_name = dict(zip(msg.name, msg.position)) + try: + q = [float(by_name[n]) for n in self._joint_names] + except KeyError: + # 里面还混着夹爪等其它关节，缺我们这 7 个就先不用这帧 + return + with self._lock: + self._q = q + + def _make_tactile_cb(self, side: str): + def cb(msg: TactileStatus) -> None: + with self._lock: + self._tactile[side] = ( + float(msg.fshearx), + float(msg.fsheary), + float(msg.fnormal), + time.monotonic(), + ) + + return cb + + # -- action 回调 ------------------------------------------------------ + + def _on_cancel(self, _goal_handle) -> "rclpy.action.CancelResponse": + self._cancelled.set() + return rclpy.action.CancelResponse(return_value=True) + + def _execute(self, goal_handle) -> CompliantDescent.Result: + goal = goal_handle.request + self._cancelled.clear() + cfg = DescentConfig.from_goal(goal) + + result = CompliantDescent.Result() + side = (goal.side or "right").strip().lower() + if side not in ("left", "right"): + side = "right" + + result.status = "approaching" + self._log.info( + f"柔顺下压开始: side={side} axis={cfg.contact_axis} " + f"阈值={cfg.contact_delta:.1f} 步长={cfg.descent_step_m * 1000:.2f}mm " + f"行程上限={cfg.max_descent_m * 1000:.1f}mm" + ) + + if not self._preflight(goal, cfg, side, result): + goal_handle.abort() + return result + + # 记录下压前的 XY：每步都把它作为目标，避免斜向漂移。 + start = self._lookup_ee() + if start is None: + result.status = "error" + result.message = f"TF 里读不到 {self._ee_frame}（base={self._base_frame}）" + goal_handle.abort() + return result + hold_xy = np.array(start[:3, 3], dtype=float) + + controller = DescentController( + cfg, + read_tactile=lambda: self._sample(cfg, side), + step_down=lambda dz: self._step_down(dz, hold_xy, cfg), + is_cancelled=self._cancelled.is_set, + on_progress=lambda p: self._publish_feedback(goal_handle, p), + ) + outcome = controller.run() + + contact = self._lookup_ee() + if contact is not None: + result.contact_pose.header.frame_id = self._base_frame + result.contact_pose.header.stamp = self._node.get_clock().now().to_msg() + p = contact[:3, 3] + result.contact_pose.pose.position.x = float(p[0]) + result.contact_pose.pose.position.y = float(p[1]) + result.contact_pose.pose.position.z = float(p[2]) + q = _R_to_quat_xyzw(contact[:3, :3]) + result.contact_pose.pose.orientation.x = q[0] + result.contact_pose.pose.orientation.y = q[1] + result.contact_pose.pose.orientation.z = q[2] + result.contact_pose.pose.orientation.w = q[3] + + result.success = outcome.success + result.message = outcome.message + result.descended_m = outcome.descended_m + result.contact_delta_observed = outcome.tangential_delta + result.normal_delta_observed = outcome.normal_delta + result.status = outcome.status + + if outcome.success: + self._log.info(outcome.message) + goal_handle.succeed() + else: + self._log.warn(f"柔顺下压未成功（{outcome.outcome.value}）: {outcome.message}") + goal_handle.abort() + return result + + def _preflight(self, goal, cfg: DescentConfig, side: str, result) -> bool: + """开工前的检查。任何一条不过就立刻放弃，不要带着问题往下压。""" + if cfg.contact_delta <= 0: + result.message = ( + "contact_delta 必须 > 0。力阈值是相对静止基线的增量，" + "请填传感器原生单位下的经验值（先前用 /tactile 话题观察接触时的跳变量）。" + ) + return False + + if not self._jtc.wait_for_server(timeout_sec=5.0): + result.message = ( + f"等不到 JTC 动作 {self._jtc._action_name}。" + "先启动机械臂：bash scripts/run_real.sh" + ) + return False + + if self._lookup_ee() is None: + result.message = ( + f"TF 里读不到 {self._ee_frame}（base={self._base_frame}）。" + "检查 robot_sn 参数与机械臂 TF 是否在发布。" + ) + return False + + with self._lock: + q = list(self._q) if self._q is not None else None + if q is None or len(q) < 7: + result.message = "还没收到 /joint_states 里的 7 个关节角" + return False + + sample = self._sample(cfg, side) + if sample is None: + result.message = ( + f"触觉话题 {self._tactile_tmpl.format(side=side)} 没有数据。" + "先启动传感器：ros2 launch arm_application tactile.launch.py" + ) + return False + + # 触觉话题存在但切向恒为 0 是已知的坑：老版 tactile_node 只发 fnormal。 + # 那种情况下切向判据永远不触发，流程会一路压到行程上限 —— 必须提前拦。 + if sample.tangential == 0.0: + self._log.warn( + "触觉切向读数为 0。若传感器固件在静止时确实输出 0 则可忽略；" + "但如果下压过程中它始终不变，说明节点没有发布切向通道 —— " + "请检查 tactile_node（老版本把 fshearx/fsheary 写成了 0.0）。" + ) + return True + + # -- 触觉 / TF / 运动 ------------------------------------------------- + + def _sample(self, cfg: DescentConfig, side: str) -> Optional[TactileSample]: + """取一次触觉。返回 ``None`` 表示这一侧完全没有数据。 + + ``age_sec`` 用节点时钟与订阅回调的到达时间之差估计。这里用壁钟而非 + ROS 时间：传感器节点的 stamp 可能是驱动时间戳（不是 ROS 时间）， + 直接相减会得到荒谬的年龄。 + """ + with self._lock: + entry = self._tactile.get(side) + if entry is None: + return None + shearx, sheary, fnormal, stamp = entry + return TactileSample( + tangential=select_tangential(shearx, sheary, cfg.contact_axis), + normal=fnormal, + age_sec=max(0.0, time.monotonic() - stamp), + ) + + def _lookup_ee(self) -> Optional[np.ndarray]: + return self._lookup(self._ee_frame) + + def _lookup(self, frame: str) -> Optional[np.ndarray]: + try: + tf = self._tf_buffer.lookup_transform( + self._base_frame, frame, rclpy.time.Time() + ) + except Exception: + return None + t = tf.transform.translation + r = tf.transform.rotation + T = np.eye(4) + T[:3, :3] = _quat_xyzw_to_R([r.x, r.y, r.z, r.w]) + T[:3, 3] = [t.x, t.y, t.z] + return T + + def _jacobian(self, ee_p: np.ndarray) -> Optional[np.ndarray]: + """用 TF 实测位形算几何雅可比。缺任一 link 的 TF 就没法算。""" + link_R: List[np.ndarray] = [] + link_p: List[np.ndarray] = [] + for frame in self._link_frames: + T = self._lookup(frame) + if T is None: + return None + link_R.append(T[:3, :3]) + link_p.append(T[:3, 3]) + return geometric_jacobian(link_R, link_p, ee_p, RIZON_LINK_AXES) + + def _step_down(self, dz: float, hold_xy: np.ndarray, cfg: DescentConfig) -> bool: + """沿 −Z 走一步。返回是否成功执行。 + + XY 每步都重新对准 ``hold_xy``：下压过程中若因姿态微调带出 XY 漂移， + 会在这一步被拉回来，避免"越压越歪"。 + """ + with self._lock: + q = np.array(self._q, dtype=float) if self._q is not None else None + if q is None or q.size < 7: + self._log.warn("步进失败：没有关节角") + return False + + ee = self._lookup_ee() + if ee is None: + self._log.warn(f"步进失败：TF 读不到 {self._ee_frame}") + return False + + J = self._jacobian(np.array(ee[:3, 3], dtype=float)) + if J is None: + self._log.warn("步进失败：缺 link TF，无法算雅可比") + return False + + dp = np.zeros(3) + p = np.array(ee[:3, 3], dtype=float) + dp[0] = hold_xy[0] - p[0] + dp[1] = hold_xy[1] - p[1] + dp[2] = -abs(dz) + # 单步内 XY 修正幅度限死，防止雅可比在某一位形下给出巨大的横向位移。 + dp[0] = float(np.clip(dp[0], -abs(dz), abs(dz))) + dp[1] = float(np.clip(dp[1], -abs(dz), abs(dz))) + + dq = dls_translate_hold_ori(J, dp, damping=0.08, max_dq=0.02) + target = q[:7] + dq[:7] + + duration = max(self.MIN_STEP_DURATION_SEC, abs(dz) / max(cfg.descent_speed_mps, 1e-6)) + traj = JointTrajectory() + traj.joint_names = list(self._joint_names) + pt = JointTrajectoryPoint() + pt.positions = [float(v) for v in target] + pt.velocities = [0.0] * 7 + pt.time_from_start.sec = int(duration) + pt.time_from_start.nanosec = int((duration - int(duration)) * 1e9) + traj.points = [pt] + + return self._send_jtc(traj, timeout_sec=duration + 1.0) + + def _send_jtc(self, traj: JointTrajectory, timeout_sec: float) -> bool: + """发一条轨迹并等同到完成。在 action 执行线程里跑，靠 executor 收应答。""" + goal = FollowJointTrajectory.Goal() + goal.trajectory = traj + handle = self._await(self._jtc.send_goal_async(goal), timeout_sec=4.0) + if handle is None or not bool(getattr(handle, "accepted", False)): + self._log.warn("JTC 未接受轨迹") + return False + future = handle.get_result_async() + if not self._await_done(future, timeout_sec=timeout_sec): + self._log.warn("JTC 结果超时") + return False + try: + inner = getattr(future.result(), "result", None) + code = getattr(inner, "error_code", 0) if inner is not None else 0 + if code not in (0, None): + self._log.warn( + f"JTC 执行失败 error_code={code} " + f"{getattr(inner, 'error_string', '')}" + ) + return False + except Exception: + pass + return True + + @staticmethod + def _await(future, timeout_sec: float): + """等一个 action future 并把结果取出来（不 spin，靠其它 executor 线程）。""" + if not CompliantDescentRunner._await_done(future, timeout_sec): + return None + try: + return future.result() + except Exception: + return None + + @staticmethod + def _await_done(future, timeout_sec: float) -> bool: + """等 future 完成。 + + 不能用 ``rclpy.spin_until_future_complete`` —— 那需要 executor，而在 + action 执行线程里再 spin 会自锁。节点的 MultiThreadedExecutor 会在其它 + 线程处理这套 action client 的回调，这里只需轮询完成标志。 + """ + deadline = time.monotonic() + max(0.05, timeout_sec) + while not future.done() and time.monotonic() < deadline: + time.sleep(0.01) + return future.done() + + def _publish_feedback(self, goal_handle, p: DescentProgress) -> None: + fb = CompliantDescent.Feedback() + fb.progress = p.fraction + fb.descended_m = p.descended_m + fb.tangential_delta = p.tangential + fb.normal_delta = p.normal + fb.status = p.status + try: + goal_handle.publish_feedback(fb) + except Exception: + pass + + +def _quat_xyzw_to_R(q: List[float]) -> np.ndarray: + x, y, z, w = [float(v) for v in q] + n = math.sqrt(x * x + y * y + z * z + w * w) + 1e-12 + x, y, z, w = x / n, y / n, z / n, w / n + return np.array( + [ + [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)], + [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)], + [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)], + ], + dtype=float, + ) + + +def _R_to_quat_xyzw(R: np.ndarray) -> List[float]: + """3x3 旋转矩阵转四元数 (x, y, z, w)。用 Shepperd 法，数值稳定。""" + m = np.asarray(R, dtype=float) + trace = m[0, 0] + m[1, 1] + m[2, 2] + if trace > 0: + s = math.sqrt(trace + 1.0) * 2.0 + w = 0.25 * s + x = (m[2, 1] - m[1, 2]) / s + y = (m[0, 2] - m[2, 0]) / s + z = (m[1, 0] - m[0, 1]) / s + elif m[0, 0] > m[1, 1] and m[0, 0] > m[2, 2]: + s = math.sqrt(1.0 + m[0, 0] - m[1, 1] - m[2, 2]) * 2.0 + w = (m[2, 1] - m[1, 2]) / s + x = 0.25 * s + y = (m[0, 1] + m[1, 0]) / s + z = (m[0, 2] + m[2, 0]) / s + elif m[1, 1] > m[2, 2]: + s = math.sqrt(1.0 + m[1, 1] - m[0, 0] - m[2, 2]) * 2.0 + w = (m[0, 2] - m[2, 0]) / s + x = (m[0, 1] + m[1, 0]) / s + y = 0.25 * s + z = (m[1, 2] + m[2, 1]) / s + else: + s = math.sqrt(1.0 + m[2, 2] - m[0, 0] - m[1, 1]) * 2.0 + w = (m[1, 0] - m[0, 1]) / s + x = (m[0, 2] + m[2, 0]) / s + y = (m[1, 2] + m[2, 1]) / s + z = 0.25 * s + return [x, y, z, w] diff --git a/src/arm_driver_flexiv/arm_driver_flexiv/flexiv_arm_driver.py b/src/arm_driver_flexiv/arm_driver_flexiv/flexiv_arm_driver.py new file mode 100644 index 0000000..ca9c518 --- /dev/null +++ b/src/arm_driver_flexiv/arm_driver_flexiv/flexiv_arm_driver.py @@ -0,0 +1,380 @@ +"""非夕 Flexiv 机械臂驱动适配器节点。 + +它提供什么 +---------- +只提供 `devices/robots/flexiv/manifest.yaml` 声明、而现有节点没有的接口： + +======================== ============================== ================================ +接口 类型 实现方式 +======================== ============================== ================================ +``/home`` ``std_srvs/Trigger`` 包装 ``/set_named_pose`` +``/enable`` ``std_srvs/SetBool`` 经 RDK 桥使能 / 下使能 +``/clear_fault`` ``std_srvs/Trigger`` 经 RDK 桥清故障并重新使能 +``/switch_tool`` ``arm_msgs/SwitchTool`` 经 RDK 桥切换工具坐标系 +``/soft_drag`` ``std_srvs/SetBool`` 经 RDK 桥开关软拖拽 +``/compliant_descent`` ``arm_msgs/CompliantDescent`` 应用层力闭环柔顺下压 +======================== ============================== ================================ + +它不重复实现什么（这些已由现有节点提供，见本包 README） +-------------------------------------------------------- +``get_arm_state`` → ``arm_control/arm_status_node`` +``follow_joint_trajectory`` → ``arm_control/real_trajectory_adapter`` +``set_named_pose`` → ``arm_control/named_pose_server`` +``servo_joint`` / ``servo_cartesian`` → ``arm_application/servo_node`` + +重复实现它们只会制造两套需要同步维护的行为，这正是本次重构要消除的问题。 + +RDK 访问策略 +------------ +本节点**不 import flexivrdk**（其 Python 绑定与 ROS 环境的 3.11 ABI 不兼容）。 +所有需要独占 RDK 的操作都委托给 ``scripts/rdk_drag_bridge.py`` 这个跑在 +``rdk_py310`` 下的长驻桥进程，并由 :class:`arm_driver_flexiv.rdk_client.RdkClient` +管理。 + +桥是**懒启动**的：节点启动时不连机器人。只有真的收到 ``/enable``、 +``/switch_tool`` 这类请求时才拉起桥。这样在没有机械臂的机器上（写文档、跑单测、 +CI）节点也能正常起来、接口也能被 ``ros2 service list`` 看到。 +""" + +from __future__ import annotations + +import threading +from typing import Any, Dict, Optional + +import rclpy +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node + +from arm_msgs.action import CompliantDescent +from arm_msgs.srv import SetNamedPose, SwitchTool +from std_srvs.srv import SetBool, Trigger + +from .compliant_descent import CompliantDescentRunner +from .rdk_client import RdkBridgeError, RdkClient +from .site_config import SiteConfig + + +class FlexivArmDriver(Node): + """非夕机械臂驱动适配器。""" + + def __init__(self) -> None: + super().__init__("flexiv_arm_driver") + + # --- 站点配置（作为参数默认值的来源）--- + self._site = SiteConfig() + + self.declare_parameter("sn", self._site.robot_sn()) + self.declare_parameter("iface", self._site.net_iface()) + self.declare_parameter("tool_name", self._site.tool_name()) + self.declare_parameter("home_pose", self._site.home_pose()) + self.declare_parameter("home_speed", self._site.home_speed()) + self.declare_parameter("set_named_pose_service", "/set_named_pose") + self.declare_parameter("rdk_python", "") + self.declare_parameter("rdk_script", "") + self.declare_parameter("rdk_log", "/tmp/flexiv_arm_driver_rdk.log") + self.declare_parameter("rdk_reply_timeout", 10.0) + self.declare_parameter("rdk_connect_timeout", 25.0) + # 装配流程开始时就该把工具切好；默认 true 让 home 顺带保证工具正确。 + self.declare_parameter("switch_tool_on_home", True) + + self._sn = str(self.get_parameter("sn").value) + self._iface = str(self.get_parameter("iface").value) + self._tool_name = str(self.get_parameter("tool_name").value) + + # ReentrantCallbackGroup + MultiThreadedExecutor：本节点会在服务回调里 + # 同步等待另一个服务（/set_named_pose）的应答，非重入的互斥组会自锁。 + self._cb = ReentrantCallbackGroup() + + self._rdk: Optional[RdkClient] = None + self._rdk_lock = threading.Lock() + + # --- 服务：RDK 相关 --- + self.create_service(SetBool, "enable", self._on_enable, callback_group=self._cb) + self.create_service(Trigger, "clear_fault", self._on_clear_fault, + callback_group=self._cb) + self.create_service(SwitchTool, "switch_tool", self._on_switch_tool, + callback_group=self._cb) + self.create_service(SetBool, "soft_drag", self._on_soft_drag, + callback_group=self._cb) + + # --- 服务：home（包装现有 named_pose_server）--- + self.create_service(Trigger, "home", self._on_home, callback_group=self._cb) + self._named_pose_client = self.create_client( + SetNamedPose, + str(self.get_parameter("set_named_pose_service").value), + callback_group=self._cb, + ) + + # --- action：柔顺下压 --- + # 把已解析的 SN 传进去，否则 runner 的 robot_sn 默认是空串， + # TF frame 会退化成 _base_link / _flange。 + self._descent = CompliantDescentRunner( + self, callback_group=self._cb, default_sn=self._sn + ) + + self.get_logger().info( + f"flexiv_arm_driver 就绪: sn={self._sn} iface={self._iface or '-'} " + f"tool={self._tool_name or '(未设置)'}" + ) + if not self._sn: + self.get_logger().warn( + "未取到机械臂 SN（site.yaml 的 robot.sn 为空）。" + "/enable、/switch_tool 等 RDK 相关服务会失败。" + ) + if not self._tool_name: + self.get_logger().warn( + "site.yaml 的 robot.tool_name 为空，回位前不会切换工具坐标系。" + "通止规装配要求工具系全程垂直向下锁定，请填上 Studio 里已保存的工具名" + "（bash scripts/rdk_switch_tool.sh --list 可列出可选项）。" + ) + + # ------------------------------------------------------------------ + # RDK 桥管理 + # ------------------------------------------------------------------ + + def _ensure_rdk(self) -> RdkClient: + """返回已连接的 RDK 客户端，必要时懒启动桥。 + + Raises + ------ + RdkBridgeError + 无法启动桥或无法连接机器人。消息里带修复建议。 + """ + with self._rdk_lock: + if self._rdk is not None and self._rdk.is_running: + return self._rdk + if not self._sn: + raise RdkBridgeError( + "没有机械臂 SN，无法连接 RDK", + "在 config/site.yaml 里设置 robot.sn（如 Rizon4-063989），" + "重启本节点", + ) + client = RdkClient( + sn=self._sn, + iface=self._iface, + python=str(self.get_parameter("rdk_python").value) or None, + script=( + _nonempty(self.get_parameter("rdk_script").value) or None + ), + log_path=str(self.get_parameter("rdk_log").value), + reply_timeout=float(self.get_parameter("rdk_reply_timeout").value), + connect_timeout=float(self.get_parameter("rdk_connect_timeout").value), + ) + client.start(connect=True) + self._rdk = client + self.get_logger().info(f"RDK 桥已连接（{self._sn}）") + return client + + def _rdk_error_reply(self, what: str, exc: Exception) -> str: + """把 RDK 异常转成给服务调用方看的一行说明，并记日志。""" + message = f"{what}失败: {exc}" + self.get_logger().error(message) + return message + + # ------------------------------------------------------------------ + # 服务回调 + # ------------------------------------------------------------------ + + def _on_enable(self, request: SetBool.Request, response: SetBool.Response): + if not request.data: + response.success = False + response.message = ( + "本驱动不支持下使能：非夕 RDK 没有安全的『伺服断开』状态，" + "下使能会让臂自由下落。要停止运动请用 /set_named_pose 回安全位，" + "或用示教器急停。" + ) + self.get_logger().warn(response.message) + return response + try: + reply = self._ensure_rdk().enable() + except Exception as exc: # noqa: BLE001 + response.success = False + response.message = self._rdk_error_reply("使能", exc) + return response + + response.success = True + response.message = ( + f"已使能（operational={reply.get('operational')}, " + f"mode={reply.get('mode', '?')}）" + ) + self.get_logger().info(response.message) + return response + + def _on_clear_fault(self, _request: Trigger.Request, response: Trigger.Response): + try: + reply = self._ensure_rdk().clear_fault() + except Exception as exc: # noqa: BLE001 + response.success = False + response.message = self._rdk_error_reply("清故障", exc) + return response + + response.success = True + response.message = ( + f"故障已清除（fault={reply.get('fault')}, " + f"operational={reply.get('operational')}）" + ) + self.get_logger().info(response.message) + return response + + def _on_switch_tool(self, request: SwitchTool.Request, response: SwitchTool.Response): + name = (request.tool_name or "").strip() + sn = (request.sn or "").strip() + if sn and sn != self._sn: + response.success = False + response.message = ( + f"本节点只服务 sn={self._sn}，请求里给的是 {sn}。" + "多臂场景请为每台臂各起一个驱动节点。" + ) + return response + + try: + reply = self._ensure_rdk().switch_tool(name) + except Exception as exc: # noqa: BLE001 + response.success = False + response.message = self._rdk_error_reply("切换工具", exc) + return response + + response.current_tool = str(reply.get("current", "")) + response.tool_mass_kg = float(reply.get("mass_kg", 0.0) or 0.0) + com = list(reply.get("com_m", []) or []) + response.tool_com_m = [float(v) for v in (com + [0.0, 0.0, 0.0])[:3]] + response.success = True + + if not name: + response.message = f"当前工具: {response.current_tool or '(空)'}" + elif reply.get("already"): + response.message = f"已经在工具 [{response.current_tool}] 上，无需切换" + else: + response.message = f"已切换到工具 [{response.current_tool}]" + + self.get_logger().info(response.message) + return response + + def _on_soft_drag(self, request: SetBool.Request, response: SetBool.Response): + try: + client = self._ensure_rdk() + if request.data: + reply = client.drag_on() + response.message = ( + f"软拖拽已开启（mode={reply.get('mode', 'soft')}）。" + "现在可以手动推动手臂。" + ) + else: + client.drag_off() + response.message = "软拖拽已关闭，已切回 IDLE。" + except Exception as exc: # noqa: BLE001 + response.success = False + response.message = self._rdk_error_reply("软拖拽", exc) + return response + + response.success = True + self.get_logger().info(response.message) + return response + + def _on_home(self, _request: Trigger.Request, response: Trigger.Response): + """回初始位。 + + 顺序很重要：先确保工具坐标系正确，再回位。否则回位后的姿态是按错误的 + TCP 算的，后续装配会从错的位置开始。 + """ + tool_name = (self._tool_name or "").strip() + if tool_name and bool(self.get_parameter("switch_tool_on_home").value): + try: + reply = self._ensure_rdk().switch_tool(tool_name) + self.get_logger().info( + f"回位前工具确认: {reply.get('current') or tool_name}" + ) + except Exception as exc: # noqa: BLE001 + # 工具切不过去就让调用方知道 —— 静默继续会让装配在错误 TCP 下跑。 + response.success = False + response.message = ( + f"回位前的工具切换失败，已中止回位以免在错误工具系下运动: {exc}" + ) + self.get_logger().error(response.message) + return response + + if not self._named_pose_client.wait_for_service(timeout_sec=5.0): + response.success = False + response.message = ( + f"等不到服务 {self._named_pose_client.srv_name}。" + "先启动机械臂控制：bash scripts/run_real.sh（或 mock 控制栈）" + ) + self.get_logger().error(response.message) + return response + + pose = str(self.get_parameter("home_pose").value) + speed = float(self.get_parameter("home_speed").value) + req = SetNamedPose.Request() + req.pose_name = pose + req.velocity_scale = speed + req.acceleration_scale = 0.0 + + future = self._named_pose_client.call_async(req) + # 本节点的回调跑在 ReentrantCallbackGroup 上，所以这里的等待不会自锁。 + if not _wait_future(future, timeout_sec=60.0): + response.success = False + response.message = f"回初始位超时（pose={pose}, speed={speed}）" + self.get_logger().error(response.message) + return response + + try: + result = future.result() + except Exception as exc: # noqa: BLE001 + response.success = False + response.message = f"回初始位调用异常: {exc}" + self.get_logger().error(response.message) + return response + + response.success = bool(result.success) + response.message = ( + f"已回初始位 {pose}: {result.message}" + if result.success + else f"回初始位失败: {result.message}" + ) + self.get_logger().info(response.message) + return response + + +def _nonempty(value: Any) -> str: + """把 ROS 参数值转成去掉首尾空白的字符串（空串 -> ""）。""" + return str(value).strip() if value is not None else "" + + +def _wait_future(future: Any, timeout_sec: float) -> bool: + """在回调线程里等一个 future 完成。 + + 不能用 ``rclpy.spin_until_future_complete`` —— 那需要拿到 executor，而在服务 + 回调里再 spin 会自锁。这里用事件等待，配合 ReentrantCallbackGroup 让应答能在 + 另一个线程被处理。 + """ + import time + + deadline = time.monotonic() + timeout_sec + while not future.done() and time.monotonic() < deadline: + time.sleep(0.01) + return future.done() + + +def main(argv: Optional[list[str]] = None) -> int: + rclpy.init(args=argv) + node = FlexivArmDriver() + executor = MultiThreadedExecutor(num_threads=4) + executor.add_node(node) + try: + executor.spin() + except KeyboardInterrupt: + pass + finally: + with node._rdk_lock: # noqa: SLF001 - 关闭时清理子进程 + if node._rdk is not None: + node._rdk.stop() + node._rdk = None + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/src/arm_driver_flexiv/arm_driver_flexiv/rdk_client.py b/src/arm_driver_flexiv/arm_driver_flexiv/rdk_client.py new file mode 100644 index 0000000..1837760 --- /dev/null +++ b/src/arm_driver_flexiv/arm_driver_flexiv/rdk_client.py @@ -0,0 +1,385 @@ +"""RDK 桥客户端：管理 `rdk_drag_bridge.py` 长驻子进程。 + +为什么需要这一层 +---------------- +`flexivrdk` 与 ROS 环境的 Python 3.11 ABI 不兼容，且 RDK 对同一台机器人是独占 +访问。所以驱动节点不直接连 RDK，而是拉起一个跑在 `rdk_py310` 下的长驻桥进程， +用 JSON 行协议与它通信。本模块把这套子进程 + 协议交互封装成一个对象。 + +设计取舍 +-------- +* **同步请求**：每条命令"发一行、等一行"。RDK 操作本来就是串行的，异步化只会 + 引入竞态。用一个锁保证多线程调用安全。 +* **超时必抛**：桥进程卡住时不能让调用方无限等。超时抛 + :class:`RdkBridgeError`，消息里带上原始命令，便于定位。 +* **可以只启动不连接**：没有真机时（CI / 纯接口测试）用 + ``start(connect=False)``，接口照样能起来，只是调用 RDK 的命令会失败。 +""" + +from __future__ import annotations + +import json +import os +import select +import subprocess +import threading +import time +from pathlib import Path +from typing import Any, Dict, List, Optional + +#: 桥进程必须用这个解释器 —— RDK 绑定只在这里有。 +DEFAULT_RDK_PYTHON = "/home/cc/rdk_py310/bin/python" + +#: 桥脚本位置（相对工作区根）。 +BRIDGE_SCRIPT_REL = "scripts/rdk_drag_bridge.py" + +#: 未指定时使用的默认日志文件。 +DEFAULT_LOG = "/tmp/flexiv_arm_driver_rdk.log" + + +class RdkBridgeError(RuntimeError): + """桥进程不可用、命令失败或超时。 + + Attributes + ---------- + hint: + 给操作员的修复建议（文件路径、检查项）。可能为空。 + """ + + def __init__(self, message: str, hint: str = "") -> None: + super().__init__(message) + self.hint = hint + + def __str__(self) -> str: + if self.hint: + return f"{super().__str__()}\n 怎么修: {self.hint}" + return super().__str__() + + +def default_rdk_python() -> str: + """桥进程解释器路径，可用 ``RDK_PYTHON`` 覆盖。""" + return os.environ.get("RDK_PYTHON", "").strip() or DEFAULT_RDK_PYTHON + + +def default_bridge_script() -> Path: + """桥脚本路径。可用 ``SITE_ROS2_ROOT`` / ``ROS2_WS`` 指向别的工作区。""" + root = ( + os.environ.get("SITE_ROS2_ROOT", "").strip() + or os.environ.get("ROS2_WS", "").strip() + ) + base = Path(root).expanduser() if root else Path.home() / "ros2" + return base / BRIDGE_SCRIPT_REL + + +class RdkClient: + """与 RDK 桥子进程的一条长连接。 + + 典型用法:: + + client = RdkClient(sn="Rizon4-063989", iface="enp4s0") + client.start() # 拉起桥并 connect + client.switch_tool("tool1") # 切到 tool1 + client.stop() + """ + + def __init__( + self, + sn: str, + iface: str = "", + *, + python: Optional[str] = None, + script: Optional[Path] = None, + log_path: Optional[str] = None, + reply_timeout: float = 10.0, + connect_timeout: float = 25.0, + ) -> None: + self.sn = sn + self.iface = iface + self.python = python or default_rdk_python() + self.script = Path(script) if script else default_bridge_script() + self.log_path = log_path or DEFAULT_LOG + self.reply_timeout = reply_timeout + self.connect_timeout = connect_timeout + + self._proc: Optional[subprocess.Popen] = None + self._lock = threading.Lock() + self._log_file = None + + # -- 生命周期 --------------------------------------------------------- + + @property + def is_running(self) -> bool: + return self._proc is not None and self._proc.poll() is None + + def start(self, *, connect: bool = True) -> None: + """拉起桥进程。``connect=True`` 时同时连接机器人。 + + Raises + ------ + RdkBridgeError + 解释器或脚本不存在，或 ``connect=True`` 而连接失败。 + """ + if self.is_running: + return + if not Path(self.python).is_file(): + raise RdkBridgeError( + f"找不到 RDK 解释器: {self.python}", + f"设置环境变量 RDK_PYTHON 指向装有 flexivrdk 的解释器；" + f"默认是 {DEFAULT_RDK_PYTHON}", + ) + if not self.script.is_file(): + raise RdkBridgeError( + f"找不到 RDK 桥脚本: {self.script}", + "确认工作区完整（scripts/rdk_drag_bridge.py 存在），" + "或用 SITE_ROS2_ROOT 指向正确的工作区", + ) + + Path(self.log_path).parent.mkdir(parents=True, exist_ok=True) + self._log_file = open(self.log_path, "a", encoding="utf-8", buffering=1) + self._log_file.write( + f"\n=== {time.strftime('%Y-%m-%d %H:%M:%S')} start " + f"sn={self.sn} iface={self.iface or '-'} ===\n" + ) + + self._proc = subprocess.Popen( + [self.python, str(self.script)], + stdin=subprocess.PIPE, + stdout=subprocess.PIPE, + stderr=self._log_file, + # 二进制 + 无缓冲：配合 _read_reply 里的 os.read 自己切行。 + # 用 text=True 的话 readline() 会为了等换行符而阻塞， + # select 的"可读"并不等于"有完整一行"，超时就会失效。 + bufsize=0, + ) + + # 先 ping 确认桥活着（能捕获"解释器起不来/语法错"这类问题）。 + self.request("ping", timeout=10.0) + + if connect: + payload: Dict[str, Any] = {"sn": self.sn} + if self.iface: + payload["iface"] = self.iface + reply = self.request("connect", timeout=self.connect_timeout, **payload) + if not reply.get("ok"): + error = reply.get("error", "未知原因") + self.stop() + raise RdkBridgeError( + f"连接机械臂 {self.sn} 失败: {error}", + f"检查网线 / `network.prefer_iface`、示教器是否 AUTO(Remote)、" + f"是否已松开急停。桥日志: {self.log_path}", + ) + + def stop(self, timeout: float = 5.0) -> None: + """请求桥退出并回收进程。幂等。""" + proc = self._proc + if proc is None: + return + try: + if proc.poll() is None: + self._write({"cmd": "quit"}) + try: + self._read_reply(timeout) + except Exception: + pass + except Exception: + pass + + try: + proc.wait(timeout=timeout) + except subprocess.TimeoutExpired: + proc.kill() + try: + proc.wait(timeout=2.0) + except subprocess.TimeoutExpired: + pass + + for stream in (proc.stdin, proc.stdout): + try: + if stream is not None: + stream.close() + except Exception: + pass + if self._log_file is not None: + try: + self._log_file.close() + except Exception: + pass + self._log_file = None + self._proc = None + + def __enter__(self) -> "RdkClient": + self.start() + return self + + def __exit__(self, *_exc: Any) -> None: + self.stop() + + # -- 协议 ------------------------------------------------------------- + + def request( + self, cmd: str, timeout: Optional[float] = None, **kwargs: Any + ) -> Dict[str, Any]: + """发一条命令并等应答。 + + Raises + ------ + RdkBridgeError + 桥未运行、进程已退出、应答超时，或应答不是合法 JSON。 + """ + with self._lock: + if not self.is_running: + raise RdkBridgeError( + "RDK 桥未运行", + f"先调用 RdkClient.start()；若刚崩溃，看日志 {self.log_path}", + ) + payload = {"cmd": cmd, **kwargs} + self._write(payload) + return self._read_reply( + timeout if timeout is not None else self.reply_timeout + ) + + def request_ok( + self, cmd: str, timeout: Optional[float] = None, **kwargs: Any + ) -> Dict[str, Any]: + """同 :meth:`request`，但 ``ok`` 为假时直接抛错。""" + reply = self.request(cmd, timeout=timeout, **kwargs) + if not reply.get("ok"): + raise RdkBridgeError( + f"命令 {cmd} 失败: {reply.get('error', '未提供原因')}", + f"桥日志: {self.log_path}", + ) + return reply + + # -- 便捷包装 --------------------------------------------------------- + + def enable(self) -> Dict[str, Any]: + """使能 + 清故障（幂等）。""" + return self.request_ok("enable") + + def clear_fault(self) -> Dict[str, Any]: + """清故障并使能。""" + return self.request_ok("clear_fault") + + def switch_tool(self, name: str = "", timeout: float = 20.0) -> Dict[str, Any]: + """切换工具。``name`` 为空时只查询当前状态。""" + return self.request_ok("switch_tool", timeout=timeout, name=name) + + def status(self) -> Dict[str, Any]: + """查询 operational / fault / mode / 当前工具。""" + return self.request_ok("status") + + def drag_on(self, mode: str = "soft") -> Dict[str, Any]: + """开启软拖拽（手眼走点用）。""" + return self.request_ok("drag_on", timeout=15.0, mode=mode) + + def drag_off(self) -> Dict[str, Any]: + """关闭软拖拽并切回 IDLE。""" + return self.request_ok("drag_off", timeout=15.0) + + def joints(self) -> List[float]: + """读关节角（拖拽期间也能读，走桥内缓存）。""" + reply = self.request_ok("joints") + return [float(v) for v in (reply.get("q") or [])] + + # -- 内部 ------------------------------------------------------------- + + def _write(self, payload: Dict[str, Any]) -> None: + proc = self._proc + if proc is None or proc.stdin is None: + raise RdkBridgeError("RDK 桥的 stdin 不可用") + line = json.dumps(payload, ensure_ascii=False) + try: + proc.stdin.write((line + "\n").encode("utf-8")) + proc.stdin.flush() + except (BrokenPipeError, OSError) as exc: + raise RdkBridgeError( + f"写入 RDK 桥失败（进程可能已退出）: {exc}", + f"查看桥日志 {self.log_path}", + ) from exc + + def _read_reply(self, timeout: float) -> Dict[str, Any]: + """带超时读一行 JSON。 + + 为什么不用 ``readline()`` + ------------------------- + ``select`` 说"可读"只代表**有字节**，不代表有**完整一行**。桥上如果 + 只写了半行（原生库中途崩、或者 JSON 被截断），``readline()`` 会一直 + 阻塞等那个永远不来的换行符 —— 超时机制完全失效，整个节点挂死。 + 所以这里改用 ``os.read`` 按块读，行切分自己做：读多少算多少， + 没有完整行就回到 ``select`` 继续等，直到超时。 + + 非 JSON 行（RDK 原生日志混进 stdout）直接跳过，不判协议损坏。 + """ + proc = self._proc + if proc is None or proc.stdout is None: + raise RdkBridgeError("RDK 桥的 stdout 不可用") + + deadline = time.monotonic() + max(0.1, timeout) + buffered = b"" + while True: + # 缓冲里可能已经有完整行（一次 read 常常带回多行），先消化掉。 + while b"\n" in buffered: + raw, buffered = buffered.split(b"\n", 1) + reply = self._parse_line(raw) + if reply is not None: + return reply + + remaining = deadline - time.monotonic() + if remaining <= 0: + raise RdkBridgeError( + f"等待 RDK 桥应答超时（{timeout:.1f}s）", + f"机器人可能未响应；查看桥日志 {self.log_path}", + ) + ready, _, _ = select.select([proc.stdout], [], [], min(remaining, 0.5)) + if not ready: + if proc.poll() is not None: + raise RdkBridgeError( + f"RDK 桥进程已退出（returncode={proc.returncode}）", + f"查看桥日志 {self.log_path}", + ) + continue + + try: + chunk = os.read(proc.stdout.fileno(), 65536) + except (BlockingIOError, InterruptedError): + continue + except OSError as exc: + raise RdkBridgeError( + f"读取 RDK 桥失败: {exc}", + f"查看桥日志 {self.log_path}", + ) from exc + if not chunk: + # EOF：进程走了，或者 stdout 被关了。 + if proc.poll() is not None: + raise RdkBridgeError( + f"RDK 桥进程已退出（returncode={proc.returncode}）", + f"查看桥日志 {self.log_path}", + ) + continue + buffered += chunk + + def _parse_line(self, raw: bytes) -> Optional[Dict[str, Any]]: + """把一行的原始字节变成应答 dict；该行不是应答时返回 ``None``。 + + 返回 ``None`` 的两种情况：空行、以及不带 ``{`` 的原生日志行。 + 后者要跳过而不是报错 —— 桥脚本已经把 ``sys.stdout`` 换成 stderr 来 + 隔离 RDK 日志，但原生库可能直接写 fd 1 绕过 Python 层。 + """ + text = raw.decode("utf-8", errors="replace").strip() + if not text: + return None + if "{" not in text: + return None + try: + reply = json.loads(text) + except json.JSONDecodeError as exc: + raise RdkBridgeError( + f"RDK 桥返回了非 JSON 内容: {text[:200]!r}", + f"桥日志 {self.log_path}（RDK 自身的日志混进 stdout 通常是原因）", + ) from exc + if not isinstance(reply, dict): + raise RdkBridgeError( + f"RDK 桥的应答不是 JSON 对象: {reply!r}", + f"查看桥日志 {self.log_path}", + ) + return reply diff --git a/src/arm_driver_flexiv/arm_driver_flexiv/site_config.py b/src/arm_driver_flexiv/arm_driver_flexiv/site_config.py new file mode 100644 index 0000000..998b280 --- /dev/null +++ b/src/arm_driver_flexiv/arm_driver_flexiv/site_config.py @@ -0,0 +1,97 @@ +"""从 ``config/site.yaml`` 读取驱动需要的默认值。 + +为什么不让 launch 注入全部参数 +------------------------------ +现有节点的做法是 launch 文件先用 ``export_site_env.py`` 读 site.yaml，再把值塞 +成 ROS 参数。那条路径的问题是有两套加载逻辑（``site_io.py`` 和 +``export_site_env.py``），且 ``ros2 run`` 直接起节点时参数全是空的。 + +这里改为：节点自己用 ``arm_interfaces`` 的加载器读 site.yaml 作为**参数默认 +值**，launch 仍然可以覆盖。于是 +``ros2 run arm_driver_flexiv flexiv_arm_driver`` 直接起也能工作， +既没有重复的加载器，也保留了 ROS 的参数覆盖惯例。 + +多臂支持 +-------- +``robot.sn`` 选定当前服务的臂，臂专属的值（IP / 网卡）去 ``robot.robots[]`` +里按 SN 找。找不到时回退到全局段 —— 这与现有节点的行为一致。 +""" + +from __future__ import annotations + +from typing import Any, Dict, Optional + +from arm_interfaces import load_site + + +class SiteConfig: + """site.yaml 的一个只读视图，只暴露驱动需要的键。""" + + def __init__(self, data: Optional[Dict[str, Any]] = None) -> None: + if data is None: + try: + data = load_site() + except Exception: + # 契约层读不了配置不该让驱动起不来 —— 参数会有默认值， + # 真正需要配置的操作会在调用时报出具体缺什么。 + data = {} + self._data: Dict[str, Any] = data if isinstance(data, dict) else {} + + # -- 机械臂 ----------------------------------------------------------- + + def robot_sn(self) -> str: + """当前服务的机械臂 SN（``robot.sn``）。""" + return str(self._robot().get("sn", "") or "").strip() + + def tool_name(self) -> str: + """回位前要切换到的工具名（``robot.tool_name``）。空=不切换。""" + return str(self._robot().get("tool_name", "") or "").strip() + + def robot_entry(self, sn: str = "") -> Dict[str, Any]: + """取 ``robot.robots[]`` 中匹配 SN 的条目；找不到返回空 dict。""" + target = (sn or self.robot_sn()).strip() + for entry in self._robot().get("robots") or []: + if isinstance(entry, dict) and str(entry.get("sn", "")).strip() == target: + return entry + return {} + + def net_iface(self) -> str: + """本臂的网卡名：优先 ``robot.robots[].iface``，回退 ``network.prefer_iface``。""" + entry = self.robot_entry() + iface = str(entry.get("iface", "") or "").strip() + if iface: + return iface + return str((self._data.get("network") or {}).get("prefer_iface", "") or "").strip() + + def robot_ip(self) -> str: + """本臂的控制器 IP（``robot.robots[].ip``）。""" + return str(self.robot_entry().get("ip", "") or "").strip() + + # -- 回位 ------------------------------------------------------------- + + def home_pose(self) -> str: + """回位用的命名位姿（``go_home.pose``，回退 ``servo.home_pose_name``）。""" + pose = str((self._data.get("go_home") or {}).get("pose", "") or "").strip() + if pose: + return pose + return str(self._servo_base().get("home_pose_name", "") or "").strip() or "home" + + def home_speed(self) -> float: + """回位速度比例（``go_home.speed``，回退 ``servo.home_pose_speed``）。""" + value = (self._data.get("go_home") or {}).get("speed") + if value is None: + value = self._servo_base().get("home_pose_speed") + try: + return float(value) + except (TypeError, ValueError): + return 0.25 + + # -- 内部 ------------------------------------------------------------- + + def _robot(self) -> Dict[str, Any]: + robot = self._data.get("robot") + return robot if isinstance(robot, dict) else {} + + def _servo_base(self) -> Dict[str, Any]: + servo = self._data.get("servo") + return servo if isinstance(servo, dict) else {} diff --git a/src/arm_driver_flexiv/package.xml b/src/arm_driver_flexiv/package.xml new file mode 100644 index 0000000..5369eb7 --- /dev/null +++ b/src/arm_driver_flexiv/package.xml @@ -0,0 +1,34 @@ + + + + arm_driver_flexiv + 0.1.0 + + 非夕 Flexiv 机械臂驱动适配器。补齐 manifest 声明但现有节点没提供的能力： + home / enable / clear_fault / switch_tool / soft_drag / compliant_descent。 + 已经存在的能力（get_arm_state / follow_joint_trajectory / set_named_pose / + 视觉伺服）由 arm_control 与 arm_application 的现有节点提供，本包不重复实现。 + 所有需要独占 RDK 的操作都经由 rdk_py310 下的长驻桥进程，本包自身不 import + flexivrdk（ABI 不兼容）。 + + arm_dev + Apache-2.0 + + rclpy + arm_msgs + arm_interfaces + arm_kinematics + std_srvs + geometry_msgs + sensor_msgs + trajectory_msgs + control_msgs + tf2_ros + python3-numpy + + python3-pytest + + + ament_python + + diff --git a/src/arm_driver_flexiv/resource/arm_driver_flexiv b/src/arm_driver_flexiv/resource/arm_driver_flexiv new file mode 100644 index 0000000..e69de29 diff --git a/src/arm_driver_flexiv/setup.cfg b/src/arm_driver_flexiv/setup.cfg new file mode 100644 index 0000000..80b4975 --- /dev/null +++ b/src/arm_driver_flexiv/setup.cfg @@ -0,0 +1,10 @@ +[develop] +script_dir=$base/lib/arm_driver_flexiv +[install] +install_scripts=$base/lib/arm_driver_flexiv + +[tool:pytest] +# ROS 2 Humble 的两个 launch-testing pytest 插件用了旧版 hook 签名， +# 当前 pytest 会在收集阶段直接报错。本包只跑纯 Python 单测，禁掉它们。 +addopts = -p no:launch_testing -p no:launch_ros +testpaths = test diff --git a/src/arm_driver_flexiv/setup.py b/src/arm_driver_flexiv/setup.py new file mode 100644 index 0000000..e1cd928 --- /dev/null +++ b/src/arm_driver_flexiv/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'arm_driver_flexiv' + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools', 'PyYAML'], + zip_safe=True, + maintainer='arm_dev', + maintainer_email='dev@example.com', + description='非夕 Flexiv 机械臂驱动适配器', + license='Apache-2.0', + entry_points={ + 'console_scripts': [ + # 主驱动节点：home / enable / clear_fault / switch_tool / soft_drag + # 以及 compliant_descent action。 + 'flexiv_arm_driver = arm_driver_flexiv.flexiv_arm_driver:main', + ], + }, +) diff --git a/src/arm_driver_flexiv/test/test_descent_control.py b/src/arm_driver_flexiv/test/test_descent_control.py new file mode 100644 index 0000000..505bfd7 --- /dev/null +++ b/src/arm_driver_flexiv/test/test_descent_control.py @@ -0,0 +1,438 @@ +"""``DescentController`` 的纯逻辑单测。 + +下压逻辑里最容易出错、也最危险的部分全在这里：接触判据的优先级、安全上限、 +触觉断流时的行为、超时与行程上限。这些都能用注入的假回调完整覆盖， +不需要机械臂、JTC、TF。 + +假时钟很关键：超时判定如果真用 ``time.sleep`` 测，一套测试要跑几十秒； +换成手动推进的假时钟后毫秒级完成，且不依赖机器快慢。 +""" + +from __future__ import annotations + +from dataclasses import replace +from typing import List, Optional + +import numpy as np +import pytest + +from arm_driver_flexiv.compliant_descent import ( + DescentConfig, + DescentController, + DescentOutcome, + DescentProgress, + TactileSample, + _R_to_quat_xyzw, + _quat_xyzw_to_R, + select_tangential, +) + +import numpy as np + + +# --------------------------------------------------------------------------- +# 测试替身 +# --------------------------------------------------------------------------- + + +class FakeClock: + """手动推进的时钟。``sleep`` 只把时间往前拨，不真的等。""" + + def __init__(self) -> None: + self.t = 0.0 + + def now(self) -> float: + return self.t + + def sleep(self, dt: float) -> None: + self.t += max(0.0, dt) + + +class ScriptedTactile: + """按预设脚本给出触觉读数。 + + 脚本是一个 ``(切向, 法向)`` 列表，交给 ``Decider`` 决定每次返回什么。 + 用列表而不是固定值，是为了模拟"下压若干步之后才接触"。 + """ + + def __init__(self, script, *, age: float = 0.0, stale_after: Optional[int] = None): + self.script = script + self.age = age + self.stale_after = stale_after + self.calls = 0 + + def __call__(self) -> Optional[TactileSample]: + idx = self.calls + self.calls += 1 + if callable(self.script): + value = self.script(idx) + else: + value = self.script[min(idx, len(self.script) - 1)] + if value is None: + return None + tangential, normal = value + age = self.age + if self.stale_after is not None and idx >= self.stale_after: + age = 999.0 + return TactileSample(tangential=tangential, normal=normal, age_sec=age) + + +def make_controller( + cfg: DescentConfig, + tactile, + *, + step_ok=None, + clock: Optional[FakeClock] = None, + cancelled=None, + on_progress=None, + log: Optional[List[float]] = None, +) -> DescentController: + """搭一个控制器，把"下压一步"换成往 log 里记一笔。""" + clock = clock or FakeClock() + steps = log if log is not None else [] + + def step_down(dz: float) -> bool: + if step_ok is not None and not step_ok(len(steps)): + return False + steps.append(dz) + return True + + return DescentController( + cfg, + read_tactile=tactile, + step_down=step_down, + now=clock.now, + sleep=clock.sleep, + is_cancelled=(cancelled or (lambda: False)), + on_progress=on_progress, + ) + + +BASE = DescentConfig( + contact_axis=0, + contact_delta=100.0, + max_normal_delta=0.0, + descent_step_m=0.0005, + descent_speed_mps=0.005, + max_descent_m=0.05, + settle_sec=0.0, + timeout_sec=10.0, +) + + +# --------------------------------------------------------------------------- +# 接触判据 +# --------------------------------------------------------------------------- + + +class TestContactDetection: + def test_already_in_contact_reports_zero_travel(self): + """起手就已经接触：不该先压一步再判断，否则会多压 0.5mm。""" + log: List[float] = [] + ctrl = make_controller(BASE, ScriptedTactile([(150.0, 5.0)]), log=log) + result = ctrl.run() + assert result.outcome is DescentOutcome.CONTACT + assert result.descended_m == 0.0 + assert log == [] + + def test_contact_after_several_steps(self): + """前 4 次读数低于阈值，之后跳变 —— 应当正好压 4 步。""" + script = [(10.0, 1.0), (20.0, 1.0), (30.0, 1.0), (40.0, 1.0), (200.0, 1.0)] + log: List[float] = [] + ctrl = make_controller(BASE, ScriptedTactile(script), log=log) + result = ctrl.run() + assert result.outcome is DescentOutcome.CONTACT + # 循环顺序是「先读、再判、最后走步」，所以第 5 次读到时已经走了 4 步。 + assert len(log) == 4 + assert result.descended_m == pytest.approx(4 * 0.0005) + assert result.tangential_delta == 200.0 + + def test_negative_tangential_also_counts(self): + """切向力往负方向跳同样算接触 —— 判据用绝对值。""" + ctrl = make_controller(BASE, ScriptedTactile([(-150.0, 1.0)])) + assert ctrl.run().outcome is DescentOutcome.CONTACT + + def test_exactly_at_threshold_counts(self): + """刚好等于阈值算接触（>= 而不是 >），避免边界上漏检。""" + ctrl = make_controller(BASE, ScriptedTactile([(100.0, 1.0)])) + assert ctrl.run().outcome is DescentOutcome.CONTACT + + def test_axis_does_not_affect_pure_logic(self): + """纯逻辑只接收已经选好的切向值 —— contact_axis 在它这里不改变结果。 + + 通道选择本身由 ``select_tangential`` 负责（见 TestSelectTangential）。 + 这条测试把边界钉住：如果你试图在纯逻辑里再按 axis 挑一次，就会重复选择 + 并挑错通道。 + """ + for axis in (0, 1): + ctrl = make_controller( + replace(BASE, contact_axis=axis), + ScriptedTactile([(500.0, 1.0)]), + ) + assert ctrl.run().outcome is DescentOutcome.CONTACT + + def test_success_flag_matches_outcome(self): + ctrl = make_controller(BASE, ScriptedTactile([(150.0, 1.0)])) + result = ctrl.run() + assert result.success is True + assert result.status == "contacting" + + +# --------------------------------------------------------------------------- +# 安全停止条件 +# --------------------------------------------------------------------------- + + +class TestSafetyStops: + def test_normal_limit_beats_contact(self): + """法向力超限和切向接触同时满足时，必须先报安全限 —— 硬顶比接触更危险。""" + ctrl = make_controller( + replace(BASE, max_normal_delta=50.0), + ScriptedTactile([(500.0, 80.0)]), + ) + result = ctrl.run() + assert result.outcome is DescentOutcome.NORMAL_LIMIT + assert result.success is False + assert result.normal_delta == 80.0 + + def test_normal_limit_disabled_when_zero(self): + """max_normal_delta <= 0 表示不检查法向力。""" + ctrl = make_controller(replace(BASE, max_normal_delta=0.0), ScriptedTactile([(10.0, 99999.0)])) + assert ctrl.run().outcome is DescentOutcome.MAX_DESCENT + + def test_max_descent_when_never_contacting(self): + log: List[float] = [] + ctrl = make_controller(BASE, ScriptedTactile([(1.0, 1.0)]), log=log) + result = ctrl.run() + assert result.outcome is DescentOutcome.MAX_DESCENT + # 行程上限 0.05 / 步长 0.0005 = 100 步；不能超过 + assert len(log) == 100 + assert result.descended_m == pytest.approx(0.05) + assert "行程上限" in result.message + + def test_timeout_stops_before_travel_limit(self): + """超时必须能提前中断 —— 即使行程还没走完。""" + cfg = replace(BASE, timeout_sec=0.6) + log: List[float] = [] + ctrl = make_controller(cfg, ScriptedTactile([(1.0, 1.0)]), log=log) + result = ctrl.run() + assert result.outcome is DescentOutcome.TIMEOUT + assert len(log) < 100 # 没走到行程上限 + assert result.success is False + + def test_tactile_disconnect_stops_immediately(self): + """读不到触觉时必须立刻停 —— 无反馈下压是危险的。""" + log: List[float] = [] + ctrl = make_controller(BASE, ScriptedTactile([None]), log=log) + result = ctrl.run() + assert result.outcome is DescentOutcome.TACTILE_STALE + assert log == [] + assert result.success is False + + def test_tactile_disconnect_after_some_steps(self): + """前 3 次有数据，之后断流：应当正好走了 3 步就停。""" + script = lambda i: (1.0, 1.0) if i < 3 else None # noqa: E731 + log: List[float] = [] + ctrl = make_controller(BASE, ScriptedTactile(script), log=log) + assert ctrl.run().outcome is DescentOutcome.TACTILE_STALE + assert len(log) == 3 + + def test_stale_tactile_stops(self): + """数据有值但已经过期（传感器卡死）也必须停。 + + 循环顺序是「先读触觉、后走步」，所以第 4 次读（idx=3）才发现过期， + 此时已经走了 3 步。 + """ + log: List[float] = [] + ctrl = make_controller( + BASE, ScriptedTactile([(1.0, 1.0)], stale_after=3), log=log + ) + result = ctrl.run() + assert result.outcome is DescentOutcome.TACTILE_STALE + assert "过期" in result.message + assert len(log) == 3 + + def test_motion_failure_stops(self): + """某一步没走成就停下，不要假装走出了这个距离。""" + log: List[float] = [] + ctrl = make_controller( + BASE, ScriptedTactile([(1.0, 1.0)]), step_ok=lambda n: n < 2, log=log + ) + result = ctrl.run() + assert result.outcome is DescentOutcome.MOTION_FAILED + assert result.descended_m == pytest.approx(2 * 0.0005) + + def test_cancel_is_honoured(self): + """走 3 步后拉取消标志：应当在下一次循环开头就停下。 + + 注意取消检查在 ``read_tactile`` **之前**，所以读到第 4 帧、走完第 4 步 + 之后才会真正退出。 + """ + log: List[float] = [] + flag = {"v": False} + + def read(): + if len(log) >= 3: + flag["v"] = True + return TactileSample(1.0, 1.0, 0.0) + + ctrl = make_controller(BASE, read, cancelled=lambda: flag["v"], log=log) + result = ctrl.run() + assert result.outcome is DescentOutcome.CANCELLED + assert result.success is False + assert len(log) == 4 + + +# --------------------------------------------------------------------------- +# feedback +# --------------------------------------------------------------------------- + + +class TestProgress: + def test_progress_reported_and_monotonic(self): + seen: List[DescentProgress] = [] + ctrl = make_controller( + BASE, ScriptedTactile([(1.0, 1.0)]), + on_progress=seen.append, + ) + result = ctrl.run() + assert result.outcome is DescentOutcome.MAX_DESCENT + # 每走一步 1 次 + 结束 1 次 + assert len(seen) == 101 + fractions = [p.fraction for p in seen] + assert fractions == sorted(fractions) + assert fractions[-1] == pytest.approx(1.0) + assert seen[-1].status == "error" + + def test_progress_on_immediate_contact_is_zero(self): + seen: List[DescentProgress] = [] + ctrl = make_controller(BASE, ScriptedTactile([(500.0, 1.0)]), on_progress=seen.append) + ctrl.run() + assert len(seen) == 1 + assert seen[0].fraction == 0.0 + assert seen[0].status == "contacting" + + +# --------------------------------------------------------------------------- +# 切向通道选择 +# --------------------------------------------------------------------------- + + +class TestSelectTangential: + """``select_tangential`` 决定装配时"看哪一路切向力"。 + + 选错通道 = 接触永远检测不到，机械臂会一路压到行程上限 —— 这在真机上可能 + 压坏通止规或工件，所以值得单独测。 + """ + + def test_axis_zero_picks_first_channel(self): + assert select_tangential(shearx=11.0, sheary=22.0, axis=0) == 11.0 + + def test_axis_one_picks_second_channel(self): + assert select_tangential(shearx=11.0, sheary=22.0, axis=1) == 22.0 + + def test_unexpected_axis_falls_back_to_first(self): + """轴号只可能是 0/1；出现别的值（配置写错）时按 0 处理而不是抛异常。""" + assert select_tangential(shearx=11.0, sheary=22.0, axis=7) == 11.0 + assert select_tangential(shearx=11.0, sheary=22.0, axis=-1) == 11.0 + + def test_result_is_always_float(self): + """ROS 消息里是 float32，纯逻辑侧统一成 float，避免 numpy 类型混进来。""" + assert isinstance(select_tangential(1, 2, 0), float) + + def test_zero_on_chosen_channel_is_preserved(self): + """选中的那一路恰好是 0（静止）时必须返回 0 —— 这正是"未接触"的正常读数， + 不能因为 0 是假值就回退到另一路。""" + assert select_tangential(shearx=0.0, sheary=99.0, axis=0) == 0.0 + + +# --------------------------------------------------------------------------- +# 配置解析 +# --------------------------------------------------------------------------- + + +class _Goal: + """假的 action goal，字段与 CompliantDescent.Goal 对齐。""" + + def __init__(self, **kw): + self.contact_axis = kw.get("contact_axis", 0.0) + self.contact_delta = kw.get("contact_delta", 0.0) + self.max_normal_delta = kw.get("max_normal_delta", 0.0) + self.descent_step_m = kw.get("descent_step_m", 0.0) + self.descent_speed_mps = kw.get("descent_speed_mps", 0.0) + self.max_descent_m = kw.get("max_descent_m", 0.0) + self.settle_sec = kw.get("settle_sec", 0.0) + self.timeout_sec = kw.get("timeout_sec", 0.0) + + +class TestConfigFromGoal: + def test_zeros_fall_back_to_defaults(self): + cfg = DescentConfig.from_goal(_Goal(contact_delta=250.0)) + assert cfg.descent_step_m == 0.0005 + assert cfg.descent_speed_mps == 0.005 + assert cfg.max_descent_m == 0.05 + assert cfg.contact_delta == 250.0 + + def test_nan_falls_back_to_default(self): + cfg = DescentConfig.from_goal(_Goal(descent_step_m=float("nan"))) + assert cfg.descent_step_m == 0.0005 + + def test_step_is_clamped_to_sane_range(self): + """手滑把 mm 当 m 填时要被夹住，不能真的走 5mm 一步。""" + big = DescentConfig.from_goal(_Goal(descent_step_m=5.0)) + assert big.descent_step_m <= 0.01 + tiny = DescentConfig.from_goal(_Goal(descent_step_m=1e-9)) + assert tiny.descent_step_m >= 1e-5 + + def test_axis_only_accepts_0_or_1(self): + assert DescentConfig.from_goal(_Goal(contact_axis=1)).contact_axis == 1 + assert DescentConfig.from_goal(_Goal(contact_axis=7)).contact_axis == 0 + + def test_step_interval_accounts_for_settle(self): + cfg = DescentConfig( + descent_step_m=0.001, descent_speed_mps=0.01, settle_sec=0.05 + ) + assert cfg.step_interval_sec == pytest.approx(0.1 + 0.05) + + def test_step_interval_guards_zero_speed(self): + cfg = DescentConfig(descent_speed_mps=0.0, settle_sec=0.03) + assert cfg.step_interval_sec == pytest.approx(0.03) + + def test_timeout_derived_when_unspecified(self): + cfg = DescentConfig( + max_descent_m=0.05, descent_speed_mps=0.01, timeout_sec=0.0 + ) + # 名义 5s，留 3 倍余量 + assert cfg.effective_timeout_sec == pytest.approx(15.0) + + def test_timeout_has_floor(self): + cfg = DescentConfig(max_descent_m=0.001, descent_speed_mps=1.0, timeout_sec=0.0) + assert cfg.effective_timeout_sec >= 2.0 + + def test_explicit_timeout_wins(self): + cfg = DescentConfig(timeout_sec=3.5) + assert cfg.effective_timeout_sec == pytest.approx(3.5) + + +# --------------------------------------------------------------------------- +# 四元数互转（结果位姿要用） +# --------------------------------------------------------------------------- + + +class TestQuaternionRoundTrip: + def test_round_trip_arbitrary_rotation(self): + R = _quat_xyzw_to_R([0.3, -0.2, 0.5, 0.78]) + back = _quat_xyzw_to_R(_R_to_quat_xyzw(R)) + assert np.allclose(R, back, atol=1e-9) + + def test_identity(self): + q = _R_to_quat_xyzw(np.eye(3)) + assert np.allclose(q, [0.0, 0.0, 0.0, 1.0], atol=1e-9) + + def test_180_deg_cases_are_stable(self): + """180° 旋转是 Shepperd 法最容易踩坑的分支（trace <= 0），逐一验证。""" + for axis in range(3): + R = -np.eye(3) + R[axis, axis] = 1.0 # 绕该轴 180° + q = _R_to_quat_xyzw(R) + assert np.allclose(_quat_xyzw_to_R(q), R, atol=1e-9), f"axis={axis}" diff --git a/src/arm_driver_flexiv/test/test_rdk_client.py b/src/arm_driver_flexiv/test/test_rdk_client.py new file mode 100644 index 0000000..63b870f --- /dev/null +++ b/src/arm_driver_flexiv/test/test_rdk_client.py @@ -0,0 +1,341 @@ +"""``RdkClient`` 的协议层单测。 + +为什么值得测 +------------ +``RdkClient`` 是驱动与 RDK 之间唯一的通道，而它的失败模式全都很难在真机上 +复现：桥进程卡住、返回半行 JSON、被 RDK 自身的日志污染 stdout、进程意外退出。 +这些情况一旦在装配现场发生，表现是"节点卡死"或"莫名其妙的解析错误"， +排查成本极高。所以这里用一个假桥脚本把这些路径全部走一遍。 + +假桥是个真实的子进程（真的走 stdin/stdout 管道与超时逻辑），只是不连机器人。 +用 ``sys.executable`` 保证在任何环境都能跑。 +""" + +from __future__ import annotations + +import sys +from pathlib import Path + +import pytest + +from arm_driver_flexiv.rdk_client import RdkBridgeError, RdkClient + + +# --------------------------------------------------------------------------- +# 假桥 +# --------------------------------------------------------------------------- + +_HANDSHAKE = """ +def _reply(obj): + print(json.dumps(obj)) + sys.stdout.flush() +""" + +_LOOP = """ +for line in sys.stdin: + line = line.strip() + if not line: + continue + try: + msg = json.loads(line) + except Exception: + _reply({"ok": False, "error": "bad json"}) + continue + cmd = msg.get("cmd", "") + if cmd == "ping": + PING() + elif cmd == "connect": + CONNECT() + elif cmd == "quit": + _reply({"ok": True}) + break + elif cmd == "hang": + pass # 故意不回，用来测超时 + elif cmd == "half": + sys.stdout.write('{"ok": true') # 半行 JSON，永不补全 + sys.stdout.flush() + elif cmd == "noisy": + sys.stdout.write("RDK: some native log line\\n") + sys.stdout.flush() + _reply({"ok": True, "after_noise": True}) + elif cmd == "boom": + _reply({"ok": False, "error": "故意失败"}) + elif cmd == "suicide": + sys.exit(7) + else: + _reply({"ok": True, "cmd": cmd, "echo": msg.get("name", ""), + "req": {k: v for k, v in msg.items() if k != "cmd"}}) +""" + +_MODES = { + "ok": dict(ping='_reply({"ok": True, "py": "fake"})', + connect='_reply({"ok": True, "sn": msg.get("sn"), "q": [0.0] * 7})'), + "connectfail": dict(ping='_reply({"ok": True, "py": "fake"})', + connect='_reply({"ok": False, "error": "网线没插"})'), + "badjson_on_ping": dict(ping='sys.stdout.write("{not valid json\\n"); sys.stdout.flush()', + connect='_reply({"ok": True})'), + "die_on_ping": dict(ping="sys.exit(9)", connect='_reply({"ok": True})'), +} + + +def make_bridge(tmp_path: Path, mode: str = "ok") -> Path: + """把假桥脚本写进 tmp 目录，返回路径。""" + parts = _MODES[mode] + src = "import json, sys\n" + _HANDSHAKE + _LOOP + src = src.replace("PING()", parts["ping"]).replace("CONNECT()", parts["connect"]) + path = tmp_path / f"fake_bridge_{mode}.py" + path.write_text(src, encoding="utf-8") + return path + + +def client_for(tmp_path: Path, mode: str = "ok", **kw) -> RdkClient: + return RdkClient( + sn="Rizon4-TEST", + iface="", + python=sys.executable, + script=make_bridge(tmp_path, mode), + log_path=str(tmp_path / "bridge.log"), + reply_timeout=kw.pop("reply_timeout", 2.0), + connect_timeout=kw.pop("connect_timeout", 5.0), + **kw, + ) + + +# --------------------------------------------------------------------------- +# 启动与关闭 +# --------------------------------------------------------------------------- + + +class TestLifecycle: + def test_start_connects_and_reports_running(self, tmp_path): + client = client_for(tmp_path) + client.start() + try: + assert client.is_running + finally: + client.stop() + assert not client.is_running + + def test_start_without_connect_leaves_link_up(self, tmp_path): + """connect=False 用于没有真机的场合：桥要起来，但不连机器人。""" + client = client_for(tmp_path) + client.start(connect=False) + try: + assert client.is_running + assert client.request("ping")["ok"] is True + finally: + client.stop() + + def test_stop_is_idempotent(self, tmp_path): + client = client_for(tmp_path) + client.start() + client.stop() + client.stop() # 不应抛 + assert not client.is_running + + def test_stop_before_start_is_safe(self, tmp_path): + client_for(tmp_path).stop() + + def test_context_manager_closes(self, tmp_path): + with client_for(tmp_path) as client: + assert client.is_running + assert not client.is_running + + def test_missing_interpreter_reports_how_to_fix(self, tmp_path): + client = RdkClient( + sn="x", python=str(tmp_path / "nope" / "python"), script=make_bridge(tmp_path) + ) + with pytest.raises(RdkBridgeError) as exc: + client.start() + # hint 必须可执行：告诉用户设 RDK_PYTHON + assert "RDK_PYTHON" in exc.value.hint + + def test_missing_script_reports_where_to_look(self, tmp_path): + client = RdkClient( + sn="x", python=sys.executable, script=tmp_path / "nope.py" + ) + with pytest.raises(RdkBridgeError) as exc: + client.start() + assert "SITE_ROS2_ROOT" in exc.value.hint + + def test_connect_failure_includes_operator_hint(self, tmp_path): + client = client_for(tmp_path, "connectfail") + with pytest.raises(RdkBridgeError) as exc: + client.start() + assert "网线没插" in str(exc.value) + assert "示教器" in exc.value.hint or "AUTO" in exc.value.hint + + def test_connect_failure_cleans_up_the_process(self, tmp_path): + """连接失败后不能留下孤儿桥进程 —— 否则 RDK 的独占访问会被它占住。""" + client = client_for(tmp_path, "connectfail") + with pytest.raises(RdkBridgeError): + client.start() + assert not client.is_running + assert client._proc is None # noqa: SLF001 + + def test_dying_bridge_on_start_is_reported(self, tmp_path): + client = client_for(tmp_path, "die_on_ping") + with pytest.raises(RdkBridgeError): + client.start() + + def test_bad_json_from_bridge_is_reported(self, tmp_path): + """RDK 的原生日志混进 stdout 会破坏协议 —— 报错要指向桥日志。""" + client = client_for(tmp_path, "badjson_on_ping") + with pytest.raises(RdkBridgeError) as exc: + client.start() + assert "非 JSON" in str(exc.value) + assert "bridge.log" in str(exc.value) + + def test_request_before_start_raises(self, tmp_path): + client = client_for(tmp_path) + with pytest.raises(RdkBridgeError) as exc: + client.request("ping") + assert "未运行" in str(exc.value) + + +# --------------------------------------------------------------------------- +# 协议交互 +# --------------------------------------------------------------------------- + + +class TestProtocol: + def test_request_returns_parsed_dict(self, tmp_path): + with client_for(tmp_path) as client: + reply = client.request("ping") + assert reply == {"ok": True, "py": "fake"} + + def test_unknown_command_still_round_trips(self, tmp_path): + with client_for(tmp_path) as client: + reply = client.request("whatever", name="hello") + assert reply["ok"] is True + assert reply["echo"] == "hello" + + def test_request_ok_raises_on_failure(self, tmp_path): + with client_for(tmp_path) as client: + with pytest.raises(RdkBridgeError) as exc: + client.request_ok("boom") + assert "故意失败" in str(exc.value) + assert "bridge.log" in exc.value.hint + + def test_request_without_ok_flag_does_not_raise(self, tmp_path): + """``request`` 是低层的，不替调用方判断 ok —— 有些命令要自己看字段。""" + with client_for(tmp_path) as client: + reply = client.request("boom") + assert reply["ok"] is False + + def test_timeout_on_silent_bridge(self, tmp_path): + with client_for(tmp_path) as client: + with pytest.raises(RdkBridgeError) as exc: + client.request("hang", timeout=0.4) + assert "超时" in str(exc.value) + + def test_timeout_mentions_log_path(self, tmp_path): + with client_for(tmp_path) as client: + with pytest.raises(RdkBridgeError) as exc: + client.request("hang", timeout=0.3) + assert "bridge.log" in exc.value.hint + + def test_half_line_json_is_not_accepted(self, tmp_path): + """只收到半行时不能当成应答 —— 必须等到换行或超时。""" + with client_for(tmp_path) as client: + with pytest.raises(RdkBridgeError) as exc: + client.request("half", timeout=0.4) + assert "超时" in str(exc.value) + + def test_native_log_lines_before_json_are_skipped(self, tmp_path): + """RDK 会把原生日志写到 stderr，但万一混进 stdout，非 JSON 行要能跳过 + 并继续读到真正的 JSON 应答，而不是直接判协议损坏。""" + with client_for(tmp_path) as client: + reply = client.request("noisy", timeout=1.0) + assert reply["after_noise"] is True + + def test_bridge_death_during_request_is_detected(self, tmp_path): + with client_for(tmp_path) as client: + with pytest.raises(RdkBridgeError) as exc: + client.request("suicide", timeout=1.5) + assert "已退出" in str(exc.value) + + def test_reply_after_bridge_death_is_refused(self, tmp_path): + with client_for(tmp_path) as client: + with pytest.raises(RdkBridgeError): + client.request("suicide", timeout=1.5) + # 进程已死，后续请求必须立刻失败而不是挂住 + with pytest.raises(RdkBridgeError): + client.request("ping", timeout=0.5) + + +# --------------------------------------------------------------------------- +# 便捷方法是否转发到正确的命令名 +# --------------------------------------------------------------------------- + + +class TestConvenienceWrappers: + def test_enable_sends_enable(self, tmp_path): + with client_for(tmp_path) as client: + assert client.enable()["cmd"] == "enable" + + def test_clear_fault_sends_clear_fault(self, tmp_path): + with client_for(tmp_path) as client: + assert client.clear_fault()["cmd"] == "clear_fault" + + def test_status_sends_status(self, tmp_path): + with client_for(tmp_path) as client: + assert client.status()["cmd"] == "status" + + def test_switch_tool_forwards_the_name(self, tmp_path): + with client_for(tmp_path) as client: + assert client.switch_tool("tool1")["echo"] == "tool1" + + def test_switch_tool_with_empty_name_queries_only(self, tmp_path): + with client_for(tmp_path) as client: + assert client.switch_tool("")["echo"] == "" + + def test_drag_on_forwards_mode(self, tmp_path): + with client_for(tmp_path) as client: + assert client.drag_on("floating")["req"]["mode"] == "floating" + + def test_drag_off_sends_drag_off(self, tmp_path): + with client_for(tmp_path) as client: + assert client.drag_off()["cmd"] == "drag_off" + + def test_joints_reads_the_q_list(self, tmp_path): + with client_for(tmp_path) as client: + q = client.joints() + # 假桥对未知命令回 {"ok":True,"cmd":"joints","echo":""}，没有 q 字段， + # 所以这里应当得到空列表而不是抛异常。 + assert q == [] + + +# --------------------------------------------------------------------------- +# 默认值解析 +# --------------------------------------------------------------------------- + + +class TestDefaults: + def test_default_python_is_overridable_by_env(self, monkeypatch): + from arm_driver_flexiv import rdk_client + + monkeypatch.setenv("RDK_PYTHON", "/opt/custom/python") + assert rdk_client.default_rdk_python() == "/opt/custom/python" + + def test_default_python_falls_back_to_hardcoded(self, monkeypatch): + from arm_driver_flexiv import rdk_client + + monkeypatch.delenv("RDK_PYTHON", raising=False) + assert rdk_client.default_rdk_python() == rdk_client.DEFAULT_RDK_PYTHON + + def test_bridge_script_follows_workspace_env(self, monkeypatch): + from arm_driver_flexiv import rdk_client + + monkeypatch.setenv("SITE_ROS2_ROOT", "/tmp/other_ws") + assert str(rdk_client.default_bridge_script()) == str( + Path("/tmp/other_ws") / rdk_client.BRIDGE_SCRIPT_REL + ) + + def test_error_str_includes_hint(self): + err = RdkBridgeError("出错了", hint="这么修") + assert "出错了" in str(err) + assert "这么修" in str(err) + + def test_error_without_hint_is_just_the_message(self): + assert str(RdkBridgeError("只有消息")) == "只有消息" diff --git a/src/arm_interfaces/arm_interfaces/capabilities.py b/src/arm_interfaces/arm_interfaces/capabilities.py index cafef9f..60201d9 100644 --- a/src/arm_interfaces/arm_interfaces/capabilities.py +++ b/src/arm_interfaces/arm_interfaces/capabilities.py @@ -57,11 +57,11 @@ ROBOT_CAPABILITIES: Dict[str, CapabilityInfo] = { "compliant_descent": CapabilityInfo( "robot", "柔顺下臂 —— 带接触力约束的下压。装配接触阶段（S07）依赖它。" ), - "set_tool": CapabilityInfo( - "robot", "设置工具坐标系（TCP + 负载）。全局工具系锁定依赖它。" - ), "switch_tool": CapabilityInfo( - "robot", "在已保存的工具坐标系之间切换（如切到 tool1）。" + "robot", + "切换到已保存的工具坐标系（如切到 tool1）。" + "RDK 只提供按名字切换（Tool.Switch），工具的参数（质量/质心/TCP）在 Studio 里预配好。" + "全局工具系锁定依赖它。", ), "home": CapabilityInfo("robot", "回到初始位姿。每个装配循环开始与异常复位依赖它。"), "enable": CapabilityInfo("robot", "伺服使能。启动自检依赖它。"), @@ -159,7 +159,6 @@ ROS_INTERFACE: Dict[str, RosInterface] = { "compliant_descent": RosInterface( "", "compliant_descent", "action", "arm_msgs/action/CompliantDescent" ), - "set_tool": RosInterface("", "set_tool", "service", "arm_msgs/srv/SetTool"), "switch_tool": RosInterface("", "switch_tool", "service", "arm_msgs/srv/SwitchTool"), "home": RosInterface("", "home", "service", "std_srvs/srv/Trigger"), "enable": RosInterface("", "enable", "service", "std_srvs/srv/SetBool"), @@ -235,7 +234,6 @@ REQUIREMENTS: Dict[str, Requirement] = { "set_named_pose", "servo_cartesian", "compliant_descent", - "set_tool", "switch_tool", "home", "enable", diff --git a/src/arm_kinematics/arm_kinematics/__init__.py b/src/arm_kinematics/arm_kinematics/__init__.py new file mode 100644 index 0000000..8788ff8 --- /dev/null +++ b/src/arm_kinematics/arm_kinematics/__init__.py @@ -0,0 +1,33 @@ +"""纯几何 / 运动学工具（只依赖 numpy，不依赖 ROS）。 + +目前提供基于 TF 实测位形的几何雅可比与阻尼最小二乘逆解， +见 :mod:`arm_kinematics.geom_ik`。 + +单独成包的动机：这套东西同时被应用层（视觉伺服、搜孔装配）和驱动层 +（柔顺下压）需要。原先它放在 ``arm_application`` 里，导致驱动层要反过来依赖 +应用层 —— 层次倒置。提取到这里之后两边都依赖一个无 ROS 依赖的底层包。 +""" + +from .geom_ik import ( + RIZON_LINK_AXES, + dls_position_step, + dls_translate_hold_ori, + dls_twist, + geometric_jacobian, + load_handeye_T, + position_jacobian, + quat_xyzw_to_R, +) + +__version__ = "0.1.0" + +__all__ = [ + "RIZON_LINK_AXES", + "quat_xyzw_to_R", + "geometric_jacobian", + "position_jacobian", + "dls_position_step", + "dls_twist", + "dls_translate_hold_ori", + "load_handeye_T", +] diff --git a/src/arm_kinematics/arm_kinematics/geom_ik.py b/src/arm_kinematics/arm_kinematics/geom_ik.py new file mode 100644 index 0000000..318a9a5 --- /dev/null +++ b/src/arm_kinematics/arm_kinematics/geom_ik.py @@ -0,0 +1,186 @@ +#!/usr/bin/env python3 +"""基于 TF 实测位形的几何雅可比 + 阻尼最小二乘（DLS）位置级逆解。 + +为什么不需要 URDF / 正运动学 +--------------------------- +本模块做的是**速度级**逆解，不是位置级解析逆解： + +1. 从 TF 直接读每个关节 link 的当前位姿（``link_R`` / ``link_p``） +2. 用固定的关节轴（``RIZON_LINK_AXES``，来自 Rizon4 URDF）构建几何雅可比 J +3. 解 ``J dq = dp`` 得到小步 ``dq``，积分即可 + +雅可比只是**当前位形**的函数，所以用 TF 实测位形算它是正确且实时的；不需要 +"给定关节角预测末端位姿"的正运动学。代价是只能走小步（大位移要迭代多次）， +而这恰好符合装配场景的需求 —— 每次挪 1mm 以内，边动边看力反馈。 + +依赖：只依赖 numpy。不 import ROS，因此可被驱动层与测试直接复用。 + +坐标系约定：所有输入输出都在**基座系**，末端位置 ``ee_p`` 与各 link 位置 +``link_p`` 必须同一参考系。 +""" + +from __future__ import annotations + +from typing import List, Optional, Sequence, Tuple + +import numpy as np + +#: Rizon4 各关节轴在**子 link 系**下的表达（来自 URDF）。 +#: 顺序对应 joint1..joint7 —— 相邻关节轴交替，标准 7 自由度布局。 +RIZON_LINK_AXES = [ + (0.0, 0.0, 1.0), # joint1 -> link1 + (0.0, 1.0, 0.0), # joint2 -> link2 + (0.0, 0.0, 1.0), + (0.0, 1.0, 0.0), + (0.0, 0.0, 1.0), + (0.0, 1.0, 0.0), + (0.0, 0.0, 1.0), +] + + +def quat_xyzw_to_R(q: Sequence[float]) -> np.ndarray: + """四元数 (x, y, z, w) 转 3x3 旋转矩阵。自动归一化，零四元数也不炸。""" + x, y, z, w = [float(v) for v in q] + n = np.sqrt(x * x + y * y + z * z + w * w) + 1e-12 + x, y, z, w = x / n, y / n, z / n, w / n + return np.array( + [ + [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)], + [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)], + [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)], + ], + dtype=float, + ) + + +def geometric_jacobian( + link_R: Sequence[np.ndarray], + link_p: Sequence[np.ndarray], + ee_p: np.ndarray, + axes_local: Sequence[Tuple[float, float, float]] = RIZON_LINK_AXES, +) -> np.ndarray: + """构建 6x7 基座系几何雅可比。前 3 行是线速度，后 3 行是角速度。 + + 参数 + ---- + link_R: + 各关节 link 在基座系下的 3x3 姿态，长度 = 关节数。 + link_p: + 各关节 link 在基座系下的 3 维位置。 + ee_p: + 末端（法兰）在基座系下的 3 维位置。 + """ + n = len(axes_local) + J = np.zeros((6, n), dtype=float) + pee = np.asarray(ee_p, dtype=float).reshape(3) + for i in range(n): + R = np.asarray(link_R[i], dtype=float).reshape(3, 3) + p = np.asarray(link_p[i], dtype=float).reshape(3) + z = R @ np.asarray(axes_local[i], dtype=float).reshape(3) + # 平动部分：关节轴 × (末端 − 关节点)；转动部分：关节轴本身 + J[:3, i] = np.cross(z, pee - p) + J[3:, i] = z + return J + + +def position_jacobian( + link_R: Sequence[np.ndarray], + link_p: Sequence[np.ndarray], + ee_p: np.ndarray, + axes_local: Sequence[Tuple[float, float, float]] = RIZON_LINK_AXES, +) -> np.ndarray: + """只取几何雅可比的平动部分（3x7）。""" + return geometric_jacobian(link_R, link_p, ee_p, axes_local)[:3, :] + + +def dls_position_step( + J: np.ndarray, + dp_base: np.ndarray, + *, + damping: float = 0.05, + max_dq: float = 0.05, +) -> np.ndarray: + """解 ``J dq = dp``（阻尼最小二乘），并把 ``|dq|`` 逐元素限幅。 + + 阻尼项 ``λ²I`` 的作用：接近奇异位形时避免关节速度爆掉。代价是有跟踪误差， + 但对小幅步进（毫米级）可以忽略。 + """ + dp = np.asarray(dp_base, dtype=float).reshape(3) + JT = J.T + A = J @ JT + (damping ** 2) * np.eye(3) + dq = JT @ np.linalg.solve(A, dp) + dq = np.clip(dq, -abs(max_dq), abs(max_dq)) + return dq + + +def dls_twist( + J6: np.ndarray, + v_base: np.ndarray, + *, + w_base: Optional[np.ndarray] = None, + damping: float = 0.08, + max_qdot: float = 0.80, + ori_weight: float = 1.2, +) -> np.ndarray: + """解 ``J qdot = [v; w]``，默认把角速度压到 0（保持姿态）。 + + ``ori_weight`` 给姿态分量额外的权重，让姿态在数值上更"硬"，减少平移时 + 末端姿态的漂移。 + """ + dx = np.zeros(6, dtype=float) + dx[:3] = np.asarray(v_base, dtype=float).reshape(3) + if w_base is not None: + dx[3:] = np.asarray(w_base, dtype=float).reshape(3) + w = np.array([1.0, 1.0, 1.0, ori_weight, ori_weight, ori_weight], dtype=float) + Jw = J6 * w[:, None] + dxw = dx * w + JT = Jw.T + A = Jw @ JT + (damping ** 2) * np.eye(6) + qdot = JT @ np.linalg.solve(A, dxw) + return np.clip(qdot, -abs(max_qdot), abs(max_qdot)) + + +def dls_translate_hold_ori( + J6: np.ndarray, + dp_base: np.ndarray, + *, + damping: float = 0.08, + max_dq: float = 0.05, + ori_weight: float = 1.2, +) -> np.ndarray: + """平移末端 ``dp`` 同时保持姿态。""" + dx = np.zeros(6, dtype=float) + dx[:3] = np.asarray(dp_base, dtype=float).reshape(3) + w = np.array([1.0, 1.0, 1.0, ori_weight, ori_weight, ori_weight], dtype=float) + Jw = J6 * w[:, None] + dxw = dx * w + JT = Jw.T + A = Jw @ JT + (damping ** 2) * np.eye(6) + dq = JT @ np.linalg.solve(A, dxw) + return np.clip(dq, -abs(max_dq), abs(max_dq)) + + +def load_handeye_T(path: str) -> np.ndarray: + """从手眼结果 YAML 里读 4x4 齐次变换矩阵（键名 ``T_4x4``）。""" + import yaml + + with open(path, "r", encoding="utf-8") as f: + data = yaml.safe_load(f) + if not isinstance(data, dict) or "T_4x4" not in data: + raise ValueError(f"No T_4x4 in hand-eye file: {path}") + T = np.array(data["T_4x4"], dtype=float) + if T.shape != (4, 4): + raise ValueError(f"T_4x4 shape {T.shape}, expected (4,4)") + return T + + +__all__ = [ + "RIZON_LINK_AXES", + "quat_xyzw_to_R", + "geometric_jacobian", + "position_jacobian", + "dls_position_step", + "dls_twist", + "dls_translate_hold_ori", + "load_handeye_T", +] diff --git a/src/arm_kinematics/package.xml b/src/arm_kinematics/package.xml new file mode 100644 index 0000000..0c7cd67 --- /dev/null +++ b/src/arm_kinematics/package.xml @@ -0,0 +1,22 @@ + + + + arm_kinematics + 0.1.0 + + 纯几何 / 运动学工具：基于 TF 实测位形的几何雅可比与阻尼最小二乘逆解。 + 只依赖 numpy，不依赖 ROS —— 因此应用层（视觉伺服）与驱动层（柔顺下压） + 都能复用它，而不会形成层次倒置。 + + arm_dev + Apache-2.0 + + python3-numpy + python3-yaml + + python3-pytest + + + ament_python + + diff --git a/src/arm_kinematics/resource/arm_kinematics b/src/arm_kinematics/resource/arm_kinematics new file mode 100644 index 0000000..e69de29 diff --git a/src/arm_kinematics/setup.cfg b/src/arm_kinematics/setup.cfg new file mode 100644 index 0000000..5b93ccd --- /dev/null +++ b/src/arm_kinematics/setup.cfg @@ -0,0 +1,5 @@ +[tool:pytest] +# ROS 2 Humble 的两个 launch-testing pytest 插件用了旧版 hook 签名， +# 当前 pytest 会在收集阶段直接报错。本包只跑纯 Python 单测，禁掉它们。 +addopts = -p no:launch_testing -p no:launch_ros +testpaths = test diff --git a/src/arm_kinematics/setup.py b/src/arm_kinematics/setup.py new file mode 100644 index 0000000..32a1ef5 --- /dev/null +++ b/src/arm_kinematics/setup.py @@ -0,0 +1,20 @@ +from setuptools import find_packages, setup + +package_name = 'arm_kinematics' + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='arm_dev', + maintainer_email='dev@example.com', + description='纯几何 / 运动学工具（几何雅可比 + DLS 逆解）', + license='Apache-2.0', + tests_require=['pytest'], +) diff --git a/src/arm_kinematics/test/test_geom_ik.py b/src/arm_kinematics/test/test_geom_ik.py new file mode 100644 index 0000000..f9f0a4a --- /dev/null +++ b/src/arm_kinematics/test/test_geom_ik.py @@ -0,0 +1,219 @@ +"""``arm_kinematics.geom_ik`` 的数学单测。 + +这些函数是整个运动链路的地基（视觉伺服、搜孔装配、柔顺下压都走它）， +但它们的正确性可以完全脱离硬件验证 —— 用已知的几何算例对答案即可。 +所以这里覆盖得比较细，尤其是不变量（限幅、奇异不炸、零输入）和解析可算的例子。 +""" + +from __future__ import annotations + +import math + +import numpy as np +import pytest + +from arm_kinematics.geom_ik import ( + RIZON_LINK_AXES, + dls_position_step, + dls_translate_hold_ori, + dls_twist, + geometric_jacobian, + position_jacobian, + quat_xyzw_to_R, +) + + +# --------------------------------------------------------------------------- +# 四元数 +# --------------------------------------------------------------------------- + +class TestQuatToR: + def test_identity(self): + assert np.allclose(quat_xyzw_to_R([0.0, 0.0, 0.0, 1.0]), np.eye(3)) + + def test_90_deg_about_z(self): + """90° 绕 z：(x,y,z,w) = (0,0,sin45°,cos45°) → x 轴转到 y 轴。""" + s = math.sin(math.pi / 4) + c = math.cos(math.pi / 4) + R = quat_xyzw_to_R([0.0, 0.0, s, c]) + assert np.allclose(R @ np.array([1.0, 0.0, 0.0]), [0.0, 1.0, 0.0], atol=1e-9) + + def test_unnormalised_input_is_normalised(self): + """未归一化的四元数也应当得到合法旋转（很多驱动给的就是未归一化的）。""" + R = quat_xyzw_to_R([0.0, 0.0, 2.0, 2.0]) # 2x 的 90° 绕 z + assert np.allclose(R @ np.array([1.0, 0.0, 0.0]), [0.0, 1.0, 0.0], atol=1e-9) + + def test_zero_quaternion_does_not_explode(self): + """零四元数（数据缺失）必须给出有限值，不能产生 NaN/Inf。""" + R = quat_xyzw_to_R([0.0, 0.0, 0.0, 0.0]) + assert np.all(np.isfinite(R)) + + def test_result_is_orthonormal(self): + R = quat_xyzw_to_R([0.3, -0.2, 0.5, 0.78]) + assert np.allclose(R @ R.T, np.eye(3), atol=1e-9) + assert np.isclose(np.linalg.det(R), 1.0) + + +# --------------------------------------------------------------------------- +# 几何雅可比 +# --------------------------------------------------------------------------- + +class TestGeometricJacobian: + def test_single_revolute_about_z(self): + """单关节绕 z 轴、关节点在原点、末端在 +x 1m 处。 + + 关节轴 z=(0,0,1)，臂向量 pee - p = (1,0,0)， + 故平动雅可比 = z × arm = (0,1,0)：关节正转使末端朝 +y 移动。 + """ + J = geometric_jacobian( + link_R=[np.eye(3)], + link_p=[np.zeros(3)], + ee_p=np.array([1.0, 0.0, 0.0]), + axes_local=[(0.0, 0.0, 1.0)], + ) + assert J.shape == (6, 1) + assert np.allclose(J[:3, 0], [0.0, 1.0, 0.0], atol=1e-12) + assert np.allclose(J[3:, 0], [0.0, 0.0, 1.0], atol=1e-12) + + def test_axis_follows_link_rotation(self): + """关节轴要跟着 link 的姿态转：link 绕 z 转 90° 后，关节轴仍在 z 上 + （因为轴本身就是 z），但绕 x 轴转 90° 后轴应变成 -y。""" + s = math.sin(math.pi / 4) + c = math.cos(math.pi / 4) + R = quat_xyzw_to_R([s, 0.0, 0.0, c]) # 绕 x 转 90°：(0,0,1) -> (0,-1,0) + J = geometric_jacobian( + link_R=[R], + link_p=[np.zeros(3)], + ee_p=np.array([1.0, 0.0, 0.0]), + axes_local=[(0.0, 0.0, 1.0)], + ) + assert np.allclose(J[3:, 0], [0.0, -1.0, 0.0], atol=1e-9) + + def test_position_jacobian_is_the_linear_block(self): + args = dict( + link_R=[np.eye(3), np.eye(3)], + link_p=[np.zeros(3), np.array([0.0, 0.0, 0.3])], + ee_p=np.array([0.4, 0.0, 0.5]), + axes_local=[(0.0, 0.0, 1.0), (0.0, 1.0, 0.0)], + ) + assert np.allclose( + position_jacobian(**args), geometric_jacobian(**args)[:3, :] + ) + + def test_seven_axis_shape(self): + """真实 Rizon4 是 7 轴；轴表长度必须与之一致，否则雅可比会错位。""" + assert len(RIZON_LINK_AXES) == 7 + J = geometric_jacobian( + link_R=[np.eye(3)] * 7, + link_p=[np.zeros(3)] * 7, + ee_p=np.array([0.5, 0.0, 0.5]), + ) + assert J.shape == (6, 7) + + def test_axis_table_matches_axis_count(self): + """axes_local 长度决定雅可比列数，传错长度应当反映在形状上（不静默出错）。""" + J = geometric_jacobian( + link_R=[np.eye(3)] * 3, + link_p=[np.zeros(3)] * 3, + ee_p=np.array([0.1, 0.0, 0.1]), + axes_local=[(0.0, 0.0, 1.0)] * 3, + ) + assert J.shape == (6, 3) + + +# --------------------------------------------------------------------------- +# DLS 逆解 +# --------------------------------------------------------------------------- + +class TestDlsPositionStep: + def test_recovers_known_solution(self): + """J = [[0],[1],[0]]，要求末端 +y 移动 0.1m → dq ≈ 0.1（阻尼带来的 + 误差极小）。""" + J = np.array([[0.0], [1.0], [0.0]]) + dq = dls_position_step(J, np.array([0.0, 0.1, 0.0]), damping=0.05, max_dq=1.0) + assert dq.shape == (1,) + assert abs(float(dq[0]) - 0.1) < 1e-3 + + def test_direction_is_correct(self): + """要求 +y 移动，关节也必须朝正方向走（符号不能反）。""" + J = np.array([[0.0], [1.0], [0.0]]) + dq = dls_position_step(J, np.array([0.0, 0.05, 0.0])) + assert float(dq[0]) > 0 + + def test_limits_large_step(self): + J = np.array([[0.0], [1.0], [0.0]]) + dq = dls_position_step(J, np.array([0.0, 10.0, 0.0]), max_dq=0.02) + assert abs(float(dq[0])) <= 0.02 + 1e-12 + + def test_singular_jacobian_does_not_explode(self): + """奇异位形（J 全零）必须给出零步长而不是 NaN/Inf —— 阻尼项就是为了这个。""" + J = np.zeros((3, 7)) + dq = dls_position_step(J, np.array([0.1, 0.0, 0.0])) + assert np.all(np.isfinite(dq)) + assert np.allclose(dq, 0.0) + + def test_near_singular_is_bounded(self): + """接近奇异时解会被阻尼压住，不应出现巨大关节速度。""" + J = np.array([[1e-8], [1e-8], [1e-8]]) + dq = dls_position_step(J, np.array([0.01, 0.01, 0.01]), max_dq=0.5) + assert np.all(np.isfinite(dq)) + assert abs(float(dq[0])) <= 0.5 + + def test_zero_displacement_gives_zero_step(self): + J = np.array([[0.0], [1.0], [0.0]]) + dq = dls_position_step(J, np.zeros(3)) + assert np.allclose(dq, 0.0) + + +class TestDlsTwist: + def test_shape_matches_joint_count(self): + dq = dls_twist(np.zeros((6, 7)), np.zeros(3)) + assert dq.shape == (7,) + + def test_respects_qdot_limit(self): + J = np.zeros((6, 7)) + J[0, 0] = 1.0 + J[1, 1] = 1.0 + J[2, 2] = 1.0 + qdot = dls_twist(J, np.array([5.0, 5.0, 5.0]), max_qdot=0.3) + assert np.all(np.abs(qdot) <= 0.3 + 1e-12) + + def test_orientation_hold_keeps_angular_velocity_small(self): + """默认 w=0 且姿态权重更高，角运动分量应当明显小于平动分量。""" + J = np.zeros((6, 3)) + J[0, 0] = 1.0 # 关节0 产生 x 平动 + J[4, 1] = 1.0 # 关节1 产生绕 y 转动 + qdot = dls_twist(J, np.array([0.1, 0.0, 0.0]), max_qdot=1.0) + assert abs(float(qdot[1])) < abs(float(qdot[0])) + + def test_explicit_angular_command_is_applied(self): + J = np.zeros((6, 1)) + J[5, 0] = 1.0 # 关节0 绕 z 转动 + qdot = dls_twist(J, np.zeros(3), w_base=np.array([0.0, 0.0, 0.2])) + assert float(qdot[0]) > 0 + + def test_singular_input_is_finite(self): + qdot = dls_twist(np.zeros((6, 7)), np.array([0.1, 0.1, 0.1])) + assert np.all(np.isfinite(qdot)) + + +class TestDlsTranslateHoldOri: + def test_pure_translation_command_ignores_rotation(self): + """只给平动分量时，转动通道不应当被激活。""" + J = np.zeros((6, 2)) + J[0, 0] = 1.0 + J[5, 1] = 1.0 # 只有关节1 影响转动 + dq = dls_translate_hold_ori(J, np.array([0.05, 0.0, 0.0]), max_dq=1.0) + assert abs(float(dq[1])) < 1e-6 + + def test_translation_direction(self): + J = np.zeros((6, 1)) + J[2, 0] = 1.0 + dq = dls_translate_hold_ori(J, np.array([0.0, 0.0, -0.02]), max_dq=1.0) + assert float(dq[0]) < 0 + + def test_limit_applies(self): + J = np.zeros((6, 1)) + J[2, 0] = 1.0 + dq = dls_translate_hold_ori(J, np.array([0.0, 0.0, -1.0]), max_dq=0.01) + assert abs(float(dq[0])) <= 0.01 + 1e-12 diff --git a/src/arm_msgs/CMakeLists.txt b/src/arm_msgs/CMakeLists.txt index 6c7512d..782be9a 100644 --- a/src/arm_msgs/CMakeLists.txt +++ b/src/arm_msgs/CMakeLists.txt @@ -23,8 +23,10 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/SetNamedPose.srv" "srv/GetArmState.srv" "srv/GripperExecute.srv" + "srv/SwitchTool.srv" "action/MoveToPose.action" "action/FollowJointTrajectory.action" + "action/CompliantDescent.action" DEPENDENCIES std_msgs geometry_msgs builtin_interfaces sensor_msgs ) diff --git a/src/arm_msgs/action/CompliantDescent.action b/src/arm_msgs/action/CompliantDescent.action new file mode 100644 index 0000000..22ab522 --- /dev/null +++ b/src/arm_msgs/action/CompliantDescent.action @@ -0,0 +1,40 @@ +# 柔顺下臂：从当前悬停位姿沿工具系 −Z 缓慢下压，直到触觉检测到接触。 +# +# 通止规装配 S07（下臂接触）专用原语。 +# +# 为什么不用 RDK 的柔顺模式：非夕 RDK 的 compliant motion 会接管整臂阻抗， +# 而装配的其它阶段（跟踪 / 对准 / 拧入）都走 JTC 位置控制；在两种控制权之间 +# 来回切换代价高、掉电风险大。这里用「小幅步进 + 力反馈」在应用层做等效： +# 每步 0.2~1.0 mm，步间等稳定，一旦切向力相对基线跳变立即停住。 +# 对通止规这种「碰到就必须停」的场景足够，而且任何一步都可以安全中断。 +# +# 力阈值全部是「相对静止基线的增量」：Orisys 读数是形变任意单位且基线会漂移， +# 绝对值没有意义。基线由传感器节点的 measure_baseline 服务在启动时测定。 + +# --- 目标与判据 --- +float64 target_x_m # 目标法兰 X（基座系，m）。NaN = 保持当前 XY 不动 +float64 target_y_m # 目标法兰 Y（基座系，m）。NaN = 保持当前 XY 不动 +float64 contact_axis # 监测哪条切向通道：0 = 第一路切向，1 = 第二路切向 +float64 contact_delta # 接触判据：|切向力| 相对静止基线的增量（传感器原生单位） +float64 max_normal_delta # 法向力安全上限（相对基线增量）。<=0 = 不检查 +string side # 用哪只手指的触觉：left | right（装配时是持规那只） + +# --- 运动参数 --- +float64 descent_step_m # 每步下压量（m），建议 0.0002 ~ 0.0010 +float64 descent_speed_mps # 名义下压速度（m/s）。步进间隔 = step / speed +float64 max_descent_m # 最多下压距离（m）。安全上限，到了判失败并停住 +float64 settle_sec # 每步后等待稳定（s）。0 = 不等，靠触觉话题新鲜度 +float64 timeout_sec # 总超时（s）。0 = 由 max_descent_m / descent_speed_mps 推算 +--- +bool success # true = 检测到接触且已停住 +string message # 失败原因：超时 / 触觉断流 / 达到行程上限 / 法向力超限 +geometry_msgs/PoseStamped contact_pose # 接触瞬间的法兰位姿（基座系） +float64 descended_m # 实际下压距离（m） +float64 contact_delta_observed # 触发瞬间的切向增量 +float64 normal_delta_observed # 触发瞬间的法向增量 +--- +float64 progress # 0.0 ~ 1.0（descended_m / max_descent_m） +float64 descended_m # 当前已下压距离（m） +float64 tangential_delta # 当前切向增量（相对基线） +float64 normal_delta # 当前法向增量（相对基线） +string status # approaching | contacting | retracting | done | error diff --git a/src/arm_msgs/srv/SwitchTool.srv b/src/arm_msgs/srv/SwitchTool.srv new file mode 100644 index 0000000..ae12a78 --- /dev/null +++ b/src/arm_msgs/srv/SwitchTool.srv @@ -0,0 +1,17 @@ +# 切换到已保存的工具坐标系（如 tool1）。 +# +# 背景：非夕的工具坐标系（TCP + 质量 + 质心）在 Studio 里预先配好并命名， +# RDK 只提供「按名字切换」一个操作。通止规装配要求工具系全程垂直向下锁定， +# 所以在流程开始前必须确认已切到正确的工具。 +# +# 实现：arm_driver_flexiv 调用 scripts/rdk_switch_tool.py（子进程，rdk_py310）。 +# 注意 RDK 是独占访问，切换时臂必须处于 IDLE 且没有其它进程连着 RDK。 +string tool_name # Studio 里已保存的工具名。空字符串 = 只查询当前状态，不切换 + +string sn # 目标臂 SN（如 Rizon4-063989）。空 = 用 site.yaml 的 robot.sn +--- +bool success # 是否已达到目标状态（已在该工具上也算成功） +string message # 失败原因；成功时为当前工具名说明 +string current_tool # 操作后的当前工具名 +float64 tool_mass_kg # 该工具的质量（kg）；查询不到为 0 +float64[3] tool_com_m # 该工具的质心（法兰系，m）；查询不到为全 0