O12 gui

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)

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):
         """更新状态显示"""

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]),
+    ])

src/gui_control/package.xml

@@ -13,6 +13,7 @@
   <test_depend>python3-pytest</test_depend>
 
   <exec_depend>linker_hand_ros2_sdk</exec_depend>
+  <exec_depend>omnihand_node</exec_depend>
 
   <export>
     <build_type>ament_python</build_type>

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())