orisys/ros_contorl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

P1：设备契约层收尾 + Flexiv 驱动适配器 + 触觉四路数据打通

Browse Source 
新增包
- arm_kinematics：从 arm_application 提取几何雅可比 + DLS 逆解（纯 numpy）。
  它被应用层和驱动层共用，留在应用层会让驱动反向依赖应用层。
  arm_application/geom_ik.py 改为 re-export shim，现有 import 一行未改。
- arm_driver_flexiv：非夕驱动适配器。只补 manifest 声明而现有节点缺失的接口
  （/home /enable /clear_fault /switch_tool /soft_drag /compliant_descent），
  已有能力不重复实现。RDK 访问经 rdk_py310 下的长驻桥，本包不 import flexivrdk。

新接口
- arm_msgs/SwitchTool.srv、arm_msgs/CompliantDescent.action
- 合并 capabilities 里语义重复的 set_tool / switch_tool

修复（都是只在真机上暴露、且难以定位的问题）
- rdk_client 会永久挂死：select 的"可读"不等于"有完整一行"，readline 遇到
  半行 JSON 会一直阻塞等换行符，超时机制完全失效。改用 os.read + 自己做行缓冲。
- CompliantDescent 行程少一步：descended + step > max 在浮点累加下会提前跳出，
  永远到不了标称行程。改用整数步数计数。
- CompliantDescentRunner 的 TF frame 退化成 _base_link：没继承节点已解析的 SN。
- resolve_orisys_config 仍指向旧路径；load_orisys_from_site 读的是旧键 config。

触觉四路数据打通
- 深度图 / 力矢量场原先被 tactile_worker 有意丢弃（注释写"图像不过队列"），
  从未发布到 ROS。新增 image_utils（数组→Image，编码自动判定）、独立的图像
  队列（maxsize=1 满则丢旧）与 /tactile/<side>/{image,vector}。
  默认关闭（tactile.image_hz=0），开启也不影响三力的 30fps 时序。
- mock 后端也会发图像，无硬件即可验证整条发布链路。

配置
- site.yaml 新增 tactile.image_hz；移除与 model 重复的旧键 tactile.config
- 型号配置从 src/arm_application/config/orisys/ 迁到
  devices/sensors/orisys/models/dd03/（随 SDK 一起放）

测试：198 个（原 139）
- arm_kinematics 24（雅可比/逆解的解析算例与不变量）
- arm_driver_flexiv 69（下压状态机 34 + RDK 协议层 35）
- arm_application 59（图像打包 36 + 矢量场探测 23）
This commit is contained in:
arm_dev
2026-09-23 11:17:12 +08:00
parent 6edb8d39ab
commit 883ed6b0b9
39 changed files with 4295 additions and 245 deletions
Download Patch File Download Diff File Expand all files Collapse all files
scripts/lib/rdk_helpers.py
+189
View File
@@ -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 "（空）")
)
scripts/rdk_drag_bridge.py
+150 -40
View File
@@ -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()