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
src/arm_kinematics/arm_kinematics/__init__.py
+33
View File
@@ -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",
]
src/arm_kinematics/arm_kinematics/geom_ik.py
+186
View File
@@ -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",
]
src/arm_kinematics/package.xml
+22
View File
@@ -0,0 +1,22 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>arm_kinematics</name>
<version>0.1.0</version>
<description>
纯几何 / 运动学工具：基于 TF 实测位形的几何雅可比与阻尼最小二乘逆解。
只依赖 numpy，不依赖 ROS —— 因此应用层（视觉伺服）与驱动层（柔顺下压）
都能复用它，而不会形成层次倒置。
</description>
<maintainer email="dev@example.com">arm_dev</maintainer>
<license>Apache-2.0</license>
<exec_depend>python3-numpy</exec_depend>
<exec_depend>python3-yaml</exec_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
src/arm_kinematics/resource/arm_kinematics
View File
src/arm_kinematics/setup.cfg
+5
View File
@@ -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
src/arm_kinematics/setup.py
+20
View File
@@ -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'],
)
src/arm_kinematics/test/test_geom_ik.py
+219
View File
@@ -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