From 2356bd624709862d77d39363c48a76410dcf776a Mon Sep 17 00:00:00 2001
From: lxp <2770281812@qq.com>
Date: Fri, 4 Sep 2026 15:35:45 +0800
Subject: [PATCH] O12 gui
---
.../gui_control/config/constants.py | 32 +++
src/gui_control/gui_control/gui_control.py | 196 +++++++++++++++---
.../launch/o12_right_gui.launch.py | 42 ++++
src/gui_control/package.xml | 1 +
src/gui_control/test/test_o12_config.py | 16 ++
5 files changed, 257 insertions(+), 30 deletions(-)
create mode 100644 src/gui_control/launch/o12_right_gui.launch.py
create mode 100644 src/gui_control/test/test_o12_config.py
diff --git a/src/gui_control/gui_control/config/constants.py b/src/gui_control/gui_control/config/constants.py
index 3049fa5..bb55b38 100644
--- a/src/gui_control/gui_control/config/constants.py
+++ b/src/gui_control/gui_control/config/constants.py
@@ -9,6 +9,10 @@ class HandConfig:
joint_names_en: Optional[List[str]] = None
init_pos: List[int] = field(default_factory=list)
preset_actions: Optional[Dict[str, List[int]]] = None
+ # Integer GUI values are divided by this scale before publishing.
+ # Legacy hands use raw u8 values (scale=1); O12 uses milliradians.
+ position_scale: int = 1
+ position_unit: str = "u8"
# ------------------------------------------------------------------
# 常量字典（仅构建一次）
@@ -258,5 +262,33 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
"拇指压感准备2": [139, 18, 130, 0, 0, 0]
}
),
+ "O12": HandConfig(
+ # O12 active-angle API order. Keep this independent from the
+ # physical motor order exposed by some SDK metadata.
+ joint_names_en=[
+ "thumb_roll", "thumb_abad", "thumb_mcp", "thumb_pip",
+ "index_abad", "index_mcp", "index_pip",
+ "middle_abad", "middle_mcp", "middle_pip",
+ "ring_mcp", "pinky_mcp",
+ ],
+ joint_names=[
+ "拇指旋转", "拇指侧摆", "拇指根部", "拇指中部",
+ "食指侧摆", "食指根部", "食指中部",
+ "中指侧摆", "中指根部", "中指中部",
+ "无名指根部", "小指根部",
+ ],
+ # Values are milliradians so QSlider can retain 0.001 rad resolution.
+ init_pos=[0] * 12,
+ preset_actions={
+ "展开": [0] * 12,
+ "食指轻弯": [0, 0, 0, 0, 0, 300, 400, 0, 0, 0, 0, 0],
+ "半握": [
+ 300, -200, -200, -500,
+ 0, 650, 700, 0, 650, 800, 700, 700,
+ ],
+ },
+ position_scale=1000,
+ position_unit="rad",
+ ),
}
HAND_CONFIGS = MappingProxyType(_HAND_CONFIGS)
diff --git a/src/gui_control/gui_control/gui_control.py b/src/gui_control/gui_control/gui_control.py
index 9e8dae0..fc169b4 100644
--- a/src/gui_control/gui_control/gui_control.py
+++ b/src/gui_control/gui_control/gui_control.py
@@ -1,5 +1,6 @@
import sys
import time, json
+import math
import threading
from dataclasses import dataclass
from typing import List, Dict
@@ -37,6 +38,12 @@ _CANONICAL_COMMAND_NAMES = {
"thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch",
"middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch",
],
+ "O12": [
+ "thumb_roll", "thumb_abad", "thumb_mcp", "thumb_pip",
+ "index_abad", "index_mcp", "index_pip",
+ "middle_abad", "middle_mcp", "middle_pip",
+ "ring_mcp", "pinky_mcp",
+ ],
}
_CANONICAL_COMMAND_BOUNDS = {
@@ -50,15 +57,35 @@ _CANONICAL_COMMAND_BOUNDS = {
"L6": [(0, 255)] * 6,
}
+# Integer slider bounds in milliradians, derived from the O12 active-angle
+# limits. Right and left thumb signs differ; all other limits are shared.
+_O12_COMMAND_BOUNDS_MRAD = {
+ "right": [
+ (0, 942), (-1387, 0), (-827, 0), (-1291, 0),
+ (-261, 261), (0, 1352), (0, 1530), (-261, 261),
+ (0, 1357), (0, 1815), (0, 1535), (0, 1535),
+ ],
+ "left": [
+ (-942, 0), (0, 1387), (-827, 0), (-1291, 0),
+ (-261, 261), (0, 1352), (0, 1530), (-261, 261),
+ (0, 1357), (0, 1815), (0, 1535), (0, 1535),
+ ],
+}
+
class ROS2NodeManager(QObject):
"""ROS2节点管理器，处理ROS通信"""
status_updated = pyqtSignal(str, str) # 状态类型, 消息内容
+ feedback_updated = pyqtSignal(object)
def __init__(self, node_name: str = "hand_control_node"):
super().__init__()
self.node = None
self.publisher = None
+ self.publisher_arc = None
+ self.state_subscription = None
+ self.speed_pub = None
+ self.torque_pub = None
self.joint_state = JointState()
self.joint_state.header = Header()
@@ -88,27 +115,40 @@ class ROS2NodeManager(QObject):
self.node.get_parameter('topic_prefix').value
)
- if self.is_arc == True:
+ if self.hand_joint == "O12":
+ topic_base = f'/o12/{self.hand_type}'
+ self.publisher = self.node.create_publisher(
+ JointState, f'{topic_base}/joint_cmd', 10
+ )
+ self.state_subscription = self.node.create_subscription(
+ JointState,
+ f'{topic_base}/joint_states',
+ self._o12_feedback_callback,
+ 10,
+ )
+ elif self.is_arc == True:
# 创建发布者
self.publisher_arc = self.node.create_publisher(
JointState,
self.topic(f'/cb_{self.hand_type}_hand_control_cmd_arc'),
10
)
- # 创建发布者
- self.publisher = self.node.create_publisher(
- JointState,
- self.topic(f'/cb_{self.hand_type}_hand_control_cmd'),
- 10
- )
+ # 创建旧型号发布者
+ if self.hand_joint != "O12":
+ self.publisher = self.node.create_publisher(
+ JointState,
+ self.topic(f'/cb_{self.hand_type}_hand_control_cmd'),
+ 10
+ )
self.snapshot_publisher = self.node.create_publisher(
JointState, self.topic('/calibration_pose_snapshot'), 10
)
# 新增 speed / torque 发布者
- self.speed_pub = self.node.create_publisher(
- String, self.topic('/cb_hand_setting_cmd'), 10)
- self.torque_pub = self.node.create_publisher(
- String, self.topic('/cb_hand_setting_cmd'), 10)
+ if self.hand_joint != "O12":
+ self.speed_pub = self.node.create_publisher(
+ String, self.topic('/cb_hand_setting_cmd'), 10)
+ self.torque_pub = self.node.create_publisher(
+ String, self.topic('/cb_hand_setting_cmd'), 10)
self.status_updated.emit(
"info",
f"ROS2节点初始化成功: {self.topic_prefix or '/'} "
@@ -141,15 +181,36 @@ class ROS2NodeManager(QObject):
while rclpy.ok() and self.node:
rclpy.spin_once(self.node, timeout_sec=0.1)
+ def _o12_feedback_callback(self, msg: JointState):
+ """Forward 12-channel O12 radian feedback safely into the Qt thread."""
+ if len(msg.position) != 12:
+ self.status_updated.emit(
+ "error", f"O12位置反馈长度错误: {len(msg.position)}，应为12"
+ )
+ return
+ self.feedback_updated.emit([float(value) for value in msg.position])
+
+ def command_bounds(self, count: int):
+ if self.hand_joint == "O12":
+ bounds = _O12_COMMAND_BOUNDS_MRAD.get(self.hand_type)
+ else:
+ bounds = _CANONICAL_COMMAND_BOUNDS.get(self.hand_joint)
+ if not bounds or len(bounds) != count:
+ return [(0, 255)] * count
+ return bounds
+
def bound_positions(self, positions: List[int]) -> List[int]:
- bounds = _CANONICAL_COMMAND_BOUNDS.get(self.hand_joint)
- if not bounds or len(bounds) != len(positions):
- return list(positions)
+ bounds = self.command_bounds(len(positions))
return [
max(minimum, min(maximum, int(value)))
for value, (minimum, maximum) in zip(positions, bounds)
]
+ def wire_positions(self, positions: List[int]) -> List[float]:
+ positions = self.bound_positions(positions)
+ scale = _HAND_CONFIGS[self.hand_joint].position_scale
+ return [float(value) / float(scale) for value in positions]
+
def publish_joint_state(self, positions: List[int]):
"""发布关节状态消息"""
if not self.publisher or not self.node:
@@ -158,15 +219,21 @@ class ROS2NodeManager(QObject):
try:
positions = self.bound_positions(positions)
+ wire_positions = self.wire_positions(positions)
self.joint_state.header.stamp = self.node.get_clock().now().to_msg()
- self.joint_state.position = [float(pos) for pos in positions]
+ self.joint_state.position = wire_positions
# self.joint_state.velocity = [0.1] * len(positions)
# self.joint_state.effort = [0.01] * len(positions)
# 如果有关节名称，添加到消息中
#hand_config = HandConfig.from_hand_type(self.hand_joint)
hand_config = _HAND_CONFIGS[self.hand_joint]
canonical_names = _CANONICAL_COMMAND_NAMES.get(self.hand_joint)
- if canonical_names and len(canonical_names) == len(positions):
+ # O12 SDK 1.1.8 may expose names in physical motor order while the
+ # position array uses active-angle order. An empty name array makes
+ # the documented position order unambiguous to the driver.
+ if self.hand_joint == "O12":
+ self.joint_state.name = []
+ elif canonical_names and len(canonical_names) == len(positions):
self.joint_state.name = canonical_names
elif len(hand_config.joint_names) == len(positions):
if hand_config.joint_names_en != None:
@@ -175,7 +242,7 @@ class ROS2NodeManager(QObject):
self.joint_state.name = hand_config.joint_names
self.publisher.publish(self.joint_state)
- if self.is_arc == True:
+ if self.is_arc == True and self.hand_joint != "O12":
if self.hand_joint == "O6":
if self.hand_type == "left":
pose = range_to_arc_left(positions,self.hand_joint)
@@ -210,14 +277,21 @@ class ROS2NodeManager(QObject):
positions = self.bound_positions(positions)
self.publish_joint_state(positions)
self.joint_state.header.stamp = self.node.get_clock().now().to_msg()
- self.joint_state.position = [float(pos) for pos in positions]
+ self.joint_state.position = self.wire_positions(positions)
canonical_names = _CANONICAL_COMMAND_NAMES.get(self.hand_joint)
- if canonical_names:
+ if canonical_names and self.hand_joint != "O12":
self.joint_state.name = canonical_names
+ elif self.hand_joint == "O12":
+ self.joint_state.name = []
self.snapshot_publisher.publish(self.joint_state)
self.status_updated.emit("info", "当前标定姿势快照已发布")
def publish_speed(self, val: int):
+ if self.hand_joint == "O12":
+ self.status_updated.emit(
+ "warning", "O12不使用旧版u8速度接口；请通过角度轨迹限制速度"
+ )
+ return
if self.hand_joint.upper() in ("O6", "L6"):
joint_len = 6
elif self.hand_joint == "L7":
@@ -237,6 +311,11 @@ class ROS2NodeManager(QObject):
self.speed_pub.publish(msg)
def publish_torque(self, val: int):
+ if self.hand_joint == "O12":
+ self.status_updated.emit(
+ "warning", "O12不使用旧版u8扭矩接口；位置GUI已禁用该设置"
+ )
+ return
if self.hand_joint.upper() in ("O6", "L6"):
joint_len = 6
elif self.hand_joint == "L7":
@@ -278,11 +357,16 @@ class HandControlGUI(QWidget):
# 设置ROS管理器
self.ros_manager = ros_manager
self.ros_manager.status_updated.connect(self.update_status)
+ self.ros_manager.feedback_updated.connect(self.on_feedback_updated)
# 获取手部配置
self.hand_joint = self.ros_manager.hand_joint
self.hand_type = self.ros_manager.hand_type
self.hand_config = _HAND_CONFIGS[self.hand_joint]
+ self.feedback_positions = None
+ # O12 only publishes after an explicit slider/preset action. This
+ # prevents opening or closing the real hand merely by launching GUI.
+ self.command_dirty = self.hand_joint != "O12"
# 初始化UI
self.init_ui()
@@ -293,6 +377,20 @@ class HandControlGUI(QWidget):
self.publish_timer.timeout.connect(self.publish_joint_state)
self.publish_timer.start()
+ def format_position(self, value: int) -> str:
+ """Format an integer GUI value in the hand's actual command unit."""
+ scale = self.hand_config.position_scale
+ if self.hand_config.position_unit == "rad":
+ radians = float(value) / float(scale)
+ return f"{radians:.3f} rad / {math.degrees(radians):.1f}°"
+ return str(value)
+
+ def command_positions(self) -> List[float]:
+ """Return current slider values converted to wire units."""
+ return self.ros_manager.wire_positions(
+ [slider.value() for slider in self.sliders]
+ )
+
def init_ui(self):
"""初始化用户界面"""
# 设置窗口属性
@@ -470,13 +568,11 @@ class HandControlGUI(QWidget):
for i, (name, value) in enumerate(zip(
self.hand_config.joint_names, self.hand_config.init_pos
)):
- bounds = _CANONICAL_COMMAND_BOUNDS.get(self.hand_joint)
- minimum, maximum = (
- bounds[i] if bounds and i < len(bounds) else (0, 255)
- )
+ bounds = self.ros_manager.command_bounds(len(self.hand_config.init_pos))
+ minimum, maximum = bounds[i]
value = max(minimum, min(maximum, int(value)))
# 创建标签
- label = QLabel(f"{name}: {value}")
+ label = QLabel(f"{name}: {self.format_position(value)}")
label.setMinimumWidth(120)
# 创建滑动条
@@ -569,6 +665,9 @@ class HandControlGUI(QWidget):
# —— 2. 新增：速度与扭矩设置（每行一个）——
quick_set_gb = QGroupBox("快速设置")
+ if self.hand_joint == "O12":
+ quick_set_gb.setEnabled(False)
+ quick_set_gb.setToolTip("O12不使用旧版0-255速度/扭矩设置接口")
qv_layout = QVBoxLayout(quick_set_gb)
# 速度行
@@ -697,7 +796,10 @@ class HandControlGUI(QWidget):
"""滑动条值改变事件处理"""
if 0 <= index < len(self.slider_labels):
joint_name = self.hand_config.joint_names[index]
- self.slider_labels[index].setText(f"{joint_name}: {value}")
+ self.slider_labels[index].setText(
+ f"{joint_name}: {self.format_position(value)}"
+ )
+ self.command_dirty = True
# 更新数值显示
self.update_value_display()
@@ -705,10 +807,23 @@ class HandControlGUI(QWidget):
def update_value_display(self):
"""更新数值显示面板内容"""
# 获取所有滑动条的当前值
- values = [slider.value() for slider in self.sliders]
-
- # 格式化显示为列表形式
- self.value_display.setText(f"{values}")
+ command = self.command_positions()
+ command_text = ", ".join(f"{value:.3f}" for value in command)
+ lines = [f"目标({self.hand_config.position_unit}): [{command_text}]"]
+ if self.feedback_positions is not None:
+ feedback_text = ", ".join(
+ f"{value:.3f}" for value in self.feedback_positions
+ )
+ lines.append(f"反馈(rad): [{feedback_text}]")
+ self.value_display.setText("\n".join(lines))
+
+ def on_feedback_updated(self, positions):
+ """Display actual O12 joint feedback without moving command sliders."""
+ self.feedback_positions = list(positions)
+ self.update_value_display()
+ if hasattr(self, "connection_status"):
+ self.connection_status.setText("O12右手已连接并收到位置反馈")
+ self.connection_status.setObjectName("StatusInfo")
def on_preset_action_clicked(self, positions: List[int]):
"""预设动作按钮点击事件处理"""
@@ -747,7 +862,25 @@ class HandControlGUI(QWidget):
self.cycle_button.setText("循环运行预设动作")
self.reset_preset_buttons_color()
- self.status_updated.emit("warning", "已停止所有动作")
+ if self.hand_joint == "O12" and self.feedback_positions is not None:
+ # Hold the latest measured pose instead of merely stopping topic
+ # publication while a previous position target is still active.
+ scale = self.hand_config.position_scale
+ hold_values = [
+ int(round(value * scale)) for value in self.feedback_positions
+ ]
+ hold_values = self.ros_manager.bound_positions(hold_values)
+ for slider, value in zip(self.sliders, hold_values):
+ slider.blockSignals(True)
+ slider.setValue(value)
+ slider.blockSignals(False)
+ self.command_dirty = True
+ self.publish_joint_state()
+ self.update_value_display()
+ self.status_updated.emit("warning", "已下发当前反馈位置并保持")
+ else:
+ self.command_dirty = False
+ self.status_updated.emit("warning", "已停止连续动作和新的位置发布")
def on_save_pose_clicked(self):
"""发布当前滑块姿势，供hand_pose_capture写入profile。"""
@@ -834,8 +967,11 @@ class HandControlGUI(QWidget):
def publish_joint_state(self):
"""发布当前关节状态"""
+ if self.hand_joint == "O12" and not self.command_dirty:
+ return
positions = [slider.value() for slider in self.sliders]
self.ros_manager.publish_joint_state(positions)
+ self.command_dirty = False
def update_status(self, status_type: str, message: str):
"""更新状态显示"""
diff --git a/src/gui_control/launch/o12_right_gui.launch.py b/src/gui_control/launch/o12_right_gui.launch.py
new file mode 100644
index 0000000..3102ab0
--- /dev/null
+++ b/src/gui_control/launch/o12_right_gui.launch.py
@@ -0,0 +1,42 @@
+#!/usr/bin/env python3
+"""Start the HCAN-connected O12 right hand and its safe position GUI."""
+
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, TimerAction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+ o12_driver = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ PathJoinSubstitution([
+ FindPackageShare("omnihand_node"),
+ "launch",
+ "omnihand_pro_2025_node.launch.py",
+ ])
+ )
+ )
+
+ o12_gui = Node(
+ package="gui_control",
+ executable="gui_control",
+ name="o12_right_gui_control",
+ output="screen",
+ parameters=[{
+ "hand_type": "right",
+ "hand_joint": "O12",
+ # Commands are coalesced, so dragging a slider sends at most 10 Hz.
+ "topic_hz": 10,
+ "is_arc": False,
+ "topic_prefix": "",
+ }],
+ )
+
+ return LaunchDescription([
+ o12_driver,
+ # Give the CANFD driver a moment to claim and initialise the adapter.
+ TimerAction(period=1.5, actions=[o12_gui]),
+ ])
diff --git a/src/gui_control/package.xml b/src/gui_control/package.xml
index b444511..4f12cdf 100644
--- a/src/gui_control/package.xml
+++ b/src/gui_control/package.xml
@@ -13,6 +13,7 @@
python3-pytest
linker_hand_ros2_sdk
+ omnihand_node
ament_python
diff --git a/src/gui_control/test/test_o12_config.py b/src/gui_control/test/test_o12_config.py
new file mode 100644
index 0000000..b616cc1
--- /dev/null
+++ b/src/gui_control/test/test_o12_config.py
@@ -0,0 +1,16 @@
+from gui_control.config.constants import HAND_CONFIGS
+
+
+def test_o12_gui_uses_active_angle_order_and_milliradians() -> None:
+ config = HAND_CONFIGS["O12"]
+
+ assert config.joint_names_en == [
+ "thumb_roll", "thumb_abad", "thumb_mcp", "thumb_pip",
+ "index_abad", "index_mcp", "index_pip",
+ "middle_abad", "middle_mcp", "middle_pip",
+ "ring_mcp", "pinky_mcp",
+ ]
+ assert config.position_scale == 1000
+ assert config.position_unit == "rad"
+ assert len(config.init_pos) == 12
+ assert all(len(pose) == 12 for pose in config.preset_actions.values())