diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..59c0593
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,27 @@
+# ROS / colcon build artifacts
+build/
+install/
+log/
+**/build/
+**/install/
+**/log/
+
+# Python
+venv/
+__pycache__/
+*.py[cod]
+*.egg-info/
+.eggs/
+dist/
+
+# Local / runtime
+.mplconfig/
+.ros_home/
+**/.mplconfig/
+.DS_Store
+*.swp
+*~
+
+# Experiment outputs (regenerable)
+reports/
+tools/reports/
diff --git a/README_启动.md b/README_启动.md
new file mode 100644
index 0000000..791266f
--- /dev/null
+++ b/README_启动.md
@@ -0,0 +1,283 @@
+# LinkerHand O6 MuJoCo（ROS2）启动说明
+
+本机跑 MuJoCo 左手仿真，可用本机或另一台电脑通过 ROS2 话题控制。
+
+---
+
+## 环境要求
+
+- Ubuntu + ROS 2 **Jazzy**
+- 工作区路径：`~/linker_hand_mujoco_ros2`
+- Python 依赖在 `venv` 中（含 `mujoco`、`PyQt5`）
+- **注意**：`ros2 launch` 使用系统 `/usr/bin/python3`，仅 `source venv` **不够**，需用下文的启动脚本或设置 `PYTHONPATH`
+
+---
+
+## 一、编译（首次或改代码后）
+
+```bash
+cd ~/linker_hand_mujoco_ros2
+source /opt/ros/jazzy/setup.bash
+colcon build --symlink-install --packages-select linker_hand_mujoco_ros2
+source install/setup.bash
+```
+
+`venv` 目录已放 `COLCON_IGNORE`，不会被 colcon 误扫。
+
+---
+
+## 二、本机启动仿真（推荐）
+
+```bash
+cd ~/linker_hand_mujoco_ros2
+./run_sim.sh
+```
+
+脚本会自动：
+
+- `source` ROS2 与本工作区
+- `ROS_DOMAIN_ID=30`，`ROS_LOCALHOST_ONLY=0`（便于多机）
+- 把 `venv` 的 `site-packages` 加入 `PYTHONPATH`（解决找不到 `mujoco`）
+
+成功后应出现：
+
+1. MuJoCo 3D 窗口（左手 O6）
+2. Joint 进度条窗口（实时关节角）
+
+### 手动启动（等效）
+
+```bash
+cd ~/linker_hand_mujoco_ros2
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+export PYTHONPATH="$PWD/venv/lib/python3.12/site-packages:$PYTHONPATH"
+ros2 launch linker_hand_mujoco_ros2 linker_hand_mujoco_ros2.launch.py
+```
+
+当前 launch 默认：
+
+| 参数 | 值 |
+|------|-----|
+| `hand_type` | `left` |
+| `hand_joint` | `O6` |
+| 控制话题 | `/cb_left_hand_control_cmd` |
+
+---
+
+## 三、控制话题说明
+
+- 话题：`/cb_left_hand_control_cmd`
+- 类型：`sensor_msgs/msg/JointState`
+- `position`：**6 个数**，范围约 **0–255**
+
+| 索引 | 含义 |
+|------|------|
+| 0 | 拇指弯曲 |
+| 1 | 拇指横摆 |
+| 2 | 食指弯曲 |
+| 3 | 中指弯曲 |
+| 4 | 无名指弯曲 |
+| 5 | 小指弯曲 |
+
+约定（当前映射）：约 **255 = 张开，0 = 握紧**。远指节由 mimic 跟随近指节。
+
+### 本机发一帧（握拳示例）
+
+```bash
+source /opt/ros/jazzy/setup.bash
+export ROS_DOMAIN_ID=30
+export ROS_LOCALHOST_ONLY=0
+
+ros2 topic pub --once /cb_left_hand_control_cmd sensor_msgs/msg/JointState "{
+ header: {stamp: {sec: 0, nanosec: 0}, frame_id: ''},
+ name: [],
+ position: [0.0, 100.0, 0.0, 0.0, 0.0, 0.0],
+ velocity: [],
+ effort: []
+}"
+```
+
+张开：
+
+```bash
+ros2 topic pub --once /cb_left_hand_control_cmd sensor_msgs/msg/JointState "{
+ header: {stamp: {sec: 0, nanosec: 0}, frame_id: ''},
+ name: [],
+ position: [255.0, 255.0, 255.0, 255.0, 255.0, 255.0],
+ velocity: [],
+ effort: []
+}"
+```
+
+---
+
+## 四、两台电脑共用 ROS2
+
+ROS2 **没有** ROS1 的 `roscore`。两边约定同一个 Domain 即可。
+
+### 两边都设置
+
+```bash
+export ROS_DOMAIN_ID=30
+export ROS_LOCALHOST_ONLY=0
+```
+
+### 电脑 A：仿真机
+
+```bash
+cd ~/linker_hand_mujoco_ros2
+./run_sim.sh
+```
+
+### 电脑 B：控制机（发指令 / handretarget / SDK）
+
+```bash
+source /opt/ros/jazzy/setup.bash
+export ROS_DOMAIN_ID=30
+export ROS_LOCALHOST_ONLY=0
+
+# 确认话题与收发端
+ros2 topic list
+ros2 topic info /cb_left_hand_control_cmd -v
+```
+
+`topic info -v` 中应能看到：
+
+- **Publisher**：例如 `handretarget_node`
+- **Subscription**：至少包含 `linker_hand_mujoco_ros2_node`
+
+### 查是否真的在发数据
+
+有 Publisher ≠ 正在发数据。请用：
+
+```bash
+ros2 topic hz /cb_left_hand_control_cmd
+ros2 topic echo /cb_left_hand_control_cmd
+```
+
+- `hz` 无输出 → 对端没在 publish
+- `echo --once` 会一直等到有一帧；无数据时像卡住，优先用上面两条
+
+### 跨机能看见话题但收不到数据时
+
+1. 确认两边 `ROS_DOMAIN_ID`、`ROS_LOCALHOST_ONLY=0` 一致，并重启节点
+2. 互相 `ping` 通；尽量同一网段 / 有线；关闭路由器 **AP 隔离**
+3. 本机 `ufw` 可先确认：`sudo ufw status`（不活动则不是防火墙）
+4. 需要时可配置 CycloneDDS 静态 Peer（两边 IP 写入 `~/cyclonedds.xml`），然后：
+
+```bash
+export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+export CYCLONEDDS_URI=file://$HOME/cyclonedds.xml
+```
+
+---
+
+## 五、常用自检命令
+
+```bash
+# 节点是否在
+ros2 node list
+
+# 话题列表（仿真未启动时可能还没有 /cb_left_hand_control_cmd）
+ros2 topic list
+
+# 订阅/发布数量与节点名
+ros2 topic info /cb_left_hand_control_cmd -v
+
+# 频率 / 内容
+ros2 topic hz /cb_left_hand_control_cmd
+ros2 topic echo /cb_left_hand_control_cmd
+```
+
+---
+
+## 六、常见问题
+
+### `ModuleNotFoundError: No module named 'mujoco'`
+
+`ros2 launch` 没用到 venv。请用：
+
+```bash
+./run_sim.sh
+```
+
+或设置 `PYTHONPATH`（见上文「手动启动」）。
+
+### `topic list` 只有 `/rosout`，没有左手话题
+
+仿真节点未运行。先 `./run_sim.sh`，再 `ros2 topic list`。
+
+### 改左右手
+
+编辑：
+
+`src/linkerhand-sim/linker_hand_mujoco_ros2/launch/linker_hand_mujoco_ros2.launch.py`
+
+- 左手：`hand_type: 'left'` → 话题 `/cb_left_hand_control_cmd`
+- 右手：`hand_type: 'right'` → 话题 `/cb_right_hand_control_cmd`
+
+改完后若未用 symlink，再执行一次 `colcon build --symlink-install`。
+
+---
+
+## 七、通用曲线录制（仿真 / 真机同一套话题）
+
+**只传一个文件给对端即可：**
+
+`tools/hand_curve_recorder_standalone.py`
+
+两边都要：已 `source` ROS2、`numpy`、`matplotlib`。
+
+```bash
+source /opt/ros/jazzy/setup.bash
+export ROS_DOMAIN_ID=30
+export ROS_LOCALHOST_ONLY=0
+
+# 仿真端
+python3 tools/hand_curve_recorder_standalone.py --hand left --channel 3 --label sim
+
+# 真机端（把该 py 拷过去后）
+python3 hand_curve_recorder_standalone.py --hand left --channel 3 --label real
+```
+
+对端发 `/cb_left_hand_control_cmd`；本机 **Ctrl+C** → `reports/hand_curves/` 下生成 csv + 三曲线图。
+
+默认还订 `/cb_left_hand_state`（`position`=0～255，`effort` 可放电流）。话题名不同时：
+
+```bash
+python3 hand_curve_recorder_standalone.py --state-topic /你的状态话题 --channel 3 --label real
+```
+
+通道：`0拇指弯 1拇指横摆 2食指 3中指 4无名指 5小指`。
+
+---
+
+要「ROS 发指令 + 仿真里出 q/v/τ 图」：
+
+```bash
+cd ~/linker_hand_mujoco_ros2
+source /opt/ros/jazzy/setup.bash
+export ROS_DOMAIN_ID=30
+export ROS_LOCALHOST_ONLY=0
+
+# 自测：订话题 + 本地 MuJoCo + 自动发中指慢扫 + 可选 viewer
+./venv/bin/python tools/ros_mujoco_record_qvt.py \
+ --hand left --finger middle --viewer --self-sweep
+
+# 对端发指令时：去掉 --self-sweep
+./venv/bin/python tools/ros_mujoco_record_qvt.py \
+ --hand left --finger middle --viewer
+# Ctrl+C → reports/O6_middle_ros/*_qvt.png
+```
+
+---
+
+## 八、文件与模型位置
+
+| 路径 | 说明 |
+|------|------|
+| `./run_sim.sh` | 本机一键启动 |
+| `src/.../launch/linker_hand_mujoco_ros2.launch.py` | 左右手 / 型号 |
+| `src/.../urdf/O6/` | O6 左右手 MJCF（来自 mujoco_testwork 验证模型） |
+| `src/.../utils/joint_monitor.py` | Joint 进度条 UI |
+| `venv/` | Python 依赖（勿放进 colcon 扫描，已 IGNORE） |
diff --git a/linker_hand_ros2_sdk_ws/src/graphic_display/graphic_display/__init__.py b/linker_hand_ros2_sdk_ws/src/graphic_display/graphic_display/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/linker_hand_ros2_sdk_ws/src/graphic_display/graphic_display/graphic_display.py b/linker_hand_ros2_sdk_ws/src/graphic_display/graphic_display/graphic_display.py
new file mode 100644
index 0000000..1acce46
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/graphic_display/graphic_display/graphic_display.py
@@ -0,0 +1,289 @@
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+import matplotlib
+matplotlib.use('Qt5Agg')
+from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg
+from matplotlib.figure import Figure
+from PyQt5 import QtWidgets, QtCore
+import numpy as np
+import json
+import sys
+from typing import Dict, List
+
+from linker_hand_ros2_sdk.LinkerHand.utils.init_linker_hand import InitLinkerHand
+
+class ForceGroupWindow(QtWidgets.QMainWindow):
+ """专用力传感器组可视化窗口"""
+ def __init__(self, group_id: int):
+ super().__init__()
+ self.setWindowTitle(f"Force Sensor Group {group_id+1}")
+ self.setGeometry(100 + group_id*50, 100 + group_id*50, 800, 400)
+
+ # 图形设置
+ self.canvas = FigureCanvasQTAgg(Figure(figsize=(8, 4)))
+ self.setCentralWidget(self.canvas)
+ self.ax = self.canvas.figure.add_subplot(111)
+ self.ax.set_title(f'Force Group {group_id+1} (5 channels)')
+ self.ax.set_xlabel('Time Step')
+ self.ax.set_ylabel('Force (N)')
+ self.ax.grid(True)
+
+ # 数据存储
+ self.buffer_size = 200
+ self.x_data = np.arange(self.buffer_size)
+ #self.channels = [f'Channel {i+1}' for i in range(5)]
+ self.channels = ["thumb","index finger","middle finger","ring finger","little finger"]
+ self.data = {name: np.full(self.buffer_size, np.nan) for name in self.channels}
+ self.lines = {}
+ self.data_ptr = 0
+
+ # 颜色设置
+ colors = ['#1f77b4', '#ff7f0e', '#2ca02c', '#d62728', '#9467bd']
+
+ # 创建曲线
+ for i, name in enumerate(self.channels):
+ self.lines[name], = self.ax.plot(
+ self.x_data,
+ self.data[name],
+ color=colors[i],
+ label=name,
+ linewidth=1.5
+ )
+
+ self.ax.legend(loc='upper right')
+ self.ax.set_xlim(0, self.buffer_size)
+ self.ax.set_ylim(0, 300) # 假设力传感器范围0-300N
+
+ # 定时刷新
+ self.timer = QtCore.QTimer()
+ self.timer.timeout.connect(self.update_plot)
+ self.timer.start(50) # 20fps
+
+ def add_data(self, new_data: List[float]):
+ """添加新数据点"""
+ self.data_ptr = (self.data_ptr + 1) % self.buffer_size
+ for name, val in zip(self.channels, new_data):
+ self.data[name][self.data_ptr] = float(val)
+
+ def update_plot(self):
+ """更新绘图"""
+ # 更新曲线数据
+ for name, line in self.lines.items():
+ line.set_ydata(np.roll(self.data[name], -self.data_ptr))
+
+ self.canvas.draw()
+
+class HandMonitor(Node):
+ def __init__(self):
+ super().__init__('graphic_display')
+
+ # 初始化Qt应用
+ self.app = QtWidgets.QApplication(sys.argv)
+
+ # 窗口管理
+ self.force_windows = {} # 存储force组窗口 {group_id: window}
+ self.temp_window = None # 温度窗口
+ self.torque_window = None # 扭矩窗口
+ self.hand_joint, self.hand_type = InitLinkerHand().current_hand()
+ if self.hand_type == "left":
+ self.topic = "/cb_left_hand_info"
+ else:
+ self.topic = "/cb_right_hand_info"
+ #self.topic = "/cb_left_hand_info"
+ # ROS2订阅
+ self.subscription = self.create_subscription(
+ String,
+ self.topic,
+ self.data_callback,
+ 10)
+
+ # Qt事件处理定时器
+ self.timer = self.create_timer(0.1, self.process_qt_events)
+
+ self.get_logger().info("Hand monitor initialized")
+
+ def data_callback(self, msg: String):
+ """处理手部数据回调"""
+ try:
+ data = json.loads(msg.data)
+ if self.hand_type == "left":
+ tmp = "left_hand"
+ else:
+ tmp = "right_hand"
+ if isinstance(data, dict) and tmp in data:
+ hand_data = data[tmp]
+
+ # 处理force数据 (每组force一个独立窗口)
+ if 'force' in hand_data:
+ force_data = hand_data['force']
+ for group_id, group_values in enumerate(force_data):
+ if len(group_values) == 5: # 每组应有5个值
+ if group_id not in self.force_windows:
+ self.force_windows[group_id] = ForceGroupWindow(group_id)
+ self.force_windows[group_id].show()
+
+ # 跨线程安全更新
+ if QtCore.QThread.currentThread() == self.app.thread():
+ self.force_windows[group_id].add_data(group_values)
+ else:
+ QtCore.QMetaObject.invokeMethod(
+ self.force_windows[group_id],
+ 'add_data',
+ QtCore.Qt.QueuedConnection,
+ QtCore.Q_ARG(list, group_values)
+ )
+
+ # 处理温度数据 (单个窗口)
+ if 'motor_temperature' in hand_data:
+ temp_data = hand_data['motor_temperature']
+ if len(temp_data) == 10: # 应有10个温度值
+ if self.temp_window is None:
+ self.create_temp_window()
+ self.update_window_data(self.temp_window, temp_data)
+
+ # 处理扭矩数据 (单个窗口)
+ # if 'torque' in hand_data:
+ # torque_data = hand_data['torque']
+ # if len(torque_data) == 5: # 应有5个扭矩值
+ # if self.torque_window is None:
+ # self.create_torque_window()
+ # self.update_window_data(self.torque_window, torque_data)
+
+ except Exception as e:
+ self.get_logger().error(f"Data processing error: {str(e)}")
+
+ def create_temp_window(self):
+ """创建温度窗口"""
+ self.temp_window = DataPlotWindow(
+ title="Motor Temperatures",
+ ylabel="Temperature (°C)",
+ channel_count=10,
+ y_range=(20, 50)
+ )
+ self.temp_window.show()
+
+ def create_torque_window(self):
+ """创建扭矩窗口"""
+ self.torque_window = DataPlotWindow(
+ title="Joint Torque",
+ ylabel="Torque (Nm)",
+ channel_count=5,
+ y_range=(-0.5, 0.5)
+ )
+ self.torque_window.show()
+
+ def update_window_data(self, window, data):
+ """通用窗口数据更新"""
+ if QtCore.QThread.currentThread() == self.app.thread():
+ window.add_data(data)
+ else:
+ QtCore.QMetaObject.invokeMethod(
+ window,
+ 'add_data',
+ QtCore.Qt.QueuedConnection,
+ QtCore.Q_ARG(list, data)
+ )
+
+ def process_qt_events(self):
+ """处理Qt事件循环"""
+ self.app.processEvents()
+
+ # 清理已关闭的窗口
+ self.force_windows = {k: v for k, v in self.force_windows.items() if v.isVisible()}
+ if self.temp_window and not self.temp_window.isVisible():
+ self.temp_window = None
+ if self.torque_window and not self.torque_window.isVisible():
+ self.torque_window = None
+
+ def run(self):
+ """启动Qt应用"""
+ self.app.exec_()
+
+ def destroy_node(self):
+ """清理资源"""
+ for window in self.force_windows.values():
+ window.close()
+ if self.temp_window:
+ self.temp_window.close()
+ if self.torque_window:
+ self.torque_window.close()
+ super().destroy_node()
+
+class DataPlotWindow(QtWidgets.QMainWindow):
+ """通用数据绘图窗口"""
+ def __init__(self, title: str, ylabel: str, channel_count: int, y_range: tuple):
+ super().__init__()
+ self.setWindowTitle(title)
+ self.setGeometry(100, 100, 800, 400)
+
+ # 图形设置
+ self.canvas = FigureCanvasQTAgg(Figure(figsize=(8, 4)))
+ self.setCentralWidget(self.canvas)
+ self.ax = self.canvas.figure.add_subplot(111)
+ self.ax.set_title(title)
+ self.ax.set_xlabel('Time Step')
+ self.ax.set_ylabel(ylabel)
+ self.ax.grid(True)
+
+ # 数据存储
+ self.buffer_size = 200
+ self.x_data = np.arange(self.buffer_size)
+ self.channels = [f'Channel {i+1}' for i in range(channel_count)]
+ #self.channels = ["thumb","index finger","middle finger","ring finger","little finger"]
+ self.data = {name: np.full(self.buffer_size, np.nan) for name in self.channels}
+ self.lines = {}
+ self.data_ptr = 0
+
+ # 创建曲线
+ colors = matplotlib.colormaps['tab20'].colors
+ for i, name in enumerate(self.channels):
+ self.lines[name], = self.ax.plot(
+ self.x_data,
+ self.data[name],
+ color=colors[i % len(colors)],
+ label=name,
+ linewidth=1.5
+ )
+
+ self.ax.legend(bbox_to_anchor=(1.05, 1), loc='upper left')
+ self.ax.set_xlim(0, self.buffer_size)
+ self.ax.set_ylim(*y_range)
+
+ # 定时刷新
+ self.timer = QtCore.QTimer()
+ self.timer.timeout.connect(self.update_plot)
+ self.timer.start(50)
+
+ def add_data(self, new_data: List[float]):
+ """添加新数据点"""
+ self.data_ptr = (self.data_ptr + 1) % self.buffer_size
+ for name, val in zip(self.channels, new_data):
+ self.data[name][self.data_ptr] = float(val)
+
+ def update_plot(self):
+ """更新绘图"""
+ for name, line in self.lines.items():
+ line.set_ydata(np.roll(self.data[name], -self.data_ptr))
+ self.canvas.draw()
+
+def main(args=None):
+ rclpy.init(args=args)
+
+ # 必须在主线程创建节点
+ monitor = HandMonitor()
+
+ # 启动Qt线程
+ from threading import Thread
+ qt_thread = Thread(target=monitor.run, daemon=True)
+ qt_thread.start()
+
+ try:
+ rclpy.spin(monitor)
+ except KeyboardInterrupt:
+ pass
+ finally:
+ monitor.destroy_node()
+ rclpy.shutdown()
+
+
diff --git a/linker_hand_ros2_sdk_ws/src/graphic_display/graphic_display/views/__init__.py b/linker_hand_ros2_sdk_ws/src/graphic_display/graphic_display/views/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/linker_hand_ros2_sdk_ws/src/graphic_display/graphic_display/views/temperature_plot.py b/linker_hand_ros2_sdk_ws/src/graphic_display/graphic_display/views/temperature_plot.py
new file mode 100644
index 0000000..536a284
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/graphic_display/graphic_display/views/temperature_plot.py
@@ -0,0 +1,68 @@
+from PyQt5.QtWidgets import QApplication, QVBoxLayout, QWidget
+from PyQt5.QtCore import QTimer
+import sys
+import random
+import matplotlib.pyplot as plt
+from matplotlib import font_manager, rcParams
+from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
+
+class TemperaturePlot(QWidget):
+ def __init__(self, num_lines=10, labels=None, title="Temperature Plot"):
+ super().__init__()
+ self.num_lines = num_lines
+ self.labels = labels if labels else [f"Line {i+1}" for i in range(num_lines)]
+ self.data = [[] for _ in range(self.num_lines)] # 初始化数据列表
+ self.max_points = 100 # 最大显示点数
+ self.setWindowTitle(title)
+ # 设置布局
+ self.layout = QVBoxLayout()
+ self.setLayout(self.layout)
+
+ # 初始化 matplotlib 图形
+ self.figure, self.ax = plt.subplots()
+ self.canvas = FigureCanvas(self.figure)
+ self.layout.addWidget(self.canvas)
+
+ # 初始化绘图曲线
+ self.lines = [self.ax.plot([], [], label=self.labels[i])[0] for i in range(self.num_lines)]
+ self.ax.set_xlim(0, self.max_points)
+ self.ax.set_ylim(0, 100)
+ self.ax.set_xlabel("Time")
+ self.ax.set_ylabel("Value")
+ self.ax.legend(loc='upper left') # 固定标签位置到左上角
+
+ def update_data(self, new_data):
+ for i in range(self.num_lines):
+ self.data[i].append(new_data[i])
+ if len(self.data[i]) > self.max_points:
+ self.data[i].pop(0)
+ self._update_plot()
+
+ def _update_plot(self):
+ """内部方法: 更新绘图"""
+ for i, line in enumerate(self.lines):
+ line.set_data(range(len(self.data[i])), self.data[i])
+
+ self.ax.set_xlim(0, self.max_points)
+ self.canvas.draw()
+
+if __name__ == "__main__":
+ app = QApplication(sys.argv)
+
+ # 示例: 创建一个包含 3 条曲线的波形图
+ waveform_plot = TemperaturePlot(num_lines=10)
+ waveform_plot.resize(800, 400)
+ waveform_plot.show()
+
+ # 模拟数据更新
+ timer = QTimer()
+
+ def update():
+ import random
+ values = [random.randint(0, 300) for _ in range(10)] # 生成随机数据
+ waveform_plot.update_data(values)
+
+ timer.timeout.connect(update)
+ timer.start(100) # 每 100 毫秒更新一次
+
+ sys.exit(app.exec_())
diff --git a/linker_hand_ros2_sdk_ws/src/graphic_display/graphic_display/views/wave_form_plot.py b/linker_hand_ros2_sdk_ws/src/graphic_display/graphic_display/views/wave_form_plot.py
new file mode 100644
index 0000000..0496b24
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/graphic_display/graphic_display/views/wave_form_plot.py
@@ -0,0 +1,68 @@
+from PyQt5.QtWidgets import QApplication, QVBoxLayout, QWidget
+from PyQt5.QtCore import QTimer
+import sys
+import random
+import matplotlib.pyplot as plt
+from matplotlib import font_manager, rcParams
+from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
+
+class WaveformPlot(QWidget):
+ def __init__(self, num_lines=3, labels=None, title="Waveform Plot"):
+ super().__init__()
+ self.num_lines = num_lines
+ self.labels = labels if labels else [f"Line {i+1}" for i in range(num_lines)]
+ self.data = [[] for _ in range(self.num_lines)] # 初始化数据列表
+ self.max_points = 100 # 最大显示点数
+ self.setWindowTitle(title)
+ # 设置布局
+ self.layout = QVBoxLayout()
+ self.setLayout(self.layout)
+
+ # 初始化 matplotlib 图形
+ self.figure, self.ax = plt.subplots()
+ self.canvas = FigureCanvas(self.figure)
+ self.layout.addWidget(self.canvas)
+
+ # 初始化绘图曲线
+ self.lines = [self.ax.plot([], [], label=self.labels[i])[0] for i in range(self.num_lines)]
+ self.ax.set_xlim(0, self.max_points)
+ self.ax.set_ylim(0, 300)
+ self.ax.set_xlabel("Time")
+ self.ax.set_ylabel("Value")
+ self.ax.legend(loc='upper left') # 固定标签位置到左上角
+
+ def update_data(self, new_data):
+ for i in range(self.num_lines):
+ self.data[i].append(new_data[i])
+ if len(self.data[i]) > self.max_points:
+ self.data[i].pop(0)
+ self._update_plot()
+
+ def _update_plot(self):
+ """内部方法: 更新绘图"""
+ for i, line in enumerate(self.lines):
+ line.set_data(range(len(self.data[i])), self.data[i])
+
+ self.ax.set_xlim(0, self.max_points)
+ self.canvas.draw()
+
+if __name__ == "__main__":
+ app = QApplication(sys.argv)
+
+ # 示例: 创建一个包含 3 条曲线的波形图
+ waveform_plot = WaveformPlot(num_lines=3)
+ waveform_plot.resize(800, 400)
+ waveform_plot.show()
+
+ # 模拟数据更新
+ timer = QTimer()
+
+ def update():
+ import random
+ values = [random.randint(0, 300) for _ in range(3)] # 生成随机数据
+ waveform_plot.update_data(values)
+
+ timer.timeout.connect(update)
+ timer.start(100) # 每 100 毫秒更新一次
+
+ sys.exit(app.exec_())
diff --git a/linker_hand_ros2_sdk_ws/src/graphic_display/package.xml b/linker_hand_ros2_sdk_ws/src/graphic_display/package.xml
new file mode 100644
index 0000000..dded93a
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/graphic_display/package.xml
@@ -0,0 +1,19 @@
+
+
+
+ graphic_display
+ 0.0.0
+ TODO: Package description
+ linker-robot
+ TODO: License declaration
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+ linker_hand_ros2_sdk
+
+
+ ament_python
+
+
diff --git a/linker_hand_ros2_sdk_ws/src/graphic_display/pyproject.toml b/linker_hand_ros2_sdk_ws/src/graphic_display/pyproject.toml
new file mode 100644
index 0000000..638dd9c
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/graphic_display/pyproject.toml
@@ -0,0 +1,3 @@
+[build-system]
+requires = ["setuptools>=61.0"]
+build-backend = "setuptools.build_meta"
diff --git a/linker_hand_ros2_sdk_ws/src/graphic_display/resource/graphic_display b/linker_hand_ros2_sdk_ws/src/graphic_display/resource/graphic_display
new file mode 100644
index 0000000..e69de29
diff --git a/linker_hand_ros2_sdk_ws/src/graphic_display/setup.cfg b/linker_hand_ros2_sdk_ws/src/graphic_display/setup.cfg
new file mode 100644
index 0000000..d03563c
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/graphic_display/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/graphic_display
+[install]
+install_scripts=$base/lib/graphic_display
diff --git a/linker_hand_ros2_sdk_ws/src/graphic_display/setup.py b/linker_hand_ros2_sdk_ws/src/graphic_display/setup.py
new file mode 100644
index 0000000..d01e911
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/graphic_display/setup.py
@@ -0,0 +1,37 @@
+'''
+Author: HJX
+Date: 2025-04-02 17:52:22
+LastEditors: Please set LastEditors
+LastEditTime: 2025-04-03 09:45:57
+FilePath: /linker_hand_ros2_sdk/src/graphic_display/setup.py
+Description:
+symbol_custom_string_obkorol_copyright:
+'''
+from setuptools import find_packages, setup
+
+package_name = 'graphic_display'
+
+setup(
+ name=package_name,
+ version='0.0.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', 'linker_hand_ros2_sdk'],
+ zip_safe=True,
+ maintainer='linker-robot',
+ maintainer_email='linker-robot@todo.todo',
+ description='TODO: Package description',
+ license='TODO: License declaration',
+ extras_require={
+ 'test': ['pytest'],
+ },
+ entry_points={
+ 'console_scripts': [
+ 'graphic_display = graphic_display.graphic_display:main'
+ ],
+ },
+)
diff --git a/linker_hand_ros2_sdk_ws/src/graphic_display/test/test_copyright.py b/linker_hand_ros2_sdk_ws/src/graphic_display/test/test_copyright.py
new file mode 100644
index 0000000..97a3919
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/graphic_display/test/test_copyright.py
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found errors'
diff --git a/linker_hand_ros2_sdk_ws/src/graphic_display/test/test_flake8.py b/linker_hand_ros2_sdk_ws/src/graphic_display/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/graphic_display/test/test_flake8.py
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+ rc, errors = main_with_errors(argv=[])
+ assert rc == 0, \
+ 'Found %d code style errors / warnings:\n' % len(errors) + \
+ '\n'.join(errors)
diff --git a/linker_hand_ros2_sdk_ws/src/graphic_display/test/test_pep257.py b/linker_hand_ros2_sdk_ws/src/graphic_display/test/test_pep257.py
new file mode 100644
index 0000000..b234a38
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/graphic_display/test/test_pep257.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found code style errors / warnings'
diff --git a/linker_hand_ros2_sdk_ws/src/gui_control/gui_control/__init__.py b/linker_hand_ros2_sdk_ws/src/gui_control/gui_control/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/linker_hand_ros2_sdk_ws/src/gui_control/gui_control/config/constants.py b/linker_hand_ros2_sdk_ws/src/gui_control/gui_control/config/constants.py
new file mode 100644
index 0000000..952ac15
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/gui_control/gui_control/config/constants.py
@@ -0,0 +1,262 @@
+# hand_config_const.py
+from typing import Dict, List, Optional
+from dataclasses import dataclass, field
+from types import MappingProxyType
+
+@dataclass(frozen=True) # frozen=True 让实例真正只读
+class HandConfig:
+ joint_names: List[str] = field(default_factory=list)
+ joint_names_en: Optional[List[str]] = None
+ init_pos: List[int] = field(default_factory=list)
+ preset_actions: Optional[Dict[str, List[int]]] = None
+
+# ------------------------------------------------------------------
+# 常量字典（仅构建一次）
+# ------------------------------------------------------------------
+_HAND_CONFIGS: Dict[str, HandConfig] = {
+ "L25": HandConfig(
+ joint_names=["大拇指根部", "食指根部", "中指根部", "无名指根部", "小拇指根部",
+ "大拇指侧摆", "食指侧摆", "中指侧摆", "无名指侧摆", "小拇指侧摆",
+ "大拇指横滚", "预留", "预留", "预留", "预留", "大拇指中部", "食指中部",
+ "中指中部", "无名指中部", "小拇指中部", "大拇指指尖", "食指指尖",
+ "中指指尖", "无名指指尖", "小拇指指尖"],
+ init_pos=[255] * 25,
+ preset_actions={
+ "握拳": [0] * 25,
+ "张开": [255] * 25,
+ "OK": [255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+ 255, 255, 255, 255, 255, 255, 0, 0, 255, 255,
+ 0, 0, 0, 255, 255]
+ }
+ ),
+ "L21": HandConfig(
+ joint_names=["大拇指根部", "食指根部", "中指根部", "无名指根部", "小拇指根部",
+ "大拇指侧摆", "食指侧摆", "中指侧摆", "无名指侧摆", "小拇指侧摆",
+ "大拇指横滚", "预留", "预留", "预留", "预留", "大拇指中部", "预留",
+ "预留", "预留", "预留", "大拇指指尖", "食指指尖", "中指指尖",
+ "无名指指尖", "小拇指指尖"],
+ init_pos=[255] * 25
+ ),
+ "L20": HandConfig(
+ joint_names=["拇指根部", "食指根部", "中指根部", "无名指根部", "小指根部",
+ "拇指侧摆", "食指侧摆", "中指侧摆", "无名指侧摆", "小指侧摆",
+ "拇指横摆", "预留", "预留", "预留", "预留", "拇指尖部", "食指末端",
+ "中指末端", "无名指末端", "小指末端"],
+ init_pos=[255, 255, 255, 255, 255, 255, 10, 100, 180, 240, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+ preset_actions={
+ "握拳": [40, 0, 0, 0, 0, 131, 10, 100, 180, 240, 19, 255, 255, 255, 255, 135, 0, 0, 0, 0],
+ "张开": [255, 255, 255, 255, 255, 255, 10, 100, 180, 240, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+ "OK": [191, 95, 255, 255, 255, 136, 107, 100, 180, 240, 72, 255, 255, 255, 255, 116, 99, 255, 255, 255],
+ "点赞": [255, 0, 0, 0, 0, 127, 10, 100, 180, 240, 255, 255, 255, 255, 255, 255, 0, 0, 0, 0],
+ "拇指对食指": [0, 0, 255, 255, 255, 186, 10, 100, 180, 240, 0, 255, 255, 255, 255, 255, 183, 255, 255, 255],
+ "拇指对中指": [0, 255, 0, 255, 255, 145, 10, 100, 180, 240, 0, 255, 255, 255, 255, 255, 255, 202, 255, 255],
+ "拇指对无名指": [0, 255, 255, 0, 255, 108, 10, 100, 180, 240, 0, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+ "拇指对小指": [0, 255, 255, 255, 0, 70, 10, 100, 180, 240, 0, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+ "准备1": [255, 255, 255, 255, 255, 255, 10, 100, 180, 240, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+ "壹": [40, 255, 0, 0, 0, 131, 125, 100, 180, 240, 19, 255, 255, 255, 255, 135, 255, 0, 0, 0],
+ "贰": [40, 255, 255, 0, 0, 81, 35, 177, 180, 240, 19, 255, 255, 255, 255, 135, 255, 255, 0, 0],
+ "叁": [40, 255, 255, 255, 0, 161, 62, 123, 180, 240, 13, 255, 255, 255, 255, 0, 255, 255, 255, 0],
+ "肆": [40, 255, 255, 255, 255, 161, 62, 123, 180, 242, 13, 255, 255, 255, 255, 0, 255, 255, 255, 255],
+ "伍": [255, 255, 255, 255, 255, 255, 10, 100, 180, 240, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+ "陆": [255, 0, 0, 0, 255, 220, 10, 100, 180, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0, 255],
+ "漆": [0, 0, 0, 0, 0, 161, 10, 127, 180, 219, 18, 255, 255, 255, 255, 255, 195, 205, 0, 0],
+ "捌": [255, 255, 0, 0, 0, 202, 104, 100, 180, 240, 233, 255, 255, 255, 255, 255, 255, 0, 0, 0],
+ "玖": [40, 255, 0, 0, 0, 131, 103, 100, 180, 240, 19, 255, 255, 255, 255, 135, 47, 0, 0, 0],
+
+ }
+ ),
+ "G20": HandConfig(
+ joint_names=["拇指根部", "食指根部", "中指根部", "无名指根部", "小指根部",
+ "拇指侧摆", "食指侧摆", "中指侧摆", "无名指侧摆", "小指侧摆",
+ "拇指横摆", "预留", "预留", "预留", "预留", "拇指尖部", "食指末端",
+ "中指末端", "无名指末端", "小指末端"],
+ init_pos=[255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+ preset_actions={
+ "点赞": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0],
+ "握拳": [96, 0, 0, 0, 0, 0, 193, 158, 128, 91, 132, 255, 255, 255, 255, 144, 0, 0, 0, 0],
+ "张开": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+ "OK": [148, 110, 255, 255, 255, 44, 164, 100, 114, 127, 178, 255, 255, 255, 255, 94, 71, 255, 255, 255],
+ "拇指对中指": [191, 255, 55, 255, 255, 96, 95, 100, 114, 127, 105, 255, 255, 255, 255, 94, 255, 108, 255, 255],
+ "拇指对无名指": [191, 255, 255, 72, 255, 115, 95, 100, 114, 127, 60, 255, 255, 255, 255, 94, 255, 255, 97, 255],
+ "拇指对小指": [191, 255, 255, 255, 55, 0, 95, 100, 114, 121, 70, 255, 255, 255, 255, 94, 255, 255, 255, 100],
+ "准备1": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0],
+ "壹": [96, 255, 0, 0, 0, 0, 190, 161, 127, 80, 68, 255, 255, 255, 255, 144, 255, 0, 0, 0],
+ "贰": [96, 255, 255, 0, 0, 0, 190, 66, 127, 80, 68, 255, 255, 255, 255, 144, 255, 255, 0, 0],
+ "叁": [96, 255, 255, 255, 0, 0, 200, 132, 76, 80, 68, 255, 255, 255, 255, 144, 255, 255, 255, 0],
+ "肆": [80, 255, 255, 255, 255, 78, 200, 132, 114, 48, 129, 255, 255, 255, 255, 64, 255, 255, 255, 255],
+ "伍": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+ "陆": [255, 0, 0, 0, 255, 255, 156, 126, 125, 42, 245, 255, 255, 255, 255, 255, 0, 0, 0, 255],
+ "漆": [38, 0, 0, 0, 0, 55, 156, 126, 125, 117, 145, 255, 255, 255, 255, 255, 164, 163, 0, 0],
+ "捌": [255, 255, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 255, 0, 0, 0],
+ "玖": [67, 255, 0, 0, 0, 37, 162, 162, 144, 100, 85, 255, 255, 255, 255, 169, 0, 0, 0, 0],
+ "动作1": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+ "动作2": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+ "动作3": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+ "动作4": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+ "动作5": [255, 255, 255, 255, 255, 255, 255, 255, 247, 255, 210, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+ "动作6": [255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 210, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+ "动作7": [255, 255, 255, 255, 255, 255, 255, 255, 247, 255, 210, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+ "动作8": [255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 210, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+ "动作9": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 210, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+ "根部1": [0, 0, 0, 0, 0, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+ "根部2": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+ "根部1": [0, 0, 0, 0, 0, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+ "根部2": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+ "末端1": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 125, 0, 0, 0, 0],
+ "末端2": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+ "末端3": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 125, 0, 0, 0, 0],
+ "末端4": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+ "默认": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+
+
+ }
+ ),
+ # 大拇指关节球版L10
+ # "L10": HandConfig(
+ # joint_names_en=["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch",
+ # "index_mcp_roll", "ring_mcp_roll", "pinky_mcp_roll", "thumb_cmc_yaw"],
+ # joint_names=["拇指根部", "拇指侧摆", "食指根部", "中指根部", "无名指根部",
+ # "小指根部", "食指侧摆", "无名指侧摆", "小指侧摆", "拇指旋转"],
+ # init_pos=[255] * 10,
+ # preset_actions={
+ # "握拳": [75, 128, 0, 0, 0, 0, 128, 128, 128, 57],
+ # "张开": [255, 128, 255, 255, 255, 255, 128, 128, 128, 128],
+ # "OK": [110, 128, 75, 255, 255, 255, 128, 128, 128, 68],
+ # "点赞": [255, 145, 0, 0, 0, 0, 0, 255, 255, 65]
+ # }
+ # ),
+ # 大拇指关节齿轮版L10
+ "L10": HandConfig(
+ joint_names_en=["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch",
+ "index_mcp_roll", "ring_mcp_roll", "pinky_mcp_roll", "thumb_cmc_roll"],
+ joint_names=["拇指根部", "拇指侧摆", "食指根部", "中指根部", "无名指根部",
+ "小指根部", "食指侧摆", "无名指侧摆", "小指侧摆", "拇指旋转"],
+ init_pos=[255] * 10,
+ preset_actions={
+ "张开": [255, 255, 255, 255, 255, 255, 128, 67, 89, 255],
+ "点赞": [255, 255, 0, 0, 0, 0, 128, 67, 89, 255],
+ "握拳": [90, 0, 0, 0, 0, 0, 128, 67, 89, 197],
+ "壹": [55, 0, 255, 0, 0, 0, 128, 67, 89, 124],
+ "贰": [55, 0, 255, 255, 0, 0, 128, 67, 89, 124],
+ "叁": [116, 255, 255, 255, 255, 0, 128, 67, 89, 255],
+ "肆": [0, 0, 255, 255, 255, 255, 128, 67, 89, 255],
+ "伍": [255, 255, 255, 255, 255, 255, 128, 67, 89, 255],
+ "陆": [255, 255, 0, 0, 0, 255, 128, 67, 89, 255],
+ "柒1": [255, 37, 119, 112, 0, 0, 128, 67, 89, 211],
+ "柒2": [91, 37, 119, 112, 0, 0, 128, 67, 89, 211],
+ "捌": [255, 255, 255, 0, 0, 0, 128, 67, 89, 255],
+ "玖": [59, 0, 134, 0, 0, 0, 128, 67, 89, 153],
+ "侧摆0": [255, 0, 255, 255, 255, 255, 128, 67, 89, 153],
+ "侧摆1": [0, 0, 255, 255, 255, 255, 255, 255, 255, 255],
+ "侧摆2": [0, 0, 255, 255, 255, 255, 0, 0, 0, 255],
+ "侧摆3": [0, 0, 255, 255, 255, 255, 255, 255, 255, 255],
+ "侧摆4": [0, 0, 255, 255, 255, 255, 0, 0, 0, 255],
+ "侧摆5": [0, 0, 255, 255, 255, 255, 255, 255, 255, 255],
+ "侧摆6": [0, 0, 255, 255, 255, 255, 128, 67, 89, 255],
+ "弯曲1": [209, 0, 124, 122, 123, 122, 128, 67, 89, 255],
+ "弯曲2": [0, 0, 255, 255, 255, 255, 128, 67, 89, 255],
+ "弯曲3": [209, 0, 124, 122, 123, 122, 128, 67, 89, 255],
+ "弯曲4": [0, 0, 255, 255, 255, 255, 128, 67, 89, 255],
+ "弯曲5": [209, 0, 124, 122, 123, 122, 128, 67, 89, 255],
+ "弯曲6": [0, 0, 255, 255, 255, 255, 128, 67, 89, 255],
+ "OK": [84, 39, 122, 255, 255, 255, 128, 67, 89, 255],
+ "拇指压感1": [134, 39, 99, 255, 255, 255, 128, 67, 89, 255],
+ "拇指压感2": [73, 39, 91, 255, 255, 255, 128, 67, 89, 255],
+ "食指压感1": [151, 39, 190, 255, 255, 255, 128, 67, 89, 255],
+ "食指压感2": [58, 39, 103, 255, 255, 255, 128, 67, 89, 255],
+ "中指压感1": [40, 39, 255, 128, 255, 255, 128, 67, 89, 209],
+ "中指压感2": [40, 39, 255, 89, 255, 255, 128, 67, 89, 209],
+ "无名指压感1": [51, 39, 255, 255, 139, 255, 128, 67, 89, 154],
+ "无名指压感2": [51, 39, 255, 255, 83, 255, 128, 67, 89, 154],
+ "小拇指压感1": [62, 39, 255, 255, 255, 155, 128, 67, 89, 101],
+ "小拇指压感2": [62, 39, 255, 255, 255, 75, 128, 67, 89, 101],
+ }
+ ),
+ # 大拇指关节球版L7
+ # "L7": HandConfig(
+ # joint_names=["大拇指弯曲", "大拇指横摆", "食指弯曲", "中指弯曲", "无名指弯曲",
+ # "小拇指弯曲", "拇指旋转"],
+ # init_pos=[250] * 7,
+ # preset_actions={
+ # "点赞": [255, 111, 0, 0, 0, 0, 86],
+ # "握拳": [71, 79, 0, 0, 0, 0, 64],
+ # "张开": [255, 111, 250, 250, 250, 250, 55],
+ # "OK": [141, 111, 168, 250, 250, 250, 86],
+
+ # }
+ # ),
+ # 大拇指关节齿轮版L7
+ "L7": HandConfig(
+ joint_names_en=["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch", "thumb_cmc_roll"],
+ joint_names=["大拇指弯曲", "大拇指横摆", "食指弯曲", "中指弯曲", "无名指弯曲", "小拇指弯曲", "拇指旋转"],
+ init_pos=[250] * 7,
+ preset_actions={
+ "张开": [255, 111, 250, 250, 250, 250, 55],
+ "点赞": [255, 255, 0, 0, 0, 0, 255],
+ "赞2": [255, 0, 0, 0, 0, 0, 255],
+ "握拳": [65, 0, 0, 0, 0, 0, 93],
+ "壹": [66, 0, 255, 0, 0, 0, 93],
+ "壹1": [61, 0, 255, 0, 0, 0, 255],
+ "贰": [0, 0, 255, 255, 0, 0, 255],
+ "叁": [0, 0, 255, 255, 255, 0, 255],
+ "肆": [0, 0, 255, 255, 255, 255, 119],
+ "伍": [255, 111, 250, 250, 250, 250, 55],
+ "OK": [99, 15, 146, 250, 250, 250, 206],
+ "拇指压感1": [99, 15, 206, 250, 250, 250, 206],
+ "拇指压感2": [109, 15, 70, 250, 250, 250, 206],
+ "食指压感1": [99, 15, 206, 250, 250, 250, 206],
+ "食指压感2": [69, 15, 140, 250, 250, 250, 206],
+ "中指压感": [82, 15, 255, 136, 250, 250, 170],
+ "无名指压感": [70, 15, 255, 255, 141, 250, 125],
+ "小拇指压感": [70, 15, 255, 255, 255, 120, 78],
+ "准备1": [70, 15, 255, 255, 255, 255, 78]
+
+ }
+ ),
+ "O6": HandConfig(
+ joint_names_en=["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "pinky_mcp_pitch", "ring_mcp_pitch"],
+ joint_names=["大拇指弯曲", "大拇指横摆", "食指弯曲", "中指弯曲", "无名指弯曲", "小拇指弯曲"],
+ init_pos=[250] * 6,
+ preset_actions={
+ "张开": [250, 250, 250, 250, 250, 250],
+ "壹": [125, 18, 255, 0, 0, 0],
+ "贰": [92, 87, 255, 255, 0, 0],
+ "叁": [92, 87, 255, 255, 255, 0],
+ "肆": [92, 87, 255, 255, 255, 255],
+ "伍": [255, 255, 255, 255, 255, 255],
+ "OK": [139, 91, 103, 250, 250, 250],
+ "点赞": [250, 79, 0, 0, 0, 0],
+ "握拳": [102, 18, 0, 0, 0, 0],
+ }
+ ),
+ "L6": HandConfig(
+ joint_names_en=["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "pinky_mcp_pitch", "ring_mcp_pitch"],
+ joint_names=["大拇指弯曲", "大拇指横摆", "食指弯曲", "中指弯曲", "无名指弯曲", "小拇指弯曲"],
+ init_pos=[250] * 6,
+ preset_actions={
+ "张开": [250, 250, 250, 250, 250, 250],
+ "壹": [0, 18, 255, 0, 0, 0],
+ "贰": [0, 39, 255, 255, 0, 0],
+ "叁": [0, 39, 255, 255, 255, 0],
+ "肆": [0, 0, 255, 255, 255, 255],
+ "伍": [255, 255, 255, 255, 255, 255],
+ "OK": [74, 13, 153, 255, 255, 255],
+ "点赞": [255, 255, 0, 0, 0, 0],
+ "握拳": [79, 11, 0, 0, 0, 0],
+ "序列动作1": [250, 250, 250, 250, 250, 250],
+ "序列动作2": [250, 250, 0, 250, 250, 0],
+ "序列动作3": [250, 250, 0, 0, 0, 0],
+ "序列动作4": [250, 250, 0, 0, 0, 255],
+ "序列动作5": [250, 250, 0, 0, 255, 255],
+ "序列动作6": [250, 250, 0, 255, 255, 255],
+ "序列动作7": [250, 250, 250, 250, 250, 250],
+ "食指压感准备1": [0, 18, 255, 0, 0, 0],
+ "食指压感测试": [9, 42, 55, 250, 250, 250],
+ "食指压感准备2": [0, 18, 255, 0, 0, 0],
+ "拇指压感准备1": [139, 18, 130, 0, 0, 0],
+ "拇指压感测试": [39, 30, 122, 250, 250, 250],
+ "拇指压感准备2": [139, 18, 130, 0, 0, 0]
+ }
+ ),
+}
+HAND_CONFIGS = MappingProxyType(_HAND_CONFIGS)
diff --git a/linker_hand_ros2_sdk_ws/src/gui_control/gui_control/gui_control.py b/linker_hand_ros2_sdk_ws/src/gui_control/gui_control/gui_control.py
new file mode 100644
index 0000000..1fc2045
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/gui_control/gui_control/gui_control.py
@@ -0,0 +1,808 @@
+import sys
+import time, json
+import threading
+from dataclasses import dataclass
+from typing import List, Dict
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String, Header
+from sensor_msgs.msg import JointState
+from PyQt5.QtCore import Qt, pyqtSignal, QTimer, QObject, QEvent
+from PyQt5.QtWidgets import (
+ QApplication, QWidget, QVBoxLayout, QHBoxLayout, QGridLayout,
+ QSlider, QLabel, QPushButton, QGroupBox, QScrollArea, QTabWidget,
+ QFrame, QSplitter, QMessageBox, QTextEdit
+)
+from PyQt5.QtGui import QFont
+
+from .utils.mapping import *
+
+from .config.constants import _HAND_CONFIGS
+LOOP_TIME = 1000 # 循环动作间隔时间 毫秒
+class ROS2NodeManager(QObject):
+ """ROS2节点管理器，处理ROS通信"""
+ status_updated = pyqtSignal(str, str) # 状态类型, 消息内容
+
+ def __init__(self, node_name: str = "hand_control_node"):
+ super().__init__()
+ self.node = None
+ self.publisher = None
+ self.joint_state = JointState()
+ self.joint_state.header = Header()
+
+ # 初始化ROS2节点
+ self.init_node(node_name)
+
+ def init_node(self, node_name: str):
+ """初始化ROS2节点"""
+ try:
+ if not rclpy.ok():
+ rclpy.init(args=None)
+ self.node = Node(node_name)
+
+ # 声明参数
+ self.node.declare_parameter('hand_type', 'left')
+ self.node.declare_parameter('hand_joint', 'L10')
+ self.node.declare_parameter('topic_hz', 30)
+ self.node.declare_parameter('is_arc', False)
+
+ # 获取参数
+ self.hand_type = self.node.get_parameter('hand_type').value
+ self.hand_joint = self.node.get_parameter('hand_joint').value
+ self.hz = self.node.get_parameter('topic_hz').value
+ self.is_arc = self.node.get_parameter('is_arc').value
+
+ if self.is_arc == True:
+ # 创建发布者
+ self.publisher_arc = self.node.create_publisher(
+ JointState, f'/cb_{self.hand_type}_hand_control_cmd_arc', 10
+ )
+ # 创建发布者
+ self.publisher = self.node.create_publisher(
+ JointState, f'/cb_{self.hand_type}_hand_control_cmd', 10
+ )
+ # 新增 speed / torque 发布者
+ self.speed_pub = self.node.create_publisher(
+ String, f'/cb_hand_setting_cmd', 10)
+ self.torque_pub = self.node.create_publisher(
+ String, f'/cb_hand_setting_cmd', 10)
+ self.status_updated.emit("info", f"ROS2节点初始化成功: {self.hand_type} {self.hand_joint}")
+
+ # 启动ROS2自旋线程
+ self.spin_thread = threading.Thread(target=self.spin_node, daemon=True)
+ self.spin_thread.start()
+ except Exception as e:
+ self.status_updated.emit("error", f"ROS2初始化失败: {str(e)}")
+ raise
+
+ def spin_node(self):
+ """运行ROS2节点自旋循环"""
+ while rclpy.ok() and self.node:
+ rclpy.spin_once(self.node, timeout_sec=0.1)
+
+ def publish_joint_state(self, positions: List[int]):
+ """发布关节状态消息"""
+ if not self.publisher or not self.node:
+ self.status_updated.emit("error", "ROS2发布者未初始化")
+ return
+
+ try:
+ 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.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]
+ if len(hand_config.joint_names) == len(positions):
+ if hand_config.joint_names_en != None:
+ self.joint_state.name = hand_config.joint_names_en
+ else:
+ self.joint_state.name = hand_config.joint_names
+
+ self.publisher.publish(self.joint_state)
+ if self.is_arc == True:
+ if self.hand_joint == "O6":
+ if self.hand_type == "left":
+ pose = range_to_arc_left(positions,self.hand_joint)
+ elif self.hand_type == "right":
+ pose = range_to_arc_right(positions,self.hand_joint)
+ elif self.hand_joint == "L7" or self.hand_joint == "L21" or self.hand_joint == "L25":
+ if self.hand_type == "left":
+ pose = range_to_arc_left(positions,self.hand_joint)
+ elif self.hand_type == "right":
+ pose = range_to_arc_right(positions,self.hand_joint)
+ elif self.hand_joint == "L10":
+ if self.hand_type == "left":
+ pose = range_to_arc_left_10(positions)
+ elif self.hand_type == "right":
+ pose = range_to_arc_right_10(positions)
+ elif self.hand_joint == "L20":
+ if self.hand_type == "left":
+ pose = range_to_arc_left_l20(positions)
+ elif self.hand_type == "right":
+ pose = range_to_arc_right_l20(positions)
+ else:
+ #print(f"当前{self.hand_joint} {self.hand_type}不支持弧度转换", flush=True)
+ pass
+ self.joint_state.position = [float(pos) for pos in pose]
+ self.publisher_arc.publish(self.joint_state)
+ self.status_updated.emit("info", "关节状态已发布")
+ except Exception as e:
+ self.status_updated.emit("error", f"发布失败: {str(e)}")
+
+ def publish_speed(self, val: int):
+ joint_len = 0
+ if (self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6"):
+ joint_len = 6
+ elif self.hand_joint == "L7":
+ joint_len = 7
+ elif self.hand_joint == "L10":
+ joint_len = 10
+ else:
+ joint_len = 5
+ msg = String()
+ v = [val] * joint_len
+ data = {
+ "setting_cmd": "set_speed",
+ "params": {"hand_type":self.hand_type,"speed": v},
+ }
+ msg.data = json.dumps(data)
+ print(f"速度值：{v}", flush=True)
+ self.speed_pub.publish(msg)
+
+ def publish_torque(self, val: int):
+ joint_len = 0
+ if (self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6"):
+ joint_len = 6
+ elif self.hand_joint == "L7":
+ joint_len = 7
+ elif self.hand_joint == "L10":
+ joint_len = 10
+ else:
+ joint_len = 5
+ msg = String()
+ v = [val] * joint_len
+ data = {
+ "setting_cmd": "set_max_torque_limits",
+ "params": {"hand_type":self.hand_type,"torque": v},
+ }
+
+ msg.data = json.dumps(data)
+ print(f"扭矩值：{v}", flush=True)
+ self.torque_pub.publish(msg)
+
+ def shutdown(self):
+ """关闭ROS2节点"""
+ if self.node:
+ self.node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+class HandControlGUI(QWidget):
+ """灵巧手控制界面"""
+ status_updated = pyqtSignal(str, str) # 状态类型, 消息内容
+
+ def __init__(self, ros_manager: ROS2NodeManager):
+ super().__init__()
+
+ # 循环控制变量
+ self.cycle_timer = None # 循环定时器
+ self.current_action_index = -1 # 当前动作索引
+ self.preset_buttons = [] # 存储预设动作按钮引用
+
+ # 设置ROS管理器
+ self.ros_manager = ros_manager
+ self.ros_manager.status_updated.connect(self.update_status)
+
+ # 获取手部配置
+ self.hand_joint = self.ros_manager.hand_joint
+ self.hand_type = self.ros_manager.hand_type
+ self.hand_config = _HAND_CONFIGS[self.hand_joint]
+
+ # 初始化UI
+ self.init_ui()
+
+ # 设置定时器发布关节状态
+ self.publish_timer = QTimer(self)
+ self.publish_timer.setInterval(int(1000 / self.ros_manager.hz))
+ self.publish_timer.timeout.connect(self.publish_joint_state)
+ self.publish_timer.start()
+
+ def init_ui(self):
+ """初始化用户界面"""
+ # 设置窗口属性
+ self.setWindowTitle(f'灵巧手控制界面 - {self.hand_type} {self.hand_joint}')
+ self.setMinimumSize(1200, 900)
+
+ # 设置样式
+ self.setStyleSheet("""
+ QWidget {
+ font-family: 'Microsoft YaHei', 'SimHei', sans-serif;
+ font-size: 12px;
+ }
+ QGroupBox {
+ border: 1px solid #CCCCCC;
+ border-radius: 6px;
+ margin-top: 6px;
+ padding: 10px;
+ }
+ QGroupBox::title {
+ subcontrol-origin: margin;
+ left: 10px;
+ padding: 0 5px 0 5px;
+ color: #165DFF;
+ font-weight: bold;
+ }
+ QSlider::groove:horizontal {
+ border: 1px solid #999999;
+ height: 8px;
+ border-radius: 4px;
+ background: #CCCCCC;
+ margin: 2px 0;
+ }
+ QSlider::handle:horizontal {
+ background: qlineargradient(x1:0, y1:0, x2:1, y2:1, stop:0 #165DFF, stop:1 #0E42D2);
+ border: 1px solid #5C8AFF;
+ width: 18px;
+ margin: -5px 0;
+ border-radius: 9px;
+ }
+ QPushButton {
+ background-color: #E0E0E0;
+ border: 1px solid #CCCCCC;
+ border-radius: 4px;
+ padding: 5px 10px;
+ min-width: 80px;
+ }
+ QPushButton:hover {
+ background-color: #F0F0F0;
+ }
+ QPushButton:pressed {
+ background-color: #D0D0D0;
+ }
+ QPushButton[category="preset"] {
+ background-color: #E6F7FF;
+ color: #1890FF;
+ border-color: #91D5FF;
+ }
+ QPushButton[category="preset"]:hover {
+ background-color: #B3E0FF;
+ }
+ QPushButton[category="action"] {
+ background-color: #FFF7E6;
+ color: #FA8C16;
+ border-color: #FFD591;
+ }
+ QPushButton[category="action"]:hover {
+ background-color: #FFE6B3;
+ }
+ QPushButton[category="danger"] {
+ background-color: #FFF1F0;
+ color: #F5222D;
+ border-color: #FFCCC7;
+ }
+ QPushButton[category="danger"]:hover {
+ background-color: #FFE8E6;
+ }
+ QLabel#StatusLabel {
+ padding: 5px;
+ border-radius: 4px;
+ }
+ QLabel#StatusInfo {
+ background-color: #F0F7FF;
+ color: #0066CC;
+ }
+ QLabel#StatusError {
+ background-color: #FFF0F0;
+ color: #CC0000;
+ }
+ /* 数值显示面板样式 */
+ QTextEdit#ValueDisplay {
+ background-color: #F8F8F8;
+ border: 1px solid #CCCCCC;
+ border-radius: 4px;
+ padding: 10px;
+ font-family: Consolas, monospace;
+ font-size: 12px;
+ }
+ """)
+
+ # 创建主垂直布局
+ main_layout = QVBoxLayout(self)
+
+ # 创建水平分割器（原有三个面板）
+ splitter = QSplitter(Qt.Horizontal)
+
+ # 创建左侧关节控制面板
+ self.joint_control_panel = self.create_joint_control_panel()
+ splitter.addWidget(self.joint_control_panel)
+
+ # 创建中间预设动作面板
+ self.preset_actions_panel = self.create_preset_actions_panel()
+ splitter.addWidget(self.preset_actions_panel)
+
+ # 创建右侧状态监控面板
+ self.status_monitor_panel = self.create_status_monitor_panel()
+ splitter.addWidget(self.status_monitor_panel)
+
+ # 设置分割器比例
+ splitter.setSizes([500, 300, 400])
+
+ # 添加分割器到主布局，并设置拉伸因子为1（可伸缩）
+ main_layout.addWidget(splitter, stretch=1)
+
+ # 创建并添加数值显示面板，设置拉伸因子为0（不可伸缩）
+ self.value_display_panel = self.create_value_display_panel()
+ main_layout.addWidget(self.value_display_panel, stretch=0)
+
+ # 初始更新数值显示
+ self.update_value_display()
+
+ def create_joint_control_panel(self):
+ """创建关节控制面板"""
+ panel = QWidget()
+ layout = QVBoxLayout(panel)
+
+ # 创建标题
+ title_label = QLabel(f"关节控制 - {self.hand_joint}")
+ title_label.setFont(QFont("Microsoft YaHei", 14, QFont.Bold))
+ layout.addWidget(title_label)
+
+ # 创建滑动条滚动区域
+ scroll_area = QScrollArea()
+ scroll_area.setWidgetResizable(True)
+ scroll_area.setFrameShape(QFrame.NoFrame)
+
+ scroll_content = QWidget()
+ self.sliders_layout = QGridLayout(scroll_content)
+ self.sliders_layout.setSpacing(10)
+
+ # 创建滑动条
+ self.create_joint_sliders()
+
+ scroll_area.setWidget(scroll_content)
+ layout.addWidget(scroll_area)
+
+ return panel
+
+
+
+ def create_joint_sliders(self):
+ """创建关节滑动条"""
+ # 清除现有滑动条
+ for i in reversed(range(self.sliders_layout.count())):
+ item = self.sliders_layout.itemAt(i)
+ if item.widget():
+ item.widget().deleteLater()
+
+ # 创建新滑动条
+ self.sliders = []
+ self.slider_labels = []
+
+ for i, (name, value) in enumerate(zip(
+ self.hand_config.joint_names, self.hand_config.init_pos
+ )):
+ # 创建标签
+ label = QLabel(f"{name}: {value}")
+ label.setMinimumWidth(120)
+
+ # 创建滑动条
+ slider = QSlider(Qt.Horizontal)
+ slider.setRange(0, 255)
+ slider.setValue(value)
+ slider.valueChanged.connect(
+ lambda val, idx=i: self.on_slider_value_changed(idx, val)
+ )
+
+ # 添加到布局
+ row, col = divmod(i, 1)
+ self.sliders_layout.addWidget(label, row, 0)
+ self.sliders_layout.addWidget(slider, row, 1)
+
+ self.sliders.append(slider)
+ self.slider_labels.append(label)
+
+ def create_preset_actions_panel(self):
+ """创建预设动作面板"""
+ panel = QWidget()
+ layout = QVBoxLayout(panel)
+
+ # 自定义预设动作
+ sys_preset_group = QGroupBox("自定义预设动作(名称不可重复)")
+ sys_preset_layout = QGridLayout(sys_preset_group)
+ sys_preset_layout.setSpacing(8)
+
+ # 添加系统预设动作按钮
+ self.create_system_preset_buttons(sys_preset_layout)
+ layout.addWidget(sys_preset_group)
+
+ # 添加动作按钮
+ actions_layout = QHBoxLayout()
+
+ # 添加循环运行按钮
+ self.cycle_button = QPushButton("循环预设动作")
+ self.cycle_button.setProperty("category", "action")
+ self.cycle_button.clicked.connect(self.on_cycle_clicked)
+ actions_layout.addWidget(self.cycle_button)
+
+ self.home_button = QPushButton("回到初始位置")
+ self.home_button.setProperty("category", "action")
+ self.home_button.clicked.connect(self.on_home_clicked)
+ actions_layout.addWidget(self.home_button)
+
+ self.stop_button = QPushButton("停止所有动作")
+ self.stop_button.setProperty("category", "danger")
+ self.stop_button.clicked.connect(self.on_stop_clicked)
+ actions_layout.addWidget(self.stop_button)
+
+ layout.addLayout(actions_layout)
+
+ return panel
+
+ def create_system_preset_buttons(self, parent_layout):
+ """创建系统预设动作按钮"""
+ self.preset_buttons = [] # 清空按钮列表
+ if self.hand_config.preset_actions:
+ buttons = []
+ for idx, (name, positions) in enumerate(self.hand_config.preset_actions.items()):
+ button = QPushButton(name)
+ button.setProperty("category", "preset")
+ button.clicked.connect(
+ lambda checked, pos=positions: self.on_preset_action_clicked(pos)
+ )
+ buttons.append(button)
+ self.preset_buttons.append(button) # 保存按钮引用
+
+ # 添加到网格布局
+ cols = 2
+ for i, button in enumerate(buttons):
+ row, col = divmod(i, cols)
+ parent_layout.addWidget(button, row, col)
+
+ def create_status_monitor_panel(self):
+ """创建状态监控面板（速度/扭矩各占一行，并实时显示滑块值）"""
+ panel = QWidget()
+ layout = QVBoxLayout(panel)
+
+ # —— 1. 标题 ——
+ title_label = QLabel("状态监控")
+ title_label.setFont(QFont("Microsoft YaHei", 14, QFont.Bold))
+ layout.addWidget(title_label)
+
+ # —— 2. 新增：速度与扭矩设置（每行一个）——
+ quick_set_gb = QGroupBox("快速设置")
+ qv_layout = QVBoxLayout(quick_set_gb)
+
+ # 速度行
+ speed_hbox = QHBoxLayout()
+ speed_hbox.addWidget(QLabel("速度:"))
+ self.speed_slider = QSlider(Qt.Horizontal)
+ self.speed_slider.setRange(0, 255)
+ self.speed_slider.setValue(255)
+ self.speed_slider.setMinimumWidth(150)
+ speed_hbox.addWidget(self.speed_slider)
+ self.speed_val_lbl = QLabel("255") # 实时值
+ self.speed_val_lbl.setMinimumWidth(30)
+ speed_hbox.addWidget(self.speed_val_lbl)
+ self.speed_btn = QPushButton("设置速度")
+ self.speed_btn.clicked.connect(
+ lambda: (
+ self.ros_manager.publish_speed(self.speed_slider.value()),
+ self.status_updated.emit(
+ "info", f"速度已设为 {self.speed_slider.value()}")
+ ))
+ speed_hbox.addWidget(self.speed_btn)
+ speed_hbox.addStretch()
+ qv_layout.addLayout(speed_hbox)
+
+ # 扭矩行
+ torque_hbox = QHBoxLayout()
+ torque_hbox.addWidget(QLabel("扭矩:"))
+ self.torque_slider = QSlider(Qt.Horizontal)
+ self.torque_slider.setRange(0, 255)
+ self.torque_slider.setValue(255)
+ self.torque_slider.setMinimumWidth(150)
+ torque_hbox.addWidget(self.torque_slider)
+ self.torque_val_lbl = QLabel("255")
+ self.torque_val_lbl.setMinimumWidth(30)
+ torque_hbox.addWidget(self.torque_val_lbl)
+ self.torque_btn = QPushButton("设置扭矩")
+ self.torque_btn.clicked.connect(
+ lambda: (
+ self.ros_manager.publish_torque(self.torque_slider.value()),
+ self.status_updated.emit(
+ "info", f"扭矩已设为 {self.torque_slider.value()}")
+ ))
+ torque_hbox.addWidget(self.torque_btn)
+ torque_hbox.addStretch()
+ qv_layout.addLayout(torque_hbox)
+
+ layout.addWidget(quick_set_gb)
+
+ # —— 3. 原有标签页部分，完全不动 ——
+ tab_widget = QTabWidget()
+
+ # 系统信息标签页
+ sys_info_widget = QWidget()
+ sys_info_layout = QVBoxLayout(sys_info_widget)
+
+ conn_group = QGroupBox("连接状态")
+ conn_layout = QVBoxLayout(conn_group)
+ if self.ros_manager.publisher.get_subscription_count() > 0:
+ self.connection_status = QLabel("ROS2节点已连接")
+ self.connection_status.setObjectName("StatusLabel")
+ self.connection_status.setObjectName("StatusInfo")
+ else:
+ self.connection_status = QLabel("ROS2节点未连接")
+ self.connection_status.setObjectName("StatusLabel")
+ self.connection_status.setObjectName("StatusError")
+ conn_layout.addWidget(self.connection_status)
+
+ hand_info_group = QGroupBox("手部信息")
+ hand_info_layout = QVBoxLayout(hand_info_group)
+ info_text = f"""手部类型: {self.hand_type}
+关节型号: {self.hand_joint}
+关节数量: {len(self.hand_config.joint_names)}
+发布频率: {self.ros_manager.hz} Hz"""
+ self.hand_info_label = QLabel(info_text)
+ self.hand_info_label.setWordWrap(True)
+ hand_info_layout.addWidget(self.hand_info_label)
+
+ sys_info_layout.addWidget(conn_group)
+ sys_info_layout.addWidget(hand_info_group)
+ sys_info_layout.addStretch()
+ tab_widget.addTab(sys_info_widget, "系统信息")
+
+ # 状态日志标签页
+ log_widget = QWidget()
+ log_layout = QVBoxLayout(log_widget)
+ self.status_log = QLabel("等待系统启动...")
+ self.status_log.setObjectName("StatusLabel")
+ self.status_log.setObjectName("StatusInfo")
+ self.status_log.setWordWrap(True)
+ self.status_log.setMinimumHeight(300)
+ log_layout.addWidget(self.status_log)
+ clear_log_btn = QPushButton("清除日志")
+ clear_log_btn.clicked.connect(self.clear_status_log)
+ log_layout.addWidget(clear_log_btn)
+ tab_widget.addTab(log_widget, "状态日志")
+
+ layout.addWidget(tab_widget)
+
+ # —— 4. 实时更新滑块值 ——
+ self.speed_slider.valueChanged.connect(
+ lambda v: self.speed_val_lbl.setText(str(v)))
+ self.torque_slider.valueChanged.connect(
+ lambda v: self.torque_val_lbl.setText(str(v)))
+ return panel
+
+ def create_value_display_panel(self):
+ """创建滑动条数值显示面板"""
+ panel = QGroupBox("关节数值列表")
+ layout = QVBoxLayout(panel)
+
+ # 设置布局上下间隔为20像素
+ layout.setContentsMargins(10, 20, 10, 20)
+
+ self.value_display = QTextEdit()
+ self.value_display.setObjectName("ValueDisplay")
+ self.value_display.setReadOnly(True) # 设置只读模式，允许复制
+ self.value_display.setMinimumHeight(60) # 调整最小高度
+ self.value_display.setMaximumHeight(80) # 限制最大高度
+ self.value_display.setText("[]")
+
+ layout.addWidget(self.value_display)
+
+ return panel
+
+ def on_slider_value_changed(self, index: int, value: int):
+ """滑动条值改变事件处理"""
+ 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.update_value_display()
+
+ def update_value_display(self):
+ """更新数值显示面板内容"""
+ # 获取所有滑动条的当前值
+ values = [slider.value() for slider in self.sliders]
+
+ # 格式化显示为列表形式
+ self.value_display.setText(f"{values}")
+
+ def on_preset_action_clicked(self, positions: List[int]):
+ """预设动作按钮点击事件处理"""
+ if len(positions) != len(self.sliders):
+ QMessageBox.warning(
+ self, "动作不匹配",
+ f"预设动作关节数量({len(positions)})与当前关节数量({len(self.sliders)})不匹配"
+ )
+ return
+
+ # 更新滑动条
+ for i, (slider, pos) in enumerate(zip(self.sliders, positions)):
+ slider.setValue(pos)
+ self.on_slider_value_changed(i, pos)
+
+ # 发布关节状态
+ self.publish_joint_state()
+
+ def on_home_clicked(self):
+ """回到初始位置按钮点击事件处理"""
+ for slider, pos in zip(self.sliders, self.hand_config.init_pos):
+ slider.setValue(pos)
+
+ self.publish_joint_state()
+ self.status_updated.emit("info", "回到初始位置")
+
+ # 更新数值显示
+ self.update_value_display()
+
+ def on_stop_clicked(self):
+ """停止所有动作按钮点击事件处理"""
+ # 停止循环定时器
+ if self.cycle_timer and self.cycle_timer.isActive():
+ self.cycle_timer.stop()
+ self.cycle_timer = None
+ self.cycle_button.setText("循环运行预设动作")
+ self.reset_preset_buttons_color()
+
+ self.status_updated.emit("warning", "已停止所有动作")
+
+ def on_cycle_clicked(self):
+ """循环运行预设动作按钮点击事件处理"""
+ if not self.hand_config.preset_actions:
+ QMessageBox.warning(self, "无预设动作", "当前手部型号没有预设动作可循环运行")
+ return
+
+ if self.cycle_timer and self.cycle_timer.isActive():
+ # 停止循环
+ self.cycle_timer.stop()
+ self.cycle_timer = None
+ self.cycle_button.setText("循环运行预设动作")
+ self.reset_preset_buttons_color()
+ self.status_updated.emit("info", "已停止循环运行预设动作")
+ else:
+ # 开始循环
+ self.current_action_index = -1 # 重置索引
+ self.cycle_timer = QTimer(self)
+ self.cycle_timer.timeout.connect(self.run_next_action)
+ self.cycle_timer.start(LOOP_TIME) # 2秒间隔
+ self.cycle_button.setText("停止循环运行")
+ self.status_updated.emit("info", "开始循环运行预设动作")
+ self.run_next_action() # 立即运行第一个动作
+
+ def run_next_action(self):
+ """运行下一个预设动作"""
+ if not self.hand_config.preset_actions:
+ return
+
+ # 重置所有按钮颜色
+ self.reset_preset_buttons_color()
+
+ # 计算下一个动作索引
+ self.current_action_index = (self.current_action_index + 1) % len(self.hand_config.preset_actions)
+
+ # 获取下一个动作
+ action_names = list(self.hand_config.preset_actions.keys())
+ action_name = action_names[self.current_action_index]
+ action_positions = self.hand_config.preset_actions[action_name]
+
+ # 执行动作
+ self.on_preset_action_clicked(action_positions)
+
+ # 高亮当前动作按钮
+ if 0 <= self.current_action_index < len(self.preset_buttons):
+ button = self.preset_buttons[self.current_action_index]
+ button.setStyleSheet("background-color: green; color: white; border-color: #91D5FF;")
+
+ self.status_updated.emit("info", f"运行预设动作: {action_name}")
+
+ def reset_preset_buttons_color(self):
+ """重置所有预设按钮颜色"""
+ for button in self.preset_buttons:
+ button.setStyleSheet("") # 恢复默认样式
+ button.setProperty("category", "preset") # 恢复类别属性
+ # 强制样式刷新
+ button.style().unpolish(button)
+ button.style().polish(button)
+
+ def on_joint_type_changed(self, joint_type: str):
+ """关节类型改变事件处理"""
+ self.hand_joint = joint_type
+ self.hand_config = _HAND_CONFIGS[self.hand_joint]
+
+ # 更新手部信息
+ info_text = f"""手部类型: {self.hand_type}
+关节型号: {self.hand_joint}
+关节数量: {len(self.hand_config.joint_names)}
+发布频率: {self.ros_manager.hz} Hz"""
+ self.hand_info_label.setText(info_text)
+
+ # 重新创建滑动条和预设按钮
+ self.create_joint_sliders()
+ self.create_system_preset_buttons(self.sys_preset_layout) # 假设sys_preset_layout是类变量
+
+ # 更新数值显示
+ self.update_value_display()
+ self.status_updated.emit("info", f"已切换到手部型号: {joint_type}")
+
+ def publish_joint_state(self):
+ """发布当前关节状态"""
+ positions = [slider.value() for slider in self.sliders]
+ self.ros_manager.publish_joint_state(positions)
+
+ def update_status(self, status_type: str, message: str):
+ """更新状态显示"""
+ # 更新连接状态
+ if status_type == "info" and "ROS2节点初始化成功" in message:
+ self.connection_status.setText("ROS2节点已连接")
+ self.connection_status.setObjectName("StatusLabel")
+ self.connection_status.setObjectName("StatusInfo")
+
+ # 更新日志
+ current_time = time.strftime("%H:%M:%S")
+ log_entry = f"[{current_time}] {message}\n"
+ current_log = self.status_log.text()
+
+ if len(current_log) > 10000: # 限制日志长度
+ current_log = current_log[-10000:]
+
+ self.status_log.setText(log_entry + current_log)
+
+ # 设置日志样式
+ self.status_log.setObjectName("StatusLabel")
+ if status_type == "error":
+ self.status_log.setObjectName("StatusError")
+ else:
+ self.status_log.setObjectName("StatusInfo")
+
+ def clear_status_log(self):
+ """清除状态日志"""
+ self.status_log.setText("日志已清除")
+ self.status_log.setObjectName("StatusLabel")
+ self.status_log.setObjectName("StatusInfo")
+
+ def closeEvent(self, event):
+ """窗口关闭事件处理"""
+ if self.cycle_timer and self.cycle_timer.isActive():
+ self.cycle_timer.stop()
+ super().closeEvent(event)
+
+def main(args=None):
+ """主函数"""
+ try:
+ # 创建ROS2节点管理器
+ ros_manager = ROS2NodeManager()
+
+ # 创建Qt应用
+ app = QApplication(sys.argv)
+
+ # 创建GUI
+ window = HandControlGUI(ros_manager)
+
+ # 连接状态更新信号
+ ros_manager.status_updated.connect(window.update_status)
+ window.status_updated = ros_manager.status_updated
+
+ # 显示窗口
+ window.show()
+
+ # 运行应用
+ exit_code = app.exec_()
+
+ # 清理ROS2
+ if rclpy.ok():
+ ros_manager.node.destroy_node()
+ rclpy.shutdown()
+
+ sys.exit(exit_code)
+ except Exception as e:
+ print(f"应用程序启动失败: {str(e)}")
+ sys.exit(1)
+
+if __name__ == '__main__':
+ main()
diff --git a/linker_hand_ros2_sdk_ws/src/gui_control/gui_control/gui_control.py.ttk b/linker_hand_ros2_sdk_ws/src/gui_control/gui_control/gui_control.py.ttk
new file mode 100644
index 0000000..aa86c30
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/gui_control/gui_control/gui_control.py.ttk
@@ -0,0 +1,790 @@
+import sys
+import time
+import json
+import threading
+from dataclasses import dataclass
+from typing import List, Dict
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String, Header
+from sensor_msgs.msg import JointState
+import tkinter as tk
+from tkinter import ttk, scrolledtext, messagebox
+import tkinter.font as tkfont
+
+from .utils.mapping import *
+from .config.constants import _HAND_CONFIGS
+LOOP_TIME = 1000 # 循环动作间隔时间 毫秒
+class ROS2NodeManager:
+ """ROS2节点管理器，处理ROS通信"""
+
+ def __init__(self, node_name: str = "hand_control_node"):
+ self.node = None
+ self.publisher = None
+ self.joint_state = JointState()
+ self.joint_state.header = Header()
+ self.status_callbacks = []
+
+ # 初始化ROS2节点
+ self.init_node(node_name)
+
+ def add_status_callback(self, callback):
+ """添加状态回调函数"""
+ self.status_callbacks.append(callback)
+
+ def emit_status(self, status_type: str, message: str):
+ """发射状态信号"""
+ for callback in self.status_callbacks:
+ callback(status_type, message)
+
+ def init_node(self, node_name: str):
+ """初始化ROS2节点"""
+ try:
+ if not rclpy.ok():
+ rclpy.init(args=None)
+ self.node = Node(node_name)
+
+ # 声明参数
+ self.node.declare_parameter('hand_type', 'left')
+ self.node.declare_parameter('hand_joint', 'L10')
+ self.node.declare_parameter('topic_hz', 30)
+ self.node.declare_parameter('is_arc', False)
+
+ # 获取参数
+ self.hand_type = self.node.get_parameter('hand_type').value
+ self.hand_joint = self.node.get_parameter('hand_joint').value
+ self.hz = self.node.get_parameter('topic_hz').value
+ self.is_arc = self.node.get_parameter('is_arc').value
+
+ if self.is_arc == True:
+ # 创建发布者
+ self.publisher_arc = self.node.create_publisher(
+ JointState, f'/cb_{self.hand_type}_hand_control_cmd_arc', 10
+ )
+ # 创建发布者
+ self.publisher = self.node.create_publisher(
+ JointState, f'/cb_{self.hand_type}_hand_control_cmd', 10
+ )
+ # 新增 speed / torque 发布者
+ self.speed_pub = self.node.create_publisher(
+ String, f'/cb_hand_setting_cmd', 10)
+ self.torque_pub = self.node.create_publisher(
+ String, f'/cb_hand_setting_cmd', 10)
+ self.emit_status("info", f"ROS2节点初始化成功: {self.hand_type} {self.hand_joint}")
+
+ # 启动ROS2自旋线程
+ self.spin_thread = threading.Thread(target=self.spin_node, daemon=True)
+ self.spin_thread.start()
+ except Exception as e:
+ self.emit_status("error", f"ROS2初始化失败: {str(e)}")
+ raise
+
+ def spin_node(self):
+ """运行ROS2节点自旋循环"""
+ while rclpy.ok() and self.node:
+ rclpy.spin_once(self.node, timeout_sec=0.1)
+
+ def publish_joint_state(self, positions: List[int]):
+ """发布关节状态消息"""
+ if not self.publisher or not self.node:
+ self.emit_status("error", "ROS2发布者未初始化")
+ return
+
+ try:
+ self.joint_state.header.stamp = self.node.get_clock().now().to_msg()
+ self.joint_state.position = [float(pos) for pos in positions]
+ hand_config = _HAND_CONFIGS[self.hand_joint]
+ if len(hand_config.joint_names) == len(positions):
+ if hand_config.joint_names_en != None:
+ self.joint_state.name = hand_config.joint_names_en
+ else:
+ self.joint_state.name = hand_config.joint_names
+
+ self.publisher.publish(self.joint_state)
+ if self.is_arc == True:
+ if self.hand_joint == "O6":
+ if self.hand_type == "left":
+ pose = range_to_arc_left(positions,self.hand_joint)
+ elif self.hand_type == "right":
+ pose = range_to_arc_right(positions,self.hand_joint)
+ elif self.hand_joint == "L7" or self.hand_joint == "L21" or self.hand_joint == "L25":
+ if self.hand_type == "left":
+ pose = range_to_arc_left(positions,self.hand_joint)
+ elif self.hand_type == "right":
+ pose = range_to_arc_right(positions,self.hand_joint)
+ elif self.hand_joint == "L10":
+ if self.hand_type == "left":
+ pose = range_to_arc_left_10(positions)
+ elif self.hand_type == "right":
+ pose = range_to_arc_right_10(positions)
+ elif self.hand_joint == "L20":
+ if self.hand_type == "left":
+ pose = range_to_arc_left_l20(positions)
+ elif self.hand_type == "right":
+ pose = range_to_arc_right_l20(positions)
+ else:
+ print(f"当前{self.hand_joint} {self.hand_type}不支持弧度转换", flush=True)
+ self.joint_state.position = [float(pos) for pos in pose]
+ self.publisher_arc.publish(self.joint_state)
+ self.emit_status("info", "关节状态已发布")
+ except Exception as e:
+ self.emit_status("error", f"发布失败: {str(e)}")
+
+ def publish_speed(self, val: int):
+ joint_len = 0
+ if (self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6"):
+ joint_len = 6
+ elif self.hand_joint == "L7":
+ joint_len = 7
+ elif self.hand_joint == "L10":
+ joint_len = 10
+ else:
+ joint_len = 5
+ msg = String()
+ v = [val] * joint_len
+ data = {
+ "setting_cmd": "set_speed",
+ "params": {"hand_type":self.hand_type,"speed": v},
+ }
+ msg.data = json.dumps(data)
+ print(f"速度值：{v}", flush=True)
+ self.speed_pub.publish(msg)
+
+ def publish_torque(self, val: int):
+ joint_len = 0
+ if (self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6"):
+ joint_len = 6
+ elif self.hand_joint == "L7":
+ joint_len = 7
+ elif self.hand_joint == "L10":
+ joint_len = 10
+ else:
+ joint_len = 5
+ msg = String()
+ v = [val] * joint_len
+ data = {
+ "setting_cmd": "set_max_torque_limits",
+ "params": {"hand_type":self.hand_type,"torque": v},
+ }
+
+ msg.data = json.dumps(data)
+ print(f"扭矩值：{v}", flush=True)
+ self.torque_pub.publish(msg)
+
+ def shutdown(self):
+ """关闭ROS2节点"""
+ if self.node:
+ self.node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+class HandControlGUI:
+ """灵巧手控制界面"""
+
+ def __init__(self, ros_manager: ROS2NodeManager):
+ self.ros_manager = ros_manager
+ self.ros_manager.add_status_callback(self.update_status)
+
+ # 获取手部配置
+ 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.cycle_timer = None
+ self.current_action_index = -1
+ self.preset_buttons = []
+
+ # 初始化UI
+ self.init_ui()
+
+ # 设置定时器发布关节状态
+ self.publish_timer_id = None
+ self.start_publish_timer()
+
+ def init_ui(self):
+ """初始化用户界面"""
+ # 创建主窗口
+ self.root = tk.Tk()
+ self.root.title(f'灵巧手控制界面 - {self.hand_type} {self.hand_joint}')
+ self.root.geometry('1300x700')
+
+ # 设置样式
+ self.style = ttk.Style()
+ self.style.configure('TFrame', background='#f0f0f0')
+ self.style.configure('TLabel', background='#f0f0f0', font=('Microsoft YaHei', 10))
+ self.style.configure('Title.TLabel', font=('Microsoft YaHei', 14, 'bold'), foreground='#165DFF')
+ self.style.configure('Group.TLabelframe', borderwidth=2, relief='groove')
+ self.style.configure('Group.TLabelframe.Label', font=('Microsoft YaHei', 10, 'bold'), foreground='#165DFF')
+ # 配置信息框样式 - 灰色背景
+ self.style.configure('Info.TFrame', background='#e0e0e0')
+ # 配置高亮样式 - 滑动时的背景色
+ self.style.configure('Highlight.TFrame', background='#e6f7ff') # 浅蓝色背景
+
+ # 初始化当前点击按钮索引
+ self.current_clicked_button = None
+
+ # 创建主框架
+ main_frame = ttk.Frame(self.root)
+ main_frame.pack(fill=tk.BOTH, expand=True, padx=10, pady=10)
+
+ # 创建水平分割的框架
+ self.paned_window = ttk.PanedWindow(main_frame, orient=tk.HORIZONTAL)
+ self.paned_window.pack(fill=tk.BOTH, expand=True)
+
+ # 创建左侧关节控制面板
+ self.joint_control_panel = self.create_joint_control_panel()
+ self.paned_window.add(self.joint_control_panel, weight=5)
+
+ # 创建中间预设动作面板
+ self.preset_actions_panel = self.create_preset_actions_panel()
+ self.paned_window.add(self.preset_actions_panel, weight=3)
+
+ # 创建右侧状态监控面板
+ self.status_monitor_panel = self.create_status_monitor_panel()
+ self.paned_window.add(self.status_monitor_panel, weight=4)
+
+ # 创建底部数值显示面板
+ self.value_display_panel = self.create_value_display_panel()
+ self.value_display_panel.pack(fill=tk.X, pady=(10, 0))
+
+ # 绑定窗口大小变化事件
+ self.joint_control_panel.bind('', self.on_joint_panel_resize)
+
+ # 初始更新数值显示
+ self.update_value_display()
+
+ def create_joint_control_panel(self):
+ """创建关节控制面板"""
+ frame = ttk.Frame(self.root)
+ frame.config(width=600) # 设置最小宽度为400像素
+ frame.pack_propagate(False) # 阻止子控件改变框架大小
+ # 创建标题
+ title_label = ttk.Label(frame, text=f"关节控制 - {self.hand_joint}", style='Title.TLabel')
+ title_label.pack(pady=(0, 10))
+
+ # 创建滚动框架
+ self.canvas = tk.Canvas(frame, bg='#f0f0f0', highlightthickness=0)
+ self.scrollbar = ttk.Scrollbar(frame, orient=tk.VERTICAL, command=self.canvas.yview)
+ self.scrollable_frame = ttk.Frame(self.canvas)
+
+ self.scrollable_frame.bind(
+ "",
+ self.on_scrollable_frame_configure # 修改为调用方法
+ )
+
+ self.canvas.create_window((0, 0), window=self.scrollable_frame, anchor="nw")
+ self.canvas.configure(yscrollcommand=self.scrollbar.set)
+
+ # 创建滑动条
+ self.create_joint_sliders(self.scrollable_frame)
+
+ self.canvas.pack(side=tk.LEFT, fill=tk.BOTH, expand=True)
+ # 初始时不显示滚动条
+ self.scrollbar.pack(side=tk.RIGHT, fill=tk.Y)
+ self.scrollbar.pack_forget() # 隐藏滚动条
+
+ return frame
+ def on_scrollable_frame_configure(self, event):
+ """滚动区域配置变化事件 - 动态显示/隐藏滚动条"""
+ # 更新滚动区域
+ self.canvas.configure(scrollregion=self.canvas.bbox("all"))
+
+ # 检查是否需要显示滚动条
+ scrollable_height = self.scrollable_frame.winfo_reqheight() # 内容所需高度
+ canvas_height = self.canvas.winfo_height() # 画布实际高度
+
+ # 如果内容高度大于画布高度，显示滚动条；否则隐藏
+ if scrollable_height > canvas_height and canvas_height > 0:
+ self.scrollbar.pack(side=tk.RIGHT, fill=tk.Y)
+ else:
+ self.scrollbar.pack_forget()
+
+ def create_joint_sliders(self, parent):
+ """创建关节滑动条"""
+ self.sliders = []
+ self.slider_labels = []
+ self.slider_frames = []
+
+ # 存储父框架的引用，用于后续调整大小
+ self.sliders_parent = parent
+
+ for i, (name, value) in enumerate(zip(
+ self.hand_config.joint_names, self.hand_config.init_pos
+ )):
+ # 创建框架 - 添加左右边距
+ slider_frame = ttk.Frame(parent)
+ slider_frame.pack(fill=tk.X, pady=5, padx=(10,0)) # 左右各10像素边距
+ self.slider_frames.append(slider_frame)
+
+ # 创建标签
+ label = ttk.Label(slider_frame, text=f"{name}: {value}", width=15)
+ label.pack(side=tk.LEFT, padx=(0, 10))
+
+ # 创建滑动条 - 初始长度设为300，但会随窗口调整
+ slider = ttk.Scale(slider_frame, from_=0, to=255, value=value,
+ orient=tk.HORIZONTAL, length=300, # 初始长度
+ command=lambda val, idx=i: self.on_slider_value_changed(idx, val))
+ slider.pack(side=tk.LEFT, fill=tk.X, expand=True)
+
+ self.sliders.append(slider)
+ self.slider_labels.append(label)
+
+ def on_joint_panel_resize(self, event):
+ """关节控制面板大小变化事件"""
+ # 获取面板当前宽度
+ panel_width = event.width
+
+ # 为所有滑动条设置新的长度
+ for slider in self.sliders:
+ # 计算新的滑动条长度（面板宽度减去标签和其他元素的估计宽度）
+ # 标签宽度约120像素 + 左右边距各10像素 + 标签与滑动条间距10像素
+ new_length = max(panel_width - 150, 100) # 最小长度100像素
+ # 重新配置滑动条
+ slider.configure(length=new_length-30)
+ # 面板大小变化后重新检查是否需要滚动条
+ self.root.after(100, self.check_scrollbar_visibility) # 延迟检查，确保高度已更新
+
+ def check_scrollbar_visibility(self):
+ """检查滚动条可见性"""
+ # 只有在画布已经有实际高度时才检查
+ if self.canvas.winfo_height() > 0:
+ scrollable_height = self.scrollable_frame.winfo_reqheight()
+ canvas_height = self.canvas.winfo_height()
+
+ if scrollable_height > canvas_height:
+ self.scrollbar.pack(side=tk.RIGHT, fill=tk.Y)
+ else:
+ self.scrollbar.pack_forget()
+
+ def create_preset_actions_panel(self):
+ """创建预设动作面板"""
+ frame = ttk.Frame(self.root)
+
+ # 系统预设动作
+ sys_preset_group = ttk.LabelFrame(frame, text="系统预设", style='Group.TLabelframe')
+ sys_preset_group.pack(fill=tk.BOTH, expand=True, pady=(0, 10))
+
+ # 创建系统预设动作按钮 - 3列布局
+ self.create_system_preset_buttons(sys_preset_group)
+
+ # 动作按钮框架
+ actions_frame = ttk.Frame(frame)
+ actions_frame.pack(fill=tk.X, pady=10)
+
+ # 循环运行按钮
+ self.cycle_button = ttk.Button(actions_frame, text="循环预设动作",
+ command=self.on_cycle_clicked, width=12)
+ self.cycle_button.pack(side=tk.LEFT, padx=5)
+
+ # 回到初始位置按钮
+ self.home_button = ttk.Button(actions_frame, text="回到初始位置",
+ command=self.on_home_clicked, width=12)
+ self.home_button.pack(side=tk.LEFT, padx=5)
+
+ # 停止所有动作按钮
+ self.stop_button = ttk.Button(actions_frame, text="停止所有动作",
+ command=self.on_stop_clicked, width=12)
+ self.stop_button.pack(side=tk.LEFT, padx=5)
+
+ return frame
+
+ def create_system_preset_buttons(self, parent):
+ """创建系统预设动作按钮 - 3列布局"""
+ self.preset_buttons = []
+ if self.hand_config.preset_actions:
+ buttons_frame = ttk.Frame(parent)
+ buttons_frame.pack(fill=tk.BOTH, expand=True, padx=10, pady=10)
+
+ buttons = []
+ for idx, (name, positions) in enumerate(self.hand_config.preset_actions.items()):
+ # 使用 tk.Button 而不是 ttk.Button，以便更好地控制背景色
+ button = tk.Button(buttons_frame, text=name, bg='#E6F7FF', fg='#1890FF', width=15,
+ relief='raised', bd=1, font=('Microsoft YaHei', 10),
+ command=lambda pos=positions, btn_idx=idx: self.on_preset_action_clicked(pos, btn_idx))
+ buttons.append(button)
+ self.preset_buttons.append(button)
+
+ # 3列布局
+ cols = 3
+ for i, button in enumerate(buttons):
+ row, col = divmod(i, cols)
+ button.grid(row=row, column=col, sticky='ew', padx=5, pady=5)
+
+ # 配置列权重
+ for i in range(cols):
+ buttons_frame.columnconfigure(i, weight=1)
+
+ def create_status_monitor_panel(self):
+ """创建状态监控面板"""
+ frame = ttk.Frame(self.root)
+
+ # 标题
+ title_label = ttk.Label(frame, text="状态监控", style='Title.TLabel')
+ title_label.pack(pady=(0, 10))
+
+ # 快速设置框架
+ quick_set_group = ttk.LabelFrame(frame, text="快速设置", style='Group.TLabelframe')
+ quick_set_group.pack(fill=tk.X, pady=(0, 10))
+
+ # 速度设置
+ speed_frame = ttk.Frame(quick_set_group)
+ speed_frame.pack(fill=tk.X, padx=10, pady=5)
+
+ ttk.Label(speed_frame, text="速度:").pack(side=tk.LEFT)
+ self.speed_var = tk.IntVar(value=255)
+ self.speed_slider = ttk.Scale(speed_frame, from_=0, to=255,
+ variable=self.speed_var, orient=tk.HORIZONTAL)
+ self.speed_slider.pack(side=tk.LEFT, fill=tk.X, expand=True, padx=5)
+ self.speed_val_label = ttk.Label(speed_frame, text="255", width=4)
+ self.speed_val_label.pack(side=tk.LEFT)
+ self.speed_btn = ttk.Button(speed_frame, text="设置速度",
+ command=self.on_speed_set)
+ self.speed_btn.pack(side=tk.LEFT, padx=5)
+
+ # 扭矩设置
+ torque_frame = ttk.Frame(quick_set_group)
+ torque_frame.pack(fill=tk.X, padx=10, pady=5)
+
+ ttk.Label(torque_frame, text="扭矩:").pack(side=tk.LEFT)
+ self.torque_var = tk.IntVar(value=255)
+ self.torque_slider = ttk.Scale(torque_frame, from_=0, to=255,
+ variable=self.torque_var, orient=tk.HORIZONTAL)
+ self.torque_slider.pack(side=tk.LEFT, fill=tk.X, expand=True, padx=5)
+ self.torque_val_label = ttk.Label(torque_frame, text="255", width=4)
+ self.torque_val_label.pack(side=tk.LEFT)
+ self.torque_btn = ttk.Button(torque_frame, text="设置扭矩",
+ command=self.on_torque_set)
+ self.torque_btn.pack(side=tk.LEFT, padx=5)
+
+ # 绑定滑块值变化事件
+ self.speed_var.trace('w', self.on_speed_changed)
+ self.torque_var.trace('w', self.on_torque_changed)
+
+ # 创建标签页
+ notebook = ttk.Notebook(frame)
+ notebook.pack(fill=tk.BOTH, expand=True)
+
+ # 系统信息标签页
+ sys_info_frame = ttk.Frame(notebook)
+ notebook.add(sys_info_frame, text="系统信息")
+
+ # 连接状态 - 灰色背景，左对齐
+ conn_group = ttk.LabelFrame(sys_info_frame, text="连接状态", style='Group.TLabelframe')
+ conn_group.pack(fill=tk.X, pady=5)
+
+ # 创建灰色背景的框架
+ conn_content_frame = ttk.Frame(conn_group, style='Info.TFrame')
+ conn_content_frame.pack(fill=tk.X, padx=10, pady=10)
+
+ if self.ros_manager.publisher.get_subscription_count() > 0:
+ self.connection_status = ttk.Label(conn_content_frame, text="ROS2节点已连接",
+ foreground="green", background='#e0e0e0',
+ anchor='w') # 左对齐
+ else:
+ self.connection_status = ttk.Label(conn_content_frame, text="ROS2节点未连接",
+ foreground="red", background='#e0e0e0',
+ anchor='w') # 左对齐
+ self.connection_status.pack(fill=tk.X)
+
+ # 手部信息 - 灰色背景，左对齐
+ hand_info_group = ttk.LabelFrame(sys_info_frame, text="手部信息", style='Group.TLabelframe')
+ hand_info_group.pack(fill=tk.X, pady=5)
+
+ # 创建灰色背景的框架
+ hand_info_content_frame = ttk.Frame(hand_info_group, style='Info.TFrame')
+ hand_info_content_frame.pack(fill=tk.X, padx=10, pady=10)
+
+ info_text = f"""手部类型: {self.hand_type}
+关节型号: {self.hand_joint}
+关节数量: {len(self.hand_config.joint_names)}
+发布频率: {self.ros_manager.hz} Hz"""
+ self.hand_info_label = ttk.Label(hand_info_content_frame, text=info_text,
+ background='#e0e0e0', anchor='w', justify='left') # 左对齐
+ self.hand_info_label.pack(fill=tk.X)
+
+ # 状态日志标签页
+ log_frame = ttk.Frame(notebook)
+ notebook.add(log_frame, text="状态日志")
+
+ # 日志文本框
+ self.status_log = scrolledtext.ScrolledText(log_frame, height=15, width=50)
+ self.status_log.pack(fill=tk.BOTH, expand=True, padx=10, pady=10)
+ self.status_log.insert(tk.END, "等待系统启动...\n")
+ self.status_log.config(state=tk.DISABLED)
+
+ # 清除日志按钮
+ clear_log_btn = ttk.Button(log_frame, text="清除日志",
+ command=self.clear_status_log)
+ clear_log_btn.pack(pady=5)
+
+ return frame
+
+ def create_value_display_panel(self):
+ """创建滑动条数值显示面板"""
+ frame = ttk.LabelFrame(self.root, text="关节数值列表", style='Group.TLabelframe')
+
+ # 创建按钮框架（放在显示框上方）
+ button_frame = ttk.Frame(frame)
+ button_frame.pack(fill=tk.X, padx=10, pady=(10, 5))
+
+ # 复制按钮 - 左对齐
+ copy_button = ttk.Button(button_frame, text="复制到剪切板",
+ command=self.copy_values_to_clipboard, width=10)
+ copy_button.pack(side=tk.LEFT)
+
+ # 数值显示框
+ self.value_display = scrolledtext.ScrolledText(frame, height=4, width=100)
+ self.value_display.pack(fill=tk.BOTH, expand=True, padx=10, pady=(0, 10))
+ self.value_display.insert(tk.END, "[]")
+ self.value_display.config(state=tk.DISABLED)
+
+ return frame
+
+ def copy_values_to_clipboard(self):
+ """复制关节数值到系统剪切板"""
+ try:
+ # 获取当前按钮引用
+ button = self.root.focus_get()
+ original_text = "复制到剪切板"
+
+ # 获取文本框内容
+ content = self.value_display.get(1.0, tk.END).strip()
+
+ # 清除文本框的选中状态
+ self.value_display.tag_remove(tk.SEL, "1.0", tk.END)
+
+ # 复制到剪切板
+ self.root.clipboard_clear()
+ self.root.clipboard_append(content)
+
+ # 改变按钮文本提示复制成功
+ if isinstance(button, ttk.Button):
+ button.config(text="已复制!")
+ # 1.5秒后恢复原文本
+ self.root.after(1500, lambda: button.config(text=original_text))
+
+ self.update_status("info", f"关节数值已复制到剪切板")
+
+ except Exception as e:
+ self.update_status("error", f"复制失败: {str(e)}")
+
+ def on_slider_value_changed(self, index: int, value: str):
+ """滑动条值改变事件处理"""
+ value_int = int(float(value))
+ if 0 <= index < len(self.slider_labels):
+ joint_name = self.hand_config.joint_names[index]
+ self.slider_labels[index].config(text=f"{joint_name}: {value_int}")
+
+ # 更新数值显示
+ self.update_value_display()
+
+ def update_value_display(self):
+ """更新数值显示面板内容"""
+ values = [int(float(slider.get())) for slider in self.sliders]
+
+ self.value_display.config(state=tk.NORMAL)
+ self.value_display.delete(1.0, tk.END)
+ self.value_display.insert(tk.END, f"{values}")
+ self.value_display.config(state=tk.DISABLED)
+
+ def on_preset_action_clicked(self, positions: List[int], button_index: int = None):
+ """预设动作按钮点击事件处理"""
+ if len(positions) != len(self.sliders):
+ messagebox.showwarning(
+ "动作不匹配",
+ f"预设动作关节数量({len(positions)})与当前关节数量({len(self.sliders)})不匹配"
+ )
+ return
+
+ # 重置所有按钮颜色
+ self.reset_preset_buttons_color()
+
+ # 高亮当前点击的按钮
+ if button_index is not None and 0 <= button_index < len(self.preset_buttons):
+ self.preset_buttons[button_index].config(bg='#1890FF', fg='white')
+ self.current_clicked_button = button_index
+
+ # 更新滑动条
+ for i, (slider, pos) in enumerate(zip(self.sliders, positions)):
+ slider.set(pos)
+ self.on_slider_value_changed(i, str(pos))
+
+ # 发布关节状态
+ self.publish_joint_state()
+
+ def on_home_clicked(self):
+ """回到初始位置按钮点击事件处理"""
+ for slider, pos in zip(self.sliders, self.hand_config.init_pos):
+ slider.set(pos)
+
+ self.publish_joint_state()
+ self.update_status("info", "回到初始位置")
+
+ # 更新数值显示
+ self.update_value_display()
+
+ def on_stop_clicked(self):
+ """停止所有动作按钮点击事件处理"""
+ # 停止循环定时器
+ if self.cycle_timer:
+ self.root.after_cancel(self.cycle_timer)
+ self.cycle_timer = None
+ self.cycle_button.config(text="循环运行预设动作")
+ self.reset_preset_buttons_color()
+
+ self.update_status("warning", "已停止所有动作")
+
+ def on_cycle_clicked(self):
+ """循环运行预设动作按钮点击事件处理"""
+ if not self.hand_config.preset_actions:
+ messagebox.showwarning("无预设动作", "当前手部型号没有预设动作可循环运行")
+ return
+
+ if self.cycle_timer:
+ # 停止循环
+ self.root.after_cancel(self.cycle_timer)
+ self.cycle_timer = None
+ self.cycle_button.config(text="循环运行预设动作")
+ self.reset_preset_buttons_color()
+ # 如果有之前点击的按钮，恢复其点击状态
+ if hasattr(self, 'current_clicked_button') and self.current_clicked_button is not None:
+ if 0 <= self.current_clicked_button < len(self.preset_buttons):
+ self.preset_buttons[self.current_clicked_button].config(bg='#1890FF', fg='white')
+ self.update_status("info", "已停止循环运行预设动作")
+ else:
+ # 开始循环
+ self.current_action_index = -1
+ self.cycle_button.config(text="停止循环运行")
+ self.update_status("info", "开始循环运行预设动作")
+ self.run_next_action()
+
+ def run_next_action(self):
+ """运行下一个预设动作"""
+ if not self.hand_config.preset_actions:
+ return
+
+ # 重置所有按钮颜色
+ self.reset_preset_buttons_color()
+
+ # 计算下一个动作索引
+ self.current_action_index = (self.current_action_index + 1) % len(self.hand_config.preset_actions)
+
+ # 获取下一个动作
+ action_names = list(self.hand_config.preset_actions.keys())
+ action_name = action_names[self.current_action_index]
+ action_positions = self.hand_config.preset_actions[action_name]
+
+ # 执行动作
+ self.on_preset_action_clicked(action_positions, self.current_action_index)
+
+ # 高亮当前动作按钮（循环模式使用不同的颜色）
+ if 0 <= self.current_action_index < len(self.preset_buttons):
+ button = self.preset_buttons[self.current_action_index]
+ button.config(bg='#52C41A', fg='white') # 绿色表示循环中的按钮
+
+ self.update_status("info", f"运行预设动作: {action_name}")
+
+ # 设置下一个动作定时器
+ self.cycle_timer = self.root.after(LOOP_TIME, self.run_next_action)
+
+ def reset_preset_buttons_color(self):
+ """重置所有预设按钮颜色"""
+ for button in self.preset_buttons:
+ button.config(bg='#E6F7FF', fg='#1890FF') # 恢复默认颜色
+
+ def on_speed_changed(self, *args):
+ """速度滑块值改变事件"""
+ self.speed_val_label.config(text=str(self.speed_var.get()))
+
+ def on_torque_changed(self, *args):
+ """扭矩滑块值改变事件"""
+ self.torque_val_label.config(text=str(self.torque_var.get()))
+
+ def on_speed_set(self):
+ """设置速度"""
+ speed_val = self.speed_var.get()
+ self.ros_manager.publish_speed(speed_val)
+ self.update_status("info", f"速度已设为 {speed_val}")
+
+ def on_torque_set(self):
+ """设置扭矩"""
+ torque_val = self.torque_var.get()
+ self.ros_manager.publish_torque(torque_val)
+ self.update_status("info", f"扭矩已设为 {torque_val}")
+
+ def publish_joint_state(self):
+ """发布当前关节状态"""
+ positions = [int(float(slider.get())) for slider in self.sliders]
+ self.ros_manager.publish_joint_state(positions)
+
+ def start_publish_timer(self):
+ """开始发布定时器"""
+ self.publish_joint_state()
+ interval = int(1000 / self.ros_manager.hz)
+ self.publish_timer_id = self.root.after(interval, self.start_publish_timer)
+
+ def update_status(self, status_type: str, message: str):
+ """更新状态显示"""
+ # 更新连接状态
+ if status_type == "info" and "ROS2节点初始化成功" in message:
+ self.connection_status.config(text="ROS2节点已连接", foreground="green")
+
+ # 更新日志
+ current_time = time.strftime("%H:%M:%S")
+ log_entry = f"[{current_time}] {message}\n"
+
+ self.status_log.config(state=tk.NORMAL)
+ self.status_log.insert(tk.END, log_entry)
+ self.status_log.see(tk.END)
+
+ # 限制日志长度
+ log_content = self.status_log.get(1.0, tk.END)
+ if len(log_content) > 10000:
+ self.status_log.delete(1.0, f"{len(log_content)-10000}.0")
+
+ self.status_log.config(state=tk.DISABLED)
+
+ # 设置日志颜色
+ if status_type == "error":
+ # 可以在Tkinter中为不同消息类型添加颜色标记
+ pass
+
+ def clear_status_log(self):
+ """清除状态日志"""
+ self.status_log.config(state=tk.NORMAL)
+ self.status_log.delete(1.0, tk.END)
+ self.status_log.insert(tk.END, "日志已清除\n")
+ self.status_log.config(state=tk.DISABLED)
+
+ def run(self):
+ """运行GUI"""
+ try:
+ self.root.mainloop()
+ finally:
+ # 清理定时器
+ if self.cycle_timer:
+ self.root.after_cancel(self.cycle_timer)
+ if self.publish_timer_id:
+ self.root.after_cancel(self.publish_timer_id)
+ # 关闭ROS2节点
+ self.ros_manager.shutdown()
+
+def main(args=None):
+ """主函数"""
+ try:
+ # 创建ROS2节点管理器
+ ros_manager = ROS2NodeManager()
+
+ # 创建GUI
+ gui = HandControlGUI(ros_manager)
+
+ # 运行应用
+ gui.run()
+
+ except Exception as e:
+ print(f"应用程序启动失败: {str(e)}")
+ sys.exit(1)
+
+if __name__ == '__main__':
+ main()
diff --git a/linker_hand_ros2_sdk_ws/src/gui_control/gui_control/utils/color_msg.py b/linker_hand_ros2_sdk_ws/src/gui_control/gui_control/utils/color_msg.py
new file mode 100644
index 0000000..059d60a
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/gui_control/gui_control/utils/color_msg.py
@@ -0,0 +1,27 @@
+#! /usr/bin/env python3
+
+import time
+
+class ColorMsg():
+ def __init__(self,msg: str,color: str = '', timestamp: bool = True) -> None:
+ self.msg = msg
+ self.color = color
+ self.timestamp = timestamp
+ self.colorMsg(msg=self.msg, color=self.color, timestamp=self.timestamp)
+
+ def colorMsg(self,msg: str, color: str = '', timestamp: bool = True):
+ str = ""
+ if timestamp:
+ str += time.strftime('%Y-%m-%d %H:%M:%S',
+ time.localtime(time.time())) + " "
+ if color == "red":
+ str += "\033[1;31;40m"
+ elif color == "green":
+ str += "\033[1;32;40m"
+ elif color == "yellow":
+ str += "\033[1;33;40m"
+ else:
+ print(str + msg)
+ return
+ str += msg + "\033[0m"
+ print(str)
\ No newline at end of file
diff --git a/linker_hand_ros2_sdk_ws/src/gui_control/gui_control/utils/mapping.py b/linker_hand_ros2_sdk_ws/src/gui_control/gui_control/utils/mapping.py
new file mode 100644
index 0000000..deb6a19
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/gui_control/gui_control/utils/mapping.py
@@ -0,0 +1,383 @@
+#---------------------------------------------------------------------------------------------------
+# L6 L
+l6_l_min = [0, 0, 0, 0, 0, 0]
+l6_l_max = [0.99, 1.39, 1.26, 1.26, 1.26, 1.26]
+l6_l_derict = [-1, -1, -1, -1, -1, -1]
+# L6 R
+l6_r_min = [0, 0, 0, 0, 0, 0]
+l6_r_max = [0.99, 1.39, 1.26, 1.26, 1.26, 1.26]
+l6_r_derict = [-1, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+# O6 L
+o6_l_min = [0, 0, 0, 0, 0, 0]
+o6_l_max = [0.58, 1.36, 1.6, 1.6, 1.6, 1.6]
+o6_l_derict = [-1, -1, -1, -1, -1, -1]
+# O6 R
+o6_r_min = [0, 0, 0, 0, 0, 0]
+o6_r_max = [0.58, 1.36, 1.6, 1.6, 1.6, 1.6]
+o6_r_derict = [-1, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+# L7 L OK
+l7_l_min = [0, 0, 0, 0, 0, 0, 0]
+l7_l_max = [0.44, 1.43, 1.62, 1.62, 1.62, 1.62, 1.01]
+l7_l_derict = [-1, -1, -1, -1, -1, -1, -1]
+# L7 R OK (urdf后续会更改！！！)
+l7_r_min = [0, -1.43, 0, 0, 0, 0, 0]
+l7_r_max = [0.75, 0, 1.62, 1.62, 1.62, 1.62, 1.54]
+l7_r_derict = [-1, 0, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+# L10 L OK
+l10_l_min = [0, 0, 0, 0, 0, 0, 0, -0.26, -0.26, -0.52]
+l10_l_max = [1.45, 1.43, 1.62, 1.62, 1.62, 1.62, 0.26, 0, 0, 1.01]
+l10_l_derict = [-1, -1, -1, -1, -1, -1, 0, -1, -1, -1]
+# L10 R OK
+l10_r_min = [0, 0, 0, 0, 0, 0, -0.26, 0, 0, -0.52]
+l10_r_max = [0.75, 1.43, 1.62, 1.62, 1.62, 1.62, 0.21, 0.21, 0.34, 1.01]
+l10_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, -1]
+#---------------------------------------------------------------------------------------------------
+# L20 L OK
+l20_l_min = [0, 0, 0, 0, 0, -0.297, -0.26, -0.26, -0.26, -0.26, 0.122, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l20_l_max = [0.87, 1.4, 1.4, 1.4, 1.4, 0.683, 0.26, 0.26, 0.26, 0.26, 1.78, 0, 0, 0, 0, 1.29, 1.08, 1.08, 1.08, 1.08]
+l20_l_derict = [-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
+# L20 R OK
+l20_r_min = [0, 0, 0, 0, 0, -0.297, -0.26, -0.26, -0.26, -0.26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l20_r_max = [0.87, 1.4, 1.4, 1.4, 1.4, 0.683, 0.26, 0.26, 0.26, 0.26, 1.78, 0, 0, 0, 0, 1.29, 1.08, 1.08, 1.08, 1.08]
+l20_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+# L21 L OK
+l21_l_min = [0, 0, 0, 0, 0, 0, 0, -0.18, -0.18, 0, -0.6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l21_l_max = [1, 1.57, 1.57, 1.57, 1.57, 1.6, 0.18, 0.18, 0.18, 0.18, 0.6, 0, 0, 0, 0, 1.57, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57]
+l21_l_derict = [-1, -1, -1, -1, -1, -1, -1, -1, -1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
+# L21 R OK
+l21_r_min = [0, 0, 0, 0, 0, 0, -0.18, -0.18, -0.18, -0.18, -0.6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l21_r_max = [1, 1.57, 1.57, 1.57, 1.57, 1.6, 0.18, 0.18, 0.18, 0.18, 0.6, 0, 0, 0, 0, 1.57, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57]
+l21_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+#---------------------------------------------------------------------------------------------------
+# L25 L OK
+l25_l_min = [0, 0, 0, 0, 0, 0, -0.26, -0.26, -0.26, -0.26, -0.26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l25_l_max = [0.9, 1.57, 1.57, 1.57, 1.57, 1.3, 0.26, 0.26, 0.26, 0.26, 0.61, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57]
+l25_l_derict = [-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1]
+# L25 R OK
+l25_r_min = [0, 0, 0, 0, 0, 0, -0.26, -0.26, -0.26, -0.26, -0.26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l25_r_max = [0.9, 1.57, 1.57, 1.57, 1.57, 1.3, 0.26, 0.26, 0.26, 0.26, 0.61, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57]
+l25_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+
+def range_to_arc_left(left_range,hand_joint):
+ num=0
+ if hand_joint == "L6":
+ num = 6
+ l_min = l6_l_min
+ l_max = l6_l_max
+ l_derict = l6_l_derict
+ elif hand_joint == "O6":
+ num = 6
+ l_min = o6_l_min
+ l_max = o6_l_max
+ l_derict = o6_l_derict
+ elif hand_joint == "L7":
+ num = 7
+ l_min = l7_l_min
+ l_max = l7_l_max
+ l_derict = l7_l_derict
+ elif hand_joint == "L10":
+ num = 10
+ l_min = l10_l_min
+ l_max = l10_l_max
+ l_derict = l10_l_derict
+ elif hand_joint == "L20":
+ num = 20
+ l_min = l20_l_min
+ l_max = l20_l_max
+ l_derict = l20_l_derict
+ elif hand_joint == "L21":
+ num = 25
+ l_min = l21_l_min
+ l_max = l21_l_max
+ l_derict = l21_l_derict
+ hand_arc = [0] * num
+ for i in range(num):
+ if hand_joint == "L20":
+ if 11 <= i <= 14: continue
+ if hand_joint == "L21":
+ if 11 <= i <= 14: continue
+ if 16 <= i <= 19: continue
+ val_l = is_within_range(left_range[i], 0, 255)
+ if l_derict[i] == -1:
+ hand_arc[i] = scale_value(val_l, 0, 255, l_max[i], l_min[i])
+ else:
+ hand_arc[i] = scale_value(val_l, 0, 255, l_min[i], l_max[i])
+ return hand_arc
+
+def range_to_arc_right(right_range,hand_joint):
+ num=0
+ if hand_joint == "L6":
+ num = 6
+ r_min = l6_r_min
+ r_max = l6_r_max
+ r_derict = l6_r_derict
+ elif hand_joint == "O6":
+ num = 6
+ r_min = o6_r_min
+ r_max = o6_r_max
+ r_derict = o6_r_derict
+ elif hand_joint == "L7":
+ num = 7
+ r_min = l7_r_min
+ r_max = l7_r_max
+ r_derict = l7_r_derict
+ elif hand_joint == "L10":
+ num = 10
+ r_min = l10_r_min
+ r_max = l10_r_max
+ r_derict = l10_r_derict
+ elif hand_joint == "L20":
+ num = 20
+ r_min = l20_r_min
+ r_max = l20_r_max
+ r_derict = l20_r_derict
+ elif hand_joint == "L21":
+ num = 25
+ r_min = l21_r_min
+ r_max = l21_r_max
+ r_derict = l21_r_derict
+ hand_arc = [0] * num
+ for i in range(num):
+ if hand_joint == "L20":
+ if 11 <= i <= 14: continue
+ if hand_joint == "L21":
+ if 11 <= i <= 14: continue
+ if 16 <= i <= 19: continue
+ val_r = is_within_range(right_range[i], 0, 255)
+ if r_derict[i] == -1:
+ hand_arc[i] = scale_value(val_r, 0, 255, r_max[i], r_min[i])
+ else:
+ hand_arc[i] = scale_value(val_r, 0, 255, r_min[i], r_max[i])
+ return hand_arc
+
+'''
+def arc_to_range_left(left_arc,hand_joint):
+ num=0
+ if hand_joint == "L7":
+ num = 7
+ l_min = l7_l_min
+ l_max = l7_l_max
+ l_derict = l7_l_derict
+ elif hand_joint == "L10":
+ num = 10
+ l_min = l10_l_min
+ l_max = l10_l_max
+ l_derict = l10_l_derict
+ elif hand_joint == "L20":
+ num = 20
+ l_min = l20_l_min
+ l_max = l20_l_max
+ l_derict = l20_l_derict
+ elif hand_joint == "L21":
+ num = 25
+ l_min = l21_l_min
+ l_max = l21_l_max
+ l_derict = l21_l_derict
+ hand_range = [0] * num
+ for i in range(num):
+ if hand_joint == "L20":
+ if 11 <= i <= 14: continue
+ if hand_joint == "L21":
+ if 11 <= i <= 14: continue
+ if 16 <= i <= 19: continue
+ val_l = is_within_range(left_arc[i], 0, 255)
+ if l_derict[i] == -1:
+ hand_range[i] = scale_value(val_l, 0, 255, l_max[i], l_min[i])
+ else:
+ hand_range[i] = scale_value(val_l, 0, 255, l_min[i], l_max[i])
+ return hand_range
+ '''
+def arc_to_range_left(hand_arc_l,hand_joint):
+ num=0
+ if hand_joint == "O6":
+ num = 6
+ l_min = o6_l_min
+ l_max = o6_l_max
+ l_derict = o6_l_derict
+ elif hand_joint == "L7":
+ num = 7
+ l_min = l7_l_min
+ l_max = l7_l_max
+ l_derict = l7_l_derict
+ elif hand_joint == "L10":
+ num = 10
+ l_min = l10_l_min
+ l_max = l10_l_max
+ l_derict = l10_l_derict
+ elif hand_joint == "L20":
+ num = 20
+ l_min = l20_l_min
+ l_max = l20_l_max
+ l_derict = l20_l_derict
+ elif hand_joint == "L21":
+ num = 25
+ l_min = l21_l_min
+ l_max = l21_l_max
+ l_derict = l21_l_derict
+ hand_range = [0] * num
+ #hand_range_l = [0] * 7
+ for i in range(num):
+ if hand_joint == "L20":
+ if 11 <= i <= 14: continue
+ if hand_joint == "L21":
+ if 11 <= i <= 14: continue
+ if 16 <= i <= 19: continue
+ val_l = is_within_range(hand_arc_l[i], l_min[i], l_max[i])
+ if l_derict[i] == -1:
+ hand_range[i] = scale_value(val_l, l_min[i], l_max[i], 255, 0)
+ else:
+ hand_range[i] = scale_value(val_l, l_min[i], l_max[i], 0, 255)
+
+ return hand_range
+
+def arc_to_range_right(right_arc,hand_joint):
+ num=0
+ if hand_joint == "O6":
+ num = 6
+ r_min = o6_r_min
+ r_max = o6_r_max
+ r_derict = o6_r_derict
+ elif hand_joint == "L7":
+ num = 7
+ r_min = l7_r_min
+ r_max = l7_r_max
+ r_derict = l7_r_derict
+ elif hand_joint == "L10":
+ num = 10
+ r_min = l10_r_min
+ r_max = l10_r_max
+ r_derict = l10_r_derict
+ elif hand_joint == "L20":
+ num = 20
+ r_min = l20_r_min
+ r_max = l20_r_max
+ r_derict = l20_r_derict
+ elif hand_joint == "L21":
+ num = 25
+ r_min = l21_r_min
+ r_max = l21_r_max
+ r_derict = l21_r_derict
+ hand_range = [0] * num
+ for i in range(num):
+ if hand_joint == "L20":
+ if 11 <= i <= 14: continue
+ if hand_joint == "L21":
+ if 11 <= i <= 14: continue
+ if 16 <= i <= 19: continue
+ val_r = is_within_range(right_arc[i], r_min[i], r_max[i])
+ if r_derict[i] == -1:
+ hand_range[i] = scale_value(val_r, r_min[i], r_max[i], 255, 0)
+ else:
+ hand_range[i] = scale_value(val_r, r_min[i], r_max[i], 0, 255)
+ return hand_range
+
+
+
+
+def range_to_arc_right_l20(hand_range_r):
+ hand_arc_r = [0] * 20
+ for i in range(20):
+ if 11 <= i <= 14: continue
+ val_r = is_within_range(hand_range_r[i], 0, 255)
+ if l20_r_derict[i] == -1:
+ hand_arc_r[i] = scale_value(val_r, 0, 255, l20_r_max[i], l20_r_min[i])
+ else:
+ hand_arc_r[i] = scale_value(val_r, 0, 255, l20_r_min[i], l20_r_max[i])
+ return hand_arc_r
+
+
+def range_to_arc_left_l20(hand_range_l):
+ hand_arc_l = [0] * 20
+ for i in range(20):
+ if 11 <= i <= 14: continue
+ val_l = is_within_range(hand_range_l[i], 0, 255)
+ if l20_l_derict[i] == -1:
+ hand_arc_l[i] = scale_value(val_l, 0, 255, l20_l_max[i], l20_l_min[i])
+ else:
+ hand_arc_l[i] = scale_value(val_l, 0, 255, l20_l_min[i], l20_l_max[i])
+ return hand_arc_l
+
+
+def arc_to_range_right_l20(hand_arc_r):
+ hand_range_r = [0] * 20
+ for i in range(20):
+ if 11 <= i <= 14: continue
+ val_r = is_within_range(hand_arc_r[i], l20_r_min[i], l20_r_max[i])
+ if l20_r_derict[i] == -1:
+ hand_range_r[i] = scale_value(val_r, l20_r_min[i], l20_r_max[i], 255, 0)
+ else:
+ hand_range_r[i] = scale_value(val_r, l20_r_min[i], l20_r_max[i], 0, 255)
+ return hand_range_r
+
+
+def arc_to_range_left_l20(hand_arc_l):
+ hand_range_l = [0] * 20
+ for i in range(20):
+ if 11 <= i <= 14: continue
+ val_l = is_within_range(hand_arc_l[i], l20_l_min[i], l20_l_max[i])
+ if l20_l_derict[i] == -1:
+ hand_range_l[i] = scale_value(val_l, l20_l_min[i], l20_l_max[i], 255, 0)
+ else:
+ hand_range_l[i] = scale_value(val_l, l20_l_min[i], l20_l_max[i], 0, 255)
+
+ return hand_range_l
+
+
+def range_to_arc_right_10(hand_range_r):
+ hand_arc_r = [0] * 10
+ for i in range(10):
+ val_r = is_within_range(hand_range_r[i], 0, 255)
+ if l10_r_derict[i] == -1:
+ hand_arc_r[i] = scale_value(val_r, 0, 255, l10_r_max[i], l10_r_min[i])
+ else:
+ hand_arc_r[i] = scale_value(val_r, 0, 255, l10_r_min[i], l10_r_max[i])
+
+ return hand_arc_r
+
+
+def range_to_arc_left_10(hand_range_l):
+ hand_arc_l = [0] * 10
+ for i in range(10):
+ val_l = is_within_range(hand_range_l[i], 0, 255)
+ if l10_l_derict[i] == -1:
+ hand_arc_l[i] = scale_value(val_l, 0, 255, l10_l_max[i], l10_l_min[i])
+ else:
+ hand_arc_l[i] = scale_value(val_l, 0, 255, l10_l_min[i], l10_l_max[i])
+ return hand_arc_l
+
+
+def arc_to_range_right_10(hand_arc_r):
+ hand_range_r = [0] * 10
+ for i in range(10):
+ val_r = is_within_range(hand_arc_r[i], l10_r_min[i], l10_r_max[i])
+ if l10_r_derict[i] == -1:
+ hand_range_r[i] = scale_value(val_r, l10_r_min[i], l10_r_max[i], 255, 0)
+ else:
+ hand_range_r[i] = scale_value(val_r, l10_r_min[i], l10_r_max[i], 0, 255)
+ return hand_range_r
+
+
+def arc_to_range_left_10(hand_arc_l):
+ hand_range_l = [0] * 10
+ for i in range(10):
+ val_l = is_within_range(hand_arc_l[i], l10_l_min[i], l10_l_max[i])
+ if l10_l_derict[i] == -1:
+ hand_range_l[i] = scale_value(val_l, l10_l_min[i], l10_l_max[i], 255, 0)
+ else:
+ hand_range_l[i] = scale_value(val_l, l10_l_min[i], l10_l_max[i], 0, 255)
+
+ return hand_range_l
+
+
+def scale_value(original_value, a_min, a_max, b_min, b_max):
+ return (original_value - a_min) * (b_max - b_min) / (a_max - a_min) + b_min
+
+
+def is_within_range(value, min_value, max_value):
+ return min(max_value, max(min_value, value))
diff --git a/linker_hand_ros2_sdk_ws/src/gui_control/launch/gui_control.launch.py b/linker_hand_ros2_sdk_ws/src/gui_control/launch/gui_control.launch.py
new file mode 100644
index 0000000..8851cf7
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/gui_control/launch/gui_control.launch.py
@@ -0,0 +1,59 @@
+#!/usr/bin/env python3
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+ # 声明参数：是否显示压力图
+ declare_show_diagram = DeclareLaunchArgument(
+ 'show_pressure_diagram',
+ default_value='true',
+ description='是否启动压力图窗口: true | false'
+ )
+
+ # 根据 show_pressure_diagram 参数决定是否包含 pressure_diagram launch 文件
+ pressure_diagram_launch = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ PathJoinSubstitution([
+ FindPackageShare('pressure_diagram'),
+ 'launch',
+ 'pressure_diagram.launch.py'
+ ])
+ ),
+ condition=IfCondition(LaunchConfiguration('show_pressure_diagram'))
+ )
+
+ return LaunchDescription([
+ declare_show_diagram,
+ pressure_diagram_launch,
+ Node(
+ package='gui_control',
+ executable='gui_control',
+ name='left_hand_control_node',
+ output='screen',
+ parameters=[{
+ 'hand_type': 'left', # 配置Linker Hand灵巧手类型 left | right 字母为小写
+ 'hand_joint': "O6", # O6\L6\L7\L10\L20\G20(工业版)\L21 字母为大写
+ 'topic_hz': 30, # topic发布频率
+ 'is_touch': True, # 是否有压力传感器
+ 'is_arc': False, # 是否发布弧度值topic
+ }],
+ ),
+ # Node(
+ # package='gui_control',
+ # executable='gui_control',
+ # name='right_hand_control_node',
+ # output='screen',
+ # parameters=[{
+ # 'hand_type': 'right',
+ # 'hand_joint': "L10",
+ # 'topic_hz': 30,
+ # 'is_touch': True,
+ # }],
+ # ),
+ ])
diff --git a/linker_hand_ros2_sdk_ws/src/gui_control/package.xml b/linker_hand_ros2_sdk_ws/src/gui_control/package.xml
new file mode 100644
index 0000000..b444511
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/gui_control/package.xml
@@ -0,0 +1,21 @@
+
+
+
+ gui_control
+ 0.0.0
+ TODO: Package description
+ linker-robot
+ TODO: License declaration
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+
+ linker_hand_ros2_sdk
+
+
+ ament_python
+
+
+
diff --git a/linker_hand_ros2_sdk_ws/src/gui_control/pyproject.toml b/linker_hand_ros2_sdk_ws/src/gui_control/pyproject.toml
new file mode 100644
index 0000000..638dd9c
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/gui_control/pyproject.toml
@@ -0,0 +1,3 @@
+[build-system]
+requires = ["setuptools>=61.0"]
+build-backend = "setuptools.build_meta"
diff --git a/linker_hand_ros2_sdk_ws/src/gui_control/resource/gui_control b/linker_hand_ros2_sdk_ws/src/gui_control/resource/gui_control
new file mode 100644
index 0000000..e69de29
diff --git a/linker_hand_ros2_sdk_ws/src/gui_control/setup.cfg b/linker_hand_ros2_sdk_ws/src/gui_control/setup.cfg
new file mode 100644
index 0000000..4691687
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/gui_control/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/gui_control
+[install]
+install_scripts=$base/lib/gui_control
diff --git a/linker_hand_ros2_sdk_ws/src/gui_control/setup.py b/linker_hand_ros2_sdk_ws/src/gui_control/setup.py
new file mode 100644
index 0000000..9f7a812
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/gui_control/setup.py
@@ -0,0 +1,31 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+import os
+from glob import glob
+from setuptools import find_packages, setup
+package_name = 'gui_control'
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=find_packages(exclude=['test']),
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ (os.path.join('share', 'gui_control', 'launch'), glob('launch/*.launch.py')),
+ ('share/' + package_name, ['package.xml']),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='linker-robot',
+ maintainer_email='linker-robot@todo.todo',
+ description='TODO: Package description',
+ license='TODO: License declaration',
+ extras_require={
+ 'test': ['pytest'],
+ },
+ entry_points={
+ 'console_scripts': [
+ 'gui_control = gui_control.gui_control:main'
+ ],
+ },
+)
diff --git a/linker_hand_ros2_sdk_ws/src/gui_control/test/test_copyright.py b/linker_hand_ros2_sdk_ws/src/gui_control/test/test_copyright.py
new file mode 100644
index 0000000..97a3919
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/gui_control/test/test_copyright.py
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found errors'
diff --git a/linker_hand_ros2_sdk_ws/src/gui_control/test/test_flake8.py b/linker_hand_ros2_sdk_ws/src/gui_control/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/gui_control/test/test_flake8.py
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+ rc, errors = main_with_errors(argv=[])
+ assert rc == 0, \
+ 'Found %d code style errors / warnings:\n' % len(errors) + \
+ '\n'.join(errors)
diff --git a/linker_hand_ros2_sdk_ws/src/gui_control/test/test_pep257.py b/linker_hand_ros2_sdk_ws/src/gui_control/test/test_pep257.py
new file mode 100644
index 0000000..b234a38
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/gui_control/test/test_pep257.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found code style errors / warnings'
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/launch/linker_hand.launch.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/launch/linker_hand.launch.py
new file mode 100644
index 0000000..7c090a6
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/launch/linker_hand.launch.py
@@ -0,0 +1,21 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+ return LaunchDescription([
+ Node(
+ package='linker_hand_ros2_sdk',
+ executable='linker_hand_sdk',
+ name='linker_hand_sdk',
+ output='screen',
+ parameters=[{
+ 'hand_type': 'left', # 配置Linker Hand灵巧手类型 left | right 字母为小写
+ 'hand_joint': "O6", # O6\L6P\L6\L7\L10\L20\G20(工业版)\L21 字母为大写
+ 'is_touch': True, # 配置Linker Hand灵巧手是否有压力传感器 True | False
+ 'can': 'can0', # 这里需要修改为实际的CAN总线名称 如果是win系统则类似于 PCAN_USBBUS1。注：蓝色盒子为Linux下can0，WIN下位PCAN_USBBUS1。透明盒子Linux下为can0，WIN下为0
+ "modbus": "None" # "None" | "/dev/ttyUSB0" 这里需要修改为实际的Modbus总线名称 如果是win系统则 COM* Ubuntu则为/dev/ttyUSB* 注意添加sudo chmod 777 /dev/ttyUSB*权限
+ }],
+ ),
+ ])
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/launch/linker_hand_double.launch.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/launch/linker_hand_double.launch.py
new file mode 100644
index 0000000..4d81d46
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/launch/linker_hand_double.launch.py
@@ -0,0 +1,35 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+ return LaunchDescription([
+ Node(
+ package='linker_hand_ros2_sdk',
+ executable='linker_hand_sdk',
+ name='linker_hand_sdk_left',
+ output='screen',
+ parameters=[{
+ 'hand_type': 'left', # 配置Linker Hand灵巧手类型 left | right 字母为小写
+ 'hand_joint': "G20", # O6\L6P\L6\L7\L10\L20\G20(工业版)\L21 字母为大写
+ 'is_touch': True, # 配置Linker Hand灵巧手是否有压力传感器 True | False
+ 'can': 'can0', # 这里需要修改为实际的CAN总线名称 如果是win系统则类似于 PCAN_USBBUS1
+ "modbus": "None" # "None" | "/dev/ttyUSB0" 这里需要修改为实际的Modbus总线名称 如果是win系统则 COM* Ubuntu则为/dev/ttyUSB*
+ }],
+ ),
+
+ Node(
+ package='linker_hand_ros2_sdk',
+ executable='linker_hand_sdk',
+ name='linker_hand_sdk_right',
+ output='screen',
+ parameters=[{
+ 'hand_type': 'right', # 配置Linker Hand灵巧手类型 left | right 字母为小写
+ 'hand_joint': "G20", # O6\L6P\L6\L7\L10\L20\G20(工业版)\L21 字母为大写
+ 'is_touch': True, # 配置Linker Hand灵巧手是否有压力传感器 True | False
+ 'can': 'can1', # 这里需要修改为实际的CAN总线名称 如果是win系统则类似于 PCAN_USBBUS1
+ "modbus": "None" # "None" | "/dev/ttyUSB0" 这里需要修改为实际的Modbus总线名称 如果是win系统则 COM* Ubuntu则为/dev/ttyUSB*
+ }],
+ ),
+ ])
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/launch/test.bak b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/launch/test.bak
new file mode 100644
index 0000000..a4d8d10
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/launch/test.bak
@@ -0,0 +1,31 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+ return LaunchDescription([
+ Node(
+ package='linker_hand_ros2_sdk',
+ executable='linker_hand_sdk',
+ name='linker_hand_sdk_left',
+ output='screen',
+ parameters=[{
+ 'hand_type': 'left',
+ 'hand_joint': "L10", # 这里需要修改为实际Linker Hand的型号 L7、L10、L20、L21、L25
+ 'is_touch': True, # 是否带有压力传感器
+ 'can': 'can0', # 这里需要修改为实际的CAN总线名称
+ }],
+ ),
+
+ Node(
+ package='linker_hand_ros2_sdk',
+ executable='linker_hand_sdk',
+ name='linker_hand_sdk_right',
+ output='screen',
+ parameters=[{
+ 'hand_type': 'right',
+ 'hand_joint': "L10", # 这里需要修改为实际Linker Hand的型号 L7、L10、L20、L21、L25
+ 'is_touch': True, # 是否带有压力传感器
+ 'can': 'can0', # 这里需要修改为实际的CAN总线名称
+ }],
+ ),
+ ])
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/__init__.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L10_positions.yaml b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L10_positions.yaml
new file mode 100644
index 0000000..ea7439d
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L10_positions.yaml
@@ -0,0 +1,146 @@
+LEFT_HAND:
+- ACTION_NAME: 张开
+ POSITION:
+ - 255
+ - 70
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+- ACTION_NAME: 捏合5CM
+ POSITION:
+ - 165
+ - 70
+ - 165
+ - 165
+ - 255
+ - 255
+ - 113
+ - 255
+ - 255
+ - 88
+- ACTION_NAME: 捏合1CM
+ POSITION:
+ - 150
+ - 70
+ - 155
+ - 155
+ - 255
+ - 255
+ - 113
+ - 255
+ - 255
+ - 88
+- ACTION_NAME: 握3CM物品
+ POSITION:
+ - 113
+ - 70
+ - 85
+ - 85
+ - 85
+ - 85
+ - 85
+ - 255
+ - 255
+ - 88
+- ACTION_NAME: 准备抓握
+ POSITION:
+ - 255
+ - 70
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 121
+- ACTION_NAME: 拇指弯曲
+ POSITION:
+ - 35
+ - 140
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 30
+- ACTION_NAME: 食指弯曲
+ POSITION:
+ - 255
+ - 70
+ - 0
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+- ACTION_NAME: shishi
+ POSITION:
+ - 85
+ - 30
+ - 255
+ - 0
+ - 0
+ - 255
+ - 0
+ - 0
+ - 0
+ - 66
+RIGHT_HAND:
+- ACTION_NAME: 张开
+ POSITION:
+ - 255
+ - 70
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+- ACTION_NAME: 捏合5CM
+ POSITION:
+ - 165
+ - 70
+ - 165
+ - 165
+ - 255
+ - 255
+ - 113
+ - 255
+ - 255
+ - 88
+- ACTION_NAME: 捏合1CM
+ POSITION:
+ - 150
+ - 70
+ - 155
+ - 155
+ - 255
+ - 255
+ - 113
+ - 255
+ - 255
+ - 88
+- ACTION_NAME: 准备抓握
+ POSITION:
+ - 255
+ - 70
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 121
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L20_positions.yaml b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L20_positions.yaml
new file mode 100644
index 0000000..e69de29
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L21_positions.yaml b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L21_positions.yaml
new file mode 100644
index 0000000..a23101c
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L21_positions.yaml
@@ -0,0 +1,110 @@
+LEFT_HAND:
+- ACTION_NAME: 张开
+ POSITION:
+ - 96
+ - 255
+ - 255
+ - 255
+ - 255
+ - 150
+ - 114
+ - 151
+ - 189
+ - 255
+ - 180
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+- ACTION_NAME: 握拳
+ POSITION:
+ - 177
+ - 0
+ - 0
+ - 0
+ - 0
+ - 51
+ - 114
+ - 151
+ - 189
+ - 255
+ - 79
+ - 255
+ - 255
+ - 255
+ - 255
+ - 131
+ - 222
+ - 244
+ - 255
+ - 255
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+RIGHT_HAND:
+- ACTION_NAME: 张开
+ POSITION:
+ - 96
+ - 255
+ - 255
+ - 255
+ - 255
+ - 150
+ - 114
+ - 151
+ - 189
+ - 255
+ - 180
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+- ACTION_NAME: 握拳
+ POSITION:
+ - 230
+ - 80
+ - 51
+ - 42
+ - 7
+ - 35
+ - 114
+ - 151
+ - 189
+ - 255
+ - 58
+ - 255
+ - 255
+ - 255
+ - 255
+ - 133
+ - 5
+ - 0
+ - 0
+ - 0
+ - 30
+ - 0
+ - 0
+ - 0
+ - 0
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L25_positions.yaml b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L25_positions.yaml
new file mode 100644
index 0000000..709b0c3
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L25_positions.yaml
@@ -0,0 +1,83 @@
+LEFT_HAND:
+- ACTION_NAME: 张开
+ POSITION:
+ - 96
+ - 255
+ - 255
+ - 255
+ - 255
+ - 150
+ - 114
+ - 151
+ - 189
+ - 255
+ - 180
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+RIGHT_HAND:
+- ACTION_NAME: 张开
+ POSITION:
+ - 96
+ - 255
+ - 255
+ - 255
+ - 255
+ - 150
+ - 114
+ - 151
+ - 189
+ - 255
+ - 180
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+- ACTION_NAME: 握拳
+ POSITION:
+ - 230
+ - 80
+ - 51
+ - 42
+ - 7
+ - 35
+ - 114
+ - 151
+ - 189
+ - 255
+ - 58
+ - 255
+ - 255
+ - 255
+ - 255
+ - 133
+ - 5
+ - 0
+ - 0
+ - 0
+ - 30
+ - 0
+ - 0
+ - 0
+ - 0
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L6_positions.yaml b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L6_positions.yaml
new file mode 100644
index 0000000..9caaf4a
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L6_positions.yaml
@@ -0,0 +1,26 @@
+LEFT_HAND:
+- ACTION_NAME: 握拳
+ POSITION:
+ - 67
+ - 151
+ - 0
+ - 0
+ - 0
+ - 0
+- ACTION_NAME: 张开
+ POSITION:
+ - 255
+ - 179
+ - 255
+ - 255
+ - 255
+ - 255
+RIGHT_HAND:
+- ACTION_NAME: 张开
+ POSITION:
+ - 255
+ - 70
+ - 255
+ - 255
+ - 255
+ - 255
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L7_positions.yaml b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L7_positions.yaml
new file mode 100644
index 0000000..d573967
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L7_positions.yaml
@@ -0,0 +1,29 @@
+LEFT_HAND:
+- ACTION_NAME: 握拳
+ POSITION:
+ - 67
+ - 151
+ - 0
+ - 0
+ - 0
+ - 0
+ - 37
+- ACTION_NAME: 张开
+ POSITION:
+ - 255
+ - 179
+ - 255
+ - 255
+ - 255
+ - 255
+ - 83
+RIGHT_HAND:
+- ACTION_NAME: 张开
+ POSITION:
+ - 255
+ - 70
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/O6_positions.yaml b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/O6_positions.yaml
new file mode 100644
index 0000000..9caaf4a
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/O6_positions.yaml
@@ -0,0 +1,26 @@
+LEFT_HAND:
+- ACTION_NAME: 握拳
+ POSITION:
+ - 67
+ - 151
+ - 0
+ - 0
+ - 0
+ - 0
+- ACTION_NAME: 张开
+ POSITION:
+ - 255
+ - 179
+ - 255
+ - 255
+ - 255
+ - 255
+RIGHT_HAND:
+- ACTION_NAME: 张开
+ POSITION:
+ - 255
+ - 70
+ - 255
+ - 255
+ - 255
+ - 255
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/__init__.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/setting.yaml b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/setting.yaml
new file mode 100644
index 0000000..b7003cb
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/setting.yaml
@@ -0,0 +1,58 @@
+VERSION: 3.1.1 # 支持O6、L6在RS485模式
+LINKER_HAND: # 手部配置信息
+ LEFT_HAND:
+ EXISTS: True # 是否存在左手
+ TOUCH: True # 是否有压力传感器
+ CAN: "can0" # 配置CAN端口 默认can0 如果MODUBS不为"None",则CAN配置失效。 如果是win系统则类似于 PCAN_USBBUS1。注：蓝色盒子为Linux下can0，WIN下位PCAN_USBBUS1。透明盒子Linux下为can0，WIN下为0
+ MODBUS: "None" # 通讯协议是否为485 默认None 如果启动485，则是设备端口 /dev/ttyUSB* CAN配置失效 当前只支持O6/L6.后续版本正在努力增加中
+ JOINT: O6 # 左手型号 O6/L6/L7/L10/L20/G20/L21/L25/
+ NAME: # 默认值，不用修改
+ - joint41
+ - joint42
+ - joint43
+ - joint44
+ - joint45
+ - joint46
+ - joint47
+ - joint48
+ - joint49
+ - joint50
+ - joint51
+ - joint52
+ - joint53
+ - joint54
+ - joint55
+ - joint56
+ - joint57
+ - joint58
+ - joint59
+ - joint60
+
+ RIGHT_HAND:
+ EXISTS: False # 是否存在右手
+ TOUCH: False # 是否有压力传感器
+ CAN: "can0" # 配置CAN端口 默认can0 如果MODUBS不为"None",则CAN配置失效。 如果是win系统则类似于 PCAN_USBBUS1。注：蓝色盒子为Linux下can0，WIN下位PCAN_USBBUS1。透明盒子Linux下为can0，WIN下为0
+ MODBUS: "None" # 通讯协议是否为485 默认None 如果启动485，则是设备端口 /dev/ttyUSB* CAN配置失效 当前只支持O6/L6.后续版本正在努力增加中
+ JOINT: O6 # 右手型号 O6/L6/L7/L10/L20/G20/L21/L25/
+ NAME: # 默认值，不用修改
+ - joint71
+ - joint72
+ - joint73
+ - joint77
+ - joint75
+ - joint76
+ - joint77
+ - joint78
+ - joint79
+ - joint80
+ - joint81
+ - joint82
+ - joint83
+ - joint84
+ - joint88
+ - joint86
+ - joint87
+ - joint88
+ - joint89
+ - joint90
+PASSWORD: "12345678" # 由于与can通讯，需要激活通讯接口用到系统管理员密码。只有Linux系统需要，windows系统不需要。RS485不需要修改，RS485需要给/dev/ttyUSB* 777权限
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/__init__.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/__init__.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_g20_can.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_g20_can.py
new file mode 100644
index 0000000..9553aed
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_g20_can.py
@@ -0,0 +1,1269 @@
+#!/usr/bin/env python3
+import can
+import time, sys, os
+import threading
+import numpy as np
+from enum import Enum
+from utils.open_can import OpenCan
+from can.exceptions import CanError
+from utils.color_msg import ColorMsg
+current_dir = os.path.dirname(os.path.abspath(__file__))
+target_dir = os.path.abspath(os.path.join(current_dir, ".."))
+sys.path.append(target_dir)
+"""
+拇指41: [拇指侧摆, 拇指横摆, 拇指根部, 预留, 预留, 拇指尖部]
+食指42: [食指侧摆, 预留, 食指根部, 预留, 预留, 食指末端]
+中指43: [中指侧摆, 预留, 中指根部, 预留, 预留, 中指末端]
+无名指44: [无名指侧摆, 预留, 无名指根部, 预留, 预留, 无名指末端]
+小指45: [小指侧摆, 预留, 小指根部, 预留, 预留, 小指末端]
+"""
+CMD_MAP = [
+ "拇指根部",
+ "食指根部",
+ "中指根部",
+ "无名指根部",
+ "小指根部",
+ "拇指侧摆",
+ "食指侧摆",
+ "中指侧摆",
+ "无名指侧摆",
+ "小指侧摆",
+ "拇指横摆",
+ "预留",
+ "预留",
+ "预留",
+ "预留",
+ "拇指尖部",
+ "食指末端",
+ "中指末端",
+ "无名指末端",
+ "小指末端"
+]
+
+class FrameProperty(Enum):
+ # 手指运动控制 - 并联型控制指令（控制所有手指同一关节）
+ ROLL_POS = 0x01 # 横滚关节位置
+ YAW_POS = 0x02 # 航向关节位置
+ ROOT1_POS = 0x03 # 指根1关节位置
+ ROOT2_POS = 0x04 # 指根2关节位置
+ ROOT3_POS = 0x05 # 指根3关节位置
+ TIP_POS = 0x06 # 指尖关节位置
+
+ # 关节速度指令
+ ROLL_SPEED = 0x09 # 横滚关节速度
+ YAW_SPEED = 0x0A # 航向关节速度
+ ROOT1_SPEED = 0x0B # 指根1关节速度
+ ROOT2_SPEED = 0x0C # 指根2关节速度
+ ROOT3_SPEED = 0x0D # 指根3关节速度
+ TIP_SPEED = 0x0E # 指尖关节速度
+
+ # 关节扭矩指令
+ ROLL_TORQUE = 0x11 # 横滚关节扭矩
+ YAW_TORQUE = 0x12 # 航向关节扭矩
+ ROOT1_TORQUE = 0x13 # 指根1关节扭矩
+ ROOT2_TORQUE = 0x14 # 指根2关节扭矩
+ ROOT3_TORQUE = 0x15 # 指根3关节扭矩
+ TIP_TORQUE = 0x16 # 指尖关节扭矩
+
+ # 关节故障码
+ ROLL_FAULT = 0x19 # 横滚关节故障码
+ YAW_FAULT = 0x1A # 航向关节故障码
+ ROOT1_FAULT = 0x1B # 指根1关节故障码
+ ROOT2_FAULT = 0x1C # 指根2关节故障码
+ ROOT3_FAULT = 0x1D # 指根3关节故障码
+ TIP_FAULT = 0x1E # 指尖关节故障码
+
+ # 关节温度
+ ROLL_TEMPERATURE = 0x21 # 横滚关节过温保护阈值
+ YAW_TEMPERATURE = 0x22 # 航向关节过温保护阈值
+ ROOT1_TEMPERATURE = 0x23 # 指根1关节过温保护阈值
+ ROOT2_TEMPERATURE = 0x24 # 指根2关节过温保护阈值
+ ROOT3_TEMPERATURE = 0x25 # 指根3关节过温保护阈值
+ TIP_TEMPERATURE = 0x26 # 指尖关节过温保护阈值
+
+ # 手指运动控制 - 串联型控制指令（控制同一手指所有关节）
+ THUMB_POS = 0x41 # 大拇指指关节位置
+ INDEX_POS = 0x42 # 食指关节位置
+ MIDDLE_POS = 0x43 # 中指关节位置
+ RING_POS = 0x44 # 无名指关节位置
+ LITTLE_POS = 0x45 # 小拇指关节位置
+
+ # 手指速度
+ THUMB_SPEED = 0x49 # 大拇指速度
+ INDEX_SPEED = 0x4A # 食指速度
+ MIDDLE_SPEED = 0x4B # 中指速度
+ RING_SPEED = 0x4C # 无名指速度
+ LITTLE_SPEED = 0x4D # 小拇指速度
+
+ # 手指扭矩
+ THUMB_TORQUE = 0x51 # 大拇指扭矩
+ INDEX_TORQUE = 0x52 # 食指扭矩
+ MIDDLE_TORQUE = 0x53 # 中指扭矩
+ RING_TORQUE = 0x54 # 无名指扭矩
+ LITTLE_TORQUE = 0x55 # 小拇指扭矩
+
+ # 手指故障码
+ THUMB_FAULT = 0x59 # 大拇指故障码
+ INDEX_FAULT = 0x5A # 食指故障码
+ MIDDLE_FAULT = 0x5B # 中指故障码
+ RING_FAULT = 0x5C # 无名指故障码
+ LITTLE_FAULT = 0x5D # 小拇指故障码
+
+ # 手指温度
+ THUMB_TEMPERATURE = 0x61 # 大拇指过温保护阈值
+ INDEX_TEMPERATURE = 0x62 # 食指过温保护阈值
+ MIDDLE_TEMPERATURE = 0x63 # 中指过温保护阈值
+ RING_TEMPERATURE = 0x64 # 无名指过温保护阈值
+ LITTLE_TEMPERATURE = 0x65 # 小拇指过温保护阈值
+
+ # 手指运动控制 - 合并指令区域
+ FINGER_SPEED = 0x81 # 设置手指速度
+ FINGER_TORQUE = 0x82 # 设置手指输出扭矩
+ FINGER_FAULT = 0x83 # 清除手指故障及故障码
+ FINGER_TEMPERATURE = 0x84 # 手指各关节温度
+
+ # 指尖传感器数据
+ HAND_NORMAL_FORCE = 0x90 # 五指法向压力
+ HAND_TANGENTIAL_FORCE = 0x91 # 五指切向压力
+ HAND_TANGENTIAL_FORCE_DIR = 0x92 # 五指切向方向
+ HAND_APPROACH_INC = 0x93 # 五指接近感应
+
+ # 手指所有数据
+ THUMB_ALL_DATA = 0x98 # 大拇指所有数据
+ INDEX_ALL_DATA = 0x99 # 食指所有数据
+ MIDDLE_ALL_DATA = 0x9A # 中指所有数据
+ RING_ALL_DATA = 0x9B # 无名指所有数据
+ LITTLE_ALL_DATA = 0x9C # 小拇指所有数据
+
+ # 触觉传感器
+ TOUCH_SENSOR_TYPE = 0xB0 # 触觉传感器类型
+ THUMB_TOUCH = 0xB1 # 大拇指触觉传感
+ INDEX_TOUCH = 0xB2 # 食指触觉传感
+ MIDDLE_TOUCH = 0xB3 # 中指触觉传感
+ RING_TOUCH = 0xB4 # 无名指触觉传感
+ LITTLE_TOUCH = 0xB5 # 小拇指触觉传感
+ PALM_TOUCH = 0xB6 # 手掌指触觉传感
+
+ # 查询指令
+ HAND_UID_GET = 0xC0 # 唯一标识码查询
+ HAND_HARDWARE_VERSION_GET = 0xC1 # 硬件版本查询
+ HAND_SOFTWARE_VERSION_GET = 0xC2 # 软件版本查询
+ HAND_COMM_ID_GET = 0xC3 # 设备id查询
+ HAND_STRUCT_VERSION_GET = 0xC4 # 结构版本号查询
+
+ # 出厂指令
+ HOST_CMD_HAND_ERASE_POS_CALI = 0xCD # 擦除位置校准值
+ HAND_COMM_ID_SET = 0xD1 # 通信ID设置
+ HAND_UID_SET = 0xF0 # 唯一标识码设置
+
+class LinkerHandG20Can:
+ def __init__(self, can_channel='can0', baudrate=1000000, can_id=0x28, yaml=""):
+ self.can_id = can_id
+ self.can_channel = can_channel
+ self.baudrate = baudrate
+ self.open_can = OpenCan(load_yaml=yaml)
+
+ self.running = True
+
+ # 初始化数据存储变量
+ self.last_thumb_pos, self.last_index_pos, self.last_ring_pos, self.last_middle_pos, self.last_little_pos = None, None, None, None, None
+ self.last_root1, self.last_yaw, self.last_roll, self.last_root2, self.last_tip = None, None, None, None, None
+
+ # 并联控制数据存储
+ self.x01, self.x02, self.x03, self.x04, self.x05, self.x06 = [], [], [], [], [], []
+ self.x09, self.x0A, self.x0B, self.x0C, self.x0D, self.x0E = [], [], [], [], [], []
+ self.x11, self.x12, self.x13, self.x14, self.x15, self.x16 = [], [], [], [], [], []
+ self.x19, self.x1A, self.x1B, self.x1C, self.x1D, self.x1E = [], [], [], [], [], []
+ self.x21, self.x22, self.x23, self.x24, self.x25, self.x26 = [], [], [], [], [], []
+
+ # 串联控制数据存储
+ self.x41, self.x42, self.x43, self.x44, self.x45 = [], [], [], [], []
+ self.x49, self.x4A, self.x4B, self.x4C, self.x4D = [0] * 6, [0] * 6, [0] * 6, [0] * 6, [0] * 6
+ self.x51, self.x52, self.x53, self.x54, self.x55 = [], [], [], [], []
+ self.x59, self.x5A, self.x5B, self.x5C, self.x5D = [], [], [], [], []
+ self.x61, self.x62, self.x63, self.x64, self.x65 = [], [], [], [], []
+
+ # 合并指令区域数据存储
+ self.x81, self.x82, self.x83, self.x84 = [], [], [], []
+
+ # 传感器数据存储
+ self.x90, self.x91, self.x92, self.x93 = [], [], [], []
+ self.x98, self.x99, self.x9A, self.x9B, self.x9C = [], [], [], [], []
+ self.xB0, self.xB1, self.xB2, self.xB3, self.xB4, self.xB5, self.xB6 = [], [], [], [], [], [], []
+ self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 5 for _ in range(4)]
+
+ # 查询指令数据存储
+ self.xC0, self.xC1, self.xC2, self.xC3, self.xC4 = [], [], [], [], []
+ self.serial_number = []
+ self.serial_number_map = {
+ 0: 0,
+ 1: 1,
+ 2: 2,
+ 3: 3,
+ }
+ # 触觉传感器矩阵数据
+ self.thumb_matrix = np.full((12, 6), -1)
+ self.index_matrix = np.full((12, 6), -1)
+ self.middle_matrix = np.full((12, 6), -1)
+ self.ring_matrix = np.full((12, 6), -1)
+ self.little_matrix = np.full((12, 6), -1)
+ self.matrix_map = {
+ 0: 0, 16: 1, 32: 2, 48: 3, 64: 4, 80: 5,
+ 96: 6, 112: 7, 128: 8, 144: 9, 160: 10, 176: 11,
+ }
+ # 全掌触觉数据缓存
+ self.thumb_matrix_palm = np.full((23, 9), -1)
+ self.thumb_matrix_palm_tmp = []
+ self.thumb_matrix_palm_mass = [-1, -1, -1]
+
+ self.index_matrix_palm = np.full((23, 9), -1)
+ self.index_matrix_palm_tmp = []
+ self.index_matrix_palm_mass = [-1, -1, -1]
+
+ self.middle_matrix_palm = np.full((23, 9), -1)
+ self.middle_matrix_palm_tmp = []
+ self.middle_matrix_palm_mass = [-1, -1, -1]
+
+ self.ring_matrix_palm = np.full((23, 9), -1)
+ self.ring_matrix_palm_tmp = []
+ self.ring_matrix_palm_mass = [-1, -1, -1]
+
+ self.little_matrix_palm = np.full((23, 9), -1)
+ self.little_matrix_palm_tmp = []
+ self.little_matrix_palm_mass = [-1, -1, -1]
+
+ self.palm_matrix_palm = np.full((28, 20), -1)
+ self.palm_matrix_palm_tmp = []
+ self.palm_matrix_palm_mass = [-1, -1]
+
+ self.bus = self.init_can_bus(channel=self.can_channel, baudrate=baudrate)
+ # 启动接收线程
+ self.receive_thread = threading.Thread(target=self.receive_response)
+ self.receive_thread.daemon = True
+ self.receive_thread.start()
+ self._check_touch_type()
+
+ self.xB0 = self.get_touch_sensor_type() # 获取触觉传感器类型,如果返回值为5：TSSP_JZG(全手掌，指尖11x9,指中6x9,指根6x9,数据以23行9列形式返回，五指各有3个合力值。手掌20x28，手掌有两个合力值，分为上掌上半部分和下半部分。)
+
+ def _check_touch_type(self):
+ '''根据SN编码判断压感类型'''
+ self.sn = self.get_serial_number()
+ time.sleep(0.1)
+ if self.sn != "-1":
+ parts = self.sn.split("-")
+ if parts[4] == "A":
+ self.touch_type = 1
+ elif parts[4] == "B":
+ self.touch_type = 2
+ self.touch_code = 0xC6 # 6*12
+ elif parts[4] == "J":
+ self.touch_type = 3
+ elif parts[4] == "F":
+ self.touch_type = 4
+ self.touch_code = 0xA4 # 4*10
+ elif parts[4] == "Z":
+ self.touch_type = -1
+ else:
+ # 如果没有SN编码则根据返回数据进行判断
+ self.touch_type = self.get_touch_type()
+
+ def init_can_bus(self, channel, baudrate):
+ """
+ 尝试按优先级连接 CAN 总线，并实现回退机制。
+ """
+ # --- 统一异常处理块开始 ---
+ try:
+ if sys.platform == "linux":
+ # Linux 优先级：1. socketcan
+ try:
+ self.open_can.open_can(self.can_channel)
+ # 尝试 socketcan
+ bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan')
+ ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow")
+ raise # 重新抛出异常，让外层 try 捕获
+ elif sys.platform == "win32":
+ # Windows 优先级：1. pcan
+ try:
+ bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow")
+ # Windows 优先级：2. candle (回退方法)
+ try:
+ bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow")
+ raise # 两个接口都失败，抛出异常
+ else:
+ raise EnvironmentError("Unsupported platform for CAN interface")
+ # --- 统一异常处理块结束 ---
+ except Exception as e:
+ # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError）
+ ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red")
+ # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行
+ raise
+
+ def send_command(self, frame_property, data_list, sleep_time=0.003):
+ """
+ 发送指令到CAN总线
+ :param frame_property: 数据帧属性
+ :param data_list: 数据载荷
+ """
+ frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+ data = [frame_property_value] + [int(val) for val in data_list]
+ msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+ try:
+ self.bus.send(msg)
+ except can.CanError as e:
+ print(f"Failed to send message: {e}")
+ self.open_can.open_can(self.can_channel)
+ time.sleep(1)
+ self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel)
+ time.sleep(1)
+ if self.is_can:
+ self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate)
+ else:
+ print("Reconnecting CAN devices ....")
+ time.sleep(sleep_time)
+
+ def receive_response(self):
+ """
+ 接收并处理CAN总线响应消息
+ """
+ while self.running:
+ try:
+ msg = self.bus.recv(timeout=1.0)
+ if msg:
+ self.process_response(msg)
+ except can.CanError as e:
+ print(f"Error receiving message: {e}")
+
+ def process_response(self, msg):
+ """
+ 处理CAN响应消息
+ """
+ if msg.arbitration_id == self.can_id:
+ frame_type = msg.data[0]
+ response_data = msg.data[1:]
+ if len(list(response_data)) == 0:
+ return
+ # 并联控制指令响应
+ if frame_type == 0x01: self.x01 = list(response_data)
+ elif frame_type == 0x02: self.x02 = list(response_data)
+ elif frame_type == 0x03: self.x03 = list(response_data)
+ elif frame_type == 0x04: self.x04 = list(response_data)
+ elif frame_type == 0x05: self.x05 = list(response_data)
+ elif frame_type == 0x06: self.x06 = list(response_data)
+ elif frame_type == 0x09: self.x09 = list(response_data)
+ elif frame_type == 0x0A: self.x0A = list(response_data)
+ elif frame_type == 0x0B: self.x0B = list(response_data)
+ elif frame_type == 0x0C: self.x0C = list(response_data)
+ elif frame_type == 0x0D: self.x0D = list(response_data)
+ elif frame_type == 0x0E: self.x0E = list(response_data)
+ elif frame_type == 0x11: self.x11 = list(response_data)
+ elif frame_type == 0x12: self.x12 = list(response_data)
+ elif frame_type == 0x13: self.x13 = list(response_data)
+ elif frame_type == 0x14: self.x14 = list(response_data)
+ elif frame_type == 0x15: self.x15 = list(response_data)
+ elif frame_type == 0x16: self.x16 = list(response_data)
+ elif frame_type == 0x19: self.x19 = list(response_data)
+ elif frame_type == 0x1A: self.x1A = list(response_data)
+ elif frame_type == 0x1B: self.x1B = list(response_data)
+ elif frame_type == 0x1C: self.x1C = list(response_data)
+ elif frame_type == 0x1D: self.x1D = list(response_data)
+ elif frame_type == 0x1E: self.x1E = list(response_data)
+ elif frame_type == 0x21: self.x21 = list(response_data)
+ elif frame_type == 0x22: self.x22 = list(response_data)
+ elif frame_type == 0x23: self.x23 = list(response_data)
+ elif frame_type == 0x24: self.x24 = list(response_data)
+ elif frame_type == 0x25: self.x25 = list(response_data)
+ elif frame_type == 0x26: self.x26 = list(response_data)
+
+ # 串联控制指令响应
+ elif frame_type == 0x41: self.x41 = list(response_data)
+ elif frame_type == 0x42: self.x42 = list(response_data)
+ elif frame_type == 0x43: self.x43 = list(response_data)
+ elif frame_type == 0x44: self.x44 = list(response_data)
+ elif frame_type == 0x45: self.x45 = list(response_data)
+ elif frame_type == 0x49: self.x49 = list(response_data)
+ elif frame_type == 0x4A: self.x4A = list(response_data)
+ elif frame_type == 0x4B: self.x4B = list(response_data)
+ elif frame_type == 0x4C: self.x4C = list(response_data)
+ elif frame_type == 0x4D: self.x4D = list(response_data)
+ elif frame_type == 0x51: self.x51 = list(response_data)
+ elif frame_type == 0x52: self.x52 = list(response_data)
+ elif frame_type == 0x53: self.x53 = list(response_data)
+ elif frame_type == 0x54: self.x54 = list(response_data)
+ elif frame_type == 0x55: self.x55 = list(response_data)
+ elif frame_type == 0x59: self.x59 = list(response_data)
+ elif frame_type == 0x5A: self.x5A = list(response_data)
+ elif frame_type == 0x5B: self.x5B = list(response_data)
+ elif frame_type == 0x5C: self.x5C = list(response_data)
+ elif frame_type == 0x5D: self.x5D = list(response_data)
+ elif frame_type == 0x61: self.x61 = list(response_data)
+ elif frame_type == 0x62: self.x62 = list(response_data)
+ elif frame_type == 0x63: self.x63 = list(response_data)
+ elif frame_type == 0x64: self.x64 = list(response_data)
+ elif frame_type == 0x65: self.x65 = list(response_data)
+
+ # 合并指令区域响应
+ elif frame_type == 0x81: self.x81 = list(response_data)
+ elif frame_type == 0x82: self.x82 = list(response_data)
+ elif frame_type == 0x83: self.x83 = list(response_data)
+ elif frame_type == 0x84: self.x84 = list(response_data)
+
+ # 传感器数据响应
+ elif frame_type == 0x90: self.x90 = list(response_data)
+ elif frame_type == 0x91: self.x91 = list(response_data)
+ elif frame_type == 0x92: self.x92 = list(response_data)
+ elif frame_type == 0x93: self.x93 = list(response_data)
+ elif frame_type == 0x98: self.x98 = list(response_data)
+ elif frame_type == 0x99: self.x99 = list(response_data)
+ elif frame_type == 0x9A: self.x9A = list(response_data)
+ elif frame_type == 0x9B: self.x9B = list(response_data)
+ elif frame_type == 0x9C: self.x9C = list(response_data)
+
+ # 触觉传感器响应
+ elif frame_type == 0xB0: self.xB0 = list(response_data)
+ elif frame_type == 0xB1:
+ d = list(response_data)
+ if self.xB0[0] == 5:
+ """全掌矩阵"""
+ self.thumb_matrix_palm_tmp.append(d) # 将返回帧存储到缓存
+ if len(d) == 4 and d[0] == 22 and d[1] == 7: # 如果是最后一帧
+ self.thumb_matrix_palm = self.build_matrix(self.thumb_matrix_palm_tmp) # 将帧数据排列成23行9列矩阵
+ self.thumb_matrix_palm_tmp=[] # 重置缓存数据
+ if len(d) == 7 and d[0] == 255:
+ self.thumb_matrix_palm_mass = self.build_matrix_mass(d) # [指尖合力值, 指中合力值, 指根合力值]
+ else:
+ if len(d) == 2:
+ self.xB1 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.thumb_matrix[index] = d[1:]
+
+ elif frame_type == 0xB2:
+ d = list(response_data)
+ if self.xB0[0] == 5:
+ """全掌矩阵"""
+ self.index_matrix_palm_tmp.append(d) # 将返回帧存储到缓存
+ if len(d) == 4 and d[0] == 22 and d[1] == 7: # 如果是最后一帧
+ self.index_matrix_palm = self.build_matrix(self.index_matrix_palm_tmp) # 将帧数据排列成23行9列矩阵
+ self.index_matrix_palm_tmp=[] # 重置缓存数据
+ if len(d) == 7 and d[0] == 255:
+ self.index_matrix_palm_mass = self.build_matrix_mass(d) # [指尖合力值, 指中合力值, 指根合力值]
+ else:
+ if len(d) == 2:
+ self.xB2 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.index_matrix[index] = d[1:]
+ elif frame_type == 0xB3:
+ d = list(response_data)
+ if self.xB0[0] == 5:
+ """全掌矩阵"""
+ self.middle_matrix_palm_tmp.append(d) # 将返回帧存储到缓存
+ if len(d) == 4 and d[0] == 22 and d[1] == 7: # 如果是最后一帧
+ self.middle_matrix_palm = self.build_matrix(self.middle_matrix_palm_tmp) # 将帧数据排列成23行9列矩阵
+ self.middle_matrix_palm_tmp=[] # 重置缓存数据
+ if len(d) == 7 and d[0] == 255:
+ self.middle_matrix_palm_mass = self.build_matrix_mass(d) # [指尖合力值, 指中合力值, 指根合力值]
+ else:
+ if len(d) == 2:
+ self.xB3 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.middle_matrix[index] = d[1:]
+ elif frame_type == 0xB4:
+ d = list(response_data)
+ if self.xB0[0] == 5:
+ """全掌矩阵"""
+ self.ring_matrix_palm_tmp.append(d) # 将返回帧存储到缓存
+ if len(d) == 4 and d[0] == 22 and d[1] == 7: # 如果是最后一帧
+ self.ring_matrix_palm = self.build_matrix(self.ring_matrix_palm_tmp) # 将帧数据排列成23行9列矩阵
+ self.ring_matrix_palm_tmp=[] # 重置缓存数据
+ if len(d) == 7 and d[0] == 255:
+ self.ring_matrix_palm_mass = self.build_matrix_mass(d) # [指尖合力值, 指中合力值, 指根合力值]
+ else:
+ if len(d) == 2:
+ self.xB4 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.ring_matrix[index] = d[1:]
+ elif frame_type == 0xB5:
+ d = list(response_data)
+ if self.xB0[0] == 5:
+ """全掌矩阵"""
+ self.little_matrix_palm_tmp.append(d) # 将返回帧存储到缓存
+ if len(d) == 4 and d[0] == 22 and d[1] == 7: # 如果是最后一帧
+ self.little_matrix_palm = self.build_matrix(self.little_matrix_palm_tmp) # 将帧数据排列成23行9列矩阵
+ self.little_matrix_palm_tmp=[] # 重置缓存数据
+ if len(d) == 7 and d[0] == 255:
+ self.little_matrix_palm_mass = self.build_matrix_mass(d) # [指尖合力值, 指中合力值, 指根合力值]
+ else:
+ if len(d) == 2:
+ self.xB5 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.little_matrix[index] = d[1:]
+ elif frame_type == 0xB6:
+ d = list(response_data)
+ if self.xB0[0] == 5:
+ """全掌矩阵"""
+ self.palm_matrix_palm_tmp.append(d) # 将返回帧存储到缓存
+ if len(d) == 7 and d[0] == 27 and d[1] == 5: # 如果是最后一帧
+ self.palm_matrix_palm = self.build_matrix(self.palm_matrix_palm_tmp) # 将帧数据排列成23行9列矩阵
+ self.palm_matrix_palm_tmp=[] # 重置缓存数据
+ if len(d) == 5 and d[0] == 255:
+ self.palm_matrix_palm_mass = self.build_matrix_mass(d)
+ else:
+ self.xB6 = d
+
+
+ # 查询指令响应
+ # elif frame_type == 0xC0: self.xC0 = list(response_data)
+ elif frame_type == 0xC0:
+ d = list(response_data)
+ index = self.serial_number_map.get(d[0])
+ if index is not None:
+ self.serial_number=self.serial_number + d[1:]
+ else:
+ self.serial_number=self.serial_number + [-1] * 6
+ elif frame_type == 0xC1: self.xC1 = list(response_data)
+ elif frame_type == 0xC2: self.xC2 = list(response_data)
+ elif frame_type == 0xC3: self.xC3 = list(response_data)
+ elif frame_type == 0xC4: self.xC4 = list(response_data)
+
+
+
+ def build_matrix(self, data, r=23, c=9):
+ rows, cols = r, c
+ matrix = np.full((rows, cols), -1)
+
+ for item in data:
+ if len(item) == 4:
+ # 最后一行：从列坐标开始放
+ row, col, v1, v2 = item
+ start_col = col # 改为 col，而不是 col+1
+ values = [v1, v2]
+
+ cur_row, cur_col = row, start_col
+ for val in values:
+ if cur_col >= cols:
+ cur_row += 1
+ cur_col = 0
+ if cur_row < rows:
+ matrix[cur_row][cur_col] = val
+ cur_col += 1
+ break
+
+ elif len(item) == 7:
+ # 普通行：从列坐标开始放
+ row, col, v1, v2, v3, v4, v5 = item
+ start_col = col # 改为 col，而不是 col+1
+ values = [v1, v2, v3, v4, v5]
+
+ cur_row, cur_col = row, start_col
+ for val in values:
+ if cur_col >= cols:
+ cur_row += 1
+ cur_col = 0
+ if cur_row < rows:
+ matrix[cur_row][cur_col] = val
+ cur_col += 1
+
+ return matrix
+
+ def build_matrix_mass(self, hex_data):
+ """
+ 处理返回的和力值的帧数据,手指返回合力值长度为3，[指尖，指中，指根]
+ 手掌返回为长度为2。[上半部,下半部]
+ 解析 CAN 数据（支持 5 字节或 7 字节）
+
+ 参数:
+ hex_data: 十六进制列表，如 [0xFF, 0x27, 0x02, 0x51, 0x05, 0x20, 0x04] (7字节)
+ 或 [0xFF, 0x92, 0x09, 0xF8, 0x00] (5字节)
+
+ 返回:
+ (id, values) 其中 id 是 int，values 是包含 int 的列表（2个或3个）
+ """
+ if len(hex_data) not in [5, 7]:
+ raise ValueError(f"数据长度不支持，需要 5 或 7 字节，实际: {len(hex_data)}")
+
+ # 获取 ID（第一个字节）
+ can_id = hex_data[0]
+
+ # 计算有多少组数据（每组2字节）
+ data_bytes = hex_data[1:] # 去掉 ID
+ num_values = len(data_bytes) // 2
+
+ # 解析数据组（小端序）
+ values = []
+ for i in range(num_values):
+ low_byte = data_bytes[i*2] # 低位字节
+ high_byte = data_bytes[i*2 + 1] # 高位字节
+ # 小端拼接：低位 + 高位<<8
+ value = low_byte | (high_byte << 8)
+ values.append(value)
+
+ return values
+
+
+ # 并联控制指令方法
+ def set_roll_positions(self, joint_ranges):
+ """设置所有手指横滚关节位置"""
+ self.send_command(FrameProperty.ROLL_POS, joint_ranges)
+
+ def set_yaw_positions(self, joint_ranges):
+ """设置所有手指航向关节位置"""
+ self.send_command(FrameProperty.YAW_POS, joint_ranges)
+
+ def set_root1_positions(self, joint_ranges):
+ """设置所有手指指根1关节位置"""
+ self.send_command(FrameProperty.ROOT1_POS, joint_ranges)
+
+ def set_root2_positions(self, joint_ranges):
+ """设置所有手指指根2关节位置"""
+ self.send_command(FrameProperty.ROOT2_POS, joint_ranges)
+
+ def set_root3_positions(self, joint_ranges):
+ """设置所有手指指根3关节位置"""
+ self.send_command(FrameProperty.ROOT3_POS, joint_ranges)
+
+ def set_tip_positions(self, joint_ranges=[80]*5):
+ """设置所有手指指尖关节位置"""
+ self.send_command(FrameProperty.TIP_POS, joint_ranges)
+
+ # 串联控制指令方法
+ def set_thumb_positions(self, joint_ranges):
+ """设置大拇指所有关节位置"""
+ self.send_command(FrameProperty.THUMB_POS, joint_ranges)
+
+ def set_index_positions(self, joint_ranges):
+ """设置食指所有关节位置"""
+ self.send_command(FrameProperty.INDEX_POS, joint_ranges)
+
+ def set_middle_positions(self, joint_ranges):
+ """设置中指所有关节位置"""
+ self.send_command(FrameProperty.MIDDLE_POS, joint_ranges)
+
+ def set_ring_positions(self, joint_ranges):
+ """设置无名指所有关节位置"""
+ self.send_command(FrameProperty.RING_POS, joint_ranges)
+
+ def set_little_positions(self, joint_ranges):
+ """设置小拇指所有关节位置"""
+ self.send_command(FrameProperty.LITTLE_POS, joint_ranges)
+
+ # 扭矩设置方法
+ def set_thumb_torque(self, torque_values):
+ """设置大拇指扭矩"""
+ self.send_command(FrameProperty.THUMB_TORQUE, torque_values)
+
+ def set_index_torque(self, torque_values):
+ """设置食指扭矩"""
+ self.send_command(FrameProperty.INDEX_TORQUE, torque_values)
+
+ def set_middle_torque(self, torque_values):
+ """设置中指扭矩"""
+ self.send_command(FrameProperty.MIDDLE_TORQUE, torque_values)
+
+ def set_ring_torque(self, torque_values):
+ """设置无名指扭矩"""
+ self.send_command(FrameProperty.RING_TORQUE, torque_values)
+
+ def set_little_torque(self, torque_values):
+ """设置小拇指扭矩"""
+ self.send_command(FrameProperty.LITTLE_TORQUE, torque_values)
+
+ # 速度设置方法
+ def set_thumb_speed(self, speed_values):
+ """设置大拇指速度"""
+ self.send_command(FrameProperty.THUMB_SPEED, speed_values)
+
+ def set_index_speed(self, speed_values):
+ """设置食指速度"""
+ self.send_command(FrameProperty.INDEX_SPEED, speed_values)
+
+ def set_middle_speed(self, speed_values):
+ """设置中指速度"""
+ self.send_command(FrameProperty.MIDDLE_SPEED, speed_values)
+
+ def set_ring_speed(self, speed_values):
+ """设置无名指速度"""
+ self.send_command(FrameProperty.RING_SPEED, speed_values)
+
+ def set_little_speed(self, speed_values):
+ """设置小拇指速度"""
+ self.send_command(FrameProperty.LITTLE_SPEED, speed_values)
+
+ # 查询方法
+ def get_thumb_positions(self):
+ """获取大拇指所有关节当前位置"""
+ self.send_command(FrameProperty.THUMB_POS, [])
+ return self.x41
+
+ def get_index_positions(self):
+ """获取食指所有关节当前位置"""
+ self.send_command(FrameProperty.INDEX_POS, [])
+ return self.x42
+
+ def get_middle_positions(self):
+ """获取中指所有关节当前位置"""
+ self.send_command(FrameProperty.MIDDLE_POS, [])
+ return self.x43
+
+ def get_ring_positions(self):
+ """获取无名指所有关节当前位置"""
+ self.send_command(FrameProperty.RING_POS, [])
+ return self.x44
+
+ def get_little_positions(self):
+ """获取小拇指所有关节当前位置"""
+ self.send_command(FrameProperty.LITTLE_POS, [])
+ return self.x45
+
+
+ def get_thumb_speed(self):
+ """获取大拇指速度"""
+ self.send_command(FrameProperty.THUMB_SPEED, [])
+
+ def get_index_speed(self):
+ """获取食指速度"""
+ self.send_command(FrameProperty.INDEX_SPEED, [])
+
+ def get_middle_speed(self):
+ """获取中指速度"""
+ self.send_command(FrameProperty.MIDDLE_SPEED, [])
+
+ def get_ring_speed(self):
+ """获取无名指速度"""
+ self.send_command(FrameProperty.RING_SPEED, [])
+
+ def get_little_speed(self):
+ """获取小拇指速度"""
+ self.send_command(FrameProperty.LITTLE_SPEED, [])
+
+ def get_thumb_torque(self):
+ """获取大拇指扭矩"""
+ self.send_command(FrameProperty.THUMB_TORQUE, [])
+
+ def get_index_torque(self):
+ """获取食指扭矩"""
+ self.send_command(FrameProperty.INDEX_TORQUE, [])
+
+ def get_middle_torque(self):
+ """获取中指扭矩"""
+ self.send_command(FrameProperty.MIDDLE_TORQUE, [])
+
+ def get_ring_torque(self):
+ """获取无名指扭矩"""
+ self.send_command(FrameProperty.RING_TORQUE, [])
+
+ def get_little_torque(self):
+ """获取小拇指扭矩"""
+ self.send_command(FrameProperty.LITTLE_TORQUE, [])
+
+ def get_thumb_fault(self):
+ """获取大拇指所有关节故障码"""
+ self.send_command(FrameProperty.THUMB_FAULT, [])
+ return self.x59
+
+ def get_index_fault(self):
+ """获取食指所有关节故障码"""
+ self.send_command(FrameProperty.INDEX_FAULT, [])
+ return self.x5A
+
+ def get_middle_fault(self):
+ """获取中指所有关节故障码"""
+ self.send_command(FrameProperty.MIDDLE_FAULT, [])
+ return self.x5B
+
+ def get_ring_fault(self):
+ """获取无名指所有关节故障码"""
+ self.send_command(FrameProperty.RING_FAULT, [])
+ return self.x5C
+
+ def get_little_fault(self):
+ """获取小拇指所有关节故障码"""
+ self.send_command(FrameProperty.LITTLE_FAULT, [])
+ return self.x5D
+
+ def get_thumb_temperature(self):
+ """获取大拇指所有关节当前温度"""
+ self.send_command(FrameProperty.THUMB_TEMPERATURE, [])
+ return self.x61
+
+ def get_index_temperature(self):
+ """获取食指所有关节当前温度"""
+ self.send_command(FrameProperty.INDEX_TEMPERATURE, [])
+ return self.x62
+
+ def get_middle_temperature(self):
+ """获取中指所有关节当前温度"""
+ self.send_command(FrameProperty.MIDDLE_TEMPERATURE, [])
+ return self.x63
+
+ def get_ring_temperature(self):
+ """获取无名指所有关节当前温度"""
+ self.send_command(FrameProperty.RING_TEMPERATURE, [])
+ return self.x64
+
+ def get_little_temperature(self):
+ """获取小拇指所有关节当前温度"""
+ self.send_command(FrameProperty.LITTLE_TEMPERATURE, [])
+ return self.x65
+
+ # 合并指令区域方法
+ def set_finger_speed(self, speed_values):
+ """设置手指速度"""
+ self.send_command(FrameProperty.FINGER_SPEED, speed_values)
+
+ def set_finger_torque(self, torque_values):
+ """设置手指输出扭矩"""
+ self.send_command(FrameProperty.FINGER_TORQUE, torque_values)
+
+ def clear_finger_faults(self, finger_mask=[1, 1, 1, 1, 1]):
+ """清除手指故障及故障码"""
+ self.send_command(FrameProperty.FINGER_FAULT, finger_mask)
+ return self.x83
+
+ def get_finger_temperature(self):
+ """获取手指各关节温度"""
+ self.send_command(FrameProperty.FINGER_TEMPERATURE, [])
+ return self.x84
+
+ # 传感器数据获取方法
+ def get_normal_force(self):
+ """获取五指法向力"""
+ self.send_command(FrameProperty.HAND_NORMAL_FORCE, [])
+ return self.x90
+
+ def get_tangential_force(self):
+ """获取五指切向力"""
+ self.send_command(FrameProperty.HAND_TANGENTIAL_FORCE, [])
+ return self.x91
+
+ def get_tangential_force_dir(self):
+ """获取五指切向力方向"""
+ self.send_command(FrameProperty.HAND_TANGENTIAL_FORCE_DIR, [])
+ return self.x92
+
+ def get_approach_inc(self):
+ """获取五指接近感应"""
+ self.send_command(FrameProperty.HAND_APPROACH_INC, [])
+ return self.x93
+
+ def get_force(self):
+ '''Get pressure sensor data'''
+ return [self.x90,self.x91,self.x92,self.x93]
+
+ # 触觉传感器方法
+ def get_touch_sensor_type(self):
+ """获取触觉传感器类型 暂仅支持G20"""
+ self.send_command(FrameProperty.TOUCH_SENSOR_TYPE, [])
+ return self.xB0[0]
+
+ def get_thumb_touch(self):
+ """获取大拇指触觉传感数据"""
+ if self.xB0[0] == 5:
+ d = [23, 9, 1]
+ sleep_time = 0.015
+ else:
+ d = [0xC6]
+ sleep_time = 0.007
+ self.send_command(FrameProperty.THUMB_TOUCH, d, sleep_time=sleep_time)
+ #return self.thumb_matrix
+
+ def get_index_touch(self):
+ """获取食指触觉传感数据"""
+ if self.xB0[0] == 5:
+ d = [23, 9, 1]
+ sleep_time = 0.015
+ else:
+ d = [0xC6]
+ sleep_time = 0.007
+ self.send_command(FrameProperty.INDEX_TOUCH, d, sleep_time=sleep_time)
+ #return self.xB2
+
+ def get_middle_touch(self):
+ """获取中指触觉传感数据"""
+ if self.xB0[0] == 5:
+ d = [23, 9, 1]
+ sleep_time = 0.015
+ else:
+ d = [0xC6]
+ sleep_time = 0.007
+ self.send_command(FrameProperty.MIDDLE_TOUCH, d, sleep_time=sleep_time)
+ #33333return self.xB3
+
+ def get_ring_touch(self):
+ """获取无名指触觉传感数据"""
+ if self.xB0[0] == 5:
+ d = [23, 9, 1]
+ sleep_time = 0.015
+ else:
+ d = [0xC6]
+ sleep_time = 0.007
+ self.send_command(FrameProperty.RING_TOUCH, d, sleep_time=sleep_time)
+ #return self.xB4
+
+ def get_little_touch(self):
+ """获取小拇指触觉传感数据"""
+ if self.xB0[0] == 5:
+ d = [23, 9, 1]
+ sleep_time = 0.015
+ else:
+ d = [0xC6]
+ sleep_time = 0.007
+ self.send_command(FrameProperty.LITTLE_TOUCH, d, sleep_time=sleep_time)
+ #return self.xB5
+
+ def get_palm_touch(self):
+ """获取手掌触觉传感数据"""
+ if self.xB0[0] == 5:
+ d = [28, 20, 1]
+ sleep_time = 0.035
+ else:
+ d = [0xC6]
+ sleep_time = 0.007
+ self.send_command(FrameProperty.PALM_TOUCH, d, sleep_time=sleep_time)
+ #return self.xB6
+
+ # 查询指令方法
+ def get_uid(self):
+ """获取设备唯一标识码"""
+ self.send_command(FrameProperty.HAND_UID_GET, [])
+ return self.xC0
+
+ def get_hardware_version(self):
+ """获取硬件版本"""
+ self.send_command(FrameProperty.HAND_HARDWARE_VERSION_GET, [])
+ return self.xC1
+
+ def get_software_version(self):
+ """获取软件版本"""
+ self.send_command(FrameProperty.HAND_SOFTWARE_VERSION_GET, [])
+ return self.xC2
+
+ def get_comm_id(self):
+ """获取设备通信ID"""
+ self.send_command(FrameProperty.HAND_COMM_ID_GET, [])
+ return self.xC3
+
+ def get_struct_version(self):
+ """获取结构版本号"""
+ self.send_command(FrameProperty.HAND_STRUCT_VERSION_GET, [])
+ return self.xC4
+
+ # 出厂指令方法
+ def erase_position_calibration(self):
+ """擦除位置校准值"""
+ self.send_command(FrameProperty.HOST_CMD_HAND_ERASE_POS_CALI, [])
+
+ def set_comm_id(self, new_id):
+ """设置通信ID"""
+ self.send_command(FrameProperty.HAND_COMM_ID_SET, [new_id])
+
+ def set_uid(self, uid_data):
+ """设置唯一标识码（内部出厂使用）"""
+ self.send_command(FrameProperty.HAND_UID_SET, uid_data)
+
+ # 辅助方法
+ def slice_list(self, input_list, slice_size):
+ """将列表按指定大小切片"""
+ return [input_list[i:i + slice_size] for i in range(0, len(input_list), slice_size)]
+ # -----------------------------------------------------
+ # API指令区域
+ #------------------------------------------------------
+ def set_joint_positions(self, joint_ranges):
+ """API接口:设置手指所有关节位置"""
+ j = self.cmd_range_to_joint_range(cmd_list=joint_ranges)
+ self.set_thumb_positions(j[0])
+ self.set_index_positions(j[1])
+ self.set_middle_positions(j[2])
+ self.set_ring_positions(j[3])
+ self.set_little_positions(j[4])
+
+ def set_speed(self, speed=[250] * 5):
+ """API接口:设置手指速度"""
+ self.set_thumb_speed(speed_values=[speed[0]] * 6)
+ self.set_index_speed(speed_values=[speed[1]] * 6)
+ self.set_middle_speed(speed_values=[speed[2]] * 6)
+ self.set_ring_speed(speed_values=[speed[3]] * 6)
+ self.set_little_speed(speed_values=[speed[4]] * 6)
+
+ def set_torque(self, torque=[250] * 5):
+ """API接口:设置手指最大扭矩"""
+ self.set_thumb_torque(torque_values=[torque[0]] * 6)
+ self.set_index_torque(torque_values=[torque[1]] * 6)
+ self.set_middle_torque(torque_values=[torque[2]] * 6)
+ self.set_ring_torque(torque_values=[torque[3]] * 6)
+ self.set_little_torque(torque_values=[torque[4]] * 6)
+
+
+ def get_version(self):
+ """API接口:获取手指嵌入式版本信息"""
+ return self.get_software_version()
+
+ def get_current_status(self):
+ """API接口:获取手指当前状态"""
+ self.get_thumb_positions()
+ self.get_index_positions()
+ self.get_middle_positions()
+ self.get_ring_positions()
+ self.get_little_positions()
+ time.sleep(0.002)
+ s = [self.x41, self.x42, self.x43, self.x44, self.x45]
+ cmd_state = self.joint_state_to_cmd_state(state=s)
+ return cmd_state
+
+ def get_current_pub_status(self):
+ """API接口:获取手指当前状态"""
+ self.get_current_status()
+
+ def get_speed(self):
+ """API接口:获取手指速度"""
+ self.get_thumb_speed()
+ self.get_index_speed()
+ self.get_middle_speed()
+ self.get_ring_speed()
+ self.get_little_speed()
+ time.sleep(0.002)
+
+ joint_speed = [self.x49, self.x4A, self.x4B, self.x4C, self.x4D]
+ state_speed = self.joint_state_to_cmd_state(state=joint_speed)
+ return state_speed
+
+ def get_touch_type(self):
+ """API接口:获取手指触觉传感器类型"""
+ self.send_command(0xb0,[],sleep_time=0.03)
+ self.send_command(0xb1,[],sleep_time=0.03)
+ t = []
+ for i in range(3):
+ t = self.xB1
+ time.sleep(0.01)
+ if len(t) == 2:
+ return 2
+ else:
+ self.send_command(0x20,[],sleep_time=0.03)
+ time.sleep(0.01)
+ if self.normal_force[0] == -1:
+ return -1
+ else:
+ return 1
+
+ def get_matrix_touch(self):
+ """API接口:获取手指触摸传感器数据"""
+ self.get_thumb_touch()
+ self.get_index_touch()
+ self.get_middle_touch()
+ self.get_ring_touch()
+ self.get_little_touch()
+ return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+ def get_matrix_touch_v2(self):
+ """API接口:获取手指触摸传感器数据"""
+ return self.get_matrix_touch()
+
+ def get_thumb_matrix_touch(self,sleep_time=0):
+ """API接口:获取[大拇指]指触摸传感器数据"""
+ self.get_thumb_touch()
+ if self.xB0[0] == 5:
+ data = self.thumb_matrix_palm
+ else:
+ data = self.thumb_matrix
+ return data
+
+ def get_index_matrix_touch(self,sleep_time=0):
+ """API接口:获取[食指]指触摸传感器数据"""
+ self.get_index_touch()
+ if self.xB0[0] == 5:
+ data = self.index_matrix_palm
+ else:
+ data = self.index_matrix
+ return data
+
+ def get_middle_matrix_touch(self,sleep_time=0):
+ """API接口:获取[中指]指触摸传感器数据"""
+ self.get_middle_touch()
+ if self.xB0[0] == 5:
+ data = self.middle_matrix_palm
+ else:
+ data = self.middle_matrix
+ return data
+
+ def get_ring_matrix_touch(self,sleep_time=0):
+ """API接口:获取[无名指]指触摸传感器数据"""
+ self.get_ring_touch()
+ if self.xB0[0] == 5:
+ data = self.ring_matrix_palm
+ else:
+ data = self.ring_matrix
+ return data
+
+ def get_little_matrix_touch(self,sleep_time=0):
+ """API接口:获取[小指]指触摸传感器数据"""
+ self.get_little_touch()
+ if self.xB0[0] == 5:
+ data = self.little_matrix_palm
+ else:
+ data = self.little_matrix
+ return data
+
+ def get_palm_matrix_touch(self,sleep_time=0):
+ """API接口:获取[小指]指触摸传感器数据"""
+ self.get_palm_touch()
+ if self.xB0[0] == 5:
+ data = self.palm_matrix_palm
+ else:
+ data = self.palm_matrix
+ return data
+
+ def get_torque(self):
+ """API接口:获取手指最大扭矩"""
+ self.get_thumb_torque()
+ self.get_index_torque()
+ self.get_middle_torque()
+ self.get_ring_torque()
+ self.get_little_torque()
+ time.sleep(0.003)
+ t = [self.x51, self.x52, self.x53, self.x54, self.x55]
+ cmd_torque = self.joint_state_to_cmd_state(state=t)
+ return cmd_torque
+
+ def get_current(self):
+ """API接口:获取手指电流"""
+ return [-1] * 20
+
+ def get_temperature(self):
+ """API接口:获取手指温度"""
+ joint_temperature = [self.get_thumb_temperature(), self.get_index_temperature(), self.get_middle_temperature(), self.get_ring_temperature(), self.get_little_temperature()]
+ cmd_temperature = self.joint_state_to_cmd_state(state=joint_temperature)
+ return cmd_temperature
+
+
+ def get_fault(self):
+ """API接口:获取手指故障代码"""
+ joint_fault = [self.get_thumb_fault(), self.get_index_fault(), self.get_middle_fault(), self.get_ring_fault(), self.get_little_fault()]
+ cmd_fault = self.joint_state_to_cmd_state(state=joint_fault)
+ return cmd_fault
+
+ def clear_faults(self):
+ """API接口:清除手指故障代码"""
+ self.clear_finger_faults(finger_mask=[1, 1, 1, 1, 1])
+
+ def cmd_range_to_joint_range(self,cmd_list):
+ """根据手指映射关系，将手指控制命令列表转换为手指分组数据形式"""
+ # 定义手指映射规则
+ finger_mapping = {
+ '拇指': [10, 5, 0, 11, 12, 15],
+ '食指': [6, 11, 1, 13, 14, 16],
+ '中指': [7, 12, 2, 13, 14, 17],
+ '无名指': [8, 13, 3, 14, 15, 18],
+ '小指': [9, 14, 4, 15, 16, 19]
+ }
+
+ result = []
+
+ for finger, indices in finger_mapping.items():
+ finger_data = [cmd_list[i] for i in indices]
+ result.append(finger_data)
+
+ return result
+
+
+ def joint_state_to_cmd_state(self, state):
+ """
+ 将关节状态转换为命令状态
+ :param state: list2 格式的数据，5×6 的二维列表
+ :return: list1 格式的 20 维列表
+ """
+ # 初始化结果列表，20个位置，预留位默认为0
+ result = [0] * 20
+
+ # list1 索引映射:
+ # 0:拇指根部, 1:食指根部, 2:中指根部, 3:无名指根部, 4:小指根部
+ # 5:拇指侧摆, 6:食指侧摆, 7:中指侧摆, 8:无名指侧摆, 9:小指侧摆
+ # 10:拇指横摆, 11-14:预留, 15:拇指尖部, 16:食指末端, 17:中指末端, 18:无名指末端, 19:小指末端
+
+ # list2 每行结构: [侧摆/横摆, 0, 根部, 0, 0, 末端/尖部]
+ # 拇指行: [横摆, 侧摆, 根部, 0, 0, 尖部] — 注意拇指特殊，第1列是横摆，第2列是侧摆
+ # 其他指: [侧摆, 0, 根部, 0, 0, 末端]
+
+ # 拇指 (第0行) — 特殊处理
+ result[10] = state[0][0] # 拇指横摆
+ result[5] = state[0][1] # 拇指侧摆
+ result[0] = state[0][2] # 拇指根部
+ result[15] = state[0][5] # 拇指尖部
+
+ # 食指 (第1行)
+ result[6] = state[1][0] # 食指侧摆
+ result[1] = state[1][2] # 食指根部
+ result[16] = state[1][5] # 食指末端
+
+ # 中指 (第2行)
+ result[7] = state[2][0] # 中指侧摆
+ result[2] = state[2][2] # 中指根部
+ result[17] = state[2][5] # 中指末端
+
+ # 无名指 (第3行)
+ result[8] = state[3][0] # 无名指侧摆
+ result[3] = state[3][2] # 无名指根部
+ result[18] = state[3][5] # 无名指末端
+
+ # 小指 (第4行)
+ result[9] = state[4][0] # 小指侧摆
+ result[4] = state[4][2] # 小指根部
+ result[19] = state[4][5] # 小指末端
+
+ # 预留位 11-14 保持为 0
+
+ return result
+
+
+ def _list_d_value(self, list1, list2):
+ """检查两个列表的值是否有显著差异"""
+ if list1 is None:
+ return True
+ for a, b in zip(list1, list2):
+ if abs(b - a) > 2:
+ return True
+ return False
+
+ def close_can_interface(self):
+ """关闭CAN接口"""
+ if self.bus:
+ self.bus.shutdown()
+ self.running = False
+
+ def get_serial_number(self):
+ try:
+ self.send_command(0xC0,[],sleep_time=0.005)
+ # 1. 使用 bytes() 函数将整数列表转换为字节对象
+ # bytes() 接收一个由 0-255 之间的整数组成的列表。
+ byte_data = bytes(self.serial_number)
+ # 2. 使用 .decode() 方法将字节对象解码为 ASCII 字符串
+ result_string = byte_data.decode('ascii')
+ if result_string == "":
+ return "-1"
+ else:
+ # print(f"原始 ASCII 码列表: {self.serial_number}")
+ # print(f"解码后的字符串: {result_string}")
+ return result_string
+ except:
+ return "-1"
+ def get_finger_order(self):
+ return ["Thumb Base", "Index Finger Base", "Middle Finger Base", "Ring Finger Base", "Pinky Finger Base", "Thumb Abduction", "Index Finger Abduction", "Middle Finger Abduction", "Ring Finger Abduction", "Pinky Finger Abduction", "Thumb Horizontal Abduction", "Reserved", "Reserved", "Reserved", "Reserved", "Thumb Tip", "Index Finger Tip", "Middle Finger Tip", "Ring Finger Tip", "Pinky Finger Tip"]
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l10_can.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l10_can.py
new file mode 100644
index 0000000..f445a58
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l10_can.py
@@ -0,0 +1,531 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+import can
+import time,sys
+import threading
+import numpy as np
+#from tabulate import tabulate
+from enum import Enum
+from utils.open_can import OpenCan
+from utils.color_msg import ColorMsg
+from can.exceptions import CanError
+
+
+
+class FrameProperty(Enum):
+ INVALID_FRAME_PROPERTY = 0x00
+ JOINT_POSITION_RCO = 0x01
+ MAX_PRESS_RCO = 0x02
+ MAX_PRESS_RCO2 = 0x03
+ JOINT_POSITION2_RCO = 0x04
+ JOINT_SPEED = 0x05
+ JOINT_SPEED2 = 0x06
+ REQUEST_DATA_RETURN = 0x09
+ JOINT_POSITION_N = 0x11
+ MAX_PRESS_N = 0x12
+ HAND_NORMAL_FORCE = 0X20
+ HAND_TANGENTIAL_FORCE = 0X21
+ HAND_TANGENTIAL_FORCE_DIR = 0X22
+ HAND_APPROACH_INC = 0X23
+ MOTOR_TEMPERATURE_1 = 0x33
+ MOTOR_TEMPERATURE_2 = 0x34
+
+class LinkerHandL10Can:
+ def __init__(self,can_id, can_channel='can0', baudrate=1000000, yaml=""):
+ self.can_id = can_id
+ self.can_channel = can_channel
+ self.baudrate = baudrate
+ self.open_can = OpenCan(load_yaml=yaml)
+ self.is_cmd = False
+ self.x01 = [-1] * 5
+ self.x02 = [-1] * 5
+ self.x03 = [-1] * 5
+ self.x04 = [-1] * 5
+ self.x05 = [-1] * 5
+ self.x06 = [-1] * 5
+ self.x33 = self.x34 = [0] * 5
+ # Fault codes
+ self.x35,self.x36 = [0] * 5,[0] * 5
+ # New pressure sensors
+ self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+
+ self.thumb_matrix = np.full((12, 6), -1)
+ self.index_matrix = np.full((12, 6), -1)
+ self.middle_matrix = np.full((12, 6), -1)
+ self.ring_matrix = np.full((12, 6), -1)
+ self.little_matrix = np.full((12, 6), -1)
+ self.matrix_map = {
+ 0: 0,
+ 16: 1,
+ 32: 2,
+ 48: 3,
+ 64: 4,
+ 80: 5,
+ 96: 6,
+ 112: 7,
+ 128: 8,
+ 144: 9,
+ 160: 10,
+ 176: 11,
+ }
+ self.serial_number = []
+ self.serial_number_map = {
+ 0: 0,
+ 1: 1,
+ 2: 2,
+ 3: 3,
+ }
+ self.can_id = can_id
+ self.joint_angles = [0] * 10
+ self.pressures = [200] * 5 # Default torque 200
+ self.bus = self.init_can_bus(can_channel, baudrate)
+ self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 5 for _ in range(4)]
+ self.version = None
+ # Start receiving thread
+ self.running = True
+ self.receive_thread = threading.Thread(target=self.receive_response)
+ self.receive_thread.daemon = True
+ self.receive_thread.start()
+ self.version = self.get_version()
+
+ def init_can_bus(self, channel, baudrate):
+ """
+ 尝试按优先级连接 CAN 总线，并实现回退机制。
+ """
+ # --- 统一异常处理块开始 ---
+ try:
+ if sys.platform == "linux":
+ # Linux 优先级：1. socketcan
+ try:
+ self.open_can.open_can(self.can_channel)
+ # 尝试 socketcan
+ bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan')
+ ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow")
+ raise # 重新抛出异常，让外层 try 捕获
+ elif sys.platform == "win32":
+ # Windows 优先级：1. pcan
+ try:
+ bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow")
+ # Windows 优先级：2. candle (回退方法)
+ try:
+ bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow")
+ raise # 两个接口都失败，抛出异常
+ else:
+ raise EnvironmentError("Unsupported platform for CAN interface")
+ # --- 统一异常处理块结束 ---
+ except Exception as e:
+ # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError）
+ ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red")
+ # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行
+ raise
+
+ def send_frame(self, frame_property, data_list,sleep=0.002):
+ """Send a single CAN frame with specified properties and data."""
+ frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+ data = [frame_property_value] + [int(val) for val in data_list]
+ msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+ try:
+ self.bus.send(msg)
+ except can.CanError as e:
+ print(f"Failed to send message: {e}")
+ self.open_can.open_can(self.can_channel)
+ time.sleep(1)
+ self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel)
+ time.sleep(1)
+ if self.is_can:
+ self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate)
+ else:
+ print("Reconnecting CAN devices ....")
+ # time.sleep(1)
+ #
+ time.sleep(sleep)
+
+ def set_joint_positions(self, joint_angles):
+ """Set the positions of 10 joints (joint_angles: list of 10 values)."""
+ self.joint_angles = joint_angles
+ self.is_cmd = True
+ # Send angle control in frames, L10 protocol splits into first 6 and last 4
+ self.send_frame(FrameProperty.JOINT_POSITION2_RCO, self.joint_angles[6:])
+ #time.sleep(0.001)
+ self.send_frame(FrameProperty.JOINT_POSITION_RCO, self.joint_angles[:6])
+ #time.sleep(0.002)
+ self.is_cmd = False
+
+
+ def set_max_torque_limits(self, pressures,type="get"):
+ """Set maximum torque limits"""
+ if type == "get":
+ self.pressures = [0.0]
+ else:
+ self.pressures = pressures[:5]
+ #self.send_frame(FrameProperty.MAX_PRESS_RCO, self.pressures)
+
+
+ def set_joint_speed_l10(self,speed=[180]*5):
+ self.x05 = speed
+ for i in range(2):
+ time.sleep(0.01)
+ self.send_frame(0x05, speed)
+ def set_speed(self,speed=[180]*5):
+ if len(speed) == 5:
+ self.x05 = speed
+ for i in range(2):
+ time.sleep(0.01)
+ self.send_frame(0x05, speed)
+ elif len(speed) == 10:
+ for i in range(2):
+ time.sleep(0.01)
+ self.send_frame(0x05, speed[:5])
+ self.send_frame(0x06, speed[5:])
+ else:
+ raise ValueError("Speed list must have 10 elements.")
+ def request_all_status(self):
+ """Get all joint positions and pressures."""
+ self.send_frame(FrameProperty.REQUEST_DATA_RETURN, [])
+ ''' -------------------Pressure Sensors---------------------- '''
+ def get_normal_force(self):
+ self.send_frame(FrameProperty.HAND_NORMAL_FORCE,[],sleep=0.004)
+
+ def get_tangential_force(self):
+ self.send_frame(FrameProperty.HAND_TANGENTIAL_FORCE,[],sleep=0.004)
+
+ def get_tangential_force_dir(self):
+ self.send_frame(FrameProperty.HAND_TANGENTIAL_FORCE_DIR,[],sleep=0.004)
+ def get_approach_inc(self):
+ self.send_frame(FrameProperty.HAND_APPROACH_INC,[],sleep=0.004)
+ ''' -------------------Motor Temperature---------------------- '''
+ def get_motor_temperature(self):
+ self.send_frame(FrameProperty.MOTOR_TEMPERATURE_1,[],sleep=0.01)
+ self.send_frame(FrameProperty.MOTOR_TEMPERATURE_2,[],sleep=0.01)
+ # Motor fault codes
+ def get_motor_fault_code(self):
+ self.send_frame(0x35,[],sleep=0.1)
+ self.send_frame(0x36,[],sleep=0.1)
+ def receive_response(self):
+ """Receive CAN responses and process them."""
+ while self.running:
+ try:
+ msg = self.bus.recv(timeout=1.0)
+ if msg:
+ self.process_response(msg)
+ except can.CanError as e:
+ print(f"Error receiving CAN message: {e}")
+
+ def process_response(self, msg):
+ """Process received CAN messages."""
+ if msg.arbitration_id == self.can_id:
+ frame_type = msg.data[0]
+ response_data = msg.data[1:]
+ if len(list(response_data)) == 0:
+ return
+ if frame_type == FrameProperty.JOINT_POSITION_RCO.value: # 0x01
+ self.x01 = list(response_data)
+ elif frame_type == FrameProperty.MAX_PRESS_RCO.value: # 0x02
+ self.x02 = list(response_data)
+ elif frame_type == FrameProperty.MAX_PRESS_RCO2.value: # 0x03
+ self.x03 = list(response_data)
+ elif frame_type == FrameProperty.JOINT_POSITION2_RCO.value: # 0x04
+ self.x04 = list(response_data)
+ elif frame_type == 0x05:
+ self.x05 = list(response_data)
+ elif frame_type == 0x06:
+ self.x06 = list(response_data)
+ elif frame_type == 0x20:
+ # Five-finger normal force
+ d = list(response_data)
+ self.normal_force = [float(i) for i in d]
+ elif frame_type == 0x21:
+ # Five-finger tangential force
+ d = list(response_data)
+ self.tangential_force = [float(i) for i in d]
+ elif frame_type == 0x22:
+ # Five-finger tangential force direction
+ d = list(response_data)
+ self.tangential_force_dir = [float(i) for i in d]
+ elif frame_type == 0x23:
+ # Five-finger approach increment
+ d = list(response_data)
+ self.approach_inc = [float(i) for i in d]
+ elif frame_type == 0x33:
+ self.x33 = list(response_data)
+ elif frame_type == 0x34:
+ self.x34 = list(response_data)
+ elif frame_type == 0x35:
+ self.x35 = list(response_data)
+ elif frame_type == 0x36:
+ self.x36 = list(response_data)
+ elif frame_type == 0xb0:
+ self.xb0 = list(response_data)
+ elif frame_type == 0xb1:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb1 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.thumb_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb2:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb2 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.index_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb3:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb3 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.middle_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb4:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb4 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.ring_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb5:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb5 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.little_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0x64:
+ self.version = list(response_data)
+ elif frame_type == 0xC2: # version number
+ self.version = list(response_data)
+ elif frame_type == 0xC0:
+ d = list(response_data)
+ index = self.serial_number_map.get(d[0])
+ if index is not None:
+ self.serial_number=self.serial_number + d[1:]
+ else:
+ self.serial_number=self.serial_number + [-1] * 6
+
+ def get_version(self):
+ self.send_frame(0x64, [], sleep=0.1)
+ time.sleep(0.1)
+ if self.version is None:
+ self.send_frame(0xC2, [], sleep=0.1)
+ time.sleep(0.1)
+ return self.version
+
+ def set_torque(self,torque=[]):
+ '''Set maximum torque'''
+ if len(torque) == 5:
+ self.send_frame(0x02, torque)
+ time.sleep(0.002)
+ self.send_frame(0x03,torque)
+ elif len(torque) > 5:
+ self.send_frame(0x02, torque[:5])
+ time.sleep(0.002)
+ self.send_frame(0x03,torque[5:])
+
+
+ def get_current_status(self):
+ '''Get current joint status'''
+ if self.is_cmd == False:
+ #if self.version != None and self.version[4] > 35:
+ self.send_frame(0x01,[],sleep=0.003)
+ self.send_frame(0x04,[],sleep=0.003)
+ state = self.x01 + self.x04
+ return state
+ else:
+ state = self.x01 + self.x04
+ return state
+
+ def get_current_pub_status(self):
+ state = self.x01 + self.x04
+ return state
+
+ def get_speed(self):
+ '''Get current speed'''
+ self.send_frame(0x05,[],sleep=0.003)
+ self.send_frame(0x06,[],sleep=0.003)
+ return self.x05 + self.x06
+
+ def get_force(self):
+ '''Get pressure sensor data'''
+ return [self.normal_force,self.tangential_force , self.tangential_force_dir , self.approach_inc]
+ def get_temperature(self):
+ '''Get current motor temperature'''
+ self.get_motor_temperature()
+ return self.x33+self.x34
+
+ def get_touch_type(self):
+ '''Get touch type'''
+ self.send_frame(0xb0,[],sleep=0.03)
+ self.send_frame(0xb1,[],sleep=0.03)
+ t = []
+ for i in range(3):
+ t = self.xb1
+ time.sleep(0.01)
+ if len(t) == 2:
+ return 2
+ else:
+ self.send_frame(0x20,[],sleep=0.03)
+ time.sleep(0.01)
+ if self.normal_force[0] == -1:
+ return -1
+ else:
+ return 1
+
+ def get_touch(self):
+ '''Get touch data'''
+ self.send_frame(0xb1,[],sleep=0.03)
+ self.send_frame(0xb2,[],sleep=0.03)
+ self.send_frame(0xb3,[],sleep=0.03)
+ self.send_frame(0xb4,[],sleep=0.03)
+ self.send_frame(0xb5,[],sleep=0.03)
+ return [self.xb1[1],self.xb2[1],self.xb3[1],self.xb4[1],self.xb5[1],0] # The last digit is palm, currently not available
+
+ def get_matrix_touch(self):
+ self.send_frame(0xb1,[0xc6],sleep=0.06)
+ self.send_frame(0xb2,[0xc6],sleep=0.06)
+ self.send_frame(0xb3,[0xc6],sleep=0.06)
+ self.send_frame(0xb4,[0xc6],sleep=0.06)
+ self.send_frame(0xb5,[0xc6],sleep=0.06)
+ return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+ def get_matrix_touch_v2(self):
+ self.send_frame(0xb1,[0xc6],sleep=0.005)
+ self.send_frame(0xb2,[0xc6],sleep=0.005)
+ self.send_frame(0xb3,[0xc6],sleep=0.005)
+ self.send_frame(0xb4,[0xc6],sleep=0.005)
+ self.send_frame(0xb5,[0xc6],sleep=0.005)
+ return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+ def get_thumb_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb1,[0xc6],sleep=sleep_time)
+ return self.thumb_matrix
+
+ def get_index_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb2,[0xc6],sleep=sleep_time)
+ return self.index_matrix
+
+ def get_middle_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb3,[0xc6],sleep=sleep_time)
+ return self.middle_matrix
+
+ def get_ring_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb4,[0xc6],sleep=sleep_time)
+ return self.ring_matrix
+
+ def get_little_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb5,[0xc6],sleep=sleep_time)
+ return self.little_matrix
+
+
+ def get_torque(self):
+ '''Get current motor torque'''
+ if self.version != None and self.version[4]< 36:
+ return [-1] * 5
+ else:
+ self.send_frame(0x02, [])
+ time.sleep(0.002)
+ self.send_frame(0x03,[])
+ time.sleep(0.002)
+ return self.x02+self.x03
+
+ def get_fault(self):
+ '''Get motor fault'''
+ self.get_motor_fault_code()
+ return self.x35+self.x36
+
+ def get_current(self):
+ '''Get current'''
+ #return [-1] * 5
+ self.send_frame(0x02, [])
+ time.sleep(0.002)
+ self.send_frame(0x03,[])
+ return self.x02+self.x03
+
+ def get_serial_number(self):
+ try:
+ self.send_frame(0xC0,[],sleep=0.005)
+ # 1. 使用 bytes() 函数将整数列表转换为字节对象
+ # bytes() 接收一个由 0-255 之间的整数组成的列表。
+ byte_data = bytes(self.serial_number)
+ # 2. 使用 .decode() 方法将字节对象解码为 ASCII 字符串
+ result_string = byte_data.decode('ascii')
+ if result_string == "":
+ return "-1"
+ else:
+ # print(f"原始 ASCII 码列表: {self.serial_number}")
+ # print(f"解码后的字符串: {result_string}")
+ return result_string
+ except:
+ return "-1"
+
+ def get_finger_order(self):
+ return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch",
+ "index_mcp_roll", "ring_mcp_roll", "pinky_mcp_roll", "thumb_cmc_roll"]
+
+ def clear_faults(self, finger_mask=[1, 1, 1, 1, 1]):
+ """L10 暂不支持清除故障码"""
+ pass
+
+ def show_fun_table(self):
+ # if len(data) != 8 or data[0] != 0x64:
+ # raise ValueError("数据格式不正确")
+ data = self.version
+ result = {
+ "自由度": data[0],
+ "机械版本": data[1],
+ "版本序号": data[2],
+ "手方向": chr(data[3]), # ASCII 转字符
+ "软件版本": f"V{data[4] >> 4}.{data[4] & 0x0F}",
+ "硬件版本": f"V{data[5] >> 4}.{data[5] & 0x0F}",
+ "修订标志": data[6],
+ "set_position": "Y",
+ "set_torque": "Y",
+ "set_speed": "Y",
+ "get_version": "Y",
+ "get_current_status": "Y",
+ "get_speed": "Y",
+ "get_temperature": "Y",
+ "get_touch_type": "Y",
+ "get_matrix_touch": "Y",
+ "get_fault": "Y",
+ "get_current": "current == torque"
+ }
+
+ #return [data[0],data[1],data[2],chr(data[3]),f"V{data[4] >> 4}.{data[4] & 0x0F}",f"V{data[5] >> 4}.{data[5] & 0x0F}",data[6]]
+ table = [[k, v] for k, v in result.items()]
+ #print(tabulate(table, tablefmt="grid"), flush=True)
+
+
+ # # 示例数据
+ # data = [0x64, 0x15, 0x03, 0x0A, 0x4C, 0x11, 0x22, 0x01]
+ # parsed = parse_version_data(data)
+
+ # # 打印结果
+ # for k, v in parsed.items():
+ # print(f"{k}: {v}")
+
+
+ def close_can_interface(self):
+ """Stop the CAN communication."""
+ self.running = False
+ if self.receive_thread.is_alive():
+ self.receive_thread.join()
+ if self.bus:
+ self.bus.shutdown()
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l20_can.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l20_can.py
new file mode 100644
index 0000000..5b5f36e
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l20_can.py
@@ -0,0 +1,478 @@
+import sys
+import time
+import can
+import threading
+from enum import Enum
+import numpy as np
+from utils.open_can import OpenCan
+from utils.color_msg import ColorMsg
+from can.exceptions import CanError
+
+class FrameProperty(Enum):
+ INVALID_FRAME_PROPERTY = 0x00 # Invalid CAN frame property | No return
+ JOINT_PITCH_R = 0x01 # Short frame pitch angle - finger base flexion | Returns this type of data
+ JOINT_YAW_R = 0x02 # Short frame yaw angle - finger abduction/adduction | Returns this type of data
+ JOINT_ROLL_R = 0x03 # Short frame roll angle - only used for thumb | Returns this type of data
+ JOINT_TIP_R = 0x04 # Short frame fingertip angle control | Returns this type of data
+ JOINT_SPEED_R = 0x05 # Short frame speed - motor running speed control | Returns this type of data
+ JOINT_CURRENT_R = 0x06 # Short frame current - motor running current feedback | Returns this type of data
+ JOINT_FAULT_R = 0x07 # Short frame fault - motor running fault feedback | Returns this type of data
+ REQUEST_DATA_RETURN = 0x09 # Request data return | Returns all data
+ JOINT_PITCH_NR = 0x11 # Pitch angle - finger base flexion | No return for this type of data
+ JOINT_YAW_NR = 0x12 # Yaw angle - finger abduction/adduction | No return for this type of data
+ JOINT_ROLL_NR = 0x13 # Roll angle - only used for thumb | No return for this type of data
+ JOINT_TIP_NR = 0x14 # Fingertip angle control | No return for this type of data
+ JOINT_SPEED_NR = 0x15 # Speed - motor running speed control | No return for this type of data
+ JOINT_CURRENT_NR = 0x16 # Current - motor running current feedback | No return for this type of data
+ JOINT_FAULT_NR = 0x17 # Fault - motor running fault feedback | No return for this type of data
+ HAND_UID = 0xC0 # Device unique identifier Read only --------
+ HAND_HARDWARE_VERSION = 0xC1 # Hardware version Read only --------
+ HAND_SOFTWARE_VERSION = 0xC2 # Software version Read only --------
+ HAND_COMM_ID = 0xC3 # Device ID Read/Write 1 byte
+ HAND_SAVE_PARAMETER = 0xCF # Save parameters Write only --------
+
+
+class LinkerHandL20Can:
+ def __init__(self, can_channel='can0', baudrate=1000000, can_id=0x28,yaml=""):
+ self.can_id = can_id
+ self.can_channel = can_channel
+ self.baudrate = baudrate
+ self.open_can = OpenCan(load_yaml=yaml)
+
+ self.running = True
+ self.x05 = [255] * 5
+ self.x06, self.x07 = [],[]
+ # New pressure sensors
+ self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+ self.x09 = self.x0b = self.x0c = self.x0d = [-1] * 5
+ self.thumb_matrix = np.full((12, 6), -1)
+ self.index_matrix = np.full((12, 6), -1)
+ self.middle_matrix = np.full((12, 6), -1)
+ self.ring_matrix = np.full((12, 6), -1)
+ self.little_matrix = np.full((12, 6), -1)
+ self.matrix_map = {
+ 0: 0,
+ 16: 1,
+ 32: 2,
+ 48: 3,
+ 64: 4,
+ 80: 5,
+ 96: 6,
+ 112: 7,
+ 128: 8,
+ 144: 9,
+ 160: 10,
+ 176: 11,
+ }
+
+ # Initialize CAN bus according to operating system
+ # try:
+ # if sys.platform == "linux":
+ # self.open_can.open_can(self.can_channel)
+ # time.sleep(0.1)
+ # self.bus = can.interface.Bus(
+ # channel=can_channel, interface="socketcan", bitrate=baudrate,
+ # can_filters=[{"can_id": can_id, "can_mask": 0x7FF}]
+ # )
+ # elif sys.platform == "win32":
+ # self.bus = can.interface.Bus(
+ # channel=can_channel, interface='pcan', bitrate=baudrate,
+ # can_filters=[{"can_id": can_id, "can_mask": 0x7FF}]
+ # )
+ # else:
+ # raise EnvironmentError("Unsupported platform for CAN interface")
+ # except:
+ # print("Please insert CAN device",flush=True)
+ self.bus = self.init_can_bus(channel=self.can_channel, baudrate=baudrate)
+ # Initialize data storage
+ self.x01, self.x02, self.x03, self.x04 = [[-1] * 5 for _ in range(4)]
+ self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = \
+ [[-1] * 5 for _ in range(4)]
+
+ # Start receive thread
+ self.get_touch_type()
+ time.sleep(0.1)
+ self.receive_thread = threading.Thread(target=self.receive_response)
+ self.receive_thread.daemon = True
+ self.receive_thread.start()
+
+ def init_can_bus(self, channel, baudrate):
+ """
+ 尝试按优先级连接 CAN 总线，并实现回退机制。
+ """
+ # --- 统一异常处理块开始 ---
+ try:
+ if sys.platform == "linux":
+ # Linux 优先级：1. socketcan
+ try:
+ self.open_can.open_can(self.can_channel)
+ # 尝试 socketcan
+ bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan')
+ ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow")
+ raise # 重新抛出异常，让外层 try 捕获
+ elif sys.platform == "win32":
+ # Windows 优先级：1. pcan
+ try:
+ bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow")
+ # Windows 优先级：2. candle (回退方法)
+ try:
+ bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow")
+ raise # 两个接口都失败，抛出异常
+ else:
+ raise EnvironmentError("Unsupported platform for CAN interface")
+ # --- 统一异常处理块结束 ---
+ except Exception as e:
+ # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError）
+ ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red")
+ # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行
+ raise
+ # def send_command(self, frame_property, data_list):
+ # print("66666")
+ # """
+ # Send command to CAN bus
+ # :param frame_property: Data frame property
+ # :param data_list: Data payload
+ # """
+ # frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+ # data = [frame_property_value] + [int(val) for val in data_list]
+ # msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+ # try:
+ # self.bus.send(msg)
+ # print(f"Message sent: ID={hex(self.can_id)}, Data={data}")
+ # except can.CanError as e:
+ # print(f"Failed to send message: {e}")
+
+ def receive_response(self):
+ """
+ Receive and process CAN bus response messages
+ """
+ while self.running:
+ try:
+ msg = self.bus.recv(timeout=1.0) # Blocking receive, 1 second timeout
+ if msg:
+ self.process_response(msg)
+ except can.CanError as e:
+ print(f"Error receiving message666: {e}",flush=True)
+
+
+ def set_finger_base(self, angles):
+ self.send_command(FrameProperty.JOINT_PITCH_NR, angles)
+
+ def set_finger_tip(self, angles):
+ self.send_command(FrameProperty.JOINT_TIP_NR, angles)
+
+ def set_finger_middle(self, angles):
+ self.send_command(FrameProperty.JOINT_YAW_NR, angles)
+
+ def set_thumb_roll(self, angle):
+ self.send_command(FrameProperty.JOINT_ROLL_NR, angle)
+
+ def send_command(self, frame_property, data_list,sleep=0.002):
+ frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+ data = [frame_property_value] + [int(val) for val in data_list]
+
+ msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+ try:
+ self.bus.send(msg)
+ except can.CanError:
+ print("Message NOT sent")
+ self.open_can.open_can(self.can_channel)
+ time.sleep(1)
+ self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel)
+ time.sleep(1)
+ if self.is_can:
+ self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate)
+ else:
+ print("Reconnecting CAN devices ....",flush=True)
+ time.sleep(sleep)
+
+ def set_joint_pitch(self, frame, angles):
+ self.send_command(frame, angles)
+
+ def set_joint_yaw(self, angles):
+ self.send_command(0x02, angles)
+
+ def set_joint_roll(self, thumb_roll):
+ self.send_command(0x03, [thumb_roll, 0, 0, 0, 0])
+
+ def set_joint_speed(self, speed):
+ self.x05 = speed
+ self.send_command(0x05, speed)
+ def set_electric_current(self, e_c=[]):
+ self.send_command(0x06, e_c)
+
+ def get_normal_force(self):
+ self.send_command(0x20,[])
+
+ def get_tangential_force(self):
+ self.send_command(0x21,[])
+
+
+ def get_tangential_force_dir(self):
+ self.send_command(0x22,[])
+
+ def get_approach_inc(self):
+ self.send_command(0x23,[])
+
+
+
+
+ def get_electric_current(self, e_c=[]):
+ self.send_command(0x06, e_c)
+
+ def request_device_info(self):
+ self.send_command(0xC0, [0])
+ self.send_command(0xC1, [0])
+ self.send_command(0xC2, [0])
+
+ def save_parameters(self):
+ self.send_command(0xCF, [])
+ def process_response(self, msg):
+ if msg.arbitration_id == self.can_id:
+ frame_type = msg.data[0]
+ response_data = msg.data[1:]
+ if len(list(response_data)) == 0:
+ return
+ if frame_type == 0x01:
+ self.x01 = list(response_data)
+ elif frame_type == 0x02:
+ self.x02 = list(response_data)
+ elif frame_type == 0x03:
+ self.x03 = list(response_data)
+ elif frame_type == 0x04:
+ self.x04 = list(response_data)
+ elif frame_type == 0xC0:
+ print(f"Device ID info: {response_data}")
+ if self.can_id == 0x28:
+ self.right_hand_info = response_data
+ elif self.can_id == 0x27:
+ self.left_hand_info = response_data
+ elif frame_type == 0x05:
+ self.x05 = list(response_data)
+ elif frame_type == 0x06:
+ self.x06 = list(response_data)
+ elif frame_type == 0x07:
+ self.x07 = list(response_data)
+ elif frame_type == 0x09:
+ self.x09 = list(response_data)
+ elif frame_type == 0x0B:
+ self.x0b = list(response_data)
+ elif frame_type == 0x0C:
+ self.x0c = list(response_data)
+ elif frame_type == 0x0D:
+ self.x0d = list(response_data)
+ elif frame_type == 0x20:
+ d = list(response_data)
+ self.normal_force = [float(i) for i in d]
+ elif frame_type == 0x21:
+ d = list(response_data)
+ self.tangential_force = [float(i) for i in d]
+ elif frame_type == 0x22:
+ d = list(response_data)
+ self.tangential_force_dir = [float(i) for i in d]
+ elif frame_type == 0x23:
+ d = list(response_data)
+ self.approach_inc = [float(i) for i in d]
+ elif frame_type == 0xb0:
+ self.xb0 = list(response_data)
+ elif frame_type == 0xb1:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb1 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.thumb_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb2:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb2 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.index_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb3:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb3 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.middle_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb4:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb4 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.ring_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb5:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb5 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.little_matrix[index] = d[1:] # Remove the first flag bit
+ def pose_slice(self, p):
+ """Slice the joint array into finger action arrays"""
+ try:
+ finger_base = [int(val) for val in p[0:5]] # Finger base
+ yaw_angles = [int(val) for val in p[5:10]] # Yaw
+ thumb_yaw = [int(val) for val in p[10:15]] # Thumb yaw to palm, others are 0
+ finger_tip = [int(val) for val in p[15:20]] # Fingertip flexion
+ return finger_base, yaw_angles, thumb_yaw, finger_tip
+ except Exception as e:
+ print(e)
+ def set_joint_positions(self, position):
+ if len(position) != 20:
+ print("L20 finger joint length is incorrect")
+ return
+ finger_base, yaw_angles, thumb_yaw, finger_tip = self.pose_slice(position)
+ self.set_thumb_roll(thumb_yaw) # Thumb yaw to palm movement
+ self.set_finger_tip(finger_tip) # Fingertip movement
+ self.set_finger_base(finger_base) # Finger base movement
+ self.set_finger_middle(yaw_angles) # Yaw movement
+ def set_speed(self, speed=[]):
+ if len(speed) != 5:
+ raise ValueError("Speed list must have 5 elements.")
+ return
+ self.send_command(0x05,speed)
+ def set_torque(self, torque=[]):
+ '''Set torque, not supported for L20'''
+ print("Set torque, not supported for L20")
+ def set_current(self, current=[]):
+ '''Set current'''
+ self.set_electric_current(e_c=current)
+ def get_version(self):
+ '''Get version, currently not supported'''
+ return [0] * 5
+ def get_current_status(self):
+ '''Get current finger joint status'''
+ self.send_command(0x01,[],sleep=0.01)
+ self.send_command(0x02,[],sleep=0.01)
+ self.send_command(0x03,[],sleep=0.01)
+ self.send_command(0x04,[],sleep=0.01)
+ return self.x01 + self.x02 + self.x03 + self.x04
+
+ def get_current_pub_status(self):
+ time.sleep(0.01)
+ return self.x01 + self.x02 + self.x03 + self.x04
+
+ def get_speed(self):
+ '''Get current motor speed'''
+ self.send_command(0x05, [0])
+ time.sleep(0.001)
+ return self.x05
+ def get_current(self):
+ '''Get current threshold'''
+ self.send_command(0x06, [0])
+ return self.x06
+ def get_torque(self):
+ '''Get current motor torque, not supported for L20'''
+ return [0] * 5
+ def get_fault(self):
+ self.send_command(0x07,[])
+ time.sleep(0.01)
+ return self.x07
+
+ def get_temperature(self):
+ '''Get motor temperature'''
+ self.send_command(0x09,[])
+ self.send_command(0x0b,[])
+ self.send_command(0x0c,[])
+ self.send_command(0x0d,[])
+
+ return self.x09+self.x0b+self.x0c+self.x0d
+
+ def clear_faults(self):
+ '''Clear motor faults'''
+ self.send_command(0x07, [1, 1, 1, 1, 1])
+
+ def get_touch_type(self):
+ '''Get touch type'''
+ t = []
+ for i in range(3):
+ self.send_command(0xb0,[],sleep=0.03)
+ if self.xb0 == [2]:
+ return 2
+ elif self.xb0 == [1]:
+ return 1
+ else:
+ self.send_command(0x20,[],sleep=0.03)
+ time.sleep(0.01)
+ if self.normal_force[0] == -1:
+ return -1
+
+ def get_touch(self):
+ '''Get touch data'''
+ self.send_command(0xb1,[],sleep=0.03)
+ self.send_command(0xb2,[],sleep=0.03)
+ self.send_command(0xb3,[],sleep=0.03)
+ self.send_command(0xb4,[],sleep=0.03)
+ self.send_command(0xb5,[],sleep=0.03)
+ return [self.xb1[1],self.xb2[1],self.xb3[1],self.xb4[1],self.xb5[1],0] # The last digit is palm, currently not available
+
+ def get_matrix_touch(self):
+ self.send_command(0xb1,[0xc6],sleep=0.04)
+ self.send_command(0xb2,[0xc6],sleep=0.04)
+ self.send_command(0xb3,[0xc6],sleep=0.04)
+ self.send_command(0xb4,[0xc6],sleep=0.04)
+ self.send_command(0xb5,[0xc6],sleep=0.04)
+ return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+
+ def get_thumb_matrix_touch(self,sleep_time=0.009):
+ self.send_command(0xb1,[0xc6],sleep=sleep_time)
+ return self.thumb_matrix
+
+ def get_index_matrix_touch(self,sleep_time=0.009):
+ self.send_command(0xb2,[0xc6],sleep=sleep_time)
+ return self.index_matrix
+
+ def get_middle_matrix_touch(self,sleep_time=0.009):
+ self.send_command(0xb3,[0xc6],sleep=sleep_time)
+ return self.middle_matrix
+
+ def get_ring_matrix_touch(self,sleep_time=0.009):
+ self.send_command(0xb4,[0xc6],sleep=sleep_time)
+ return self.ring_matrix
+
+ def get_little_matrix_touch(self,sleep_time=0.009):
+ self.send_command(0xb5,[0xc6],sleep=sleep_time)
+ return self.little_matrix
+
+
+ def get_faults(self):
+ '''Get motor fault codes'''
+ self.send_command(0x07, [])
+ return self.x07
+ def get_force(self):
+ '''Get pressure sensor data'''
+ return [self.normal_force,self.tangential_force,self.tangential_force_dir,self.approach_inc]
+
+ def get_serial_number(self):
+ return [0] * 6
+
+ def show_fun_table(self):
+ pass
+
+ def get_finger_order(self):
+ return []
+
+ def close_can_interface(self):
+ if self.bus:
+ self.bus.shutdown() # Close CAN bus
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l21_can.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l21_can.py
new file mode 100644
index 0000000..66ba97a
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l21_can.py
@@ -0,0 +1,822 @@
+#!/usr/bin/env python3
+import can
+import time, sys, os
+import threading
+import numpy as np
+from enum import Enum
+from utils.open_can import OpenCan
+from utils.color_msg import ColorMsg
+from can.exceptions import CanError
+current_dir = os.path.dirname(os.path.abspath(__file__))
+target_dir = os.path.abspath(os.path.join(current_dir, ".."))
+sys.path.append(target_dir)
+
+class FrameProperty(Enum):
+ # Finger motion control - parallel control commands
+ ROLL_POS = 0x01 # Roll joint position
+ YAWPOS = 0x02 # Yaw joint position
+ ROOT1_POS = 0x03 # Root joint 1 position
+ ROOT2_POS = 0x04 # Root joint 2 position
+ ROOT3_POS = 0x05 # Root joint 3 position
+ TIP_POS = 0x06 # Fingertip joint position
+ # Finger motion control - serial control commands
+ THUMB_POS = 0x41 # Thumb joint position
+ INDEX_POS = 0x42 # Index finger joint position
+ MIDDLE_POS = 0x43 # Middle finger joint position
+ RING_POS = 0x44 # Ring finger joint position
+ LITTLE_POS = 0x45 # Little finger joint position
+
+ # Finger motion control - speed
+ ROLL_SPEED = 0x09 # Roll joint speed
+ YAW_SPEED = 0x0A # Yaw joint speed
+ ROOT1_SPEED = 0x0B # Root joint 1 speed
+ ROOT2_SPEED = 0x0C # Root joint 2 speed
+ ROOT3_SPEED = 0x0D # Root joint 3 speed
+ TIP_SPEED = 0x0E # Fingertip joint speed
+ THUMB_SPEED = 0x49 # Thumb speed
+ INDEX_SPEED = 0x4A # Index finger speed
+ MIDDLE_SPEED = 0x4B # Middle finger speed
+ RING_SPEED = 0x4C # Ring finger speed
+ LITTLE_SPEED = 0x4D # Little finger speed
+
+ # Finger motion control - torque
+ ROLL_TORQUE = 0x11 # Roll joint torque
+ YAW_TORQUE = 0x12 # Yaw joint torque
+ ROOT1_TORQUE = 0x13 # Root joint 1 torque
+ ROOT2_TORQUE = 0x14 # Root joint 2 torque
+ ROOT3_TORQUE = 0x15 # Root joint 3 torque
+ TIP_TORQUE = 0x16 # Fingertip joint torque
+ THUMB_TORQUE = 0x51 # Thumb torque
+ INDEX_TORQUE = 0x52 # Index finger torque
+ MIDDLE_TORQUE = 0x53 # Middle finger torque
+ RING_TORQUE = 0x54 # Ring finger torque
+ LITTLE_TORQUE = 0x55 # Little finger torque
+
+ THUMB_FAULT = 0x59 # Thumb fault code | Returns this type of data
+ INDEX_FAULT = 0x5A # Index finger fault code | Returns this type of data
+ MIDDLE_FAULT = 0x5B # Middle finger fault code | Returns this type of data
+ RING_FAULT = 0x5C # Ring finger fault code | Returns this type of data
+ LITTLE_FAULT = 0x5D # Little finger fault code | Returns this type of data
+
+ # Finger faults and temperature
+ ROLL_FAULT = 0x19 # Roll joint fault code
+ YAW_FAULT = 0x1A # Yaw joint fault code
+ ROOT1_FAULT = 0x1B # Root joint 1 fault code
+ ROOT2_FAULT = 0x1C # Root joint 2 fault code
+ ROOT3_FAULT = 0x1D # Root joint 3 fault code
+ TIP_FAULT = 0x1E # Fingertip joint fault code
+ ROLL_TEMPERATURE = 0x21 # Roll joint over-temperature protection threshold
+ YAW_TEMPERATURE = 0x22 # Yaw joint over-temperature protection threshold
+ ROOT1_TEMPERATURE = 0x23 # Root joint 1 over-temperature protection threshold
+ ROOT2_TEMPERATURE = 0x24 # Root joint 2 over-temperature protection threshold
+ ROOT3_TEMPERATURE = 0x25 # Root joint 3 over-temperature protection threshold
+ TIP_TEMPERATURE = 0x26 # Fingertip joint over-temperature protection threshold
+ THUMB_TEMPERATURE = 0x61 # Thumb over-temperature protection threshold
+ INDEX_TEMPERATURE = 0x62 # Index finger over-temperature protection threshold
+ MIDDLE_TEMPERATURE = 0x63 # Middle finger over-temperature protection threshold
+ RING_TEMPERATURE = 0x64 # Ring finger over-temperature protection threshold
+ LITTLE_TEMPERATURE = 0x65 # Little finger over-temperature protection threshold
+
+ # Configuration and preset actions
+ HAND_UID = 0xC0 # Device unique identifier
+ HAND_HARDWARE_VERSION = 0xC1 # Hardware version
+ HAND_SOFTWARE_VERSION = 0xC2 # Software version
+ HAND_COMM_ID = 0xC3 # Device ID
+ HAND_FACTORY_RESET = 0xCE # Restore factory settings
+ HAND_SAVE_PARAMETER = 0xCF # Save parameters
+
+ # Tactile sensor data
+ HAND_NORMAL_FORCE = 0x90 # Normal force of five fingers
+ HAND_TANGENTIAL_FORCE = 0x91 # Tangential force of five fingers
+ HAND_TANGENTIAL_FORCE_DIR = 0x92 # Tangential direction of five fingers
+ HAND_APPROACH_INC = 0x93 # Approach sensing of five fingers
+
+ TOUCH_SENSOR_TYPE = 0xB0 # Sensor type
+ THUMB_TOUCH = 0xB1 # Thumb tactile sensing
+ INDEX_TOUCH = 0xB2 # Index finger tactile sensing
+ MIDDLE_TOUCH = 0xB3 # Middle finger tactile sensing
+ RING_TOUCH = 0xB4 # Ring finger tactile sensing
+ LITTLE_TOUCH = 0xB5 # Little finger tactile sensing
+ PALM_TOUCH = 0xB6 # Palm tactile sensing
+
+ # Action control
+ ACTION_PLAY = 0xA0 # Action
+
+ # Combined command area
+ FINGER_SPEED = 0x81 # Set maximum finger speed
+ FINGER_TORQUE = 0x82 # Set maximum finger torque
+ FINGER_FAULT = 0x83 # Clear finger faults and fault codes
+ FINGER_TEMPERATURE = 0x84 # Finger joint temperatures
+
+class LinkerHandL21Can:
+ def __init__(self, can_channel='can0', baudrate=1000000, can_id=0x28,yaml=""):
+ self.can_id = can_id
+ self.can_channel = can_channel
+ self.baudrate = baudrate
+ self.open_can = OpenCan(load_yaml=yaml)
+
+ self.running = True
+ self.last_thumb_pos, self.last_index_pos,self.last_ring_pos,self.last_middle_pos, self.last_little_pos = None,None,None,None,None
+ self.x01, self.x02, self.x03, self.x04,self.x05,self.x06,self.x07, self.x08,self.x09,self.x0A,self.x0B,self.x0C,self.x0D,self.x0E,self.speed = [],[],[],[],[],[],[],[],[],[],[],[],[],[],[]
+ self.last_root1,self.last_yaw,self.last_roll,self.last_root2,self.last_tip = None,None,None,None,None
+ # Speed
+ self.x49, self.x4a, self.x4b, self.x4c, self.x4d,self.xc1 = [],[],[],[],[],[]
+ self.x41,self.x42,self.x43,self.x44,self.x45 = [],[],[],[],[]
+ self.x83 = [-1] * 5
+ # Torque
+ self.x51, self.x52, self.x53, self.x54,self.x55 = [],[],[],[],[]
+ # Fault codes
+ self.x59,self.x5a,self.x5b,self.x5c,self.x5d = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+ # Temperature thresholds
+ self.x61,self.x62,self.x63,self.x64,self.x65 = [],[],[],[],[]
+ # Pressure sensors
+ self.x90,self.x91,self.x92,self.x93 = [],[],[],[]
+ # New pressure sensors
+ self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5,self.xb6 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+ self.thumb_matrix = np.full((12, 6), -1)
+ self.index_matrix = np.full((12, 6), -1)
+ self.middle_matrix = np.full((12, 6), -1)
+ self.ring_matrix = np.full((12, 6), -1)
+ self.little_matrix = np.full((12, 6), -1)
+ self.matrix_map = {
+ 0: 0,
+ 16: 1,
+ 32: 2,
+ 48: 3,
+ 64: 4,
+ 80: 5,
+ 96: 6,
+ 112: 7,
+ 128: 8,
+ 144: 9,
+ 160: 10,
+ 176: 11,
+ }
+ # Initialize CAN bus according to operating system
+ # try:
+ # if sys.platform == "linux":
+ # self.open_can.open_can(self.can_channel)
+ # time.sleep(0.1)
+ # self.bus = can.interface.Bus(
+ # channel=can_channel, interface="socketcan", bitrate=baudrate,
+ # can_filters=[{"can_id": can_id, "can_mask": 0x7FF}]
+ # )
+ # elif sys.platform == "win32":
+ # self.bus = can.interface.Bus(
+ # channel=can_channel, interface='pcan', bitrate=baudrate,
+ # can_filters=[{"can_id": can_id, "can_mask": 0x7FF}]
+ # )
+ # else:
+ # raise EnvironmentError("Unsupported platform for CAN interface")
+ # except:
+ # print("Please insert CAN device")
+ self.bus = self.init_can_bus(channel=self.can_channel, baudrate=baudrate)
+
+ # Start receive thread
+ self.receive_thread = threading.Thread(target=self.receive_response)
+ self.receive_thread.daemon = True
+ self.receive_thread.start()
+
+ def init_can_bus(self, channel, baudrate):
+ """
+ 尝试按优先级连接 CAN 总线，并实现回退机制。
+ """
+ # --- 统一异常处理块开始 ---
+ try:
+ if sys.platform == "linux":
+ # Linux 优先级：1. socketcan
+ try:
+ self.open_can.open_can(self.can_channel)
+ # 尝试 socketcan
+ bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan')
+ ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow")
+ raise # 重新抛出异常，让外层 try 捕获
+ elif sys.platform == "win32":
+ # Windows 优先级：1. pcan
+ try:
+ bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow")
+ # Windows 优先级：2. candle (回退方法)
+ try:
+ bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow")
+ raise # 两个接口都失败，抛出异常
+ else:
+ raise EnvironmentError("Unsupported platform for CAN interface")
+ # --- 统一异常处理块结束 ---
+ except Exception as e:
+ # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError）
+ ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red")
+ # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行
+ raise
+
+ def send_command(self, frame_property, data_list,sleep_time=0.003):
+ """
+ Send command to CAN bus
+ :param frame_property: Data frame property
+ :param data_list: Data payload
+ """
+ frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+ data = [frame_property_value] + [int(val) for val in data_list]
+ msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+ try:
+ self.bus.send(msg)
+ except can.CanError as e:
+ print(f"Failed to send message: {e}")
+ self.open_can.open_can(self.can_channel)
+ time.sleep(1)
+ self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel)
+ time.sleep(1)
+ if self.is_can:
+ self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate)
+ else:
+ print("Reconnecting CAN devices ....")
+ time.sleep(sleep_time)
+
+ def receive_response(self):
+ """
+ Receive and process CAN bus response messages
+ """
+ while self.running:
+ try:
+ msg = self.bus.recv(timeout=1.0)
+ if msg:
+ self.process_response(msg)
+ except can.CanError as e:
+ print(f"Error receiving message: {e}")
+
+
+ def set_joint_positions(self, joint_ranges):
+ if len(joint_ranges) == 25:
+ l21_pose = self.joint_map(joint_ranges)
+ # Use list comprehension to split the list into subarrays of 6 elements each
+ chunks = [l21_pose[i:i+6] for i in range(0, 30, 6)]
+ for i in range(3):
+ self.send_command(FrameProperty.THUMB_POS, chunks[0])
+ time.sleep(0.001)
+ self.send_command(FrameProperty.INDEX_POS, chunks[1])
+ time.sleep(0.001)
+ self.send_command(FrameProperty.MIDDLE_POS, chunks[2])
+ time.sleep(0.001)
+ self.send_command(FrameProperty.RING_POS, chunks[3])
+ time.sleep(0.001)
+ self.send_command(FrameProperty.LITTLE_POS, chunks[4])
+ time.sleep(0.001)
+
+ def set_joint_positions_by_topic(self, joint_ranges):
+ if len(joint_ranges) == 25:
+ l21_pose = self.slice_list(joint_ranges,5)
+ if self._list_d_value(self.last_root1, l21_pose[0]):
+ self.set_root1_positions(l21_pose[0])
+ self.last_root1 = l21_pose[0]
+ if self._list_d_value(self.last_yaw, l21_pose[1]):
+ self.set_yaw_positions(l21_pose[1])
+ self.last_yaw = l21_pose[1]
+ if self._list_d_value(self.last_roll, l21_pose[2]):
+ self.set_roll_positions(l21_pose[2])
+ self.last_roll = l21_pose[2]
+ if self._list_d_value(self.last_root2, l21_pose[3]):
+ self.set_root2_positions(l21_pose[3])
+ self.last_root2 = l21_pose[3]
+ if self._list_d_value(self.last_tip, l21_pose[4]):
+ self.set_tip_positions(l21_pose[4])
+ self.last_tip = l21_pose[4]
+
+
+ def slice_list(self, input_list, slice_size):
+ """
+ Slice a list into pieces of specified size.
+
+ Args:
+ input_list (list): The list to be sliced.
+ slice_size (int): Number of elements per slice.
+
+ Returns:
+ list of lists: The sliced list.
+ """
+ sliced_list = [input_list[i:i + slice_size] for i in range(0, len(input_list), slice_size)]
+ return sliced_list
+
+ def _list_d_value(self,list1, list2):
+ if list1 == None:
+ return True
+ for a, b in zip(list1, list2):
+ if abs(b - a) > 2:
+ return True
+ break
+ return False
+ # Set all finger roll joint positions
+ def set_roll_positions(self, joint_ranges):
+ self.send_command(FrameProperty.ROLL_POS, joint_ranges)
+ # Set all finger yaw joint positions
+ def set_yaw_positions(self, joint_ranges):
+ self.send_command(FrameProperty.YAW_POS, joint_ranges)
+ # Set all finger root1 joint positions
+ def set_root1_positions(self, joint_ranges):
+ self.send_command(FrameProperty.ROOT1_POS, joint_ranges)
+ # Set all finger root2 joint positions
+ def set_root2_positions(self, joint_ranges):
+ self.send_command(FrameProperty.ROOT2_POS, joint_ranges)
+ # Set all finger root3 joint positions
+ def set_root3_positions(self, joint_ranges):
+ self.send_command(FrameProperty.ROOT3_POS, joint_ranges)
+ # Set all finger tip joint positions
+ def set_tip_positions(self, joint_ranges=[80]*5):
+ self.send_command(FrameProperty.TIP_POS, joint_ranges)
+ # Set thumb torque
+ def set_thumb_torque(self, j=[]):
+ self.send_command(FrameProperty.THUMB_TORQUE, j)
+ # Set index finger torque
+ def set_index_torque(self, j=[]):
+ self.send_command(FrameProperty.INDEX_TORQUE, j)
+ # Set middle finger torque
+ def set_middle_torque(self, j=[]):
+ self.send_command(FrameProperty.MIDDLE_TORQUE, j)
+ # Set ring finger torque
+ def set_ring_torque(self, j=[]):
+ self.send_command(FrameProperty.RING_TORQUE, j)
+ # Set little finger torque
+ def set_little_torque(self, j=[]):
+ self.send_command(FrameProperty.LITTLE_TORQUE, j)
+
+ # Get thumb joint positions
+ def get_thumb_positions(self,j=[0]):
+ self.send_command(FrameProperty.THUMB_POS, j)
+ # Get index finger joint positions
+ def get_index_positions(self, j=[0]):
+ self.send_command(FrameProperty.INDEX_POS,j)
+ # Get middle finger joint positions
+ def get_middle_positions(self, j=[0]):
+ self.send_command(FrameProperty.MIDDLE_POS,j)
+ # Get ring finger joint positions
+ def get_ring_positions(self, j=[0]):
+ self.send_command(FrameProperty.RING_POS,j)
+ # Get little finger joint positions
+ def get_little_positions(self, j=[0]):
+ self.send_command(FrameProperty.LITTLE_POS, j)
+ # Get all thumb motor fault codes
+ def get_thumbn_fault(self,j=[]):
+ self.send_command(FrameProperty.THUMB_FAULT,j)
+ # Get all index finger motor fault codes
+ def get_index_fault(self,j=[]):
+ self.send_command(FrameProperty.INDEX_FAULT,j)
+ # Get all middle finger motor fault codes
+ def get_middle_fault(self,j=[]):
+ self.send_command(FrameProperty.MIDDLE_FAULT,j)
+ # Get all ring finger motor fault codes
+ def get_ring_fault(self,j=[]):
+ self.send_command(FrameProperty.RING_FAULT,j)
+ # Get all little finger motor fault codes
+ def get_little_fault(self,j=[]):
+ self.send_command(FrameProperty.LITTLE_FAULT,j)
+ # Get thumb temperature threshold
+ def get_thumb_threshold(self,j=[]):
+ self.send_command(FrameProperty.THUMB_TEMPERATURE, '')
+ # Get index finger temperature threshold
+ def get_index_threshold(self,j=[]):
+ self.send_command(FrameProperty.INDEX_TEMPERATURE, j)
+ # Get middle finger temperature threshold
+ def get_middle_threshold(self,j=[]):
+ self.send_command(FrameProperty.MIDDLE_TEMPERATURE, j)
+ # Get ring finger temperature threshold
+ def get_ring_threshold(self,j=[]):
+ self.send_command(FrameProperty.RING_TEMPERATURE, j)
+ # Get little finger temperature threshold
+ def get_little_threshold(self,j=[]):
+ self.send_command(FrameProperty.LITTLE_TEMPERATURE, j)
+
+ # Disable mode 01
+ def set_disability_mode(self, j=[1,1,1,1,1]):
+ self.send_command(0x85,j)
+ # Enable mode 00
+ def set_enable_mode(self, j=[00,00,00,00,00]):
+ self.send_command(0x85,j)
+
+ # Set all finger torques
+ def set_torque(self,torque=[250]*5):
+ t = torque[0]
+ i = torque[1]
+ m = torque[2]
+ r = torque[3]
+ l = torque[4]
+ self.set_thumb_torque(j=[t]*5)
+ self.set_index_torque(j=[i]*5)
+ self.set_middle_torque(j=[m]*5)
+ self.set_ring_torque(j=[r]*5)
+ self.set_little_torque(j=[l]*5)
+
+ def set_speed(self, speed):
+ self.speed = speed
+ if len(speed) < 25:
+ thumb_speed = [self.speed[0]]*5
+ index_speed = [self.speed[1]]*5
+ middle_speed = [self.speed[2]]*5
+ ring_speed = [self.speed[3]]*5
+ little_speed = [self.speed[4]]*5
+ else:
+ thumb_speed = [self.speed[0],self.speed[1],self.speed[2],self.speed[3],self.speed[4]]
+ index_speed = [self.speed[5],self.speed[6],self.speed[7],self.speed[8],self.speed[9]]
+ middle_speed = [self.speed[10],self.speed[11],self.speed[12],self.speed[13],self.speed[14]]
+ ring_speed = [self.speed[15],self.speed[16],self.speed[17],self.speed[18],self.speed[19]]
+ little_speed = [self.speed[20],self.speed[21],self.speed[22],self.speed[23],self.speed[24]]
+ self.send_command(FrameProperty.THUMB_SPEED, thumb_speed)
+ self.send_command(FrameProperty.INDEX_SPEED, index_speed)
+ self.send_command(FrameProperty.MIDDLE_SPEED, middle_speed)
+ self.send_command(FrameProperty.RING_SPEED, ring_speed)
+ self.send_command(FrameProperty.LITTLE_SPEED, little_speed)
+
+ def set_finger_torque(self, torque):
+ self.send_command(0x42, torque)
+
+ def request_device_info(self):
+ self.send_command(0xC0, [0])
+ self.send_command(0xC1, [0])
+ self.send_command(0xC2, [0])
+
+ def save_parameters(self):
+ self.send_command(0xCF, [])
+ def process_response(self, msg):
+ if msg.arbitration_id == self.can_id:
+ frame_type = msg.data[0]
+ response_data = msg.data[1:]
+ if len(list(response_data)) == 0:
+ return
+ if frame_type == 0x01:
+ self.x01 = list(response_data)
+ elif frame_type == 0x02:
+ self.x02 = list(response_data)
+ elif frame_type == 0x03:
+ self.x03 = list(response_data)
+ elif frame_type == 0x04:
+ self.x04 = list(response_data)
+ elif frame_type == 0x05:
+ self.x05 = list(response_data)
+ elif frame_type == 0x06:
+ self.x06 = list(response_data)
+ elif frame_type == 0xC0:
+ print(f"Device ID info: {response_data}")
+ if self.can_id == 0x28:
+ self.right_hand_info = response_data
+ elif self.can_id == 0x27:
+ self.left_hand_info = response_data
+ elif frame_type == 0x08:
+ self.x08 = list(response_data)
+ elif frame_type == 0x09:
+ self.x09 = list(response_data)
+ elif frame_type == 0x0A:
+ self.x0A = list(response_data)
+ elif frame_type == 0x0B:
+ self.x0B = list(response_data)
+ elif frame_type == 0x0C:
+ self.x0C = list(response_data)
+ elif frame_type == 0x0D:
+ self.x0D = list(response_data)
+ elif frame_type == 0x22:
+ d = list(response_data)
+ self.tangential_force_dir = [float(i) for i in d]
+ elif frame_type == 0x23:
+ d = list(response_data)
+ self.approach_inc = [float(i) for i in d]
+ elif frame_type == 0x41:
+ self.x41 = list(response_data)
+ elif frame_type == 0x42:
+ self.x42 = list(response_data)
+ elif frame_type == 0x43:
+ self.x43 = list(response_data)
+ elif frame_type == 0x44:
+ self.x44 = list(response_data)
+ elif frame_type == 0x45:
+ self.x45 = list(response_data)
+ elif frame_type == 0x49:
+ self.x49 = list(response_data)
+ elif frame_type == 0x4a:
+ self.x4a = list(response_data)
+ elif frame_type == 0x4b:
+ self.x4b = list(response_data)
+ elif frame_type == 0x4c:
+ self.x4c = list(response_data)
+ elif frame_type == 0x4d:
+ self.x4d = list(response_data)
+ elif frame_type == 0xc1:
+ self.xc1 = list(response_data)
+ elif frame_type == 0x51:
+ self.x51 = list(response_data)
+ elif frame_type == 0x52:
+ self.x52 = list(response_data)
+ elif frame_type == 0x53:
+ self.x53 = list(response_data)
+ elif frame_type == 0x54:
+ self.x54 = list(response_data)
+ elif frame_type == 0x55:
+ self.x55 = list(response_data)
+ elif frame_type == 0x59:
+ self.x59 = list(response_data)
+ elif frame_type == 0x5a:
+ self.x5a = list(response_data)
+ elif frame_type == 0x5b:
+ self.x5b = list(response_data)
+ elif frame_type == 0x5c:
+ self.x5c = list(response_data)
+ elif frame_type == 0x5d:
+ self.x5d = list(response_data)
+ elif frame_type == 0x61:
+ self.x61 = list(response_data)
+ elif frame_type == 0x62:
+ self.x62 = list(response_data)
+ elif frame_type == 0x63:
+ self.x63 = list(response_data)
+ elif frame_type == 0x64:
+ self.x64 = list(response_data)
+ elif frame_type == 0x65:
+ self.x65 = list(response_data)
+ elif frame_type == 0x83:
+ self.x83 = list(response_data)
+ elif frame_type == 0x90:
+ self.x90 = list(response_data)
+ elif frame_type == 0x91:
+ self.x91 = list(response_data)
+ elif frame_type == 0x92:
+ self.x92 = list(response_data)
+ elif frame_type == 0x93:
+ self.x93 = list(response_data)
+ elif frame_type == 0xb0:
+ self.xb0 = list(response_data)
+ elif frame_type == 0xb1:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb1 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.thumb_matrix[index] = d[1:]
+ elif frame_type == 0xb2:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb2 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.index_matrix[index] = d[1:]
+ elif frame_type == 0xb3:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb3 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.middle_matrix[index] = d[1:]
+ elif frame_type == 0xb4:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb4 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.ring_matrix[index] = d[1:]
+ elif frame_type == 0xb5:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb5 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.little_matrix[index] = d[1:]
+ elif frame_type == 0xb6:
+ self.xb6 = list(response_data)
+
+ def joint_map(self, pose):
+ # l21 CAN data by default receives 30 data
+ l21_pose = [0.0] * 30
+
+ mapping = {
+ 0: 10, 1: 5, 2: 0, 3: 15, 4: None, 5: 20,
+ 6: None, 7: 6, 8: 1, 9: 16, 10: None, 11: 21,
+ 12: None, 13: 7, 14: 2, 15: 17, 16: None, 17: 22,
+ 18: None, 19: 8, 20: 3, 21: 18, 22: None, 23: 23,
+ 24: None, 25: 9, 26: 4, 27: 19, 28: None, 29: 24
+ }
+
+ for l21_idx, pose_idx in mapping.items():
+ if pose_idx is not None:
+ l21_pose[l21_idx] = pose[pose_idx]
+
+ return l21_pose
+
+ def state_to_cmd(self, l21_state):
+ pose = [0.0] * 25
+
+ mapping = {
+ 0: 10, 1: 5, 2: 0, 3: 15, 5: 20, 7: 6,
+ 8: 1, 9: 16, 11: 21, 13:7, 14: 2, 15: 17, 17: 22,
+ 19: 8, 20: 3, 21: 18, 23: 23, 25: 9, 26: 4,
+ 27: 19, 29: 24
+ }
+ for l21_idx, pose_idx in mapping.items():
+ pose[pose_idx] = l21_state[l21_idx]
+ return pose
+ def action_play(self):
+ self.send_command(0xA0,[])
+ def get_current_status(self, j=''):
+ self.send_command(FrameProperty.THUMB_POS, j,sleep_time=0.001)
+ self.send_command(FrameProperty.INDEX_POS,j,sleep_time=0.001)
+ self.send_command(FrameProperty.MIDDLE_POS,j,sleep_time=0.001)
+ self.send_command(FrameProperty.RING_POS,j,sleep_time=0.001)
+ self.send_command(FrameProperty.LITTLE_POS, j,sleep_time=0.001)
+ state= self.x41+ self.x42+ self.x43+ self.x44+ self.x45
+ if len(state) == 30:
+ l21_state = self.state_to_cmd(l21_state=state)
+ return l21_state
+
+ def get_current_pub_status(self):
+ state= self.x41+ self.x42+ self.x43+ self.x44+ self.x45
+ if len(state) == 30:
+ l21_state = self.state_to_cmd(l21_state=state)
+ return l21_state
+
+ def get_current_state_topic(self):
+ self.send_command(0x01,[])
+ self.send_command(0x02,[])
+ self.send_command(0x03,[])
+ self.send_command(0x04,[])
+ self.send_command(0x06,[])
+ state = self.x03+self.x02+self.x01+self.x04+self.x06
+ return state
+
+ def get_speed(self,j=''):
+ self.send_command(FrameProperty.THUMB_SPEED, j)
+ self.send_command(FrameProperty.INDEX_SPEED, j)
+ self.send_command(FrameProperty.MIDDLE_SPEED, j)
+ self.send_command(FrameProperty.RING_SPEED, j)
+ self.send_command(FrameProperty.LITTLE_SPEED, j)
+ speed = self.x49+ self.x4a+ self.x4b+ self.x4c+ self.x4d
+ if len(speed) == 30:
+ l21_speed = self.state_to_cmd(l21_state=speed)
+ return l21_speed
+
+ # def get_finger_torque(self):
+ # return self.finger_torque()
+ def get_fault(self):
+ self.get_thumbn_fault()
+ self.get_index_fault()
+ self.get_middle_fault()
+ self.get_ring_fault()
+ self.get_little_fault()
+ return [self.x59]+[self.x5a]+[self.x5b]+[self.x5c]+[self.x5d]
+ def get_threshold(self):
+ self.get_thumb_threshold()
+ self.get_index_threshold()
+ self.get_middle_threshold()
+ self.get_ring_threshold()
+ self.get_little_threshold()
+ return [self.x61]+[self.x62]+[self.x63]+[self.x64]+[self.x65]
+ def get_version(self):
+ if self.xc1 == []:
+ self.send_command(FrameProperty.HAND_HARDWARE_VERSION,[])
+ return self.xc1
+ def get_normal_force(self):
+ self.send_command(FrameProperty.HAND_NORMAL_FORCE,[])
+ return self.x90
+ def get_tangential_force(self):
+ self.send_command(FrameProperty.HAND_TANGENTIAL_FORCE,[])
+ return self.x91
+ def get_tangential_force_dir(self):
+ self.send_command(FrameProperty.HAND_TANGENTIAL_FORCE_DIR,[])
+ return self.x92
+ def get_approach_inc(self):
+ self.send_command(FrameProperty.HAND_APPROACH_INC,[])
+ return self.x93
+
+ def get_touch_type(self):
+ '''Get tactile sensor type data'''
+ self.send_command(FrameProperty.TOUCH_SENSOR_TYPE,[])
+ try:
+ return self.xb0[0]
+ except:
+ pass
+ def get_finger_torque(self):
+ self.send_command(FrameProperty.THUMB_TORQUE,[])
+ self.send_command(FrameProperty.INDEX_TORQUE,[])
+ self.send_command(FrameProperty.MIDDLE_TORQUE,[])
+ self.send_command(FrameProperty.RING_TORQUE,[])
+ self.send_command(FrameProperty.LITTLE_TORQUE,[])
+ return self.x51+self.x52+self.x53+self.x54+self.x55
+
+ def get_torque(self):
+ return self.get_finger_torque()
+
+ def get_thumb_touch(self):
+ '''Get thumb tactile sensor data'''
+ self.send_command(FrameProperty.THUMB_TOUCH,[],sleep_time=0.015)
+ return self.xb1
+
+ def get_index_touch(self):
+ '''Get index finger tactile sensor data'''
+ self.send_command(FrameProperty.INDEX_TOUCH,[0xc6],sleep_time=0.015)
+ return self.xb2
+
+ def get_middle_touch(self):
+ '''Get middle finger tactile sensor data'''
+ self.send_command(FrameProperty.MIDDLE_TOUCH,[],sleep_time=0.015)
+ return self.xb3
+
+ def get_ring_touch(self):
+ '''Get ring finger tactile sensor data'''
+ self.send_command(FrameProperty.RING_TOUCH,[],sleep_time=0.015)
+ return self.xb4
+
+ def get_little_touch(self):
+ '''Get little finger tactile sensor data'''
+ self.send_command(FrameProperty.LITTLE_TOUCH,[],sleep_time=0.015)
+ return self.xb5
+
+ def get_palm_touch(self):
+ '''Get palm tactile sensor data'''
+ self.send_command(FrameProperty.PALM_TOUCH,[],sleep_time=0.015)
+ return self.xb6
+
+ def get_force(self):
+ '''Get pressure sensor data'''
+ return [self.x90,self.x91 , self.x92 , self.x93]
+
+ def get_touch(self):
+ '''Get tactile sensor data'''
+ self.get_thumb_touch()
+ self.get_index_touch()
+ self.get_middle_touch()
+ self.get_ring_touch()
+ self.get_little_touch()
+ self.get_palm_touch()
+ try:
+ return [self.xb1[1],self.xb2[1] , self.xb3[1] , self.xb4[1],self.xb5[1],self.xb6[1]]
+ except:
+ pass
+
+ def get_matrix_touch(self):
+ self.send_command(0xb1,[0xc6],sleep_time=0.04)
+ self.send_command(0xb2,[0xc6],sleep_time=0.04)
+ self.send_command(0xb3,[0xc6],sleep_time=0.04)
+ self.send_command(0xb4,[0xc6],sleep_time=0.04)
+ self.send_command(0xb5,[0xc6],sleep_time=0.04)
+ return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+ def get_current(self):
+ '''Not supported yet'''
+ return [0] * 21
+ def get_temperature(self):
+ self.get_thumb_threshold()
+ self.get_index_threshold()
+ self.get_middle_threshold()
+ self.get_ring_threshold()
+ self.get_little_threshold()
+ return self.x61+self.x62+self.x63+self.x64+self.x65
+
+ def get_serial_number(self):
+ return [0] * 6
+
+ def get_finger_order(self):
+ return [
+ "thumb_root",
+ "index_finger_root",
+ "middle_finger_root",
+ "ring_finger_root",
+ "little_finger_root",
+ "thumb_abduction",
+ "index_finger_abduction",
+ "middle_finger_abduction",
+ "ring_finger_abduction",
+ "little_finger_abduction",
+ "thumb_roll",
+ "reserved",
+ "reserved",
+ "reserved",
+ "reserved",
+ "thumb_middle_joint",
+ "reserved",
+ "reserved",
+ "reserved",
+ "reserved",
+ "thumb_tip",
+ "index_finger_tip",
+ "middle_finger_tip",
+ "ring_finger_tip",
+ "little_finger_tip"
+ ]
+
+ def clear_faults(self):
+ '''Clear motor faults'''
+ self.send_command(0x83, [1, 1, 1, 1, 1],sleep_time=0.003)
+ return self.x83
+
+ def close_can_interface(self):
+ if self.bus:
+ self.bus.shutdown() # Close CAN bus
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l24_can.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l24_can.py
new file mode 100644
index 0000000..d5192f6
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l24_can.py
@@ -0,0 +1,448 @@
+#!/usr/bin/env python3
+import can
+import time,sys,os
+import threading
+import numpy as np
+from enum import Enum
+current_dir = os.path.dirname(os.path.abspath(__file__))
+target_dir = os.path.abspath(os.path.join(current_dir, ".."))
+sys.path.append(target_dir)
+from utils.color_msg import ColorMsg
+
+class FrameProperty(Enum):
+ INVALID_FRAME_PROPERTY = 0x00 # 无效的can帧属性 | 无返回
+ # 并行指令区域
+ ROLL_POS = 0x01 # 横滚关节位置 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+ YAW_POS = 0x02 # 航向关节位置 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+ ROOT1_POS = 0x03 # 指根1关节位置 | 最接近手掌的指根关节
+ ROOT2_POS = 0x04 # 指根2关节位置 | 最接近手掌的指根关节
+ ROOT3_POS = 0x05 # 指根3关节位置 | 最接近手掌的指根关节
+ TIP_POS = 0x06 # 指尖关节位置 | 最接近手掌的指根关节
+
+ ROLL_SPEED = 0x09 # 横滚关节速度 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+ YAW_SPEED = 0x0A # 航向关节速度 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+ ROOT1_SPEED = 0x0B # 指根1关节速度 | 最接近手掌的指根关节
+ ROOT2_SPEED = 0x0C # 指根2关节速度 | 最接近手掌的指根关节
+ ROOT3_SPEED = 0x0D # 指根3关节速度 | 最接近手掌的指根关节
+ TIP_SPEED = 0x0E # 指尖关节速度 | 最接近手掌的指根关节
+
+ ROLL_TORQUE = 0x11 # 横滚关节扭矩 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+ YAW_TORQUE = 0x12 # 航向关节扭矩 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+ ROOT1_TORQUE = 0x13 # 指根1关节扭矩 | 最接近手掌的指根关节
+ ROOT2_TORQUE = 0x14 # 指根2关节扭矩 | 最接近手掌的指根关节
+ ROOT3_TORQUE = 0x15 # 指根3关节扭矩 | 最接近手掌的指根关节
+ TIP_TORQUE = 0x16 # 指尖关节扭矩 | 最接近手掌的指根关节
+
+ ROLL_FAULT = 0x19 # 横滚关节故障码 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+ YAW_FAULT = 0x1A # 航向关节故障码 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+ ROOT1_FAULT = 0x1B # 指根1关节故障码 | 最接近手掌的指根关节
+ ROOT2_FAULT = 0x1C # 指根2关节故障码 | 最接近手掌的指根关节
+ ROOT3_FAULT = 0x1D # 指根3关节故障码 | 最接近手掌的指根关节
+ TIP_FAULT = 0x1E # 指尖关节故障码 | 最接近手掌的指根关节
+
+ ROLL_TEMPERATURE = 0x21 # 横滚关节温度 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+ YAW_TEMPERATURE = 0x22 # 航向关节温度 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+ ROOT1_TEMPERATURE = 0x23 # 指根1关节温度 | 最接近手掌的指根关节
+ ROOT2_TEMPERATURE = 0x24 # 指根2关节温度 | 最接近手掌的指根关节
+ ROOT3_TEMPERATURE = 0x25 # 指根3关节温度 | 最接近手掌的指根关节
+ TIP_TEMPERATURE = 0x26 # 指尖关节温度 | 最接近手掌的指根关节
+ # 并行指令区域
+
+ # 串行指令区域
+ THUMB_POS = 0x41 # 大拇指指关节位置 | 返回本类型数据
+ INDEX_POS = 0x42 # 食指关节位置 | 返回本类型数据
+ MIDDLE_POS = 0x43 # 中指关节位置 | 返回本类型数据
+ RING_POS = 0x44 # 无名指关节位置 | 返回本类型数据
+ LITTLE_POS = 0x45 # 小拇指关节位置 | 返回本类型数据
+
+ THUMB_SPEED = 0x49 # 大拇指速度 | 返回本类型数据
+ INDEX_SPEED = 0x4A # 食指速度 | 返回本类型数据
+ MIDDLE_SPEED = 0x4B # 中指速度 | 返回本类型数据
+ RING_SPEED = 0x4C # 无名指速度 | 返回本类型数据
+ LITTLE_SPEED = 0x4D # 小拇指速度 | 返回本类型数据
+
+ THUMB_TORQUE = 0x51 # 大拇指扭矩 | 返回本类型数据
+ INDEX_TORQUE = 0x52 # 食指扭矩 | 返回本类型数据
+ MIDDLE_TORQUE = 0x53 # 中指扭矩 | 返回本类型数据
+ RING_TORQUE = 0x54 # 无名指扭矩 | 返回本类型数据
+ LITTLE_TORQUE = 0x55 # 小拇指扭矩 | 返回本类型数据
+
+ THUMB_FAULT = 0x59 # 大拇指故障码 | 返回本类型数据
+ INDEX_FAULT = 0x5A # 食指故障码 | 返回本类型数据
+ MIDDLE_FAULT = 0x5B # 中指故障码 | 返回本类型数据
+ RING_FAULT = 0x5C # 无名指故障码 | 返回本类型数据
+ LITTLE_FAULT = 0x5D # 小拇指故障码 | 返回本类型数据
+
+ THUMB_TEMPERATURE = 0x61 # 大拇指温度 | 返回本类型数据
+ INDEX_TEMPERATURE = 0x62 # 食指温度 | 返回本类型数据
+ MIDDLE_TEMPERATURE = 0x63 # 中指温度 | 返回本类型数据
+ RING_TEMPERATURE = 0x64 # 无名指温度 | 返回本类型数据
+ LITTLE_TEMPERATURE = 0x65 # 小拇指温度 | 返回本类型数据
+ # 串行指令区域
+
+ # 合并指令区域，同一手指非必要单控数据合并
+ FINGER_SPEED = 0x81 # 手指速度 | 返回本类型数据
+ FINGER_TORQUE = 0x82 # 转矩 | 返回本类型数据
+ FINGER_FAULT = 0x83 # 手指故障码 | 返回本类型数据
+
+ # 指尖传感器数据组
+ HAND_NORMAL_FORCE = 0x90 # 五指法向压力
+ HAND_TANGENTIAL_FORCE = 0x91 # 五指切向压力
+ HAND_TANGENTIAL_FORCE_DIR = 0x92 # 五指切向方向
+ HAND_APPROACH_INC = 0x93 # 五指接近感应
+
+ THUMB_ALL_DATA = 0x98 # 大拇指所有数据
+ INDEX_ALL_DATA = 0x99 # 食指所有数据
+ MIDDLE_ALL_DATA = 0x9A # 中指所有数据
+ RING_ALL_DATA = 0x9B # 无名指所有数据
+ LITTLE_ALL_DATA = 0x9C # 小拇指所有数据
+ # 动作指令 ·ACTION
+ ACTION_PLAY = 0xA0 # 动作
+
+ # 配置命令·CONFIG
+ HAND_UID = 0xC0 # 设备唯一标识码
+ HAND_HARDWARE_VERSION = 0xC1 # 硬件版本
+ HAND_SOFTWARE_VERSION = 0xC2 # 软件版本
+ HAND_COMM_ID = 0xC3 # 设备id
+ HAND_FACTORY_RESET = 0xCE # 恢复出厂设置
+ HAND_SAVE_PARAMETER = 0xCF # 保存参数
+
+ WHOLE_FRAME = 0xF0 # 整帧传输 | 返回一字节帧属性+整个结构体485及网络传输专属
+
+class LinkerHandL24Can:
+ def __init__(self, config, can_channel='can0', baudrate=1000000, can_id=0x28):
+ self.config = config
+ self.can_id = can_id
+ self.running = True
+ self.x01, self.x02, self.x03, self.x04,self.x05,self.x06,self.x07, self.x08,self.x09,self.x0A,self.x0B,self.x0C,self.x0D,self.x0E,self.speed = [],[],[],[],[],[],[],[],[],[],[],[],[],[],[]
+ # 速度
+ self.x49, self.x4a, self.x4b, self.x4c, self.x4d = [],[],[],[],[]
+ self.x41,self.x42,self.x43,self.x44,self.x45 = [],[],[],[],[]
+ # 根据操作系统初始化 CAN 总线
+ if sys.platform == "linux":
+ self.bus = can.interface.Bus(
+ channel=can_channel, interface="socketcan", bitrate=baudrate,
+ can_filters=[{"can_id": can_id, "can_mask": 0x7FF}]
+ )
+ elif sys.platform == "win32":
+ self.bus = can.interface.Bus(
+ channel=can_channel, interface='pcan', bitrate=baudrate,
+ can_filters=[{"can_id": can_id, "can_mask": 0x7FF}]
+ )
+ else:
+ raise EnvironmentError("Unsupported platform for CAN interface")
+
+ # 根据 can_id 初始化 publisher 和相关参数
+ if can_id == 0x28: # 左手
+ self.hand_exists = config['LINKER_HAND']['LEFT_HAND']['EXISTS']
+ self.hand_joint = config['LINKER_HAND']['LEFT_HAND']['JOINT']
+ self.hand_names = config['LINKER_HAND']['LEFT_HAND']['NAME']
+ elif can_id == 0x27: # 右手
+
+ self.hand_exists = config['LINKER_HAND']['RIGHT_HAND']['EXISTS']
+ self.hand_joint = config['LINKER_HAND']['RIGHT_HAND']['JOINT']
+ self.hand_names = config['LINKER_HAND']['RIGHT_HAND']['NAME']
+
+
+ # 启动接收线程
+ self.receive_thread = threading.Thread(target=self.receive_response)
+ self.receive_thread.daemon = True
+ self.receive_thread.start()
+
+ def send_command(self, frame_property, data_list):
+ """
+ 发送命令到 CAN 总线
+ :param frame_property: 数据帧属性
+ :param data_list: 数据载荷
+ """
+ frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+ data = [frame_property_value] + [int(val) for val in data_list]
+ msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+ try:
+ self.bus.send(msg)
+ #print(f"Message sent: ID={hex(self.can_id)}, Data={data}")
+ except can.CanError as e:
+ print(f"Failed to send message: {e}")
+ time.sleep(0.002)
+
+ def receive_response(self):
+ """
+ 接收并处理 CAN 总线的响应消息
+ """
+ while self.running:
+ try:
+ msg = self.bus.recv(timeout=1.0) # 阻塞接收，1 秒超时
+ if msg:
+ self.process_response(msg)
+ except can.CanError as e:
+ print(f"Error receiving message: {e}")
+
+
+ def set_joint_positions(self, joint_ranges):
+ if len(joint_ranges) == 25:
+ l24_pose = self.joint_map(joint_ranges)
+ # 使用列表推导式将列表每6个元素切成一个子数组
+ chunks = [l24_pose[i:i+6] for i in range(0, 30, 6)]
+ self.send_command(FrameProperty.THUMB_POS, chunks[0])
+ self.send_command(FrameProperty.INDEX_POS, chunks[1])
+ self.send_command(FrameProperty.MIDDLE_POS, chunks[2])
+ self.send_command(FrameProperty.RING_POS, chunks[3])
+ self.send_command(FrameProperty.LITTLE_POS, chunks[4])
+ #self.set_tip_positions(joint_ranges[:5])
+ #print(l24_pose)
+
+ # 设置所有手指横滚关节位置
+ def set_roll_positions(self, joint_ranges):
+ self.send_command(FrameProperty.ROLL_POS, joint_ranges)
+ # 设置所有手指航向关节位置
+ def set_yaw_positions(self, joint_ranges):
+ self.send_command(FrameProperty.YAW_POS, joint_ranges)
+ # 设置所有手指指根1关节位置
+ def set_root1_positions(self, joint_ranges):
+ self.send_command(FrameProperty.ROOT1_POS, joint_ranges)
+ # 设置所有手指指根2关节位置
+ def set_root2_positions(self, joint_ranges):
+ self.send_command(FrameProperty.ROOT2_POS, joint_ranges)
+ # 设置所有手指指根3关节位置
+ def set_root3_positions(self, joint_ranges):
+ self.send_command(FrameProperty.ROOT3_POS, joint_ranges)
+ # 设置所有手指指尖关节位置
+ def set_tip_positions(self, joint_ranges=[80]*5):
+ self.send_command(FrameProperty.TIP_POS, joint_ranges)
+ # 获取大拇指指关节位置
+ def get_thumb_positions(self,j=[0]):
+ self.send_command(FrameProperty.THUMB_POS, j)
+ # 获取食指关节位置
+ def get_index_positions(self, j=[0]):
+ self.send_command(FrameProperty.INDEX_POS,j)
+ # 获取中指关节位置
+ def get_middle_positions(self, j=[0]):
+ self.send_command(FrameProperty.MIDDLE_POS,j)
+ # 获取无名指关节位置
+ def get_ring_positions(self, j=[0]):
+ self.send_command(FrameProperty.RING_POS,j)
+ # 获取小拇指关节位置
+ def get_little_positions(self, j=[0]):
+ self.send_command(FrameProperty.LITTLE_POS, j)
+ # 失能01模式
+ def set_disability_mode(self, j=[1,1,1,1,1]):
+ self.send_command(0x85,j)
+ # 使能00模式
+ def set_enable_mode(self, j=[00,00,00,00,00]):
+ self.send_command(0x85,j)
+
+
+ def set_speed(self, speed):
+ self.speed = [speed]*6
+ ColorMsg(msg=f"L24设置速度为:{self.speed}", color="yellow")
+ self.send_command(FrameProperty.THUMB_SPEED, self.speed)
+ self.send_command(FrameProperty.INDEX_SPEED, self.speed)
+ self.send_command(FrameProperty.MIDDLE_SPEED, self.speed)
+ self.send_command(FrameProperty.RING_SPEED, self.speed)
+ self.send_command(FrameProperty.LITTLE_SPEED, self.speed)
+
+ def set_finger_torque(self, torque):
+ self.send_command(0x42, torque)
+
+ def request_device_info(self):
+ self.send_command(0xC0, [0])
+ self.send_command(0xC1, [0])
+ self.send_command(0xC2, [0])
+
+ def save_parameters(self):
+ self.send_command(0xCF, [])
+ def process_response(self, msg):
+ if msg.arbitration_id == self.can_id:
+ frame_type = msg.data[0]
+ response_data = msg.data[1:]
+ if frame_type == 0x01:
+ self.x01 = list(response_data)
+ elif frame_type == 0x02:
+ self.x02 = list(response_data)
+ elif frame_type == 0x03:
+ self.x03 = list(response_data)
+ elif frame_type == 0x04:
+ self.x04 = list(response_data)
+ elif frame_type == 0x05:
+ self.x05 = list(response_data)
+ elif frame_type == 0x06:
+ self.x06 = list(response_data)
+ print("_-"*20)
+ print(self.x06)
+ elif frame_type == 0xC0:
+ print(f"Device ID info: {response_data}")
+ if self.can_id == 0x28:
+ self.right_hand_info = response_data
+ elif self.can_id == 0x27:
+ self.left_hand_info = response_data
+ elif frame_type == 0x08:
+ self.x08 = list(response_data)
+ elif frame_type == 0x09:
+ self.x09 = list(response_data)
+ elif frame_type == 0x0A:
+ self.x0A = list(response_data)
+ elif frame_type == 0x0B:
+ self.x0B = list(response_data)
+ elif frame_type == 0x0C:
+ self.x0C = list(response_data)
+ elif frame_type == 0x0D:
+ self.x0D = list(response_data)
+ elif frame_type == 0x22:
+ #ColorMsg(msg=f"五指切向压力方向：{list(response_data)}")
+ d = list(response_data)
+ self.tangential_force_dir = [float(i) for i in d]
+ elif frame_type == 0x23:
+ #ColorMsg(msg=f"五指接近度：{list(response_data)}")
+ d = list(response_data)
+ self.approach_inc = [float(i) for i in d]
+ elif frame_type == 0x41: # 拇指关节位置返回值
+ self.x41 = list(response_data)
+ elif frame_type == 0x42: # 食指关节位置返回值
+ self.x42 = list(response_data)
+ elif frame_type == 0x43: # 中指关节位置返回值
+ self.x43 = list(response_data)
+ elif frame_type == 0x44: # 无名指关节位置返回值
+ self.x44 = list(response_data)
+ elif frame_type == 0x45: # 小拇指关节位置返回值
+ self.x45 = list(response_data)
+ elif frame_type == 0x49: # 拇指速度返回值
+ self.x49 = list(response_data)
+ elif frame_type == 0x4a: # 食指速度返回值
+ self.x4a = list(response_data)
+ elif frame_type == 0x4b: # 中指速度返回值
+ self.x4b = list(response_data)
+ elif frame_type == 0x4c: # 无名指速度返回值
+ self.x4c = list(response_data)
+ elif frame_type == 0x4d: # 小拇指速度返回值
+ self.x4d = list(response_data)
+
+ # topic映射L24
+ def joint_map(self, pose):
+ # L24 CAN数据默认接收30个数据
+ l24_pose = [0.0] * 30 # 初始化l24_pose为30个0.0
+
+ # 映射表，通过字典简化映射关系
+ mapping = {
+ 0: 10, 1: 5, 2: 0, 3: 15, 4: None, 5: 20,
+ 6: None, 7: 6, 8: 1, 9: 16, 10: None, 11: 21,
+ 12: None, 13: None, 14: 2, 15: 17, 16: None, 17: 22,
+ 18: None, 19: 8, 20: 3, 21: 18, 22: None, 23: 23,
+ 24: None, 25: 9, 26: 4, 27: 19, 28: None, 29: 24
+ }
+
+ # 遍历映射字典，进行值的映射
+ for l24_idx, pose_idx in mapping.items():
+ if pose_idx is not None:
+ l24_pose[l24_idx] = pose[pose_idx]
+
+ return l24_pose
+
+ # 将L24的状态值转换为CMD格式的状态值
+ def state_to_cmd(self, l24_state):
+ # L24 CAN默认接收30个数据，初始化pose为25个0.0
+ pose = [0.0] * 25 # 原来控制L24的指令数据为25个
+
+ # 映射关系，字典中存储l24_state索引和pose索引之间的映射关系
+ mapping = {
+ 0: 10, 1: 5, 2: 0, 3: 15, 5: 20, 7: 6,
+ 8: 1, 9: 16, 11: 21, 14: 2, 15: 17, 17: 22,
+ 19: 8, 20: 3, 21: 18, 23: 23, 25: 9, 26: 4,
+ 27: 19, 29: 24
+ }
+ # 遍历映射字典，更新pose的值
+ for l24_idx, pose_idx in mapping.items():
+ pose[pose_idx] = l24_state[l24_idx]
+ return pose
+
+ # 获取所有关节数据
+ def get_current_status(self, j=''):
+ time.sleep(0.01)
+ self.send_command(FrameProperty.THUMB_POS, j)
+ self.send_command(FrameProperty.INDEX_POS,j)
+ self.send_command(FrameProperty.MIDDLE_POS,j)
+ self.send_command(FrameProperty.RING_POS,j)
+ self.send_command(FrameProperty.LITTLE_POS, j)
+ #return self.x41, self.x42, self.x43, self.x44, self.x45
+ time.sleep(0.1)
+ state= self.x41+ self.x42+ self.x43+ self.x44+ self.x45
+ if len(state) == 30:
+ l24_state = self.state_to_cmd(l24_state=state)
+ return l24_state
+
+ def get_speed(self,j=''):
+ time.sleep(0.1)
+ self.send_command(FrameProperty.THUMB_SPEED, j) # 大拇指速度
+ self.send_command(FrameProperty.INDEX_SPEED, j) # 食指速度
+ self.send_command(FrameProperty.MIDDLE_SPEED, j) # 中指速度
+ self.send_command(FrameProperty.RING_SPEED, j) # 无名指速度
+ self.send_command(FrameProperty.LITTLE_SPEED, j) # 小拇指速度
+ speed = self.x49+ self.x4a+ self.x4b+ self.x4c+ self.x4d
+ if len(speed) == 30:
+ l24_speed = self.state_to_cmd(l24_state=speed)
+ return l24_speed
+
+ def get_finger_torque(self):
+ return self.finger_torque
+ # def get_current(self):
+ # return self.x06
+ # def get_fault(self):
+ # return self.x07
+
+ def clear_faults(self, finger_mask=[1, 1, 1, 1, 1]):
+ """L24 暂不支持清除故障码"""
+ pass
+
+ def close_can_interface(self):
+ if self.bus:
+ self.bus.shutdown() # 关闭 CAN 总线
+
+ '''
+ 这个方法只用于展示数据关系映射，使用的话最好使用上面的方法
+ '''
+ def joint_map_2(self, pose):
+ l24_pose = [0.0]*30 #L24 CAN默认接收30个数据 pose控制L24发送的指令数据默认25个，这里进行映射
+ '''
+ 需要进行映射
+ # L24 CAN数据格式
+ #["拇指横摆0-10", "拇指侧摆1-5", "拇指根部2-0", "拇指中部3-15", "预留4-", "拇指指尖5-20", "预留6-", "食指侧摆7-6", "食指根部8-1", "食指中部9-16", "预留10-", "食指指尖11-21", "预留12-", "预留13-", "中指根部14-2", "中指中部15-17", "预留16-", "中指指尖17-22", "预留18-", "无名指侧摆19-8", "无名指根部20-3", "无名指中部21-18", "预留22-", "无名指指尖23-23", "预留24-", "小指侧摆25-9", "小指根部26-4", "小指中部27-19", "预留28-", "小指指尖29-24"]
+ # CMD 接收到的数据格式
+ #["拇指根部0", "食指根部1", "中指根部2", "无名指根部3","小指根部4","拇指侧摆5","食指侧摆6","中指侧摆","无名指侧摆8","小指侧摆9","拇指横摆10","预留","预留","预留","预留","拇指中部15","食指中部16","中指中部17","无名指中部18","小指中部19","拇指指尖20","食指指尖21","中指指尖22","无名指指尖23","小指指尖24"]
+ '''
+ l24_pose[0] = pose[10]
+ l24_pose[1] = pose[5]
+ l24_pose[2] = pose[0]
+ l24_pose[3] = pose[15]
+ l24_pose[4] = 0.0
+ l24_pose[5] = pose[20]
+ l24_pose[6] = 0.0
+ l24_pose[7] = pose[6]
+ l24_pose[8] = pose[1]
+ l24_pose[9] = pose[16]
+ l24_pose[10] = 0.0
+ l24_pose[11] = pose[21]
+ l24_pose[12] = 0.0
+ l24_pose[13] = 0.0
+ l24_pose[14] = pose[2]
+ l24_pose[15] = pose[17]
+ l24_pose[16] = 0.0
+ l24_pose[17] = pose[22]
+ l24_pose[18] = 0.0
+ l24_pose[19] = pose[8]
+ l24_pose[20] = pose[3]
+ l24_pose[21] = pose[18]
+ l24_pose[22] = 0.0
+ l24_pose[23] = pose[23]
+ l24_pose[24] = 0.0
+ l24_pose[25] = pose[9]
+ l24_pose[26] = pose[4]
+ l24_pose[27] = pose[19]
+ l24_pose[28] = 0.0
+ l24_pose[29] = pose[24]
+ return l24_pose
+
+ def get_finger_order(self):
+ return []
+ def get_serial_number(self):
+ return [0] * 6
+ def show_fun_table(self):
+ pass
\ No newline at end of file
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l25_can.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l25_can.py
new file mode 100644
index 0000000..97f91b4
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l25_can.py
@@ -0,0 +1,848 @@
+#!/usr/bin/env python3
+import can
+import time,sys,os
+import threading
+import numpy as np
+from enum import Enum
+from utils.open_can import OpenCan
+from utils.color_msg import ColorMsg
+from can.exceptions import CanError
+current_dir = os.path.dirname(os.path.abspath(__file__))
+target_dir = os.path.abspath(os.path.join(current_dir, ".."))
+sys.path.append(target_dir)
+
+class FrameProperty(Enum):
+ INVALID_FRAME_PROPERTY = 0x00 # Invalid CAN frame property | No response
+ # Parallel command area
+ ROLL_POS = 0x01 # Roll joint position | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger [10,11,12,13,14]
+ YAW_POS = 0x02 # Yaw joint position | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger [5,6,7,8,9]
+ ROOT1_POS = 0x03 # Root1 joint position | The root joint closest to the palm [0,1,2,3,4]
+ ROOT2_POS = 0x04 # Root2 joint position | The root joint closest to the palm [15, 16,17,18,19]
+ ROOT3_POS = 0x05 # Root3 joint position | The root joint closest to the palm Not available
+ TIP_POS = 0x06 # Fingertip joint position | The root joint closest to the palm [20,21,22,23,24]
+
+ ROLL_SPEED = 0x09 # Roll joint speed | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger
+ YAW_SPEED = 0x0A # Yaw joint speed | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger
+ ROOT1_SPEED = 0x0B # Root1 joint speed | The root joint closest to the palm
+ ROOT2_SPEED = 0x0C # Root2 joint speed | The root joint closest to the palm
+ ROOT3_SPEED = 0x0D # Root3 joint speed | The root joint closest to the palm
+ TIP_SPEED = 0x0E # Fingertip joint speed | The root joint closest to the palm
+
+ ROLL_TORQUE = 0x11 # Roll joint torque | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger
+ YAW_TORQUE = 0x12 # Yaw joint torque | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger
+ ROOT1_TORQUE = 0x13 # Root1 joint torque | The root joint closest to the palm
+ ROOT2_TORQUE = 0x14 # Root2 joint torque | The root joint closest to the palm
+ ROOT3_TORQUE = 0x15 # Root3 joint torque | The root joint closest to the palm
+ TIP_TORQUE = 0x16 # Fingertip joint torque | The root joint closest to the palm
+
+ ROLL_FAULT = 0x19 # Roll joint fault code | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger
+ YAW_FAULT = 0x1A # Yaw joint fault code | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger
+ ROOT1_FAULT = 0x1B # Root1 joint fault code | The root joint closest to the palm
+ ROOT2_FAULT = 0x1C # Root2 joint fault code | The root joint closest to the palm
+ ROOT3_FAULT = 0x1D # Root3 joint fault code | The root joint closest to the palm
+ TIP_FAULT = 0x1E # Fingertip joint fault code | The root joint closest to the palm
+
+ ROLL_TEMPERATURE = 0x21 # Roll joint temperature | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger
+ YAW_TEMPERATURE = 0x22 # Yaw joint temperature | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger
+ ROOT1_TEMPERATURE = 0x23 # Root1 joint temperature | The root joint closest to the palm
+ ROOT2_TEMPERATURE = 0x24 # Root2 joint temperature | The root joint closest to the palm
+ ROOT3_TEMPERATURE = 0x25 # Root3 joint temperature | The root joint closest to the palm
+ TIP_TEMPERATURE = 0x26 # Fingertip joint temperature | The root joint closest to the palm
+ # Parallel command area
+
+ # Serial command area
+ THUMB_POS = 0x41 # Thumb joint position | Returns this type of data
+ INDEX_POS = 0x42 # Index finger joint position | Returns this type of data
+ MIDDLE_POS = 0x43 # Middle finger joint position | Returns this type of data
+ RING_POS = 0x44 # Ring finger joint position | Returns this type of data
+ LITTLE_POS = 0x45 # Little finger joint position | Returns this type of data
+
+ THUMB_SPEED = 0x49 # Thumb speed | Returns this type of data
+ INDEX_SPEED = 0x4A # Index finger speed | Returns this type of data
+ MIDDLE_SPEED = 0x4B # Middle finger speed | Returns this type of data
+ RING_SPEED = 0x4C # Ring finger speed | Returns this type of data
+ LITTLE_SPEED = 0x4D # Little finger speed | Returns this type of data
+
+ THUMB_TORQUE = 0x51 # Thumb torque | Returns this type of data
+ INDEX_TORQUE = 0x52 # Index finger torque | Returns this type of data
+ MIDDLE_TORQUE = 0x53 # Middle finger torque | Returns this type of data
+ RING_TORQUE = 0x54 # Ring finger torque | Returns this type of data
+ LITTLE_TORQUE = 0x55 # Little finger torque | Returns this type of data
+
+ THUMB_FAULT = 0x59 # Thumb fault code | Returns this type of data
+ INDEX_FAULT = 0x5A # Index finger fault code | Returns this type of data
+ MIDDLE_FAULT = 0x5B # Middle finger fault code | Returns this type of data
+ RING_FAULT = 0x5C # Ring finger fault code | Returns this type of data
+ LITTLE_FAULT = 0x5D # Little finger fault code | Returns this type of data
+
+ THUMB_TEMPERATURE = 0x61 # Thumb temperature | Returns this type of data
+ INDEX_TEMPERATURE = 0x62 # Index finger temperature | Returns this type of data
+ MIDDLE_TEMPERATURE = 0x63 # Middle finger temperature | Returns this type of data
+ RING_TEMPERATURE = 0x64 # Ring finger temperature | Returns this type of data
+ LITTLE_TEMPERATURE = 0x65 # Little finger temperature | Returns this type of data
+ # Serial command area
+
+ # Merged command area, non-essential single control data of the same finger is merged
+ FINGER_SPEED = 0x81 # Finger speed | Returns this type of data
+ FINGER_TORQUE = 0x82 # Torque | Returns this type of data
+ FINGER_FAULT = 0x83 # Finger fault code | Returns this type of data
+
+ # Fingertip sensor data group
+ HAND_NORMAL_FORCE = 0x90 # Normal force of five fingers
+ HAND_TANGENTIAL_FORCE = 0x91 # Tangential force of five fingers
+ HAND_TANGENTIAL_FORCE_DIR = 0x92 # Tangential direction of five fingers
+ HAND_APPROACH_INC = 0x93 # Proximity sensing of five fingers
+
+ THUMB_ALL_DATA = 0x98 # All data of thumb
+ INDEX_ALL_DATA = 0x99 # All data of index finger
+ MIDDLE_ALL_DATA = 0x9A # All data of middle finger
+ RING_ALL_DATA = 0x9B # All data of ring finger
+ LITTLE_ALL_DATA = 0x9C # All data of little finger
+ # Action command ·ACTION
+ ACTION_PLAY = 0xA0 # Action
+
+ # Configuration command ·CONFIG
+ HAND_UID = 0xC0 # Device unique identifier
+ HAND_HARDWARE_VERSION = 0xC1 # Hardware version
+ HAND_SOFTWARE_VERSION = 0xC2 # Software version
+ HAND_COMM_ID = 0xC3 # Device id
+ HAND_FACTORY_RESET = 0xCE # Restore factory settings
+ HAND_SAVE_PARAMETER = 0xCF # Save parameters
+
+ WHOLE_FRAME = 0xF0 # Whole frame transmission | Returns one byte frame property + the entire structure for 485 and network transmission only
+
+class LinkerHandL25Can:
+ def __init__(self, can_channel='can0', baudrate=1000000, can_id=0x28,yaml=""):
+ self.can_id = can_id
+ self.can_channel = can_channel
+ self.baudrate = baudrate
+ self.open_can = OpenCan(load_yaml=yaml)
+
+ self.running = True
+ self.last_thumb_pos, self.last_index_pos,self.last_ring_pos,self.last_middle_pos, self.last_little_pos = None,None,None,None,None
+ self.x01, self.x02, self.x03, self.x04,self.x05,self.x06,self.x07, self.x08,self.x09,self.x0A,self.x0B,self.x0C,self.x0D,self.x0E,self.speed = [],[],[],[],[],[],[],[],[],[],[],[],[],[],[]
+ self.last_root1,self.last_yaw,self.last_roll,self.last_root2,self.last_tip = None,None,None,None,None
+ # 速度
+ self.x49, self.x4a, self.x4b, self.x4c, self.x4d,self.xc1 = [],[],[],[],[],[]
+ self.x41,self.x42,self.x43,self.x44,self.x45 = [],[],[],[],[]
+ # 扭矩
+ self.x51, self.x52, self.x53, self.x54,self.x55 = [],[],[],[],[]
+ # 故障码
+ self.x59,self.x5a,self.x5b,self.x5c,self.x5d = [],[],[],[],[]
+ # 温度阈值
+ self.x61,self.x62,self.x63,self.x64,self.x65 = [],[],[],[],[]
+ # 压感
+ self.x90,self.x91,self.x92,self.x93 = [],[],[],[]
+ # 新压感
+ self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+ self.thumb_matrix = np.full((12, 6), -1)
+ self.index_matrix = np.full((12, 6), -1)
+ self.middle_matrix = np.full((12, 6), -1)
+ self.ring_matrix = np.full((12, 6), -1)
+ self.little_matrix = np.full((12, 6), -1)
+ self.matrix_map = {
+ 0: 0,
+ 16: 1,
+ 32: 2,
+ 48: 3,
+ 64: 4,
+ 80: 5,
+ 96: 6,
+ 112: 7,
+ 128: 8,
+ 144: 9,
+ 160: 10,
+ 176: 11,
+ }
+ # 根据操作系统初始化 CAN 总线
+ # try:
+ # if sys.platform == "linux":
+ # self.open_can.open_can(self.can_channel)
+ # time.sleep(0.1)
+ # self.bus = can.interface.Bus(
+ # channel=can_channel, interface="socketcan", bitrate=baudrate,
+ # can_filters=[{"can_id": can_id, "can_mask": 0x7FF}]
+ # )
+ # elif sys.platform == "win32":
+ # self.bus = can.interface.Bus(
+ # channel=can_channel, interface='pcan', bitrate=baudrate,
+ # can_filters=[{"can_id": can_id, "can_mask": 0x7FF}]
+ # )
+ # else:
+ # raise EnvironmentError("Unsupported platform for CAN interface")
+ # except:
+ # print("Please insert CAN device")
+ self.bus = self.init_can_bus(channel=self.can_channel, baudrate=baudrate)
+ # 启动接收线程
+ self.receive_thread = threading.Thread(target=self.receive_response)
+ self.receive_thread.daemon = True
+ self.receive_thread.start()
+
+ def init_can_bus(self, channel, baudrate):
+ """
+ 尝试按优先级连接 CAN 总线，并实现回退机制。
+ """
+ # --- 统一异常处理块开始 ---
+ try:
+ if sys.platform == "linux":
+ # Linux 优先级：1. socketcan
+ try:
+ self.open_can.open_can(self.can_channel)
+ # 尝试 socketcan
+ bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan')
+ ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow")
+ raise # 重新抛出异常，让外层 try 捕获
+ elif sys.platform == "win32":
+ # Windows 优先级：1. pcan
+ try:
+ bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow")
+ # Windows 优先级：2. candle (回退方法)
+ try:
+ bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow")
+ raise # 两个接口都失败，抛出异常
+ else:
+ raise EnvironmentError("Unsupported platform for CAN interface")
+ # --- 统一异常处理块结束 ---
+ except Exception as e:
+ # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError）
+ ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red")
+ # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行
+ raise
+
+ def send_command(self, frame_property, data_list):
+ """
+ Send command to CAN bus
+ :param frame_property: Data frame properties
+ :param data_list: Data payload
+ """
+ frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+ data = [frame_property_value] + [int(val) for val in data_list]
+ msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+ try:
+ self.bus.send(msg)
+ #print(f"Message sent: ID={hex(self.can_id)}, Data={data}")
+ except can.CanError as e:
+ print(f"Failed to send message: {e}")
+ self.open_can.open_can(self.can_channel)
+ time.sleep(1)
+ self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel)
+ time.sleep(1)
+ if self.is_can:
+ self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate)
+ else:
+ print("Reconnecting CAN devices ....")
+ time.sleep(0.001)
+
+ def receive_response(self):
+ """
+ Receive and process response messages from CAN bus
+ """
+ while self.running:
+ try:
+ msg = self.bus.recv(timeout=1.0) # 阻塞接收，1 秒超时
+ if msg:
+ self.process_response(msg)
+ except can.CanError as e:
+ print(f"Error receiving message: {e}")
+
+
+ def set_joint_positions(self, joint_ranges):
+ if len(joint_ranges) == 25:
+ l25_pose = self.joint_map(joint_ranges)
+ # 使用列表推导式将列表每6个元素切成一个子数组
+ chunks = [l25_pose[i:i+6] for i in range(0, 30, 6)]
+ self.send_command(FrameProperty.THUMB_POS, chunks[0])
+ #time.sleep(0.001)
+ self.send_command(FrameProperty.INDEX_POS, chunks[1])
+ #time.sleep(0.001)
+ self.send_command(FrameProperty.MIDDLE_POS, chunks[2])
+ #time.sleep(0.001)
+ self.send_command(FrameProperty.RING_POS, chunks[3])
+ #time.sleep(0.001)
+ self.send_command(FrameProperty.LITTLE_POS, chunks[4])
+ #time.sleep(0.001)
+
+ def set_joint_positions_by_topic(self, joint_ranges):
+ if len(joint_ranges) == 25:
+ # Finger Joint Position Constants
+ #ROLL_POS = 0x01 # Roll joint position | Coordinate system based on finger base, rotation angle defined when finger is straight [10,11,12,13,14]
+ #YAW_POS = 0x02 # Yaw joint position | Coordinate system based on finger base, rotation angle defined when finger is straight [5,6,7,8,9]
+ #ROOT1_POS = 0x03 # Root1 joint position | Joint closest to the palm [0,1,2,3,4]
+ #ROOT2_POS = 0x04 # Root2 joint position | Joint closest to the palm [15,16,17,18,19]
+ #ROOT3_POS = 0x05 # Root3 joint position | Joint closest to the palm (currently unused)
+ #TIP_POS = 0x06 # Tip joint position | Joint closest to the palm [20,21,22,23,24]
+
+ # Finger joint names mapping (Chinese to English translation):
+ # ["Thumb root", "Index root", "Middle root", "Ring root", "Pinky root",
+ # "Thumb yaw", "Index yaw", "Middle yaw", "Ring yaw", "Pinky yaw",
+ # "Thumb roll", "Reserved", "Reserved", "Reserved", "Reserved",
+ # "Thumb middle", "Index middle", "Middle middle", "Ring middle", "Pinky middle",
+ # "Thumb tip", "Index tip", "Middle tip", "Ring tip", "Pinky tip"]
+
+
+ l25_pose = self.slice_list(joint_ranges,5)
+ if self._list_d_value(self.last_root1, l25_pose[0]):
+ self.set_root1_positions(l25_pose[0])
+ self.last_root1 = l25_pose[0]
+ if self._list_d_value(self.last_yaw, l25_pose[1]):
+ self.set_yaw_positions(l25_pose[1])
+ self.last_yaw = l25_pose[1]
+ if self._list_d_value(self.last_roll, l25_pose[2]):
+ self.set_roll_positions(l25_pose[2])
+ self.last_roll = l25_pose[2]
+ if self._list_d_value(self.last_root2, l25_pose[3]):
+ self.set_root2_positions(l25_pose[3])
+ self.last_root2 = l25_pose[3]
+ if self._list_d_value(self.last_tip, l25_pose[4]):
+ self.set_tip_positions(l25_pose[4])
+ self.last_tip = l25_pose[4]
+
+
+ def slice_list(self, input_list, slice_size):
+ """
+ Split a list into chunks of specified size.
+
+ Parameters:
+ input_list (list): The list to be chunked.
+ slice_size (int): Number of elements in each chunk.
+
+ Returns:
+ list of lists: The chunked list.
+ """
+ # Implementation using list comprehension
+ sliced_list = [input_list[i:i + slice_size] for i in range(0, len(input_list), slice_size)]
+ return sliced_list
+
+ def _list_d_value(self,list1, list2):
+ if list1 == None:
+ return True
+ for a, b in zip(list1, list2):
+ if abs(b - a) > 2:
+ return True
+ break
+ return False
+ # Set roll joint positions for all fingers
+ def set_roll_positions(self, joint_ranges):
+ self.send_command(FrameProperty.ROLL_POS, joint_ranges)
+ # Set yaw joint positions for all fingers
+ def set_yaw_positions(self, joint_ranges):
+ print(joint_ranges)
+ self.send_command(FrameProperty.YAW_POS, joint_ranges)
+ # Set base joint 1 positions for all fingers
+ def set_root1_positions(self, joint_ranges):
+ self.send_command(FrameProperty.ROOT1_POS, joint_ranges)
+ # Set base joint 2 positions for all fingers
+ def set_root2_positions(self, joint_ranges):
+ self.send_command(FrameProperty.ROOT2_POS, joint_ranges)
+ # Set base joint 3 positions for all fingers
+ def set_root3_positions(self, joint_ranges):
+ self.send_command(FrameProperty.ROOT3_POS, joint_ranges)
+ # Set fingertip joint positions for all fingers
+ def set_tip_positions(self, joint_ranges=[80]*5):
+ self.send_command(FrameProperty.TIP_POS, joint_ranges)
+ # Set thumb torque parameters
+ def set_thumb_torque(self, j=[]):
+ self.send_command(FrameProperty.THUMB_TORQUE, j)
+ # Set index finger torque
+ def set_index_torque(self, j=[]):
+ self.send_command(FrameProperty.INDEX_TORQUE, j)
+ # Set middle finger torque
+ def set_middle_torque(self, j=[]):
+ self.send_command(FrameProperty.MIDDLE_TORQUE, j)
+ # Set ring finger torque
+ def set_ring_torque(self, j=[]):
+ self.send_command(FrameProperty.RING_TORQUE, j)
+ # Set little finger torque
+ def set_little_torque(self, j=[]):
+ self.send_command(FrameProperty.LITTLE_TORQUE, j)
+
+ # Get thumb joint position
+ def get_thumb_positions(self,j=[0]):
+ self.send_command(FrameProperty.THUMB_POS, j)
+ # Get index finger joint positions
+ def get_index_positions(self, j=[0]):
+ self.send_command(FrameProperty.INDEX_POS,j)
+ # Get middle finger joint position
+ def get_middle_positions(self, j=[0]):
+ self.send_command(FrameProperty.MIDDLE_POS,j)
+ # Retrieve the position of the ring finger joint
+ def get_ring_positions(self, j=[0]):
+ self.send_command(FrameProperty.RING_POS,j)
+ # Retrieve the position of the little finger joint
+ def get_little_positions(self, j=[0]):
+ self.send_command(FrameProperty.LITTLE_POS, j)
+ # All fault codes of motors in the thumb
+ def get_thumbn_fault(self,j=[]):
+ self.send_command(FrameProperty.THUMB_FAULT,j)
+ # All motor fault codes for the index finger
+ def get_index_fault(self,j=[]):
+ self.send_command(FrameProperty.INDEX_FAULT,j)
+ # All motor fault codes for the middle finger
+ def get_middle_fault(self,j=[]):
+ self.send_command(FrameProperty.MIDDLE_FAULT,j)
+ # All motor fault codes for the ring finger
+ def get_ring_fault(self,j=[]):
+ self.send_command(FrameProperty.RING_FAULT,j)
+ # All motor fault codes for the little finger
+ def get_little_fault(self,j=[]):
+ self.send_command(FrameProperty.LITTLE_FAULT,j)
+ # Temperature threshold for the thumb motors
+ def get_thumb_threshold(self,j=[]):
+ self.send_command(FrameProperty.THUMB_TEMPERATURE, '')
+ # Temperature threshold for the index finger motors
+ def get_index_threshold(self,j=[]):
+ self.send_command(FrameProperty.INDEX_TEMPERATURE, j)
+ # Temperature threshold for the middle finger motors
+ def get_middle_threshold(self,j=[]):
+ self.send_command(FrameProperty.MIDDLE_TEMPERATURE, j)
+ # Temperature threshold for the ring finger motors
+ def get_ring_threshold(self,j=[]):
+ self.send_command(FrameProperty.RING_TEMPERATURE, j)
+ # Little finger temperature threshold
+ def get_little_threshold(self,j=[]):
+ self.send_command(FrameProperty.LITTLE_TEMPERATURE, j)
+
+
+ def set_disability_mode(self, j=[1,1,1,1,1]):
+ self.send_command(0x85,j)
+
+ def set_enable_mode(self, j=[00,00,00,00,00]):
+ self.send_command(0x85,j)
+
+ # Set torque for all fingers
+ def set_torque(self,torque=[250]*5):
+ t = torque[0]
+ i = torque[1]
+ m = torque[2]
+ r = torque[3]
+ l = torque[4]
+ self.set_thumb_torque(j=[t]*5)
+ self.set_index_torque(j=[i]*5)
+ self.set_middle_torque(j=[m]*5)
+ self.set_ring_torque(j=[r]*5)
+ self.set_little_torque(j=[l]*5)
+
+ def set_speed(self, speed):
+ self.speed = speed
+ if len(speed) < 25:
+ thumb_speed = [self.speed[0]]*5
+ index_speed = [self.speed[1]]*5
+ middle_speed = [self.speed[2]]*5
+ ring_speed = [self.speed[3]]*5
+ little_speed = [self.speed[4]]*5
+ else:
+ thumb_speed = [self.speed[0],self.speed[1],self.speed[2],self.speed[3],self.speed[4]]
+ index_speed = [self.speed[5],self.speed[6],self.speed[7],self.speed[8],self.speed[9]]
+ middle_speed = [self.speed[10],self.speed[11],self.speed[12],self.speed[13],self.speed[14]]
+ ring_speed = [self.speed[15],self.speed[16],self.speed[17],self.speed[18],self.speed[19]]
+ little_speed = [self.speed[20],self.speed[21],self.speed[22],self.speed[23],self.speed[24]]
+ self.send_command(FrameProperty.THUMB_SPEED, thumb_speed)
+ self.send_command(FrameProperty.INDEX_SPEED, index_speed)
+ self.send_command(FrameProperty.MIDDLE_SPEED, middle_speed)
+ self.send_command(FrameProperty.RING_SPEED, ring_speed)
+ self.send_command(FrameProperty.LITTLE_SPEED, little_speed)
+
+ def set_finger_torque(self, torque):
+ self.send_command(0x42, torque)
+
+ def request_device_info(self):
+ self.send_command(0xC0, [0])
+ self.send_command(0xC1, [0])
+ self.send_command(0xC2, [0])
+
+ def save_parameters(self):
+ self.send_command(0xCF, [])
+ def process_response(self, msg):
+ if msg.arbitration_id == self.can_id:
+ frame_type = msg.data[0]
+ response_data = msg.data[1:]
+ if len(list(response_data)) == 0:
+ return
+ if frame_type == 0x01:
+ self.x01 = list(response_data)
+ elif frame_type == 0x02:
+ self.x02 = list(response_data)
+ elif frame_type == 0x03:
+ self.x03 = list(response_data)
+ elif frame_type == 0x04:
+ self.x04 = list(response_data)
+ elif frame_type == 0x05:
+ self.x05 = list(response_data)
+ elif frame_type == 0x06:
+ self.x06 = list(response_data)
+ elif frame_type == 0xC0:
+ print(f"Device ID info: {response_data}")
+ if self.can_id == 0x28:
+ self.right_hand_info = response_data
+ elif self.can_id == 0x27:
+ self.left_hand_info = response_data
+ elif frame_type == 0x08:
+ self.x08 = list(response_data)
+ elif frame_type == 0x09:
+ self.x09 = list(response_data)
+ elif frame_type == 0x0A:
+ self.x0A = list(response_data)
+ elif frame_type == 0x0B:
+ self.x0B = list(response_data)
+ elif frame_type == 0x0C:
+ self.x0C = list(response_data)
+ elif frame_type == 0x0D:
+ self.x0D = list(response_data)
+ elif frame_type == 0x22:
+ d = list(response_data)
+ self.tangential_force_dir = [float(i) for i in d]
+ elif frame_type == 0x23:
+ d = list(response_data)
+ self.approach_inc = [float(i) for i in d]
+ elif frame_type == 0x41:
+ self.x41 = list(response_data)
+ elif frame_type == 0x42:
+
+ self.x42 = list(response_data)
+ elif frame_type == 0x43:
+ self.x43 = list(response_data)
+ elif frame_type == 0x44:
+
+ self.x44 = list(response_data)
+ elif frame_type == 0x45:
+ self.x45 = list(response_data)
+ elif frame_type == 0x49:
+ self.x49 = list(response_data)
+ elif frame_type == 0x4a:
+ self.x4a = list(response_data)
+ elif frame_type == 0x4b:
+ self.x4b = list(response_data)
+ elif frame_type == 0x4c:
+ self.x4c = list(response_data)
+ elif frame_type == 0x4d:
+ self.x4d = list(response_data)
+ elif frame_type == 0xc1:
+ self.xc1 = list(response_data)
+ elif frame_type == 0x51:
+ self.x51 = list(response_data)
+ elif frame_type == 0x52:
+ self.x52 = list(response_data)
+ elif frame_type == 0x53:
+ self.x53 = list(response_data)
+ elif frame_type == 0x54:
+ self.x54 = list(response_data)
+ elif frame_type == 0x55:
+ self.x55 = list(response_data)
+ elif frame_type == 0x59:
+ self.x59 = list(response_data)
+ elif frame_type == 0x5a:
+ self.x5a = list(response_data)
+ elif frame_type == 0x5b:
+ self.x5b = list(response_data)
+ elif frame_type == 0x5c:
+ self.x5c = list(response_data)
+ elif frame_type == 0x5d:
+ self.x5d = list(response_data)
+ elif frame_type == 0x61:
+ self.x61 = list(response_data)
+ elif frame_type == 0x62:
+ self.x62 = list(response_data)
+ elif frame_type == 0x63:
+ self.x63 = list(response_data)
+ elif frame_type == 0x64:
+ self.x64 = list(response_data)
+ elif frame_type == 0x65:
+ self.x65 = list(response_data)
+ elif frame_type == 0x90:
+ self.x90 = list(response_data)
+ elif frame_type == 0x91:
+ self.x91 = list(response_data)
+ elif frame_type == 0x92:
+ self.x92 = list(response_data)
+ elif frame_type == 0x93:
+ self.x93 = list(response_data)
+ elif frame_type == 0xb0:
+ self.xb0 = list(response_data)
+ elif frame_type == 0xb1:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb1 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.thumb_matrix[index] = d[1:]
+ elif frame_type == 0xb2:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb2 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.index_matrix[index] = d[1:]
+ elif frame_type == 0xb3:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb3 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.middle_matrix[index] = d[1:]
+ elif frame_type == 0xb4:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb4 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.ring_matrix[index] = d[1:]
+ elif frame_type == 0xb5:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb5 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.little_matrix[index] = d[1:]
+
+
+ def joint_map(self, pose):
+ l25_pose = [0.0] * 30
+
+ # 映射表，通过字典简化映射关系
+ mapping = {
+ 0: 10, 1: 5, 2: 0, 3: 15, 4: None, 5: 20,
+ 6: None, 7: 6, 8: 1, 9: 16, 10: None, 11: 21,
+ 12: None, 13: 7, 14: 2, 15: 17, 16: None, 17: 22,
+ 18: None, 19: 8, 20: 3, 21: 18, 22: None, 23: 23,
+ 24: None, 25: 9, 26: 4, 27: 19, 28: None, 29: 24
+ }
+
+ # 遍历映射字典，进行值的映射
+ for l25_idx, pose_idx in mapping.items():
+ if pose_idx is not None:
+ l25_pose[l25_idx] = pose[pose_idx]
+
+ return l25_pose
+
+
+ def state_to_cmd(self, l25_state):
+
+ pose = [0.0] * 25
+ mapping = {
+ 0: 10, 1: 5, 2: 0, 3: 15, 5: 20, 7: 6,
+ 8: 1, 9: 16, 11: 21, 13:7, 14: 2, 15: 17, 17: 22,
+ 19: 8, 20: 3, 21: 18, 23: 23, 25: 9, 26: 4,
+ 27: 19, 29: 24
+ }
+ # 遍历映射字典，更新pose的值
+ for l25_idx, pose_idx in mapping.items():
+ pose[pose_idx] = l25_state[l25_idx]
+ return pose
+ def action_play(self):
+ self.send_command(0xA0,[])
+
+ def get_current_status(self, j=''):
+ self.send_command(FrameProperty.THUMB_POS, j)
+ #time.sleep(0.001)
+ self.send_command(FrameProperty.INDEX_POS,j)
+ #time.sleep(0.001)
+ self.send_command(FrameProperty.MIDDLE_POS,j)
+ #time.sleep(0.001)
+ self.send_command(FrameProperty.RING_POS,j)
+ #time.sleep(0.001)
+ self.send_command(FrameProperty.LITTLE_POS, j)
+ #time.sleep(0.001)
+ state= self.x41+ self.x42+ self.x43+ self.x44+ self.x45
+ if len(state) == 30:
+ l25_state = self.state_to_cmd(l25_state=state)
+ return l25_state
+
+ def get_current_pub_status(self):
+ state= self.x41+ self.x42+ self.x43+ self.x44+ self.x45
+ if len(state) == 30:
+ l25_state = self.state_to_cmd(l25_state=state)
+ return l25_state
+
+ def get_current_state_topic(self):
+ self.send_command(0x01,[])
+ #time.sleep(0.001)
+ self.send_command(0x02,[])
+ # time.sleep(0.001)
+ self.send_command(0x03,[])
+ #time.sleep(0.001)
+ self.send_command(0x04,[])
+ #time.sleep(0.001)
+ self.send_command(0x06,[])
+ #time.sleep(0.001)
+ state = self.x03+self.x02+self.x01+self.x04+self.x06
+ return state
+ def get_speed(self,j=''):
+ self.send_command(FrameProperty.THUMB_SPEED, j)
+ #time.sleep(0.01)
+ self.send_command(FrameProperty.INDEX_SPEED, j)
+ #time.sleep(0.01)
+ self.send_command(FrameProperty.MIDDLE_SPEED, j)
+ #time.sleep(0.01)
+ self.send_command(FrameProperty.RING_SPEED, j)
+ #time.sleep(0.01)
+ self.send_command(FrameProperty.LITTLE_SPEED, j)
+ #time.sleep(0.01)
+ speed = self.x49+ self.x4a+ self.x4b+ self.x4c+ self.x4d
+ if len(speed) == 30:
+ l25_speed = self.state_to_cmd(l25_state=speed)
+ return l25_speed
+
+ def get_finger_torque(self):
+ self.send_command(FrameProperty.THUMB_TORQUE,[])
+ self.send_command(FrameProperty.INDEX_TORQUE,[])
+ self.send_command(FrameProperty.MIDDLE_TORQUE,[])
+ self.send_command(FrameProperty.RING_TORQUE,[])
+ self.send_command(FrameProperty.LITTLE_TORQUE,[])
+ return self.x51+self.x52+self.x53+self.x54+self.x55
+
+ def get_torque(self):
+ return self.get_finger_torque()
+ def get_fault(self):
+ self.get_thumbn_fault()
+ #time.sleep(0.001)
+ self.get_index_fault()
+ #time.sleep(0.001)
+ self.get_middle_fault()
+ #time.sleep(0.001)
+ self.get_ring_fault()
+ #time.sleep(0.001)
+ self.get_little_fault()
+ #time.sleep(0.001)
+ return [self.x59]+[self.x5a]+[self.x5b]+[self.x5c]+[self.x5d]
+ def get_threshold(self):
+ self.get_thumb_threshold()
+ self.get_index_threshold()
+ self.get_middle_threshold()
+ self.get_ring_threshold()
+ self.get_little_threshold()
+ return [self.x61]+[self.x62]+[self.x63]+[self.x64]+[self.x65]
+ def get_version(self):
+ if self.xc1 == []:
+ self.send_command(FrameProperty.HAND_HARDWARE_VERSION,[])
+ return self.xc1
+ def get_normal_force(self):
+ self.send_command(FrameProperty.HAND_NORMAL_FORCE,[])
+ return self.x90
+ def get_tangential_force(self):
+ self.send_command(FrameProperty.HAND_TANGENTIAL_FORCE,[])
+ return self.x91
+ def get_tangential_force_dir(self):
+ self.send_command(FrameProperty.HAND_TANGENTIAL_FORCE_DIR,[])
+ return self.x92
+ def get_approach_inc(self):
+ self.send_command(FrameProperty.HAND_APPROACH_INC,[])
+ return self.x93
+ def get_force(self):
+ '''获取压感数据'''
+ return [self.x90,self.x91 , self.x92 , self.x93]
+
+ def get_matrix_touch(self):
+ self.send_command(0xb1,[0xc6])
+ time.sleep(0.03)
+ self.send_command(0xb2,[0xc6])
+ time.sleep(0.03)
+ self.send_command(0xb3,[0xc6])
+ time.sleep(0.03)
+ self.send_command(0xb4,[0xc6])
+ time.sleep(0.03)
+ self.send_command(0xb5,[0xc6])
+ time.sleep(0.03)
+ return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+ def get_touch_type(self):
+ '''Get touch type'''
+ self.send_command(0xb1,[])
+ time.sleep(0.03)
+ if len(self.xb1) == 2:
+ return 2
+ else:
+ return -1
+
+ def get_touch(self):
+ '''Get touch data (not supported yet)'''
+ return [-1] * 6
+
+
+ def get_current(self):
+ return [0] * 21
+ def get_temperature(self):
+ self.get_thumb_threshold()
+ self.get_index_threshold()
+ self.get_middle_threshold()
+ self.get_ring_threshold()
+ self.get_little_threshold()
+ return [self.x61]+[self.x62]+[self.x63]+[self.x64]+[self.x65]
+
+ def get_finger_order(self):
+ return ["Thumb root", "Index root", "Middle root", "Ring root", "Little root",
+ "Thumb abduction", "Index abduction", "Middle abduction", "Ring abduction", "Little abduction",
+ "Thumb roll", "Reserved", "Reserved", "Reserved", "Reserved",
+ "Thumb middle", "Index middle", "Middle middle", "Ring middle", "Little middle",
+ "Thumb tip", "Index tip", "Middle tip", "Ring tip", "Little tip"]
+
+ def close_can_interface(self):
+ if self.bus:
+ self.bus.shutdown()
+ def clear_faults(self, finger_mask=[1, 1, 1, 1, 1]):
+ """L25 暂不支持清除故障码"""
+ pass
+ '''
+ 这个方法只用于展示数据关系映射，使用的话最好使用上面的方法
+ '''
+ def joint_map_2(self, pose):
+ l25_pose = [0.0]*30 #L25 CAN默认接收30个数据 pose控制L25发送的指令数据默认25个，这里进行映射
+ '''
+ 需要进行映射
+ # L25 CAN数据格式
+ #["拇指横摆0-10", "拇指侧摆1-5", "拇指根部2-0", "拇指中部3-15", "预留4-", "拇指指尖5-20", "预留6-", "食指侧摆7-6", "食指根部8-1", "食指中部9-16", "预留10-", "食指指尖11-21", "预留12-", "预留13-", "中指根部14-2", "中指中部15-17", "预留16-", "中指指尖17-22", "预留18-", "无名指侧摆19-8", "无名指根部20-3", "无名指中部21-18", "预留22-", "无名指指尖23-23", "预留24-", "小指侧摆25-9", "小指根部26-4", "小指中部27-19", "预留28-", "小指指尖29-24"]
+ # CMD 接收到的数据格式
+ #["拇指根部0", "食指根部1", "中指根部2", "无名指根部3","小指根部4","拇指侧摆5","食指侧摆6","中指侧摆","无名指侧摆8","小指侧摆9","拇指横摆10","预留","预留","预留","预留","拇指中部15","食指中部16","中指中部17","无名指中部18","小指中部19","拇指指尖20","食指指尖21","中指指尖22","无名指指尖23","小指指尖24"]
+ '''
+ l25_pose[0] = pose[10]
+ l25_pose[1] = pose[5]
+ l25_pose[2] = pose[0]
+ l25_pose[3] = pose[15]
+ l25_pose[4] = 0.0
+ l25_pose[5] = pose[20]
+ l25_pose[6] = 0.0
+ l25_pose[7] = pose[6]
+ l25_pose[8] = pose[1]
+ l25_pose[9] = pose[16]
+ l25_pose[10] = 0.0
+ l25_pose[11] = pose[21]
+ l25_pose[12] = 0.0
+ l25_pose[13] = 0.0
+ l25_pose[14] = pose[2]
+ l25_pose[15] = pose[17]
+ l25_pose[16] = 0.0
+ l25_pose[17] = pose[22]
+ l25_pose[18] = 0.0
+ l25_pose[19] = pose[8]
+ l25_pose[20] = pose[3]
+ l25_pose[21] = pose[18]
+ l25_pose[22] = 0.0
+ l25_pose[23] = pose[23]
+ l25_pose[24] = 0.0
+ l25_pose[25] = pose[9]
+ l25_pose[26] = pose[4]
+ l25_pose[27] = pose[19]
+ l25_pose[28] = 0.0
+ l25_pose[29] = pose[24]
+ return l25_pose
+
+ def get_serial_number(self):
+ return [0] * 6
+ def show_fun_table(self):
+ pass
\ No newline at end of file
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l6_can.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l6_can.py
new file mode 100644
index 0000000..d5cca5d
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l6_can.py
@@ -0,0 +1,426 @@
+import can
+import time, sys
+import threading
+import numpy as np
+from utils.open_can import OpenCan
+from utils.color_msg import ColorMsg
+from can.exceptions import CanError
+
+
+class LinkerHandL6Can:
+ def __init__(self, can_id, can_channel='can0', baudrate=1000000,yaml=""):
+ self.can_id = can_id
+ self.can_channel = can_channel
+ self.baudrate = baudrate
+ self.open_can = OpenCan(load_yaml=yaml)
+
+ self.x01 = [0] * 6 # 关节位置
+ self.x02 = [-1] * 6 # 转矩限制
+ self.x05 = [0] * 6 # 速度
+ self.x07 = [-1] * 6 # 加速度
+ self.x33 = [0] * 6 # 温度
+ self.x35 = [0] * 6 # 关节错误码
+ self.x36 = [-1] * 6 # 电流
+ self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+
+ self.thumb_matrix = np.full((12, 6), -1)
+ self.index_matrix = np.full((12, 6), -1)
+ self.middle_matrix = np.full((12, 6), -1)
+ self.ring_matrix = np.full((12, 6), -1)
+ self.little_matrix = np.full((12, 6), -1)
+ self.matrix_map = {
+ 0: 0,
+ 16: 1,
+ 32: 2,
+ 48: 3,
+ 64: 4,
+ 80: 5,
+ 96: 6,
+ 112: 7,
+ 128: 8,
+ 144: 9,
+ 160: 10,
+ 176: 11,
+ }
+ self.serial_number = []
+ self.serial_number_map = {
+ 0: 0,
+ 1: 1,
+ 2: 2,
+ 3: 3,
+ }
+ # Fault codes
+
+ self.joint_angles = [0] * 6
+ self.pressures = [200] * 6 # Default torque 200
+ self.bus = self.init_can_bus(can_channel, baudrate)
+ self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 6 for _ in range(4)]
+ self.is_lock = False
+ self.version = None
+ # Start the receiving thread
+ self.running = True
+ self.receive_thread = threading.Thread(target=self.receive_response)
+ self.receive_thread.daemon = True
+ self.receive_thread.start()
+
+ def init_can_bus(self, channel, baudrate):
+ """
+ 尝试按优先级连接 CAN 总线，并实现回退机制。
+ """
+ # --- 统一异常处理块开始 ---
+ try:
+ if sys.platform == "linux":
+ # Linux 优先级：1. socketcan
+ try:
+ self.open_can.open_can(self.can_channel)
+ # 尝试 socketcan
+ bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan')
+ ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow")
+ raise # 重新抛出异常，让外层 try 捕获
+ elif sys.platform == "win32":
+ # Windows 优先级：1. pcan
+ try:
+ bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow")
+ # Windows 优先级：2. candle (回退方法)
+ try:
+ bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow")
+ raise # 两个接口都失败，抛出异常
+ else:
+ raise EnvironmentError("Unsupported platform for CAN interface")
+ # --- 统一异常处理块结束 ---
+ except Exception as e:
+ # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError）
+ ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red")
+ # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行
+ raise
+
+ def send_frame(self, frame_property, data_list,sleep=0.003):
+ """Send a single CAN frame with specified properties and data."""
+ frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+ data = [frame_property_value] + [int(val) for val in data_list]
+ msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+ try:
+ self.bus.send(msg)
+ except can.CanError as e:
+ print(f"Failed to send message: {e}")
+ self.open_can.open_can(self.can_channel)
+ time.sleep(1)
+ self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel)
+ time.sleep(1)
+ if self.is_can:
+ self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate)
+ else:
+ print("Reconnecting CAN devices ....")
+ time.sleep(sleep)
+
+ def set_joint_positions(self, joint_angles):
+ """Set the positions of 10 joints (joint_angles: list of 10 values)."""
+ if len(joint_angles) > 6:
+ self.joint_angles = joint_angles[:6]
+ else:
+ self.joint_angles = joint_angles
+ # Send angle control in frames
+ self.send_frame(0x01, self.joint_angles, sleep=0.003)
+
+ def set_max_torque_limits(self, pressures, type="get"):
+ """Set maximum torque limits."""
+ if type == "get":
+ self.pressures = [0.0]
+ else:
+ self.pressures = pressures[:6]
+
+ def set_torque(self, torque=[180] * 6):
+ """Set L6 maximum torque limits."""
+ if len(torque) != 6:
+ raise ValueError("Torque list must have 6 elements.")
+ return
+ self.send_frame(0x02, torque)
+
+ def set_speed(self, speed=[180] * 6):
+ """Set L6 speed."""
+ if len(speed) != 6:
+ raise ValueError("Speed list must have 6 elements.")
+ return
+ self.x05 = speed
+ for i in range(2):
+ time.sleep(0.001)
+ self.send_frame(0x05, speed)
+
+ ''' -------------------Pressure Sensors---------------------- '''
+ def get_normal_force(self):
+ self.send_frame(0x20, [],sleep=0.01)
+
+ def get_tangential_force(self):
+ self.send_frame(0x21, [],sleep=0.01)
+
+ def get_tangential_force_dir(self):
+ self.send_frame(0x22, [],sleep=0.01)
+
+ def get_approach_inc(self):
+ self.send_frame(0x23, [],sleep=0.01)
+
+ ''' -------------------Motor Temperature---------------------- '''
+ def get_motor_temperature(self):
+ self.send_frame(0x33, [])
+
+ # Motor fault codes
+ def get_motor_fault_code(self):
+ self.send_frame(0x35, [])
+
+ def receive_response(self):
+ """Receive CAN responses and process them."""
+ while self.running:
+ try:
+ msg = self.bus.recv(timeout=1.0)
+ if msg:
+ self.process_response(msg)
+ except can.CanError as e:
+ print(f"Error receiving CAN message: {e}")
+
+ def process_response(self, msg):
+ """Process received CAN messages."""
+ #if msg.arbitration_id == self.can_id:
+ if msg.arbitration_id in (self.can_id, self.can_id + 8):
+ try:
+ frame_type = msg.data[0]
+ response_data = msg.data[1:]
+ if len(list(response_data)) == 0:
+ return
+ except:
+ return
+ if frame_type == 0x01: # 0x01
+ self.x01 = list(response_data)
+ elif frame_type == 0x02: # 0x02
+ self.x02 = list(response_data)
+ elif frame_type == 0x05: # Set speed
+ self.x05 = list(response_data)
+ elif frame_type == 0x20:
+ d = list(response_data)
+ self.normal_force = [float(i) for i in d]
+ elif frame_type == 0x21:
+ d = list(response_data)
+ self.tangential_force = [float(i) for i in d]
+ elif frame_type == 0x22:
+ d = list(response_data)
+ self.tangential_force_dir = [float(i) for i in d]
+ elif frame_type == 0x23:
+ d = list(response_data)
+ self.approach_inc = [float(i) for i in d]
+ elif frame_type == 0x33: # L6 temperature
+ self.x33 = list(response_data)
+ elif frame_type == 0x35: # L6 fault codes
+ self.x35 = list(response_data)
+ elif frame_type == 0x36: # L6 电流
+ self.x36 = list(response_data)
+ elif frame_type == 0xb0:
+ self.xb0 = list(response_data)
+ elif frame_type == 0xb1:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb1 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.thumb_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb2:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb2 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.index_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb3:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb3 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.middle_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb4:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb4 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.ring_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb5:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb5 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.little_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0x64: # L6 version number
+ self.version = list(response_data)
+ elif frame_type == 0xC2: # L6 version number
+ self.version = list(response_data)
+ elif frame_type == 0xC0:
+ d = list(response_data)
+ index = self.serial_number_map.get(d[0])
+ if index is not None:
+ self.serial_number=self.serial_number + d[1:]
+ else:
+ self.serial_number=self.serial_number + [-1] * 6
+
+
+
+ def get_version(self):
+ self.send_frame(0x64, [],sleep=0.1)
+ time.sleep(0.1)
+ if self.version is None:
+ self.send_frame(0xC2, [],sleep=0.1)
+ time.sleep(0.1)
+ return self.version
+
+ def get_current_status(self):
+ self.send_frame(0x01, [],sleep=0.005)
+ return self.x01
+
+ def get_current_pub_status(self):
+ return self.x01
+
+ def get_speed(self):
+ #self.send_frame(0x05, [],sleep=0.003)
+ #print("L6暂不支持读取实时速度")
+ return [0] * 6
+
+ def get_current(self):
+ '''Not supported yet.'''
+ self.send_frame(0x36, [],sleep=0.005)
+ return self.x36
+
+
+
+ def get_torque(self):
+ '''Not supported yet.'''
+ self.send_frame(0x2, [],sleep=0.01)
+ return self.x02
+
+ def get_touch_type(self):
+ '''Get touch type'''
+ self.send_frame(0xb1,[])
+ t = []
+ for i in range(3):
+ t = self.xb1
+ time.sleep(0.01)
+ if len(t) == 2:
+ return 2
+ else:
+ self.send_frame(0x20,[],sleep=0.03)
+ time.sleep(0.01)
+ if self.normal_force[0] == -1:
+ return -1
+ else:
+ return 1
+
+
+ def get_touch(self):
+ '''Get touch data'''
+ self.send_frame(0xb1,[],sleep=0.03)
+ self.send_frame(0xb2,[],sleep=0.03)
+ self.send_frame(0xb3,[],sleep=0.03)
+ self.send_frame(0xb4,[],sleep=0.03)
+ self.send_frame(0xb5,[],sleep=0.03)
+ return [self.xb1[1],self.xb2[1],self.xb3[1],self.xb4[1],self.xb5[1],0] # The last digit is palm, currently not available
+
+ def get_matrix_touch(self):
+ self.send_frame(0xb1,[0xc6],sleep=0.01)
+ self.send_frame(0xb2,[0xc6],sleep=0.01)
+ self.send_frame(0xb3,[0xc6],sleep=0.01)
+ self.send_frame(0xb4,[0xc6],sleep=0.01)
+ self.send_frame(0xb5,[0xc6],sleep=0.01)
+
+ return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+ def get_matrix_touch_v2(self):
+ self.send_frame(0xb1,[0xc6],sleep=0.009)
+ self.send_frame(0xb2,[0xc6],sleep=0.009)
+ self.send_frame(0xb3,[0xc6],sleep=0.009)
+ self.send_frame(0xb4,[0xc6],sleep=0.009)
+ self.send_frame(0xb5,[0xc6],sleep=0.009)
+ return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+ def get_thumb_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb1,[0xc6],sleep=sleep_time)
+ return self.thumb_matrix
+
+ def get_index_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb2,[0xc6],sleep=sleep_time)
+ return self.index_matrix
+
+ def get_middle_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb3,[0xc6],sleep=sleep_time)
+ return self.middle_matrix
+
+ def get_ring_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb4,[0xc6],sleep=sleep_time)
+ return self.ring_matrix
+
+ def get_little_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb5,[0xc6],sleep=sleep_time)
+ return self.little_matrix
+
+ def get_force(self):
+ '''Get pressure.'''
+ return [self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc]
+
+ def get_temperature(self):
+ '''Get temperature.'''
+ self.get_motor_temperature()
+ return self.x33
+
+ def get_fault(self):
+ '''Get faults.'''
+ self.get_motor_fault_code()
+ return self.x35
+
+ def get_finger_order(self):
+ return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
+
+ def show_fun_table(self):
+ pass
+
+ def clear_faults(self, finger_mask=[1, 1, 1, 1, 1]):
+ """O6 暂不支持清除故障码"""
+ pass
+
+ def get_serial_number(self):
+ try:
+ self.send_frame(0xC0,[],sleep=0.005)
+ # 1. 使用 bytes() 函数将整数列表转换为字节对象
+ # bytes() 接收一个由 0-255 之间的整数组成的列表。
+ byte_data = bytes(self.serial_number)
+ # 2. 使用 .decode() 方法将字节对象解码为 ASCII 字符串
+ result_string = byte_data.decode('ascii')
+ if result_string == "":
+ return "-1"
+ else:
+ # print(f"原始 ASCII 码列表: {self.serial_number}")
+ # print(f"解码后的字符串: {result_string}")
+ return result_string
+ except:
+ return "-1"
+
+ def close_can_interface(self):
+ """Stop the CAN communication."""
+ self.running = False
+ if self.receive_thread.is_alive():
+ self.receive_thread.join()
+ if self.bus:
+ self.bus.shutdown()
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l7_can.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l7_can.py
new file mode 100644
index 0000000..446d24a
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l7_can.py
@@ -0,0 +1,419 @@
+import can
+import time, sys
+import threading
+import numpy as np
+from utils.open_can import OpenCan
+from utils.color_msg import ColorMsg
+from can.exceptions import CanError
+
+
+class LinkerHandL7Can:
+ def __init__(self, can_id, can_channel='can0', baudrate=1000000,yaml=""):
+ self.can_id = can_id
+ self.can_channel = can_channel
+ self.baudrate = baudrate
+ self.open_can = OpenCan(load_yaml=yaml)
+
+ self.x01 = [0] * 7
+ self.x02 = [-1] * 7
+ self.x05 = [0] * 7
+ self.x33 = [0] * 7
+ self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+ self.thumb_matrix = np.full((12, 6), -1)
+ self.index_matrix = np.full((12, 6), -1)
+ self.middle_matrix = np.full((12, 6), -1)
+ self.ring_matrix = np.full((12, 6), -1)
+ self.little_matrix = np.full((12, 6), -1)
+ self.matrix_map = {
+ 0: 0,
+ 16: 1,
+ 32: 2,
+ 48: 3,
+ 64: 4,
+ 80: 5,
+ 96: 6,
+ 112: 7,
+ 128: 8,
+ 144: 9,
+ 160: 10,
+ 176: 11,
+ }
+ self.serial_number = []
+ self.serial_number_map = {
+ 0: 0,
+ 1: 1,
+ 2: 2,
+ 3: 3,
+ }
+ # Fault codes
+ self.x35 = [0] * 7, [0] * 7
+ self.joint_angles = [0] * 10
+ self.pressures = [200] * 7 # Default torque 200
+ self.bus = self.init_can_bus(can_channel, baudrate)
+ self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 7 for _ in range(4)]
+ self.is_lock = False
+ self.version = None
+ # Start the receiving thread
+ self.running = True
+ self.receive_thread = threading.Thread(target=self.receive_response)
+ self.receive_thread.daemon = True
+ self.receive_thread.start()
+
+ def init_can_bus(self, channel, baudrate):
+ """
+ 尝试按优先级连接 CAN 总线，并实现回退机制。
+ """
+ # --- 统一异常处理块开始 ---
+ try:
+ if sys.platform == "linux":
+ # Linux 优先级：1. socketcan
+ try:
+ self.open_can.open_can(self.can_channel)
+ # 尝试 socketcan
+ bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan')
+ ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow")
+ raise # 重新抛出异常，让外层 try 捕获
+ elif sys.platform == "win32":
+ # Windows 优先级：1. pcan
+ try:
+ bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow")
+ # Windows 优先级：2. candle (回退方法)
+ try:
+ bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow")
+ raise # 两个接口都失败，抛出异常
+ else:
+ raise EnvironmentError("Unsupported platform for CAN interface")
+ # --- 统一异常处理块结束 ---
+ except Exception as e:
+ # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError）
+ ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red")
+ # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行
+ raise
+
+ def send_frame(self, frame_property, data_list,sleep=0.005):
+ """Send a single CAN frame with specified properties and data."""
+ frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+ data = [frame_property_value] + [int(val) for val in data_list]
+ msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+ try:
+ self.bus.send(msg)
+ except can.CanError as e:
+ print(f"Failed to send message: {e}")
+ self.open_can.open_can(self.can_channel)
+ time.sleep(1)
+ self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel)
+ time.sleep(1)
+ if self.is_can:
+ self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate)
+ else:
+ print("Reconnecting CAN devices ....")
+ time.sleep(sleep)
+
+ def set_joint_positions(self, joint_angles):
+ """Set the positions of 10 joints (joint_angles: list of 10 values)."""
+ self.is_lock = True
+ if len(joint_angles) > 7:
+ self.joint_angles = joint_angles[:7]
+ else:
+ self.joint_angles = joint_angles
+ # Send angle control in frames
+ self.send_frame(0x01, self.joint_angles, sleep=0.003)
+ self.is_lock = False
+
+ def set_max_torque_limits(self, pressures, type="get"):
+ """Set maximum torque limits."""
+ if type == "get":
+ self.pressures = [0.0]
+ else:
+ self.pressures = pressures[:7]
+
+ def set_torque(self, torque=[180] * 7):
+ """Set L7 maximum torque limits."""
+ if len(torque) != 7:
+ raise ValueError("Torque list must have 7 elements.")
+ return
+ self.send_frame(0x02, torque)
+
+ def set_speed(self, speed=[180] * 7):
+ """Set L7 speed."""
+ if len(speed) != 7:
+ raise ValueError("Speed list must have 7 elements.")
+ return
+ self.x05 = speed
+ for i in range(2):
+ time.sleep(0.001)
+ self.send_frame(0x05, speed)
+
+ ''' -------------------Pressure Sensors---------------------- '''
+ def get_normal_force(self):
+ self.send_frame(0x20, [],sleep=0.004)
+
+ def get_tangential_force(self):
+ self.send_frame(0x21, [],sleep=0.004)
+
+ def get_tangential_force_dir(self):
+ self.send_frame(0x22, [],sleep=0.004)
+
+ def get_approach_inc(self):
+ self.send_frame(0x23, [],sleep=0.004)
+
+ ''' -------------------Motor Temperature---------------------- '''
+ def get_motor_temperature(self):
+ self.send_frame(0x33, [])
+
+ # Motor fault codes
+ def get_motor_fault_code(self):
+ self.send_frame(0x35, [])
+
+ def receive_response(self):
+ """Receive CAN responses and process them."""
+ while self.running:
+ try:
+ msg = self.bus.recv(timeout=1.0)
+ if msg:
+ self.process_response(msg)
+ except can.CanError as e:
+ print(f"Error receiving CAN message: {e}")
+
+ def process_response(self, msg):
+ """Process received CAN messages."""
+ if msg.arbitration_id == self.can_id:
+ frame_type = msg.data[0]
+ response_data = msg.data[1:]
+ if len(list(response_data)) == 0:
+ return
+ if frame_type == 0x01: # 0x01
+ self.x01 = list(response_data)
+ elif frame_type == 0x02: # 0x02
+ self.x02 = list(response_data)
+ elif frame_type == 0x05: # Set speed
+ self.x05 = list(response_data)
+ elif frame_type == 0x20:
+ d = list(response_data)
+ self.normal_force = [float(i) for i in d]
+ elif frame_type == 0x21:
+ d = list(response_data)
+ self.tangential_force = [float(i) for i in d]
+ elif frame_type == 0x22:
+ d = list(response_data)
+ self.tangential_force_dir = [float(i) for i in d]
+ elif frame_type == 0x23:
+ d = list(response_data)
+ self.approach_inc = [float(i) for i in d]
+ elif frame_type == 0x33: # L7 temperature
+ self.x33 = list(response_data)
+ elif frame_type == 0x35: # L7 fault codes
+ self.x35 = list(response_data)
+ elif frame_type == 0xb0:
+ self.xb0 = list(response_data)
+ elif frame_type == 0xb1:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb1 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.thumb_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb2:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb2 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.index_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb3:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb3 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.middle_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb4:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb4 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.ring_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb5:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb5 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.little_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0x64: # L7 version number
+ self.version = list(response_data)
+ elif frame_type == 0xC2: # O6 version number
+ self.version = list(response_data)
+ elif frame_type == 0xC0:
+ d = list(response_data)
+ index = self.serial_number_map.get(d[0])
+ if index is not None:
+ self.serial_number=self.serial_number + d[1:]
+ else:
+ self.serial_number=self.serial_number + [-1] * 6
+
+ def get_version(self):
+ self.send_frame(0x64, [], sleep=0.1)
+ time.sleep(0.1)
+ if self.version is None:
+ self.send_frame(0xC2, [], sleep=0.1)
+ time.sleep(0.1)
+ return self.version
+
+ def get_current_status(self):
+ if self.is_lock:
+ return self.x01
+ elif self.is_lock == False:
+ self.send_frame(0x01, [],sleep=0.003)
+ return self.x01
+
+ def get_current_pub_status(self):
+ return self.x01
+
+ def get_speed(self):
+ self.send_frame(0x05, [],sleep=0.003)
+ return self.x05
+
+ def get_current(self):
+ '''Not supported yet.'''
+ self.send_frame(0x2, [],sleep=0.1)
+ return self.x02
+
+
+
+ def get_torque(self):
+ '''Not supported yet.'''
+ self.send_frame(0x2, [],sleep=0.01)
+ return self.x02
+
+ def get_touch_type(self):
+ '''Get touch type'''
+ self.send_frame(0xb1,[])
+ t = []
+ for i in range(3):
+ t = self.xb1
+ time.sleep(0.01)
+ if len(t) == 2:
+ return 2
+ else:
+ self.send_frame(0x20,[],sleep=0.03)
+ time.sleep(0.01)
+ if self.normal_force[0] == -1:
+ return -1
+ else:
+ return 1
+
+
+ def get_touch(self):
+ '''Get touch data'''
+ self.send_frame(0xb1,[],sleep=0.03)
+ self.send_frame(0xb2,[],sleep=0.03)
+ self.send_frame(0xb3,[],sleep=0.03)
+ self.send_frame(0xb4,[],sleep=0.03)
+ self.send_frame(0xb5,[],sleep=0.03)
+ return [self.xb1[1],self.xb2[1],self.xb3[1],self.xb4[1],self.xb5[1],0] # The last digit is palm, currently not available
+
+ def get_matrix_touch(self):
+ self.send_frame(0xb1,[0xc6],sleep=0.01)
+ self.send_frame(0xb2,[0xc6],sleep=0.01)
+ self.send_frame(0xb3,[0xc6],sleep=0.01)
+ self.send_frame(0xb4,[0xc6],sleep=0.01)
+ self.send_frame(0xb5,[0xc6],sleep=0.01)
+
+ return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+ def get_matrix_touch_v2(self):
+ self.send_frame(0xb1,[0xc6],sleep=0.005)
+ self.send_frame(0xb2,[0xc6],sleep=0.005)
+ self.send_frame(0xb3,[0xc6],sleep=0.005)
+ self.send_frame(0xb4,[0xc6],sleep=0.005)
+ self.send_frame(0xb5,[0xc6],sleep=0.005)
+ return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+
+ def get_thumb_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb1,[0xc6],sleep=sleep_time)
+ return self.thumb_matrix
+
+ def get_index_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb2,[0xc6],sleep=sleep_time)
+ return self.index_matrix
+
+ def get_middle_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb3,[0xc6],sleep=sleep_time)
+ return self.middle_matrix
+
+ def get_ring_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb4,[0xc6],sleep=sleep_time)
+ return self.ring_matrix
+
+ def get_little_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb5,[0xc6],sleep=sleep_time)
+ return self.little_matrix
+
+ def get_force(self):
+ '''Get pressure.'''
+ return [self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc]
+
+ def get_temperature(self):
+ '''Get temperature.'''
+ self.get_motor_temperature()
+ return self.x33
+
+ def get_fault(self):
+ '''Get faults.'''
+ self.get_motor_fault_code()
+ return self.x35
+
+ def get_serial_number(self):
+ try:
+ self.send_frame(0xC0,[],sleep=0.005)
+ # 1. 使用 bytes() 函数将整数列表转换为字节对象
+ # bytes() 接收一个由 0-255 之间的整数组成的列表。
+ byte_data = bytes(self.serial_number)
+ # 2. 使用 .decode() 方法将字节对象解码为 ASCII 字符串
+ result_string = byte_data.decode('ascii')
+ if result_string == "":
+ return "-1"
+ else:
+ # print(f"原始 ASCII 码列表: {self.serial_number}")
+ # print(f"解码后的字符串: {result_string}")
+ return result_string
+ except:
+ return "-1"
+
+ def get_finger_order(self):
+ return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch", "thumb_cmc_roll"]
+
+ def show_fun_table(self):
+ pass
+
+ def clear_faults(self, finger_mask=[1, 1, 1, 1, 1]):
+ """L7 暂不支持清除故障码"""
+ pass
+
+ def close_can_interface(self):
+ """Stop the CAN communication."""
+ self.running = False
+ if self.receive_thread.is_alive():
+ self.receive_thread.join()
+ if self.bus:
+ self.bus.shutdown()
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_o6_can.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_o6_can.py
new file mode 100644
index 0000000..5c95ab8
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_o6_can.py
@@ -0,0 +1,447 @@
+import can
+import time, sys
+import threading
+import numpy as np
+from utils.open_can import OpenCan
+from utils.color_msg import ColorMsg
+from can.exceptions import CanError
+
+
+class LinkerHandO6Can:
+ def __init__(self, can_id, can_channel='can0', baudrate=1000000,yaml=""):
+ self.can_id = can_id
+ self.can_channel = can_channel
+ self.baudrate = baudrate
+ self.open_can = OpenCan(load_yaml=yaml)
+
+ self.x01 = [0] * 6 # 关节位置
+ self.x02 = [-1] * 6 # 转矩限制
+ self.x05 = [0] * 6 # 速度
+ self.x07 = [-1] * 6 # 加速度
+ self.x33 = [0] * 6 # 温度
+ self.x35 = [0] * 6 # 关节错误码
+ self.x36 = [-1] * 6 # 电流
+ self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+
+ self.thumb_matrix = np.full((10, 4), -1)
+ self.index_matrix = np.full((10, 4), -1)
+ self.middle_matrix = np.full((10, 4), -1)
+ self.ring_matrix = np.full((10, 4), -1)
+ self.little_matrix = np.full((10, 4), -1)
+ self.matrix_map = {
+ 0: 0,
+ 16: 1,
+ 32: 2,
+ 48: 3,
+ 64: 4,
+ 80: 5,
+ 96: 6,
+ 112: 7,
+ 128: 8,
+ 144: 9,
+ 160: 10,
+ 176: 11,
+ }
+ self.serial_number = []
+ self.serial_number_map = {
+ 0: 0,
+ 1: 1,
+ 2: 2,
+ 3: 3,
+ }
+ # Fault codes
+
+ self.joint_angles = [0] * 6
+ self.pressures = [200] * 6 # Default torque 200
+ self.bus = self.init_can_bus(can_channel, baudrate)
+ self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 6 for _ in range(4)]
+ self.is_lock = False
+ self.version = None
+ # Start the receiving thread
+ self.running = True
+
+ self.receive_thread = threading.Thread(target=self.receive_response)
+ self.receive_thread.daemon = True
+ self.receive_thread.start()
+ time.sleep(0.1)
+ self._check_touch_type()
+
+ def _check_touch_type(self):
+ '''根据SN编码判断压感类型'''
+ self.sn = self.get_serial_number()
+ time.sleep(0.1)
+ if self.sn != "-1":
+ parts = self.sn.split("-")
+ if parts[4] == "A":
+ self.touch_type = 1
+ elif parts[4] == "B":
+ self.touch_type = 2
+ self.touch_code = 0xA4 # 6*12 O6 一律0XA4
+ elif parts[4] == "J":
+ self.touch_type = 3
+ elif parts[4] == "F":
+ self.touch_type = 4
+ self.touch_code = 0xA4 # 4*10
+ elif parts[4] == "Z":
+ self.touch_type = -1
+ else:
+ # 如果没有SN编码则根据返回数据进行判断
+ self.touch_type = self.get_touch_type()
+
+
+ def init_can_bus(self, channel, baudrate):
+ """
+ 尝试按优先级连接 CAN 总线，并实现回退机制。
+ """
+ # --- 统一异常处理块开始 ---
+ try:
+ if sys.platform == "linux":
+ # Linux 优先级：1. socketcan
+ try:
+ self.open_can.open_can(self.can_channel)
+ # 尝试 socketcan
+ bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan')
+ ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow")
+ raise # 重新抛出异常，让外层 try 捕获
+ elif sys.platform == "win32":
+ # Windows 优先级：1. pcan
+ try:
+ bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow")
+ # Windows 优先级：2. candle (回退方法)
+ try:
+ bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow")
+ raise # 两个接口都失败，抛出异常
+ else:
+ raise EnvironmentError("Unsupported platform for CAN interface")
+ # --- 统一异常处理块结束 ---
+ except Exception as e:
+ # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError）
+ ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red")
+ # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行
+ raise
+
+ def send_frame(self, frame_property, data_list,sleep=0.005):
+ """Send a single CAN frame with specified properties and data."""
+ frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+ data = [frame_property_value] + [int(val) for val in data_list]
+ msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+ try:
+ self.bus.send(msg)
+ except can.CanError as e:
+ print(f"Failed to send message: {e}")
+ self.open_can.open_can(self.can_channel)
+ time.sleep(1)
+ self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel)
+ time.sleep(1)
+ if self.is_can:
+ self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate)
+ else:
+ print("Reconnecting CAN devices ....")
+ time.sleep(sleep)
+
+ def set_joint_positions(self, joint_angles):
+ """Set the positions of 10 joints (joint_angles: list of 10 values)."""
+ if len(joint_angles) > 6:
+ self.joint_angles = joint_angles[:6]
+ else:
+ self.joint_angles = joint_angles
+ # Send angle control in frames
+ self.send_frame(0x01, self.joint_angles, sleep=0.003)
+
+ def set_max_torque_limits(self, pressures, type="get"):
+ """Set maximum torque limits."""
+ if type == "get":
+ self.pressures = [0.0]
+ else:
+ self.pressures = pressures[:6]
+
+ def set_torque(self, torque=[180] * 6):
+ """Set L6 maximum torque limits."""
+ if len(torque) != 6:
+ raise ValueError("Torque list must have 6 elements.")
+ return
+ self.send_frame(0x02, torque)
+
+ def set_speed(self, speed=[180] * 6):
+ """Set L6 speed."""
+ if len(speed) != 6:
+ raise ValueError("Speed list must have 6 elements.")
+ return
+ self.x05 = speed
+ for i in range(2):
+ time.sleep(0.001)
+ self.send_frame(0x05, speed)
+
+ ''' -------------------Pressure Sensors---------------------- '''
+ def get_normal_force(self):
+ self.send_frame(0x20, [],sleep=0.01)
+
+ def get_tangential_force(self):
+ self.send_frame(0x21, [],sleep=0.01)
+
+ def get_tangential_force_dir(self):
+ self.send_frame(0x22, [],sleep=0.01)
+
+ def get_approach_inc(self):
+ self.send_frame(0x23, [],sleep=0.01)
+
+ ''' -------------------Motor Temperature---------------------- '''
+ def get_motor_temperature(self):
+ self.send_frame(0x33, [])
+
+ # Motor fault codes
+ def get_motor_fault_code(self):
+ self.send_frame(0x35, [])
+
+ def receive_response(self):
+ """Receive CAN responses and process them."""
+ while self.running:
+ try:
+ msg = self.bus.recv(timeout=1.0)
+ if msg:
+ self.process_response(msg)
+ except can.CanError as e:
+ print(f"Error receiving CAN message: {e}")
+
+ def process_response(self, msg):
+ """Process received CAN messages."""
+ if msg.arbitration_id == self.can_id:
+ frame_type = msg.data[0]
+ response_data = msg.data[1:]
+ if len(list(response_data)) == 0:
+ return
+ if frame_type == 0x01: # 0x01
+ self.x01 = list(response_data)
+ elif frame_type == 0x02: # 0x02
+ self.x02 = list(response_data)
+ elif frame_type == 0x05: # Set speed
+ self.x05 = list(response_data)
+ elif frame_type == 0x20:
+ d = list(response_data)
+ self.normal_force = [float(i) for i in d]
+ elif frame_type == 0x21:
+ d = list(response_data)
+ self.tangential_force = [float(i) for i in d]
+ elif frame_type == 0x22:
+ d = list(response_data)
+ self.tangential_force_dir = [float(i) for i in d]
+ elif frame_type == 0x23:
+ d = list(response_data)
+ self.approach_inc = [float(i) for i in d]
+ elif frame_type == 0x33: # O6 temperature
+ self.x33 = list(response_data)
+ elif frame_type == 0x35: # O6 fault codes
+ self.x35 = list(response_data)
+ elif frame_type == 0x36: # O6 电流
+ self.x36 = list(response_data)
+ elif frame_type == 0xb0:
+ self.xb0 = list(response_data)
+ elif frame_type == 0xb1:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb1 = d
+ elif len(d) == 5:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.thumb_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb2:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb2 = d
+ elif len(d) == 5:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.index_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb3:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb3 = d
+ elif len(d) == 5:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.middle_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb4:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb4 = d
+ elif len(d) == 5:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.ring_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb5:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb5 = d
+ elif len(d) == 5:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.little_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0x64: # O6 version number
+ self.version = list(response_data)
+ elif frame_type == 0xC2: # O6 version number
+ self.version = list(response_data)
+ elif frame_type == 0xC0:
+ d = list(response_data)
+ index = self.serial_number_map.get(d[0])
+ if index is not None:
+ self.serial_number += d[1:]
+ else:
+ self.serial_number=self.serial_number + [-1] * 6
+
+
+
+ def get_version(self):
+ self.send_frame(0x64, [],sleep=0.1)
+ time.sleep(0.1)
+ if self.version is None:
+ self.send_frame(0xC2, [],sleep=0.1)
+ time.sleep(0.1)
+ return self.version
+
+ def get_current_status(self):
+ self.send_frame(0x01, [],sleep=0.005)
+ return self.x01
+
+ def get_current_pub_status(self):
+ return self.x01
+
+ def get_speed(self):
+ self.send_frame(0x05, [],sleep=0.002)
+ #print("L6暂不支持读取实时速度")
+ return self.x05
+
+ def get_current(self):
+ '''Not supported yet.'''
+ self.send_frame(0x36, [],sleep=0.005)
+ return self.x36
+
+
+
+ def get_torque(self):
+ '''Not supported yet.'''
+ self.send_frame(0x2, [],sleep=0.01)
+ return self.x02
+
+ def get_touch_type(self):
+ '''Get touch type'''
+ self.send_frame(0xb1,[])
+ t = []
+ for i in range(3):
+ t = self.xb1
+ time.sleep(0.01)
+ if len(t) == 2:
+ return 2
+ else:
+ self.send_frame(0x20,[],sleep=0.03)
+ time.sleep(0.01)
+ if self.normal_force[0] == -1:
+ return -1
+ else:
+ return 1
+
+
+ def get_touch(self):
+ '''Get touch data'''
+ self.send_frame(0xb1,[],sleep=0.03)
+ self.send_frame(0xb2,[],sleep=0.03)
+ self.send_frame(0xb3,[],sleep=0.03)
+ self.send_frame(0xb4,[],sleep=0.03)
+ self.send_frame(0xb5,[],sleep=0.03)
+ return [self.xb1[1],self.xb2[1],self.xb3[1],self.xb4[1],self.xb5[1],0] # The last digit is palm, currently not available
+
+ def get_matrix_touch(self):
+ self.send_frame(0xb1,[self.touch_code],sleep=0.01)
+ self.send_frame(0xb2,[self.touch_code],sleep=0.01)
+ self.send_frame(0xb3,[self.touch_code],sleep=0.01)
+ self.send_frame(0xb4,[self.touch_code],sleep=0.01)
+ self.send_frame(0xb5,[self.touch_code],sleep=0.01)
+
+ return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+ def get_matrix_touch_v2(self):
+ self.send_frame(0xb1,[self.touch_code],sleep=0.009)
+ self.send_frame(0xb2,[self.touch_code],sleep=0.009)
+ self.send_frame(0xb3,[self.touch_code],sleep=0.009)
+ self.send_frame(0xb4,[self.touch_code],sleep=0.009)
+ self.send_frame(0xb5,[self.touch_code],sleep=0.009)
+ return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+ def get_thumb_matrix_touch(self,sleep_time=0.002):
+ self.send_frame(0xb1,[self.touch_code],sleep=sleep_time)
+ return self.thumb_matrix
+
+ def get_index_matrix_touch(self,sleep_time=0.002):
+ self.send_frame(0xb2,[self.touch_code],sleep=sleep_time)
+ return self.index_matrix
+
+ def get_middle_matrix_touch(self,sleep_time=0.002):
+ self.send_frame(0xb3,[self.touch_code],sleep=sleep_time)
+ return self.middle_matrix
+
+ def get_ring_matrix_touch(self,sleep_time=0.002):
+ self.send_frame(0xb4,[self.touch_code],sleep=sleep_time)
+ return self.ring_matrix
+
+ def get_little_matrix_touch(self,sleep_time=0.002):
+ self.send_frame(0xb5,[self.touch_code],sleep=sleep_time)
+ return self.little_matrix
+
+ def get_force(self):
+ '''Get pressure.'''
+ return [self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc]
+
+ def get_temperature(self):
+ '''Get temperature.'''
+ self.get_motor_temperature()
+ return self.x33
+
+ def get_fault(self):
+ '''Get faults.'''
+ self.get_motor_fault_code()
+ return self.x35
+
+ def get_finger_order(self):
+ return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
+
+ def clear_faults(self, finger_mask=[1, 1, 1, 1, 1]):
+ """O6 暂不支持清除故障码"""
+ pass
+ def show_fun_table(self):
+ pass
+
+ def get_serial_number(self):
+ try:
+ self.send_frame(0xC0,[],sleep=0.005)
+ # 1. 使用 bytes() 函数将整数列表转换为字节对象
+ # bytes() 接收一个由 0-255 之间的整数组成的列表。
+ byte_data = bytes(self.serial_number)
+ # 2. 使用 .decode() 方法将字节对象解码为 ASCII 字符串
+ result_string = byte_data.decode('ascii')
+ if result_string == "":
+ return "-1"
+ else:
+ # print(f"原始 ASCII 码列表: {self.serial_number}")
+ # print(f"解码后的字符串: {result_string}")
+ return result_string
+ except:
+ return "-1"
+
+ def close_can_interface(self):
+ """Stop the CAN communication."""
+ self.running = False
+ if self.receive_thread.is_alive():
+ self.receive_thread.join()
+ if self.bus:
+ self.bus.shutdown()
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l10_rs485.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l10_rs485.py
new file mode 100644
index 0000000..8db78ab
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l10_rs485.py
@@ -0,0 +1,345 @@
+#!/usr/bin/env python3
+import os
+import time
+import struct
+from typing import Dict, List
+import numpy as np
+from pymodbus.client import ModbusSerialClient
+_INTERVAL = 0.005 # 8 ms
+class LinkerHandL10RS485:
+ KEYS = ["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch",
+ "ring_mcp_pitch", "pinky_mcp_pitch", "index_mcp_roll", "ring_mcp_roll",
+ "pinky_mcp_roll", "thumb_cmc_yaw"]
+
+ def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200):
+ self.slave = hand_id
+ self.cli = ModbusSerialClient(
+ port=modbus_port,
+ baudrate=baudrate,
+ bytesize=8,
+ parity="N",
+ stopbits=1,
+ timeout=0.05, # 50 ms 超时
+ retries=3, # 重试次数
+ retry_on_empty=True,
+ handle_local_echo=False
+ )
+ # 在 pymodbus 3.5.1 中，连接需要显式调用 connect()
+ self.connected = self.cli.connect()
+ if not self.connected:
+ raise ConnectionError(f"RS485 connect fail to {modbus_port}")
+
+ # --------------------------------------------------
+ # 批量读取接口
+ # --------------------------------------------------
+ def read_angles(self) -> List[int]:
+ time.sleep(_INTERVAL)
+ rsp = self.cli.read_input_registers(address=0, count=10, slave=self.slave)
+ if rsp.isError():
+ raise RuntimeError(f"read_angles failed: {rsp}")
+ return rsp.registers
+
+ def read_torques(self) -> List[int]:
+ time.sleep(_INTERVAL)
+ rsp = self.cli.read_input_registers(address=10, count=10, slave=self.slave)
+ if rsp.isError():
+ raise RuntimeError(f"read_torques failed: {rsp}")
+ return rsp.registers
+
+ def read_speeds(self) -> List[int]:
+ time.sleep(_INTERVAL)
+ rsp = self.cli.read_input_registers(address=20, count=10, slave=self.slave)
+ if rsp.isError():
+ raise RuntimeError(f"read_speeds failed: {rsp}")
+ return rsp.registers
+
+ def read_temperatures(self) -> List[int]:
+ time.sleep(_INTERVAL)
+ rsp = self.cli.read_input_registers(address=40, count=10, slave=self.slave)
+ if rsp.isError():
+ raise RuntimeError(f"read_temperatures failed: {rsp}")
+ return rsp.registers
+
+ def read_error_codes(self) -> List[int]:
+ time.sleep(_INTERVAL)
+ rsp = self.cli.read_input_registers(address=50, count=10, slave=self.slave)
+ if rsp.isError():
+ raise RuntimeError(f"read_error_codes failed: {rsp}")
+ return rsp.registers
+
+ def read_versions(self) -> dict:
+ time.sleep(_INTERVAL)
+ rsp = self.cli.read_input_registers(address=158, count=6, slave=self.slave)
+ if rsp.isError():
+ raise RuntimeError(f"read_versions failed: {rsp}")
+ keys = ["hand_freedom", "hand_version", "hand_number",
+ "hand_direction", "software_version", "hardware_version"]
+ #return dict(zip(keys, rsp.registers))
+ return rsp.registers
+
+ # --------------------------------------------------
+ # 5 个压力传感器
+ # --------------------------------------------------
+ def read_pressure_thumb(self) -> np.ndarray:
+ return np.array(self._pressure(1), dtype=np.uint8)
+
+ def read_pressure_index(self) -> np.ndarray:
+ return np.array(self._pressure(2), dtype=np.uint8)
+
+ def read_pressure_middle(self) -> np.ndarray:
+ return np.array(self._pressure(3), dtype=np.uint8)
+
+ def read_pressure_ring(self) -> np.ndarray:
+ return np.array(self._pressure(4), dtype=np.uint8)
+
+ def read_pressure_pinky(self) -> np.ndarray:
+ return np.array(self._pressure(5), dtype=np.uint8)
+
+ # def _pressure(self, finger: int) -> List[int]:
+ # time.sleep(_INTERVAL)
+ # # 先选择手指
+ # wrsp = self.cli.write_register(address=60, value=finger, slave=self.slave)
+ # if wrsp.isError():
+ # raise RuntimeError(f"write finger select {finger} failed: {wrsp}")
+
+ # time.sleep(_INTERVAL)
+ # # 读取压力传感器数据 (96个寄存器)
+ # rrsp = self.cli.read_input_registers(address=62, count=96, slave=self.slave)
+ # if rrsp.isError():
+ # raise RuntimeError(f"read pressure finger={finger} failed: {rrsp}")
+ # return np.array(rrsp.registers, dtype=np.uint8)
+ def _pressure(self, finger: int) -> np.ndarray:
+ """
+ 6x12 (72点) 矩阵尺寸。
+ Modbus 地址 60/62。
+ """
+ rows = 12 # 12 行
+ cols = 6 # 6 列
+ finger_size = rows * cols # 72 个数据点
+
+ # modbus 地址和计数
+ write_address = 70 # 写入手指选择
+ read_address = 72 # 读取压力数据
+ read_count = 96 # 读取 96 个寄存器
+ skip_count = 10 # 跳过前 10 个校验点
+
+ # 0. 参数校验和手指写入值确定
+ if finger < 1 or finger > 5:
+ raise ValueError(f"无效的手指编号: {finger}。手指编号应在 1 到 5 之间。")
+
+ finger_write_value = finger
+
+ # 1. 写入手指选择寄存器 (地址 60)
+ time.sleep(0.008)
+ wrsp = self.cli.write_register(address=write_address, value=finger_write_value, slave=self.slave)
+ if wrsp.isError():
+ raise RuntimeError(f"写入手指选择 {finger} 到地址 {write_address} 失败: {wrsp}")
+
+ # 写入后等待片刻
+ time.sleep(0.008)
+
+ # 2. 读取地址 62 的数据
+ rrsp = self.cli.read_input_registers(address=read_address, count=read_count, slave=self.slave)
+
+ if rrsp.isError():
+ raise RuntimeError(f"读取地址 {read_address} 压力数据失败: {rrsp}")
+
+ registers_16bit: List[int] = rrsp.registers
+
+ # 3. 核心数据处理
+ # a. 提取低 8 位数据 (得到 96 个 8 位数据点)
+ final_data_96 = [reg_value & 255 for reg_value in registers_16bit]
+
+ # b. 跳过前 10 个校验/头部数据点 (得到 86 个有效数据点)
+ effective_data = np.array(final_data_96[skip_count:], dtype=np.uint8)
+ # c. 截取当前手指的矩阵数据 (从 86 个有效点中截取 72 个点)
+ start_idx = 0
+ end_idx = finger_size # 72
+
+ finger_data_flat = effective_data[start_idx:end_idx]
+
+ # d. 验证数据长度
+ if finger_data_flat.size != finger_size:
+ raise ValueError(
+ f"数据提取失败。期望 {finger_size} 点 ({rows}x{cols})，"
+ f"但仅截取到 {finger_data_flat.size} 点。请检查协议，确认地址 62 是否一次性返回了所有手指数据。"
+ )
+
+ # e. 重塑为二维矩阵 (12 行 6 列)
+ finger_matrix = finger_data_flat.reshape((rows, cols))
+
+ return finger_matrix
+
+
+
+
+ # --------------------------------------------------
+ # 批量写入接口
+ # --------------------------------------------------
+ def write_angles(self, vals: List[int]):
+ vals = [int(x) for x in vals]
+ if not self.is_valid_10xuint8(vals):
+ raise ValueError("需要 10 个 0-255 整数")
+
+ time.sleep(_INTERVAL)
+ rsp = self.cli.write_registers(address=0, values=vals, slave=self.slave)
+ if rsp.isError():
+ raise RuntimeError(f"write_angles failed: {rsp}")
+
+ def write_speeds(self, vals: List[int]):
+ vals = [int(x) for x in vals]
+ if not self.is_valid_10xuint8(vals):
+ raise ValueError("需要 10 个 0-255 整数")
+
+ time.sleep(_INTERVAL)
+ rsp = self.cli.write_registers(address=20, values=vals, slave=self.slave)
+ if rsp.isError():
+ raise RuntimeError(f"write_speeds failed: {rsp}")
+
+ def write_torques(self, vals: List[int]):
+ vals = [int(x) for x in vals]
+ if not self.is_valid_10xuint8(vals):
+ raise ValueError("需要 10 个 0-255 整数")
+
+ time.sleep(_INTERVAL)
+ rsp = self.cli.write_registers(address=10, values=vals, slave=self.slave)
+ if rsp.isError():
+ raise RuntimeError(f"write_torques failed: {rsp}")
+
+ # --------------------------------------------------
+ # 上下文管理
+ # --------------------------------------------------
+ def close(self):
+ if self.connected:
+ self.cli.close()
+ self.connected = False
+
+ def __enter__(self):
+ return self
+
+ def __exit__(self, exc_type, exc_val, exc_tb):
+ self.close()
+
+ # --------------------------------------------------
+ # 工具函数
+ # --------------------------------------------------
+ def is_valid_10xuint8(self, lst) -> bool:
+ if len(lst) != 10:
+ return False
+ return all(isinstance(x, int) and 0 <= x <= 255 for x in lst)
+
+ # --------------------------------------------------
+ # 固定 API 接口
+ # --------------------------------------------------
+ def set_joint_positions(self, joint_angles=None):
+ joint_angles = joint_angles or [0] * 10
+ self.write_angles(joint_angles)
+
+ def set_speed(self, speed=None):
+ speed = speed or [200] * 10
+ self.write_speeds(speed)
+
+ def set_torque(self, torque=None):
+ torque = torque or [200] * 10
+ self.write_torques(torque)
+
+ def set_current(self, current=None):
+ print("当前L10不支持设置电流", flush=True)
+
+ def get_version(self) -> dict:
+ return self.read_versions()
+
+ def get_current(self):
+ print("当前L10不支持获取电流", flush=True)
+
+ def get_state(self) -> List[int]:
+ return self.read_angles()
+
+ def get_state_for_pub(self) -> List[int]:
+ return self.get_state()
+
+ def get_current_status(self) -> List[int]:
+ return self.get_state()
+
+ def get_speed(self) -> List[int]:
+ return self.read_speeds()
+
+ def get_joint_speed(self) -> List[int]:
+ return self.get_speed()
+
+ def get_touch_type(self) -> int:
+ return 2
+
+ def get_normal_force(self) -> List[int]:
+ return [-1] * 5
+
+ def get_tangential_force(self) -> List[int]:
+ return [-1] * 5
+
+ def get_approach_inc(self) -> List[int]:
+ return [-1] * 5
+
+ def get_touch(self) -> List[int]:
+ return [-1] * 5
+
+ def get_thumb_matrix_touch(self,sleep_time=0):
+ return self._pressure(1)
+
+ def get_index_matrix_touch(self,sleep_time=0):
+ return self._pressure(2)
+
+ def get_middle_matrix_touch(self,sleep_time=0):
+ return self._pressure(3)
+
+ def get_ring_matrix_touch(self,sleep_time=0):
+ return self._pressure(4)
+
+ def get_little_matrix_touch(self,sleep_time=0):
+ return self._pressure(5)
+
+ def get_matrix_touch(self) -> List[List[int]]:
+ return self.get_thumb_matrix_touch(),self.get_index_matrix_touch(), self.get_middle_matrix_touch(), self.get_ring_matrix_touch(), self.get_little_matrix_touch()
+
+ def get_matrix_touch_v2(self) -> List[List[int]]:
+ return self.get_matrix_touch()
+
+ def get_torque(self) -> List[int]:
+ return self.read_torques()
+
+ def get_temperature(self) -> List[int]:
+ return self.read_temperatures()
+
+ def get_fault(self) -> List[int]:
+ return self.read_error_codes()
+
+ def get_serial_number(self):
+ return [0] * 6
+
+ def clear_faults(self):
+ pass
+
+# ------------------- demo -------------------
+if __name__ == "__main__":
+ try:
+ with LinkerHandL10RS485(hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200) as hand:
+ print("连接成功!")
+
+ # 测试读取角度
+ angles = hand.read_angles()
+ print("角度:", dict(zip(LinkerHandL10RS485.KEYS, angles)))
+
+ # 测试读取版本信息
+ ver = hand.get_version()
+ print("版本信息:", ver)
+
+ # 测试压力传感器
+ print("拇指压力传感器数据长度:", len(hand.read_pressure_thumb()))
+
+ # 测试其他读取功能
+ print("电流:", hand.read_torques())
+ print("速度:", hand.read_speeds())
+ print("温度:", hand.read_temperatures())
+ print("错误码:", hand.read_error_codes())
+
+ except Exception as e:
+ print(f"错误: {e}")
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py
new file mode 100644
index 0000000..81db0e2
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py
@@ -0,0 +1,460 @@
+#!/usr/bin/env python3
+import os
+import time
+from pymodbus.client import ModbusSerialClient
+from typing import List, Dict
+import numpy as np
+
+_INTERVAL = 0.006 # 8 ms
+
+class LinkerHandL6RS485:
+ """L6机械手 Modbus-RTU 控制类"""
+
+ # 6个关节名称
+ JOINT_NAMES = ["thumb_pitch", "thumb_yaw", "index_pitch",
+ "middle_pitch", "ring_pitch", "little_pitch"]
+
+ # 手指名称
+ FINGER_NAMES = ["thumb", "index", "middle", "ring", "little"]
+
+ def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200):
+ """
+ 初始化L6机械手
+ hand_id: 右手0x27(39), 左手0x28(40)
+ modbus_port: 串口设备路径
+ baudrate: 波特率，固定115200
+ """
+ self.slave = hand_id
+ self.cli = ModbusSerialClient(
+ port=modbus_port,
+ baudrate=baudrate,
+ bytesize=8,
+ parity="N",
+ stopbits=1,
+ timeout=0.05
+ )
+ # pymodbus 3.5.1 需要显式连接
+ self.connected = self.cli.connect()
+ if not self.connected:
+ raise ConnectionError(f"RS485连接失败，端口: {modbus_port}")
+
+ def _read_input_registers(self, address: int, count: int) -> List[int]:
+ """读取输入寄存器"""
+ time.sleep(_INTERVAL)
+ result = self.cli.read_input_registers(address=address, count=count, slave=self.slave)
+ if result.isError():
+ raise RuntimeError(f"读取输入寄存器失败: address={address}, count={count}")
+ return result.registers
+
+ def _write_register(self, address: int, value: int):
+ """写入单个寄存器"""
+ time.sleep(_INTERVAL)
+ result = self.cli.write_register(address=address, value=value, slave=self.slave)
+ if result.isError():
+ raise RuntimeError(f"写入寄存器失败: address={address}, value={value}")
+
+ def _write_registers(self, address: int, values: List[int]):
+ """写入多个寄存器"""
+ time.sleep(_INTERVAL)
+ result = self.cli.write_registers(address=address, values=values, slave=self.slave)
+ if result.isError():
+ raise RuntimeError(f"写入多个寄存器失败: address={address}, values={values}")
+
+ # --------------------------------------------------
+ # 基础读取接口
+ # --------------------------------------------------
+
+ def read_angles(self) -> List[int]:
+ """读取6个关节角度 (输入寄存器 0-5)"""
+ return self._read_input_registers(0, 6)
+
+ def read_torques(self) -> List[int]:
+ """读取6个关节转矩 (输入寄存器 6-11)"""
+ return self._read_input_registers(6, 6)
+
+ def read_speeds(self) -> List[int]:
+ """读取6个关节速度 (输入寄存器 12-17)"""
+ return self._read_input_registers(12, 6)
+
+ def read_temperatures(self) -> List[int]:
+ """读取6个关节温度 (输入寄存器 18-23)"""
+ return self._read_input_registers(18, 6)
+
+ def read_error_codes(self) -> List[int]:
+ """读取6个关节错误码 (输入寄存器 24-29)"""
+ return self._read_input_registers(24, 6)
+
+ # --------------------------------------------------
+ # 压力传感器接口
+ # --------------------------------------------------
+
+ # def _pressure(self, finger: int) -> List[int]:
+ # """内部：选手指 → 读压力数据"""
+ # # 选择手指 (保持寄存器 36)
+ # self._write_register(36, finger)
+ # time.sleep(_INTERVAL)
+ # # 读取压力数据 (输入寄存器 52-122)
+ # return np.array(self._read_input_registers(52, 71))
+ def _pressure(self, finger: int) -> np.ndarray:
+ """
+ 6x12 (72点) 矩阵尺寸。
+ Modbus 地址 60/62。
+ """
+ rows = 12 # 12 行
+ cols = 6 # 6 列
+ finger_size = rows * cols # 72 个数据点
+
+ # modbus 地址和计数 (按协议文档)
+ write_address = 36 # 写入手指选择 (保持寄存器)
+ read_address = 52 # 读取压力数据 (输入寄存器)
+ read_count = 71 # 读取 71 个寄存器
+ skip_count = 0 # 不跳过数据点
+
+ # 0. 参数校验和手指写入值确定
+ if finger < 1 or finger > 5:
+ raise ValueError(f"无效的手指编号: {finger}。手指编号应在 1 到 5 之间。")
+
+ finger_write_value = finger
+
+ # 1. 写入手指选择寄存器 (地址 36)
+ time.sleep(0.08)
+ wrsp = self.cli.write_register(address=write_address, value=finger_write_value, slave=self.slave)
+ if wrsp.isError():
+ raise RuntimeError(f"写入手指选择 {finger} 到地址 {write_address} 失败: {wrsp}")
+
+ # 写入后等待片刻
+ time.sleep(0.08)
+
+ # 2. 读取地址 52 的数据
+ rrsp = self.cli.read_input_registers(address=read_address, count=read_count, slave=self.slave)
+
+ if rrsp.isError():
+ raise RuntimeError(f"读取地址 {read_address} 压力数据失败: {rrsp}")
+
+ registers_16bit: List[int] = rrsp.registers
+
+ # 3. 核心数据处理
+ # a. 提取低 8 位数据 (得到 71 个 8 位数据点)
+ final_data_71 = [reg_value & 255 for reg_value in registers_16bit]
+
+ # b. 不跳过数据点 (按协议)
+ effective_data = np.array(final_data_71, dtype=np.uint8)
+
+ # c. 截取当前手指的矩阵数据 (71 个有效点中截取 72 个点，可能需要多读)
+ # 注: 协议返回 71 个点，手指数 1-5，每个手指需要 72 点
+ # 这里取全部数据
+ start_idx = 0
+ end_idx = min(len(effective_data), finger_size) # 取较小值
+
+ finger_data_flat = effective_data[start_idx:end_idx]
+
+ # d. 验证数据长度
+ if finger_data_flat.size < finger_size:
+ # 如果数据不足，尝试多读一些
+ rrsp2 = self.cli.read_input_registers(address=read_address + read_count, count=10, slave=self.slave)
+ if not rrsp2.isError():
+ extra_data = [reg_value & 255 for reg_value in rrsp2.registers]
+ finger_data_flat = np.concatenate([finger_data_flat, np.array(extra_data, dtype=np.uint8)])
+
+ # 最终确保有足够数据
+ if finger_data_flat.size >= finger_size:
+ finger_data_flat = finger_data_flat[:finger_size]
+ else:
+ raise ValueError(
+ f"数据提取失败。期望 {finger_size} 点 ({rows}x{cols})，"
+ f"但仅获取到 {finger_data_flat.size} 点。请检查协议。"
+ )
+
+ # e. 重塑为二维矩阵 (12 行 6 列)
+ finger_matrix = finger_data_flat.reshape((rows, cols))
+
+ return finger_matrix
+
+ def read_pressure_thumb(self) -> np.ndarray:
+ """读取大拇指压力数据"""
+ return np.array(self._pressure(1), dtype=np.uint8)
+
+ def read_pressure_index(self) -> np.ndarray:
+ """读取食指压力数据"""
+ return np.array(self._pressure(2), dtype=np.uint8)
+
+ def read_pressure_middle(self) -> np.ndarray:
+ """读取中指压力数据"""
+ return np.array(self._pressure(3), dtype=np.uint8)
+
+ def read_pressure_ring(self) -> np.ndarray:
+ """读取无名指压力数据"""
+ return np.array(self._pressure(4), dtype=np.uint8)
+
+ def read_pressure_little(self) -> np.ndarray:
+ """读取小拇指压力数据"""
+ return np.array(self._pressure(5), dtype=np.uint8)
+
+ # --------------------------------------------------
+ # 版本信息接口
+ # --------------------------------------------------
+
+ def read_versions(self) -> Dict[str, int]:
+ """读取版本信息 (输入寄存器 148-155)"""
+ result = self._read_input_registers(148, 8)
+
+ return {
+ "hand_freedom": result[0],
+ "hand_version": result[1],
+ "hand_number": result[2],
+ "hand_direction": result[3],
+ "software_version_major": result[4],
+ "software_version_minor": result[5] if len(result) > 5 else 0,
+ "software_version_revision": result[6] if len(result) > 6 else 0,
+ "hardware_version": result[7] if len(result) > 7 else 0
+ }
+
+ # --------------------------------------------------
+ # 写入接口
+ # --------------------------------------------------
+
+ def write_angles(self, vals: List[int]):
+ """设置6个关节角度 (保持寄存器 0-5)"""
+ vals = [int(x) for x in vals]
+ if not self.is_valid_6xuint8(vals):
+ raise ValueError("需要6个0-255的整数")
+ self._write_registers(0, vals)
+
+ def write_torques(self, vals: List[int]):
+ """设置6个关节转矩 (保持寄存器 6-11)"""
+ vals = [int(x) for x in vals]
+ if not self.is_valid_6xuint8(vals):
+ raise ValueError("需要6个0-255的整数")
+ self._write_registers(6, vals)
+
+ def write_speeds(self, vals: List[int]):
+ """设置6个关节速度 (保持寄存器 12-17)"""
+ vals = [int(x) for x in vals]
+ if not self.is_valid_6xuint8(vals):
+ raise ValueError("需要6个0-255的整数")
+ self._write_registers(12, vals)
+
+ # --------------------------------------------------
+ # 上下文管理
+ # --------------------------------------------------
+
+ def close(self):
+ """关闭连接"""
+ if self.connected:
+ self.cli.close()
+ self.connected = False
+
+ def __enter__(self):
+ return self
+
+ def __exit__(self, exc_type, exc_val, exc_tb):
+ self.close()
+
+ # --------------------------------------------------
+ # API固定接口函数
+ # --------------------------------------------------
+
+ def is_valid_6xuint8(self, lst) -> bool:
+ """验证6个0-255的整数列表"""
+ if len(lst) != 6:
+ return False
+ return all(isinstance(x, int) and 0 <= x <= 255 for x in lst)
+
+ def set_joint_positions(self, joint_angles=None):
+ """设置关节位置"""
+ joint_angles = joint_angles or [0] * 6
+ self.write_angles(joint_angles)
+
+ def set_speed(self, speed=None):
+ """设置速度"""
+ speed = speed or [200] * 6
+ self.write_speeds(speed)
+
+ def set_torque(self, torque=None):
+ """设置扭矩"""
+ torque = torque or [200] * 6
+ self.write_torques(torque)
+
+ def set_current(self, current=None):
+ """设置电流 (L6不支持)"""
+ print("当前L6不支持设置电流", flush=True)
+
+ def get_version(self) -> list:
+ """获取版本信息"""
+ versions = self.read_versions()
+ return [
+ versions.get("hand_freedom", 0),
+ versions.get("hand_version", 0),
+ versions.get("hand_number", 0),
+ versions.get("hand_direction", 0),
+ versions.get("software_version_major", 0),
+ versions.get("hardware_version", 0)
+ ]
+
+ def get_current(self):
+ """获取电流 (L6不支持)"""
+ print("当前L6不支持获取电流", flush=True)
+ return []
+
+ def get_state(self) -> list:
+ """获取关节状态"""
+ return self.read_angles()
+
+ def get_state_for_pub(self) -> list:
+ return self.get_state()
+
+ def get_current_status(self) -> list:
+ return self.get_state()
+
+ def get_speed(self) -> list:
+ """获取当前速度"""
+ return self.read_speeds()
+
+ def get_joint_speed(self) -> list:
+ return self.get_speed()
+
+ def get_touch_type(self) -> int:
+ """获取压感类型 (2=矩阵式)"""
+ return 2
+
+ def get_normal_force(self) -> list:
+ """获取压感数据：点式"""
+ return [-1] * 5
+
+ def get_tangential_force(self) -> list:
+ """获取压感数据：点式"""
+ return [-1] * 5
+
+ def get_approach_inc(self) -> list:
+ """获取压感数据：点式"""
+ return [-1] * 5
+
+ def get_touch(self) -> list:
+ return [-1] * 5
+
+ def get_thumb_matrix_touch(self,sleep_time=0):
+ return self._pressure(1)
+
+ def get_index_matrix_touch(self,sleep_time=0):
+ return self._pressure(2)
+
+ def get_middle_matrix_touch(self,sleep_time=0):
+ return self._pressure(3)
+
+ def get_ring_matrix_touch(self,sleep_time=0):
+ return self._pressure(4)
+
+ def get_little_matrix_touch(self,sleep_time=0):
+ return self._pressure(5)
+
+ def get_matrix_touch(self) -> list:
+ """获取压感数据：矩阵式"""
+ return [self._pressure(1), self._pressure(2), self._pressure(3),
+ self._pressure(4), self._pressure(5)]
+
+ def get_matrix_touch_v2(self) -> list:
+ """获取压感数据：矩阵式"""
+ return self.get_matrix_touch()
+
+ def get_torque(self) -> list:
+ """获取当前扭矩"""
+ return self.read_torques()
+
+ def get_temperature(self) -> list:
+ """获取当前电机温度"""
+ return self.read_temperatures()
+
+ def get_fault(self) -> list:
+ """获取当前电机故障码"""
+ return self.read_error_codes()
+
+ def get_serial_number(self):
+ return [0] * 6
+
+ def get_finger_order(self):
+ return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
+
+ # --------------------------------------------------
+ # 便捷方法
+ # --------------------------------------------------
+
+ def relax(self):
+ """所有手指伸直"""
+ self.set_joint_positions([255] * 6)
+
+ def fist(self):
+ """所有手指握拳"""
+ self.set_joint_positions([0] * 6)
+
+ def dump_status(self):
+ """打印状态信息"""
+ print("=" * 50)
+ print("L6机械手状态信息")
+ print("=" * 50)
+
+ try:
+ # 关节状态
+ angles = self.read_angles()
+ torques = self.read_torques()
+ speeds = self.read_speeds()
+ temps = self.read_temperatures()
+ errors = self.read_error_codes()
+
+ print("关节状态:")
+ for i, name in enumerate(self.JOINT_NAMES):
+ print(f" {name:15s}: 角度={angles[i]:3d}, 扭矩={torques[i]:3d}, "
+ f"速度={speeds[i]:3d}, 温度={temps[i]:2d}℃, 错误={errors[i]:2d}")
+
+ # 版本信息
+ versions = self.read_versions()
+ print("\n版本信息:")
+ for key, value in versions.items():
+ print(f" {key:20s}: {value}")
+
+ # 压力传感器测试
+ print("\n压力传感器测试:")
+ thumb_pressure = self.read_pressure_thumb()
+ print(f"大拇指压力数据长度: {len(thumb_pressure)}")
+
+ except Exception as e:
+ print(f"读取状态时出错: {e}")
+
+ print("=" * 50)
+
+
+# ------------------- 演示程序 -------------------
+if __name__ == "__main__":
+ # 使用示例
+ try:
+ with LinkerHandL6RS485(hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200) as hand:
+ print("连接成功!")
+
+ # 打印状态信息
+ hand.dump_status()
+
+ # 测试基本控制
+ print("\n测试控制功能...")
+ print("伸直手指...")
+ hand.relax()
+ time.sleep(2)
+
+ print("握拳...")
+ hand.fist()
+ time.sleep(2)
+
+ print("恢复伸直...")
+ hand.relax()
+
+ # 测试压力传感器
+ print("\n测试压力传感器...")
+ thumb_matrix = hand.get_thumb_matrix_touch()
+ print(f"大拇指压力数据: {len(thumb_matrix)}个点")
+
+ # 获取所有手指压力数据
+ all_matrices = hand.get_matrix_touch()
+ for i, name in enumerate(hand.FINGER_NAMES):
+ matrix = all_matrices[i]
+ print(f"{name}手指压力数据长度: {len(matrix)}")
+
+ except Exception as e:
+ print(f"错误: {e}")
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py.bak b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py.bak
new file mode 100644
index 0000000..d68b621
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py.bak
@@ -0,0 +1,444 @@
+#!/usr/bin/env python3
+import os
+import time
+from pymodbus.client import ModbusSerialClient
+from typing import List, Dict
+import numpy as np
+
+_INTERVAL = 0.006 # 8 ms
+
+class LinkerHandL6RS485:
+ """L6机械手 Modbus-RTU 控制类"""
+
+ # 6个关节名称
+ JOINT_NAMES = ["thumb_pitch", "thumb_yaw", "index_pitch",
+ "middle_pitch", "ring_pitch", "little_pitch"]
+
+ # 手指名称
+ FINGER_NAMES = ["thumb", "index", "middle", "ring", "little"]
+
+ def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200):
+ """
+ 初始化L6机械手
+ hand_id: 右手0x27(39), 左手0x28(40)
+ modbus_port: 串口设备路径
+ baudrate: 波特率，固定115200
+ """
+ self.slave = hand_id
+ self.cli = ModbusSerialClient(
+ port=modbus_port,
+ baudrate=baudrate,
+ bytesize=8,
+ parity="N",
+ stopbits=1,
+ timeout=0.05
+ )
+ # pymodbus 3.5.1 需要显式连接
+ self.connected = self.cli.connect()
+ if not self.connected:
+ raise ConnectionError(f"RS485连接失败，端口: {modbus_port}")
+
+ def _read_input_registers(self, address: int, count: int) -> List[int]:
+ """读取输入寄存器"""
+ time.sleep(_INTERVAL)
+ result = self.cli.read_input_registers(address=address, count=count, slave=self.slave)
+ if result.isError():
+ raise RuntimeError(f"读取输入寄存器失败: address={address}, count={count}")
+ return result.registers
+
+ def _write_register(self, address: int, value: int):
+ """写入单个寄存器"""
+ time.sleep(_INTERVAL)
+ result = self.cli.write_register(address=address, value=value, slave=self.slave)
+ if result.isError():
+ raise RuntimeError(f"写入寄存器失败: address={address}, value={value}")
+
+ def _write_registers(self, address: int, values: List[int]):
+ """写入多个寄存器"""
+ time.sleep(_INTERVAL)
+ result = self.cli.write_registers(address=address, values=values, slave=self.slave)
+ if result.isError():
+ raise RuntimeError(f"写入多个寄存器失败: address={address}, values={values}")
+
+ # --------------------------------------------------
+ # 基础读取接口
+ # --------------------------------------------------
+
+ def read_angles(self) -> List[int]:
+ """读取6个关节角度 (输入寄存器 0-5)"""
+ return self._read_input_registers(0, 6)
+
+ def read_torques(self) -> List[int]:
+ """读取6个关节转矩 (输入寄存器 6-11)"""
+ return self._read_input_registers(6, 6)
+
+ def read_speeds(self) -> List[int]:
+ """读取6个关节速度 (输入寄存器 12-17)"""
+ return self._read_input_registers(12, 6)
+
+ def read_temperatures(self) -> List[int]:
+ """读取6个关节温度 (输入寄存器 18-23)"""
+ return self._read_input_registers(18, 6)
+
+ def read_error_codes(self) -> List[int]:
+ """读取6个关节错误码 (输入寄存器 24-29)"""
+ return self._read_input_registers(24, 6)
+
+ # --------------------------------------------------
+ # 压力传感器接口
+ # --------------------------------------------------
+
+ # def _pressure(self, finger: int) -> List[int]:
+ # """内部：选手指 → 读压力数据"""
+ # # 选择手指 (保持寄存器 36)
+ # self._write_register(36, finger)
+ # time.sleep(_INTERVAL)
+ # # 读取压力数据 (输入寄存器 52-122)
+ # return np.array(self._read_input_registers(52, 71))
+ def _pressure(self, finger: int) -> np.ndarray:
+ """
+ 6x12 (72点) 矩阵尺寸。
+ Modbus 地址 60/62。
+ """
+ rows = 12 # 12 行
+ cols = 6 # 6 列
+ finger_size = rows * cols # 72 个数据点
+
+ # modbus 地址和计数
+ write_address = 60 # 写入手指选择
+ read_address = 62 # 读取压力数据
+ read_count = 96 # 读取 96 个寄存器
+ skip_count = 10 # 跳过前 10 个校验点
+
+ # 0. 参数校验和手指写入值确定
+ if finger < 1 or finger > 5:
+ raise ValueError(f"无效的手指编号: {finger}。手指编号应在 1 到 5 之间。")
+
+ finger_write_value = finger
+
+ # 1. 写入手指选择寄存器 (地址 60)
+ time.sleep(0.008)
+ wrsp = self.cli.write_register(address=write_address, value=finger_write_value, slave=self.slave)
+ if wrsp.isError():
+ raise RuntimeError(f"写入手指选择 {finger} 到地址 {write_address} 失败: {wrsp}")
+
+ # 写入后等待片刻
+ time.sleep(0.008)
+
+ # 2. 读取地址 62 的数据
+ rrsp = self.cli.read_input_registers(address=read_address, count=read_count, slave=self.slave)
+
+ if rrsp.isError():
+ raise RuntimeError(f"读取地址 {read_address} 压力数据失败: {rrsp}")
+
+ registers_16bit: List[int] = rrsp.registers
+
+ # 3. 核心数据处理
+ # a. 提取低 8 位数据 (得到 96 个 8 位数据点)
+ final_data_96 = [reg_value & 255 for reg_value in registers_16bit]
+
+ # b. 跳过前 10 个校验/头部数据点 (得到 86 个有效数据点)
+ effective_data = np.array(final_data_96[skip_count:], dtype=np.uint8)
+ # c. 截取当前手指的矩阵数据 (从 86 个有效点中截取 72 个点)
+ start_idx = 0
+ end_idx = finger_size # 72
+
+ finger_data_flat = effective_data[start_idx:end_idx]
+
+ # d. 验证数据长度
+ if finger_data_flat.size != finger_size:
+ raise ValueError(
+ f"数据提取失败。期望 {finger_size} 点 ({rows}x{cols})，"
+ f"但仅截取到 {finger_data_flat.size} 点。请检查协议，确认地址 62 是否一次性返回了所有手指数据。"
+ )
+
+ # e. 重塑为二维矩阵 (12 行 6 列)
+ finger_matrix = finger_data_flat.reshape((rows, cols))
+
+ return finger_matrix
+
+ def read_pressure_thumb(self) -> np.ndarray:
+ """读取大拇指压力数据"""
+ return np.array(self._pressure(1), dtype=np.uint8)
+
+ def read_pressure_index(self) -> np.ndarray:
+ """读取食指压力数据"""
+ return np.array(self._pressure(2), dtype=np.uint8)
+
+ def read_pressure_middle(self) -> np.ndarray:
+ """读取中指压力数据"""
+ return np.array(self._pressure(3), dtype=np.uint8)
+
+ def read_pressure_ring(self) -> np.ndarray:
+ """读取无名指压力数据"""
+ return np.array(self._pressure(4), dtype=np.uint8)
+
+ def read_pressure_little(self) -> np.ndarray:
+ """读取小拇指压力数据"""
+ return np.array(self._pressure(5), dtype=np.uint8)
+
+ # --------------------------------------------------
+ # 版本信息接口
+ # --------------------------------------------------
+
+ def read_versions(self) -> Dict[str, int]:
+ """读取版本信息 (输入寄存器 148-155)"""
+ result = self._read_input_registers(148, 8)
+
+ return {
+ "hand_freedom": result[0],
+ "hand_version": result[1],
+ "hand_number": result[2],
+ "hand_direction": result[3],
+ "software_version_major": result[4],
+ "software_version_minor": result[5] if len(result) > 5 else 0,
+ "software_version_revision": result[6] if len(result) > 6 else 0,
+ "hardware_version": result[7] if len(result) > 7 else 0
+ }
+
+ # --------------------------------------------------
+ # 写入接口
+ # --------------------------------------------------
+
+ def write_angles(self, vals: List[int]):
+ """设置6个关节角度 (保持寄存器 0-5)"""
+ vals = [int(x) for x in vals]
+ if not self.is_valid_6xuint8(vals):
+ raise ValueError("需要6个0-255的整数")
+ self._write_registers(0, vals)
+
+ def write_torques(self, vals: List[int]):
+ """设置6个关节转矩 (保持寄存器 6-11)"""
+ vals = [int(x) for x in vals]
+ if not self.is_valid_6xuint8(vals):
+ raise ValueError("需要6个0-255的整数")
+ self._write_registers(6, vals)
+
+ def write_speeds(self, vals: List[int]):
+ """设置6个关节速度 (保持寄存器 12-17)"""
+ vals = [int(x) for x in vals]
+ if not self.is_valid_6xuint8(vals):
+ raise ValueError("需要6个0-255的整数")
+ self._write_registers(12, vals)
+
+ # --------------------------------------------------
+ # 上下文管理
+ # --------------------------------------------------
+
+ def close(self):
+ """关闭连接"""
+ if self.connected:
+ self.cli.close()
+ self.connected = False
+
+ def __enter__(self):
+ return self
+
+ def __exit__(self, exc_type, exc_val, exc_tb):
+ self.close()
+
+ # --------------------------------------------------
+ # API固定接口函数
+ # --------------------------------------------------
+
+ def is_valid_6xuint8(self, lst) -> bool:
+ """验证6个0-255的整数列表"""
+ if len(lst) != 6:
+ return False
+ return all(isinstance(x, int) and 0 <= x <= 255 for x in lst)
+
+ def set_joint_positions(self, joint_angles=None):
+ """设置关节位置"""
+ joint_angles = joint_angles or [0] * 6
+ self.write_angles(joint_angles)
+
+ def set_speed(self, speed=None):
+ """设置速度"""
+ speed = speed or [200] * 6
+ self.write_speeds(speed)
+
+ def set_torque(self, torque=None):
+ """设置扭矩"""
+ torque = torque or [200] * 6
+ self.write_torques(torque)
+
+ def set_current(self, current=None):
+ """设置电流 (L6不支持)"""
+ print("当前L6不支持设置电流", flush=True)
+
+ def get_version(self) -> list:
+ """获取版本信息"""
+ versions = self.read_versions()
+ return [
+ versions.get("hand_freedom", 0),
+ versions.get("hand_version", 0),
+ versions.get("hand_number", 0),
+ versions.get("hand_direction", 0),
+ versions.get("software_version_major", 0),
+ versions.get("hardware_version", 0)
+ ]
+
+ def get_current(self):
+ """获取电流 (L6不支持)"""
+ print("当前L6不支持获取电流", flush=True)
+ return []
+
+ def get_state(self) -> list:
+ """获取关节状态"""
+ return self.read_angles()
+
+ def get_state_for_pub(self) -> list:
+ return self.get_state()
+
+ def get_current_status(self) -> list:
+ return self.get_state()
+
+ def get_speed(self) -> list:
+ """获取当前速度"""
+ return self.read_speeds()
+
+ def get_joint_speed(self) -> list:
+ return self.get_speed()
+
+ def get_touch_type(self) -> int:
+ """获取压感类型 (2=矩阵式)"""
+ return 2
+
+ def get_normal_force(self) -> list:
+ """获取压感数据：点式"""
+ return [-1] * 5
+
+ def get_tangential_force(self) -> list:
+ """获取压感数据：点式"""
+ return [-1] * 5
+
+ def get_approach_inc(self) -> list:
+ """获取压感数据：点式"""
+ return [-1] * 5
+
+ def get_touch(self) -> list:
+ return [-1] * 5
+
+ def get_thumb_matrix_touch(self,sleep_time=0):
+ return self._pressure(1)
+
+ def get_index_matrix_touch(self,sleep_time=0):
+ return self._pressure(2)
+
+ def get_middle_matrix_touch(self,sleep_time=0):
+ return self._pressure(3)
+
+ def get_ring_matrix_touch(self,sleep_time=0):
+ return self._pressure(4)
+
+ def get_little_matrix_touch(self,sleep_time=0):
+ return self._pressure(5)
+
+ def get_matrix_touch(self) -> list:
+ """获取压感数据：矩阵式"""
+ return [self._pressure(1), self._pressure(2), self._pressure(3),
+ self._pressure(4), self._pressure(5)]
+
+ def get_matrix_touch_v2(self) -> list:
+ """获取压感数据：矩阵式"""
+ return self.get_matrix_touch()
+
+ def get_torque(self) -> list:
+ """获取当前扭矩"""
+ return self.read_torques()
+
+ def get_temperature(self) -> list:
+ """获取当前电机温度"""
+ return self.read_temperatures()
+
+ def get_fault(self) -> list:
+ """获取当前电机故障码"""
+ return self.read_error_codes()
+
+ def get_serial_number(self):
+ return [0] * 6
+
+ # --------------------------------------------------
+ # 便捷方法
+ # --------------------------------------------------
+
+ def relax(self):
+ """所有手指伸直"""
+ self.set_joint_positions([255] * 6)
+
+ def fist(self):
+ """所有手指握拳"""
+ self.set_joint_positions([0] * 6)
+
+ def dump_status(self):
+ """打印状态信息"""
+ print("=" * 50)
+ print("L6机械手状态信息")
+ print("=" * 50)
+
+ try:
+ # 关节状态
+ angles = self.read_angles()
+ torques = self.read_torques()
+ speeds = self.read_speeds()
+ temps = self.read_temperatures()
+ errors = self.read_error_codes()
+
+ print("关节状态:")
+ for i, name in enumerate(self.JOINT_NAMES):
+ print(f" {name:15s}: 角度={angles[i]:3d}, 扭矩={torques[i]:3d}, "
+ f"速度={speeds[i]:3d}, 温度={temps[i]:2d}℃, 错误={errors[i]:2d}")
+
+ # 版本信息
+ versions = self.read_versions()
+ print("\n版本信息:")
+ for key, value in versions.items():
+ print(f" {key:20s}: {value}")
+
+ # 压力传感器测试
+ print("\n压力传感器测试:")
+ thumb_pressure = self.read_pressure_thumb()
+ print(f"大拇指压力数据长度: {len(thumb_pressure)}")
+
+ except Exception as e:
+ print(f"读取状态时出错: {e}")
+
+ print("=" * 50)
+
+
+# ------------------- 演示程序 -------------------
+if __name__ == "__main__":
+ # 使用示例
+ try:
+ with LinkerHandL6RS485(hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200) as hand:
+ print("连接成功!")
+
+ # 打印状态信息
+ hand.dump_status()
+
+ # 测试基本控制
+ print("\n测试控制功能...")
+ print("伸直手指...")
+ hand.relax()
+ time.sleep(2)
+
+ print("握拳...")
+ hand.fist()
+ time.sleep(2)
+
+ print("恢复伸直...")
+ hand.relax()
+
+ # 测试压力传感器
+ print("\n测试压力传感器...")
+ thumb_matrix = hand.get_thumb_matrix_touch()
+ print(f"大拇指压力数据: {len(thumb_matrix)}个点")
+
+ # 获取所有手指压力数据
+ all_matrices = hand.get_matrix_touch()
+ for i, name in enumerate(hand.FINGER_NAMES):
+ matrix = all_matrices[i]
+ print(f"{name}手指压力数据长度: {len(matrix)}")
+
+ except Exception as e:
+ print(f"错误: {e}")
\ No newline at end of file
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l7_rs485.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l7_rs485.py
new file mode 100644
index 0000000..274a1ae
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l7_rs485.py
@@ -0,0 +1,423 @@
+#!/usr/bin/env python3
+import time
+from typing import List, Dict, Union
+import numpy as np
+from pymodbus.client import ModbusSerialClient
+from pymodbus.exceptions import ModbusException
+
+# --- 协议常量和寄存器地址定义 (根据 O7 协议文件) ---
+
+# RS485 通信设置
+DEFAULT_BAUDRATE = 115200
+
+# O7机械手七个可控关节的键名 (根据保持寄存器和输入寄存器地址 0-6)
+O7_JOINT_KEYS = [
+ "Thumb_Pitch", "Thumb_Yaw", "Index_Pitch", "Middle_Pitch",
+ "Ring_Pitch", "Little_Pitch", "Thumb_Roll"
+]
+
+# 保持寄存器地址 (写操作 FC 16)
+HR_ADDR = {
+ "Position_Start": 0, # 关节目标位置 (7 个寄存器: 0-6)
+ "Torque_Start": 7, # 关节目标转矩 (7 个寄存器: 7-13)
+ "Speed_Start": 14, # 关节目标速度 (7 个寄存器: 14-20)
+ "Pressure_Select": 42 # 压力传感器数据选择 (1 个寄存器)
+ # 21-41 为堵转保护阈值、时间和扭矩，暂未实现
+}
+
+# 输入寄存器地址 (读操作 FC 04)
+IR_ADDR = {
+ "Current_Position_Start": 0, # 当前关节位置 (7 个寄存器: 0-6)
+ "Current_Torque_Start": 7, # 当前关节转矩 (7 个寄存器: 7-13)
+ "Current_Speed_Start": 14, # 当前关节速度 (7 个寄存器: 14-20)
+ "Current_Temperature_Start": 21, # 当前关节温度 (7 个寄存器: 21-27)
+ "Error_Code_Start": 28, # 当前关节错误码 (7 个寄存器: 28-34)
+ "Tip_Force_Start": 35, # 指尖力数据 (20 个寄存器: 35-54)
+ "Pressure_Data_Start": 57, # 压力传感器数据起始 (96 个寄存器: 57-152)
+ "Version_Start": 153 # 版本信息 (6 个寄存器: 153-158)
+}
+
+# 辅助常量
+_JOINT_COUNT = 7
+_VERSION_COUNT = 6
+_TIP_FORCE_COUNT = 20
+_PRESSURE_REG_COUNT = 96
+_PRESSURE_ROWS = 12 # 从 IR 56 (0xC6) 推断
+_PRESSURE_COLS = 6 # 从 IR 56 (0xC6) 推断
+_PRESSURE_DATA_SIZE = _PRESSURE_ROWS * _PRESSURE_COLS # 72
+_PRESSURE_HEADER_SKIP = 10 # 假设跳过 10 个头部/校验字节
+
+# 通信间隔时间 (使用 L10 参考中的 5ms)
+_INTERVAL = 0.005
+
+
+class LinkerHandL7RS485:
+ """
+ O7机械手 Modbus RTU (RS485) 控制类。
+ 使用 pymodbus 3.5.1 版本和 O7 机械手协议。
+ """
+ def __init__(self,
+ hand_id: int = 0x27,
+ modbus_port: str = "/dev/ttyUSB0",
+ baudrate: int = DEFAULT_BAUDRATE,
+ timeout: float = 0.05):
+ """
+ 初始化 Modbus 客户端。
+
+ :param hand_id: Modbus 从站地址 (0x27: 右手, 0x28: 左手)
+ :param modbus_port: 串口名称
+ :param baudrate: 波特率 (默认为 115200)
+ :param timeout: 通信超时时间 (秒)
+ """
+ self.slave = hand_id
+ self.cli = ModbusSerialClient(
+ port=modbus_port,
+ baudrate=baudrate,
+ bytesize=8,
+ parity="N",
+ stopbits=1, # 确保与 pymodbus 3.x 兼容的写法
+ timeout=timeout,
+ retries=3, # 重试次数
+ retry_on_empty=True,
+ handle_local_echo=False,
+ method='rtu'
+ )
+
+ # 尝试连接
+ self.connected = self.cli.connect()
+ if not self.connected:
+ raise ConnectionError(f"RS485 connect fail to {modbus_port} with ID {hex(hand_id)}.")
+
+ print(f"O7机械手 Modbus ID {hex(hand_id)} 连接成功到 {modbus_port}。")
+
+
+ # --------------------------------------------------
+ # 核心读写函数 (基于 pymodbus 3.5.1)
+ # --------------------------------------------------
+
+ def _read_input_registers(self, address: int, count: int) -> List[int]:
+ """封装 Modbus 读取输入寄存器 (FC 04) 操作。"""
+ time.sleep(_INTERVAL)
+ try:
+ rsp = self.cli.read_input_registers(
+ address=address,
+ count=count,
+ slave=self.slave
+ )
+
+ # 使用 L10 参考中验证过的 3.x 兼容错误检查
+ if rsp.isError():
+ raise RuntimeError(f"Modbus FC04 读取失败。地址: {address}, 错误: {rsp}")
+
+ return rsp.registers
+
+ except ModbusException as e:
+ # 捕获通信超时、CRC 错误等 Modbus 异常
+ raise RuntimeError(f"Modbus 通信异常。地址: {address}, 错误: {e}")
+ except Exception as e:
+ raise RuntimeError(f"未知读取异常。地址: {address}, 错误: {e}")
+
+ def _write_holding_registers(self, address: int, values: List[int]):
+ """封装 Modbus 写入保持寄存器 (FC 16) 操作。"""
+ time.sleep(_INTERVAL)
+
+ # 批量写入 (FC 16)
+ if len(values) > 1:
+ write_func = self.cli.write_registers
+ # 单个写入 (FC 06)
+ elif len(values) == 1:
+ write_func = lambda address, values, slave: self.cli.write_register(address, values[0], slave)
+ else:
+ raise ValueError("写入值列表不能为空。")
+
+ try:
+ rsp = write_func(
+ address=address,
+ values=values,
+ slave=self.slave
+ )
+
+ if rsp.isError():
+ raise RuntimeError(f"Modbus FC16 写入失败。地址: {address}, 错误: {rsp}")
+
+ except ModbusException as e:
+ raise RuntimeError(f"Modbus 通信异常。地址: {address}, 错误: {e}")
+ except Exception as e:
+ raise RuntimeError(f"未知写入异常。地址: {address}, 错误: {e}")
+
+ # --------------------------------------------------
+ # 读操作 (Read API)
+ # --------------------------------------------------
+
+ def get_joint_positions(self) -> Dict[str, int]:
+ """读取当前关节位置 (地址 0-6)。"""
+ registers = self._read_input_registers(IR_ADDR["Current_Position_Start"], _JOINT_COUNT)
+ #return dict(zip(O7_JOINT_KEYS, registers))
+ return registers
+
+ def get_current_torques(self) -> Dict[str, int]:
+ """读取当前关节转矩 (地址 7-13)。"""
+ registers = self._read_input_registers(IR_ADDR["Current_Torque_Start"], _JOINT_COUNT)
+ #return dict(zip(O7_JOINT_KEYS, registers))
+ return registers
+
+ def get_current_speeds(self) -> Dict[str, int]:
+ """读取当前关节速度 (地址 14-20)。"""
+ registers = self._read_input_registers(IR_ADDR["Current_Speed_Start"], _JOINT_COUNT)
+ #return dict(zip(O7_JOINT_KEYS, registers))
+ return registers
+
+ def get_temperatures(self) -> Dict[str, int]:
+ """读取当前关节温度 (地址 21-27)。"""
+ registers = self._read_input_registers(IR_ADDR["Current_Temperature_Start"], _JOINT_COUNT)
+ #return dict(zip(O7_JOINT_KEYS, registers))
+ return registers
+
+ def get_error_codes(self) -> Dict[str, int]:
+ """读取当前关节错误码 (地址 28-34)。"""
+ registers = self._read_input_registers(IR_ADDR["Error_Code_Start"], _JOINT_COUNT)
+ #return dict(zip(O7_JOINT_KEYS, registers))
+ return registers
+
+ def get_tip_forces(self) -> List[int]:
+ """读取指尖法向力、切向力等数据 (地址 35-54)。"""
+ return self._read_input_registers(IR_ADDR["Tip_Force_Start"], _TIP_FORCE_COUNT)
+
+ def get_version(self) -> List[int]:
+ """读取版本信息 (地址 153-158)。"""
+ return self._read_input_registers(IR_ADDR["Version_Start"], _VERSION_COUNT)
+
+ def get_pressure_matrix(self, finger_id: int) -> np.ndarray:
+ """
+ 读取特定手指的压力传感器数据矩阵。
+
+ :param finger_id: 手指编号 (1: 大拇指, 2: 食指, 3: 中指, 4: 无名指, 5: 小拇指)
+ :return: 12x6 的压力数据矩阵 (np.ndarray)
+ """
+ if not (1 <= finger_id <= 5):
+ raise ValueError(f"无效的手指编号: {finger_id}。应在 1 到 5 之间。")
+
+ # 1. 写入手指选择寄存器 (HR 42)
+ # 使用单个写入 (FC 06)
+ self._write_holding_registers(HR_ADDR["Pressure_Select"], [finger_id])
+
+ # 2. 读取压力传感器数据 (IR 57, 96 个寄存器)
+ time.sleep(_INTERVAL) # 等待数据更新
+ registers_16bit: List[int] = self._read_input_registers(
+ IR_ADDR["Pressure_Data_Start"],
+ _PRESSURE_REG_COUNT
+ )
+
+ # 3. 数据解析 (假设与 L10 类似的数据格式: 低 8 位有效，有头部数据)
+
+ # a. 提取低 8 位数据 (得到 96 个 8 位数据点)
+ final_data_96 = [reg_value & 255 for reg_value in registers_16bit]
+
+ # b. 跳过头部数据点
+ effective_data = np.array(final_data_96[_PRESSURE_HEADER_SKIP:], dtype=np.uint8)
+
+ # c. 截取当前手指的矩阵数据 (72 个点)
+ finger_data_flat = effective_data[:_PRESSURE_DATA_SIZE]
+
+ if finger_data_flat.size != _PRESSURE_DATA_SIZE:
+ raise ValueError(
+ f"压力数据提取失败。期望 {_PRESSURE_DATA_SIZE} 点，"
+ f"但仅截取到 {finger_data_flat.size} 点。请检查协议解析逻辑。"
+ )
+
+ # d. 重塑为二维矩阵 (12 行 6 列)
+ finger_matrix = finger_data_flat.reshape((_PRESSURE_ROWS, _PRESSURE_COLS))
+ return finger_matrix
+
+
+ # --------------------------------------------------
+ # 写操作 (Write API)
+ # --------------------------------------------------
+
+ def set_joint_positions(self, joint_angles: List[int]):
+ """
+ 设置所有 7 个关节的目标位置 (地址 0-6)。
+ :param joint_angles: 7 个 0-255 的整数值列表
+ """
+ if len(joint_angles) != _JOINT_COUNT:
+ raise ValueError(f"需要 {_JOINT_COUNT} 个关节位置值，提供了 {len(joint_angles)} 个。")
+ self._write_holding_registers(HR_ADDR["Position_Start"], joint_angles)
+
+ def set_torques(self, torques: List[int]):
+ """
+ 设置所有 7 个关节的目标转矩 (地址 7-13)。
+ :param torques: 7 个 0-255 的整数值列表
+ """
+ if len(torques) != _JOINT_COUNT:
+ raise ValueError(f"需要 {_JOINT_COUNT} 个关节转矩值，提供了 {len(torques)} 个。")
+ self._write_holding_registers(HR_ADDR["Torque_Start"], torques)
+
+
+ def set_speeds(self, speeds: List[int]):
+ """
+ 设置所有 7 个关节的目标速度 (地址 14-20)。
+ :param speeds: 7 个 0-255 的整数值列表
+ """
+ if len(speeds) != _JOINT_COUNT:
+ raise ValueError(f"需要 {_JOINT_COUNT} 个关节速度值，提供了 {len(speeds)} 个。")
+ self._write_holding_registers(HR_ADDR["Speed_Start"], speeds)
+
+ def set_speed(self, speed:List[int] = [200] * 7):
+ self.set_speeds(speed)
+
+ def set_torque(self, torque: List[int] = [250] * 7):
+ self.set_torques(torque)
+
+ def set_current(self, current=None):
+ print("当前L7不支持设置电流", flush=True)
+
+ def get_current(self):
+ #print("当前L7不支持获取电流", flush=True)
+ return [-1] * 7
+
+ def get_state(self) -> List[int]:
+ return self.get_joint_positions()
+
+
+ def get_state_for_pub(self) -> List[int]:
+ return self.get_joint_positions()
+
+ def get_current_status(self) -> List[int]:
+ return self.get_joint_positions()
+
+ def get_speed(self) -> List[int]:
+ return self.get_current_speeds()
+
+ def get_joint_speed(self) -> List[int]:
+ return self.get_speed()
+
+ def get_touch_type(self) -> int:
+ return 2
+
+ def get_normal_force(self) -> List[int]:
+ return [-1] * 5
+
+ def get_tangential_force(self) -> List[int]:
+ return [-1] * 5
+
+ def get_approach_inc(self) -> List[int]:
+ return [-1] * 5
+
+ def get_touch(self) -> List[int]:
+ return [-1] * 5
+
+ def get_thumb_matrix_touch(self,sleep_time=0):
+ return self.get_pressure_matrix(finger_id=1)
+
+ def get_index_matrix_touch(self,sleep_time=0):
+ return self.get_pressure_matrix(finger_id=2)
+
+ def get_middle_matrix_touch(self,sleep_time=0):
+ return self.get_pressure_matrix(finger_id=3)
+
+ def get_ring_matrix_touch(self,sleep_time=0):
+ return self.get_pressure_matrix(finger_id=4)
+
+ def get_little_matrix_touch(self,sleep_time=0):
+ return self.get_pressure_matrix(finger_id=5)
+
+ def get_matrix_touch(self) -> List[List[int]]:
+ return self.get_thumb_matrix_touch(),self.get_index_matrix_touch(), self.get_middle_matrix_touch(), self.get_ring_matrix_touch(), self.get_little_matrix_touch()
+
+ def get_matrix_touch_v2(self) -> List[List[int]]:
+ return self.get_matrix_touch()
+
+ def get_torque(self) -> List[int]:
+ return self.get_current_torques()
+
+ def get_temperature(self) -> List[int]:
+ return self.get_temperatures()
+
+ def get_fault(self) -> List[int]:
+ return self.get_error_codes()
+
+ def get_serial_number(self):
+ return [0] * 6
+
+ def get_finger_order(self):
+ return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch", "thumb_cmc_roll"]
+
+ def show_fun_table(self):
+ pass
+
+ def clear_faults(self):
+ pass
+ # --------------------------------------------------
+ # 上下文管理
+ # --------------------------------------------------
+
+ def close(self):
+ """断开 Modbus 连接。"""
+ if self.connected:
+ self.cli.close()
+ self.connected = False
+ print("Modbus 连接已断开。")
+
+ def __enter__(self):
+ return self
+
+ def __exit__(self, exc_type, exc_val, exc_tb):
+ self.close()
+
+
+# ------------------- Demo/使用示例 -------------------
+if __name__ == "__main__":
+ # --- 配置区域 ---
+ # 右手 Modbus ID: 0x27 (39)
+ # 左手 Modbus ID: 0x28 (40)
+ TARGET_HAND_ID = 0x28 # <--- 请根据需要修改为 0x27 或 0x28
+ PORT = "/dev/ttyUSB0" # <--- 请修改为您的实际串口，例如 'COM3'
+
+ try:
+ # 使用上下文管理器，确保连接自动关闭
+ with LinkerHandL7RS485(hand_id=TARGET_HAND_ID, modbus_port=PORT) as hand:
+ print("\n--- 1. 读取当前状态 ---")
+
+ # 读取当前关节位置、速度、转矩
+ angles = hand.get_joint_positions()
+ print(f"当前关节位置 (7DOF): {angles}")
+
+ speeds = hand.get_current_speeds()
+ print(f"当前关节速度: {speeds}")
+
+ # 读取传感器和错误信息
+ temps = hand.get_temperatures()
+ print(f"关节温度: {temps}")
+
+ errors = hand.get_error_codes()
+ print(f"关节错误码: {errors}")
+
+ # 读取版本
+ version_info = hand.get_version()
+ print(f"版本信息 (Hand_freedom, ..., hardware_version): {version_info}")
+
+ # --- 2. 写入指令示例 ---
+ print("\n--- 2. 写入指令示例 (设置所有关节到 128) ---")
+
+ # 假设要将所有关节位置设置到中间值 128
+ target_angles = [128] * _JOINT_COUNT
+ hand.set_joint_positions(target_angles)
+ print(f"写入目标角度: {target_angles}")
+
+ # 假设要设置所有关节的速度到 100
+ target_speeds = [100] * _JOINT_COUNT
+ hand.set_speeds(target_speeds)
+ print(f"写入目标速度: {target_speeds}")
+
+ # --- 3. 压力传感器读取示例 ---
+ print("\n--- 3. 压力传感器读取 (大拇指 1) ---")
+ thumb_matrix = hand.get_pressure_matrix(finger_id=1)
+ print(f"大拇指压力矩阵 (12x6):")
+ print(thumb_matrix)
+
+ except ConnectionError as e:
+ print(f"致命错误: 连接失败。{e}")
+ except RuntimeError as e:
+ print(f"致命错误: Modbus 操作失败。{e}")
+ except Exception as e:
+ print(f"捕获到未知异常: {e}")
\ No newline at end of file
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_o6_rs485.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_o6_rs485.py
new file mode 100644
index 0000000..98240fd
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_o6_rs485.py
@@ -0,0 +1,671 @@
+#!/usr/bin/env python3
+"""
+O6 机械手 Modbus-RTU 控制类 (基于 pymodbus 3.5.1)
+"""
+
+import os
+import time
+from typing import List, Dict, Any # 引入 Any 来表示灵活的输入类型
+import numpy as np
+import logging
+from threading import Lock # 用于线程安全和总线仲裁
+
+# 导入 pymodbus 客户端
+from pymodbus.client import ModbusSerialClient
+from struct import error as StructError
+
+logging.basicConfig(
+ level=logging.INFO,
+ format="[%(asctime)s] %(levelname)-8s %(message)s",
+ datefmt="%H:%M:%S"
+)
+
+# ------------------------------------------------------------------
+# 读输入寄存器地址枚举（功能码 04，只读）- 按照 O6 协议文档定义
+# ------------------------------------------------------------------
+REG_RD_CURRENT_THUMB_PITCH = 0 # 大拇指弯曲角度（0-255，小=弯，大=伸）
+REG_RD_CURRENT_THUMB_YAW = 1 # 大拇指横摆角度（0-255，小=靠掌心，大=远离）
+REG_RD_CURRENT_INDEX_PITCH = 2 # 食指弯曲角度
+REG_RD_CURRENT_MIDDLE_PITCH = 3 # 中指弯曲角度
+REG_RD_CURRENT_RING_PITCH = 4 # 无名指弯曲角度
+REG_RD_CURRENT_LITTLE_PITCH = 5 # 小拇指弯曲角度
+REG_RD_CURRENT_THUMB_TORQUE = 6 # 大拇指弯曲转矩（0-255）
+REG_RD_CURRENT_THUMB_YAW_TORQUE = 7 # 大拇指横摆转矩
+REG_RD_CURRENT_INDEX_TORQUE = 8 # 食指转矩
+REG_RD_CURRENT_MIDDLE_TORQUE = 9 # 中指转矩
+REG_RD_CURRENT_RING_TORQUE = 10 # 无名指转矩
+REG_RD_CURRENT_LITTLE_TORQUE = 11 # 小拇指转矩
+REG_RD_CURRENT_THUMB_SPEED = 12 # 大拇指弯曲速度（0-255）
+REG_RD_CURRENT_THUMB_YAW_SPEED = 13 # 大拇指横摆速度
+REG_RD_CURRENT_INDEX_SPEED = 14 # 食指速度
+REG_RD_CURRENT_MIDDLE_SPEED = 15 # 中指速度
+REG_RD_CURRENT_RING_SPEED = 16 # 无名指速度
+REG_RD_CURRENT_LITTLE_SPEED = 17 # 小拇指速度
+REG_RD_THUMB_TEMP = 18 # 大拇指弯曲温度（0-70℃）
+REG_RD_THUMB_YAW_TEMP = 19 # 大拇指横摆温度
+REG_RD_INDEX_TEMP = 20 # 食指温度
+REG_RD_MIDDLE_TEMP = 21 # 中指温度
+REG_RD_RING_TEMP = 22 # 无名指温度
+REG_RD_LITTLE_TEMP = 23 # 小拇指温度
+REG_RD_THUMB_ERROR = 24 # 大拇指错误码
+REG_RD_THUMB_YAW_ERROR = 25 # 大拇指横摆错误码
+REG_RD_INDEX_ERROR = 26 # 食指错误码
+REG_RD_MIDDLE_ERROR = 27 # 中指错误码
+REG_RD_RING_ERROR = 28 # 无名指错误码
+REG_RD_LITTLE_ERROR = 29 # 小拇指错误码
+
+# 版本号/设备编号寄存器（地址 30-44，共15个寄存器）
+REG_RD_HAND_FREEDOM = 30 # Hand_freedom - 设备编号 / 自由度（与机械手上标签相同）
+REG_RD_HAND_VERSION = 31 # hand_version - 手版本
+REG_RD_HAND_NUMBER_HIGH = 32 # hand_number_高位 - 设备编号（高字节）
+REG_RD_HAND_NUMBER_MID = 33 # hand_number_中位 - 设备编号（中字节）
+REG_RD_HAND_NUMBER_LOW = 34 # hand_number_低位 - 设备编号（低字节）
+REG_RD_HAND_DIRECTION = 35 # hand_direction - 手方向（左/右）
+REG_RD_HARDWARE_VERSION_HIGH = 36 # hardware_version_高位 - 硬件版本（高字节）
+REG_RD_HARDWARE_VERSION_MID = 37 # hardware_version_中位 - 硬件版本（中字节）
+REG_RD_HARDWARE_VERSION_LOW = 38 # hardware_version_低位 - 硬件版本（低字节）
+REG_RD_SOFTWARE_VERSION_HIGH = 39 # software_version_高位 - 软件版本（高字节）
+REG_RD_SOFTWARE_VERSION_MID = 40 # software_version_中位 - 软件版本（中字节）
+REG_RD_SOFTWARE_VERSION_LOW = 41 # software_version_低位 - 软件版本（低字节）
+REG_RD_MECHANICAL_VERSION_HIGH = 42 # mechanical_version_高位 - 机械版本（高字节）
+REG_RD_MECHANICAL_VERSION_MID = 43 # mechanical_version_中位 - 机械版本（中字节）
+REG_RD_MECHANICAL_VERSION_LOW = 44 # mechanical_version_低位 - 机械版本（低字节）
+
+# 力传感器寄存器（地址 45-87+，动态范围）
+REG_RD_PRESSURE_SENSING_ID = 45 # Pressure_Sensing_ID - 压力传感器ID (0-5)
+REG_RD_PRESSURE_SENSING_SPEC = 46 # Pressure_Sensing_Specifications - 传感器数据规格
+
+
+# ------------------------------------------------------------------
+# 写保持寄存器地址枚举（功能码 16，读写）- 保持原样
+# ------------------------------------------------------------------
+REG_WR_THUMB_PITCH = 0 # 大拇指弯曲角度（0-255）
+REG_WR_THUMB_YAW = 1 # 大拇指横摆角度
+REG_WR_INDEX_PITCH = 2 # 食指弯曲角度
+REG_WR_MIDDLE_PITCH = 3 # 中指弯曲角度
+REG_WR_RING_PITCH = 4 # 无名指弯曲角度
+REG_WR_LITTLE_PITCH = 5 # 小拇指弯曲角度
+REG_WR_THUMB_TORQUE = 6 # 大拇指弯曲转矩
+REG_WR_THUMB_YAW_TORQUE = 7 # 大拇指横摆转矩
+REG_WR_INDEX_TORQUE = 8 # 食指转矩
+REG_WR_MIDDLE_TORQUE = 9 # 中指转矩
+REG_WR_RING_TORQUE = 10 # 无名指转矩
+REG_WR_LITTLE_TORQUE = 11 # 小拇指转矩
+REG_WR_THUMB_SPEED = 12 # 大拇指弯曲速度
+REG_WR_THUMB_YAW_SPEED = 13 # 大拇指横摆速度
+REG_WR_INDEX_SPEED = 14 # 食指速度
+REG_WR_MIDDLE_SPEED = 15 # 中指速度
+REG_WR_RING_SPEED = 16 # 无名指速度
+REG_WR_LITTLE_SPEED = 17 # 小拇指速度
+
+
+class LinkerHandO6RS485:
+ """O6 机械手 Modbus-RTU 控制类，使用 pymodbus 3.5.1"""
+
+ TTL_TIMEOUT = 0.15 # 串口超时
+ FRAME_GAP = 0.030 # 30 ms
+
+ # KEYS for easy indexing
+ JOINT_KEYS = ["thumb_pitch", "thumb_yaw", "index_pitch",
+ "middle_pitch", "ring_pitch", "little_pitch"]
+
+ def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200):
+ self._id = hand_id
+ self._last_ts = 0.0 # 上一次帧结束时间
+ self._lock = Lock() # 总线访问锁
+
+ # 使用 pymodbus 3.x 客户端
+ self.cli = ModbusSerialClient(
+ port=modbus_port,
+ baudrate=baudrate,
+ bytesize=8,
+ parity="N",
+ stopbits=1,
+ timeout=self.TTL_TIMEOUT,
+ handle_local_echo=False
+ )
+
+ try:
+ logging.info(f"Connecting to Modbus RTU on {modbus_port}...")
+ self.connected = self.cli.connect()
+ if not self.connected:
+ raise ConnectionError(f"RS485 connect fail to {modbus_port}")
+ logging.info("Connection successful.")
+ except Exception as e:
+ logging.error(f"Initialization failed: {e}")
+ raise
+
+ # ----------------------------------------------------------
+ # 辅助方法
+ # ----------------------------------------------------------
+ def _bus_free(self):
+ """保证距离上一帧 ≥ 30 ms"""
+ with self._lock:
+ elapse = time.perf_counter() - self._last_ts
+ if elapse < self.FRAME_GAP:
+ time.sleep(self.FRAME_GAP - elapse)
+
+ def _execute_read(self, address: int, count: int) -> List[int]:
+ """执行 Modbus 读取操作 (功能码 04), 带总线仲裁。"""
+ self._bus_free()
+
+ rsp = self.cli.read_input_registers(
+ address=address,
+ count=count,
+ slave=self._id
+ )
+
+ self._last_ts = time.perf_counter()
+
+ if rsp.isError():
+ raise RuntimeError(f"Modbus Read Failed (Addr={address}, Count={count}): {rsp}")
+
+ # 确保返回的值是 Python 原生整数
+ return [int(reg) for reg in rsp.registers]
+
+ def _execute_write(self, address: int, values: List[int]):
+ """执行 Modbus 批量写入操作 (功能码 16), 带总线仲裁。"""
+ self._bus_free()
+
+ # values 必须是 Python 原生整数列表
+ rsp = self.cli.write_registers(
+ address=address,
+ values=values,
+ slave=self._id
+ )
+
+ self._last_ts = time.perf_counter()
+
+ if rsp.isError():
+ raise RuntimeError(f"Modbus Write Failed (Addr={address}, Values={values}): {rsp}")
+
+ # ----------------------------------------------------------
+ # 批量读取和数据封装（优化通信效率）
+ # ----------------------------------------------------------
+ def read_all_angles(self) -> List[int]:
+ return self._execute_read(REG_RD_CURRENT_THUMB_PITCH, 6)
+
+ def read_all_torques(self) -> List[int]:
+ return self._execute_read(REG_RD_CURRENT_THUMB_TORQUE, 6)
+
+ def read_all_speeds(self) -> List[int]:
+ return self._execute_read(REG_RD_CURRENT_THUMB_SPEED, 6)
+
+ def read_all_temperatures(self) -> List[int]:
+ return self._execute_read(REG_RD_THUMB_TEMP, 6)
+
+ def read_all_errors(self) -> List[int]:
+ return self._execute_read(REG_RD_THUMB_ERROR, 6)
+
+ # ----------------------------------------------------------
+ # 版本号/设备编号读取（按照协议文档：地址30-44，共15个寄存器）
+ # ----------------------------------------------------------
+ def read_all_versions(self) -> str:
+ """一次性读取全部15个寄存器 (地址30-44)，返回以 '.' 连接的字符串。
+
+ 按 O6 协议文档的版本号格式返回：
+ hand_freedom.hand_version.hand_number_high.hand_number_mid.hand_number_low
+ .hand_direction.hardware_ver_hardware_ver_m.hardware_ver_l
+ .software_ver_h.software_ver_m.software_ver_l
+ .mechanical_ver_h.mechanical_ver_m.mechanical_ver_l
+
+ 例如: "6.1.001.002.003.0.1.2.3.4.5.6.7.8.9"
+ """
+ raw = self._execute_read(REG_RD_HAND_FREEDOM, 15)
+ return ".".join(str(v) for v in raw)
+
+ # ----------------------------------------------------------
+ # 基于 read_all_versions() 的设备编号解析方法
+ # ----------------------------------------------------------
+ def _parse_versions(self):
+ """解析 read_all_versions() 返回的字符串为字典"""
+ parts = self.read_all_versions().split(".")
+ if len(parts) < 15:
+ return {}
+ return {
+ "hand_freedom": int(parts[0]),
+ "hand_version": int(parts[1]),
+ "hand_number_high": int(parts[2]),
+ "hand_number_mid": int(parts[3]),
+ "hand_number_low": int(parts[4]),
+ "hand_direction": int(parts[5]),
+ "hw_ver_high": int(parts[6]),
+ "hw_ver_mid": int(parts[7]),
+ "hw_ver_low": int(parts[8]),
+ "sw_ver_high": int(parts[9]),
+ "sw_ver_mid": int(parts[10]),
+ "sw_ver_low": int(parts[11]),
+ "mech_ver_high": int(parts[12]),
+ "mech_ver_mid": int(parts[13]),
+ "mech_ver_low": int(parts[14]),
+ }
+
+ def get_device_number(self) -> str:
+ """获取设备编号（与机械手上标签相同）。格式：高位+中位+低位 拼接的字符串。"""
+ v = self._parse_versions()
+ if not v:
+ return "0"
+ high, mid, low = v["hand_number_high"], v["hand_number_mid"], v["hand_number_low"]
+ # 将每个字节格式化为无前导零的整数（与标签显示一致）
+ return f"{high}{mid}{low}"
+
+ def get_device_number_value(self) -> int:
+ """获取设备编号数值"""
+ v = self._parse_versions()
+ if not v:
+ return 0
+ high, mid, low = v["hand_number_high"], v["hand_number_mid"], v["hand_number_low"]
+ return high * 65536 + mid * 256 + low
+
+ def get_hardware_version(self) -> str:
+ """获取硬件版本号。格式：高.中.低"""
+ v = self._parse_versions()
+ if not v:
+ return "0.0.0"
+ return f"{v['hw_ver_high']}.{v['hw_ver_mid']}.{v['hw_ver_low']}"
+
+ def get_software_version(self) -> str:
+ """获取软件版本号。格式：高.中.低"""
+ v = self._parse_versions()
+ if not v:
+ return "0.0.0"
+ return f"{v['sw_ver_high']}.{v['sw_ver_mid']}.{v['sw_ver_low']}"
+
+ def get_mechanical_version(self) -> str:
+ """获取机械版本号。格式：高.中.低"""
+ v = self._parse_versions()
+ if not v:
+ return "0.0.0"
+ return f"{v['mech_ver_high']}.{v['mech_ver_mid']}.{v['mech_ver_low']}"
+
+ def get_hand_freedom(self) -> int:
+ """获取自由度（与机械手上标签相同）"""
+ v = self._parse_versions()
+ return v.get("hand_freedom", 0)
+
+ def get_hand_version_raw(self) -> int:
+ """获取手版本原始值"""
+ v = self._parse_versions()
+ return v.get("hand_version", 0)
+
+ def get_hand_direction(self) -> str:
+ """获取手方向，转换为字符：76→'L', 82→'R'"""
+ v = self._parse_versions()
+ val = v.get("hand_direction", 0)
+ if val in (76, 82):
+ return chr(val)
+ # fallback: 直接转为字符（如果值在可打印范围内）
+ return chr(val) if 32 < val < 128 else f'Unknown({val})'
+
+
+ # ----------------------------------------------------------
+ # 只读属性（单个寄存器读取）
+ # ----------------------------------------------------------
+ def _read_reg(self, addr: int) -> int:
+ """读单个输入寄存器（功能码 04），带 30 ms 帧间隔"""
+ return self._execute_read(addr, 1)[0]
+
+ def get_thumb_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_PITCH)
+ def get_thumb_yaw(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_YAW)
+ def get_index_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_INDEX_PITCH)
+ def get_middle_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_MIDDLE_PITCH)
+ def get_ring_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_RING_PITCH)
+ def get_little_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_LITTLE_PITCH)
+
+ def get_thumb_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_TORQUE)
+ def get_thumb_yaw_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_YAW_TORQUE)
+ def get_index_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_INDEX_TORQUE)
+ def get_middle_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_MIDDLE_TORQUE)
+ def get_ring_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_RING_TORQUE)
+ def get_little_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_LITTLE_TORQUE)
+
+ def get_thumb_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_SPEED)
+ def get_thumb_yaw_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_YAW_SPEED)
+ def get_index_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_INDEX_SPEED)
+ def get_middle_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_MIDDLE_SPEED)
+ def get_ring_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_RING_SPEED)
+ def get_little_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_LITTLE_SPEED)
+
+ def get_thumb_temp(self) -> int: return self._read_reg(REG_RD_THUMB_TEMP)
+ def get_thumb_yaw_temp(self) -> int: return self._read_reg(REG_RD_THUMB_YAW_TEMP)
+ def get_index_temp(self) -> int: return self._read_reg(REG_RD_INDEX_TEMP)
+ def get_middle_temp(self) -> int: return self._read_reg(REG_RD_MIDDLE_TEMP)
+ def get_ring_temp(self) -> int: return self._read_reg(REG_RD_RING_TEMP)
+ def get_little_temp(self) -> int: return self._read_reg(REG_RD_LITTLE_TEMP)
+
+ def get_thumb_error(self) -> int: return self._read_reg(REG_RD_THUMB_ERROR)
+ def get_thumb_yaw_error(self) -> int: return self._read_reg(REG_RD_THUMB_YAW_ERROR)
+ def get_index_error(self) -> int: return self._read_reg(REG_RD_INDEX_ERROR)
+ def get_middle_error(self) -> int: return self._read_reg(REG_RD_MIDDLE_ERROR)
+ def get_ring_error(self) -> int: return self._read_reg(REG_RD_RING_ERROR)
+ def get_little_error(self) -> int: return self._read_reg(REG_RD_LITTLE_ERROR)
+
+
+ # ----------------------------------------------------------
+ # 批量 Getter (使用 read_all_... 方法)
+ # ----------------------------------------------------------
+ def get_state(self) -> List[int]:
+ """获取手指电机状态 (角度)"""
+ return self.read_all_angles()
+
+ def get_torque(self) -> List[int]:
+ """获取当前扭矩"""
+ return self.read_all_torques()
+
+ def get_speed(self) -> List[int]:
+ """获取当前速度"""
+ return self.read_all_speeds()
+
+ def get_temperature(self) -> List[int]:
+ """获取当前电机温度"""
+ return self.read_all_temperatures()
+
+ def get_fault(self) -> List[int]:
+ """获取当前电机故障码"""
+ return self.read_all_errors()
+
+ def get_version(self) -> str:
+ """获取当前固件版本号（已转换为字符串格式）"""
+ return self.read_all_versions()
+
+
+ # ----------------------------------------------------------
+ # 写保持寄存器 (单个寄存器写入)
+ # ----------------------------------------------------------
+ def _write_reg(self, addr: int, value: int):
+ """写单个保持寄存器（功能码 16），带 30 ms 帧间隔"""
+ if not 0 <= value <= 255:
+ raise ValueError("value must be 0-255")
+
+ # 确保 value 是 Python 原生 int
+ self._execute_write(addr, [int(value)])
+
+ def _write_regs(self, addr: int, values: List[int]):
+ """写多个保持寄存器（功能码 16），带 30 ms 帧间隔"""
+ # 此时 values 应该已经是经过 is_valid_6xuint8 验证并转换的 Python int 列表
+ if not all(0 <= v <= 255 for v in values):
+ # 这行理论上不应触发，因为上层调用已校验
+ raise ValueError("All values must be 0-255")
+ self._execute_write(addr, values)
+
+
+ def set_thumb_pitch(self, v: int): self._write_reg(REG_WR_THUMB_PITCH, v)
+ def set_thumb_yaw(self, v: int): self._write_reg(REG_WR_THUMB_YAW, v)
+ def set_index_pitch(self, v: int): self._write_reg(REG_WR_INDEX_PITCH, v)
+ def set_middle_pitch(self, v: int): self._write_reg(REG_WR_MIDDLE_PITCH, v)
+ def set_ring_pitch(self, v: int): self._write_reg(REG_WR_RING_PITCH, v)
+ def set_little_pitch(self, v: int): self._write_reg(REG_WR_LITTLE_PITCH, v)
+
+ def set_thumb_torque(self, v: int): self._write_reg(REG_WR_THUMB_TORQUE, v)
+ def set_thumb_yaw_torque(self, v: int): self._write_reg(REG_WR_THUMB_YAW_TORQUE, v)
+ def set_index_torque(self, v: int): self._write_reg(REG_WR_INDEX_TORQUE, v)
+ def set_middle_torque(self, v: int): self._write_reg(REG_WR_MIDDLE_TORQUE, v)
+ def set_ring_torque(self, v: int): self._write_reg(REG_WR_RING_TORQUE, v)
+ def set_little_torque(self, v: int): self._write_reg(REG_WR_LITTLE_TORQUE, v)
+
+ def set_thumb_speed(self, v: int): self._write_reg(REG_WR_THUMB_SPEED, v)
+ def set_thumb_yaw_speed(self, v: int): self._write_reg(REG_WR_THUMB_YAW_SPEED, v)
+ def set_index_speed(self, v: int): self._write_reg(REG_WR_INDEX_SPEED, v)
+ def set_middle_speed(self, v: int): self._write_reg(REG_WR_MIDDLE_SPEED, v)
+ def set_ring_speed(self, v: int): self._write_reg(REG_WR_RING_SPEED, v)
+ def set_little_speed(self, v: int): self._write_reg(REG_WR_LITTLE_SPEED, v)
+
+ # ----------------------------------------------------------
+ # 固定函数 (采用批量写入优化)
+ # ----------------------------------------------------------
+ def is_valid_6xuint8(self, lst: List[Any]) -> bool:
+ """
+ 验证6个0-255的整数列表。
+ 允许输入包含浮点数、NumPy整数等可转换为 int 的类型，并进行范围校验。
+ """
+ if not (isinstance(lst, list) and len(lst) == 6):
+ return False
+
+ try:
+ # 关键：尝试将所有元素转换为 Python 原生 int
+ int_values = [int(v) for v in lst]
+ except (ValueError, TypeError):
+ # 转换失败，列表中包含不可转换的元素
+ return False
+
+ # 校验转换后的整数列表是否在 0-255 范围内
+ return all(0 <= x <= 255 for x in int_values)
+
+ def set_joint_positions(self, joint_angles: List[Any] = None):
+ joint_angles = joint_angles or [0] * 6
+
+ if not self.is_valid_6xuint8(joint_angles):
+ logging.error(f"Invalid joint angles received: {joint_angles}")
+ raise ValueError("Joint angles must be a list of 6 values between 0 and 255 (convertible to int).")
+
+ # 强制转换为 Modbus 兼容的 Python 原生 int 列表
+ int_angles = [int(v) for v in joint_angles]
+
+ # 批量写入 6 个角度寄存器 (从 REG_WR_THUMB_PITCH 地址 0 开始, count=6)
+ self._write_regs(REG_WR_THUMB_PITCH, int_angles)
+
+ def set_speed(self, speed: List[Any] = None):
+ speed = speed or [200] * 6
+ if not self.is_valid_6xuint8(speed):
+ logging.error(f"Invalid speed values received: {speed}")
+ raise ValueError("Speed values must be a list of 6 values between 0 and 255 (convertible to int).")
+
+ int_speed = [int(v) for v in speed]
+ self._write_regs(REG_WR_THUMB_SPEED, int_speed)
+
+ def set_torque(self, torque: List[Any] = None):
+ torque = torque or [200] * 6
+ if not self.is_valid_6xuint8(torque):
+ logging.error(f"Invalid torque values received: {torque}")
+ raise ValueError("Torque values must be a list of 6 values between 0 and 255 (convertible to int).")
+
+ int_torque = [int(v) for v in torque]
+ self._write_regs(REG_WR_THUMB_TORQUE, int_torque)
+
+ # ... (其他固定函数保持不变) ...
+
+ def set_current(self, current: List[int] = None):
+ print("当前O6不支持设置电流", flush=True)
+ pass
+
+ def get_state_for_pub(self) -> list:
+ return self.get_state()
+
+ def get_current_status(self) -> list:
+ return self.get_state()
+
+ def get_joint_speed(self) -> list:
+ return self.get_speed()
+
+ def get_touch_type(self) -> list:
+ return -1
+
+ def get_normal_force(self) -> list:
+ return [-1] * 5
+
+ def get_tangential_force(self) -> list:
+ return [-1] * 5
+
+ def get_approach_inc(self) -> list:
+ return [-1] * 5
+
+ def get_touch(self) -> list:
+ return [-1] * 5
+
+ def _pressure(self, finger: int) -> np.ndarray:
+ """
+ 读取压力传感器数据 (10x4矩阵)
+ """
+ rows = 10 # 10行
+ cols = 4 # 4列
+ finger_size = rows * cols # 40个数据点
+
+ # modbus 地址 (按O6协议文档)
+ write_address = 18 # 写入手指选择 (保持寄存器)
+ read_address = 47 # 读取压力数据 (输入寄存器)
+ read_count = 40 # 读取40个寄存器
+
+ # 0. 参数校验
+ if finger < 1 or finger > 5:
+ raise ValueError(f"无效的手指编号: {finger}。手指编号应在 1 到 5 之间。")
+
+ # 1. 写入手指选择寄存器 (地址18)
+ time.sleep(0.01)
+ self._write_reg(write_address, finger)
+
+ # 2. 读取压力数据
+ data = self._execute_read(read_address, read_count)
+
+ # 3. 转换为numpy数组并重塑为10x4矩阵
+ finger_matrix = np.array(data, dtype=np.uint8).reshape((rows, cols))
+
+ return finger_matrix
+
+ def get_thumb_matrix_touch(self,sleep_time=0):
+ return np.array(self._pressure(1), dtype=np.uint8)
+
+
+ def get_index_matrix_touch(self,sleep_time=0):
+ return np.array(self._pressure(2), dtype=np.uint8)
+
+ def get_middle_matrix_touch(self,sleep_time=0):
+ return np.array(self._pressure(3), dtype=np.uint8)
+
+ def get_ring_matrix_touch(self,sleep_time=0):
+ return np.array(self._pressure(4), dtype=np.uint8)
+
+ def get_little_matrix_touch(self,sleep_time=0):
+ return np.array(self._pressure(5), dtype=np.uint8)
+
+ def get_matrix_touch(self) -> list:
+ thumb_matrix = np.full((12, 6), -1)
+ index_matrix = np.full((12, 6), -1)
+ middle_matrix = np.full((12, 6), -1)
+ ring_matrix = np.full((12, 6), -1)
+ little_matrix = np.full((12, 6), -1)
+ return thumb_matrix , index_matrix , middle_matrix , ring_matrix , little_matrix
+
+ def get_serial_number(self):
+
+ return "["+str(self.get_hand_freedom())+"-"+str(self.get_mechanical_version())+"-"+str(self.get_device_number())+"-"+str(self.get_hand_direction())+"]"
+
+ def get_matrix_touch_v2(self) -> list:
+ return self.get_matrix_touch()
+
+ def get_finger_order(self):
+ return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
+
+ def clear_faults(self):
+ pass
+
+ def close(self):
+ if hasattr(self, 'connected') and self.connected:
+ self.cli.close()
+ self.connected = False
+ logging.info("Modbus connection closed.")
+
+ def __enter__(self):
+ return self
+
+ def __exit__(self, exc_type, exc_val, exc_tb):
+ self.close()
+
+ # ----------------------------------------------------------
+ # 便捷函数
+ # ----------------------------------------------------------
+ def set_all_fingers(self, pitch: int):
+ """同时设置五指弯曲角度（0-255），使用批量写入"""
+ # 允许传入 float/numpy int 等可转换为 int 的类型
+ try:
+ pitch_int = int(pitch)
+ except (ValueError, TypeError):
+ raise ValueError("Pitch value must be a number convertible to int (0-255)")
+
+ if not 0 <= pitch_int <= 255:
+ raise ValueError("Pitch value must be 0-255")
+
+ # 批量设置所有 6 个关节的角度
+ self.set_joint_positions([pitch_int] * 6)
+
+ def relax(self):
+ """全部手指伸直（255）"""
+ self.set_all_fingers(255)
+
+ def fist(self):
+ """全部手指弯曲（0）"""
+ self.set_all_fingers(0)
+
+ def dump_status(self):
+ """打印当前所有可读状态 (使用批量读取优化)"""
+ print("--------- O6 Hand Status ---------")
+
+ angles = self.get_state()
+ temps = self.get_temperature()
+ errors = self.get_fault()
+
+ # 解析版本号字符串
+ v = self._parse_versions()
+
+ if v:
+ device_num_str = f"{v['hand_number_high']}{v['hand_number_mid']}{v['hand_number_low']}"
+ hw_ver = f"{v['hw_ver_high']}.{v['hw_ver_mid']}.{v['hw_ver_low']}"
+ sw_ver = f"{v['sw_ver_high']}.{v['sw_ver_mid']}.{v['sw_ver_low']}"
+ mech_ver = f"{v['mech_ver_high']}.{v['mech_ver_mid']}.{v['mech_ver_low']}"
+
+ print(f"Device Number: {device_num_str}")
+ print(f"HWSW Version: HW={hw_ver} SW={sw_ver}")
+ print(f"Mechanical Ver: {mech_ver}")
+ print(f"Hand Freedom: {v['hand_freedom']}")
+ print(f"Full Version: {self.read_all_versions()}")
+
+ print(f"Joint Angles: {angles}")
+ print(f"Temperature: {temps}℃")
+ print(f"Error Codes: {errors}")
+ print("----------------------------------")
+
+
+# ------------------------------------------------------------------
+# 命令行快速测试
+# ------------------------------------------------------------------
+if __name__ == "__main__":
+ import argparse
+
+ # 假设默认站号是 0x27 (39)
+ DEFAULT_HAND_ID = 0x27
+
+ parser = argparse.ArgumentParser(description="O6 Hand Modbus tester (using pymodbus 3.5.1)")
+ parser.add_argument("-p", "--port", required=True, help="串口, 如 /dev/ttyUSB0")
+ parser.add_argument("-l", "--left", action="store_const", const=0x28, default=DEFAULT_HAND_ID, dest='hand_id', help="左手 (0x28)，默认右手 (0x27)")
+
+ args = parser.parse_args()
+
+ try:
+ # 使用 with 语句确保连接关闭，这是 pymodbus 的推荐用法
+ with LinkerHandO6RS485(hand_id=args.hand_id, modbus_port=args.port, baudrate=115200) as hand:
+ hand.dump_status()
+
+ # 测试新增的设备编号读取方法
+ print("\n--- 设备信息 ---")
+ print(f"设备编号（字符串）: {hand.get_device_number()}")
+ print(f"设备编号（数值）: {hand.get_device_number_value()}")
+ print(f"硬件版本号: {hand.get_hardware_version()}")
+ print(f"软件版本号: {hand.get_software_version()}")
+ print(f"机械版本号: {hand.get_mechanical_version()}")
+
+ print("\n执行 relax → 伸直")
+ hand.relax()
+ time.sleep(1)
+ print("执行 fist → 握拳")
+ hand.fist()
+ time.sleep(1)
+ hand.relax()
+ print("演示完成")
+
+ except ConnectionError as e:
+ print(f"连接错误: {e}")
+ except RuntimeError as e:
+ print(f"Modbus 运行时错误: {e}")
+ except StructError as e:
+ print(f"数据结构错误 (请检查输入数据类型是否为原生int): {e}")
+ except Exception as e:
+ print(f"发生其他错误: {e}")
\ No newline at end of file
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_o6_rs485.py.bak b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_o6_rs485.py.bak
new file mode 100644
index 0000000..cb08dd9
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_o6_rs485.py.bak
@@ -0,0 +1,1157 @@
+#!/usr/bin/env python3
+"""
+O6 机械手 Modbus-RTU 控制类 (基于 pymodbus 3.5.1)
+"""
+
+import os
+import time
+from typing import List, Dict, Any # 引入 Any 来表示灵活的输入类型
+import numpy as np
+import logging
+from threading import Lock # 用于线程安全和总线仲裁
+
+# 导入 pymodbus 客户端
+from pymodbus.client import ModbusSerialClient
+from struct import error as StructError
+
+logging.basicConfig(
+ level=logging.INFO,
+ format="[%(asctime)s] %(levelname)-8s %(message)s",
+ datefmt="%H:%M:%S"
+)
+
+# ------------------------------------------------------------------
+# 读输入寄存器地址枚举（功能码 04，只读）- 保持原样
+# ------------------------------------------------------------------
+REG_RD_CURRENT_THUMB_PITCH = 0 # 大拇指弯曲角度（0-255，小=弯，大=伸）
+REG_RD_CURRENT_THUMB_YAW = 1 # 大拇指横摆角度（0-255，小=靠掌心，大=远离）
+REG_RD_CURRENT_INDEX_PITCH = 2 # 食指弯曲角度
+REG_RD_CURRENT_MIDDLE_PITCH = 3 # 中指弯曲角度
+REG_RD_CURRENT_RING_PITCH = 4 # 无名指弯曲角度
+REG_RD_CURRENT_LITTLE_PITCH = 5 # 小拇指弯曲角度
+REG_RD_CURRENT_THUMB_TORQUE = 6 # 大拇指弯曲转矩（0-255）
+REG_RD_CURRENT_THUMB_YAW_TORQUE = 7 # 大拇指横摆转矩
+REG_RD_CURRENT_INDEX_TORQUE = 8 # 食指转矩
+REG_RD_CURRENT_MIDDLE_TORQUE = 9 # 中指转矩
+REG_RD_CURRENT_RING_TORQUE = 10 # 无名指转矩
+REG_RD_CURRENT_LITTLE_TORQUE = 11 # 小拇指转矩
+REG_RD_CURRENT_THUMB_SPEED = 12 # 大拇指弯曲速度（0-255）
+REG_RD_CURRENT_THUMB_YAW_SPEED = 13 # 大拇指横摆速度
+REG_RD_CURRENT_INDEX_SPEED = 14 # 食指速度
+REG_RD_CURRENT_MIDDLE_SPEED = 15 # 中指速度
+REG_RD_CURRENT_RING_SPEED = 16 # 无名指速度
+REG_RD_CURRENT_LITTLE_SPEED = 17 # 小拇指速度
+REG_RD_THUMB_TEMP = 18 # 大拇指弯曲温度（0-70℃）
+REG_RD_THUMB_YAW_TEMP = 19 # 大拇指横摆温度
+REG_RD_INDEX_TEMP = 20 # 食指温度
+REG_RD_MIDDLE_TEMP = 21 # 中指温度
+REG_RD_RING_TEMP = 22 # 无名指温度
+REG_RD_LITTLE_TEMP = 23 # 小拇指温度
+REG_RD_THUMB_ERROR = 24 # 大拇指错误码
+REG_RD_THUMB_YAW_ERROR = 25 # 大拇指横摆错误码
+REG_RD_INDEX_ERROR = 26 # 食指错误码
+REG_RD_MIDDLE_ERROR = 27 # 中指错误码
+REG_RD_RING_ERROR = 28 # 无名指错误码
+REG_RD_LITTLE_ERROR = 29 # 小拇指错误码
+REG_RD_HAND_FREEDOM = 30 # 版本号（与机械手标签相同）
+REG_RD_HAND_VERSION = 31 # 手版本
+REG_RD_HAND_NUMBER = 32 # 手编号
+REG_RD_HAND_DIRECTION = 33 # 手方向（左/右）
+REG_RD_SOFTWARE_VERSION = 34 # 软件版本
+REG_RD_HARDWARE_VERSION = 35 # 硬件版本
+
+
+# ------------------------------------------------------------------
+# 写保持寄存器地址枚举（功能码 16，读写）- 保持原样
+# ------------------------------------------------------------------
+REG_WR_THUMB_PITCH = 0 # 大拇指弯曲角度（0-255）
+REG_WR_THUMB_YAW = 1 # 大拇指横摆角度
+REG_WR_INDEX_PITCH = 2 # 食指弯曲角度
+REG_WR_MIDDLE_PITCH = 3 # 中指弯曲角度
+REG_WR_RING_PITCH = 4 # 无名指弯曲角度
+REG_WR_LITTLE_PITCH = 5 # 小拇指弯曲角度
+REG_WR_THUMB_TORQUE = 6 # 大拇指弯曲转矩
+REG_WR_THUMB_YAW_TORQUE = 7 # 大拇指横摆转矩
+REG_WR_INDEX_TORQUE = 8 # 食指转矩
+REG_WR_MIDDLE_TORQUE = 9 # 中指转矩
+REG_WR_RING_TORQUE = 10 # 无名指转矩
+REG_WR_LITTLE_TORQUE = 11 # 小拇指转矩
+REG_WR_THUMB_SPEED = 12 # 大拇指弯曲速度
+REG_WR_THUMB_YAW_SPEED = 13 # 大拇指横摆速度
+REG_WR_INDEX_SPEED = 14 # 食指速度
+REG_WR_MIDDLE_SPEED = 15 # 中指速度
+REG_WR_RING_SPEED = 16 # 无名指速度
+REG_WR_LITTLE_SPEED = 17 # 小拇指速度
+
+
+class LinkerHandO6RS485:
+ """O6 机械手 Modbus-RTU 控制类，使用 pymodbus 3.5.1"""
+
+ TTL_TIMEOUT = 0.15 # 串口超时
+ FRAME_GAP = 0.030 # 30 ms
+
+ # KEYS for easy indexing
+ JOINT_KEYS = ["thumb_pitch", "thumb_yaw", "index_pitch",
+ "middle_pitch", "ring_pitch", "little_pitch"]
+
+ def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200):
+ self._id = hand_id
+ self._last_ts = 0.0 # 上一次帧结束时间
+ self._lock = Lock() # 总线访问锁
+
+ # 使用 pymodbus 3.x 客户端
+ self.cli = ModbusSerialClient(
+ port=modbus_port,
+ baudrate=baudrate,
+ bytesize=8,
+ parity="N",
+ stopbits=1,
+ timeout=self.TTL_TIMEOUT,
+ handle_local_echo=False
+ )
+
+ try:
+ logging.info(f"Connecting to Modbus RTU on {modbus_port}...")
+ self.connected = self.cli.connect()
+ if not self.connected:
+ raise ConnectionError(f"RS485 connect fail to {modbus_port}")
+ logging.info("Connection successful.")
+ except Exception as e:
+ logging.error(f"Initialization failed: {e}")
+ raise
+
+ # ----------------------------------------------------------
+ # 辅助方法
+ # ----------------------------------------------------------
+ def _bus_free(self):
+ """保证距离上一帧 ≥ 30 ms"""
+ with self._lock:
+ elapse = time.perf_counter() - self._last_ts
+ if elapse < self.FRAME_GAP:
+ time.sleep(self.FRAME_GAP - elapse)
+
+ def _execute_read(self, address: int, count: int) -> List[int]:
+ """执行 Modbus 读取操作 (功能码 04), 带总线仲裁。"""
+ self._bus_free()
+
+ rsp = self.cli.read_input_registers(
+ address=address,
+ count=count,
+ slave=self._id
+ )
+
+ self._last_ts = time.perf_counter()
+
+ if rsp.isError():
+ raise RuntimeError(f"Modbus Read Failed (Addr={address}, Count={count}): {rsp}")
+
+ # 确保返回的值是 Python 原生整数
+ return [int(reg) for reg in rsp.registers]
+
+ def _execute_write(self, address: int, values: List[int]):
+ """执行 Modbus 批量写入操作 (功能码 16), 带总线仲裁。"""
+ self._bus_free()
+
+ # values 必须是 Python 原生整数列表
+ rsp = self.cli.write_registers(
+ address=address,
+ values=values,
+ slave=self._id
+ )
+
+ self._last_ts = time.perf_counter()
+
+ if rsp.isError():
+ raise RuntimeError(f"Modbus Write Failed (Addr={address}, Values={values}): {rsp}")
+
+ # ----------------------------------------------------------
+ # 批量读取和数据封装（优化通信效率）
+ # ----------------------------------------------------------
+ def read_all_angles(self) -> List[int]:
+ return self._execute_read(REG_RD_CURRENT_THUMB_PITCH, 6)
+
+ def read_all_torques(self) -> List[int]:
+ return self._execute_read(REG_RD_CURRENT_THUMB_TORQUE, 6)
+
+ def read_all_speeds(self) -> List[int]:
+ return self._execute_read(REG_RD_CURRENT_THUMB_SPEED, 6)
+
+ def read_all_temperatures(self) -> List[int]:
+ return self._execute_read(REG_RD_THUMB_TEMP, 6)
+
+ def read_all_errors(self) -> List[int]:
+ return self._execute_read(REG_RD_THUMB_ERROR, 6)
+
+ def read_all_versions(self) -> List[int]:
+ return self._execute_read(REG_RD_HAND_FREEDOM, 6)
+
+
+ # ----------------------------------------------------------
+ # 只读属性（单个寄存器读取）
+ # ----------------------------------------------------------
+ def _read_reg(self, addr: int) -> int:
+ """读单个输入寄存器（功能码 04），带 30 ms 帧间隔"""
+ return self._execute_read(addr, 1)[0]
+
+ def get_thumb_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_PITCH)
+ def get_thumb_yaw(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_YAW)
+ def get_index_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_INDEX_PITCH)
+ def get_middle_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_MIDDLE_PITCH)
+ def get_ring_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_RING_PITCH)
+ def get_little_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_LITTLE_PITCH)
+
+ def get_thumb_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_TORQUE)
+ def get_thumb_yaw_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_YAW_TORQUE)
+ def get_index_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_INDEX_TORQUE)
+ def get_middle_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_MIDDLE_TORQUE)
+ def get_ring_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_RING_TORQUE)
+ def get_little_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_LITTLE_TORQUE)
+
+ def get_thumb_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_SPEED)
+ def get_thumb_yaw_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_YAW_SPEED)
+ def get_index_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_INDEX_SPEED)
+ def get_middle_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_MIDDLE_SPEED)
+ def get_ring_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_RING_SPEED)
+ def get_little_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_LITTLE_SPEED)
+
+ def get_thumb_temp(self) -> int: return self._read_reg(REG_RD_THUMB_TEMP)
+ def get_thumb_yaw_temp(self) -> int: return self._read_reg(REG_RD_THUMB_YAW_TEMP)
+ def get_index_temp(self) -> int: return self._read_reg(REG_RD_INDEX_TEMP)
+ def get_middle_temp(self) -> int: return self._read_reg(REG_RD_MIDDLE_TEMP)
+ def get_ring_temp(self) -> int: return self._read_reg(REG_RD_RING_TEMP)
+ def get_little_temp(self) -> int: return self._read_reg(REG_RD_LITTLE_TEMP)
+
+ def get_thumb_error(self) -> int: return self._read_reg(REG_RD_THUMB_ERROR)
+ def get_thumb_yaw_error(self) -> int: return self._read_reg(REG_RD_THUMB_YAW_ERROR)
+ def get_index_error(self) -> int: return self._read_reg(REG_RD_INDEX_ERROR)
+ def get_middle_error(self) -> int: return self._read_reg(REG_RD_MIDDLE_ERROR)
+ def get_ring_error(self) -> int: return self._read_reg(REG_RD_RING_ERROR)
+ def get_little_error(self) -> int: return self._read_reg(REG_RD_LITTLE_ERROR)
+
+ def get_hand_freedom(self) -> int: return self._read_reg(REG_RD_HAND_FREEDOM)
+ def get_hand_version(self) -> int: return self._read_reg(REG_RD_HAND_VERSION)
+ def get_hand_number(self) -> int: return self._read_reg(REG_RD_HAND_NUMBER)
+ def get_hand_direction(self) -> int: return self._read_reg(REG_RD_HAND_DIRECTION)
+ def get_software_version(self) -> int: return self._read_reg(REG_RD_SOFTWARE_VERSION)
+ def get_hardware_version(self) -> int: return self._read_reg(REG_RD_HARDWARE_VERSION)
+
+ # ----------------------------------------------------------
+ # 批量 Getter (使用 read_all_... 方法)
+ # ----------------------------------------------------------
+ def get_state(self) -> List[int]:
+ """获取手指电机状态 (角度)"""
+ return self.read_all_angles()
+
+ def get_torque(self) -> List[int]:
+ """获取当前扭矩"""
+ return self.read_all_torques()
+
+ def get_speed(self) -> List[int]:
+ """获取当前速度"""
+ return self.read_all_speeds()
+
+ def get_temperature(self) -> List[int]:
+ """获取当前电机温度"""
+ return self.read_all_temperatures()
+
+ def get_fault(self) -> List[int]:
+ """获取当前电机故障码"""
+ return self.read_all_errors()
+
+ def get_version(self) -> list:
+ """获取当前固件版本号"""
+ return self.read_all_versions()
+
+
+ # ----------------------------------------------------------
+ # 写保持寄存器 (单个寄存器写入)
+ # ----------------------------------------------------------
+ def _write_reg(self, addr: int, value: int):
+ """写单个保持寄存器（功能码 16），带 30 ms 帧间隔"""
+ if not 0 <= value <= 255:
+ raise ValueError("value must be 0-255")
+
+ # 确保 value 是 Python 原生 int
+ self._execute_write(addr, [int(value)])
+
+ def _write_regs(self, addr: int, values: List[int]):
+ """写多个保持寄存器（功能码 16），带 30 ms 帧间隔"""
+ # 此时 values 应该已经是经过 is_valid_6xuint8 验证并转换的 Python int 列表
+ if not all(0 <= v <= 255 for v in values):
+ # 这行理论上不应触发，因为上层调用已校验
+ raise ValueError("All values must be 0-255")
+ self._execute_write(addr, values)
+
+
+ def set_thumb_pitch(self, v: int): self._write_reg(REG_WR_THUMB_PITCH, v)
+ def set_thumb_yaw(self, v: int): self._write_reg(REG_WR_THUMB_YAW, v)
+ def set_index_pitch(self, v: int): self._write_reg(REG_WR_INDEX_PITCH, v)
+ def set_middle_pitch(self, v: int): self._write_reg(REG_WR_MIDDLE_PITCH, v)
+ def set_ring_pitch(self, v: int): self._write_reg(REG_WR_RING_PITCH, v)
+ def set_little_pitch(self, v: int): self._write_reg(REG_WR_LITTLE_PITCH, v)
+
+ def set_thumb_torque(self, v: int): self._write_reg(REG_WR_THUMB_TORQUE, v)
+ def set_thumb_yaw_torque(self, v: int): self._write_reg(REG_WR_THUMB_YAW_TORQUE, v)
+ def set_index_torque(self, v: int): self._write_reg(REG_WR_INDEX_TORQUE, v)
+ def set_middle_torque(self, v: int): self._write_reg(REG_WR_MIDDLE_TORQUE, v)
+ def set_ring_torque(self, v: int): self._write_reg(REG_WR_RING_TORQUE, v)
+ def set_little_torque(self, v: int): self._write_reg(REG_WR_LITTLE_TORQUE, v)
+
+ def set_thumb_speed(self, v: int): self._write_reg(REG_WR_THUMB_SPEED, v)
+ def set_thumb_yaw_speed(self, v: int): self._write_reg(REG_WR_THUMB_YAW_SPEED, v)
+ def set_index_speed(self, v: int): self._write_reg(REG_WR_INDEX_SPEED, v)
+ def set_middle_speed(self, v: int): self._write_reg(REG_WR_MIDDLE_SPEED, v)
+ def set_ring_speed(self, v: int): self._write_reg(REG_WR_RING_SPEED, v)
+ def set_little_speed(self, v: int): self._write_reg(REG_WR_LITTLE_SPEED, v)
+
+ # ----------------------------------------------------------
+ # 固定函数 (采用批量写入优化)
+ # ----------------------------------------------------------
+ def is_valid_6xuint8(self, lst: List[Any]) -> bool:
+ """
+ 验证6个0-255的整数列表。
+ 允许输入包含浮点数、NumPy整数等可转换为 int 的类型，并进行范围校验。
+ """
+ if not (isinstance(lst, list) and len(lst) == 6):
+ return False
+
+ try:
+ # 关键：尝试将所有元素转换为 Python 原生 int
+ int_values = [int(v) for v in lst]
+ except (ValueError, TypeError):
+ # 转换失败，列表中包含不可转换的元素
+ return False
+
+ # 校验转换后的整数列表是否在 0-255 范围内
+ return all(0 <= x <= 255 for x in int_values)
+
+ def set_joint_positions(self, joint_angles: List[Any] = None):
+ joint_angles = joint_angles or [0] * 6
+
+ if not self.is_valid_6xuint8(joint_angles):
+ logging.error(f"Invalid joint angles received: {joint_angles}")
+ raise ValueError("Joint angles must be a list of 6 values between 0 and 255 (convertible to int).")
+
+ # 强制转换为 Modbus 兼容的 Python 原生 int 列表
+ int_angles = [int(v) for v in joint_angles]
+
+ # 批量写入 6 个角度寄存器 (从 REG_WR_THUMB_PITCH 地址 0 开始, count=6)
+ self._write_regs(REG_WR_THUMB_PITCH, int_angles)
+
+ def set_speed(self, speed: List[Any] = None):
+ speed = speed or [200] * 6
+ if not self.is_valid_6xuint8(speed):
+ logging.error(f"Invalid speed values received: {speed}")
+ raise ValueError("Speed values must be a list of 6 values between 0 and 255 (convertible to int).")
+
+ int_speed = [int(v) for v in speed]
+ self._write_regs(REG_WR_THUMB_SPEED, int_speed)
+
+ def set_torque(self, torque: List[Any] = None):
+ torque = torque or [200] * 6
+ if not self.is_valid_6xuint8(torque):
+ logging.error(f"Invalid torque values received: {torque}")
+ raise ValueError("Torque values must be a list of 6 values between 0 and 255 (convertible to int).")
+
+ int_torque = [int(v) for v in torque]
+ self._write_regs(REG_WR_THUMB_TORQUE, int_torque)
+
+ # ... (其他固定函数保持不变) ...
+
+ def set_current(self, current: List[int] = None):
+ print("当前O6不支持设置电流", flush=True)
+ pass
+
+ def get_state_for_pub(self) -> list:
+ return self.get_state()
+
+ def get_current_status(self) -> list:
+ return self.get_state()
+
+ def get_joint_speed(self) -> list:
+ return self.get_speed()
+
+ def get_touch_type(self) -> list:
+ return -1
+
+ def get_normal_force(self) -> list:
+ return [-1] * 5
+
+ def get_tangential_force(self) -> list:
+ return [-1] * 5
+
+ def get_approach_inc(self) -> list:
+ return [-1] * 5
+
+ def get_touch(self) -> list:
+ return [-1] * 5
+
+ def get_thumb_matrix_touch(self,sleep_time=0):
+ return np.full((12, 6), -1)
+
+ def get_index_matrix_touch(self,sleep_time=0):
+ return np.full((12, 6), -1)
+
+ def get_middle_matrix_touch(self,sleep_time=0):
+ return np.full((12, 6), -1)
+
+ def get_ring_matrix_touch(self,sleep_time=0):
+ return np.full((12, 6), -1)
+
+ def get_little_matrix_touch(self,sleep_time=0):
+ return np.full((12, 6), -1)
+
+ def get_matrix_touch(self) -> list:
+ thumb_matrix = np.full((12, 6), -1)
+ index_matrix = np.full((12, 6), -1)
+ middle_matrix = np.full((12, 6), -1)
+ ring_matrix = np.full((12, 6), -1)
+ little_matrix = np.full((12, 6), -1)
+ return thumb_matrix , index_matrix , middle_matrix , ring_matrix , little_matrix
+
+ def get_serial_number(self):
+ return [0] * 6
+
+ def get_matrix_touch_v2(self) -> list:
+ return self.get_matrix_touch()
+
+ def get_finger_order(self):
+ return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
+
+ # ----------------------------------------------------------
+ # 上下文管理
+ # ----------------------------------------------------------
+ def close(self):
+ if hasattr(self, 'connected') and self.connected:
+ self.cli.close()
+ self.connected = False
+ logging.info("Modbus connection closed.")
+
+ def __enter__(self):
+ return self
+
+ def __exit__(self, exc_type, exc_val, exc_tb):
+ self.close()
+
+ # ----------------------------------------------------------
+ # 便捷函数
+ # ----------------------------------------------------------
+ def set_all_fingers(self, pitch: int):
+ """同时设置五指弯曲角度（0-255），使用批量写入"""
+ # 允许传入 float/numpy int 等可转换为 int 的类型
+ try:
+ pitch_int = int(pitch)
+ except (ValueError, TypeError):
+ raise ValueError("Pitch value must be a number convertible to int (0-255)")
+
+ if not 0 <= pitch_int <= 255:
+ raise ValueError("Pitch value must be 0-255")
+
+ # 批量设置所有 6 个关节的角度
+ self.set_joint_positions([pitch_int] * 6)
+
+ def relax(self):
+ """全部手指伸直（255）"""
+ self.set_all_fingers(255)
+
+ def fist(self):
+ """全部手指弯曲（0）"""
+ self.set_all_fingers(0)
+
+ def dump_status(self):
+ """打印当前所有可读状态 (使用批量读取优化)"""
+ print("--------- O6 Hand Status ---------")
+
+ angles = self.get_state()
+ temps = self.get_temperature()
+ errors = self.get_fault()
+ versions = self.get_version()
+
+ print(f"Joint Angles: {angles}")
+ print(f"Temperature: {temps}℃")
+ print(f"Error Codes: {errors}")
+ print(f"Versions: {versions}")
+ print("----------------------------------")
+
+# ------------------------------------------------------------------
+# 命令行快速测试
+# ------------------------------------------------------------------
+if __name__ == "__main__":
+ import argparse
+
+ # 假设默认站号是 0x27 (39)
+ DEFAULT_HAND_ID = 0x27
+
+ parser = argparse.ArgumentParser(description="O6 Hand Modbus tester (using pymodbus 3.5.1)")
+ parser.add_argument("-p", "--port", required=True, help="串口, 如 /dev/ttyUSB0")
+ parser.add_argument("-l", "--left", action="store_const", const=0x28, default=DEFAULT_HAND_ID, dest='hand_id', help="左手 (0x28)，默认右手 (0x27)")
+
+ args = parser.parse_args()
+
+ try:
+ # 使用 with 语句确保连接关闭，这是 pymodbus 的推荐用法
+ with LinkerHandO6RS485(hand_id=args.hand_id, modbus_port=args.port, baudrate=115200) as hand:
+ hand.dump_status()
+ print("执行 relax → 伸直")
+ hand.relax()
+ time.sleep(1)
+ print("执行 fist → 握拳")
+ hand.fist()
+ time.sleep(1)
+ hand.relax()
+ print("演示完成")
+
+ except ConnectionError as e:
+ print(f"连接错误: {e}")
+ except RuntimeError as e:
+ print(f"Modbus 运行时错误: {e}")
+ except StructError as e:
+ print(f"数据结构错误 (请检查输入数据类型是否为原生int): {e}")
+ except Exception as e:
+ print(f"发生其他错误: {e}")
+
+
+==========================================================================================================================
+
+
+#!/usr/bin/env python3
+"""
+O6 机械手 Modbus-RTU 控制类 (基于 pymodbus 3.5.1)
+"""
+
+import os
+import time
+from typing import List, Dict, Any # 引入 Any 来表示灵活的输入类型
+import numpy as np
+import logging
+from threading import Lock # 用于线程安全和总线仲裁
+
+# 导入 pymodbus 客户端
+from pymodbus.client import ModbusSerialClient
+from struct import error as StructError
+
+logging.basicConfig(
+ level=logging.INFO,
+ format="[%(asctime)s] %(levelname)-8s %(message)s",
+ datefmt="%H:%M:%S"
+)
+
+# ------------------------------------------------------------------
+# 读输入寄存器地址枚举（功能码 04，只读）- 按照 O6 协议文档定义
+# ------------------------------------------------------------------
+REG_RD_CURRENT_THUMB_PITCH = 0 # 大拇指弯曲角度（0-255，小=弯，大=伸）
+REG_RD_CURRENT_THUMB_YAW = 1 # 大拇指横摆角度（0-255，小=靠掌心，大=远离）
+REG_RD_CURRENT_INDEX_PITCH = 2 # 食指弯曲角度
+REG_RD_CURRENT_MIDDLE_PITCH = 3 # 中指弯曲角度
+REG_RD_CURRENT_RING_PITCH = 4 # 无名指弯曲角度
+REG_RD_CURRENT_LITTLE_PITCH = 5 # 小拇指弯曲角度
+REG_RD_CURRENT_THUMB_TORQUE = 6 # 大拇指弯曲转矩（0-255）
+REG_RD_CURRENT_THUMB_YAW_TORQUE = 7 # 大拇指横摆转矩
+REG_RD_CURRENT_INDEX_TORQUE = 8 # 食指转矩
+REG_RD_CURRENT_MIDDLE_TORQUE = 9 # 中指转矩
+REG_RD_CURRENT_RING_TORQUE = 10 # 无名指转矩
+REG_RD_CURRENT_LITTLE_TORQUE = 11 # 小拇指转矩
+REG_RD_CURRENT_THUMB_SPEED = 12 # 大拇指弯曲速度（0-255）
+REG_RD_CURRENT_THUMB_YAW_SPEED = 13 # 大拇指横摆速度
+REG_RD_CURRENT_INDEX_SPEED = 14 # 食指速度
+REG_RD_CURRENT_MIDDLE_SPEED = 15 # 中指速度
+REG_RD_CURRENT_RING_SPEED = 16 # 无名指速度
+REG_RD_CURRENT_LITTLE_SPEED = 17 # 小拇指速度
+REG_RD_THUMB_TEMP = 18 # 大拇指弯曲温度（0-70℃）
+REG_RD_THUMB_YAW_TEMP = 19 # 大拇指横摆温度
+REG_RD_INDEX_TEMP = 20 # 食指温度
+REG_RD_MIDDLE_TEMP = 21 # 中指温度
+REG_RD_RING_TEMP = 22 # 无名指温度
+REG_RD_LITTLE_TEMP = 23 # 小拇指温度
+REG_RD_THUMB_ERROR = 24 # 大拇指错误码
+REG_RD_THUMB_YAW_ERROR = 25 # 大拇指横摆错误码
+REG_RD_INDEX_ERROR = 26 # 食指错误码
+REG_RD_MIDDLE_ERROR = 27 # 中指错误码
+REG_RD_RING_ERROR = 28 # 无名指错误码
+REG_RD_LITTLE_ERROR = 29 # 小拇指错误码
+
+# 版本号/设备编号寄存器（地址 30-44，共15个寄存器）
+REG_RD_HAND_FREEDOM = 30 # Hand_freedom - 设备编号 / 自由度（与机械手上标签相同）
+REG_RD_HAND_VERSION = 31 # hand_version - 手版本
+REG_RD_HAND_NUMBER_HIGH = 32 # hand_number_高位 - 设备编号（高字节）
+REG_RD_HAND_NUMBER_MID = 33 # hand_number_中位 - 设备编号（中字节）
+REG_RD_HAND_NUMBER_LOW = 34 # hand_number_低位 - 设备编号（低字节）
+REG_RD_HAND_DIRECTION = 35 # hand_direction - 手方向（左/右）
+REG_RD_HARDWARE_VERSION_HIGH = 36 # hardware_version_高位 - 硬件版本（高字节）
+REG_RD_HARDWARE_VERSION_MID = 37 # hardware_version_中位 - 硬件版本（中字节）
+REG_RD_HARDWARE_VERSION_LOW = 38 # hardware_version_低位 - 硬件版本（低字节）
+REG_RD_SOFTWARE_VERSION_HIGH = 39 # software_version_高位 - 软件版本（高字节）
+REG_RD_SOFTWARE_VERSION_MID = 40 # software_version_中位 - 软件版本（中字节）
+REG_RD_SOFTWARE_VERSION_LOW = 41 # software_version_低位 - 软件版本（低字节）
+REG_RD_MECHANICAL_VERSION_HIGH = 42 # mechanical_version_高位 - 机械版本（高字节）
+REG_RD_MECHANICAL_VERSION_MID = 43 # mechanical_version_中位 - 机械版本（中字节）
+REG_RD_MECHANICAL_VERSION_LOW = 44 # mechanical_version_低位 - 机械版本（低字节）
+
+# 力传感器寄存器（地址 45-87+，动态范围）
+REG_RD_PRESSURE_SENSING_ID = 45 # Pressure_Sensing_ID - 压力传感器ID (0-5)
+REG_RD_PRESSURE_SENSING_SPEC = 46 # Pressure_Sensing_Specifications - 传感器数据规格
+
+
+# ------------------------------------------------------------------
+# 写保持寄存器地址枚举（功能码 16，读写）- 保持原样
+# ------------------------------------------------------------------
+REG_WR_THUMB_PITCH = 0 # 大拇指弯曲角度（0-255）
+REG_WR_THUMB_YAW = 1 # 大拇指横摆角度
+REG_WR_INDEX_PITCH = 2 # 食指弯曲角度
+REG_WR_MIDDLE_PITCH = 3 # 中指弯曲角度
+REG_WR_RING_PITCH = 4 # 无名指弯曲角度
+REG_WR_LITTLE_PITCH = 5 # 小拇指弯曲角度
+REG_WR_THUMB_TORQUE = 6 # 大拇指弯曲转矩
+REG_WR_THUMB_YAW_TORQUE = 7 # 大拇指横摆转矩
+REG_WR_INDEX_TORQUE = 8 # 食指转矩
+REG_WR_MIDDLE_TORQUE = 9 # 中指转矩
+REG_WR_RING_TORQUE = 10 # 无名指转矩
+REG_WR_LITTLE_TORQUE = 11 # 小拇指转矩
+REG_WR_THUMB_SPEED = 12 # 大拇指弯曲速度
+REG_WR_THUMB_YAW_SPEED = 13 # 大拇指横摆速度
+REG_WR_INDEX_SPEED = 14 # 食指速度
+REG_WR_MIDDLE_SPEED = 15 # 中指速度
+REG_WR_RING_SPEED = 16 # 无名指速度
+REG_WR_LITTLE_SPEED = 17 # 小拇指速度
+
+
+class LinkerHandO6RS485:
+ """O6 机械手 Modbus-RTU 控制类，使用 pymodbus 3.5.1"""
+
+ TTL_TIMEOUT = 0.15 # 串口超时
+ FRAME_GAP = 0.030 # 30 ms
+
+ # KEYS for easy indexing
+ JOINT_KEYS = ["thumb_pitch", "thumb_yaw", "index_pitch",
+ "middle_pitch", "ring_pitch", "little_pitch"]
+
+ def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200):
+ self._id = hand_id
+ self._last_ts = 0.0 # 上一次帧结束时间
+ self._lock = Lock() # 总线访问锁
+
+ # 使用 pymodbus 3.x 客户端
+ self.cli = ModbusSerialClient(
+ port=modbus_port,
+ baudrate=baudrate,
+ bytesize=8,
+ parity="N",
+ stopbits=1,
+ timeout=self.TTL_TIMEOUT,
+ handle_local_echo=False
+ )
+
+ try:
+ logging.info(f"Connecting to Modbus RTU on {modbus_port}...")
+ self.connected = self.cli.connect()
+ if not self.connected:
+ raise ConnectionError(f"RS485 connect fail to {modbus_port}")
+ logging.info("Connection successful.")
+ except Exception as e:
+ logging.error(f"Initialization failed: {e}")
+ raise
+
+ # ----------------------------------------------------------
+ # 辅助方法
+ # ----------------------------------------------------------
+ def _bus_free(self):
+ """保证距离上一帧 ≥ 30 ms"""
+ with self._lock:
+ elapse = time.perf_counter() - self._last_ts
+ if elapse < self.FRAME_GAP:
+ time.sleep(self.FRAME_GAP - elapse)
+
+ def _execute_read(self, address: int, count: int) -> List[int]:
+ """执行 Modbus 读取操作 (功能码 04), 带总线仲裁。"""
+ self._bus_free()
+
+ rsp = self.cli.read_input_registers(
+ address=address,
+ count=count,
+ slave=self._id
+ )
+
+ self._last_ts = time.perf_counter()
+
+ if rsp.isError():
+ raise RuntimeError(f"Modbus Read Failed (Addr={address}, Count={count}): {rsp}")
+
+ # 确保返回的值是 Python 原生整数
+ return [int(reg) for reg in rsp.registers]
+
+ def _execute_write(self, address: int, values: List[int]):
+ """执行 Modbus 批量写入操作 (功能码 16), 带总线仲裁。"""
+ self._bus_free()
+
+ # values 必须是 Python 原生整数列表
+ rsp = self.cli.write_registers(
+ address=address,
+ values=values,
+ slave=self._id
+ )
+
+ self._last_ts = time.perf_counter()
+
+ if rsp.isError():
+ raise RuntimeError(f"Modbus Write Failed (Addr={address}, Values={values}): {rsp}")
+
+ # ----------------------------------------------------------
+ # 批量读取和数据封装（优化通信效率）
+ # ----------------------------------------------------------
+ def read_all_angles(self) -> List[int]:
+ return self._execute_read(REG_RD_CURRENT_THUMB_PITCH, 6)
+
+ def read_all_torques(self) -> List[int]:
+ return self._execute_read(REG_RD_CURRENT_THUMB_TORQUE, 6)
+
+ def read_all_speeds(self) -> List[int]:
+ return self._execute_read(REG_RD_CURRENT_THUMB_SPEED, 6)
+
+ def read_all_temperatures(self) -> List[int]:
+ return self._execute_read(REG_RD_THUMB_TEMP, 6)
+
+ def read_all_errors(self) -> List[int]:
+ return self._execute_read(REG_RD_THUMB_ERROR, 6)
+
+ # ----------------------------------------------------------
+ # 版本号/设备编号读取（按照协议文档：地址30-44，共15个寄存器）
+ # ----------------------------------------------------------
+ def read_all_versions(self) -> str:
+ """一次性读取全部15个寄存器 (地址30-44)，返回以 '.' 连接的字符串。
+
+ 按 O6 协议文档的版本号格式返回：
+ hand_freedom.hand_version.hand_number_high.hand_number_mid.hand_number_low
+ .hand_direction.hardware_ver_hardware_ver_m.hardware_ver_l
+ .software_ver_h.software_ver_m.software_ver_l
+ .mechanical_ver_h.mechanical_ver_m.mechanical_ver_l
+
+ 例如: "6.1.001.002.003.0.1.2.3.4.5.6.7.8.9"
+ """
+ raw = self._execute_read(REG_RD_HAND_FREEDOM, 15)
+ return ".".join(str(v) for v in raw)
+
+ # ----------------------------------------------------------
+ # 基于 read_all_versions() 的设备编号解析方法
+ # ----------------------------------------------------------
+ def _parse_versions(self):
+ """解析 read_all_versions() 返回的字符串为字典"""
+ parts = self.read_all_versions().split(".")
+ if len(parts) < 15:
+ return {}
+ return {
+ "hand_freedom": int(parts[0]),
+ "hand_version": int(parts[1]),
+ "hand_number_high": int(parts[2]),
+ "hand_number_mid": int(parts[3]),
+ "hand_number_low": int(parts[4]),
+ "hand_direction": int(parts[5]),
+ "hw_ver_high": int(parts[6]),
+ "hw_ver_mid": int(parts[7]),
+ "hw_ver_low": int(parts[8]),
+ "sw_ver_high": int(parts[9]),
+ "sw_ver_mid": int(parts[10]),
+ "sw_ver_low": int(parts[11]),
+ "mech_ver_high": int(parts[12]),
+ "mech_ver_mid": int(parts[13]),
+ "mech_ver_low": int(parts[14]),
+ }
+
+ def get_device_number(self) -> str:
+ """获取设备编号（与机械手上标签相同）。格式：高位+中位+低位 拼接的字符串。"""
+ v = self._parse_versions()
+ if not v:
+ return "0"
+ high, mid, low = v["hand_number_high"], v["hand_number_mid"], v["hand_number_low"]
+ # 将每个字节格式化为无前导零的整数（与标签显示一致）
+ return f"{high}{mid}{low}"
+
+ def get_device_number_value(self) -> int:
+ """获取设备编号数值"""
+ v = self._parse_versions()
+ if not v:
+ return 0
+ high, mid, low = v["hand_number_high"], v["hand_number_mid"], v["hand_number_low"]
+ return high * 65536 + mid * 256 + low
+
+ def get_hardware_version(self) -> str:
+ """获取硬件版本号。格式：高.中.低"""
+ v = self._parse_versions()
+ if not v:
+ return "0.0.0"
+ return f"{v['hw_ver_high']}.{v['hw_ver_mid']}.{v['hw_ver_low']}"
+
+ def get_software_version(self) -> str:
+ """获取软件版本号。格式：高.中.低"""
+ v = self._parse_versions()
+ if not v:
+ return "0.0.0"
+ return f"{v['sw_ver_high']}.{v['sw_ver_mid']}.{v['sw_ver_low']}"
+
+ def get_mechanical_version(self) -> str:
+ """获取机械版本号。格式：高.中.低"""
+ v = self._parse_versions()
+ if not v:
+ return "0.0.0"
+ return f"{v['mech_ver_high']}.{v['mech_ver_mid']}.{v['mech_ver_low']}"
+
+ def get_hand_freedom(self) -> int:
+ """获取自由度（与机械手上标签相同）"""
+ v = self._parse_versions()
+ return v.get("hand_freedom", 0)
+
+ def get_hand_version_raw(self) -> int:
+ """获取手版本原始值"""
+ v = self._parse_versions()
+ return v.get("hand_version", 0)
+
+ def get_hand_direction(self) -> str:
+ """获取手方向，转换为字符：76→'L', 82→'R'"""
+ v = self._parse_versions()
+ val = v.get("hand_direction", 0)
+ if val in (76, 82):
+ return chr(val)
+ # fallback: 直接转为字符（如果值在可打印范围内）
+ return chr(val) if 32 < val < 128 else f'Unknown({val})'
+
+
+ # ----------------------------------------------------------
+ # 只读属性（单个寄存器读取）
+ # ----------------------------------------------------------
+ def _read_reg(self, addr: int) -> int:
+ """读单个输入寄存器（功能码 04），带 30 ms 帧间隔"""
+ return self._execute_read(addr, 1)[0]
+
+ def get_thumb_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_PITCH)
+ def get_thumb_yaw(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_YAW)
+ def get_index_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_INDEX_PITCH)
+ def get_middle_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_MIDDLE_PITCH)
+ def get_ring_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_RING_PITCH)
+ def get_little_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_LITTLE_PITCH)
+
+ def get_thumb_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_TORQUE)
+ def get_thumb_yaw_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_YAW_TORQUE)
+ def get_index_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_INDEX_TORQUE)
+ def get_middle_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_MIDDLE_TORQUE)
+ def get_ring_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_RING_TORQUE)
+ def get_little_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_LITTLE_TORQUE)
+
+ def get_thumb_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_SPEED)
+ def get_thumb_yaw_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_YAW_SPEED)
+ def get_index_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_INDEX_SPEED)
+ def get_middle_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_MIDDLE_SPEED)
+ def get_ring_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_RING_SPEED)
+ def get_little_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_LITTLE_SPEED)
+
+ def get_thumb_temp(self) -> int: return self._read_reg(REG_RD_THUMB_TEMP)
+ def get_thumb_yaw_temp(self) -> int: return self._read_reg(REG_RD_THUMB_YAW_TEMP)
+ def get_index_temp(self) -> int: return self._read_reg(REG_RD_INDEX_TEMP)
+ def get_middle_temp(self) -> int: return self._read_reg(REG_RD_MIDDLE_TEMP)
+ def get_ring_temp(self) -> int: return self._read_reg(REG_RD_RING_TEMP)
+ def get_little_temp(self) -> int: return self._read_reg(REG_RD_LITTLE_TEMP)
+
+ def get_thumb_error(self) -> int: return self._read_reg(REG_RD_THUMB_ERROR)
+ def get_thumb_yaw_error(self) -> int: return self._read_reg(REG_RD_THUMB_YAW_ERROR)
+ def get_index_error(self) -> int: return self._read_reg(REG_RD_INDEX_ERROR)
+ def get_middle_error(self) -> int: return self._read_reg(REG_RD_MIDDLE_ERROR)
+ def get_ring_error(self) -> int: return self._read_reg(REG_RD_RING_ERROR)
+ def get_little_error(self) -> int: return self._read_reg(REG_RD_LITTLE_ERROR)
+
+
+ # ----------------------------------------------------------
+ # 批量 Getter (使用 read_all_... 方法)
+ # ----------------------------------------------------------
+ def get_state(self) -> List[int]:
+ """获取手指电机状态 (角度)"""
+ return self.read_all_angles()
+
+ def get_torque(self) -> List[int]:
+ """获取当前扭矩"""
+ return self.read_all_torques()
+
+ def get_speed(self) -> List[int]:
+ """获取当前速度"""
+ return self.read_all_speeds()
+
+ def get_temperature(self) -> List[int]:
+ """获取当前电机温度"""
+ return self.read_all_temperatures()
+
+ def get_fault(self) -> List[int]:
+ """获取当前电机故障码"""
+ return self.read_all_errors()
+
+ def get_version(self) -> str:
+ """获取当前固件版本号（已转换为字符串格式）"""
+ return self.read_all_versions()
+
+
+ # ----------------------------------------------------------
+ # 写保持寄存器 (单个寄存器写入)
+ # ----------------------------------------------------------
+ def _write_reg(self, addr: int, value: int):
+ """写单个保持寄存器（功能码 16），带 30 ms 帧间隔"""
+ if not 0 <= value <= 255:
+ raise ValueError("value must be 0-255")
+
+ # 确保 value 是 Python 原生 int
+ self._execute_write(addr, [int(value)])
+
+ def _write_regs(self, addr: int, values: List[int]):
+ """写多个保持寄存器（功能码 16），带 30 ms 帧间隔"""
+ # 此时 values 应该已经是经过 is_valid_6xuint8 验证并转换的 Python int 列表
+ if not all(0 <= v <= 255 for v in values):
+ # 这行理论上不应触发，因为上层调用已校验
+ raise ValueError("All values must be 0-255")
+ self._execute_write(addr, values)
+
+
+ def set_thumb_pitch(self, v: int): self._write_reg(REG_WR_THUMB_PITCH, v)
+ def set_thumb_yaw(self, v: int): self._write_reg(REG_WR_THUMB_YAW, v)
+ def set_index_pitch(self, v: int): self._write_reg(REG_WR_INDEX_PITCH, v)
+ def set_middle_pitch(self, v: int): self._write_reg(REG_WR_MIDDLE_PITCH, v)
+ def set_ring_pitch(self, v: int): self._write_reg(REG_WR_RING_PITCH, v)
+ def set_little_pitch(self, v: int): self._write_reg(REG_WR_LITTLE_PITCH, v)
+
+ def set_thumb_torque(self, v: int): self._write_reg(REG_WR_THUMB_TORQUE, v)
+ def set_thumb_yaw_torque(self, v: int): self._write_reg(REG_WR_THUMB_YAW_TORQUE, v)
+ def set_index_torque(self, v: int): self._write_reg(REG_WR_INDEX_TORQUE, v)
+ def set_middle_torque(self, v: int): self._write_reg(REG_WR_MIDDLE_TORQUE, v)
+ def set_ring_torque(self, v: int): self._write_reg(REG_WR_RING_TORQUE, v)
+ def set_little_torque(self, v: int): self._write_reg(REG_WR_LITTLE_TORQUE, v)
+
+ def set_thumb_speed(self, v: int): self._write_reg(REG_WR_THUMB_SPEED, v)
+ def set_thumb_yaw_speed(self, v: int): self._write_reg(REG_WR_THUMB_YAW_SPEED, v)
+ def set_index_speed(self, v: int): self._write_reg(REG_WR_INDEX_SPEED, v)
+ def set_middle_speed(self, v: int): self._write_reg(REG_WR_MIDDLE_SPEED, v)
+ def set_ring_speed(self, v: int): self._write_reg(REG_WR_RING_SPEED, v)
+ def set_little_speed(self, v: int): self._write_reg(REG_WR_LITTLE_SPEED, v)
+
+ # ----------------------------------------------------------
+ # 固定函数 (采用批量写入优化)
+ # ----------------------------------------------------------
+ def is_valid_6xuint8(self, lst: List[Any]) -> bool:
+ """
+ 验证6个0-255的整数列表。
+ 允许输入包含浮点数、NumPy整数等可转换为 int 的类型，并进行范围校验。
+ """
+ if not (isinstance(lst, list) and len(lst) == 6):
+ return False
+
+ try:
+ # 关键：尝试将所有元素转换为 Python 原生 int
+ int_values = [int(v) for v in lst]
+ except (ValueError, TypeError):
+ # 转换失败，列表中包含不可转换的元素
+ return False
+
+ # 校验转换后的整数列表是否在 0-255 范围内
+ return all(0 <= x <= 255 for x in int_values)
+
+ def set_joint_positions(self, joint_angles: List[Any] = None):
+ joint_angles = joint_angles or [0] * 6
+
+ if not self.is_valid_6xuint8(joint_angles):
+ logging.error(f"Invalid joint angles received: {joint_angles}")
+ raise ValueError("Joint angles must be a list of 6 values between 0 and 255 (convertible to int).")
+
+ # 强制转换为 Modbus 兼容的 Python 原生 int 列表
+ int_angles = [int(v) for v in joint_angles]
+
+ # 批量写入 6 个角度寄存器 (从 REG_WR_THUMB_PITCH 地址 0 开始, count=6)
+ self._write_regs(REG_WR_THUMB_PITCH, int_angles)
+
+ def set_speed(self, speed: List[Any] = None):
+ speed = speed or [200] * 6
+ if not self.is_valid_6xuint8(speed):
+ logging.error(f"Invalid speed values received: {speed}")
+ raise ValueError("Speed values must be a list of 6 values between 0 and 255 (convertible to int).")
+
+ int_speed = [int(v) for v in speed]
+ self._write_regs(REG_WR_THUMB_SPEED, int_speed)
+
+ def set_torque(self, torque: List[Any] = None):
+ torque = torque or [200] * 6
+ if not self.is_valid_6xuint8(torque):
+ logging.error(f"Invalid torque values received: {torque}")
+ raise ValueError("Torque values must be a list of 6 values between 0 and 255 (convertible to int).")
+
+ int_torque = [int(v) for v in torque]
+ self._write_regs(REG_WR_THUMB_TORQUE, int_torque)
+
+ # ... (其他固定函数保持不变) ...
+
+ def set_current(self, current: List[int] = None):
+ print("当前O6不支持设置电流", flush=True)
+ pass
+
+ def get_state_for_pub(self) -> list:
+ return self.get_state()
+
+ def get_current_status(self) -> list:
+ return self.get_state()
+
+ def get_joint_speed(self) -> list:
+ return self.get_speed()
+
+ def get_touch_type(self) -> list:
+ return -1
+
+ def get_normal_force(self) -> list:
+ return [-1] * 5
+
+ def get_tangential_force(self) -> list:
+ return [-1] * 5
+
+ def get_approach_inc(self) -> list:
+ return [-1] * 5
+
+ def get_touch(self) -> list:
+ return [-1] * 5
+
+ def get_thumb_matrix_touch(self,sleep_time=0):
+ return np.full((12, 6), -1)
+
+ def get_index_matrix_touch(self,sleep_time=0):
+ return np.full((12, 6), -1)
+
+ def get_middle_matrix_touch(self,sleep_time=0):
+ return np.full((12, 6), -1)
+
+ def get_ring_matrix_touch(self,sleep_time=0):
+ return np.full((12, 6), -1)
+
+ def get_little_matrix_touch(self,sleep_time=0):
+ return np.full((12, 6), -1)
+
+ def get_matrix_touch(self) -> list:
+ thumb_matrix = np.full((12, 6), -1)
+ index_matrix = np.full((12, 6), -1)
+ middle_matrix = np.full((12, 6), -1)
+ ring_matrix = np.full((12, 6), -1)
+ little_matrix = np.full((12, 6), -1)
+ return thumb_matrix , index_matrix , middle_matrix , ring_matrix , little_matrix
+
+ def get_serial_number(self):
+
+ return "["+str(self.get_hand_freedom())+"-"+str(self.get_mechanical_version())+"-"+str(self.get_device_number())+"-"+str(self.get_hand_direction())+"]"
+
+ def get_matrix_touch_v2(self) -> list:
+ return self.get_matrix_touch()
+
+ def get_finger_order(self):
+ return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
+
+ def clear_faults(self):
+ pass
+
+ def close(self):
+ if hasattr(self, 'connected') and self.connected:
+ self.cli.close()
+ self.connected = False
+ logging.info("Modbus connection closed.")
+
+ def __enter__(self):
+ return self
+
+ def __exit__(self, exc_type, exc_val, exc_tb):
+ self.close()
+
+ # ----------------------------------------------------------
+ # 便捷函数
+ # ----------------------------------------------------------
+ def set_all_fingers(self, pitch: int):
+ """同时设置五指弯曲角度（0-255），使用批量写入"""
+ # 允许传入 float/numpy int 等可转换为 int 的类型
+ try:
+ pitch_int = int(pitch)
+ except (ValueError, TypeError):
+ raise ValueError("Pitch value must be a number convertible to int (0-255)")
+
+ if not 0 <= pitch_int <= 255:
+ raise ValueError("Pitch value must be 0-255")
+
+ # 批量设置所有 6 个关节的角度
+ self.set_joint_positions([pitch_int] * 6)
+
+ def relax(self):
+ """全部手指伸直（255）"""
+ self.set_all_fingers(255)
+
+ def fist(self):
+ """全部手指弯曲（0）"""
+ self.set_all_fingers(0)
+
+ def dump_status(self):
+ """打印当前所有可读状态 (使用批量读取优化)"""
+ print("--------- O6 Hand Status ---------")
+
+ angles = self.get_state()
+ temps = self.get_temperature()
+ errors = self.get_fault()
+
+ # 解析版本号字符串
+ v = self._parse_versions()
+
+ if v:
+ device_num_str = f"{v['hand_number_high']}{v['hand_number_mid']}{v['hand_number_low']}"
+ hw_ver = f"{v['hw_ver_high']}.{v['hw_ver_mid']}.{v['hw_ver_low']}"
+ sw_ver = f"{v['sw_ver_high']}.{v['sw_ver_mid']}.{v['sw_ver_low']}"
+ mech_ver = f"{v['mech_ver_high']}.{v['mech_ver_mid']}.{v['mech_ver_low']}"
+
+ print(f"Device Number: {device_num_str}")
+ print(f"HWSW Version: HW={hw_ver} SW={sw_ver}")
+ print(f"Mechanical Ver: {mech_ver}")
+ print(f"Hand Freedom: {v['hand_freedom']}")
+ print(f"Full Version: {self.read_all_versions()}")
+
+ print(f"Joint Angles: {angles}")
+ print(f"Temperature: {temps}℃")
+ print(f"Error Codes: {errors}")
+ print("----------------------------------")
+
+
+# ------------------------------------------------------------------
+# 命令行快速测试
+# ------------------------------------------------------------------
+if __name__ == "__main__":
+ import argparse
+
+ # 假设默认站号是 0x27 (39)
+ DEFAULT_HAND_ID = 0x27
+
+ parser = argparse.ArgumentParser(description="O6 Hand Modbus tester (using pymodbus 3.5.1)")
+ parser.add_argument("-p", "--port", required=True, help="串口, 如 /dev/ttyUSB0")
+ parser.add_argument("-l", "--left", action="store_const", const=0x28, default=DEFAULT_HAND_ID, dest='hand_id', help="左手 (0x28)，默认右手 (0x27)")
+
+ args = parser.parse_args()
+
+ try:
+ # 使用 with 语句确保连接关闭，这是 pymodbus 的推荐用法
+ with LinkerHandO6RS485(hand_id=args.hand_id, modbus_port=args.port, baudrate=115200) as hand:
+ hand.dump_status()
+
+ # 测试新增的设备编号读取方法
+ print("\n--- 设备信息 ---")
+ print(f"设备编号（字符串）: {hand.get_device_number()}")
+ print(f"设备编号（数值）: {hand.get_device_number_value()}")
+ print(f"硬件版本号: {hand.get_hardware_version()}")
+ print(f"软件版本号: {hand.get_software_version()}")
+ print(f"机械版本号: {hand.get_mechanical_version()}")
+
+ print("\n执行 relax → 伸直")
+ hand.relax()
+ time.sleep(1)
+ print("执行 fist → 握拳")
+ hand.fist()
+ time.sleep(1)
+ hand.relax()
+ print("演示完成")
+
+ except ConnectionError as e:
+ print(f"连接错误: {e}")
+ except RuntimeError as e:
+ print(f"Modbus 运行时错误: {e}")
+ except StructError as e:
+ print(f"数据结构错误 (请检查输入数据类型是否为原生int): {e}")
+ except Exception as e:
+ print(f"发生其他错误: {e}")
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/linker_hand_api.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/linker_hand_api.py
new file mode 100644
index 0000000..631a7c9
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/linker_hand_api.py
@@ -0,0 +1,355 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+import sys, os, time,threading
+sys.path.append(os.path.dirname(os.path.abspath(__file__)))
+from utils.mapping import *
+from utils.color_msg import ColorMsg
+from utils.load_write_yaml import LoadWriteYaml
+from utils.open_can import OpenCan
+
+class LinkerHandApi:
+ def __init__(self, hand_type="left", hand_joint="L10", modbus = "None",can="can0"): # Ubuntu:can0 win:PCAN_USBBUS1
+ self.last_position = []
+ self.yaml = LoadWriteYaml()
+ self.config = self.yaml.load_setting_yaml()
+ self.version = self.config["VERSION"]
+ self.can = can
+ ColorMsg(msg=f"Current SDK version: {self.version}", color="green")
+ self.hand_joint = hand_joint
+ self.hand_type = hand_type
+ self.is_palm_touch = -1 # 是否为全掌压力传感器
+ if self.hand_type == "left":
+ self.hand_id = 0x28 # Left hand
+ if self.hand_type == "right":
+ self.hand_id = 0x27 # Right hand
+ if self.hand_joint.upper() == "O6":
+ if modbus != "None":
+ from core.rs485.linker_hand_o6_rs485 import LinkerHandO6RS485
+ self.hand = LinkerHandO6RS485(hand_id=self.hand_id,modbus_port=modbus,baudrate=115200)
+ else:
+ from core.can.linker_hand_o6_can import LinkerHandO6Can
+ self.hand = LinkerHandO6Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml)
+ if self.hand_joint == "L6":
+ if modbus != "None":
+ from core.rs485.linker_hand_l6_rs485 import LinkerHandL6RS485
+ self.hand = LinkerHandL6RS485(hand_id=self.hand_id,modbus_port=modbus,baudrate=115200)
+ else:
+ from core.can.linker_hand_l6_can import LinkerHandL6Can
+ self.hand = LinkerHandL6Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml)
+ if self.hand_joint == "L7":
+ if modbus != "None":
+ from core.rs485.linker_hand_l7_rs485 import LinkerHandL7RS485
+ self.hand = LinkerHandL7RS485(hand_id=self.hand_id,modbus_port=modbus,baudrate=115200)
+ else:
+ from core.can.linker_hand_l7_can import LinkerHandL7Can
+ self.hand = LinkerHandL7Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml)
+ if self.hand_joint == "L10":
+ if modbus != "None":
+ from core.rs485.linker_hand_l10_rs485 import LinkerHandL10RS485
+ self.hand = LinkerHandL10RS485(hand_id=self.hand_id,modbus_port=modbus,baudrate=115200)
+ else:
+ from core.can.linker_hand_l10_can import LinkerHandL10Can
+ self.hand = LinkerHandL10Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml)
+ if self.hand_joint == "L20":
+ from core.can.linker_hand_l20_can import LinkerHandL20Can
+ self.hand = LinkerHandL20Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml)
+ if self.hand_joint == "G20":
+ from core.can.linker_hand_g20_can import LinkerHandG20Can
+ self.hand = LinkerHandG20Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml)
+ time.sleep(0.01)
+ self.is_palm_touch = self.hand.get_touch_sensor_type()
+ ColorMsg(msg=f"传感器类型:{self.is_palm_touch}")
+ if self.hand_joint == "L21":
+ from core.can.linker_hand_l21_can import LinkerHandL21Can
+ self.hand = LinkerHandL21Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml)
+ if self.hand_joint == "L25":
+ from core.can.linker_hand_l25_can import LinkerHandL25Can
+ self.hand = LinkerHandL25Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml)
+ # Open can0
+ if sys.platform == "linux" and modbus=="None":
+ self.open_can = OpenCan(load_yaml=self.yaml)
+ self.open_can.open_can(self.can)
+ self.is_can = self.open_can.is_can_up_sysfs(interface=self.can)
+ if not self.is_can:
+ ColorMsg(msg=f"{self.can} interface is not open", color="red")
+ sys.exit(1)
+ version = self.get_embedded_version()
+ self.serial_number = self.get_serial_number()
+ if version == None or len(version) == 0:
+ ColorMsg(msg="Warning: Hardware version number not recognized, it is recommended to terminate the program and re insert USB to CAN conversion", color="yellow")
+ else:
+ ColorMsg(msg=f"Embedded:{version}", color="green")
+ ColorMsg(msg=f"Linker Hand Serial Number: {self.serial_number}", color="green")
+
+
+ # Five-finger movement
+ def finger_move(self, pose=[]):
+ '''
+ Five-finger movement
+ @params: pose list L7 len(7) | L10 len(10) | L20 len(20) | L25 len(25) 0~255
+ '''
+
+ if len(pose) == 0:
+ return
+ pose = [int(v) for v in pose]
+ if any(not isinstance(x, (int, float)) or x < 0 or x > 255 for x in pose):
+ ColorMsg(msg=f"The numerical range cannot be less than 0 or greater than 255",color="red")
+ return
+ if (self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6") and len(pose) == 6:
+ self.hand.set_joint_positions(pose)
+ elif self.hand_joint == "L7" and len(pose) == 7:
+ self.hand.set_joint_positions(pose)
+ elif self.hand_joint == "L10" and len(pose) == 10:
+ self.hand.set_joint_positions(pose)
+ elif self.hand_joint == "L20" and len(pose) == 20:
+ self.hand.set_joint_positions(pose)
+ elif self.hand_joint == "G20" and len(pose) == 20:
+ self.hand.set_joint_positions(pose)
+ elif self.hand_joint == "L21" and len(pose) == 25:
+ self.hand.set_joint_positions(pose)
+ elif self.hand_joint == "L25" and len(pose) == 25:
+ self.hand.set_joint_positions(pose)
+ else:
+ ColorMsg(msg=f"Current LinkerHand is {self.hand_type}{self.hand_joint}, action sequence is {pose}, does not match", color="red")
+ self.last_position = pose
+
+ def _get_normal_force(self):
+ '''# Get normal force'''
+ self.hand.get_normal_force()
+
+ def _get_tangential_force(self):
+ '''# Get tangential force'''
+ self.hand.get_tangential_force()
+
+ def _get_tangential_force_dir(self):
+ '''# Get tangential force direction'''
+ self.hand.get_tangential_force_dir()
+
+ def _get_approach_inc(self):
+ '''# Get approach increment'''
+ self.hand.get_approach_inc()
+
+
+ def set_speed(self, speed=[100]*5):
+ '''# Set speed'''
+ has_non_int = any(not isinstance(x, (int, float)) or x < 0 or x > 255 for x in speed)
+ if has_non_int:
+ print("Set Speed The numerical range can only be positive integers or floating-point numbers between 0 and 255", flush=True)
+ return
+ if len(speed) < 5:
+ print("数据长度不够,至少5个元素", flush=True)
+ return
+ if self.hand_joint == "L7" and len(speed) < 7:
+ print("数据长度不够,至少7个元素", flush=True)
+ return
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} set speed to {speed}", color="green")
+ self.hand.set_speed(speed=speed)
+
+ def set_joint_speed(self, speed=[100]*5):
+ '''Set speed by topic'''
+ if len(speed) == 0:
+ return
+ if any(not isinstance(x, (int, float)) or x < 10 or x > 255 for x in speed):
+ ColorMsg(msg=f"The numerical range cannot be less than 10 or greater than 255",color="red")
+ return
+ self.hand.set_speed(speed=speed)
+
+ def set_torque(self, torque=[180] * 5):
+ '''Set maximum torque'''
+ has_non_int = any(not isinstance(x, (int, float)) or x < 0 or x > 255 for x in torque)
+ if has_non_int:
+ print("Set Torque The numerical range can only be positive integers or floating-point numbers between 0 and 255", flush=True)
+ return
+ if len(torque) < 5:
+ print("数据长度不够,至少5个元素", flush=True)
+ return
+ if self.hand_joint == "L7" and len(torque) < 7:
+ print("数据长度不够,至少7个元素", flush=True)
+ return
+ if (self.hand_joint == "L6" or self.hand_joint == "O6") and len(torque) != 6:
+ print("L6 or O6数据长度错误,至少6个元素", flush=True)
+ return
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} set maximum torque to {torque}", color="green")
+ return self.hand.set_torque(torque=torque)
+
+
+ def set_current(self, current=[250] * 5):
+ '''Set current L7/L10/L25 not supported'''
+ if any(not isinstance(x, (int, float)) or x < 0 or x > 255 for x in current):
+ print("Set Current The numerical range can only be positive integers or floating-point numbers between 0 and 255", flush=True)
+ return
+ if self.hand_joint == "L20":
+ return self.hand.set_current(current=current)
+ else:
+ pass
+
+ def get_embedded_version(self):
+ '''Get embedded version'''
+ return self.hand.get_version()
+
+ def get_serial_number(self):
+ '''Get serial number'''
+ try:
+ return self.hand.sn
+ except:
+ return self.hand.get_serial_number()
+
+ def get_current(self):
+ '''Get current'''
+ return self.hand.get_current()
+
+ def get_state(self):
+ '''Get current joint state'''
+ return self.hand.get_current_status()
+
+
+ def get_state_for_pub(self):
+ return self.hand.get_current_pub_status()
+
+ def get_speed(self):
+ '''Get speed'''
+ return self.hand.get_speed()
+
+
+ def get_joint_speed(self):
+ speed = []
+ if self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6":
+ return self.hand.get_speed()
+ elif self.hand_joint == "L7":
+ return self.hand.get_speed()
+ elif self.hand_joint == "L10":
+ speed = self.hand.get_speed()
+ return speed
+ elif self.hand_joint == "G20":
+ return self.hand.get_speed()
+ elif self.hand_joint == "L20":
+ speed = self.hand.get_speed()
+ return [255, speed[1], speed[2], speed[3], speed[4], 255, 255, 255, 255, 255, speed[0], 255, 255, 255, 255, 255, 255, 255, 255, 255]
+ elif self.hand_joint == "L21":
+ return self.hand.get_speed()
+ elif self.hand_joint == "L25":
+ return self.hand.get_speed()
+
+ def get_touch_type(self):
+ '''Get touch type'''
+ try:
+ return self.hand.touch_type
+ except:
+ return self.hand.get_touch_type()
+
+ def get_force(self):
+ '''Get normal force, tangential force, tangential force direction, approach sensing data'''
+ self._get_normal_force()
+ self._get_tangential_force()
+ self._get_tangential_force_dir()
+ self._get_approach_inc()
+ return self.hand.get_force()
+
+ def get_touch(self):
+ '''Get touch data'''
+ return self.hand.get_touch()
+
+ def get_matrix_touch(self):
+ return self.hand.get_matrix_touch()
+
+ def get_matrix_touch_v2(self):
+ return self.hand.get_matrix_touch_v2()
+
+
+ def get_thumb_matrix_touch(self,sleep_time=0):
+ if sleep_time > 0:
+ return self.hand.get_thumb_matrix_touch(sleep_time=sleep_time)
+ else:
+ return self.hand.get_thumb_matrix_touch()
+
+ def get_index_matrix_touch(self,sleep_time=0):
+ if sleep_time > 0:
+ return self.hand.get_index_matrix_touch(sleep_time=sleep_time)
+ else:
+ return self.hand.get_index_matrix_touch()
+
+ def get_middle_matrix_touch(self,sleep_time=0):
+ if sleep_time > 0:
+ return self.hand.get_middle_matrix_touch(sleep_time=sleep_time)
+ else:
+ return self.hand.get_middle_matrix_touch()
+
+ def get_ring_matrix_touch(self,sleep_time=0):
+ if sleep_time > 0:
+ return self.hand.get_ring_matrix_touch(sleep_time=sleep_time)
+ else:
+ return self.hand.get_ring_matrix_touch()
+
+ def get_little_matrix_touch(self,sleep_time=0):
+ if sleep_time > 0:
+ return self.hand.get_little_matrix_touch(sleep_time=sleep_time)
+ else:
+ return self.hand.get_little_matrix_touch()
+
+ def get_palm_matrix_touch(self,sleep_time=0):
+ if self.is_palm_touch == 5:
+ if sleep_time > 0:
+ return self.hand.get_palm_matrix_touch(sleep_time=sleep_time)
+ else:
+ return self.hand.get_palm_matrix_touch()
+
+ def get_torque(self):
+ '''Get current maximum torque'''
+ return self.hand.get_torque()
+
+ def get_temperature(self):
+ '''Get current motor temperature'''
+ return self.hand.get_temperature()
+
+ def get_fault(self):
+ '''Get motor fault code'''
+ return self.hand.get_fault()
+
+ def clear_faults(self):
+ '''Clear motor fault codes Not supported yet, currently only supports L20'''
+ self.hand.clear_faults()
+ return [0] * 5
+
+ def set_enable(self):
+ '''Set motor enable Only supports L25'''
+ if self.hand_joint == "L25":
+ self.hand.set_enable_mode()
+ else:
+ pass
+
+ def set_disable(self):
+ '''Set motor disable Only supports L25'''
+ if self.hand_joint == "L25":
+ self.hand.set_disability_mode()
+ else:
+ pass
+
+ def get_finger_order(self):
+ '''Get finger motor order'''
+ # if self.hand_joint == "L21" or self.hand_joint == "L25" or self.hand_joint == "G20":
+ # return self.hand.get_finger_order()
+ # else:
+ # return []
+ return self.hand.get_finger_order()
+
+ def range_to_arc_left(self, state, hand_joint):
+ return range_to_arc_left(left_range=state, hand_joint=hand_joint)
+
+ def range_to_arc_right(self, state, hand_joint):
+ return range_to_arc_right(right_range=state, hand_joint=hand_joint)
+
+ def arc_to_range_left(self,state,hand_joint):
+ return arc_to_range_left(hand_arc_l=state,hand_joint=hand_joint)
+
+ def arc_to_range_right(self,state,hand_joint):
+ return arc_to_range_right(right_arc=state,hand_joint=hand_joint)
+
+ def show_fun_table(self):
+ self.hand.show_fun_table()
+
+ def close_can(self):
+ if sys.platform == "linux" and modbus=="None":
+ self.open_can.close_can(can=self.can)
+
+if __name__ == "__main__":
+ hand = LinkerHandApi(hand_type="right", hand_joint="L10")
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/__init__.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/color_msg.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/color_msg.py
new file mode 100644
index 0000000..9ba06ac
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/color_msg.py
@@ -0,0 +1,27 @@
+#! /usr/bin/env python3
+
+import time
+
+class ColorMsg():
+ def __init__(self,msg: str,color: str = '', timestamp: bool = True) -> None:
+ self.msg = msg
+ self.color = color
+ self.timestamp = timestamp
+ self.colorMsg(msg=self.msg, color=self.color, timestamp=self.timestamp)
+
+ def colorMsg(self,msg: str, color: str = '', timestamp: bool = True):
+ str = ""
+ if timestamp:
+ str += time.strftime('%Y-%m-%d %H:%M:%S',
+ time.localtime(time.time())) + " "
+ if color == "red":
+ str += "\033[1;31;40m"
+ elif color == "green":
+ str += "\033[1;32;40m"
+ elif color == "yellow":
+ str += "\033[1;33;40m"
+ else:
+ print(str + msg, flush=True)
+ return
+ str += msg + "\033[0m"
+ print(str, flush=True)
\ No newline at end of file
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/init_linker_hand.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/init_linker_hand.py
new file mode 100644
index 0000000..6bfcae7
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/init_linker_hand.py
@@ -0,0 +1,81 @@
+'''
+Author: HJX
+Date: 2025-04-01 14:09:21
+LastEditors: Please set LastEditors
+LastEditTime: 2025-04-08 11:18:23
+FilePath: /Linker_Hand_SDK_ROS/src/linker_hand_sdk_ros/scripts/LinkerHand/utils/init_linker_hand.py
+Description:
+symbol_custom_string_obkorol_copyright:
+'''
+import yaml, os, sys
+sys.path.append(os.path.dirname(os.path.abspath(__file__)))
+from load_write_yaml import LoadWriteYaml
+
+class InitLinkerHand():
+ def __init__(self):
+ self.yaml = LoadWriteYaml()
+ self.setting = self.yaml.load_setting_yaml()
+
+ def current_hand(self):
+ '''
+ 初始化灵巧手
+ return: hand_joint str L7/L10/L20/L21/L25, hand_type str left or right
+ '''
+ # 左手是否配置
+ self.left_hand = None
+ self.left_hand_joint = None
+ self.left_hand_type = None
+ self.left_hand_force = None
+ self.left_hand_pose = None
+ self.left_hand_torque = [200, 200, 200, 200, 200]
+ self.left_hand_speed = [80, 200, 200, 200, 200]
+ # 右手是否配置
+ self.right_hand = None
+ self.right_hand_joint = None
+ self.right_hand_type = None
+ self.right_hand_force = None
+ self.right_hand_pose = None
+ self.right_hand_torque = [200, 200, 200, 200, 200]
+ self.right_hand_speed = [80, 200, 200, 200, 200]
+ if self.setting['LINKER_HAND']['LEFT_HAND']['EXISTS'] == True:
+ self.left_hand = True
+ self.left_hand_joint = self.setting['LINKER_HAND']['LEFT_HAND']['JOINT']
+ self.left_hand_type = "left"
+ self.left_hand_force = self.setting['LINKER_HAND']['LEFT_HAND']['TOUCH']
+ if self.left_hand_joint == "L7":
+ # The data length of L7 is 7, reinitialize here
+ self.left_hand_pose = [255, 200, 255, 255, 255, 255, 180]
+ self.left_hand_torque = [250, 250, 250, 250, 250, 250, 250]
+ self.left_hand_speed = [120, 180, 180, 180, 180, 180, 180]
+ elif self.left_hand_joint == "L10":
+ self.left_hand_pose = [255, 200, 255, 255, 255, 255, 180, 180, 180, 41]
+ elif self.left_hand_joint == "L20":
+ self.left_hand_pose = [255,255,255,255,255,255,10,100,180,240,245,255,255,255,255,255,255,255,255,255]
+ elif self.left_hand_joint == "L21":
+ self.left_hand_pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+ elif self.left_hand_joint == "L25":
+ self.left_hand_pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+ # 判断右手是否配置
+ if self.setting['LINKER_HAND']['RIGHT_HAND']['EXISTS'] == True:
+ self.right_hand = True
+ self.right_hand_joint = self.setting['LINKER_HAND']['RIGHT_HAND']['JOINT']
+ self.right_hand_type = "right"
+ self.right_hand_force = self.setting['LINKER_HAND']['RIGHT_HAND']['TOUCH']
+ if self.right_hand_joint == "L7":
+ # The data length of L7 is 7, reinitialize here
+ self.right_hand_pose = [255, 200, 255, 255, 255, 255, 180]
+ self.right_hand_torque = [250, 250, 250, 250, 250, 250, 250]
+ self.right_hand_speed = [120, 250, 250, 250, 250, 250, 250]
+ elif self.right_hand_joint == "L10":
+ self.right_hand_pose = [255, 200, 255, 255, 255, 255, 180, 180, 180, 41]
+ elif self.right_hand_joint == "L20":
+ self.right_hand_pose = [255,255,255,255,255,255,10,100,180,240,245,255,255,255,255,255,255,255,255,255]
+ elif self.right_hand_joint == "L21":
+ self.right_hand_pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+ elif self.right_hand_joint == "L25":
+ self.right_hand_pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+
+
+ return self.left_hand ,self.left_hand_joint ,self.left_hand_type ,self.left_hand_force,self.left_hand_pose, self.left_hand_torque, self.left_hand_speed ,self.right_hand ,self.right_hand_joint ,self.right_hand_type ,self.right_hand_force,self.right_hand_pose, self.right_hand_torque, self.right_hand_speed,self.setting
+
+
\ No newline at end of file
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/load_write_yaml.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/load_write_yaml.py
new file mode 100644
index 0000000..160674c
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/load_write_yaml.py
@@ -0,0 +1,101 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+'''
+Author: HJX
+Date: 2025-04-01 14:09:21
+LastEditors: Please set LastEditors
+LastEditTime: 2025-04-11 10:19:01
+FilePath: /LinkerHand_Python_SDK/LinkerHand/utils/load_write_yaml.py
+Description:
+symbol_custom_string_obkorol_copyright:
+'''
+import yaml, os, sys
+class LoadWriteYaml():
+ def __init__(self):
+ # 由于是API形式，这里要给配置文件目录绝对路径
+ #yaml_path = "/home/linkerhand/ROS2/linker_hand_ros2_sdk/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand"
+ yaml_path = os.path.dirname(os.path.abspath(__file__)) + "/../../LinkerHand"
+ self.setting_path = yaml_path+"/config/setting.yaml"
+ self.l7_positions = yaml_path+"/config/L7_positions.yaml"
+ self.l10_positions = yaml_path+"/config/L10_positions.yaml"
+ self.l20_positions = yaml_path+"/config/L20_positions.yaml"
+ self.l21_positions = yaml_path+"/config/L21_positions.yaml"
+ self.l25_positions = yaml_path+"/config/L25_positions.yaml"
+
+
+ def load_setting_yaml(self):
+ try:
+ with open(self.setting_path, 'r', encoding='utf-8') as file:
+ setting = yaml.safe_load(file)
+ self.sdk_version = setting["VERSION"]
+ self.left_hand_exists = setting['LINKER_HAND']['LEFT_HAND']['EXISTS']
+ self.left_hand_names = setting['LINKER_HAND']['LEFT_HAND']['NAME']
+ self.left_hand_joint = setting['LINKER_HAND']['LEFT_HAND']['JOINT']
+ self.left_hand_force = setting['LINKER_HAND']['LEFT_HAND']['TOUCH']
+ self.right_hand_exists = setting['LINKER_HAND']['RIGHT_HAND']['EXISTS']
+ self.right_hand_names = setting['LINKER_HAND']['RIGHT_HAND']['NAME']
+ self.right_hand_joint = setting['LINKER_HAND']['RIGHT_HAND']['JOINT']
+ self.right_hand_force = setting['LINKER_HAND']['RIGHT_HAND']['TOUCH']
+ self.password = setting['PASSWORD']
+ except Exception as e:
+ setting = None
+ print(f"Error reading setting.yaml: {e}")
+ self.setting = setting
+ return self.setting
+
+ def load_action_yaml(self,hand_joint="",hand_type=""):
+ if hand_joint == "L20":
+ action_path = self.l20_positions
+ elif hand_joint == "L10":
+ action_path = self.l10_positions
+ elif hand_joint == "L25":
+ action_path = self.l25_positions
+ elif hand_joint == "L21":
+ action_path = self.l21_positions
+ elif hand_joint == "L7":
+ action_path = self.l7_positions
+ print(action_path)
+ try:
+ with open(action_path, 'r', encoding='utf-8') as file:
+ yaml_data = yaml.safe_load(file)
+ if hand_type == "left":
+ self.action_yaml = yaml_data["LEFT_HAND"]
+ else:
+ self.action_yaml = yaml_data["RIGHT_HAND"]
+ except Exception as e:
+ self.action_yaml = None
+ print(f"yaml配置文件不存在: {e}")
+ return self.action_yaml
+
+ def write_to_yaml(self, action_name, action_pos,hand_joint="",hand_type=""):
+ a = False
+ if hand_joint == "L20":
+ action_path = self.l20_positions
+ elif hand_joint == "L10":
+ action_path = self.l10_positions
+ elif hand_joint == "L7":
+ action_path = self.l7_positions
+ elif hand_joint == "L21":
+ action_path = self.l21_positions
+ elif hand_joint == "L25":
+ action_path = self.l25_positions
+ try:
+ with open(action_path, 'r', encoding='utf-8') as file:
+ yaml_data = yaml.safe_load(file)
+ print(yaml_data)
+ if hand_type == "left":
+ if yaml_data["LEFT_HAND"] == None:
+ yaml_data["LEFT_HAND"] = []
+ yaml_data["LEFT_HAND"].append({"ACTION_NAME": action_name, "POSITION": action_pos})
+ elif hand_type == "right":
+ if yaml_data["RIGHT_HAND"] == None:
+ yaml_data["RIGHT_HAND"] = []
+ yaml_data["RIGHT_HAND"].append({"ACTION_NAME": action_name, "POSITION": action_pos})
+ with open(action_path, 'w', encoding='utf-8') as file:
+ yaml.safe_dump(yaml_data, file, allow_unicode=True)
+ a = True
+ except Exception as e:
+ a = False
+ print(f"Error writing to yaml file: {e}")
+ return a
+
\ No newline at end of file
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/mapping.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/mapping.py
new file mode 100644
index 0000000..deb6a19
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/mapping.py
@@ -0,0 +1,383 @@
+#---------------------------------------------------------------------------------------------------
+# L6 L
+l6_l_min = [0, 0, 0, 0, 0, 0]
+l6_l_max = [0.99, 1.39, 1.26, 1.26, 1.26, 1.26]
+l6_l_derict = [-1, -1, -1, -1, -1, -1]
+# L6 R
+l6_r_min = [0, 0, 0, 0, 0, 0]
+l6_r_max = [0.99, 1.39, 1.26, 1.26, 1.26, 1.26]
+l6_r_derict = [-1, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+# O6 L
+o6_l_min = [0, 0, 0, 0, 0, 0]
+o6_l_max = [0.58, 1.36, 1.6, 1.6, 1.6, 1.6]
+o6_l_derict = [-1, -1, -1, -1, -1, -1]
+# O6 R
+o6_r_min = [0, 0, 0, 0, 0, 0]
+o6_r_max = [0.58, 1.36, 1.6, 1.6, 1.6, 1.6]
+o6_r_derict = [-1, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+# L7 L OK
+l7_l_min = [0, 0, 0, 0, 0, 0, 0]
+l7_l_max = [0.44, 1.43, 1.62, 1.62, 1.62, 1.62, 1.01]
+l7_l_derict = [-1, -1, -1, -1, -1, -1, -1]
+# L7 R OK (urdf后续会更改！！！)
+l7_r_min = [0, -1.43, 0, 0, 0, 0, 0]
+l7_r_max = [0.75, 0, 1.62, 1.62, 1.62, 1.62, 1.54]
+l7_r_derict = [-1, 0, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+# L10 L OK
+l10_l_min = [0, 0, 0, 0, 0, 0, 0, -0.26, -0.26, -0.52]
+l10_l_max = [1.45, 1.43, 1.62, 1.62, 1.62, 1.62, 0.26, 0, 0, 1.01]
+l10_l_derict = [-1, -1, -1, -1, -1, -1, 0, -1, -1, -1]
+# L10 R OK
+l10_r_min = [0, 0, 0, 0, 0, 0, -0.26, 0, 0, -0.52]
+l10_r_max = [0.75, 1.43, 1.62, 1.62, 1.62, 1.62, 0.21, 0.21, 0.34, 1.01]
+l10_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, -1]
+#---------------------------------------------------------------------------------------------------
+# L20 L OK
+l20_l_min = [0, 0, 0, 0, 0, -0.297, -0.26, -0.26, -0.26, -0.26, 0.122, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l20_l_max = [0.87, 1.4, 1.4, 1.4, 1.4, 0.683, 0.26, 0.26, 0.26, 0.26, 1.78, 0, 0, 0, 0, 1.29, 1.08, 1.08, 1.08, 1.08]
+l20_l_derict = [-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
+# L20 R OK
+l20_r_min = [0, 0, 0, 0, 0, -0.297, -0.26, -0.26, -0.26, -0.26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l20_r_max = [0.87, 1.4, 1.4, 1.4, 1.4, 0.683, 0.26, 0.26, 0.26, 0.26, 1.78, 0, 0, 0, 0, 1.29, 1.08, 1.08, 1.08, 1.08]
+l20_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+# L21 L OK
+l21_l_min = [0, 0, 0, 0, 0, 0, 0, -0.18, -0.18, 0, -0.6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l21_l_max = [1, 1.57, 1.57, 1.57, 1.57, 1.6, 0.18, 0.18, 0.18, 0.18, 0.6, 0, 0, 0, 0, 1.57, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57]
+l21_l_derict = [-1, -1, -1, -1, -1, -1, -1, -1, -1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
+# L21 R OK
+l21_r_min = [0, 0, 0, 0, 0, 0, -0.18, -0.18, -0.18, -0.18, -0.6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l21_r_max = [1, 1.57, 1.57, 1.57, 1.57, 1.6, 0.18, 0.18, 0.18, 0.18, 0.6, 0, 0, 0, 0, 1.57, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57]
+l21_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+#---------------------------------------------------------------------------------------------------
+# L25 L OK
+l25_l_min = [0, 0, 0, 0, 0, 0, -0.26, -0.26, -0.26, -0.26, -0.26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l25_l_max = [0.9, 1.57, 1.57, 1.57, 1.57, 1.3, 0.26, 0.26, 0.26, 0.26, 0.61, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57]
+l25_l_derict = [-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1]
+# L25 R OK
+l25_r_min = [0, 0, 0, 0, 0, 0, -0.26, -0.26, -0.26, -0.26, -0.26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l25_r_max = [0.9, 1.57, 1.57, 1.57, 1.57, 1.3, 0.26, 0.26, 0.26, 0.26, 0.61, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57]
+l25_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+
+def range_to_arc_left(left_range,hand_joint):
+ num=0
+ if hand_joint == "L6":
+ num = 6
+ l_min = l6_l_min
+ l_max = l6_l_max
+ l_derict = l6_l_derict
+ elif hand_joint == "O6":
+ num = 6
+ l_min = o6_l_min
+ l_max = o6_l_max
+ l_derict = o6_l_derict
+ elif hand_joint == "L7":
+ num = 7
+ l_min = l7_l_min
+ l_max = l7_l_max
+ l_derict = l7_l_derict
+ elif hand_joint == "L10":
+ num = 10
+ l_min = l10_l_min
+ l_max = l10_l_max
+ l_derict = l10_l_derict
+ elif hand_joint == "L20":
+ num = 20
+ l_min = l20_l_min
+ l_max = l20_l_max
+ l_derict = l20_l_derict
+ elif hand_joint == "L21":
+ num = 25
+ l_min = l21_l_min
+ l_max = l21_l_max
+ l_derict = l21_l_derict
+ hand_arc = [0] * num
+ for i in range(num):
+ if hand_joint == "L20":
+ if 11 <= i <= 14: continue
+ if hand_joint == "L21":
+ if 11 <= i <= 14: continue
+ if 16 <= i <= 19: continue
+ val_l = is_within_range(left_range[i], 0, 255)
+ if l_derict[i] == -1:
+ hand_arc[i] = scale_value(val_l, 0, 255, l_max[i], l_min[i])
+ else:
+ hand_arc[i] = scale_value(val_l, 0, 255, l_min[i], l_max[i])
+ return hand_arc
+
+def range_to_arc_right(right_range,hand_joint):
+ num=0
+ if hand_joint == "L6":
+ num = 6
+ r_min = l6_r_min
+ r_max = l6_r_max
+ r_derict = l6_r_derict
+ elif hand_joint == "O6":
+ num = 6
+ r_min = o6_r_min
+ r_max = o6_r_max
+ r_derict = o6_r_derict
+ elif hand_joint == "L7":
+ num = 7
+ r_min = l7_r_min
+ r_max = l7_r_max
+ r_derict = l7_r_derict
+ elif hand_joint == "L10":
+ num = 10
+ r_min = l10_r_min
+ r_max = l10_r_max
+ r_derict = l10_r_derict
+ elif hand_joint == "L20":
+ num = 20
+ r_min = l20_r_min
+ r_max = l20_r_max
+ r_derict = l20_r_derict
+ elif hand_joint == "L21":
+ num = 25
+ r_min = l21_r_min
+ r_max = l21_r_max
+ r_derict = l21_r_derict
+ hand_arc = [0] * num
+ for i in range(num):
+ if hand_joint == "L20":
+ if 11 <= i <= 14: continue
+ if hand_joint == "L21":
+ if 11 <= i <= 14: continue
+ if 16 <= i <= 19: continue
+ val_r = is_within_range(right_range[i], 0, 255)
+ if r_derict[i] == -1:
+ hand_arc[i] = scale_value(val_r, 0, 255, r_max[i], r_min[i])
+ else:
+ hand_arc[i] = scale_value(val_r, 0, 255, r_min[i], r_max[i])
+ return hand_arc
+
+'''
+def arc_to_range_left(left_arc,hand_joint):
+ num=0
+ if hand_joint == "L7":
+ num = 7
+ l_min = l7_l_min
+ l_max = l7_l_max
+ l_derict = l7_l_derict
+ elif hand_joint == "L10":
+ num = 10
+ l_min = l10_l_min
+ l_max = l10_l_max
+ l_derict = l10_l_derict
+ elif hand_joint == "L20":
+ num = 20
+ l_min = l20_l_min
+ l_max = l20_l_max
+ l_derict = l20_l_derict
+ elif hand_joint == "L21":
+ num = 25
+ l_min = l21_l_min
+ l_max = l21_l_max
+ l_derict = l21_l_derict
+ hand_range = [0] * num
+ for i in range(num):
+ if hand_joint == "L20":
+ if 11 <= i <= 14: continue
+ if hand_joint == "L21":
+ if 11 <= i <= 14: continue
+ if 16 <= i <= 19: continue
+ val_l = is_within_range(left_arc[i], 0, 255)
+ if l_derict[i] == -1:
+ hand_range[i] = scale_value(val_l, 0, 255, l_max[i], l_min[i])
+ else:
+ hand_range[i] = scale_value(val_l, 0, 255, l_min[i], l_max[i])
+ return hand_range
+ '''
+def arc_to_range_left(hand_arc_l,hand_joint):
+ num=0
+ if hand_joint == "O6":
+ num = 6
+ l_min = o6_l_min
+ l_max = o6_l_max
+ l_derict = o6_l_derict
+ elif hand_joint == "L7":
+ num = 7
+ l_min = l7_l_min
+ l_max = l7_l_max
+ l_derict = l7_l_derict
+ elif hand_joint == "L10":
+ num = 10
+ l_min = l10_l_min
+ l_max = l10_l_max
+ l_derict = l10_l_derict
+ elif hand_joint == "L20":
+ num = 20
+ l_min = l20_l_min
+ l_max = l20_l_max
+ l_derict = l20_l_derict
+ elif hand_joint == "L21":
+ num = 25
+ l_min = l21_l_min
+ l_max = l21_l_max
+ l_derict = l21_l_derict
+ hand_range = [0] * num
+ #hand_range_l = [0] * 7
+ for i in range(num):
+ if hand_joint == "L20":
+ if 11 <= i <= 14: continue
+ if hand_joint == "L21":
+ if 11 <= i <= 14: continue
+ if 16 <= i <= 19: continue
+ val_l = is_within_range(hand_arc_l[i], l_min[i], l_max[i])
+ if l_derict[i] == -1:
+ hand_range[i] = scale_value(val_l, l_min[i], l_max[i], 255, 0)
+ else:
+ hand_range[i] = scale_value(val_l, l_min[i], l_max[i], 0, 255)
+
+ return hand_range
+
+def arc_to_range_right(right_arc,hand_joint):
+ num=0
+ if hand_joint == "O6":
+ num = 6
+ r_min = o6_r_min
+ r_max = o6_r_max
+ r_derict = o6_r_derict
+ elif hand_joint == "L7":
+ num = 7
+ r_min = l7_r_min
+ r_max = l7_r_max
+ r_derict = l7_r_derict
+ elif hand_joint == "L10":
+ num = 10
+ r_min = l10_r_min
+ r_max = l10_r_max
+ r_derict = l10_r_derict
+ elif hand_joint == "L20":
+ num = 20
+ r_min = l20_r_min
+ r_max = l20_r_max
+ r_derict = l20_r_derict
+ elif hand_joint == "L21":
+ num = 25
+ r_min = l21_r_min
+ r_max = l21_r_max
+ r_derict = l21_r_derict
+ hand_range = [0] * num
+ for i in range(num):
+ if hand_joint == "L20":
+ if 11 <= i <= 14: continue
+ if hand_joint == "L21":
+ if 11 <= i <= 14: continue
+ if 16 <= i <= 19: continue
+ val_r = is_within_range(right_arc[i], r_min[i], r_max[i])
+ if r_derict[i] == -1:
+ hand_range[i] = scale_value(val_r, r_min[i], r_max[i], 255, 0)
+ else:
+ hand_range[i] = scale_value(val_r, r_min[i], r_max[i], 0, 255)
+ return hand_range
+
+
+
+
+def range_to_arc_right_l20(hand_range_r):
+ hand_arc_r = [0] * 20
+ for i in range(20):
+ if 11 <= i <= 14: continue
+ val_r = is_within_range(hand_range_r[i], 0, 255)
+ if l20_r_derict[i] == -1:
+ hand_arc_r[i] = scale_value(val_r, 0, 255, l20_r_max[i], l20_r_min[i])
+ else:
+ hand_arc_r[i] = scale_value(val_r, 0, 255, l20_r_min[i], l20_r_max[i])
+ return hand_arc_r
+
+
+def range_to_arc_left_l20(hand_range_l):
+ hand_arc_l = [0] * 20
+ for i in range(20):
+ if 11 <= i <= 14: continue
+ val_l = is_within_range(hand_range_l[i], 0, 255)
+ if l20_l_derict[i] == -1:
+ hand_arc_l[i] = scale_value(val_l, 0, 255, l20_l_max[i], l20_l_min[i])
+ else:
+ hand_arc_l[i] = scale_value(val_l, 0, 255, l20_l_min[i], l20_l_max[i])
+ return hand_arc_l
+
+
+def arc_to_range_right_l20(hand_arc_r):
+ hand_range_r = [0] * 20
+ for i in range(20):
+ if 11 <= i <= 14: continue
+ val_r = is_within_range(hand_arc_r[i], l20_r_min[i], l20_r_max[i])
+ if l20_r_derict[i] == -1:
+ hand_range_r[i] = scale_value(val_r, l20_r_min[i], l20_r_max[i], 255, 0)
+ else:
+ hand_range_r[i] = scale_value(val_r, l20_r_min[i], l20_r_max[i], 0, 255)
+ return hand_range_r
+
+
+def arc_to_range_left_l20(hand_arc_l):
+ hand_range_l = [0] * 20
+ for i in range(20):
+ if 11 <= i <= 14: continue
+ val_l = is_within_range(hand_arc_l[i], l20_l_min[i], l20_l_max[i])
+ if l20_l_derict[i] == -1:
+ hand_range_l[i] = scale_value(val_l, l20_l_min[i], l20_l_max[i], 255, 0)
+ else:
+ hand_range_l[i] = scale_value(val_l, l20_l_min[i], l20_l_max[i], 0, 255)
+
+ return hand_range_l
+
+
+def range_to_arc_right_10(hand_range_r):
+ hand_arc_r = [0] * 10
+ for i in range(10):
+ val_r = is_within_range(hand_range_r[i], 0, 255)
+ if l10_r_derict[i] == -1:
+ hand_arc_r[i] = scale_value(val_r, 0, 255, l10_r_max[i], l10_r_min[i])
+ else:
+ hand_arc_r[i] = scale_value(val_r, 0, 255, l10_r_min[i], l10_r_max[i])
+
+ return hand_arc_r
+
+
+def range_to_arc_left_10(hand_range_l):
+ hand_arc_l = [0] * 10
+ for i in range(10):
+ val_l = is_within_range(hand_range_l[i], 0, 255)
+ if l10_l_derict[i] == -1:
+ hand_arc_l[i] = scale_value(val_l, 0, 255, l10_l_max[i], l10_l_min[i])
+ else:
+ hand_arc_l[i] = scale_value(val_l, 0, 255, l10_l_min[i], l10_l_max[i])
+ return hand_arc_l
+
+
+def arc_to_range_right_10(hand_arc_r):
+ hand_range_r = [0] * 10
+ for i in range(10):
+ val_r = is_within_range(hand_arc_r[i], l10_r_min[i], l10_r_max[i])
+ if l10_r_derict[i] == -1:
+ hand_range_r[i] = scale_value(val_r, l10_r_min[i], l10_r_max[i], 255, 0)
+ else:
+ hand_range_r[i] = scale_value(val_r, l10_r_min[i], l10_r_max[i], 0, 255)
+ return hand_range_r
+
+
+def arc_to_range_left_10(hand_arc_l):
+ hand_range_l = [0] * 10
+ for i in range(10):
+ val_l = is_within_range(hand_arc_l[i], l10_l_min[i], l10_l_max[i])
+ if l10_l_derict[i] == -1:
+ hand_range_l[i] = scale_value(val_l, l10_l_min[i], l10_l_max[i], 255, 0)
+ else:
+ hand_range_l[i] = scale_value(val_l, l10_l_min[i], l10_l_max[i], 0, 255)
+
+ return hand_range_l
+
+
+def scale_value(original_value, a_min, a_max, b_min, b_max):
+ return (original_value - a_min) * (b_max - b_min) / (a_max - a_min) + b_min
+
+
+def is_within_range(value, min_value, max_value):
+ return min(max_value, max(min_value, value))
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/open_can.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/open_can.py
new file mode 100644
index 0000000..96aa932
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/open_can.py
@@ -0,0 +1,145 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+'''
+Author: HJX
+Date: 2025-04-01 14:09:21
+LastEditors: Please set LastEditors
+LastEditTime: 2025-04-11 09:15:31
+FilePath: /Linker_Hand_SDK_ROS/src/linker_hand_sdk_ros/scripts/LinkerHand/utils/open_can.py
+Description:
+symbol_custom_string_obkorol_copyright:
+'''
+import sys,os,time,subprocess
+sys.path.append(os.path.dirname(os.path.abspath(__file__)))
+from color_msg import ColorMsg
+from load_write_yaml import LoadWriteYaml
+# from ament_index_python.packages import get_package_share_directory
+import os
+
+
+class OpenCan:
+ def __init__(self,load_yaml=None):
+ self.yaml = LoadWriteYaml()
+ self.password = self.yaml.load_setting_yaml()["PASSWORD"]
+
+ def open_can0(self):
+ try:
+ # 检查 can0 接口是否已存在并处于 up 状态
+ result = subprocess.run(
+ ["ip", "link", "show", "can0"],
+ check=True,
+ text=True,
+ capture_output=True
+ )
+ if "state UP" in result.stdout:
+ return
+ # 如果没有处于 UP 状态，则配置接口
+ subprocess.run(
+ ["sudo", "-S", "ip", "link", "set", "can0", "up", "type", "can", "bitrate", "1000000"],
+ input=f"{self.password}\n",
+ check=True,
+ text=True,
+ capture_output=True
+ )
+
+ except subprocess.CalledProcessError as e:
+ pass
+ except Exception as e:
+ pass
+ def open_can(self,can="can0"):
+ try:
+ # 检查 can0 接口是否已存在并处于 up 状态
+ result = subprocess.run(
+ ["ip", "link", "show", can],
+ check=True,
+ text=True,
+ capture_output=True
+ )
+ if "state UP" in result.stdout:
+ return
+ # 如果没有处于 UP 状态，则配置接口
+ subprocess.run(
+ ["sudo", "-S", "ip", "link", "set", can, "up", "type", "can", "bitrate", "1000000"],
+ input=f"{self.password}\n",
+ check=True,
+ text=True,
+ capture_output=True
+ )
+ except subprocess.CalledProcessError as e:
+ pass
+ except Exception as e:
+ pass
+
+
+ def is_can_up_sysfs(self, interface="can0"):
+ # 检查接口目录是否存在
+ if not os.path.exists(f"/sys/class/net/{interface}"):
+ return False
+ # 读取接口状态
+ try:
+ with open(f"/sys/class/net/{interface}/operstate", "r") as f:
+ state = f.read().strip()
+ if state == "up":
+ return True
+ except Exception as e:
+ print(f"Error reading CAN interface state: {e}")
+ return False
+
+ def close_can0(self):
+ try:
+ # 检查 can0 接口是否存在
+ result = subprocess.run(
+ ["ip", "link", "show", "can0"],
+ check=True,
+ text=True,
+ capture_output=True
+ )
+
+ # 如果接口存在且处于 UP 状态，则关闭它
+ if "state UP" in result.stdout:
+ subprocess.run(
+ ["sudo", "-S", "ip", "link", "set", "can0", "down"],
+ input=f"{self.password}\n",
+ check=True,
+ text=True,
+ capture_output=True
+ )
+ return True
+ return False
+
+ except subprocess.CalledProcessError as e:
+ print(f"Error closing CAN interface: {e}")
+ return False
+ except Exception as e:
+ print(f"Unexpected error: {e}")
+ return False
+
+ def close_can(self,can="can0"):
+ try:
+ # 检查 can0 接口是否存在
+ result = subprocess.run(
+ ["ip", "link", "show", can],
+ check=True,
+ text=True,
+ capture_output=True
+ )
+
+ # 如果接口存在且处于 UP 状态，则关闭它
+ if "state UP" in result.stdout:
+ subprocess.run(
+ ["sudo", "-S", "ip", "link", "set", can, "down"],
+ input=f"{self.password}\n",
+ check=True,
+ text=True,
+ capture_output=True
+ )
+ return True
+ return False
+
+ except subprocess.CalledProcessError as e:
+ print(f"Error closing CAN interface: {e}")
+ return False
+ except Exception as e:
+ print(f"Unexpected error: {e}")
+ return False
+
\ No newline at end of file
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/__init__.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py
new file mode 100755
index 0000000..45d729e
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py
@@ -0,0 +1,421 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+'''
+编译: colcon build --symlink-install
+启动命令:ros2 run linker_hand_ros2_sdk linker_hand_sdk
+'''
+from re import A
+import rclpy,sys # ROS2 Python接口库
+import time
+import numpy as np
+from rclpy.node import Node # ROS2 节点类
+from rclpy.clock import Clock
+from std_msgs.msg import String, Header, Float32MultiArray
+from sensor_msgs.msg import JointState, PointCloud2, PointField
+import time, json, threading
+from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
+from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
+from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
+
+
+class LinkerHand(Node):
+ def __init__(self, name):
+ super().__init__(name)
+ # 声明参数（带默认值）
+ self.declare_parameter('hand_type', 'left')
+ self.declare_parameter('hand_joint', 'L6')
+ self.declare_parameter('is_touch', False)
+ self.declare_parameter('can', 'can0')
+ self.declare_parameter('modbus', "None")
+
+ # ros时间获取
+ self.stamp_clock = Clock()
+ # 获取参数值
+ self.hand_type = self.get_parameter('hand_type').value
+ self.hand_joint = self.get_parameter('hand_joint').value
+ self.is_touch = self.get_parameter('is_touch').value
+ self.can = self.get_parameter('can').value
+ self.modbus = self.get_parameter('modbus').value
+ self.sdk_v = 2
+ self.sleep_time = 0.005
+ self.cmd_lock = False
+ self.last_hand_post_cmd = None # 最新手指位置命令
+ self.last_hand_vel_cmd = None # 最新手指速度命令
+ self.last_hand_eff_cmd = None # 最新手指力矩命令
+
+ self.last_hand_state = [-1] * 10
+ self.last_hand_vel = [-1] * 10
+ self.force = [[-1] * 5] * 4
+ self.matrix_dic = {
+ "stamp":{
+ "sec": 0,
+ "nanosec": 0,
+ },
+ "thumb_matrix":[[-1] * 6 for _ in range(12)],
+ "index_matrix":[[-1] * 6 for _ in range(12)],
+ "middle_matrix":[[-1] * 6 for _ in range(12)],
+ "ring_matrix":[[-1] * 6 for _ in range(12)],
+ "little_matrix":[[-1] * 6 for _ in range(12)]
+ }
+ # 压感矩阵合值，单位g 克
+ self.matrix_mass_dic = {
+ "stamp":{
+ "secs": 0,
+ "nsecs": 0,
+ },
+ "thumb_mass":[-1],
+ "index_mass":[-1],
+ "middle_mass":[-1],
+ "ring_mass":[-1],
+ "little_mass":[-1]
+ }
+ self.last_hand_info = {
+ "version": [-1], # Dexterous hand version number
+ "hand_joint": self.hand_joint, # Dexterous hand joint type
+ "speed": [-1] * 10, # Current speed threshold of the dexterous hand
+ "current": [-1] * 10, # Current of the dexterous hand
+ "fault": [-1] * 10, # Current fault of the dexterous hand
+ "motor_temperature": [-1] * 10, # Current motor temperature of the dexterous hand
+ "torque": [-1] * 10, # Current torque of the dexterous hand
+ "is_touch":self.is_touch,
+ "touch_type": -1,
+ "finger_order": None # Finger motor order
+ }
+ self.version = []
+ self.touch_type = -1
+ self.hz = 1.0/60.0
+
+ self.hand_setting_sub = self.create_subscription(String,'/cb_hand_setting_cmd', self.hand_setting_cb, 10)
+ self._init_hand()
+ time.sleep(1)
+ self.run_count = 0 # 计数器，用于记录运行次数
+ self.timer = self.create_timer(0.01, self.run) # 100 Hz
+ self.thread_pub_state = threading.Thread(target=self.pub_state)
+ self.thread_pub_state.daemon = True
+ self.thread_pub_state.start()
+
+ def _init_hand(self):
+ self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
+ time.sleep(0.1)
+ self.touch_type = self.api.get_touch_type()
+ self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+ self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
+ self.hand_info_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_info', 10)
+ if self.is_touch == True:
+ if self.modbus != "None":
+ self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+ self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+ self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+ elif self.touch_type > 1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
+ self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+ self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+ self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+ elif self.touch_type != -1 and self.modbus == "None":
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
+ self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
+ else:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
+ self.is_touch = False
+
+ self.embedded_version = self.api.get_embedded_version()
+ pose = None
+ torque = [200, 200, 200, 200, 200]
+ speed = [200, 250, 250, 250, 250]
+ if self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6" or self.hand_joint.upper() == "L6P":
+ pose = [200, 255, 255, 255, 255, 180]
+ torque = [250, 250, 250, 250, 250, 250]
+ # O6 最大速度阈值
+ speed = [200, 250, 250, 250, 250, 250]
+ elif self.hand_joint == "L7":
+ # The data length of L7 is 7, reinitialize here
+ pose = [255, 200, 255, 255, 255, 255, 180]
+ torque = [250, 250, 250, 250, 250, 250, 250]
+ speed = [120, 250, 250, 250, 250, 250, 250]
+ elif self.hand_joint == "L10":
+ torque = [255] * 10
+ pose = [255, 200, 255, 255, 255, 255, 180, 180, 180, 41]
+ speed = [200, 250, 250, 250, 250, 250, 250, 250, 250, 250]
+ elif self.hand_joint == "L20":
+ pose = [255,255,255,255,255,255,10,100,180,240,245,255,255,255,255,255,255,255,255,255]
+ elif self.hand_joint == "L21":
+ pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+ elif self.hand_joint == "L25":
+ pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+ if pose is not None:
+ for i in range(1):
+ self.api.set_speed(speed=speed)
+ time.sleep(0.1)
+ self.api.set_torque(torque=torque)
+ time.sleep(0.1)
+ self.api.finger_move(pose=pose)
+ time.sleep(0.1)
+
+ def list_check(self,pose):
+ if isinstance(pose, list) == False:
+ return False
+ if len(self.last_hand_post_cmd) != len(pose):
+ return False
+ return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
+
+ def hand_control_cb(self, msg):
+ if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
+ self.last_hand_post_cmd = msg.position
+ if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
+ self.last_hand_vel_cmd = msg.velocity
+ if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
+ self.last_hand_eff_cmd = msg.effort
+
+ def run(self):
+ if self.sdk_v == 1:
+ self.sleep_time = 0.009
+ if self.hand_state_pub.get_subscription_count() > 0:
+ # 优先获取手指状态并且发布
+ self.last_hand_state = self.api.get_state()
+ time.sleep(0.003)
+ self.last_hand_vel = self.api.get_joint_speed()
+ time.sleep(0.002)
+ if self.cmd_lock == False:
+ if self.last_hand_post_cmd != None:
+ self.api.finger_move(pose=self.last_hand_post_cmd)
+ self.last_hand_post_cmd = None
+ if self.last_hand_vel_cmd != None:
+ vel = list(self.last_hand_vel_cmd)
+ if all(x == 0 for x in vel):
+ pass
+ else:
+ if (str(self.hand_joint).upper() == "O6" or str(self.hand_joint).upper() == "L6" or str(self.hand_joint).upper() == "L6P") and len(vel) == 6:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L7" and len(vel) == 7:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L10" and len(vel) == 10:
+ speed = [vel[0],vel[2],vel[3],vel[4],vel[5]]
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L20" and len(vel) == 20:
+ speed = [vel[10],vel[1],vel[2],vel[3],vel[4]]
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L21" and len(vel) == 25:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L25" and len(vel) == 25:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ self.last_hand_vel_cmd = None
+ time.sleep(0.003)
+ if self.run_count == 3 and self.is_touch == True and self.touch_type == 1 and self.modbus == "None" and self.touch_pub.get_subscription_count() > 0:
+ """单点式压力传感器"""
+ self.force = self.api.get_force()
+ if self.is_touch == True and (self.touch_type > 1 or self.modbus != "None") and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0):
+ """矩阵式压力传感器"""
+ if self.run_count == 3:
+ self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=self.sleep_time).tolist()
+ if self.run_count == 4:
+ self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=self.sleep_time).tolist()
+ if self.run_count == 5:
+ self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=self.sleep_time).tolist()
+ if self.run_count == 6:
+ self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=self.sleep_time).tolist()
+ if self.run_count == 7:
+ self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=self.sleep_time).tolist()
+ time.sleep(0.005)
+ if self.run_count == 8 and self.hand_info_pub.get_subscription_count() > 0:
+ """手部信息"""
+ self.last_hand_info = {
+ "version": self.embedded_version, # Dexterous hand version number
+ "hand_joint": self.hand_joint, # Dexterous hand joint type
+ "speed": self.api.get_speed(), # Current speed threshold of the dexterous hand
+ "current": self.api.get_current(), # Current of the dexterous hand
+ "fault": self.api.get_fault(), # Current fault of the dexterous hand
+ "motor_temperature": self.api.get_temperature(), # Current motor temperature of the dexterous hand
+ "torque": self.api.get_torque(), # Current torque of the dexterous hand
+ "is_touch":self.is_touch,
+ "touch_type": self.touch_type,
+ "finger_order": self.api.get_finger_order() # Finger motor order
+ }
+
+ if self.run_count == 9:
+ self.api.clear_faults() # 自动清除错误编码
+ self.run_count = 0
+ self.run_count += 1
+ time.sleep(0.003)
+
+
+ def pub_state(self):
+ while True:
+ if self.hand_state_pub.get_subscription_count() > 0:
+ msg = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
+ self.hand_state_pub.publish(msg)
+ if self.is_touch == True and self.touch_type == 1 and self.modbus == "None" and self.touch_pub.get_subscription_count() > 0:
+ msg = Float32MultiArray()
+ msg.data = [float(val) for sublist in self.force for val in sublist]
+ self.touch_pub.publish(msg)
+ if self.is_touch == True and (self.touch_type > 1 or self.modbus != "None") and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0):
+ # 发布矩阵压感数据JSON格式
+ self.pub_matrix_dic()
+ # 发布矩阵压感和值JSON格式
+ self.pub_matrix_mass(dic=self.matrix_dic)
+ # 发布矩阵压感点云格式
+ self.pub_matrix_point_cloud()
+ if self.hand_info_pub.get_subscription_count() > 0:
+ msg = String()
+ msg.data = json.dumps(self.last_hand_info)
+ self.hand_info_pub.publish(msg)
+ time.sleep(self.hz)
+
+ def pub_matrix_mass(self, dic):
+ """发布矩阵数据合值 单位g 克 JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_mass_dic["stamp"]["secs"] = t_secs
+ self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
+ self.matrix_mass_dic["unit"] = "g"
+ self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
+ self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
+ self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
+ self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
+ self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
+ msg.data = json.dumps(self.matrix_mass_dic)
+ self.matrix_touch_mass_pub.publish(msg)
+
+ def pub_matrix_point_cloud(self):
+ """发布矩阵数据点云格式"""
+ tmp_dic = self.matrix_dic.copy()
+ del tmp_dic['stamp'] # 去掉时间戳字段
+ all_matrices = list(tmp_dic.values()) # 5 帧，每帧 6×12=72 个数
+ # 摊平到一维：360 个 float
+ flat_list = [v for frame in all_matrices for v in frame] # 360
+ flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
+ fields = [PointField(
+ name='val',
+ offset=0,
+ datatype=PointField.UINT8,
+ count=1
+ )]
+ pc = PointCloud2()
+ pc.header.stamp = self.stamp_clock.now().to_msg()
+ pc.header.frame_id = ''
+ pc.height = 1
+ pc.width = flat.size # 360
+ pc.fields = fields
+ pc.is_bigendian = False
+ pc.point_step = 1 # 1 个 float32
+ pc.row_step = pc.point_step * pc.width
+ pc.data = flat.tobytes() # 1440 字节
+ self.matrix_touch_pub_pc.publish(pc)
+
+ def pub_matrix_dic(self):
+ """发布矩阵数据JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_dic["stamp"]["secs"] = t_secs
+ self.matrix_dic["stamp"]["nsecs"] = t_nsecs
+ msg.data = json.dumps(self.matrix_dic)
+ self.matrix_touch_pub.publish(msg)
+
+ def joint_state_msg(self, pose,vel=[]):
+ joint_state = JointState()
+ joint_state.header = Header()
+ joint_state.header.stamp = self.get_clock().now().to_msg()
+ joint_state.name = self.api.get_finger_order()
+ joint_state.position = [float(x) for x in pose]
+ if len(vel) > 1:
+ joint_state.velocity = [float(x) for x in vel]
+ else:
+ joint_state.velocity = [0.0] * len(pose)
+ joint_state.effort = [0.0] * len(pose)
+ return joint_state
+
+
+
+
+
+ def hand_setting_cb(self,msg):
+ '''控制命令回调'''
+ data = json.loads(msg.data)
+ print(f"Received setting command: {data['setting_cmd']}",flush=True)
+ try:
+ if data["params"]["hand_type"] == "left":
+ hand = self.api
+ hand_left = True
+ elif data["params"]["hand_type"] == "right":
+ hand = self.api
+ hand_right = True
+ else:
+ print("Please specify the hand part to be set",flush=True)
+ return
+ self.cmd_lock = True
+ # Set maximum torque
+ if data["setting_cmd"] == "set_max_torque_limits": # Set maximum torque
+ torque = list(data["params"]["torque"])
+ hand.set_torque(torque=torque)
+
+ if data["setting_cmd"] == "set_speed": # Set speed
+ if isinstance(data["params"]["speed"], list) == True:
+ speed = data["params"]["speed"]
+ hand.set_speed(speed=speed)
+ else:
+ ColorMsg(msg=f"Speed parameter error, speed must be a list", color="red")
+ if data["setting_cmd"] == "clear_faults": # Clear faults
+ if hand_left == True and self.hand_joint == "L10" :
+ ColorMsg(msg=f"L10 left hand cannot clear faults")
+ elif hand_right == True and self.hand_joint == "L10" :
+ ColorMsg(msg=f"L10 right hand cannot clear faults")
+ else:
+ hand.clear_faults()
+ if data["setting_cmd"] == "get_faults": # Get faults
+ f = hand.get_fault()
+ ColorMsg(msg=f"Get faults: {f}")
+ if data["setting_cmd"] == "electric_current": # Get current
+ ColorMsg(msg=f"Get current: {hand.get_current()}")
+ if data["setting_cmd"] == "set_electric_current": # Set current
+ if isinstance(data["params"]["current"], list) == True:
+ hand.set_current(data["params"]["current"])
+ if data["setting_cmd"] == "show_fun_table": # Get faults
+ f = hand.show_fun_table()
+ except:
+ print("命令参数错误")
+ self.cmd_lock = False
+ finally:
+ self.cmd_lock = False
+
+
+ def close_can(self):
+ self.api.open_can.close_can(can=self.can)
+ sys.exit(0)
+
+
+def main(args=None):
+ try:
+ rclpy.init(args=args)
+ node = LinkerHand("linker_hand_sdk")
+ embedded_version = node.embedded_version
+ if len(embedded_version) == 3 or node.hand_joint.upper() == "O6" or node.hand_joint.upper() == "L6" or node.hand_joint.upper() == "G20":
+ ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
+ node.sdk_v = 2
+ elif len(embedded_version) == 6 and node.hand_joint == "L10":
+ ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
+ node.sdk_v = 2
+ elif len(embedded_version) > 4 and ((embedded_version[0]==10 and embedded_version[4]>35) or (embedded_version[0]==7 and embedded_version[4]>50) or (embedded_version[0] == 6)):
+ ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
+ node.sdk_v = 2
+ else:
+ ColorMsg(msg=f"SDK V1", color="green")
+ node.sdk_v = 1
+ rclpy.spin(node) # 主循环，监听 ROS 回调
+ except KeyboardInterrupt:
+ print("收到 Ctrl+C，准备退出...")
+ finally:
+ # node.close_can() # 关闭 CAN 或其他硬件资源
+ # node.destroy_node() # 销毁 ROS 节点
+ # rclpy.shutdown() # 关闭 ROS
+ print("程序已退出。")
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py.bak b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py.bak
new file mode 100644
index 0000000..81c31a9
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py.bak
@@ -0,0 +1,414 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+'''
+编译: colcon build --symlink-install
+启动命令:ros2 run linker_hand_ros2_sdk linker_hand_sdk
+'''
+from re import A
+import rclpy,sys # ROS2 Python接口库
+import time
+import numpy as np
+from rclpy.node import Node # ROS2 节点类
+from rclpy.clock import Clock
+from std_msgs.msg import String, Header, Float32MultiArray
+from sensor_msgs.msg import JointState, PointCloud2, PointField
+import time, json, threading
+from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
+from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
+from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
+
+
+class LinkerHand(Node):
+ def __init__(self, name):
+ super().__init__(name)
+ # 声明参数（带默认值）
+ self.declare_parameter('hand_type', 'left')
+ self.declare_parameter('hand_joint', 'L6')
+ self.declare_parameter('is_touch', False)
+ self.declare_parameter('can', 'can0')
+ self.declare_parameter('modbus', "None")
+
+ # ros时间获取
+ self.stamp_clock = Clock()
+ # 获取参数值
+ self.hand_type = self.get_parameter('hand_type').value
+ self.hand_joint = self.get_parameter('hand_joint').value
+ self.is_touch = self.get_parameter('is_touch').value
+ self.can = self.get_parameter('can').value
+ self.modbus = self.get_parameter('modbus').value
+ self.sdk_v = 2
+ self.sleep_time = 0.005
+ self.cmd_lock = False
+ self.last_hand_post_cmd = None # 最新手指位置命令
+ self.last_hand_vel_cmd = None # 最新手指速度命令
+ self.last_hand_eff_cmd = None # 最新手指力矩命令
+
+ self.last_hand_state = [-1] * 10
+ self.last_hand_vel = [-1] * 10
+ self.force = [[-1] * 5] * 4
+ self.matrix_dic = {
+ "stamp":{
+ "sec": 0,
+ "nanosec": 0,
+ },
+ "thumb_matrix":[[-1] * 6 for _ in range(12)],
+ "index_matrix":[[-1] * 6 for _ in range(12)],
+ "middle_matrix":[[-1] * 6 for _ in range(12)],
+ "ring_matrix":[[-1] * 6 for _ in range(12)],
+ "little_matrix":[[-1] * 6 for _ in range(12)]
+ }
+ # 压感矩阵合值，单位g 克
+ self.matrix_mass_dic = {
+ "stamp":{
+ "secs": 0,
+ "nsecs": 0,
+ },
+ "thumb_mass":[-1],
+ "index_mass":[-1],
+ "middle_mass":[-1],
+ "ring_mass":[-1],
+ "little_mass":[-1]
+ }
+ self.last_hand_info = {
+ "version": [-1], # Dexterous hand version number
+ "hand_joint": self.hand_joint, # Dexterous hand joint type
+ "speed": [-1] * 10, # Current speed threshold of the dexterous hand
+ "current": [-1] * 10, # Current of the dexterous hand
+ "fault": [-1] * 10, # Current fault of the dexterous hand
+ "motor_temperature": [-1] * 10, # Current motor temperature of the dexterous hand
+ "torque": [-1] * 10, # Current torque of the dexterous hand
+ "is_touch":self.is_touch,
+ "touch_type": -1,
+ "finger_order": None # Finger motor order
+ }
+ self.version = []
+ self.touch_type = -1
+ self.hz = 1.0/60.0
+
+ self.hand_setting_sub = self.create_subscription(String,'/cb_hand_setting_cmd', self.hand_setting_cb, 10)
+ self._init_hand()
+ time.sleep(1)
+ self.run_count = 0 # 计数器，用于记录运行次数
+ self.timer = self.create_timer(0.01, self.run) # 100 Hz
+ self.thread_pub_state = threading.Thread(target=self.pub_state)
+ self.thread_pub_state.daemon = True
+ self.thread_pub_state.start()
+
+ def _init_hand(self):
+ self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
+ time.sleep(0.1)
+ self.touch_type = self.api.get_touch_type()
+ self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+ self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
+ self.hand_info_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_info', 10)
+ if self.is_touch == True:
+ if self.touch_type > 1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
+ self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+ self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+ self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+ elif self.touch_type != -1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
+ self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
+ else:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
+ self.is_touch = False
+ self.embedded_version = self.api.get_embedded_version()
+ pose = None
+ torque = [200, 200, 200, 200, 200]
+ speed = [200, 250, 250, 250, 250]
+ if self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6" or self.hand_joint.upper() == "L6P":
+ pose = [200, 255, 255, 255, 255, 180]
+ torque = [250, 250, 250, 250, 250, 250]
+ # O6 最大速度阈值
+ speed = [200, 250, 250, 250, 250, 250]
+ elif self.hand_joint == "L7":
+ # The data length of L7 is 7, reinitialize here
+ pose = [255, 200, 255, 255, 255, 255, 180]
+ torque = [250, 250, 250, 250, 250, 250, 250]
+ speed = [120, 250, 250, 250, 250, 250, 250]
+ elif self.hand_joint == "L10":
+ torque = [255] * 10
+ pose = [255, 200, 255, 255, 255, 255, 180, 180, 180, 41]
+ speed = [200, 250, 250, 250, 250, 250, 250, 250, 250, 250]
+ elif self.hand_joint == "L20":
+ pose = [255,255,255,255,255,255,10,100,180,240,245,255,255,255,255,255,255,255,255,255]
+ elif self.hand_joint == "L21":
+ pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+ elif self.hand_joint == "L25":
+ pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+ if pose is not None:
+ for i in range(1):
+ self.api.set_speed(speed=speed)
+ time.sleep(0.1)
+ self.api.set_torque(torque=torque)
+ time.sleep(0.1)
+ self.api.finger_move(pose=pose)
+ time.sleep(0.1)
+
+ def list_check(self,pose):
+ if isinstance(pose, list) == False:
+ return False
+ if len(self.last_hand_post_cmd) != len(pose):
+ return False
+ return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
+
+ def hand_control_cb(self, msg):
+ if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
+ self.last_hand_post_cmd = msg.position
+ if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
+ self.last_hand_vel_cmd = msg.velocity
+ if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
+ self.last_hand_eff_cmd = msg.effort
+
+ def run(self):
+ if self.sdk_v == 1:
+ self.sleep_time = 0.009
+ if self.hand_state_pub.get_subscription_count() > 0:
+ # 优先获取手指状态并且发布
+ self.last_hand_state = self.api.get_state()
+ time.sleep(0.003)
+ self.last_hand_vel = self.api.get_joint_speed()
+ time.sleep(0.002)
+ if self.cmd_lock == False:
+ if self.last_hand_post_cmd != None:
+ self.api.finger_move(pose=self.last_hand_post_cmd)
+ self.last_hand_post_cmd = None
+ if self.last_hand_vel_cmd != None:
+ vel = list(self.last_hand_vel_cmd)
+ if all(x == 0 for x in vel):
+ pass
+ else:
+ if (str(self.hand_joint).upper() == "O6" or str(self.hand_joint).upper() == "L6" or str(self.hand_joint).upper() == "L6P") and len(vel) == 6:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L7" and len(vel) == 7:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L10" and len(vel) == 10:
+ speed = [vel[0],vel[2],vel[3],vel[4],vel[5]]
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L20" and len(vel) == 20:
+ speed = [vel[10],vel[1],vel[2],vel[3],vel[4]]
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L21" and len(vel) == 25:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L25" and len(vel) == 25:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ self.last_hand_vel_cmd = None
+ time.sleep(0.003)
+ if self.run_count == 3 and self.is_touch == True and self.touch_type == 1 and self.touch_pub.get_subscription_count() > 0:
+ """单点式压力传感器"""
+ self.force = self.api.get_force()
+ if self.is_touch == True and self.touch_type > 1 and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0):
+ """矩阵式压力传感器"""
+ if self.run_count == 3:
+ self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=self.sleep_time).tolist()
+ if self.run_count == 4:
+ self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=self.sleep_time).tolist()
+ if self.run_count == 5:
+ self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=self.sleep_time).tolist()
+ if self.run_count == 6:
+ self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=self.sleep_time).tolist()
+ if self.run_count == 7:
+ self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=self.sleep_time).tolist()
+ time.sleep(0.005)
+ if self.run_count == 8 and self.hand_info_pub.get_subscription_count() > 0:
+ """手部信息"""
+ self.last_hand_info = {
+ "version": self.embedded_version, # Dexterous hand version number
+ "hand_joint": self.hand_joint, # Dexterous hand joint type
+ "speed": self.api.get_speed(), # Current speed threshold of the dexterous hand
+ "current": self.api.get_current(), # Current of the dexterous hand
+ "fault": self.api.get_fault(), # Current fault of the dexterous hand
+ "motor_temperature": self.api.get_temperature(), # Current motor temperature of the dexterous hand
+ "torque": self.api.get_torque(), # Current torque of the dexterous hand
+ "is_touch":self.is_touch,
+ "touch_type": self.touch_type,
+ "finger_order": self.api.get_finger_order() # Finger motor order
+ }
+ if self.run_count == 9:
+ self.run_count = 0
+ self.run_count += 1
+ time.sleep(0.003)
+
+
+ def pub_state(self):
+ while True:
+ if self.hand_state_pub.get_subscription_count() > 0:
+ msg = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
+ self.hand_state_pub.publish(msg)
+ if self.is_touch == True and self.touch_type == 1 and self.touch_pub.get_subscription_count() > 0:
+ msg = Float32MultiArray()
+ msg.data = [float(val) for sublist in self.force for val in sublist]
+ self.touch_pub.publish(msg)
+ if self.is_touch == True and self.touch_type > 1 and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0):
+ # 发布矩阵压感数据JSON格式
+ self.pub_matrix_dic()
+ # 发布矩阵压感和值JSON格式
+ self.pub_matrix_mass(dic=self.matrix_dic)
+ # 发布矩阵压感点云格式
+ self.pub_matrix_point_cloud()
+ if self.hand_info_pub.get_subscription_count() > 0:
+ msg = String()
+ msg.data = json.dumps(self.last_hand_info)
+ self.hand_info_pub.publish(msg)
+ time.sleep(self.hz)
+
+ def pub_matrix_mass(self, dic):
+ """发布矩阵数据合值 单位g 克 JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_mass_dic["stamp"]["secs"] = t_secs
+ self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
+ self.matrix_mass_dic["unit"] = "g"
+ self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
+ self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
+ self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
+ self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
+ self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
+ msg.data = json.dumps(self.matrix_mass_dic)
+ self.matrix_touch_mass_pub.publish(msg)
+
+ def pub_matrix_point_cloud(self):
+ """发布矩阵数据点云格式"""
+ tmp_dic = self.matrix_dic.copy()
+ del tmp_dic['stamp'] # 去掉时间戳字段
+ all_matrices = list(tmp_dic.values()) # 5 帧，每帧 6×12=72 个数
+ # 摊平到一维：360 个 float
+ flat_list = [v for frame in all_matrices for v in frame] # 360
+ flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
+ fields = [PointField(
+ name='val',
+ offset=0,
+ datatype=PointField.UINT8,
+ count=1
+ )]
+ pc = PointCloud2()
+ pc.header.stamp = self.stamp_clock.now().to_msg()
+ pc.header.frame_id = ''
+ pc.height = 1
+ pc.width = flat.size # 360
+ pc.fields = fields
+ pc.is_bigendian = False
+ pc.point_step = 1 # 1 个 float32
+ pc.row_step = pc.point_step * pc.width
+ pc.data = flat.tobytes() # 1440 字节
+ self.matrix_touch_pub_pc.publish(pc)
+
+ def pub_matrix_dic(self):
+ """发布矩阵数据JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_dic["stamp"]["secs"] = t_secs
+ self.matrix_dic["stamp"]["nsecs"] = t_nsecs
+ msg.data = json.dumps(self.matrix_dic)
+ self.matrix_touch_pub.publish(msg)
+
+ def joint_state_msg(self, pose,vel=[]):
+ joint_state = JointState()
+ joint_state.header = Header()
+ joint_state.header.stamp = self.get_clock().now().to_msg()
+ joint_state.name = self.api.get_finger_order()
+ joint_state.position = [float(x) for x in pose]
+ if len(vel) > 1:
+ joint_state.velocity = [float(x) for x in vel]
+ else:
+ joint_state.velocity = [0.0] * len(pose)
+ joint_state.effort = [0.0] * len(pose)
+ return joint_state
+
+
+
+
+
+ def hand_setting_cb(self,msg):
+ '''控制命令回调'''
+ data = json.loads(msg.data)
+ print(f"Received setting command: {data['setting_cmd']}",flush=True)
+ try:
+ if data["params"]["hand_type"] == "left":
+ hand = self.api
+ hand_left = True
+ elif data["params"]["hand_type"] == "right":
+ hand = self.api
+ hand_right = True
+ else:
+ print("Please specify the hand part to be set",flush=True)
+ return
+ self.cmd_lock = True
+ # Set maximum torque
+ if data["setting_cmd"] == "set_max_torque_limits": # Set maximum torque
+ torque = list(data["params"]["torque"])
+ hand.set_torque(torque=torque)
+
+ if data["setting_cmd"] == "set_speed": # Set speed
+ if isinstance(data["params"]["speed"], list) == True:
+ speed = data["params"]["speed"]
+ hand.set_speed(speed=speed)
+ else:
+ ColorMsg(msg=f"Speed parameter error, speed must be a list", color="red")
+ if data["setting_cmd"] == "clear_faults": # Clear faults
+ if hand_left == True and self.hand_joint == "L10" :
+ ColorMsg(msg=f"L10 left hand cannot clear faults")
+ elif hand_right == True and self.hand_joint == "L10" :
+ ColorMsg(msg=f"L10 right hand cannot clear faults")
+ else:
+ hand.clear_faults()
+ if data["setting_cmd"] == "get_faults": # Get faults
+ f = hand.get_fault()
+ ColorMsg(msg=f"Get faults: {f}")
+ if data["setting_cmd"] == "electric_current": # Get current
+ ColorMsg(msg=f"Get current: {hand.get_current()}")
+ if data["setting_cmd"] == "set_electric_current": # Set current
+ if isinstance(data["params"]["current"], list) == True:
+ hand.set_current(data["params"]["current"])
+ if data["setting_cmd"] == "show_fun_table": # Get faults
+ f = hand.show_fun_table()
+ except:
+ print("命令参数错误")
+ self.cmd_lock = False
+ finally:
+ self.cmd_lock = False
+
+
+ def close_can(self):
+ self.api.open_can.close_can(can=self.can)
+ sys.exit(0)
+
+
+def main(args=None):
+ try:
+ rclpy.init(args=args)
+ node = LinkerHand("linker_hand_sdk")
+ embedded_version = node.embedded_version
+ if len(embedded_version) == 3 or node.hand_joint.upper() == "O6" or node.hand_joint.upper() == "L6" or node.hand_joint.upper() == "G20":
+ ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
+ node.sdk_v = 2
+ elif len(embedded_version) == 6 and node.hand_joint == "L10":
+ ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
+ node.sdk_v = 2
+ elif len(embedded_version) > 4 and ((embedded_version[0]==10 and embedded_version[4]>35) or (embedded_version[0]==7 and embedded_version[4]>50) or (embedded_version[0] == 6)):
+ ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
+ node.sdk_v = 2
+ else:
+ ColorMsg(msg=f"SDK V1", color="green")
+ node.sdk_v = 1
+ rclpy.spin(node) # 主循环，监听 ROS 回调
+ except KeyboardInterrupt:
+ print("收到 Ctrl+C，准备退出...")
+ finally:
+ # node.close_can() # 关闭 CAN 或其他硬件资源
+ # node.destroy_node() # 销毁 ROS 节点
+ # rclpy.shutdown() # 关闭 ROS
+ print("程序已退出。")
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_g20.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_g20.py
new file mode 100644
index 0000000..2bf1f83
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_g20.py
@@ -0,0 +1,258 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+from re import A
+import rclpy,sys # ROS2 Python接口库
+import time
+import argparse
+import numpy as np
+from rclpy.node import Node # ROS2 节点类
+from rclpy.clock import Clock
+from std_msgs.msg import String, Header, Float32MultiArray
+from sensor_msgs.msg import JointState, PointCloud2, PointField
+import time, json, threading
+from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
+from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
+from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
+
+# Linker Hand 型号
+HAND_JOINT = "G20"
+# 默认手指关节位置
+DEFAULT_POSITION = [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+# 默认手指关节速度
+DEFAULT_SPEED=[255, 255, 255, 255, 255]
+# 默认手指关节力矩
+DEFAULT_TORQUE = [255, 255, 255, 255, 255]
+# 压感传感器延迟时间
+TOUCH_SLEEP_TIME = 0.003
+
+
+class LinkerHandAdvancedG20(Node):
+ def __init__(self, name, hand_type, can, is_touch):
+ super().__init__(name)
+ self.hand_type = hand_type
+ self.hand_joint = HAND_JOINT
+ if is_touch == "true":
+ self.is_touch = True
+ else:
+ self.is_touch = False
+ self.can = can
+ self.modbus = "None"
+ time.sleep(0.1)
+ self._check_linker_hand_type()
+ self.last_hand_post_cmd = None # 最新手指位置命令
+ self.last_hand_vel_cmd = None # 最新手指速度命令
+ self.last_hand_eff_cmd = None # 最新手指力矩命令
+ self.matrix_dic = {
+ "stamp":{
+ "sec": 0,
+ "nanosec": 0,
+ },
+ "thumb_matrix":[[-1] * 6 for _ in range(12)],
+ "index_matrix":[[-1] * 6 for _ in range(12)],
+ "middle_matrix":[[-1] * 6 for _ in range(12)],
+ "ring_matrix":[[-1] * 6 for _ in range(12)],
+ "little_matrix":[[-1] * 6 for _ in range(12)]
+ }
+ # 压感矩阵合值，单位g 克
+ self.matrix_mass_dic = {
+ "stamp":{
+ "secs": 0,
+ "nsecs": 0,
+ },
+ "thumb_mass":[-1],
+ "index_mass":[-1],
+ "middle_mass":[-1],
+ "ring_mass":[-1],
+ "little_mass":[-1]
+ }
+ self.hz = 1.0/60.0
+ # ros时间获取
+ self.stamp_clock = Clock()
+ self._init_hand()
+ time.sleep(2)
+ self.count = 0
+ self.timer = self.create_timer(self.hz, self.run) # 100 Hz
+
+
+ def _check_linker_hand_type(self):
+ if self.modbus != "None":
+ ColorMsg(msg=f"Modbus暂不支持", color="red")
+ sys.exit(0)
+ if self.hand_joint.upper() != "G20":
+ ColorMsg(msg=f"Linker Hand hand_joint参数错误", color="red")
+ sys.exit(0)
+
+ def _init_hand(self):
+ self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
+ time.sleep(0.1)
+ self.touch_type = self.api.get_touch_type()
+ self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+ self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
+ if self.is_touch == True:
+ if self.touch_type > 1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
+ self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+ self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+ self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+ elif self.touch_type != -1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
+ self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
+ else:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
+ self.is_touch = False
+ self.embedded_version = self.api.get_embedded_version()
+ self.api.set_speed(speed=DEFAULT_SPEED)
+ time.sleep(0.1)
+ self.api.set_torque(torque=DEFAULT_TORQUE)
+ time.sleep(0.1)
+ self.api.finger_move(pose=DEFAULT_POSITION)
+ time.sleep(0.1)
+
+ def hand_control_cb(self, msg):
+ if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
+ self.last_hand_post_cmd = msg.position
+ if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
+ self.last_hand_vel_cmd = msg.velocity
+ if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
+ self.last_hand_eff_cmd = msg.effort
+
+ def list_check(self,pose):
+ if isinstance(pose, list) == False:
+ return False
+ if len(self.last_hand_post_cmd) != len(pose):
+ return False
+ return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
+
+ def joint_state_msg(self, pose,vel=[]):
+ joint_state = JointState()
+ joint_state.header = Header()
+ joint_state.header.stamp = self.get_clock().now().to_msg()
+ joint_state.name = self.api.get_finger_order()
+ joint_state.position = [float(x) for x in pose]
+ if len(vel) > 1:
+ joint_state.velocity = [float(x) for x in vel]
+ else:
+ joint_state.velocity = [0.0] * len(pose)
+ joint_state.effort = [0.0] * len(pose)
+ return joint_state
+
+ def run(self):
+ # 执行手控制指令
+ if self.last_hand_post_cmd != None:
+ self.api.finger_move(pose=self.last_hand_post_cmd)
+ self.last_hand_post_cmd = None
+ # 优先获取手指状态并且发布
+ self.last_hand_state = self.api.get_state()
+ self.last_hand_vel = [0.0] * len(self.last_hand_state)
+ # 发布手状态
+ msg_state = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
+ self.hand_state_pub.publish(msg_state)
+
+ if self.is_touch == True:
+ # 获取压感数据
+ if self.count == 2:
+ self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=TOUCH_SLEEP_TIME).tolist()
+ if self.count == 4:
+ self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=TOUCH_SLEEP_TIME).tolist()
+ if self.count == 6:
+ self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=TOUCH_SLEEP_TIME).tolist()
+ if self.count == 8:
+ self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=TOUCH_SLEEP_TIME).tolist()
+ if self.count == 10:
+ self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=TOUCH_SLEEP_TIME).tolist()
+ # 发布矩阵压感数据JSON格式
+ self.pub_matrix_dic()
+ # 发布矩阵压感和值JSON格式
+ self.pub_matrix_mass(dic=self.matrix_dic)
+ # 发布矩阵压感点云格式
+ self.pub_matrix_point_cloud()
+ self.count += 1
+ if self.count == 11:
+ self.count = 0
+
+ def pub_matrix_dic(self):
+ """发布矩阵数据JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_dic["stamp"]["secs"] = t_secs
+ self.matrix_dic["stamp"]["nsecs"] = t_nsecs
+ msg.data = json.dumps(self.matrix_dic)
+ self.matrix_touch_pub.publish(msg)
+
+ def pub_matrix_mass(self, dic):
+ """发布矩阵数据合值 单位g 克 JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_mass_dic["stamp"]["secs"] = t_secs
+ self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
+ self.matrix_mass_dic["unit"] = "g"
+ self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
+ self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
+ self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
+ self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
+ self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
+ msg.data = json.dumps(self.matrix_mass_dic)
+ self.matrix_touch_mass_pub.publish(msg)
+
+ def pub_matrix_point_cloud(self):
+ tmp_dic = self.matrix_dic.copy()
+ del tmp_dic['stamp'] # 去掉时间戳字段
+ all_matrices = list(tmp_dic.values()) # 5 帧，每帧 6×12=72 个数 or 5 帧，每帧 4×10=40 个数 列x行
+ # 摊平到一维
+ flat_list = [v for frame in all_matrices for v in frame]
+ flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
+ fields = [PointField(name='val', offset=0, datatype=PointField.UINT8, count=1)]
+ pc = PointCloud2()
+ pc.header.stamp = self.get_clock().now().to_msg()
+ pc.header.frame_id = '' # 可改成你需要的坐标系
+ pc.height = 1
+ pc.width = flat.size # 360
+ pc.fields = fields
+ pc.is_bigendian = False
+ pc.point_step = 1 # 1 个 float32
+ pc.row_step = pc.point_step * pc.width
+ pc.data = flat.tobytes() # 1440 字节
+ self.matrix_touch_pub_pc.publish(pc)
+
+
+ def close_can(self):
+ self.api.open_can.close_can(can=self.can)
+ sys.exit(0)
+
+
+def main(args=None):
+ '''
+ 本节点用于收集手指状态和压感数据。
+ '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz
+ /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 30Hz
+ '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 30Hz
+ 启动命令:
+ ros2 run linker_hand_ros2_sdk linker_hand_advanced_g20 --hand_type left --can can0 --is_touch true
+ '''
+ try:
+ rclpy.init(args=args)
+ parser = argparse.ArgumentParser()
+ parser.add_argument('--hand_type', required=True)
+ parser.add_argument('--can', required=True)
+ parser.add_argument('--is_touch', choices=['true','false'], required=True)
+
+ args = parser.parse_args()
+ node = LinkerHandAdvancedG20(name="linker_hand_advanced_g20",hand_type=args.hand_type,can=args.can,is_touch=args.is_touch)
+ embedded_version = node.embedded_version
+ rclpy.spin(node) # 主循环，监听 ROS 回调
+ except KeyboardInterrupt:
+ print("收到 Ctrl+C，准备退出...")
+ finally:
+ # node.close_can() # 关闭 CAN 或其他硬件资源
+ # node.destroy_node() # 销毁 ROS 节点
+ # rclpy.shutdown() # 关闭 ROS
+ print("程序已退出。")
\ No newline at end of file
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l10.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l10.py
new file mode 100644
index 0000000..37a7be3
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l10.py
@@ -0,0 +1,246 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+from re import A
+import rclpy,sys # ROS2 Python接口库
+import time
+import argparse
+import numpy as np
+from rclpy.node import Node # ROS2 节点类
+from rclpy.clock import Clock
+from std_msgs.msg import String, Header, Float32MultiArray
+from sensor_msgs.msg import JointState, PointCloud2, PointField
+import time, json, threading
+from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
+from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
+from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
+
+class LinkerHandAdvancedL10(Node):
+ def __init__(self, name, hand_type, can, is_touch):
+ super().__init__(name)
+ self.hand_type = hand_type
+ self.hand_joint = "L10"
+ if is_touch == "true":
+ self.is_touch = True
+ else:
+ self.is_touch = False
+ self.can = can
+ self.modbus = "None"
+ time.sleep(0.1)
+ self._check_linker_hand_type()
+ self.last_hand_post_cmd = None # 最新手指位置命令
+ self.last_hand_vel_cmd = None # 最新手指速度命令
+ self.last_hand_eff_cmd = None # 最新手指力矩命令
+ self.count = 0 # 循环计数器
+ self.matrix_dic = {
+ "stamp":{
+ "sec": 0,
+ "nanosec": 0,
+ },
+ "thumb_matrix":[[-1] * 6 for _ in range(12)],
+ "index_matrix":[[-1] * 6 for _ in range(12)],
+ "middle_matrix":[[-1] * 6 for _ in range(12)],
+ "ring_matrix":[[-1] * 6 for _ in range(12)],
+ "little_matrix":[[-1] * 6 for _ in range(12)]
+ }
+ # 压感矩阵合值，单位g 克
+ self.matrix_mass_dic = {
+ "stamp":{
+ "secs": 0,
+ "nsecs": 0,
+ },
+ "thumb_mass":[-1],
+ "index_mass":[-1],
+ "middle_mass":[-1],
+ "ring_mass":[-1],
+ "little_mass":[-1]
+ }
+ self.hz = 1.0/60.0
+ # ros时间获取
+ self.stamp_clock = Clock()
+ self._init_hand()
+ time.sleep(2)
+ self.timer = self.create_timer(self.hz, self.run) # 100 Hz
+
+
+ def _check_linker_hand_type(self):
+ if self.modbus != "None":
+ ColorMsg(msg=f"Modbus暂不支持", color="red")
+ sys.exit(0)
+ if self.hand_joint.upper() != "L10":
+ ColorMsg(msg=f"L10以外其他Linker Hand暂不支持", color="red")
+ sys.exit(0)
+
+
+ def _init_hand(self):
+ self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
+ time.sleep(0.1)
+ self.touch_type = self.api.get_touch_type()
+ self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+ self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
+ if self.is_touch == True:
+ if self.touch_type > 1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
+ self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+ self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+ self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+ elif self.touch_type != -1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
+ self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
+ else:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
+ self.is_touch = False
+ self.embedded_version = self.api.get_embedded_version()
+ if self.hand_joint.upper() == "L10":
+ pose = [255, 200, 255, 255, 255, 255, 180, 180, 180, 41]
+ torque = [255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+ speed = [255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+ self.api.set_speed(speed=speed)
+ time.sleep(0.1)
+ self.api.set_torque(torque=torque)
+ time.sleep(0.1)
+ self.api.finger_move(pose=pose)
+ time.sleep(0.1)
+ self.serial_number = self.api.get_serial_number()
+
+ def hand_control_cb(self, msg):
+ if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
+ self.last_hand_post_cmd = msg.position
+ if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
+ self.last_hand_vel_cmd = msg.velocity
+ if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
+ self.last_hand_eff_cmd = msg.effort
+
+ def list_check(self,pose):
+ if isinstance(pose, list) == False:
+ return False
+ if len(self.last_hand_post_cmd) != len(pose):
+ return False
+ return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
+
+ def joint_state_msg(self, pose,vel=[]):
+ joint_state = JointState()
+ joint_state.header = Header()
+ joint_state.header.stamp = self.get_clock().now().to_msg()
+ joint_state.name = self.api.get_finger_order()
+ joint_state.position = [float(x) for x in pose]
+ if len(vel) > 1:
+ joint_state.velocity = [float(x) for x in vel]
+ else:
+ joint_state.velocity = [0.0] * len(pose)
+ joint_state.effort = [0.0] * len(pose)
+ return joint_state
+
+ def run(self):
+ # 执行手控制指令
+ if self.last_hand_post_cmd != None:
+ self.api.finger_move(pose=self.last_hand_post_cmd)
+ self.last_hand_post_cmd = None
+ # 优先获取手指状态并且发布
+ self.last_hand_state = self.api.get_state()
+ self.last_hand_vel = [0.0] * len(self.last_hand_state)
+ # 发布手状态
+ msg_state = self.joint_state_msg(self.last_hand_state)
+ self.hand_state_pub.publish(msg_state)
+ # 获取压感数据
+ if self.is_touch == True:
+ self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=0.003).tolist()
+ self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=0.004).tolist()
+ self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=0.003).tolist()
+ self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=0.004).tolist()
+ self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=0.004).tolist()
+ # 发布矩阵压感数据JSON格式
+ self.pub_matrix_dic()
+ # 发布矩阵压感和值JSON格式
+ self.pub_matrix_mass(dic=self.matrix_dic)
+ # 发布矩阵压感点云格式
+ self.pub_matrix_point_cloud()
+
+
+ def pub_matrix_dic(self):
+ """发布矩阵数据JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_dic["stamp"]["secs"] = t_secs
+ self.matrix_dic["stamp"]["nsecs"] = t_nsecs
+ msg.data = json.dumps(self.matrix_dic)
+ self.matrix_touch_pub.publish(msg)
+
+ def pub_matrix_mass(self, dic):
+ """发布矩阵数据合值 单位g 克 JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_mass_dic["stamp"]["secs"] = t_secs
+ self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
+ self.matrix_mass_dic["unit"] = "g"
+ self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
+ self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
+ self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
+ self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
+ self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
+ msg.data = json.dumps(self.matrix_mass_dic)
+ self.matrix_touch_mass_pub.publish(msg)
+
+ def pub_matrix_point_cloud(self):
+ tmp_dic = self.matrix_dic.copy()
+ del tmp_dic['stamp'] # 去掉时间戳字段
+ all_matrices = list(tmp_dic.values()) # 5 帧，每帧 6×12=72 个数 or 5 帧，每帧 4×10=40 个数 列x行
+ # 摊平到一维
+ flat_list = [v for frame in all_matrices for v in frame]
+ flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
+ fields = [PointField(name='val', offset=0, datatype=PointField.UINT8, count=1)]
+ pc = PointCloud2()
+ pc.header.stamp = self.get_clock().now().to_msg()
+ pc.header.frame_id = '' # 可改成你需要的坐标系
+ pc.height = 1
+ pc.width = flat.size # 360
+ pc.fields = fields
+ pc.is_bigendian = False
+ pc.point_step = 1 # 1 个 float32
+ pc.row_step = pc.point_step * pc.width
+ pc.data = flat.tobytes() # 1440 字节
+ self.matrix_touch_pub_pc.publish(pc)
+
+
+ def close_can(self):
+ self.api.open_can.close_can(can=self.can)
+ sys.exit(0)
+
+
+def main(args=None):
+ '''
+ 本节点用于收集手指状态和压感数据。
+ '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz
+ /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 30Hz
+ '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 30Hz
+ '/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+ self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+ 启动命令:
+ ros2 run linker_hand_ros2_sdk linker_hand_advanced_l10 --hand_type left --can can0 --is_touch true
+ '''
+ try:
+ rclpy.init(args=args)
+ parser = argparse.ArgumentParser()
+ parser.add_argument('--hand_type', required=True)
+ parser.add_argument('--can', required=True)
+ parser.add_argument('--is_touch', choices=['true','false'], required=True)
+
+ args = parser.parse_args()
+ node = LinkerHandAdvancedL10(name="linker_hand_advanced_l10",hand_type=args.hand_type,can=args.can,is_touch=args.is_touch)
+ embedded_version = node.embedded_version
+ rclpy.spin(node) # 主循环，监听 ROS 回调
+ except KeyboardInterrupt:
+ print("收到 Ctrl+C，准备退出...")
+ finally:
+ # node.close_can() # 关闭 CAN 或其他硬件资源
+ # node.destroy_node() # 销毁 ROS 节点
+ # rclpy.shutdown() # 关闭 ROS
+ print("程序已退出。")
\ No newline at end of file
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l6.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l6.py
new file mode 100644
index 0000000..2672283
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l6.py
@@ -0,0 +1,251 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+'''
+编译: colcon build --symlink-install
+启动命令:ros2 run linker_hand_ros2_sdk linker_hand_sdk
+'''
+from re import A
+import rclpy,sys # ROS2 Python接口库
+import time
+import numpy as np
+import argparse
+from rclpy.node import Node # ROS2 节点类
+from rclpy.clock import Clock
+from std_msgs.msg import String, Header, Float32MultiArray
+from sensor_msgs.msg import JointState, PointCloud2, PointField
+import time, json, threading
+from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
+from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
+from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
+
+class LinkerHandAdvancedL6(Node):
+ def __init__(self, name, hand_type, can, is_touch):
+ super().__init__(name)
+ self.hand_type = hand_type
+ self.hand_joint = "L6"
+ if is_touch == "true":
+ self.is_touch = True
+ else:
+ self.is_touch = False
+ self.can = can
+ self.modbus = "None"
+ time.sleep(0.1)
+ self._check_linker_hand_type()
+ self.last_hand_post_cmd = None # 最新手指位置命令
+ self.last_hand_vel_cmd = None # 最新手指速度命令
+ self.last_hand_eff_cmd = None # 最新手指力矩命令
+ self.count = 0 # 循环计数器
+ self.matrix_dic = {
+ "stamp":{
+ "sec": 0,
+ "nanosec": 0,
+ },
+ "thumb_matrix":[[-1] * 6 for _ in range(12)],
+ "index_matrix":[[-1] * 6 for _ in range(12)],
+ "middle_matrix":[[-1] * 6 for _ in range(12)],
+ "ring_matrix":[[-1] * 6 for _ in range(12)],
+ "little_matrix":[[-1] * 6 for _ in range(12)]
+ }
+ # 压感矩阵合值，单位g 克
+ self.matrix_mass_dic = {
+ "stamp":{
+ "secs": 0,
+ "nsecs": 0,
+ },
+ "thumb_mass":[-1],
+ "index_mass":[-1],
+ "middle_mass":[-1],
+ "ring_mass":[-1],
+ "little_mass":[-1]
+ }
+ self.hz = 1.0/60.0
+ # ros时间获取
+ self.stamp_clock = Clock()
+ self._init_hand()
+ time.sleep(2)
+ self.timer = self.create_timer(self.hz, self.run) # 60 Hz
+
+
+ def _check_linker_hand_type(self):
+ if self.modbus != "None":
+ ColorMsg(msg=f"Modbus暂不支持", color="red")
+ sys.exit(0)
+ if self.hand_joint.upper() != "L6":
+ ColorMsg(msg=f"L6以外其他Linker Hand暂不支持", color="red")
+ sys.exit(0)
+
+ def _init_hand(self):
+ self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
+ time.sleep(0.1)
+ self.touch_type = self.api.get_touch_type()
+ self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+ self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
+ if self.is_touch == True:
+ if self.touch_type > 1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
+ self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+ self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+ self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+ elif self.touch_type != -1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
+ self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
+ else:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
+ self.is_touch = False
+ self.embedded_version = self.api.get_embedded_version()
+ if self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6":
+ pose = [200, 255, 255, 255, 255, 180]
+ torque = [255, 255, 255, 255, 255, 255]
+ # O6 最大速度阈值
+ speed = [255, 255, 255, 255, 255, 255]
+ self.api.set_speed(speed=speed)
+ time.sleep(0.1)
+ self.api.set_torque(torque=torque)
+ time.sleep(0.1)
+ self.api.finger_move(pose=pose)
+ time.sleep(0.1)
+
+ def hand_control_cb(self, msg):
+ if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
+ self.last_hand_post_cmd = msg.position
+ if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
+ self.last_hand_vel_cmd = msg.velocity
+ if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
+ self.last_hand_eff_cmd = msg.effort
+
+ def list_check(self,pose):
+ if isinstance(pose, list) == False:
+ return False
+ if len(self.last_hand_post_cmd) != len(pose):
+ return False
+ return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
+
+ def joint_state_msg(self, pose,vel=[]):
+ joint_state = JointState()
+ joint_state.header = Header()
+ joint_state.header.stamp = self.get_clock().now().to_msg()
+ joint_state.name = self.api.get_finger_order()
+ joint_state.position = [float(x) for x in pose]
+ if len(vel) > 1:
+ joint_state.velocity = [float(x) for x in vel]
+ else:
+ joint_state.velocity = [0.0] * len(pose)
+ joint_state.effort = [0.0] * len(pose)
+ return joint_state
+
+ def run(self):
+ # 执行手控制指令
+ if self.last_hand_post_cmd != None:
+ self.api.finger_move(pose=self.last_hand_post_cmd)
+ self.last_hand_post_cmd = None
+ #time.sleep(0.002)
+ # 优先获取手指状态并且发布
+ self.last_hand_state = self.api.get_state()
+ self.last_hand_vel = self.api.get_joint_speed()
+ # 发布手状态
+ msg_state = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
+ self.hand_state_pub.publish(msg_state)
+ time.sleep(0.002)
+ # 获取压感数据
+ if self.is_touch == True:
+ self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=0.003).tolist()
+ self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=0.003).tolist()
+ self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=0.003).tolist()
+ self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=0.003).tolist()
+ self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=0.003).tolist()
+ # 发布矩阵压感数据JSON格式
+ self.pub_matrix_dic()
+ # 发布矩阵压感和值JSON格式
+ self.pub_matrix_mass(dic=self.matrix_dic)
+ # 发布矩阵压感点云格式
+ self.pub_matrix_point_cloud()
+
+ def pub_matrix_dic(self):
+ """发布矩阵数据JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_dic["stamp"]["secs"] = t_secs
+ self.matrix_dic["stamp"]["nsecs"] = t_nsecs
+ msg.data = json.dumps(self.matrix_dic)
+ self.matrix_touch_pub.publish(msg)
+
+ def pub_matrix_mass(self, dic):
+ """发布矩阵数据合值 单位g 克 JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_mass_dic["stamp"]["secs"] = t_secs
+ self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
+ self.matrix_mass_dic["unit"] = "g"
+ self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
+ self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
+ self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
+ self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
+ self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
+ msg.data = json.dumps(self.matrix_mass_dic)
+ self.matrix_touch_mass_pub.publish(msg)
+
+ def pub_matrix_point_cloud(self):
+ tmp_dic = self.matrix_dic.copy()
+ del tmp_dic['stamp'] # 去掉时间戳字段
+ all_matrices = list(tmp_dic.values()) # 5 帧，每帧 6×12=72 个数 or 5 帧，每帧 4×10=40 个数 列x行
+ # 摊平到一维
+ flat_list = [v for frame in all_matrices for v in frame]
+ flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
+ fields = [PointField(name='val', offset=0, datatype=PointField.UINT8, count=1)]
+ pc = PointCloud2()
+ pc.header.stamp = self.get_clock().now().to_msg()
+ pc.header.frame_id = '' # 可改成你需要的坐标系
+ pc.height = 1
+ pc.width = flat.size # 360
+ pc.fields = fields
+ pc.is_bigendian = False
+ pc.point_step = 1 # 1 个 float32
+ pc.row_step = pc.point_step * pc.width
+ pc.data = flat.tobytes() # 1440 字节
+ self.matrix_touch_pub_pc.publish(pc)
+
+
+
+ def close_can(self):
+ self.api.open_can.close_can(can=self.can)
+ sys.exit(0)
+
+
+def main(args=None):
+ '''
+ 本节点用于收集手指状态和压感数据。
+ '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz
+ /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 40Hz
+ '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 40Hz
+ 启动命令:
+ ros2 run linker_hand_ros2_sdk linker_hand_advanced_l6 --hand_type left --can can0 --is_touch true
+ '''
+ try:
+ rclpy.init(args=args)
+ parser = argparse.ArgumentParser()
+ parser.add_argument('--hand_type', required=True)
+ parser.add_argument('--can', required=True)
+ parser.add_argument('--is_touch', choices=['true','false'], required=True)
+
+ args = parser.parse_args()
+ node = LinkerHandAdvancedL6(name="linker_hand_advanced_l6",hand_type=args.hand_type,can=args.can,is_touch=args.is_touch)
+ embedded_version = node.embedded_version
+ if embedded_version[2] < 8 and len(embedded_version) != 3:
+ ColorMsg(msg=f"固件版本过低，请升级固件到V{embedded_version[0]}.{embedded_version[1]}.8及以上版本", color="red")
+ sys.exit(0)
+ rclpy.spin(node) # 主循环，监听 ROS 回调
+ except KeyboardInterrupt:
+ print("收到 Ctrl+C，准备退出...")
+ finally:
+ # node.close_can() # 关闭 CAN 或其他硬件资源
+ # node.destroy_node() # 销毁 ROS 节点
+ # rclpy.shutdown() # 关闭 ROS
+ print("程序已退出。")
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l7.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l7.py
new file mode 100644
index 0000000..7ed58bd
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l7.py
@@ -0,0 +1,281 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+'''
+编译: colcon build --symlink-install
+启动命令:ros2 run linker_hand_ros2_sdk linker_hand_sdk
+'''
+from re import A
+import rclpy,sys # ROS2 Python接口库
+import time
+import argparse
+import numpy as np
+from rclpy.node import Node # ROS2 节点类
+from rclpy.clock import Clock
+from std_msgs.msg import String, Header, Float32MultiArray
+from sensor_msgs.msg import JointState, PointCloud2, PointField
+import time, json, threading
+from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
+from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
+from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
+
+class LinkerHandAdvancedL7(Node):
+ def __init__(self, name, hand_type, can, is_touch):
+ super().__init__(name)
+ self.hand_type = hand_type
+ self.hand_joint = "L7"
+ if is_touch == "true":
+ self.is_touch = True
+ else:
+ self.is_touch = False
+ self.can = can
+ self.modbus = "None"
+ time.sleep(0.1)
+ self._check_linker_hand_type()
+ self.last_hand_post_cmd = None # 最新手指位置命令
+ self.last_hand_vel_cmd = None # 最新手指速度命令
+ self.last_hand_eff_cmd = None # 最新手指力矩命令
+ self.count = 0 # 循环计数器
+ self.matrix_dic = {
+ "stamp":{
+ "sec": 0,
+ "nanosec": 0,
+ },
+ "thumb_matrix":[[-1] * 6 for _ in range(12)],
+ "index_matrix":[[-1] * 6 for _ in range(12)],
+ "middle_matrix":[[-1] * 6 for _ in range(12)],
+ "ring_matrix":[[-1] * 6 for _ in range(12)],
+ "little_matrix":[[-1] * 6 for _ in range(12)]
+ }
+ # 压感矩阵合值，单位g 克
+ self.matrix_mass_dic = {
+ "stamp":{
+ "secs": 0,
+ "nsecs": 0,
+ },
+ "thumb_mass":[-1],
+ "index_mass":[-1],
+ "middle_mass":[-1],
+ "ring_mass":[-1],
+ "little_mass":[-1]
+ }
+ self.hz = 1.0/60.0
+ # ros时间获取
+ self.stamp_clock = Clock()
+ self._init_hand()
+ time.sleep(2)
+ self.timer = self.create_timer(self.hz, self.run) # 100 Hz
+
+
+ def _check_linker_hand_type(self):
+ if self.modbus != "None":
+ ColorMsg(msg=f"Modbus暂不支持", color="red")
+ sys.exit(0)
+ if self.hand_joint.upper() != "L7":
+ ColorMsg(msg=f"L6以外其他Linker Hand暂不支持", color="red")
+ sys.exit(0)
+
+
+ def _init_hand(self):
+ self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
+ time.sleep(0.1)
+ self.touch_type = self.api.get_touch_type()
+ self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+ self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
+ if self.is_touch == True:
+ if self.touch_type > 1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
+ self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+ self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+ self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+ elif self.touch_type != -1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
+ self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
+ else:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
+ self.is_touch = False
+ self.embedded_version = self.api.get_embedded_version()
+ if self.hand_joint.upper() == "L7":
+ pose = [255, 200, 255, 255, 255, 255, 180]
+ torque = [255] * 7
+ speed = [255] * 7
+ self.api.set_speed(speed=speed)
+ time.sleep(0.1)
+ self.api.set_torque(torque=torque)
+ time.sleep(0.1)
+ self.api.finger_move(pose=pose)
+ time.sleep(0.1)
+ self.serial_number = self.api.get_serial_number()
+
+ def hand_control_cb(self, msg):
+ if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
+ self.last_hand_post_cmd = msg.position
+ if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
+ self.last_hand_vel_cmd = msg.velocity
+ if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
+ self.last_hand_eff_cmd = msg.effort
+
+ def list_check(self,pose):
+ if isinstance(pose, list) == False:
+ return False
+ if len(self.last_hand_post_cmd) != len(pose):
+ return False
+ return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
+
+ def joint_state_msg(self, pose,vel=[]):
+ joint_state = JointState()
+ joint_state.header = Header()
+ joint_state.header.stamp = self.get_clock().now().to_msg()
+ joint_state.name = self.api.get_finger_order()
+ joint_state.position = [float(x) for x in pose]
+ if len(vel) > 1:
+ joint_state.velocity = [float(x) for x in vel]
+ else:
+ joint_state.velocity = [0.0] * len(pose)
+ joint_state.effort = [0.0] * len(pose)
+ return joint_state
+
+ def run(self):
+ # 优先获取手指状态并且发布
+ self.last_hand_state = self.api.get_state()
+ self.last_hand_vel = self.api.get_joint_speed()
+ # 发布手状态
+ msg_state = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
+ self.hand_state_pub.publish(msg_state)
+ # 执行手控制指令
+ if self.last_hand_post_cmd != None:
+ self.api.finger_move(pose=self.last_hand_post_cmd)
+ time.sleep(0.003)
+ self.last_hand_post_cmd = None
+ if self.last_hand_vel_cmd != None:
+ vel = list(self.last_hand_vel_cmd)
+ if all(x == 0 for x in vel):
+ pass
+ else:
+ if (str(self.hand_joint).upper() == "O6" or str(self.hand_joint).upper() == "L6" or str(self.hand_joint).upper() == "L6P") and len(vel) == 6:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L7" and len(vel) == 7:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L10" and len(vel) == 10:
+ speed = [vel[0],vel[2],vel[3],vel[4],vel[5]]
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L20" and len(vel) == 20:
+ speed = [vel[10],vel[1],vel[2],vel[3],vel[4]]
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L21" and len(vel) == 25:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L25" and len(vel) == 25:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ self.last_hand_vel_cmd = None
+ time.sleep(0.005)
+ # 获取压感数据
+ if self.is_touch == True:
+ if self.count == 3:
+ self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=0.006).tolist()
+ if self.count == 4:
+ self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=0.006).tolist()
+ if self.count == 5:
+ self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=0.006).tolist()
+ if self.count == 6:
+ self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=0.006).tolist()
+ if self.count == 7:
+ self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=0.006).tolist()
+ # 发布矩阵压感数据JSON格式
+ self.pub_matrix_dic()
+ # 发布矩阵压感和值JSON格式
+ self.pub_matrix_mass(dic=self.matrix_dic)
+ # 发布矩阵压感点云格式
+ self.pub_matrix_point_cloud()
+ self.count += 1
+ if self.count == 8:
+ self.count = 0
+ time.sleep(0.006)
+
+ def pub_matrix_dic(self):
+ """发布矩阵数据JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_dic["stamp"]["secs"] = t_secs
+ self.matrix_dic["stamp"]["nsecs"] = t_nsecs
+ msg.data = json.dumps(self.matrix_dic)
+ self.matrix_touch_pub.publish(msg)
+
+ def pub_matrix_mass(self, dic):
+ """发布矩阵数据合值 单位g 克 JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_mass_dic["stamp"]["secs"] = t_secs
+ self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
+ self.matrix_mass_dic["unit"] = "g"
+ self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
+ self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
+ self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
+ self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
+ self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
+ msg.data = json.dumps(self.matrix_mass_dic)
+ self.matrix_touch_mass_pub.publish(msg)
+
+ def pub_matrix_point_cloud(self):
+ tmp_dic = self.matrix_dic.copy()
+ del tmp_dic['stamp'] # 去掉时间戳字段
+ all_matrices = list(tmp_dic.values()) # 5 帧，每帧 6×12=72 个数 or 5 帧，每帧 4×10=40 个数 列x行
+ # 摊平到一维
+ flat_list = [v for frame in all_matrices for v in frame]
+ flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
+ fields = [PointField(name='val', offset=0, datatype=PointField.UINT8, count=1)]
+ pc = PointCloud2()
+ pc.header.stamp = self.get_clock().now().to_msg()
+ pc.header.frame_id = '' # 可改成你需要的坐标系
+ pc.height = 1
+ pc.width = flat.size # 360
+ pc.fields = fields
+ pc.is_bigendian = False
+ pc.point_step = 1 # 1 个 float32
+ pc.row_step = pc.point_step * pc.width
+ pc.data = flat.tobytes() # 1440 字节
+ self.matrix_touch_pub_pc.publish(pc)
+
+
+ def close_can(self):
+ self.api.open_can.close_can(can=self.can)
+ sys.exit(0)
+
+
+def main(args=None):
+ '''
+ 本节点用于收集手指状态和压感数据。
+ '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz
+ /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 40Hz
+ '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 40Hz
+ 启动命令:
+ ros2 run linker_hand_ros2_sdk linker_hand_advanced_l7 --hand_type left --can can0 --is_touch true
+ '''
+ try:
+ rclpy.init(args=args)
+ parser = argparse.ArgumentParser()
+ parser.add_argument('--hand_type', required=True)
+ parser.add_argument('--can', required=True)
+ parser.add_argument('--is_touch', choices=['true','false'], required=True)
+
+ args = parser.parse_args()
+ node = LinkerHandAdvancedL7(name="linker_hand_collect_l7",hand_type=args.hand_type,can=args.can,is_touch=args.is_touch)
+ embedded_version = node.embedded_version
+ rclpy.spin(node) # 主循环，监听 ROS 回调
+ except KeyboardInterrupt:
+ print("收到 Ctrl+C，准备退出...")
+ finally:
+ # node.close_can() # 关闭 CAN 或其他硬件资源
+ # node.destroy_node() # 销毁 ROS 节点
+ # rclpy.shutdown() # 关闭 ROS
+ print("程序已退出。")
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_o6.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_o6.py
new file mode 100644
index 0000000..42638c8
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_o6.py
@@ -0,0 +1,252 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+'''
+编译: colcon build --symlink-install
+启动命令:ros2 run linker_hand_ros2_sdk linker_hand_sdk
+'''
+from re import A
+import rclpy,sys # ROS2 Python接口库
+import time
+import argparse
+import numpy as np
+from rclpy.node import Node # ROS2 节点类
+from rclpy.clock import Clock
+from std_msgs.msg import String, Header, Float32MultiArray
+from sensor_msgs.msg import JointState, PointCloud2, PointField
+import time, json, threading
+from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
+from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
+from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
+
+class LinkerHandAdvancedO6(Node):
+ def __init__(self, name, hand_type, can, is_touch):
+ super().__init__(name)
+ self.hand_type = hand_type
+ self.hand_joint = "O6"
+ if is_touch == "true":
+ self.is_touch = True
+ else:
+ self.is_touch = False
+ self.can = can
+ self.modbus = "None"
+ time.sleep(0.1)
+ self._check_linker_hand_type()
+ self.last_hand_post_cmd = None # 最新手指位置命令
+ self.last_hand_vel_cmd = None # 最新手指速度命令
+ self.last_hand_eff_cmd = None # 最新手指力矩命令
+ self.matrix_dic = {
+ "stamp":{
+ "sec": 0,
+ "nanosec": 0,
+ },
+ "thumb_matrix":[[-1] * 6 for _ in range(12)],
+ "index_matrix":[[-1] * 6 for _ in range(12)],
+ "middle_matrix":[[-1] * 6 for _ in range(12)],
+ "ring_matrix":[[-1] * 6 for _ in range(12)],
+ "little_matrix":[[-1] * 6 for _ in range(12)]
+ }
+ # 压感矩阵合值，单位g 克
+ self.matrix_mass_dic = {
+ "stamp":{
+ "secs": 0,
+ "nsecs": 0,
+ },
+ "thumb_mass":[-1],
+ "index_mass":[-1],
+ "middle_mass":[-1],
+ "ring_mass":[-1],
+ "little_mass":[-1]
+ }
+ self.hz = 1.0/60.0
+ # ros时间获取
+ self.stamp_clock = Clock()
+ self._init_hand()
+ time.sleep(2)
+ self.timer = self.create_timer(self.hz, self.run) # 100 Hz
+
+
+ def _check_linker_hand_type(self):
+ if self.modbus != "None":
+ ColorMsg(msg=f"Modbus暂不支持", color="red")
+ sys.exit(0)
+ if self.hand_joint.upper() != "O6":
+ ColorMsg(msg=f"O6以外其他Linker Hand暂不支持", color="red")
+ sys.exit(0)
+
+ def _init_hand(self):
+ self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
+ time.sleep(0.1)
+ self.touch_type = self.api.get_touch_type()
+ self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+ self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
+ if self.is_touch == True:
+ if self.touch_type > 1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
+ self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+ self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+ self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+ elif self.touch_type != -1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
+ self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
+ else:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
+ self.is_touch = False
+ self.embedded_version = self.api.get_embedded_version()
+ if self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6":
+ pose = [200, 255, 255, 255, 255, 180]
+ torque = [255, 255, 255, 255, 255, 255]
+ # O6 最大速度阈值
+ speed = [255, 255, 255, 255, 255, 255]
+ self.api.set_speed(speed=speed)
+ time.sleep(0.1)
+ self.api.set_torque(torque=torque)
+ time.sleep(0.1)
+ self.api.finger_move(pose=pose)
+ time.sleep(0.1)
+
+ def hand_control_cb(self, msg):
+ if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
+ self.last_hand_post_cmd = msg.position
+ if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
+ self.last_hand_vel_cmd = msg.velocity
+ if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
+ self.last_hand_eff_cmd = msg.effort
+
+ def list_check(self,pose):
+ if isinstance(pose, list) == False:
+ return False
+ if len(self.last_hand_post_cmd) != len(pose):
+ return False
+ return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
+
+ def joint_state_msg(self, pose,vel=[]):
+ joint_state = JointState()
+ joint_state.header = Header()
+ joint_state.header.stamp = self.get_clock().now().to_msg()
+ joint_state.name = self.api.get_finger_order()
+ joint_state.position = [float(x) for x in pose]
+ if len(vel) > 1:
+ joint_state.velocity = [float(x) for x in vel]
+ else:
+ joint_state.velocity = [0.0] * len(pose)
+ joint_state.effort = [0.0] * len(pose)
+ return joint_state
+
+ def run(self):
+ # 执行手控制指令
+ if self.last_hand_post_cmd != None:
+ self.api.finger_move(pose=self.last_hand_post_cmd)
+ self.last_hand_post_cmd = None
+ if self.last_hand_vel_cmd != None:
+ vel = list(self.last_hand_vel_cmd)
+ if all(x == 0 for x in vel):
+ pass
+ else:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ self.last_hand_vel_cmd = None
+ # 优先获取手指状态并且发布
+ self.last_hand_state = self.api.get_state()
+ self.last_hand_vel = self.api.get_joint_speed()
+ # 发布手状态
+ msg_state = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
+ self.hand_state_pub.publish(msg_state)
+ if self.is_touch == True:
+ # 获取压感数据
+ self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=0.002).tolist()
+ self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=0.002).tolist()
+ self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=0.002).tolist()
+ self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=0.002).tolist()
+ self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=0.002).tolist()
+ # 发布矩阵压感数据JSON格式
+ self.pub_matrix_dic()
+ # 发布矩阵压感和值JSON格式
+ self.pub_matrix_mass(dic=self.matrix_dic)
+ # 发布矩阵压感点云格式
+ self.pub_matrix_point_cloud()
+
+ def pub_matrix_dic(self):
+ """发布矩阵数据JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_dic["stamp"]["secs"] = t_secs
+ self.matrix_dic["stamp"]["nsecs"] = t_nsecs
+ msg.data = json.dumps(self.matrix_dic)
+ self.matrix_touch_pub.publish(msg)
+
+ def pub_matrix_mass(self, dic):
+ """发布矩阵数据合值 单位g 克 JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_mass_dic["stamp"]["secs"] = t_secs
+ self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
+ self.matrix_mass_dic["unit"] = "g"
+ self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
+ self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
+ self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
+ self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
+ self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
+ msg.data = json.dumps(self.matrix_mass_dic)
+ self.matrix_touch_mass_pub.publish(msg)
+
+ def pub_matrix_point_cloud(self):
+ tmp_dic = self.matrix_dic.copy()
+ del tmp_dic['stamp'] # 去掉时间戳字段
+ all_matrices = list(tmp_dic.values()) # 5 帧，每帧 6×12=72 个数 or 5 帧，每帧 4×10=40 个数 列x行
+ # 摊平到一维
+ flat_list = [v for frame in all_matrices for v in frame]
+ flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
+ fields = [PointField(name='val', offset=0, datatype=PointField.UINT8, count=1)]
+ pc = PointCloud2()
+ pc.header.stamp = self.get_clock().now().to_msg()
+ pc.header.frame_id = '' # 可改成你需要的坐标系
+ pc.height = 1
+ pc.width = flat.size # 360
+ pc.fields = fields
+ pc.is_bigendian = False
+ pc.point_step = 1 # 1 个 float32
+ pc.row_step = pc.point_step * pc.width
+ pc.data = flat.tobytes() # 1440 字节
+ self.matrix_touch_pub_pc.publish(pc)
+
+
+ def close_can(self):
+ self.api.open_can.close_can(can=self.can)
+ sys.exit(0)
+
+
+def main(args=None):
+ '''
+ 本节点用于收集手指状态和压感数据。
+ '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz
+ /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 50Hz
+ '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 50Hz
+ 启动命令:
+ ros2 run linker_hand_ros2_sdk linker_hand_advanced_o6 --hand_type right --can can0 --is_touch true
+ '''
+ try:
+ rclpy.init(args=args)
+ parser = argparse.ArgumentParser()
+ parser.add_argument('--hand_type', required=True)
+ parser.add_argument('--can', required=True)
+ parser.add_argument('--is_touch', choices=['true','false'], required=True)
+
+ args = parser.parse_args()
+ node = LinkerHandAdvancedO6(name="linker_hand_advanced_o6",hand_type=args.hand_type,can=args.can,is_touch=args.is_touch)
+ embedded_version = node.embedded_version
+ rclpy.spin(node) # 主循环，监听 ROS 回调
+ except KeyboardInterrupt:
+ print("收到 Ctrl+C，准备退出...")
+ finally:
+ # node.close_can() # 关闭 CAN 或其他硬件资源
+ # node.destroy_node() # 销毁 ROS 节点
+ # rclpy.shutdown() # 关闭 ROS
+ print("程序已退出。")
\ No newline at end of file
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_g20_palm_touch.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_g20_palm_touch.py
new file mode 100644
index 0000000..eda447c
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_g20_palm_touch.py
@@ -0,0 +1,268 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+from re import A
+import rclpy,sys # ROS2 Python接口库
+import time
+import argparse
+import numpy as np
+from rclpy.node import Node # ROS2 节点类
+from rclpy.clock import Clock
+from std_msgs.msg import String, Header, Float32MultiArray
+from sensor_msgs.msg import JointState, PointCloud2, PointField
+import time, json, threading
+from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
+from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
+from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
+
+# Linker Hand 型号
+HAND_JOINT = "G20"
+# 默认手指关节位置
+DEFAULT_POSITION = [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+# 默认手指关节速度
+DEFAULT_SPEED=[255, 255, 255, 255, 255]
+# 默认手指关节力矩
+DEFAULT_TORQUE = [255, 255, 255, 255, 255]
+# 压感传感器延迟时间
+TOUCH_SLEEP_TIME = 0.003
+
+
+
+class LinkerHandAdvancedG20(Node):
+ def __init__(self, name, hand_type, can, is_touch):
+ super().__init__(name)
+ self.hand_type = hand_type
+ self.hand_joint = HAND_JOINT
+ if is_touch == "true":
+ self.is_touch = True
+ else:
+ self.is_touch = False
+ self.can = can
+ self.modbus = "None"
+ time.sleep(0.1)
+ self._check_linker_hand_type()
+ self.last_hand_post_cmd = None # 最新手指位置命令
+ self.last_hand_vel_cmd = None # 最新手指速度命令
+ self.last_hand_eff_cmd = None # 最新手指力矩命令
+ self.matrix_dic = {
+ "stamp":{
+ "sec": 0,
+ "nanosec": 0,
+ },
+ "thumb_matrix":[[-1] * 6 for _ in range(12)],
+ "index_matrix":[[-1] * 6 for _ in range(12)],
+ "middle_matrix":[[-1] * 6 for _ in range(12)],
+ "ring_matrix":[[-1] * 6 for _ in range(12)],
+ "little_matrix":[[-1] * 6 for _ in range(12)]
+ }
+ # 压感矩阵合值，单位g 克
+ self.matrix_mass_dic = {
+ "stamp":{
+ "secs": 0,
+ "nsecs": 0,
+ },
+ "thumb_mass":[-1],
+ "index_mass":[-1],
+ "middle_mass":[-1],
+ "ring_mass":[-1],
+ "little_mass":[-1]
+ }
+ self.hz = 1.0/60.0
+ # ros时间获取
+ self.stamp_clock = Clock()
+ self._init_hand()
+ time.sleep(2)
+ self.count = 0
+ self.timer = self.create_timer(self.hz, self.run)
+
+
+ def _check_linker_hand_type(self):
+ if self.modbus != "None":
+ ColorMsg(msg=f"Modbus暂不支持", color="red")
+ sys.exit(0)
+ if self.hand_joint.upper() != "G20":
+ ColorMsg(msg=f"Linker Hand hand_joint参数错误", color="red")
+ sys.exit(0)
+
+ def _init_hand(self):
+ self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
+ time.sleep(0.1)
+ self.touch_type = self.api.get_touch_type()
+ self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+ self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
+ if self.is_touch == True:
+ if self.touch_type > 1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
+ self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+ #self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+ self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+ elif self.touch_type != -1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
+ self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
+ else:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
+ self.is_touch = False
+ self.embedded_version = self.api.get_embedded_version()
+ self.api.set_speed(speed=DEFAULT_SPEED)
+ time.sleep(0.1)
+ self.api.set_torque(torque=DEFAULT_TORQUE)
+ time.sleep(0.1)
+ self.api.finger_move(pose=DEFAULT_POSITION)
+ time.sleep(0.1)
+ self.palm_touch = self.api.is_palm_touch
+ if self.palm_touch == 5:
+ self.touch_sleep_time = 0.03
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} 全掌压感版", color="green")
+ else:
+ self.touch_sleep_time = 0.003
+
+ def hand_control_cb(self, msg):
+ if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
+ self.last_hand_post_cmd = msg.position
+ if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
+ self.last_hand_vel_cmd = msg.velocity
+ if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
+ self.last_hand_eff_cmd = msg.effort
+
+ def list_check(self,pose):
+ if isinstance(pose, list) == False:
+ return False
+ if len(self.last_hand_post_cmd) != len(pose):
+ return False
+ return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
+
+ def joint_state_msg(self, pose,vel=[]):
+ joint_state = JointState()
+ joint_state.header = Header()
+ joint_state.header.stamp = self.get_clock().now().to_msg()
+ joint_state.name = self.api.get_finger_order()
+ joint_state.position = [float(x) for x in pose]
+ if len(vel) > 1:
+ joint_state.velocity = [float(x) for x in vel]
+ else:
+ joint_state.velocity = [0.0] * len(pose)
+ joint_state.effort = [0.0] * len(pose)
+ return joint_state
+
+ def run(self):
+ # 执行手控制指令
+ if self.last_hand_post_cmd != None:
+ self.api.finger_move(pose=self.last_hand_post_cmd)
+ self.last_hand_post_cmd = None
+ # 优先获取手指状态并且发布
+ self.last_hand_state = self.api.get_state()
+ self.last_hand_vel = [0.0] * len(self.last_hand_state)
+ # 发布手状态
+ msg_state = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
+ self.hand_state_pub.publish(msg_state)
+
+ if self.is_touch == True:
+ # 获取压感数据
+ if self.count == 2:
+ self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=self.touch_sleep_time).tolist()
+ if self.count == 4:
+ self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=self.touch_sleep_time).tolist()
+ if self.count == 6:
+ self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=self.touch_sleep_time).tolist()
+ if self.count == 8:
+ self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=self.touch_sleep_time).tolist()
+ if self.count == 10:
+ self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=self.touch_sleep_time).tolist()
+ if self.count == 14 and self.palm_touch == 5:
+ self.matrix_dic["palm_matrix"] = self.api.get_palm_matrix_touch(sleep_time=self.touch_sleep_time).tolist()
+ # 发布矩阵压感数据JSON格式
+ self.pub_matrix_dic()
+ # 发布矩阵压感和值JSON格式
+ self.pub_matrix_mass(dic=self.matrix_dic)
+ # 发布矩阵压感点云格式
+ #self.pub_matrix_point_cloud()
+ self.count += 1
+ if self.count == 15:
+ self.count = 0
+
+ def pub_matrix_dic(self):
+ """发布矩阵数据JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_dic["stamp"]["secs"] = t_secs
+ self.matrix_dic["stamp"]["nsecs"] = t_nsecs
+ msg.data = json.dumps(self.matrix_dic)
+ self.matrix_touch_pub.publish(msg)
+
+ def pub_matrix_mass(self, dic):
+ """发布矩阵数据合值 单位g 克 JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_mass_dic["stamp"]["secs"] = t_secs
+ self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
+ self.matrix_mass_dic["unit"] = "g"
+ self.matrix_mass_dic["thumb_mass"] = self.api.hand.thumb_matrix_palm_mass
+ self.matrix_mass_dic["index_mass"] = self.api.hand.index_matrix_palm_mass
+ self.matrix_mass_dic["middle_mass"] = self.api.hand.middle_matrix_palm_mass
+ self.matrix_mass_dic["ring_mass"] = self.api.hand.ring_matrix_palm_mass
+ self.matrix_mass_dic["little_mass"] = self.api.hand.little_matrix_palm_mass
+ self.matrix_mass_dic["palm_mass"] = self.api.hand.palm_matrix_palm_mass
+ msg.data = json.dumps(self.matrix_mass_dic)
+ self.matrix_touch_mass_pub.publish(msg)
+
+ # def pub_matrix_point_cloud(self):
+ # tmp_dic = self.matrix_dic.copy()
+ # del tmp_dic['stamp'] # 去掉时间戳字段
+ # all_matrices = list(tmp_dic.values()) # 5 帧，每帧 6×12=72 个数 or 5 帧，每帧 4×10=40 个数 列x行
+ # # 摊平到一维
+ # flat_list = [v for frame in all_matrices for v in frame]
+ # flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
+ # fields = [PointField(name='val', offset=0, datatype=PointField.UINT8, count=1)]
+ # pc = PointCloud2()
+ # pc.header.stamp = self.get_clock().now().to_msg()
+ # pc.header.frame_id = '' # 可改成你需要的坐标系
+ # pc.height = 1
+ # pc.width = flat.size # 360
+ # pc.fields = fields
+ # pc.is_bigendian = False
+ # pc.point_step = 1 # 1 个 float32
+ # pc.row_step = pc.point_step * pc.width
+ # pc.data = flat.tobytes() # 1440 字节
+ # self.matrix_touch_pub_pc.publish(pc)
+
+
+ def close_can(self):
+ self.api.open_can.close_can(can=self.can)
+ sys.exit(0)
+
+
+def main(args=None):
+ '''
+ 本节点用于收集手指状态和压感数据。
+ '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz
+ /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 30Hz
+ '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 30Hz
+ 启动命令:
+ ros2 run linker_hand_ros2_sdk linker_hand_g20_palm_touch --hand_type left --can can0 --is_touch true
+ '''
+ try:
+ rclpy.init(args=args)
+ parser = argparse.ArgumentParser()
+ parser.add_argument('--hand_type', required=True)
+ parser.add_argument('--can', required=True)
+ parser.add_argument('--is_touch', choices=['true','false'], required=True)
+
+ args = parser.parse_args()
+ node = LinkerHandAdvancedG20(name="linker_hand_g20_palm_touch",hand_type=args.hand_type,can=args.can,is_touch=args.is_touch)
+ embedded_version = node.embedded_version
+ rclpy.spin(node) # 主循环，监听 ROS 回调
+ except KeyboardInterrupt:
+ print("收到 Ctrl+C，准备退出...")
+ finally:
+ node.close_can() # 关闭 CAN 或其他硬件资源
+ # node.destroy_node() # 销毁 ROS 节点
+ # rclpy.shutdown() # 关闭 ROS
+ print("程序已退出。")
\ No newline at end of file
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/package.xml b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/package.xml
new file mode 100644
index 0000000..665b626
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/package.xml
@@ -0,0 +1,21 @@
+
+
+
+ linker_hand_ros2_sdk
+ 0.0.0
+ TODO: Package description
+ linker-robot
+ TODO: License declaration
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+
+ rclpy
+ launch
+
+
+ ament_python
+
+
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/pyproject.toml b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/pyproject.toml
new file mode 100644
index 0000000..638dd9c
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/pyproject.toml
@@ -0,0 +1,3 @@
+[build-system]
+requires = ["setuptools>=61.0"]
+build-backend = "setuptools.build_meta"
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/resource/linker_hand_ros2_sdk b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/resource/linker_hand_ros2_sdk
new file mode 100644
index 0000000..e69de29
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/setup.cfg b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/setup.cfg
new file mode 100644
index 0000000..eb74b8f
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/linker_hand_ros2_sdk
+[install]
+install_scripts=$base/lib/linker_hand_ros2_sdk
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/setup.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/setup.py
new file mode 100644
index 0000000..2e4ed2e
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/setup.py
@@ -0,0 +1,50 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+import os
+from glob import glob
+from setuptools import find_packages, setup
+
+package_name = 'linker_hand_ros2_sdk'
+
+this_dir = os.path.abspath(os.path.dirname(__file__))
+custom_dir = os.path.join(this_dir, package_name, "LinkerHand")
+
+data_files = [
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
+]
+
+# for root, dirs, files in os.walk(custom_dir):
+# if files:
+# relative_path = os.path.relpath(root, os.path.join(this_dir, package_name))
+# target_path = os.path.join('share', package_name, relative_path)
+# # 修复这里：路径必须是相对路径
+# files_full_path = [os.path.relpath(os.path.join(root, f), start=os.getcwd()) for f in files]
+# data_files.append((target_path, files_full_path))
+
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=find_packages(include=[package_name, f"{package_name}.*"]),
+ data_files=data_files,
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='linker-robot',
+ maintainer_email='linker-robot@todo.todo',
+ description='ROS2 SDK for Linker Hand',
+ license='TODO: License declaration',
+ entry_points={
+ 'console_scripts': [
+ 'linker_hand_sdk = linker_hand_ros2_sdk.linker_hand:main',
+ 'linker_hand_advanced_o6 = linker_hand_ros2_sdk.linker_hand_advanced_o6:main',
+ 'linker_hand_advanced_l6 = linker_hand_ros2_sdk.linker_hand_advanced_l6:main',
+ 'linker_hand_advanced_l7 = linker_hand_ros2_sdk.linker_hand_advanced_l7:main',
+ 'linker_hand_advanced_l10 = linker_hand_ros2_sdk.linker_hand_advanced_l10:main',
+ 'linker_hand_advanced_g20 = linker_hand_ros2_sdk.linker_hand_advanced_g20:main',
+ 'linker_hand_g20_palm_touch = linker_hand_ros2_sdk.linker_hand_g20_palm_touch:main',
+ ],
+ },
+)
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/test/test_copyright.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/test/test_copyright.py
new file mode 100644
index 0000000..97a3919
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/test/test_copyright.py
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found errors'
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/test/test_flake8.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/test/test_flake8.py
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+ rc, errors = main_with_errors(argv=[])
+ assert rc == 0, \
+ 'Found %d code style errors / warnings:\n' % len(errors) + \
+ '\n'.join(errors)
diff --git a/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/test/test_pep257.py b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/test/test_pep257.py
new file mode 100644
index 0000000..b234a38
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/test/test_pep257.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found code style errors / warnings'
diff --git a/linker_hand_ros2_sdk_ws/src/pressure_diagram/launch/pressure_diagram.launch.py b/linker_hand_ros2_sdk_ws/src/pressure_diagram/launch/pressure_diagram.launch.py
new file mode 100644
index 0000000..34a5800
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/pressure_diagram/launch/pressure_diagram.launch.py
@@ -0,0 +1,17 @@
+#!/usr/bin/env python3
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+ return LaunchDescription([
+ Node(
+ package='pressure_diagram',
+ executable='pressure_diagram',
+ name='pressure_diagram_node',
+ output='screen',
+ parameters=[{
+
+ }],
+ ),
+
+ ])
\ No newline at end of file
diff --git a/linker_hand_ros2_sdk_ws/src/pressure_diagram/package.xml b/linker_hand_ros2_sdk_ws/src/pressure_diagram/package.xml
new file mode 100644
index 0000000..15650cc
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/pressure_diagram/package.xml
@@ -0,0 +1,28 @@
+
+
+
+ pressure_diagram
+ 0.0.0
+ ROS2 Pressure Diagram - Real-time pressure sensor visualization for Linker Hand
+ linkerhand
+ Apache-2.0
+
+
+ rclpy
+ std_msgs
+ ament_index_python
+
+
+ python3-pyqt5
+ python3-pyqtgraph
+ python3-numpy
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+
+
+ ament_python
+
+
diff --git a/linker_hand_ros2_sdk_ws/src/pressure_diagram/pressure_diagram/__init__.py b/linker_hand_ros2_sdk_ws/src/pressure_diagram/pressure_diagram/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/linker_hand_ros2_sdk_ws/src/pressure_diagram/pressure_diagram/pressure_diagram.py b/linker_hand_ros2_sdk_ws/src/pressure_diagram/pressure_diagram/pressure_diagram.py
new file mode 100644
index 0000000..9e78244
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/pressure_diagram/pressure_diagram/pressure_diagram.py
@@ -0,0 +1,421 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+import signal
+import sys
+import json
+import numpy as np
+from std_msgs.msg import String
+from PyQt5 import QtWidgets, QtCore, QtGui
+import pyqtgraph as pg
+
+pg.setConfigOption('imageAxisOrder', 'row-major')
+pg.setConfigOption('useOpenGL', False)
+
+class PressureDiagram(Node, QtWidgets.QMainWindow):
+ def __init__(self):
+ # 先调用 Node 的 __init__，避免 super() 歧义
+ Node.__init__(self, 'pressure_diagram')
+ QtWidgets.QMainWindow.__init__(self)
+
+ # 键名与ROS数据一致
+ self.fingers_matrix = ['thumb_matrix', 'index_matrix', 'middle_matrix', 'ring_matrix', 'little_matrix']
+ self.fingers_mass = ['thumb_mass', 'index_mass', 'middle_mass', 'ring_mass', 'little_mass']
+
+ self.finger_colors = [
+ (255, 75, 75), # Thumb - Red
+ (50, 205, 50), # Index - Green
+ (65, 105, 255), # Middle - Blue
+ (255, 215, 0), # Ring - Gold
+ (218, 112, 214) # Pinky - Purple
+ ]
+
+ # 波形图数据
+ self.wave_data = {f: np.zeros(100) for f in self.fingers_mass}
+ self.current_wave_values = {f: 0.0 for f in self.fingers_mass}
+
+ # 热力图数据 - 12行×6列 (高大于宽)
+ self.matrix_data = {f: np.zeros((12, 6)) for f in self.fingers_matrix}
+ self.matrix_shapes = {f: (12, 6) for f in self.fingers_matrix} # (rows, cols)
+ self.data_received = {f: False for f in self.fingers_matrix}
+
+ self.wave_sub = None
+ self.matrix_sub = None
+
+ # ROS2 节流计时器
+ self.last_matrix_log_time = {f: None for f in self.fingers_matrix}
+
+ # QoS 配置
+ qos_profile = QoSProfile(
+ reliability=ReliabilityPolicy.BEST_EFFORT,
+ history=HistoryPolicy.KEEP_LAST,
+ depth=100
+ )
+ self.qos_profile = qos_profile
+
+ self.init_ui()
+ self.setup_ros()
+
+ def init_ui(self):
+ self.setWindowTitle("Robotic Hand Sensor Fusion Interface")
+ self.resize(1600, 900)
+ self.setStyleSheet("background-color: #0f172a;")
+
+ central_widget = QtWidgets.QWidget()
+ self.setCentralWidget(central_widget)
+
+ main_layout = QtWidgets.QHBoxLayout(central_widget)
+ main_layout.setSpacing(15)
+ main_layout.setContentsMargins(10, 10, 10, 10)
+
+ # ============== 左侧：波形图 ==============
+ left_widget = self.create_waveform_panel()
+ main_layout.addWidget(left_widget, stretch=1)
+
+ line = QtWidgets.QFrame()
+ line.setFrameShape(QtWidgets.QFrame.VLine)
+ line.setStyleSheet("QFrame { background-color: #334155; max-width: 2px; }")
+ main_layout.addWidget(line)
+
+ # ============== 右侧：热力图 ==============
+ right_widget = self.create_heatmap_panel()
+ main_layout.addWidget(right_widget, stretch=1)
+
+ self.timer = QtCore.QTimer()
+ self.timer.timeout.connect(self.update_display)
+ self.timer.start(33)
+
+ def create_waveform_panel(self):
+ panel = QtWidgets.QWidget()
+ layout = QtWidgets.QVBoxLayout(panel)
+ layout.setSpacing(8)
+
+ wave_ctrl = QtWidgets.QFrame()
+ wave_ctrl.setStyleSheet("""QFrame { background-color: #1e293b; border: 2px solid #3b82f6; border-radius: 4px; }""")
+ wave_layout = QtWidgets.QHBoxLayout(wave_ctrl)
+ wave_layout.setContentsMargins(12, 6, 12, 6)
+
+ title = QtWidgets.QLabel("WAVEFORM MONITOR (30Hz)")
+ title.setStyleSheet("color: #60a5fa; font-weight: bold; font-size: 13px;")
+
+ self.wave_combo = QtWidgets.QComboBox()
+ self.wave_combo.addItems([
+ "/cb_left_hand_matrix_touch_mass",
+ "/cb_right_hand_matrix_touch_mass"
+ ])
+ self.wave_combo.setStyleSheet("""
+ QComboBox { color: white; background-color: #334155; border: 1px solid #475569; border-radius: 4px; padding: 4px 8px; min-width: 260px; }
+ QComboBox QAbstractItemView { background-color: #1e293b; color: white; selection-background-color: #3b82f6; }
+ """)
+ self.wave_combo.currentTextChanged.connect(self.switch_wave_topic)
+
+ wave_layout.addWidget(title)
+ wave_layout.addWidget(self.wave_combo)
+ wave_layout.addStretch()
+ layout.addWidget(wave_ctrl)
+
+ self.wave_widget = pg.GraphicsLayoutWidget()
+ self.wave_widget.setBackground('#0f172a')
+ layout.addWidget(self.wave_widget, stretch=1)
+
+ self.wave_plots = {}
+ self.wave_curves = {}
+
+ for i, finger in enumerate(self.fingers_mass):
+ p = self.wave_widget.addPlot(row=i, col=0)
+ p.setMenuEnabled(False)
+ p.setMouseEnabled(x=False, y=False)
+ p.setYRange(0, 4500)
+ p.setXRange(0, 100)
+ p.showGrid(x=True, y=True, alpha=0.3)
+ p.getAxis('left').setTextPen('#64748b')
+ p.getAxis('bottom').setTextPen('#64748b')
+
+ color = QtGui.QColor(*self.finger_colors[i])
+ display_name = finger.replace('_mass', '').upper()
+ p.setTitle(f"[ {display_name} ]", color=color, size='11pt')
+
+ if i == 4:
+ p.getAxis('bottom').setLabel('Time', color='#64748b')
+ else:
+ p.getAxis('bottom').setStyle(showValues=False)
+
+ pen = pg.mkPen(color=self.finger_colors[i], width=2.5)
+ curve = p.plot(pen=pen)
+ self.wave_curves[finger] = curve
+
+ text = pg.TextItem(text="0", color=(255,255,255), anchor=(1, 0.5))
+ text.setFont(QtGui.QFont("Arial", 10, QtGui.QFont.Bold))
+ p.addItem(text)
+ self.wave_curves[finger + '_text'] = text
+
+ return panel
+
+ def create_heatmap_panel(self):
+ panel = QtWidgets.QWidget()
+ layout = QtWidgets.QVBoxLayout(panel)
+ layout.setSpacing(8)
+
+ heat_ctrl = QtWidgets.QFrame()
+ heat_ctrl.setStyleSheet("""QFrame { background-color: #1e293b; border: 2px solid #f43f5e; border-radius: 4px; }""")
+ heat_layout = QtWidgets.QHBoxLayout(heat_ctrl)
+ heat_layout.setContentsMargins(12, 6, 12, 6)
+
+ title = QtWidgets.QLabel("PRESSURE MATRIX (12×6)")
+ title.setStyleSheet("color: #f43f5e; font-weight: bold; font-size: 13px;")
+
+ # 单位标签
+ unit_label = QtWidgets.QLabel("Max Value (g)")
+ unit_label.setStyleSheet("color: #94a3b8; font-size: 11px; border: none; background: transparent;")
+ heat_layout.addWidget(unit_label)
+ heat_layout.addSpacing(10)
+
+ self.matrix_combo = QtWidgets.QComboBox()
+ self.matrix_combo.addItems([
+ "/cb_left_hand_matrix_touch",
+ "/cb_right_hand_matrix_touch"
+ ])
+ self.matrix_combo.setStyleSheet("""
+ QComboBox { color: white; background-color: #334155; border: 1px solid #475569; border-radius: 4px; padding: 4px 8px; min-width: 260px; }
+ QComboBox QAbstractItemView { background-color: #1e293b; color: white; selection-background-color: #f43f5e; }
+ """)
+ self.matrix_combo.currentTextChanged.connect(self.switch_matrix_topic)
+
+ heat_layout.addWidget(title)
+ heat_layout.addWidget(self.matrix_combo)
+ heat_layout.addStretch()
+ layout.addWidget(heat_ctrl)
+
+ heat_container = QtWidgets.QWidget()
+ heat_grid = QtWidgets.QGridLayout(heat_container)
+ heat_grid.setSpacing(10)
+ heat_grid.setContentsMargins(5, 5, 5, 5)
+
+ self.matrix_plots = {}
+ self.matrix_images = {}
+ self.matrix_peaks = {}
+
+ positions = [
+ ('thumb_matrix', 0, 0), ('index_matrix', 0, 1), ('middle_matrix', 0, 2),
+ ('ring_matrix', 1, 0), ('little_matrix', 1, 1)
+ ]
+
+ self.colormap = pg.colormap.get('plasma')
+
+ for idx, (finger, row, col) in enumerate(positions):
+ container = QtWidgets.QWidget()
+ vbox = QtWidgets.QVBoxLayout(container)
+ vbox.setSpacing(2)
+ vbox.setContentsMargins(0, 0, 0, 0)
+
+ display_name = finger.replace('_matrix', '').upper()
+ name_label = QtWidgets.QLabel(display_name)
+ name_label.setAlignment(QtCore.Qt.AlignCenter)
+ color_hex = '#{:02x}{:02x}{:02x}'.format(*self.finger_colors[idx])
+ name_label.setStyleSheet(f"color: {color_hex}; font-weight: bold; font-size: 12px;")
+ vbox.addWidget(name_label)
+
+ # 固定比例显示 12×6 (高:宽 = 2:1)
+ view = pg.PlotWidget()
+ view.setMenuEnabled(False)
+ view.setMouseEnabled(x=False, y=False)
+ view.setAspectLocked(True, ratio=6/12) # X:Y = 1:2
+ view.setMaximumSize(240, 280)
+ view.setMinimumSize(120, 160)
+ view.setBackground('#0f172a')
+ view.hideAxis('left')
+ view.hideAxis('bottom')
+
+ img = pg.ImageItem()
+ img.setLookupTable(self.colormap.getLookupTable())
+ img.setImage(np.zeros((12, 6)), levels=[0, 100])
+ view.addItem(img)
+
+ peak_text = pg.TextItem(text="", color=(255,255,255), anchor=(0.5, 0.5))
+ peak_text.setFont(QtGui.QFont("Arial", 10, QtGui.QFont.Bold))
+ view.addItem(peak_text)
+
+ vbox.addWidget(view, stretch=1)
+ heat_grid.addWidget(container, row, col)
+
+ self.matrix_plots[finger] = view
+ self.matrix_images[finger] = img
+ self.matrix_peaks[finger] = peak_text
+
+ # 颜色条
+ legend_widget = QtWidgets.QWidget()
+ legend_layout = QtWidgets.QVBoxLayout(legend_widget)
+ legend_layout.setAlignment(QtCore.Qt.AlignCenter)
+
+ lbl_max = QtWidgets.QLabel("MAX")
+ lbl_max.setStyleSheet("color: #fbbf24; font-size: 9px;")
+ lbl_max.setAlignment(QtCore.Qt.AlignCenter)
+
+ gradient = pg.GradientWidget(orientation='right')
+ gradient.setMaximumWidth(25)
+ gradient.setMaximumHeight(180)
+ gradient.setColorMap(self.colormap)
+
+ lbl_min = QtWidgets.QLabel("0")
+ lbl_min.setStyleSheet("color: #3b82f6; font-size: 9px;")
+ lbl_min.setAlignment(QtCore.Qt.AlignCenter)
+
+ legend_layout.addWidget(lbl_max)
+ legend_layout.addWidget(gradient, stretch=1)
+ legend_layout.addWidget(lbl_min)
+
+ heat_grid.addWidget(legend_widget, 1, 2)
+
+ layout.addWidget(heat_container, stretch=1)
+ return panel
+
+ def setup_ros(self):
+ self.get_logger().info("Setting up ROS...")
+ self.subscribe_waveform("/cb_left_hand_matrix_touch_mass")
+ self.subscribe_matrix("/cb_left_hand_matrix_touch")
+
+ def subscribe_waveform(self, topic):
+ if self.wave_sub:
+ self.destroy_subscription(self.wave_sub)
+ self.wave_sub = self.create_subscription(String, topic, self.wave_callback, self.qos_profile)
+ self.get_logger().info(f"WAVEFORM: {topic}")
+
+ def subscribe_matrix(self, topic):
+ if self.matrix_sub:
+ self.destroy_subscription(self.matrix_sub)
+ self.matrix_sub = self.create_subscription(String, topic, self.matrix_callback, self.qos_profile)
+ self.get_logger().info(f"MATRIX: {topic}")
+
+ def switch_wave_topic(self, topic):
+ self.subscribe_waveform(topic)
+ for f in self.fingers_mass:
+ self.wave_data[f] = np.zeros(100)
+ self.current_wave_values[f] = 0.0
+
+ def switch_matrix_topic(self, topic):
+ self.subscribe_matrix(topic)
+ for f in self.fingers_matrix:
+ self.matrix_data[f] = np.zeros((12, 6))
+ self.matrix_shapes[f] = (12, 6)
+ self.data_received[f] = False
+
+ def wave_callback(self, msg):
+ try:
+ data = json.loads(msg.data)
+ for finger in self.fingers_mass:
+ if finger in data:
+ self.current_wave_values[finger] = float(data[finger])
+ except Exception as e:
+ pass
+
+ def matrix_callback(self, msg):
+ """处理 12×6 矩阵数据"""
+ try:
+ data = json.loads(msg.data)
+
+ for finger in self.fingers_matrix:
+ if finger not in data:
+ continue
+
+ arr = np.array(data[finger], dtype=np.float32)
+
+ if arr.ndim == 2:
+ rows, cols = arr.shape
+ self.data_received[finger] = True
+ self.matrix_data[finger] = arr
+ self.matrix_shapes[finger] = (rows, cols)
+
+ # ROS2 节流日志
+ current_time = self.get_clock().now()
+ if self.last_matrix_log_time[finger] is None or \
+ (current_time - self.last_matrix_log_time[finger]).nanoseconds > 3e9:
+ self.last_matrix_log_time[finger] = current_time
+ self.get_logger().info(f"{finger}: {rows}×{cols}, max={arr.max():.1f}")
+
+ except Exception as e:
+ self.get_logger().error(f"Matrix error: {str(e)}")
+
+ def update_display(self):
+ # 更新波形图
+ for finger in self.fingers_mass:
+ self.wave_data[finger] = np.roll(self.wave_data[finger], -1)
+ self.wave_data[finger][-1] = self.current_wave_values[finger]
+ self.wave_curves[finger].setData(self.wave_data[finger])
+
+ val = self.current_wave_values[finger]
+ self.wave_curves[finger + '_text'].setText(f"{val:.0f}")
+ self.wave_curves[finger + '_text'].setPos(99, val)
+
+ # 更新热力图 - 修复翻转问题
+ for finger in self.fingers_matrix:
+ data = self.matrix_data[finger]
+ rows, cols = self.matrix_shapes[finger] # 12×6
+
+ if not self.data_received[finger]:
+ continue
+
+ img = self.matrix_images[finger]
+ view = self.matrix_plots[finger]
+ peak_text = self.matrix_peaks[finger]
+
+ max_val = data.max()
+
+ if max_val > 0:
+ display_data = data / max_val if max_val > 1 else data
+ else:
+ display_data = data
+
+ # **关键修复**: 垂直翻转，使第0行(ROS数据第1行)显示在顶部
+ display_data_flipped = np.flipud(display_data)
+
+ # 显示翻转后的数据 (12×6)
+ img.setImage(display_data_flipped, autoLevels=False, levels=[0, 1])
+
+ # 设置视图范围
+ view.setRange(xRange=(-0.5, cols-0.5), yRange=(-0.5, rows-0.5))
+
+ # 显示数值：矩阵中的最大压力值，单位g(克)
+ if max_val > 0:
+ # 找到原始数据中的最大值位置
+ max_idx = np.unravel_index(np.argmax(data), data.shape)
+
+ # 峰值显示格式: 最大值 + 单位
+ peak_text.setText(f"{max_val:.0f}")
+
+ # 坐标转换：因为图像翻转了，y坐标也要翻转
+ # 原始行号row，在翻转后的图像中是 (rows-1-row)
+ flipped_row = (rows - 1) - max_idx[0]
+ peak_text.setPos(max_idx[1], flipped_row)
+ else:
+ peak_text.setText("0")
+
+def signal_handler(sig, frame):
+ QtWidgets.QApplication.quit()
+ sys.exit(0)
+
+def main(args=None):
+ rclpy.init(args=args)
+
+ app = QtWidgets.QApplication(sys.argv)
+ app.setStyle('Fusion')
+
+ signal.signal(signal.SIGINT, signal_handler)
+
+ gui = PressureDiagram()
+ gui.show()
+
+ # ROS2 spin 在 Qt 定时器中处理
+ def ros_spin():
+ rclpy.spin_once(gui, timeout_sec=0)
+
+ ros_timer = QtCore.QTimer()
+ ros_timer.timeout.connect(ros_spin)
+ ros_timer.start(1) # ~1000Hz
+
+ sys.exit(app.exec_())
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/linker_hand_ros2_sdk_ws/src/pressure_diagram/resource/pressure_diagram b/linker_hand_ros2_sdk_ws/src/pressure_diagram/resource/pressure_diagram
new file mode 100644
index 0000000..e69de29
diff --git a/linker_hand_ros2_sdk_ws/src/pressure_diagram/setup.cfg b/linker_hand_ros2_sdk_ws/src/pressure_diagram/setup.cfg
new file mode 100644
index 0000000..0a18512
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/pressure_diagram/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/pressure_diagram
+[install]
+install_scripts=$base/lib/pressure_diagram
diff --git a/linker_hand_ros2_sdk_ws/src/pressure_diagram/setup.py b/linker_hand_ros2_sdk_ws/src/pressure_diagram/setup.py
new file mode 100644
index 0000000..7c61879
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/pressure_diagram/setup.py
@@ -0,0 +1,34 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+import os
+from glob import glob
+from setuptools import find_packages, setup
+package_name = 'pressure_diagram'
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=find_packages(exclude=['test']),
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='linkerhand',
+ maintainer_email='linkerhand@todo.todo',
+ description='ROS2 Pressure Diagram - Real-time pressure sensor visualization for Linker Hand',
+ license='Apache-2.0',
+ extras_require={
+ 'test': [
+ 'pytest',
+ ],
+ },
+ entry_points={
+ 'console_scripts': [
+ 'pressure_diagram = pressure_diagram.pressure_diagram:main',
+ ],
+ },
+)
diff --git a/linker_hand_ros2_sdk_ws/src/pressure_diagram/test/test_copyright.py b/linker_hand_ros2_sdk_ws/src/pressure_diagram/test/test_copyright.py
new file mode 100644
index 0000000..97a3919
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/pressure_diagram/test/test_copyright.py
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found errors'
diff --git a/linker_hand_ros2_sdk_ws/src/pressure_diagram/test/test_flake8.py b/linker_hand_ros2_sdk_ws/src/pressure_diagram/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/pressure_diagram/test/test_flake8.py
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+ rc, errors = main_with_errors(argv=[])
+ assert rc == 0, \
+ 'Found %d code style errors / warnings:\n' % len(errors) + \
+ '\n'.join(errors)
diff --git a/linker_hand_ros2_sdk_ws/src/pressure_diagram/test/test_pep257.py b/linker_hand_ros2_sdk_ws/src/pressure_diagram/test/test_pep257.py
new file mode 100644
index 0000000..b234a38
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/pressure_diagram/test/test_pep257.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found code style errors / warnings'
diff --git a/linker_hand_ros2_sdk_ws/src/setting_info/package.xml b/linker_hand_ros2_sdk_ws/src/setting_info/package.xml
new file mode 100644
index 0000000..d09764f
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/setting_info/package.xml
@@ -0,0 +1,21 @@
+
+
+
+ setting_info
+ 0.0.0
+ TODO: Package description
+ linkerhand
+ TODO: License declaration
+
+ rclpy
+ std_msgs
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+
+
+ ament_python
+
+
diff --git a/linker_hand_ros2_sdk_ws/src/setting_info/resource/setting_info b/linker_hand_ros2_sdk_ws/src/setting_info/resource/setting_info
new file mode 100644
index 0000000..e69de29
diff --git a/linker_hand_ros2_sdk_ws/src/setting_info/setting_info/__init__.py b/linker_hand_ros2_sdk_ws/src/setting_info/setting_info/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/linker_hand_ros2_sdk_ws/src/setting_info/setting_info/setting_info_node.py b/linker_hand_ros2_sdk_ws/src/setting_info/setting_info/setting_info_node.py
new file mode 100644
index 0000000..6c4969f
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/setting_info/setting_info/setting_info_node.py
@@ -0,0 +1,78 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+'''
+编译: colcon build --symlink-install
+启动命令:ros2 run linker_hand_ros2_sdk linker_hand_sdk
+'''
+import rclpy,math,sys # ROS2 Python接口库
+from rclpy.node import Node # ROS2 节点类
+import rclpy.time
+from std_msgs.msg import String, Header, Float32MultiArray
+from sensor_msgs.msg import JointState
+import time,threading, json
+
+
+class SettingInfoNode(Node):
+ def __init__(self):
+ super().__init__("setting_info_node")
+ self.hand_joint = "L10"
+ self.hand_type = "left"
+ self.hand_info = {}
+ self.cb_count = 0
+ self.hand_info_sub = self.create_subscription(String, f"/cb_{self.hand_type}_hand_info",self.setting_cb,10)
+ self.setting_pub = self.create_publisher(String, '/cb_hand_setting_cmd', 10)
+
+
+ def setting_cb(self, msg):
+ self.cb_count += 1
+ self.hand_info = json.loads(msg.data)
+ if self.cb_count == 5:
+ print(self.hand_info)
+ sys.exit(1)
+ time.sleep(1)
+
+
+ def pub_msg(self, cmd_dic):
+ count = 0
+ while True:
+ msg = String()
+ msg.data = json.dumps(cmd_dic)
+ print(msg)
+ self.setting_pub.publish(msg)
+ time.sleep(1)
+ if count == 5:
+ break
+ count += 1
+
+
+ # 设置速度
+ def set_speed(self, speed=[91] * 10):
+ cmd_dic = {
+ "setting_cmd": "set_speed",
+ "params":{
+ "hand_type": self.hand_type,
+ "speed":speed
+ }
+ }
+ self.pub_msg(cmd_dic=cmd_dic)
+
+ # 设置扭矩
+ def set_max_torque_limits(self, torque=[100] * 5):
+ cmd_dic = {
+ "setting_cmd": "set_max_torque_limits",
+ "params":{
+ "hand_type": self.hand_type,
+ "torque":torque
+ }
+ }
+ self.pub_msg(cmd_dic=cmd_dic)
+
+def main(args=None):
+ rclpy.init(args=args)
+ set = SettingInfoNode()
+ pub_ther = threading.Thread(target=set.set_speed)
+ pub_ther.daemon = True
+ pub_ther.start()
+ #set.set_speed()
+
+ rclpy.spin(set)
\ No newline at end of file
diff --git a/linker_hand_ros2_sdk_ws/src/setting_info/setup.cfg b/linker_hand_ros2_sdk_ws/src/setting_info/setup.cfg
new file mode 100644
index 0000000..b464f0c
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/setting_info/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/setting_info
+[install]
+install_scripts=$base/lib/setting_info
diff --git a/linker_hand_ros2_sdk_ws/src/setting_info/setup.py b/linker_hand_ros2_sdk_ws/src/setting_info/setup.py
new file mode 100644
index 0000000..b442181
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/setting_info/setup.py
@@ -0,0 +1,26 @@
+from setuptools import find_packages, setup
+
+package_name = 'setting_info'
+
+setup(
+ name=package_name,
+ version='0.0.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='linkerhand',
+ maintainer_email='linkerhand@todo.todo',
+ description='TODO: Package description',
+ license='TODO: License declaration',
+ tests_require=['pytest'],
+ entry_points={
+ 'console_scripts': [
+ 'setting_info_node = setting_info.setting_info_node:main',
+ ],
+ },
+)
diff --git a/linker_hand_ros2_sdk_ws/src/setting_info/test/test_copyright.py b/linker_hand_ros2_sdk_ws/src/setting_info/test/test_copyright.py
new file mode 100644
index 0000000..97a3919
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/setting_info/test/test_copyright.py
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found errors'
diff --git a/linker_hand_ros2_sdk_ws/src/setting_info/test/test_flake8.py b/linker_hand_ros2_sdk_ws/src/setting_info/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/setting_info/test/test_flake8.py
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+ rc, errors = main_with_errors(argv=[])
+ assert rc == 0, \
+ 'Found %d code style errors / warnings:\n' % len(errors) + \
+ '\n'.join(errors)
diff --git a/linker_hand_ros2_sdk_ws/src/setting_info/test/test_pep257.py b/linker_hand_ros2_sdk_ws/src/setting_info/test/test_pep257.py
new file mode 100644
index 0000000..b234a38
--- /dev/null
+++ b/linker_hand_ros2_sdk_ws/src/setting_info/test/test_pep257.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found code style errors / warnings'
diff --git a/run_middle_step_experiment.sh b/run_middle_step_experiment.sh
new file mode 100755
index 0000000..081eb1d
--- /dev/null
+++ b/run_middle_step_experiment.sh
@@ -0,0 +1,25 @@
+#!/usr/bin/env bash
+# 快捷入口：O6 中指 step 实验（真机 / 仿真 / 对比）
+set -eo pipefail
+ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-31}"
+export ROS_LOCALHOST_ONLY=0
+
+set +u
+source /opt/ros/jazzy/setup.bash 2>/dev/null || true
+set -u
+
+MODE="${1:-}"
+if [[ -z "$MODE" ]]; then
+ cat <<'EOF'
+用法:
+ ./run_middle_step_experiment.sh real # 真机实验（需 can0 + SDK）
+ ./run_middle_step_experiment.sh sim # 仿真实验（MuJoCo）
+ ./run_middle_step_experiment.sh compare # 对比图（需先 real + sim）
+
+时序（两边相同）: 3s 张开 → 3s 握紧 → 3s 张开 → 2s 余量
+EOF
+ exit 1
+fi
+
+exec python3 "$ROOT/tools/run_middle_step_experiment.py" "$MODE" "${@:2}"
diff --git a/run_real_hand.sh b/run_real_hand.sh
new file mode 100755
index 0000000..98607e9
--- /dev/null
+++ b/run_real_hand.sh
@@ -0,0 +1,29 @@
+#!/usr/bin/env bash
+# 启动真机 LinkerHand ROS2 SDK（O6 左手 / can0）
+set -eo pipefail
+WS="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)/linker_hand_ros2_sdk_ws"
+
+# ROS setup.bash 会引用未定义变量，不能开 nounset (-u)
+set +u
+source /opt/ros/jazzy/setup.bash
+source "$WS/install/setup.bash"
+set -u
+
+export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-30}"
+export ROS_LOCALHOST_ONLY=0
+
+# 开启 CAN（需 sudo；蓝灯常亮）
+if ! ip link show can0 &>/dev/null; then
+ echo "[warn] 未检测到 can0，请插入 USB-CAN 后执行："
+ echo " sudo ip link set can0 up type can bitrate 1000000"
+else
+ if ! ip link show can0 | grep -q 'UP'; then
+ echo "[info] 尝试拉起 can0 ..."
+ sudo ip link set can0 up type can bitrate 1000000 || true
+ fi
+fi
+
+echo "启动: ros2 launch linker_hand_ros2_sdk linker_hand.launch.py"
+echo "配置: hand_type=left hand_joint=O6 can=can0"
+set +u
+exec ros2 launch linker_hand_ros2_sdk linker_hand.launch.py "$@"
diff --git a/run_real_middle_record.sh b/run_real_middle_record.sh
new file mode 100755
index 0000000..c677559
--- /dev/null
+++ b/run_real_middle_record.sh
@@ -0,0 +1,3 @@
+#!/usr/bin/env bash
+# 已迁移到统一实验脚本（真机 / 仿真同一套发令时序）
+exec "$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)/run_middle_step_experiment.sh" real "$@"
diff --git a/run_sim.sh b/run_sim.sh
new file mode 100755
index 0000000..d20207f
--- /dev/null
+++ b/run_sim.sh
@@ -0,0 +1,24 @@
+#!/usr/bin/env bash
+# 本机启动左手 O6 MuJoCo 仿真（ROS2 用系统 Python，依赖从 venv/user site 补齐）
+set -euo pipefail
+
+ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+cd "$ROOT"
+
+# ROS2
+# shellcheck disable=SC1091
+source /opt/ros/jazzy/setup.bash
+# shellcheck disable=SC1091
+source "$ROOT/install/setup.bash"
+
+# 多机通信
+export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-30}"
+export ROS_LOCALHOST_ONLY=0
+
+# ros2 launch 走系统 python3；把 venv 的 site-packages 挂上（含 mujoco/PyQt5）
+VENV_SITE="$ROOT/venv/lib/python3.12/site-packages"
+if [[ -d "$VENV_SITE" ]]; then
+ export PYTHONPATH="$VENV_SITE${PYTHONPATH:+:$PYTHONPATH}"
+fi
+
+exec ros2 launch linker_hand_mujoco_ros2 linker_hand_mujoco_ros2.launch.py "$@"
diff --git a/src/linkerhand-sim/LICENSE b/src/linkerhand-sim/LICENSE
new file mode 100644
index 0000000..261eeb9
--- /dev/null
+++ b/src/linkerhand-sim/LICENSE
@@ -0,0 +1,201 @@
+ Apache License
+ Version 2.0, January 2004
+ http://www.apache.org/licenses/
+
+ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+ 1. Definitions.
+
+ "License" shall mean the terms and conditions for use, reproduction,
+ and distribution as defined by Sections 1 through 9 of this document.
+
+ "Licensor" shall mean the copyright owner or entity authorized by
+ the copyright owner that is granting the License.
+
+ "Legal Entity" shall mean the union of the acting entity and all
+ other entities that control, are controlled by, or are under common
+ control with that entity. For the purposes of this definition,
+ "control" means (i) the power, direct or indirect, to cause the
+ direction or management of such entity, whether by contract or
+ otherwise, or (ii) ownership of fifty percent (50%) or more of the
+ outstanding shares, or (iii) beneficial ownership of such entity.
+
+ "You" (or "Your") shall mean an individual or Legal Entity
+ exercising permissions granted by this License.
+
+ "Source" form shall mean the preferred form for making modifications,
+ including but not limited to software source code, documentation
+ source, and configuration files.
+
+ "Object" form shall mean any form resulting from mechanical
+ transformation or translation of a Source form, including but
+ not limited to compiled object code, generated documentation,
+ and conversions to other media types.
+
+ "Work" shall mean the work of authorship, whether in Source or
+ Object form, made available under the License, as indicated by a
+ copyright notice that is included in or attached to the work
+ (an example is provided in the Appendix below).
+
+ "Derivative Works" shall mean any work, whether in Source or Object
+ form, that is based on (or derived from) the Work and for which the
+ editorial revisions, annotations, elaborations, or other modifications
+ represent, as a whole, an original work of authorship. For the purposes
+ of this License, Derivative Works shall not include works that remain
+ separable from, or merely link (or bind by name) to the interfaces of,
+ the Work and Derivative Works thereof.
+
+ "Contribution" shall mean any work of authorship, including
+ the original version of the Work and any modifications or additions
+ to that Work or Derivative Works thereof, that is intentionally
+ submitted to Licensor for inclusion in the Work by the copyright owner
+ or by an individual or Legal Entity authorized to submit on behalf of
+ the copyright owner. For the purposes of this definition, "submitted"
+ means any form of electronic, verbal, or written communication sent
+ to the Licensor or its representatives, including but not limited to
+ communication on electronic mailing lists, source code control systems,
+ and issue tracking systems that are managed by, or on behalf of, the
+ Licensor for the purpose of discussing and improving the Work, but
+ excluding communication that is conspicuously marked or otherwise
+ designated in writing by the copyright owner as "Not a Contribution."
+
+ "Contributor" shall mean Licensor and any individual or Legal Entity
+ on behalf of whom a Contribution has been received by Licensor and
+ subsequently incorporated within the Work.
+
+ 2. Grant of Copyright License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ copyright license to reproduce, prepare Derivative Works of,
+ publicly display, publicly perform, sublicense, and distribute the
+ Work and such Derivative Works in Source or Object form.
+
+ 3. Grant of Patent License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ (except as stated in this section) patent license to make, have made,
+ use, offer to sell, sell, import, and otherwise transfer the Work,
+ where such license applies only to those patent claims licensable
+ by such Contributor that are necessarily infringed by their
+ Contribution(s) alone or by combination of their Contribution(s)
+ with the Work to which such Contribution(s) was submitted. If You
+ institute patent litigation against any entity (including a
+ cross-claim or counterclaim in a lawsuit) alleging that the Work
+ or a Contribution incorporated within the Work constitutes direct
+ or contributory patent infringement, then any patent licenses
+ granted to You under this License for that Work shall terminate
+ as of the date such litigation is filed.
+
+ 4. Redistribution. You may reproduce and distribute copies of the
+ Work or Derivative Works thereof in any medium, with or without
+ modifications, and in Source or Object form, provided that You
+ meet the following conditions:
+
+ (a) You must give any other recipients of the Work or
+ Derivative Works a copy of this License; and
+
+ (b) You must cause any modified files to carry prominent notices
+ stating that You changed the files; and
+
+ (c) You must retain, in the Source form of any Derivative Works
+ that You distribute, all copyright, patent, trademark, and
+ attribution notices from the Source form of the Work,
+ excluding those notices that do not pertain to any part of
+ the Derivative Works; and
+
+ (d) If the Work includes a "NOTICE" text file as part of its
+ distribution, then any Derivative Works that You distribute must
+ include a readable copy of the attribution notices contained
+ within such NOTICE file, excluding those notices that do not
+ pertain to any part of the Derivative Works, in at least one
+ of the following places: within a NOTICE text file distributed
+ as part of the Derivative Works; within the Source form or
+ documentation, if provided along with the Derivative Works; or,
+ within a display generated by the Derivative Works, if and
+ wherever such third-party notices normally appear. The contents
+ of the NOTICE file are for informational purposes only and
+ do not modify the License. You may add Your own attribution
+ notices within Derivative Works that You distribute, alongside
+ or as an addendum to the NOTICE text from the Work, provided
+ that such additional attribution notices cannot be construed
+ as modifying the License.
+
+ You may add Your own copyright statement to Your modifications and
+ may provide additional or different license terms and conditions
+ for use, reproduction, or distribution of Your modifications, or
+ for any such Derivative Works as a whole, provided Your use,
+ reproduction, and distribution of the Work otherwise complies with
+ the conditions stated in this License.
+
+ 5. Submission of Contributions. Unless You explicitly state otherwise,
+ any Contribution intentionally submitted for inclusion in the Work
+ by You to the Licensor shall be under the terms and conditions of
+ this License, without any additional terms or conditions.
+ Notwithstanding the above, nothing herein shall supersede or modify
+ the terms of any separate license agreement you may have executed
+ with Licensor regarding such Contributions.
+
+ 6. Trademarks. This License does not grant permission to use the trade
+ names, trademarks, service marks, or product names of the Licensor,
+ except as required for reasonable and customary use in describing the
+ origin of the Work and reproducing the content of the NOTICE file.
+
+ 7. Disclaimer of Warranty. Unless required by applicable law or
+ agreed to in writing, Licensor provides the Work (and each
+ Contributor provides its Contributions) on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+ implied, including, without limitation, any warranties or conditions
+ of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+ PARTICULAR PURPOSE. You are solely responsible for determining the
+ appropriateness of using or redistributing the Work and assume any
+ risks associated with Your exercise of permissions under this License.
+
+ 8. Limitation of Liability. In no event and under no legal theory,
+ whether in tort (including negligence), contract, or otherwise,
+ unless required by applicable law (such as deliberate and grossly
+ negligent acts) or agreed to in writing, shall any Contributor be
+ liable to You for damages, including any direct, indirect, special,
+ incidental, or consequential damages of any character arising as a
+ result of this License or out of the use or inability to use the
+ Work (including but not limited to damages for loss of goodwill,
+ work stoppage, computer failure or malfunction, or any and all
+ other commercial damages or losses), even if such Contributor
+ has been advised of the possibility of such damages.
+
+ 9. Accepting Warranty or Additional Liability. While redistributing
+ the Work or Derivative Works thereof, You may choose to offer,
+ and charge a fee for, acceptance of support, warranty, indemnity,
+ or other liability obligations and/or rights consistent with this
+ License. However, in accepting such obligations, You may act only
+ on Your own behalf and on Your sole responsibility, not on behalf
+ of any other Contributor, and only if You agree to indemnify,
+ defend, and hold each Contributor harmless for any liability
+ incurred by, or claims asserted against, such Contributor by reason
+ of your accepting any such warranty or additional liability.
+
+ END OF TERMS AND CONDITIONS
+
+ APPENDIX: How to apply the Apache License to your work.
+
+ To apply the Apache License to your work, attach the following
+ boilerplate notice, with the fields enclosed by brackets "[]"
+ replaced with your own identifying information. (Don't include
+ the brackets!) The text should be enclosed in the appropriate
+ comment syntax for the file format. We also recommend that a
+ file or class name and description of purpose be included on the
+ same "printed page" as the copyright notice for easier
+ identification within third-party archives.
+
+ Copyright [yyyy] [name of copyright owner]
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
diff --git a/src/linkerhand-sim/README.md b/src/linkerhand-sim/README.md
new file mode 100644
index 0000000..b3ffded
--- /dev/null
+++ b/src/linkerhand-sim/README.md
@@ -0,0 +1,10 @@
+# linker_hand_sim
+[Isaac-Gym by Python3](https://github.com/linkerbotai/linker_hand_sim/blob/main/linker_hand_isaac_gym_urdf/README.md)
+
+[Mujoco by ros noetic](linker_hand_mujoco_ros/README_CN.md)
+
+[Mujoco by ros2 jazzy](linker_hand_mujoco_ros2/README_CN.MD)
+
+[PyBullet by ros noetic](linker_hand_pybullet_ros/README_PyBullet_CN.md)
+
+[PyBullet by ros2 jazzy](linker_hand_pybullet_ros2/README_CN.MD)
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/README_CN.MD b/src/linkerhand-sim/linker_hand_mujoco_ros2/README_CN.MD
new file mode 100644
index 0000000..0aa8d5e
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/README_CN.MD
@@ -0,0 +1,60 @@
+# 1. **概述**
+
+灵心巧手，创造万物。
+
+LinkerHand 灵巧手 ROS2 SDK 是由灵心巧手（北京）科技有限公司开发的一款软件工具，用于驱动其灵巧手系列产品，并提供功能示例。它支持多种设备（如笔记本、台式机、树莓派、Jetson 等），主要服务于人型机器人、工业自动化和科研院所等领域，适用于人型机器人、柔性化生产线、具身大模型训练和数据采集等场景。
+
+# 1.1 **说明**
+本程序为LinkerHand制作系列灵巧手Mujoco仿真环境，便于使用者熟悉LinkerHand灵巧手系列产品的使用方式方法，以及进行仿真环境下的模型训练和数据采集
+
+# 2. **使用说明**
+```bash
+$ mkdir -p linker_hand_mujoco_ros2/src #创建目录
+$ cd linker_hand_mujoco_ros2/src #进入目录
+$ # 1. 克隆仓库（使用 sparse 模式 + blob 过滤，节省空间）
+$ git clone --filter=blob:none --sparse https://github.com/linker-bot/linkerhand-sim.git
+$ # 2. 进入仓库目录
+$ cd linkerhand-sim
+$ # 3. 设置 sparse-checkout 目录
+$ git sparse-checkout set linker_hand_mujoco_ros2
+$ cd linker_hand_mujoco_ros2/src/linker_hand_sim/
+$ pip install -r requirements.txt
+$ /usr/bin/python3 -m pip install mujoco # 使用ROS2调用mujoco必须使用系统环境下的python3安装
+```
+- 修改linker_hand_mujoco_ros2/launch/linker_hand_mujoco_ros2.launch.py
+根据文件内参数说明修改即可
+```bash
+$ cd linker_hand_mujoco_ros2/
+$ colcon build --symlink-install
+$ source ./install/setup.bash
+$ ros2 launch linker_hand_mujoco_ros2 linker_hand_mujoco_ros2.launch.py
+```
+
+# 3. **topic说明**
+- /cb_right_hand_control_cmd or /cb_left_hand_control_cmd
+```bash
+ros2 topic pub /joint_states sensor_msgs/msg/JointState '
+{
+ header: { stamp: { sec: 0, nanosec: 0 }, frame_id: "" },
+ name: [],
+ position: [200.0, 200.0, 200.0, 200.0, 200.0, 200.0, 200.0, 200.0, 200.0, 200.0],
+ velocity: [],
+ effort: []
+}'
+
+```
+- position 说明
+ L6: ["大拇指弯曲", "大拇指横摆","食指弯曲", "中指弯曲", "无名指弯曲", "小拇指弯曲"]
+
+ L7: ["大拇指弯曲", "大拇指横摆","食指弯曲", "中指弯曲", "无名指弯曲","小拇指弯曲","拇指旋转"]
+
+ L10: ["拇指根部", "拇指侧摆","食指根部", "中指根部", "无名指根部","小指根部","食指侧摆","无名指侧摆","小指侧摆","拇指旋转"]
+
+ L20: ["拇指根部", "食指根部", "中指根部", "无名指根部","小指根部","拇指侧摆","食指侧摆","中指侧摆","无名指侧摆","小指侧摆","拇指横摆","预留","预留","预留","预留","拇指尖部","食指末端","中指末端","无名指末端","小指末端"]
+
+ L21: ["大拇指根部", "食指根部", "中指根部","无名指根部","小拇指根部","大拇指侧摆","食指侧摆","中指侧摆","无名指侧摆","小拇指侧摆","大拇指横滚","预留","预留","预留","预留","大拇指中部","预留","预留","预留","预留","大拇指指尖","食指指尖","中指指尖","无名指指尖","小拇指指尖"]
+
+ L25: ["大拇指根部", "食指根部", "中指根部","无名指根部","小拇指根部","大拇指侧摆","食指侧摆","中指侧摆","无名指侧摆","小拇指侧摆","大拇指横滚","预留","预留","预留","预留","大拇指中部","食指中部","中指中部","无名指中部","小拇指中部","大拇指指尖","食指指尖","中指指尖","无名指指尖","小拇指指尖"]
+
+# 3.1 **GUI控制**
+可以使用 linker_hand_ros2_sdk的[gui_control](https://github.com/linkerbotai/linker_hand_ros2_sdk/blob/main/README_CN.md)控制仿真环境
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/launch/hand_curve_recorder.launch.py b/src/linkerhand-sim/linker_hand_mujoco_ros2/launch/hand_curve_recorder.launch.py
new file mode 100644
index 0000000..b1017a4
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/launch/hand_curve_recorder.launch.py
@@ -0,0 +1,35 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+ return LaunchDescription([
+ DeclareLaunchArgument("hand_type", default_value="left"),
+ DeclareLaunchArgument("channel", default_value="3"), # 中指
+ DeclareLaunchArgument("label", default_value="run"), # sim / real
+ DeclareLaunchArgument("output_dir", default_value="reports/hand_curves"),
+ DeclareLaunchArgument("hz", default_value="100.0"),
+ # 话题留空则用 /cb_{hand}_hand_control_cmd 与 /cb_{hand}_hand_state
+ DeclareLaunchArgument("cmd_topic", default_value=""),
+ DeclareLaunchArgument("state_topic", default_value=""),
+ DeclareLaunchArgument("current_topic", default_value=""),
+ Node(
+ package="linker_hand_mujoco_ros2",
+ executable="hand_curve_recorder",
+ name="hand_curve_recorder",
+ output="screen",
+ parameters=[{
+ "hand_type": LaunchConfiguration("hand_type"),
+ "channel": LaunchConfiguration("channel"),
+ "label": LaunchConfiguration("label"),
+ "output_dir": LaunchConfiguration("output_dir"),
+ "hz": LaunchConfiguration("hz"),
+ "cmd_topic": LaunchConfiguration("cmd_topic"),
+ "state_topic": LaunchConfiguration("state_topic"),
+ "current_topic": LaunchConfiguration("current_topic"),
+ "use_cmd_as_joint": True,
+ }],
+ ),
+ ])
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/launch/linker_hand_mujoco_ros2.launch.py b/src/linkerhand-sim/linker_hand_mujoco_ros2/launch/linker_hand_mujoco_ros2.launch.py
new file mode 100644
index 0000000..1e5a79c
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/launch/linker_hand_mujoco_ros2.launch.py
@@ -0,0 +1,19 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+ return LaunchDescription([
+ Node(
+ package='linker_hand_mujoco_ros2',
+ executable='linker_hand_mujoco_ros2_node',
+ name='linker_hand_mujoco_ros2_node',
+ output='screen',
+ parameters=[{
+ 'hand_type': 'left',
+ # O6 / L6 / L7 / L10 / L20 / L21
+ 'hand_joint': "O6",
+ 'topic_hz': 30,
+ 'is_touch': True,
+ }],
+ ),
+ ])
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/__init__.py b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/hand_curve_recorder.py b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/hand_curve_recorder.py
new file mode 100644
index 0000000..dce09f5
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/hand_curve_recorder.py
@@ -0,0 +1,252 @@
+#!/usr/bin/env python3
+"""通用手部曲线录制 + 画图（仿真 / 真机同一套 ROS 话题）。
+
+默认订阅：
+ /cb__hand_control_cmd 指令 0～255 (sensor_msgs/JointState.position)
+ /cb__hand_state 反馈 0～255 (position)；电流可放 effort，单位 A
+
+真机有 state 就录 joint + current；只有 cmd 也能录指令曲线。
+Ctrl+C 结束 → 写 CSV + 画指定通道的 角/速/流(或力矩) 三图。
+"""
+
+from __future__ import annotations
+
+import csv
+import os
+import time
+from datetime import datetime
+from pathlib import Path
+
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Float32MultiArray
+
+# O6/L6 六维通道名（与控制协议一致）
+CHANNEL_NAMES = [
+ "thumb_bend",
+ "thumb_yaw",
+ "index",
+ "middle",
+ "ring",
+ "pinky",
+]
+
+
+def _pad(seq, n, fill=float("nan")):
+ out = [fill] * n
+ for i, v in enumerate(list(seq)[:n]):
+ out[i] = float(v)
+ return out
+
+
+class HandCurveRecorder(Node):
+ def __init__(self):
+ super().__init__("hand_curve_recorder")
+
+ self.declare_parameter("hand_type", "left")
+ self.declare_parameter("cmd_topic", "") # 空则自动 /cb_{hand}_hand_control_cmd
+ self.declare_parameter("state_topic", "") # 空则自动 /cb_{hand}_hand_state
+ self.declare_parameter("current_topic", "") # 可选 Float32MultiArray；空则用 state.effort
+ self.declare_parameter("n_channels", 6)
+ self.declare_parameter("channel", 3) # 画图关注通道：中指=3
+ self.declare_parameter("output_dir", "reports/hand_curves")
+ self.declare_parameter("label", "run") # 文件名标签 sim / real / ...
+ self.declare_parameter("hz", 100.0) # 落盘采样率（定时器）
+ self.declare_parameter("use_cmd_as_joint", True) # 无 state 时用 cmd 当 joint 画图
+
+ hand = self.get_parameter("hand_type").value
+ self.n = int(self.get_parameter("n_channels").value)
+ self.channel = int(self.get_parameter("channel").value)
+ self.use_cmd_as_joint = bool(self.get_parameter("use_cmd_as_joint").value)
+ self.label = str(self.get_parameter("label").value)
+
+ cmd_topic = self.get_parameter("cmd_topic").value or f"/cb_{hand}_hand_control_cmd"
+ state_topic = self.get_parameter("state_topic").value or f"/cb_{hand}_hand_state"
+ current_topic = self.get_parameter("current_topic").value
+
+ out = Path(self.get_parameter("output_dir").value).expanduser()
+ if not out.is_absolute():
+ # 相对路径：相对当前工作目录
+ out = Path.cwd() / out
+ out.mkdir(parents=True, exist_ok=True)
+ self.output_dir = out
+
+ self._lock_cmd = [float("nan")] * self.n
+ self._lock_joint = [float("nan")] * self.n
+ self._lock_current = [float("nan")] * self.n
+ self._have_state = False
+ self._have_current = False
+ self._rows = []
+ self._t0 = time.perf_counter()
+
+ self.create_subscription(JointState, cmd_topic, self._on_cmd, 50)
+ self.create_subscription(JointState, state_topic, self._on_state, 50)
+ if current_topic:
+ self.create_subscription(
+ Float32MultiArray, current_topic, self._on_current, 50
+ )
+
+ period = 1.0 / max(1.0, float(self.get_parameter("hz").value))
+ self.create_timer(period, self._on_timer)
+
+ self.get_logger().info(
+ f"录制中 | cmd={cmd_topic} | state={state_topic} | "
+ f"current={current_topic or 'state.effort'} | "
+ f"plot_channel={self.channel}({CHANNEL_NAMES[self.channel] if 0 <= self.channel < len(CHANNEL_NAMES) else '?'}) | "
+ f"out={self.output_dir}"
+ )
+ self.get_logger().info("对端发控制指令即可；Ctrl+C 结束并画图")
+
+ def _on_cmd(self, msg: JointState):
+ self._lock_cmd = _pad(msg.position, self.n)
+
+ def _on_state(self, msg: JointState):
+ self._have_state = True
+ self._lock_joint = _pad(msg.position, self.n)
+ if msg.effort and len(msg.effort) > 0:
+ self._have_current = True
+ self._lock_current = _pad(msg.effort, self.n)
+
+ def _on_current(self, msg: Float32MultiArray):
+ self._have_current = True
+ self._lock_current = _pad(msg.data, self.n)
+
+ def _on_timer(self):
+ t = time.perf_counter() - self._t0
+ joint = list(self._lock_joint)
+ if (not self._have_state) and self.use_cmd_as_joint:
+ joint = list(self._lock_cmd)
+ self._rows.append(
+ {
+ "t_s": t,
+ "cmd": list(self._lock_cmd),
+ "joint": joint,
+ "current": list(self._lock_current),
+ }
+ )
+
+ def save_and_plot(self):
+ if not self._rows:
+ self.get_logger().warn("没有录到数据，跳过保存")
+ return
+
+ stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
+ ch = self.channel
+ ch_name = CHANNEL_NAMES[ch] if 0 <= ch < len(CHANNEL_NAMES) else f"ch{ch}"
+ stem = f"{self.label}_{ch_name}_{stamp}"
+
+ csv_path = self.output_dir / f"{stem}.csv"
+ with csv_path.open("w", newline="") as f:
+ w = csv.writer(f)
+ header = ["t_s"]
+ for i in range(self.n):
+ name = CHANNEL_NAMES[i] if i < len(CHANNEL_NAMES) else f"ch{i}"
+ header += [f"cmd_{name}", f"joint_{name}", f"current_{name}"]
+ w.writerow(header)
+ for row in self._rows:
+ line = [f"{row['t_s']:.6f}"]
+ for i in range(self.n):
+ line += [
+ f"{row['cmd'][i]:.6f}",
+ f"{row['joint'][i]:.6f}",
+ f"{row['current'][i]:.6f}",
+ ]
+ w.writerow(line)
+
+ self.get_logger().info(f"CSV → {csv_path} ({len(self._rows)} samples)")
+ self.get_logger().info(
+ f"state={'yes' if self._have_state else 'no(use cmd)'} "
+ f"current={'yes' if self._have_current else 'no'}"
+ )
+
+ png_path = self.output_dir / f"{stem}_qvt.png"
+ try:
+ self._plot(png_path, ch, ch_name)
+ self.get_logger().info(f"图 → {png_path}")
+ except Exception as e:
+ self.get_logger().error(f"画图失败: {e}")
+
+ def _plot(self, png_path: Path, ch: int, ch_name: str):
+ import matplotlib
+
+ matplotlib.use("Agg")
+ import matplotlib.pyplot as plt
+
+ t = np.array([r["t_s"] for r in self._rows], dtype=float)
+ cmd = np.array([r["cmd"][ch] for r in self._rows], dtype=float)
+ joint = np.array([r["joint"][ch] for r in self._rows], dtype=float)
+ current = np.array([r["current"][ch] for r in self._rows], dtype=float)
+
+ # 速度：对 joint(u8) 差分；若只有 cmd 则对 cmd 差分
+ y = joint.copy()
+ if np.all(np.isnan(y)):
+ y = cmd.copy()
+ # 用秒做梯度 → u8/s
+ v = np.gradient(y, t)
+ # 简单移动平均低通
+ if len(v) >= 5:
+ kernel = np.ones(5) / 5.0
+ v = np.convolve(v, kernel, mode="same")
+
+ fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
+ src = "joint" if self._have_state else "cmd(as joint)"
+ fig.suptitle(
+ f"hand_curve_recorder ch={ch}:{ch_name} label={self.label} src={src}",
+ fontsize=12,
+ )
+
+ axes[0].plot(t, cmd, "k--", lw=1.2, label="command_u8")
+ axes[0].plot(t, joint, "C0", lw=1.6, label="joint_u8")
+ axes[0].set_ylabel("position (0-255)")
+ axes[0].legend(loc="best")
+ axes[0].grid(True, alpha=0.3)
+
+ axes[1].plot(t, v, "C1", lw=1.6, label="d(joint)/dt")
+ axes[1].set_ylabel("velocity (u8/s)")
+ axes[1].legend(loc="best")
+ axes[1].grid(True, alpha=0.3)
+
+ ylab = "current / effort"
+ if self._have_current:
+ axes[2].plot(t, current, "C3", lw=1.6, label="current/effort")
+ else:
+ axes[2].text(
+ 0.5,
+ 0.5,
+ "no current topic / state.effort\n(only cmd+joint recorded)",
+ ha="center",
+ va="center",
+ transform=axes[2].transAxes,
+ )
+ axes[2].set_ylabel(ylab)
+ axes[2].set_xlabel("time (s)")
+ axes[2].legend(loc="best")
+ axes[2].grid(True, alpha=0.3)
+
+ fig.tight_layout()
+ fig.savefig(png_path, dpi=140)
+ plt.close(fig)
+
+
+def main(args=None):
+ # 无显示环境时 matplotlib 可写缓存
+ os.environ.setdefault("MPLCONFIGDIR", str(Path.cwd() / ".mplconfig"))
+ Path(os.environ["MPLCONFIGDIR"]).mkdir(parents=True, exist_ok=True)
+
+ rclpy.init(args=args)
+ node = HandCurveRecorder()
+ try:
+ rclpy.spin(node)
+ except KeyboardInterrupt:
+ pass
+ finally:
+ node.save_and_plot()
+ node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2.py b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2.py
new file mode 100644
index 0000000..180cdf5
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2.py
@@ -0,0 +1,189 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+import time
+import threading
+import sys
+import os
+
+import numpy as np
+import mujoco
+import mujoco.viewer
+from PyQt5.QtWidgets import QApplication
+from .utils.mapping import *
+from .utils.joint_monitor import JointMonitorWindow
+
+JOINT_CONFIG = {
+ "L6": {
+ "map": L6_JOINT_MAP,
+ "arc": L6_JOINT_ARC,
+ "mimic": None,
+ },
+ "O6": {
+ "map": O6_JOINT_MAP,
+ "arc": O6_JOINT_ARC,
+ "mimic": O6_MIMIC,
+ },
+ "L7": {
+ "map": L7_JOINT_MAP,
+ "arc": L7_JOINT_ARC,
+ "mimic": None,
+ },
+ "L10": {
+ "map": L10_JOINT_MAP,
+ "arc": L10_JOINT_ARC,
+ "mimic": None,
+ },
+ "L20": {
+ "map": L20_JOINT_MAP,
+ "arc": L20_JOINT_ARC,
+ "mimic": None,
+ },
+ "L21": {
+ "map": L21_JOINT_MAP,
+ "arc": L21_JOINT_ARC,
+ "mimic": None,
+ },
+}
+
+
+class MujocoNode(Node):
+ def __init__(self):
+ super().__init__('linker_hand_mujoco_ros2_node')
+ self.declare_parameter("hand_type", "right")
+ self.hand_type = self.get_parameter('hand_type').get_parameter_value().string_value
+ self.declare_parameter("hand_joint", "L10")
+ self.hand_joint = self.get_parameter('hand_joint').get_parameter_value().string_value
+ self.create_subscription(
+ JointState,
+ f"/cb_{self.hand_type}_hand_control_cmd",
+ self.hand_cb,
+ 10,
+ )
+
+ joint_config = JOINT_CONFIG.get(self.hand_joint)
+ if joint_config:
+ self.joint_map = joint_config["map"]
+ self.joint_arc = joint_config["arc"]
+ self.joint_mimic = joint_config.get("mimic")
+ else:
+ self.joint_map = None
+ self.joint_arc = None
+ self.joint_mimic = None
+
+ XML_PATH = (
+ os.path.dirname(os.path.abspath(__file__))
+ + f"/urdf/{self.hand_joint.upper()}/linker_hand_{self.hand_joint.lower()}_{self.hand_type}/"
+ f"linker_hand_{self.hand_joint.lower()}_{self.hand_type}.xml"
+ )
+
+ self.model = mujoco.MjModel.from_xml_path(XML_PATH)
+ self.model.dof_damping[:] = 0.8
+ self.data = mujoco.MjData(self.model)
+
+ print("=" * 20, flush=True)
+ print(mujoco.mj_versionString(), flush=True)
+ print("=" * 20, flush=True)
+ self.data.qpos[:] = 0
+ self.data.qvel[:] = 0
+ self.model.opt.disableflags = 0 if self.hand_joint == "O6" else 1
+ mujoco.mj_forward(self.model, self.data)
+
+ self.joint_names = []
+ self.joint_qpos_addrs = []
+ self.joint_ranges = []
+ for i in range(self.model.njnt):
+ # 只显示铰链关节（手控相关），跳过 free / slide
+ if self.model.jnt_type[i] != int(mujoco.mjtJoint.mjJNT_HINGE):
+ continue
+ name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_JOINT, i) or f"joint_{i}"
+ qposadr = int(self.model.jnt_qposadr[i])
+ lo = float(self.model.jnt_range[i, 0])
+ hi = float(self.model.jnt_range[i, 1])
+ if hi <= lo:
+ lo, hi = 0.0, 1.57
+ self.joint_names.append(name)
+ self.joint_qpos_addrs.append(qposadr)
+ self.joint_ranges.append((lo, hi))
+ print(f"Joint {i}: {name} range=[{lo:.3f}, {hi:.3f}]", flush=True)
+
+ joint_count = self.model.nu
+ self.ctrl_values = np.zeros(joint_count)
+ self.ctrl_ranges = self.model.actuator_ctrlrange.copy()
+
+ self.joint_ui = JointMonitorWindow(
+ self.joint_names,
+ self.joint_ranges,
+ title=f"Joint ({self.hand_joint} {self.hand_type})",
+ )
+ self.joint_ui.show()
+
+ self._ui_update_interval = 0.05 # 20 Hz 刷新进度条
+ self._last_ui_update = 0.0
+
+ sim_thread = threading.Thread(target=self.mujoco_thread, daemon=True)
+ sim_thread.start()
+
+ def mujoco_thread(self):
+ with mujoco.viewer.launch_passive(self.model, self.data) as viewer:
+ print("MuJoCo viewer running...", flush=True)
+ while viewer.is_running():
+ self.data.ctrl[:] = self.ctrl_values
+ mujoco.mj_step(self.model, self.data)
+ viewer.sync()
+
+ now = time.time()
+ if now - self._last_ui_update >= self._ui_update_interval:
+ self._last_ui_update = now
+ values = [
+ float(self.data.qpos[addr]) for addr in self.joint_qpos_addrs
+ ]
+ self.joint_ui.update_values(values)
+
+ time.sleep(0.001)
+
+ def hand_cb(self, data):
+ position = data.position
+ try:
+ if self.joint_map is not None:
+ if self.hand_type == "left":
+ tmp = range_to_arc_left(position, self.hand_joint)
+ elif self.hand_type == "right":
+ tmp = range_to_arc_right(position, self.hand_joint)
+ else:
+ return
+ res = self.map_position_array(tmp, self.joint_map)
+ if self.joint_mimic:
+ res = apply_mimic(res, self.joint_mimic, self.joint_arc)
+ self.ctrl_values[:] = res
+ except Exception as e:
+ self.get_logger().error(f"Error in hand_cb: {e}")
+
+ def map_position_array(self, position, joint_map):
+ mapped_array = [0.0] * len(joint_map)
+ for target_idx, source_idx in joint_map.items():
+ if source_idx < len(position):
+ mapped_array[target_idx] = position[source_idx]
+ return mapped_array
+
+
+def main(args=None):
+ # Qt GUI 必须在主线程
+ app = QApplication.instance() or QApplication(sys.argv)
+
+ rclpy.init(args=args)
+ node = MujocoNode()
+
+ spin_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
+ spin_thread.start()
+
+ exit_code = app.exec_()
+
+ node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+ sys.exit(exit_code)
+
+
+if __name__ == '__main__':
+ main()
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/base_link.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/base_link.STL
new file mode 100644
index 0000000..66bd83f
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/base_link.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link0.STL
new file mode 100644
index 0000000..8eb082a
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link1.STL
new file mode 100644
index 0000000..44d513f
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link2.STL
new file mode 100644
index 0000000..b62a597
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link3.STL
new file mode 100644
index 0000000..8c29da2
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link4.STL
new file mode 100644
index 0000000..dd215f0
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/linker_hand_l10_left.urdf b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/linker_hand_l10_left.urdf
new file mode 100644
index 0000000..76cf37e
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/linker_hand_l10_left.urdf
@@ -0,0 +1,1512 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/linker_hand_l10_left.xml b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/linker_hand_l10_left.xml
new file mode 100644
index 0000000..d235e37
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/linker_hand_l10_left.xml
@@ -0,0 +1,195 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link0.STL
new file mode 100644
index 0000000..754c500
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link1.STL
new file mode 100644
index 0000000..194847e
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link2.STL
new file mode 100644
index 0000000..fd6c112
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link3.STL
new file mode 100644
index 0000000..1fccf5c
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link4.STL
new file mode 100644
index 0000000..67a0339
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/middle_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/middle_link0.STL
new file mode 100644
index 0000000..09ab875
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/middle_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/middle_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/middle_link1.STL
new file mode 100644
index 0000000..4813804
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/middle_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/middle_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/middle_link2.STL
new file mode 100644
index 0000000..fda4dff
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/middle_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/middle_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/middle_link3.STL
new file mode 100644
index 0000000..a3746e9
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/middle_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link0.STL
new file mode 100644
index 0000000..acfbe14
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link1.STL
new file mode 100644
index 0000000..365d4c8
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link2.STL
new file mode 100644
index 0000000..7115054
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link3.STL
new file mode 100644
index 0000000..c848977
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link4.STL
new file mode 100644
index 0000000..77b4fc6
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link0.STL
new file mode 100644
index 0000000..995681c
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link1.STL
new file mode 100644
index 0000000..48a0bb7
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link2.STL
new file mode 100644
index 0000000..6f32a9a
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link3.STL
new file mode 100644
index 0000000..67276b2
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link4.STL
new file mode 100644
index 0000000..06fff54
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link5.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link5.STL
new file mode 100644
index 0000000..0313140
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link5.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/base_link.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/base_link.STL
new file mode 100644
index 0000000..a13c5e9
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/base_link.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link0.STL
new file mode 100644
index 0000000..79be9d9
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link1.STL
new file mode 100644
index 0000000..ff23d89
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link2.STL
new file mode 100644
index 0000000..1562779
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link3.STL
new file mode 100644
index 0000000..c1b7332
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link4.STL
new file mode 100644
index 0000000..024d190
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/linker_hand_l10_right.urdf b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/linker_hand_l10_right.urdf
new file mode 100644
index 0000000..c522f35
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/linker_hand_l10_right.urdf
@@ -0,0 +1,1537 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/linker_hand_l10_right.xml b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/linker_hand_l10_right.xml
new file mode 100644
index 0000000..9441116
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/linker_hand_l10_right.xml
@@ -0,0 +1,195 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link0.STL
new file mode 100644
index 0000000..925cac8
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link1.STL
new file mode 100644
index 0000000..a204f8a
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link2.STL
new file mode 100644
index 0000000..b686aa4
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link3.STL
new file mode 100644
index 0000000..9914fcf
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link4.STL
new file mode 100644
index 0000000..f9c539e
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/middle_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/middle_link0.STL
new file mode 100644
index 0000000..e0246e7
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/middle_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/middle_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/middle_link1.STL
new file mode 100644
index 0000000..905e8ae
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/middle_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/middle_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/middle_link2.STL
new file mode 100644
index 0000000..44d98b6
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/middle_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/middle_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/middle_link3.STL
new file mode 100644
index 0000000..f3d5d24
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/middle_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link0.STL
new file mode 100644
index 0000000..0644bed
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link1.STL
new file mode 100644
index 0000000..75b6914
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link2.STL
new file mode 100644
index 0000000..0d7e140
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link3.STL
new file mode 100644
index 0000000..d2cb151
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link4.STL
new file mode 100644
index 0000000..0ac9e8f
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link0.STL
new file mode 100644
index 0000000..eb73343
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link1.STL
new file mode 100644
index 0000000..a93e934
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link2.STL
new file mode 100644
index 0000000..c429e07
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link3.STL
new file mode 100644
index 0000000..0c6599a
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link4.STL
new file mode 100644
index 0000000..8476b29
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link5.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link5.STL
new file mode 100644
index 0000000..b043327
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link5.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/base_link.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/base_link.STL
new file mode 100644
index 0000000..334f5df
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/base_link.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link0.STL
new file mode 100644
index 0000000..35c6b91
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link1.STL
new file mode 100644
index 0000000..2ef499e
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link2.STL
new file mode 100644
index 0000000..7f4b8d0
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link3.STL
new file mode 100644
index 0000000..d7b27fb
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link4.STL
new file mode 100644
index 0000000..e53e3f5
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/linker_hand_l20_left.urdf b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/linker_hand_l20_left.urdf
new file mode 100644
index 0000000..3105751
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/linker_hand_l20_left.urdf
@@ -0,0 +1,1536 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/linker_hand_l20_left.xml b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/linker_hand_l20_left.xml
new file mode 100644
index 0000000..f8e6968
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/linker_hand_l20_left.xml
@@ -0,0 +1,225 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link0.STL
new file mode 100644
index 0000000..4f236e6
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link1.STL
new file mode 100644
index 0000000..e20c785
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link2.STL
new file mode 100644
index 0000000..630aff8
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link3.STL
new file mode 100644
index 0000000..f6262b5
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link4.STL
new file mode 100644
index 0000000..6511b29
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link0.STL
new file mode 100644
index 0000000..48a30ea
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link1.STL
new file mode 100644
index 0000000..40796aa
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link2.STL
new file mode 100644
index 0000000..81c524a
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link3.STL
new file mode 100644
index 0000000..5df3b80
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link4.STL
new file mode 100644
index 0000000..b7c61cb
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link0.STL
new file mode 100644
index 0000000..3714a04
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link1.STL
new file mode 100644
index 0000000..0776fd0
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link2.STL
new file mode 100644
index 0000000..630aff8
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link3.STL
new file mode 100644
index 0000000..d5e6a4b
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link4.STL
new file mode 100644
index 0000000..f002fb5
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link0.STL
new file mode 100644
index 0000000..481bc60
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link1.STL
new file mode 100644
index 0000000..faccfca
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link2.STL
new file mode 100644
index 0000000..fdb350f
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link3.STL
new file mode 100644
index 0000000..cb95087
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link4.STL
new file mode 100644
index 0000000..fc217c7
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link5.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link5.STL
new file mode 100644
index 0000000..58b8cf1
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link5.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/base_link.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/base_link.STL
new file mode 100644
index 0000000..6ecefe4
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/base_link.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link0.STL
new file mode 100644
index 0000000..a572166
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link1.STL
new file mode 100644
index 0000000..869fdbc
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link2.STL
new file mode 100644
index 0000000..5f5ffc3
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link3.STL
new file mode 100644
index 0000000..6da1cc2
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link4.STL
new file mode 100644
index 0000000..c1f6ec7
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/linker_hand_l20_right.urdf b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/linker_hand_l20_right.urdf
new file mode 100644
index 0000000..e4f0d52
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/linker_hand_l20_right.urdf
@@ -0,0 +1,1536 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/linker_hand_l20_right.xml b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/linker_hand_l20_right.xml
new file mode 100644
index 0000000..440a833
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/linker_hand_l20_right.xml
@@ -0,0 +1,202 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link0.STL
new file mode 100644
index 0000000..25e4401
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link1.STL
new file mode 100644
index 0000000..40bbdef
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link2.STL
new file mode 100644
index 0000000..27609a0
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link3.STL
new file mode 100644
index 0000000..6da1cc2
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link4.STL
new file mode 100644
index 0000000..1d6e30e
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link0.STL
new file mode 100644
index 0000000..dde5eca
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link1.STL
new file mode 100644
index 0000000..6aa00ce
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link2.STL
new file mode 100644
index 0000000..4fdc281
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link3.STL
new file mode 100644
index 0000000..6da1cc2
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link4.STL
new file mode 100644
index 0000000..f09fc8e
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link0.STL
new file mode 100644
index 0000000..f5f3e87
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link1.STL
new file mode 100644
index 0000000..fe8a90d
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link2.STL
new file mode 100644
index 0000000..136c34d
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link3.STL
new file mode 100644
index 0000000..6da1cc2
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link4.STL
new file mode 100644
index 0000000..c97b64c
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link0.STL
new file mode 100644
index 0000000..7b1b3c2
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link1.STL
new file mode 100644
index 0000000..ed67d9f
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link2.STL
new file mode 100644
index 0000000..d76314c
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link3.STL
new file mode 100644
index 0000000..332665c
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link4.STL
new file mode 100644
index 0000000..7bd7b82
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link5.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link5.STL
new file mode 100644
index 0000000..9cf2093
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link5.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/hand_base_link.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/hand_base_link.STL
new file mode 100644
index 0000000..3c8a804
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/hand_base_link.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/index_metacarpals.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/index_metacarpals.STL
new file mode 100644
index 0000000..dee80a8
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/index_metacarpals.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/index_middle.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/index_middle.STL
new file mode 100644
index 0000000..d283916
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/index_middle.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/index_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/index_proximal.STL
new file mode 100644
index 0000000..bef360b
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/index_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/linker_hand_l21_left.urdf b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/linker_hand_l21_left.urdf
new file mode 100644
index 0000000..0173301
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/linker_hand_l21_left.urdf
@@ -0,0 +1,1033 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/linker_hand_l21_left.xml b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/linker_hand_l21_left.xml
new file mode 100644
index 0000000..6532f89
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/linker_hand_l21_left.xml
@@ -0,0 +1,163 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/linkerhand_l21_left.xml.bak b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/linkerhand_l21_left.xml.bak
new file mode 100644
index 0000000..b598a60
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/linkerhand_l21_left.xml.bak
@@ -0,0 +1,183 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ >
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/middle_metacarpals.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/middle_metacarpals.STL
new file mode 100644
index 0000000..0d4a05d
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/middle_metacarpals.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/middle_middle.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/middle_middle.STL
new file mode 100644
index 0000000..73f8068
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/middle_middle.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/middle_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/middle_proximal.STL
new file mode 100644
index 0000000..0adfd93
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/middle_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/pinky_metacarpals.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/pinky_metacarpals.STL
new file mode 100644
index 0000000..c7c6e46
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/pinky_metacarpals.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/pinky_middle.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/pinky_middle.STL
new file mode 100644
index 0000000..1e27abf
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/pinky_middle.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/pinky_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/pinky_proximal.STL
new file mode 100644
index 0000000..3e1bd8a
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/pinky_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/ring_metacarpals.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/ring_metacarpals.STL
new file mode 100644
index 0000000..3fb6123
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/ring_metacarpals.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/ring_middle.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/ring_middle.STL
new file mode 100644
index 0000000..6986186
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/ring_middle.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/ring_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/ring_proximal.STL
new file mode 100644
index 0000000..d0f1637
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/ring_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_distal.STL
new file mode 100644
index 0000000..2eb54d8
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_distal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_metacarpals.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_metacarpals.STL
new file mode 100644
index 0000000..e4f3158
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_metacarpals.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_metacarpals_base1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..da39fc0
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_metacarpals_base1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_metacarpals_base2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..3d24ae2
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_metacarpals_base2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_proximal.STL
new file mode 100644
index 0000000..6b7f4d1
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/hand_base_link.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/hand_base_link.STL
new file mode 100644
index 0000000..9d1ea49
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/hand_base_link.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/index_metacarpals.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/index_metacarpals.STL
new file mode 100644
index 0000000..e0e4944
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/index_metacarpals.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/index_middle.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/index_middle.STL
new file mode 100644
index 0000000..7d16604
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/index_middle.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/index_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/index_proximal.STL
new file mode 100644
index 0000000..c001f27
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/index_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/linker_hand_l21_right.urdf b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/linker_hand_l21_right.urdf
new file mode 100644
index 0000000..f42dc99
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/linker_hand_l21_right.urdf
@@ -0,0 +1,1033 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/linker_hand_l21_right.xml b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/linker_hand_l21_right.xml
new file mode 100644
index 0000000..8a2f735
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/linker_hand_l21_right.xml
@@ -0,0 +1,163 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/middle_metacarpals.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/middle_metacarpals.STL
new file mode 100644
index 0000000..56cae1c
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/middle_metacarpals.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/middle_middle.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/middle_middle.STL
new file mode 100644
index 0000000..9c22903
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/middle_middle.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/middle_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/middle_proximal.STL
new file mode 100644
index 0000000..534cff7
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/middle_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/pinky_metacarpals.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/pinky_metacarpals.STL
new file mode 100644
index 0000000..e78b9ca
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/pinky_metacarpals.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/pinky_middle.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/pinky_middle.STL
new file mode 100644
index 0000000..05fb04f
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/pinky_middle.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/pinky_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/pinky_proximal.STL
new file mode 100644
index 0000000..992adda
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/pinky_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/ring_metacarpals.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/ring_metacarpals.STL
new file mode 100644
index 0000000..96c1afa
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/ring_metacarpals.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/ring_middle.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/ring_middle.STL
new file mode 100644
index 0000000..fce4efb
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/ring_middle.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/ring_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/ring_proximal.STL
new file mode 100644
index 0000000..69a8833
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/ring_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_distal.STL
new file mode 100644
index 0000000..36c6f8e
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_distal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_metacarpals.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_metacarpals.STL
new file mode 100644
index 0000000..40d76f2
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_metacarpals.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_metacarpals_base1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..3fdfee8
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_metacarpals_base1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_metacarpals_base2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..5585fe8
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_metacarpals_base2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_proximal.STL
new file mode 100644
index 0000000..3f5b5af
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/hand_base_link.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/hand_base_link.STL
new file mode 100644
index 0000000..b7ab0c1
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/hand_base_link.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/index_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/index_distal.STL
new file mode 100644
index 0000000..db30679
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/index_distal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/index_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/index_proximal.STL
new file mode 100644
index 0000000..f427e3c
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/index_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/linker_hand_l6_left.xml b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/linker_hand_l6_left.xml
new file mode 100644
index 0000000..1761791
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/linker_hand_l6_left.xml
@@ -0,0 +1,127 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/linkerhand_o6_left.urdf b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/linkerhand_o6_left.urdf
new file mode 100644
index 0000000..884c3fb
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/linkerhand_o6_left.urdf
@@ -0,0 +1,706 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/middle_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/middle_distal.STL
new file mode 100644
index 0000000..a4838a5
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/middle_distal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/middle_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/middle_proximal.STL
new file mode 100644
index 0000000..e8d4268
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/middle_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/pinky_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/pinky_distal.STL
new file mode 100644
index 0000000..abcb9c0
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/pinky_distal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/pinky_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/pinky_proximal.STL
new file mode 100644
index 0000000..5f013e6
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/pinky_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/ring_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/ring_distal.STL
new file mode 100644
index 0000000..b8247b1
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/ring_distal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/ring_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/ring_proximal.STL
new file mode 100644
index 0000000..7c47ff6
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/ring_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/thumb_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/thumb_distal.STL
new file mode 100644
index 0000000..19eee73
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/thumb_distal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/thumb_metacarpals.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/thumb_metacarpals.STL
new file mode 100644
index 0000000..9f1ab59
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/thumb_metacarpals.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/thumb_metacarpals_base2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..5e66834
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/thumb_metacarpals_base2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/hand_base_link.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/hand_base_link.STL
new file mode 100644
index 0000000..6ab2f6f
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/hand_base_link.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/index_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/index_distal.STL
new file mode 100644
index 0000000..e530ec5
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/index_distal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/index_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/index_proximal.STL
new file mode 100644
index 0000000..86a10cb
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/index_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/linker_hand_l6_right.xml b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/linker_hand_l6_right.xml
new file mode 100644
index 0000000..3b7a7d1
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/linker_hand_l6_right.xml
@@ -0,0 +1,127 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/linkerhand_o6_right.urdf b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/linkerhand_o6_right.urdf
new file mode 100644
index 0000000..359e81c
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/linkerhand_o6_right.urdf
@@ -0,0 +1,705 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/middle_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/middle_distal.STL
new file mode 100644
index 0000000..1582dfe
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/middle_distal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/middle_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/middle_proximal.STL
new file mode 100644
index 0000000..2a46d3f
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/middle_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/pinky_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/pinky_distal.STL
new file mode 100644
index 0000000..581901e
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/pinky_distal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/pinky_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/pinky_proximal.STL
new file mode 100644
index 0000000..b0326b3
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/pinky_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/ring_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/ring_distal.STL
new file mode 100644
index 0000000..1c2830a
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/ring_distal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/ring_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/ring_proximal.STL
new file mode 100644
index 0000000..b98a3db
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/ring_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/thumb_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/thumb_distal.STL
new file mode 100644
index 0000000..e102dbe
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/thumb_distal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/thumb_metacarpals.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/thumb_metacarpals.STL
new file mode 100644
index 0000000..df9bccf
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/thumb_metacarpals.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/thumb_metacarpals_base2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..0546e91
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/thumb_metacarpals_base2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/base_link.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/base_link.STL
new file mode 100644
index 0000000..66bd83f
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/base_link.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link0.STL
new file mode 100644
index 0000000..8eb082a
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link1.STL
new file mode 100644
index 0000000..44d513f
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link2.STL
new file mode 100644
index 0000000..b62a597
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link3.STL
new file mode 100644
index 0000000..8c29da2
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link4.STL
new file mode 100644
index 0000000..dd215f0
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/linker_hand_l7_left.xml b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/linker_hand_l7_left.xml
new file mode 100644
index 0000000..a6ab383
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/linker_hand_l7_left.xml
@@ -0,0 +1,197 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/linkerhand_o7_left.urdf b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/linkerhand_o7_left.urdf
new file mode 100644
index 0000000..bc956d9
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/linkerhand_o7_left.urdf
@@ -0,0 +1,1512 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link0.STL
new file mode 100644
index 0000000..754c500
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link1.STL
new file mode 100644
index 0000000..194847e
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link2.STL
new file mode 100644
index 0000000..fd6c112
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link3.STL
new file mode 100644
index 0000000..1fccf5c
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link4.STL
new file mode 100644
index 0000000..67a0339
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/middle_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/middle_link0.STL
new file mode 100644
index 0000000..09ab875
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/middle_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/middle_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/middle_link1.STL
new file mode 100644
index 0000000..4813804
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/middle_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/middle_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/middle_link2.STL
new file mode 100644
index 0000000..fda4dff
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/middle_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/middle_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/middle_link3.STL
new file mode 100644
index 0000000..a3746e9
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/middle_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link0.STL
new file mode 100644
index 0000000..acfbe14
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link1.STL
new file mode 100644
index 0000000..365d4c8
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link2.STL
new file mode 100644
index 0000000..7115054
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link3.STL
new file mode 100644
index 0000000..c848977
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link4.STL
new file mode 100644
index 0000000..77b4fc6
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link0.STL
new file mode 100644
index 0000000..995681c
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link1.STL
new file mode 100644
index 0000000..48a0bb7
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link2.STL
new file mode 100644
index 0000000..6f32a9a
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link3.STL
new file mode 100644
index 0000000..67276b2
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link4.STL
new file mode 100644
index 0000000..06fff54
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link5.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link5.STL
new file mode 100644
index 0000000..0313140
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link5.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/base_link.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/base_link.STL
new file mode 100644
index 0000000..a13c5e9
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/base_link.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link0.STL
new file mode 100644
index 0000000..79be9d9
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link1.STL
new file mode 100644
index 0000000..ff23d89
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link2.STL
new file mode 100644
index 0000000..1562779
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link3.STL
new file mode 100644
index 0000000..c1b7332
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link4.STL
new file mode 100644
index 0000000..024d190
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/linker_hand_l7_right.urdf b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/linker_hand_l7_right.urdf
new file mode 100644
index 0000000..733e337
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/linker_hand_l7_right.urdf
@@ -0,0 +1,1537 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/linker_hand_l7_right.xml b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/linker_hand_l7_right.xml
new file mode 100644
index 0000000..8c67e4a
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/linker_hand_l7_right.xml
@@ -0,0 +1,180 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link0.STL
new file mode 100644
index 0000000..925cac8
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link1.STL
new file mode 100644
index 0000000..a204f8a
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link2.STL
new file mode 100644
index 0000000..b686aa4
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link3.STL
new file mode 100644
index 0000000..9914fcf
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link4.STL
new file mode 100644
index 0000000..f9c539e
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/middle_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/middle_link0.STL
new file mode 100644
index 0000000..e0246e7
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/middle_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/middle_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/middle_link1.STL
new file mode 100644
index 0000000..905e8ae
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/middle_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/middle_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/middle_link2.STL
new file mode 100644
index 0000000..44d98b6
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/middle_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/middle_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/middle_link3.STL
new file mode 100644
index 0000000..f3d5d24
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/middle_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link0.STL
new file mode 100644
index 0000000..0644bed
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link1.STL
new file mode 100644
index 0000000..75b6914
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link2.STL
new file mode 100644
index 0000000..0d7e140
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link3.STL
new file mode 100644
index 0000000..d2cb151
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link4.STL
new file mode 100644
index 0000000..0ac9e8f
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link0.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link0.STL
new file mode 100644
index 0000000..eb73343
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link0.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link1.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link1.STL
new file mode 100644
index 0000000..a93e934
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link1.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link2.STL
new file mode 100644
index 0000000..c429e07
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link3.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link3.STL
new file mode 100644
index 0000000..0c6599a
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link3.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link4.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link4.STL
new file mode 100644
index 0000000..8476b29
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link4.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link5.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link5.STL
new file mode 100644
index 0000000..b043327
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link5.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/hand_base_link.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/hand_base_link.STL
new file mode 100644
index 0000000..b7ab0c1
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/hand_base_link.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/index_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/index_distal.STL
new file mode 100644
index 0000000..db30679
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/index_distal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/index_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/index_proximal.STL
new file mode 100644
index 0000000..f427e3c
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/index_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/linker_hand_o6_left.xml b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/linker_hand_o6_left.xml
new file mode 100644
index 0000000..45b4e90
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/linker_hand_o6_left.xml
@@ -0,0 +1,157 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/middle_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/middle_distal.STL
new file mode 100644
index 0000000..a4838a5
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/middle_distal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/middle_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/middle_proximal.STL
new file mode 100644
index 0000000..e8d4268
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/middle_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/pinky_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/pinky_distal.STL
new file mode 100644
index 0000000..abcb9c0
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/pinky_distal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/pinky_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/pinky_proximal.STL
new file mode 100644
index 0000000..5f013e6
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/pinky_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/ring_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/ring_distal.STL
new file mode 100644
index 0000000..b8247b1
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/ring_distal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/ring_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/ring_proximal.STL
new file mode 100644
index 0000000..7c47ff6
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/ring_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/thumb_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/thumb_distal.STL
new file mode 100644
index 0000000..19eee73
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/thumb_distal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/thumb_metacarpals.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/thumb_metacarpals.STL
new file mode 100644
index 0000000..9f1ab59
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/thumb_metacarpals.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/thumb_metacarpals_base2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..5e66834
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/thumb_metacarpals_base2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/hand_base_link.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/hand_base_link.STL
new file mode 100644
index 0000000..b7ab0c1
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/hand_base_link.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/index_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/index_distal.STL
new file mode 100644
index 0000000..db30679
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/index_distal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/index_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/index_proximal.STL
new file mode 100644
index 0000000..f427e3c
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/index_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/linker_hand_o6_right.xml b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/linker_hand_o6_right.xml
new file mode 100644
index 0000000..8723091
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/linker_hand_o6_right.xml
@@ -0,0 +1,157 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/middle_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/middle_distal.STL
new file mode 100644
index 0000000..a4838a5
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/middle_distal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/middle_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/middle_proximal.STL
new file mode 100644
index 0000000..e8d4268
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/middle_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/pinky_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/pinky_distal.STL
new file mode 100644
index 0000000..abcb9c0
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/pinky_distal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/pinky_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/pinky_proximal.STL
new file mode 100644
index 0000000..5f013e6
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/pinky_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/ring_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/ring_distal.STL
new file mode 100644
index 0000000..b8247b1
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/ring_distal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/ring_proximal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/ring_proximal.STL
new file mode 100644
index 0000000..7c47ff6
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/ring_proximal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/thumb_distal.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/thumb_distal.STL
new file mode 100644
index 0000000..19eee73
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/thumb_distal.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/thumb_metacarpals.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/thumb_metacarpals.STL
new file mode 100644
index 0000000..9f1ab59
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/thumb_metacarpals.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/thumb_metacarpals_base2.STL b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..5e66834
Binary files /dev/null and b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/thumb_metacarpals_base2.STL differ
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/utils/__init__.py b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/utils/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/utils/joint_monitor.py b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/utils/joint_monitor.py
new file mode 100644
index 0000000..a228a10
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/utils/joint_monitor.py
@@ -0,0 +1,148 @@
+"""MuJoCo 关节实时监控面板（进度条显示各关节角度）。"""
+
+from PyQt5.QtCore import Qt, pyqtSignal
+from PyQt5.QtGui import QFont
+from PyQt5.QtWidgets import (
+ QFrame,
+ QHBoxLayout,
+ QLabel,
+ QProgressBar,
+ QScrollArea,
+ QSizePolicy,
+ QVBoxLayout,
+ QWidget,
+)
+
+
+class JointMonitorWindow(QWidget):
+ """深色 Joint 面板：左侧关节名，右侧进度条 + 数值。"""
+
+ values_updated = pyqtSignal(object) # list[float]
+
+ def __init__(self, joint_names, joint_ranges, title="Joint", parent=None):
+ super().__init__(parent)
+ self.joint_names = list(joint_names)
+ self.joint_ranges = [(float(lo), float(hi)) for lo, hi in joint_ranges]
+ self._bars = []
+ self._value_labels = []
+
+ self.setWindowTitle(f"{title} Monitor")
+ self.setMinimumWidth(760)
+ self.setMinimumHeight(860)
+ self.resize(860, 980)
+ self.setStyleSheet(
+ """
+ QWidget {
+ background-color: #2b2b2b;
+ color: #d0d0d0;
+ font-size: 22px;
+ }
+ QLabel#header {
+ background-color: #5a1a1a;
+ color: #f0f0f0;
+ font-size: 30px;
+ font-weight: bold;
+ padding: 18px 22px;
+ border-radius: 4px;
+ }
+ QLabel#jointName {
+ color: #e8e8e8;
+ font-size: 22px;
+ font-weight: 600;
+ }
+ QLabel#jointValue {
+ color: #ffffff;
+ font-size: 24px;
+ font-weight: bold;
+ min-width: 140px;
+ }
+ QProgressBar {
+ border: 2px solid #666;
+ border-radius: 5px;
+ background-color: #1e1e1e;
+ text-align: center;
+ color: #e0e0e0;
+ min-height: 38px;
+ max-height: 38px;
+ }
+ QProgressBar::chunk {
+ background-color: #a0a0a0;
+ border-radius: 4px;
+ }
+ QScrollArea {
+ border: none;
+ }
+ """
+ )
+
+ root = QVBoxLayout(self)
+ root.setContentsMargins(18, 18, 18, 18)
+ root.setSpacing(16)
+
+ header = QLabel(title)
+ header.setObjectName("header")
+ header.setFont(QFont("Sans Serif", 24, QFont.Bold))
+ root.addWidget(header)
+
+ scroll = QScrollArea()
+ scroll.setWidgetResizable(True)
+ scroll.setHorizontalScrollBarPolicy(Qt.ScrollBarAlwaysOff)
+ body = QWidget()
+ body_layout = QVBoxLayout(body)
+ body_layout.setContentsMargins(8, 8, 8, 8)
+ body_layout.setSpacing(14)
+
+ for name, (lo, hi) in zip(self.joint_names, self.joint_ranges):
+ row = QFrame()
+ row_layout = QHBoxLayout(row)
+ row_layout.setContentsMargins(6, 6, 6, 6)
+ row_layout.setSpacing(18)
+
+ name_label = QLabel(name)
+ name_label.setObjectName("jointName")
+ name_label.setMinimumWidth(260)
+ name_label.setSizePolicy(QSizePolicy.Fixed, QSizePolicy.Preferred)
+
+ bar = QProgressBar()
+ bar.setRange(0, 1000)
+ bar.setValue(0)
+ bar.setFormat("")
+ bar.setTextVisible(False)
+
+ value_label = QLabel("0.000")
+ value_label.setObjectName("jointValue")
+ value_label.setAlignment(Qt.AlignRight | Qt.AlignVCenter)
+
+ row_layout.addWidget(name_label)
+ row_layout.addWidget(bar, stretch=1)
+ row_layout.addWidget(value_label)
+ body_layout.addWidget(row)
+ self._bars.append(bar)
+ self._value_labels.append(value_label)
+
+ body_layout.addStretch(1)
+ scroll.setWidget(body)
+ root.addWidget(scroll)
+
+ self.values_updated.connect(self._on_values_updated)
+
+ def update_values(self, values):
+ """线程安全：任意线程调用，通过信号刷新 UI。"""
+ self.values_updated.emit(list(values))
+
+ def _on_values_updated(self, values):
+ for i, val in enumerate(values):
+ if i >= len(self._bars):
+ break
+ lo, hi = self.joint_ranges[i]
+ span = hi - lo
+ if span <= 1e-9:
+ pct = 0
+ else:
+ pct = int(1000 * max(0.0, min(1.0, (float(val) - lo) / span)))
+ self._bars[i].setValue(pct)
+ if abs(val) < 1e-4 and val != 0.0:
+ text = f"{val:.2e}"
+ else:
+ text = f"{val:.3f}"
+ self._value_labels[i].setText(text)
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/utils/mapping.py b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/utils/mapping.py
new file mode 100644
index 0000000..6816d1c
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/utils/mapping.py
@@ -0,0 +1,240 @@
+import sys,os
+# /---------------------------------------------------------------
+L6_JOINT_MAP = {
+ 0:1, 1:0, 2:0, 3:2, 4:2, 5:3, 6:3, 7:4, 8:4, 9:5, 10:5
+ }
+L6_JOINT_ARC = [(0, 1.54), (0, 0.52), (0,0.96), (0,1.57), (0,1.40), (0,1.57), (0,1.40), (0,1.57), (0,1.40), (0,1.57), (0,1.40)]
+
+# O6：与 L6 同构（6 路控制），使用 mujoco_testwork 验证后的关节限位
+# actuator 顺序: yaw, pitch, ip, index_mcp, index_dip, middle_*, ring_*, pinky_*
+O6_JOINT_MAP = {
+ 0: 1, 1: 0, 2: 0, 3: 2, 4: 2, 5: 3, 6: 3, 7: 4, 8: 4, 9: 5, 10: 5
+ }
+O6_JOINT_ARC = [
+ (0, 1.36), (0, 0.58), (0, 1.08),
+ (0, 1.60), (0, 1.43),
+ (0, 1.60), (0, 1.43),
+ (0, 1.60), (0, 1.43),
+ (0, 1.60), (0, 1.43),
+]
+# mimic: (slave_actuator_idx, master_actuator_idx, ratio)
+O6_MIMIC = [
+ (2, 1, 1.86), # thumb_ip <- thumb_cmc_pitch
+ (4, 3, 0.89), # index_dip <- index_mcp
+ (6, 5, 0.89),
+ (8, 7, 0.89),
+ (10, 9, 0.89),
+]
+
+# /---------------------------------------------------------------
+L7_JOINT_MAP = {
+ 0: 6, 1: 1, 2: 0, 3: 0, 4: 0,
+ 5: 2, 6: 2, 7: 2,
+ 8: 3, 9: 3, 10: 3,
+ 11: 4, 12: 4, 13: 4,
+ 14: 5, 15: 5, 16: 5
+}
+L7_JOINT_ARC = [(-0.52,1.01), (0,1.43), (0,0.44), (0,1.45), (0,1.57), (0,1.41), (0,1.62), (0,0.96), (0,1.41), (0,1.62), (0,0.96), (0,1.41), (0,1.62), (0,0.96), (0,1.41), (0,1.62), (0,0.96)]
+# /---------------------------------------------------------------
+
+L10_JOINT_MAP = {
+ 0: 9, 1: 1, 2: 0, 3: 0, 4: 0, 5: 6,
+ 6: 2, 7: 2, 8: 2, 9: 3, 10: 3, 11: 3,
+ 12: 7, 13: 4, 14: 4, 15: 4, 16: 8, 17: 5,
+ 18: 5, 19: 5
+}
+L10_JOINT_ARC = [(-0.1396, 0.349), (0, 1.57), (-1.57, 0), (-1.57, 0), (-1.57, 0), (-0.26, 0.26), (0, 1.396), (0, 1.57), (0, 1.57), (0, 1.57), (0, 1.57), (0, 1.57), (-0.26, 0.26), (0, 1.57), (0, 1.57), (0, 1.57), (-0.26, 0.26), (0, 1.57), (0, 1.57), (0, 1.57)]
+# /---------------------------------------------------------------
+L20_JOINT_MAP = {
+ 0: 10, 1: 5, 2: 0, 3: 15, 4: 15,
+ 5: 6, 6: 1, 7: 16, 8: 16,
+ 9: 7, 10: 2, 11: 17, 12: 17,
+ 13: 8, 14: 3, 15: 18, 16: 18,
+ 17: 9, 18: 4, 19: 19, 20:19
+}
+
+
+L20_JOINT_ARC = [(-0.297,0.683), (0.122,1.78), (0,0.87), (0,1.29), (0,1.29), (-0.26,0.26), (0,1.4), (0,1.08), (0,1.15), (-0.26,0.26), (0,1.4), (0,1.08), (0,1.15), (-0.26,0.26), (0,1.4), (0,1.08), (0,1.15), (-0.26,0.26), (0,1.4), (0,1.08), (0,1.15)]
+
+# /---------------------------------------------------------------
+# 注意L21的拇指控制为数组后4位
+L21_JOINT_MAP = {
+ 0: 6, 1: 1, 2: 21,
+ 3: 7, 4: 2, 5: 22,
+ 6: 8, 7: 3, 8: 23,
+ 9: 9, 10: 4, 11: 24,
+ 12: 10, 13: 5,
+ 14: 0, 15: 15, 16: 20
+}
+L21_JOINT_ARC = [(-0.18, 0.18),(0, 1.57),(0, 1.57),(-0.18, 0.18),(0, 1.57),(0, 1.57),(-0.18, 0.18),(0, 1.57),(0, 1.57),(0, 0.18),(0, 1.57),(0, 1.57),(-0.6, 0.6),(0, 1.6),(0, 1),(0, 1.57),(0, 1.57)]
+
+
+
+#---------------------------------------------------------------------------------------------------
+# L6 L
+l6_l_min = [0, 0, 0, 0, 0, 0]
+l6_l_max = [1.54, 1.52, 1.57, 1.57, 1.57, 1.57]
+l6_l_derict = [-1, -1, -1, -1, -1, -1]
+# L6 R
+l6_r_min = [0, 0, 0, 0, 0, 0]
+l6_r_max = [1.54, 1.52, 1.57, 1.57, 1.57, 1.57]
+l6_r_derict = [-1, -1, -1, -1, -1, -1]
+# O6 L [拇指弯曲, 拇指横摆, 食指, 中指, 无名指, 小指]
+o6_l_min = [0, 0, 0, 0, 0, 0]
+o6_l_max = [0.58, 1.30, 1.60, 1.60, 1.60, 1.60]
+o6_l_derict = [-1, -1, -1, -1, -1, -1]
+# O6 R
+o6_r_min = [0, 0, 0, 0, 0, 0]
+o6_r_max = [0.58, 1.36, 1.60, 1.60, 1.60, 1.60]
+o6_r_derict = [-1, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+# L7 L OK
+l7_l_min = [0, 0, 0, 0, 0, 0, -0.52]
+l7_l_max = [0.44, 1.43, 1.62, 1.62, 1.62, 1.62, 1.01]
+l7_l_derict = [-1, -1, -1, -1, -1, -1, -1]
+# L7 R OK (urdf后续会更改！！！)
+l7_r_min = [0, -1.43, 0, 0, 0, 0, 0]
+l7_r_max = [0.75, 0, 1.62, 1.62, 1.62, 1.62, 1.54]
+l7_r_derict = [-1, 0, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+# L10 L OK
+l10_l_min = [0, 0, 0, 0, 0, 0, 0, -0.26, -0.26, -0.52]
+l10_l_max = [1.45, 1.43, 1.62, 1.62, 1.62, 1.62, 0.26, 0, 0, 1.01]
+l10_l_derict = [-1, -1, -1, -1, -1, -1, 0, -1, -1, -1]
+# L10 R OK
+l10_r_min = [0, 0, 0, 0, 0, 0, -0.26, 0, 0, -0.52]
+l10_r_max = [0.75, 1.43, 1.62, 1.62, 1.62, 1.62, 0, 0.13, 0.26, 1.01]
+l10_r_derict = [-1, -1, -1, -1, -1, -1, -1, 0, 0, -1]
+#---------------------------------------------------------------------------------------------------
+# L20 L OK
+l20_l_min = [0, 0, 0, 0, 0, -0.297, -0.26, -0.26, -0.26, -0.26, 0.122, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l20_l_max = [0.87, 1.4, 1.4, 1.4, 1.4, 0.683, 0.26, 0.26, 0.26, 0.26, 1.78, 0, 0, 0, 0, 1.29, 1.08, 1.08, 1.08, 1.08]
+l20_l_derict = [-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
+# L20 R OK
+l20_r_min = [0, 0, 0, 0, 0, -0.297, -0.26, -0.26, -0.26, -0.26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l20_r_max = [0.87, 1.4, 1.4, 1.4, 1.4, 0.683, 0.26, 0.26, 0.26, 0.26, 1.78, 0, 0, 0, 0, 1.29, 1.08, 1.08, 1.08, 1.08]
+l20_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+# L21 L OK
+l21_l_min = [0, 0, 0, 0, 0, 0, 0, -0.18, -0.18, 0, -0.6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l21_l_max = [1, 1.57, 1.57, 1.57, 1.57, 1.6, 0.18, 0.18, 0.18, 0.18, 0.6, 0, 0, 0, 0, 1.57, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57]
+l21_l_derict = [-1, -1, -1, -1, -1, -1, -1, -1, -1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
+# L21 R OK
+l21_r_min = [0, 0, 0, 0, 0, 0, -0.18, -0.18, -0.18, -0.18, -0.6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l21_r_max = [1, 1.57, 1.57, 1.57, 1.57, 1.6, 0.18, 0.18, 0.18, 0.18, 0.6, 0, 0, 0, 0, 1.57, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57]
+l21_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+#---------------------------------------------------------------------------------------------------
+# L25 L OK
+l25_l_min = [0, 0, 0, 0, 0, 0, -0.26, -0.26, -0.26, -0.26, -0.26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l25_l_max = [0.9, 1.57, 1.57, 1.57, 1.57, 1.3, 0.26, 0.26, 0.26, 0.26, 0.61, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57]
+l25_l_derict = [-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1]
+# L25 R OK
+l25_r_min = [0, 0, 0, 0, 0, 0, -0.26, -0.26, -0.26, -0.26, -0.26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l25_r_max = [0.9, 1.57, 1.57, 1.57, 1.57, 1.3, 0.26, 0.26, 0.26, 0.26, 0.61, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57]
+l25_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+
+def range_to_arc_left(left_range,hand_joint):
+ num=0
+ if hand_joint in ("L6", "O6"):
+ num = 6
+ if hand_joint == "O6":
+ l_min, l_max, l_derict = o6_l_min, o6_l_max, o6_l_derict
+ else:
+ l_min, l_max, l_derict = l6_l_min, l6_l_max, l6_l_derict
+ elif hand_joint == "L7":
+ num = 7
+ l_min = l7_l_min
+ l_max = l7_l_max
+ l_derict = l7_l_derict
+ elif hand_joint == "L10":
+ num = 10
+ l_min = l10_l_min
+ l_max = l10_l_max
+ l_derict = l10_l_derict
+ elif hand_joint == "L20":
+ num = 20
+ l_min = l20_l_min
+ l_max = l20_l_max
+ l_derict = l20_l_derict
+ elif hand_joint == "L21":
+ num = 25
+ l_min = l21_l_min
+ l_max = l21_l_max
+ l_derict = l21_l_derict
+ hand_arc = [0] * num
+ for i in range(num):
+ if hand_joint == "L20":
+ if 11 <= i <= 14: continue
+ if hand_joint == "L21":
+ if 11 <= i <= 14: continue
+ if 16 <= i <= 19: continue
+ val_l = is_within_range(left_range[i], 0, 255)
+ if l_derict[i] == -1:
+ hand_arc[i] = scale_value(val_l, 0, 255, l_max[i], l_min[i])
+ else:
+ hand_arc[i] = scale_value(val_l, 0, 255, l_min[i], l_max[i])
+ return hand_arc
+
+def range_to_arc_right(right_range,hand_joint):
+ num=0
+ if hand_joint in ("L6", "O6"):
+ num = 6
+ if hand_joint == "O6":
+ r_min, r_max, r_derict = o6_r_min, o6_r_max, o6_r_derict
+ else:
+ r_min, r_max, r_derict = l6_r_min, l6_r_max, l6_r_derict
+ elif hand_joint == "L7":
+ num = 7
+ r_min = l7_r_min
+ r_max = l7_r_max
+ r_derict = l7_r_derict
+ elif hand_joint == "L10":
+ num = 10
+ r_min = l10_r_min
+ r_max = l10_r_max
+ r_derict = l10_r_derict
+ elif hand_joint == "L20":
+ num = 20
+ r_min = l20_r_min
+ r_max = l20_r_max
+ r_derict = l20_r_derict
+ elif hand_joint == "L21":
+ num = 25
+ r_min = l21_r_min
+ r_max = l21_r_max
+ r_derict = l21_r_derict
+ hand_arc = [0] * num
+ for i in range(num):
+ if hand_joint == "L20":
+ if 11 <= i <= 14: continue
+ if hand_joint == "L21":
+ if 11 <= i <= 14: continue
+ if 16 <= i <= 19: continue
+ val_r = is_within_range(right_range[i], 0, 255)
+ if r_derict[i] == -1:
+ hand_arc[i] = scale_value(val_r, 0, 255, r_max[i], r_min[i])
+ else:
+ hand_arc[i] = scale_value(val_r, 0, 255, r_min[i], r_max[i])
+ return hand_arc
+
+
+def apply_mimic(ctrl_values, mimic_rules, joint_arc):
+ """按主从比例写入从动关节目标，并裁剪到 ctrlrange。"""
+ if not mimic_rules:
+ return ctrl_values
+ out = list(ctrl_values)
+ for slave, master, ratio in mimic_rules:
+ val = out[master] * ratio
+ lo, hi = joint_arc[slave]
+ out[slave] = max(lo, min(hi, val))
+ return out
+
+
+def scale_value(original_value, a_min, a_max, b_min, b_max):
+ return (original_value - a_min) * (b_max - b_min) / (a_max - a_min) + b_min
+
+
+def is_within_range(value, min_value, max_value):
+ return min(max_value, max(min_value, value))
\ No newline at end of file
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/package.xml b/src/linkerhand-sim/linker_hand_mujoco_ros2/package.xml
new file mode 100644
index 0000000..e53ff61
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/package.xml
@@ -0,0 +1,22 @@
+
+
+
+ linker_hand_mujoco_ros2
+ 0.0.0
+ TODO: Package description
+ linkerhand
+ TODO: License declaration
+
+ rclpy
+ std_msgs
+ sensor_msgs
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+
+
+ ament_python
+
+
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/requirements.txt b/src/linkerhand-sim/linker_hand_mujoco_ros2/requirements.txt
new file mode 100644
index 0000000..b9df699
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/requirements.txt
@@ -0,0 +1,20 @@
+python-can
+dm_env
+pexpect
+pyquaternion
+pyagxrobots
+pycryptodome
+ipython
+h5py
+PyYAML
+tqdm
+wandb
+pybullet
+mediapipe
+pyqt5
+pyqtgraph
+dm_control
+uvicorn
+matplotlib
+sapien
+mujoco
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/resource/linker_hand_mujoco_ros2 b/src/linkerhand-sim/linker_hand_mujoco_ros2/resource/linker_hand_mujoco_ros2
new file mode 100644
index 0000000..e69de29
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/setup.cfg b/src/linkerhand-sim/linker_hand_mujoco_ros2/setup.cfg
new file mode 100644
index 0000000..0b18a0d
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/linker_hand_mujoco_ros2
+[install]
+install_scripts=$base/lib/linker_hand_mujoco_ros2
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/setup.py b/src/linkerhand-sim/linker_hand_mujoco_ros2/setup.py
new file mode 100644
index 0000000..652c296
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/setup.py
@@ -0,0 +1,30 @@
+import os
+from glob import glob
+from setuptools import find_packages, setup
+
+package_name = 'linker_hand_mujoco_ros2'
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=find_packages(exclude=['test']),
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
+ ('share/' + package_name, ['package.xml']),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='linkerhand',
+ maintainer_email='linkerhand@todo.todo',
+ description='TODO: Package description',
+ license='TODO: License declaration',
+ tests_require=['pytest'],
+ entry_points={
+ 'console_scripts': [
+ 'linker_hand_mujoco_ros2_node=linker_hand_mujoco_ros2.linker_hand_mujoco_ros2:main',
+ 'hand_curve_recorder=linker_hand_mujoco_ros2.hand_curve_recorder:main',
+ ],
+ },
+)
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/test/test_copyright.py b/src/linkerhand-sim/linker_hand_mujoco_ros2/test/test_copyright.py
new file mode 100644
index 0000000..97a3919
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/test/test_copyright.py
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found errors'
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/test/test_flake8.py b/src/linkerhand-sim/linker_hand_mujoco_ros2/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/test/test_flake8.py
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+ rc, errors = main_with_errors(argv=[])
+ assert rc == 0, \
+ 'Found %d code style errors / warnings:\n' % len(errors) + \
+ '\n'.join(errors)
diff --git a/src/linkerhand-sim/linker_hand_mujoco_ros2/test/test_pep257.py b/src/linkerhand-sim/linker_hand_mujoco_ros2/test/test_pep257.py
new file mode 100644
index 0000000..b234a38
--- /dev/null
+++ b/src/linkerhand-sim/linker_hand_mujoco_ros2/test/test_pep257.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found code style errors / warnings'
diff --git a/src/linkerhand-sim/requirements.txt b/src/linkerhand-sim/requirements.txt
new file mode 100644
index 0000000..f4a1204
--- /dev/null
+++ b/src/linkerhand-sim/requirements.txt
@@ -0,0 +1,16 @@
+dm_env
+pexpect
+pyquaternion
+pyagxrobots
+pycryptodome
+ipython
+PyYAML
+tqdm
+wandb
+pybullet
+mediapipe
+pyqt5
+pyqtgraph
+dm_control
+uvicorn
+mujoco
\ No newline at end of file
diff --git a/tools/friction_id/sweep_index_plot.py b/tools/friction_id/sweep_index_plot.py
new file mode 100644
index 0000000..287281a
--- /dev/null
+++ b/tools/friction_id/sweep_index_plot.py
@@ -0,0 +1,239 @@
+#!/usr/bin/env python3
+"""O6 单指位置控制：从最小扫到最大，记录并绘制 角度/速度/力矩。"""
+
+from __future__ import annotations
+
+import argparse
+from pathlib import Path
+
+import matplotlib
+
+matplotlib.use("Agg")
+import matplotlib.pyplot as plt
+import mujoco
+import mujoco.viewer
+import numpy as np
+
+ROOT = Path(__file__).resolve().parents[2]
+DEFAULT_XML = (
+ ROOT
+ / "src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2"
+ / "urdf/O6/linker_hand_o6_left/linker_hand_o6_left.xml"
+)
+
+# 四指：根节 mcp_pitch + 远端 dip（mimic 0.89）
+# ROS 6 维索引：0拇指弯 1拇指横摆 2食指 3中指 4无名指 5小指
+FINGER_CFG = {
+ "index": {
+ "mcp": "index_mcp_pitch",
+ "dip": "index_dip",
+ "mcp_act": "index_mcp_pitch_pos",
+ "dip_act": "index_dip_pos",
+ "mimic": 0.89,
+ "ros_idx": 2,
+ },
+ "middle": {
+ "mcp": "middle_mcp_pitch",
+ "dip": "middle_dip",
+ "mcp_act": "middle_mcp_pitch_pos",
+ "dip_act": "middle_dip_pos",
+ "mimic": 0.89,
+ "ros_idx": 3,
+ },
+ "ring": {
+ "mcp": "ring_mcp_pitch",
+ "dip": "ring_dip",
+ "mcp_act": "ring_mcp_pitch_pos",
+ "dip_act": "ring_dip_pos",
+ "mimic": 0.89,
+ "ros_idx": 4,
+ },
+ "pinky": {
+ "mcp": "pinky_mcp_pitch",
+ "dip": "pinky_dip",
+ "mcp_act": "pinky_mcp_pitch_pos",
+ "dip_act": "pinky_dip_pos",
+ "mimic": 0.89,
+ "ros_idx": 5,
+ },
+}
+
+
+def main():
+ parser = argparse.ArgumentParser()
+ parser.add_argument("--xml", type=Path, default=DEFAULT_XML)
+ parser.add_argument("--hand", choices=["left", "right"], default="left")
+ parser.add_argument(
+ "--finger",
+ choices=list(FINGER_CFG.keys()),
+ default="middle",
+ help="要扫的手指（默认 middle）",
+ )
+ parser.add_argument("--duration", type=float, default=8.0, help="单向扫过时长 (s)")
+ parser.add_argument("--out", type=Path, default=None)
+ parser.add_argument("--viewer", action="store_true", help="打开 MuJoCo viewer")
+ args = parser.parse_args()
+
+ cfg = FINGER_CFG[args.finger]
+ if args.out is None:
+ args.out = ROOT / f"reports/O6_{args.finger}_sweep"
+
+ if args.hand == "right":
+ args.xml = (
+ ROOT
+ / "src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2"
+ / "urdf/O6/linker_hand_o6_right/linker_hand_o6_right.xml"
+ )
+
+ args.out.mkdir(parents=True, exist_ok=True)
+ model = mujoco.MjModel.from_xml_path(str(args.xml))
+ data = mujoco.MjData(model)
+
+ mcp_jnt = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, cfg["mcp"])
+ dip_jnt = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, cfg["dip"])
+ mcp_act = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, cfg["mcp_act"])
+ dip_act = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, cfg["dip_act"])
+ if min(mcp_jnt, dip_jnt, mcp_act, dip_act) < 0:
+ raise RuntimeError(f"找不到 {args.finger} joint/actuator，请检查 MJCF")
+
+ mcp_qadr = int(model.jnt_qposadr[mcp_jnt])
+ mcp_dadr = int(model.jnt_dofadr[mcp_jnt])
+ mcp_lo, mcp_hi = map(float, model.jnt_range[mcp_jnt])
+ dip_lo, dip_hi = map(float, model.jnt_range[dip_jnt])
+ mimic = float(cfg["mimic"])
+ dt = float(model.opt.timestep)
+
+ half = args.duration
+ total_t = 2.0 * half
+ n_steps = int(total_t / dt)
+ t_arr = np.arange(n_steps) * dt
+
+ def cmd_at(t: float) -> float:
+ if t <= half:
+ return mcp_lo + (mcp_hi - mcp_lo) * (t / half)
+ u = (t - half) / half
+ return mcp_hi + (mcp_lo - mcp_hi) * u
+
+ data.ctrl[:] = 0.0
+ mujoco.mj_forward(model, data)
+
+ logs = {
+ "t": [],
+ "cmd_mcp": [],
+ "q_mcp": [],
+ "v_mcp": [],
+ "tau_mcp": [],
+ "q_dip": [],
+ "tau_dip": [],
+ }
+
+ viewer = None
+ if args.viewer:
+ viewer = mujoco.viewer.launch_passive(model, data)
+
+ for i in range(n_steps):
+ t = t_arr[i]
+ q_cmd = cmd_at(t)
+ dip_cmd = float(np.clip(q_cmd * mimic, dip_lo, dip_hi))
+ data.ctrl[mcp_act] = q_cmd
+ data.ctrl[dip_act] = dip_cmd
+ mujoco.mj_step(model, data)
+
+ logs["t"].append(t)
+ logs["cmd_mcp"].append(q_cmd)
+ logs["q_mcp"].append(float(data.qpos[mcp_qadr]))
+ logs["v_mcp"].append(float(data.qvel[mcp_dadr]))
+ logs["tau_mcp"].append(float(data.actuator_force[mcp_act]))
+ logs["q_dip"].append(float(data.qpos[int(model.jnt_qposadr[dip_jnt])]))
+ logs["tau_dip"].append(float(data.actuator_force[dip_act]))
+
+ if viewer is not None:
+ viewer.sync()
+
+ if viewer is not None:
+ viewer.close()
+
+ for k in logs:
+ logs[k] = np.asarray(logs[k], dtype=float)
+
+ csv_path = args.out / f"{args.finger}_sweep_{args.hand}.csv"
+ header = "t_s,cmd_mcp_rad,q_mcp_rad,v_mcp_rad_s,tau_mcp_Nm,q_dip_rad,tau_dip_Nm"
+ np.savetxt(
+ csv_path,
+ np.column_stack(
+ [
+ logs["t"],
+ logs["cmd_mcp"],
+ logs["q_mcp"],
+ logs["v_mcp"],
+ logs["tau_mcp"],
+ logs["q_dip"],
+ logs["tau_dip"],
+ ]
+ ),
+ delimiter=",",
+ header=header,
+ comments="",
+ )
+
+ fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
+ fig.suptitle(
+ f"O6 {args.hand} {cfg['mcp']} position sweep "
+ f"range=[{mcp_lo:.2f}, {mcp_hi:.2f}] rad",
+ fontsize=13,
+ )
+
+ axes[0].plot(logs["t"], np.rad2deg(logs["cmd_mcp"]), "k--", lw=1.2, label="cmd")
+ axes[0].plot(logs["t"], np.rad2deg(logs["q_mcp"]), "C0", lw=1.8, label="q")
+ axes[0].set_ylabel("angle (deg)")
+ axes[0].legend(loc="best")
+ axes[0].grid(True, alpha=0.3)
+
+ axes[1].plot(logs["t"], np.rad2deg(logs["v_mcp"]), "C1", lw=1.8, label="v")
+ axes[1].set_ylabel("velocity (deg/s)")
+ axes[1].legend(loc="best")
+ axes[1].grid(True, alpha=0.3)
+
+ axes[2].plot(logs["t"], logs["tau_mcp"], "C3", lw=1.8, label="tau (actuator_force)")
+ axes[2].set_ylabel("torque (N·m)")
+ axes[2].set_xlabel("time (s)")
+ axes[2].legend(loc="best")
+ axes[2].grid(True, alpha=0.3)
+
+ fig.tight_layout()
+ png_path = args.out / f"{args.finger}_sweep_{args.hand}_qvt.png"
+ fig.savefig(png_path, dpi=140)
+ plt.close(fig)
+
+ # ROS 示例：只动该指
+ pos = [255.0] * 6
+ pos_open = pos.copy()
+ pos_close = pos.copy()
+ pos_close[cfg["ros_idx"]] = 0.0
+
+ print("=" * 60)
+ print(f"finger: {args.finger} joint: {cfg['mcp']}")
+ print(f"model: {args.xml}")
+ print(
+ f"range: [{mcp_lo:.3f}, {mcp_hi:.3f}] rad "
+ f"= [{np.rad2deg(mcp_lo):.1f}, {np.rad2deg(mcp_hi):.1f}] deg"
+ )
+ print(f"mimic: dip = mcp * {mimic}")
+ print(f"sweep: {mcp_lo:.3f} -> {mcp_hi:.3f} -> {mcp_lo:.3f} total {total_t:.1f}s")
+ print(f"CSV: {csv_path}")
+ print(f"plot: {png_path}")
+ print(
+ "peak |q|={:.3f} rad |v|={:.3f} rad/s |tau|={:.3f} N·m".format(
+ np.max(np.abs(logs["q_mcp"])),
+ np.max(np.abs(logs["v_mcp"])),
+ np.max(np.abs(logs["tau_mcp"])),
+ )
+ )
+ print("=" * 60)
+ print(f"ROS 只弯{args.finger}（索引 {cfg['ros_idx']}）握紧示例 position:")
+ print(f" {pos_close}")
+ print(f"张开示例: {pos_open}")
+
+
+if __name__ == "__main__":
+ main()
diff --git a/tools/hand_curve_recorder_standalone.py b/tools/hand_curve_recorder_standalone.py
new file mode 100755
index 0000000..739a6ae
--- /dev/null
+++ b/tools/hand_curve_recorder_standalone.py
@@ -0,0 +1,241 @@
+#!/usr/bin/env python3
+"""手部曲线录制 + 画图（单文件，仿真/真机通用）。
+
+依赖（两边都要）：
+ - 已 source ROS2（jazzy）
+ - pip: numpy matplotlib
+ - ros: rclpy sensor_msgs std_msgs
+
+用法示例：
+ python3 hand_curve_recorder_standalone.py --hand left --channel 3 --label sim
+ # 对端发 /cb_left_hand_control_cmd
+ # Ctrl+C → 当前目录下 reports/hand_curves/ 生成 csv + png
+"""
+
+from __future__ import annotations
+
+import argparse
+import csv
+import os
+import time
+from datetime import datetime
+from pathlib import Path
+
+import numpy as np
+
+CHANNEL_NAMES = [
+ "thumb_bend",
+ "thumb_yaw",
+ "index",
+ "middle",
+ "ring",
+ "pinky",
+]
+
+
+def _pad(seq, n, fill=float("nan")):
+ out = [fill] * n
+ for i, v in enumerate(list(seq)[:n]):
+ out[i] = float(v)
+ return out
+
+
+def plot_curves(rows, ch, ch_name, label, have_state, have_current, png_path):
+ import matplotlib
+
+ matplotlib.use("Agg")
+ import matplotlib.pyplot as plt
+
+ t = np.array([r["t_s"] for r in rows], dtype=float)
+ cmd = np.array([r["cmd"][ch] for r in rows], dtype=float)
+ joint = np.array([r["joint"][ch] for r in rows], dtype=float)
+ current = np.array([r["current"][ch] for r in rows], dtype=float)
+
+ y = joint.copy()
+ if np.all(np.isnan(y)):
+ y = cmd.copy()
+ v = np.gradient(y, t)
+ if len(v) >= 5:
+ v = np.convolve(v, np.ones(5) / 5.0, mode="same")
+
+ fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
+ src = "joint" if have_state else "cmd(as joint)"
+ fig.suptitle(
+ f"recorder ch={ch}:{ch_name} label={label} src={src}",
+ fontsize=12,
+ )
+
+ axes[0].plot(t, cmd, "k--", lw=1.2, label="command_u8")
+ axes[0].plot(t, joint, "C0", lw=1.6, label="joint_u8")
+ axes[0].set_ylabel("position (0-255)")
+ axes[0].legend(loc="best")
+ axes[0].grid(True, alpha=0.3)
+
+ axes[1].plot(t, v, "C1", lw=1.6, label="d(joint)/dt")
+ axes[1].set_ylabel("velocity (u8/s)")
+ axes[1].legend(loc="best")
+ axes[1].grid(True, alpha=0.3)
+
+ if have_current:
+ axes[2].plot(t, current, "C3", lw=1.6, label="current/effort")
+ else:
+ axes[2].text(
+ 0.5,
+ 0.5,
+ "no current / state.effort",
+ ha="center",
+ va="center",
+ transform=axes[2].transAxes,
+ )
+ axes[2].set_ylabel("current / effort")
+ axes[2].set_xlabel("time (s)")
+ axes[2].legend(loc="best")
+ axes[2].grid(True, alpha=0.3)
+
+ fig.tight_layout()
+ fig.savefig(png_path, dpi=140)
+ plt.close(fig)
+
+
+def main():
+ parser = argparse.ArgumentParser(description="Hand curve recorder (ROS2, single file)")
+ parser.add_argument("--hand", default="left", choices=["left", "right"])
+ parser.add_argument("--channel", type=int, default=3, help="0拇指弯..3中指..5小指")
+ parser.add_argument("--label", default="run", help="sim / real / ...")
+ parser.add_argument("--hz", type=float, default=100.0)
+ parser.add_argument("--n", type=int, default=6)
+ parser.add_argument("--out", default="reports/hand_curves")
+ parser.add_argument("--cmd-topic", default="", help="空则 /cb_{hand}_hand_control_cmd")
+ parser.add_argument("--state-topic", default="", help="空则 /cb_{hand}_hand_state")
+ parser.add_argument(
+ "--current-topic",
+ default="",
+ help="可选 Float32MultiArray；空则用 state.effort",
+ )
+ parser.add_argument(
+ "--no-cmd-as-joint",
+ action="store_true",
+ help="没有 state 时不要用 cmd 代替 joint",
+ )
+ args = parser.parse_args()
+
+ os.environ.setdefault("MPLCONFIGDIR", str(Path.cwd() / ".mplconfig"))
+ Path(os.environ["MPLCONFIGDIR"]).mkdir(parents=True, exist_ok=True)
+
+ import rclpy
+ from rclpy.node import Node
+ from sensor_msgs.msg import JointState
+ from std_msgs.msg import Float32MultiArray
+
+ cmd_topic = args.cmd_topic or f"/cb_{args.hand}_hand_control_cmd"
+ state_topic = args.state_topic or f"/cb_{args.hand}_hand_state"
+ out_dir = Path(args.out).expanduser()
+ if not out_dir.is_absolute():
+ out_dir = Path.cwd() / out_dir
+ out_dir.mkdir(parents=True, exist_ok=True)
+
+ n = args.n
+ ch = args.channel
+ use_cmd_as_joint = not args.no_cmd_as_joint
+
+ class Recorder(Node):
+ def __init__(self):
+ super().__init__("hand_curve_recorder_py")
+ self.cmd = [float("nan")] * n
+ self.joint = [float("nan")] * n
+ self.current = [float("nan")] * n
+ self.have_state = False
+ self.have_current = False
+ self.rows = []
+ self.t0 = time.perf_counter()
+
+ self.create_subscription(JointState, cmd_topic, self.on_cmd, 50)
+ self.create_subscription(JointState, state_topic, self.on_state, 50)
+ if args.current_topic:
+ self.create_subscription(
+ Float32MultiArray, args.current_topic, self.on_current, 50
+ )
+ self.create_timer(1.0 / max(1.0, args.hz), self.on_timer)
+ self.get_logger().info(
+ f"recording | cmd={cmd_topic} | state={state_topic} | "
+ f"current={args.current_topic or 'state.effort'} | "
+ f"ch={ch} | out={out_dir}"
+ )
+ self.get_logger().info("Ctrl+C to stop and plot")
+
+ def on_cmd(self, msg: JointState):
+ self.cmd = _pad(msg.position, n)
+
+ def on_state(self, msg: JointState):
+ self.have_state = True
+ self.joint = _pad(msg.position, n)
+ if msg.effort:
+ self.have_current = True
+ self.current = _pad(msg.effort, n)
+
+ def on_current(self, msg: Float32MultiArray):
+ self.have_current = True
+ self.current = _pad(msg.data, n)
+
+ def on_timer(self):
+ joint = list(self.joint)
+ if (not self.have_state) and use_cmd_as_joint:
+ joint = list(self.cmd)
+ self.rows.append(
+ {
+ "t_s": time.perf_counter() - self.t0,
+ "cmd": list(self.cmd),
+ "joint": joint,
+ "current": list(self.current),
+ }
+ )
+
+ rclpy.init()
+ node = Recorder()
+ try:
+ rclpy.spin(node)
+ except KeyboardInterrupt:
+ pass
+
+ rows = node.rows
+ have_state = node.have_state
+ have_current = node.have_current
+ node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+ if not rows:
+ print("no samples recorded")
+ return
+
+ ch_name = CHANNEL_NAMES[ch] if 0 <= ch < len(CHANNEL_NAMES) else f"ch{ch}"
+ stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
+ stem = f"{args.label}_{ch_name}_{stamp}"
+ csv_path = out_dir / f"{stem}.csv"
+ png_path = out_dir / f"{stem}_qvt.png"
+
+ with csv_path.open("w", newline="") as f:
+ w = csv.writer(f)
+ header = ["t_s"]
+ for i in range(n):
+ name = CHANNEL_NAMES[i] if i < len(CHANNEL_NAMES) else f"ch{i}"
+ header += [f"cmd_{name}", f"joint_{name}", f"current_{name}"]
+ w.writerow(header)
+ for row in rows:
+ line = [f"{row['t_s']:.6f}"]
+ for i in range(n):
+ line += [
+ f"{row['cmd'][i]:.6f}",
+ f"{row['joint'][i]:.6f}",
+ f"{row['current'][i]:.6f}",
+ ]
+ w.writerow(line)
+
+ plot_curves(rows, ch, ch_name, args.label, have_state, have_current, png_path)
+ print(f"CSV -> {csv_path} ({len(rows)} samples)")
+ print(f"plot -> {png_path}")
+ print(f"state={'yes' if have_state else 'no'} current={'yes' if have_current else 'no'}")
+
+
+if __name__ == "__main__":
+ main()
diff --git a/tools/middle_step_cmd_pub.py b/tools/middle_step_cmd_pub.py
new file mode 100755
index 0000000..6af32fa
--- /dev/null
+++ b/tools/middle_step_cmd_pub.py
@@ -0,0 +1,113 @@
+#!/usr/bin/env python3
+"""O6 中指 step 控制指令（真机 / 仿真共用，时序完全一致）。
+
+时序（默认）：
+ pre_wait=3s 保持张开
+ hold=3s 中指握紧 (middle u8=0)
+ hold=3s 中指张开 (middle u8=255)
+
+用法（单独发令，配合录制脚本）：
+ export ROS_DOMAIN_ID=31
+ python3 tools/middle_step_cmd_pub.py
+"""
+
+from __future__ import annotations
+
+import argparse
+import time
+
+# 与 run_middle_step_experiment.py 共用，改这里两边一起变
+DEFAULT_PRE_WAIT = 3.0
+DEFAULT_HOLD = 3.0
+DEFAULT_RATE_HZ = 10.0
+FINGER_ROS_IDX = {"index": 2, "middle": 3, "ring": 4, "pinky": 5}
+
+
+def hold_finger_pose(pub, hand: str, finger: str, u8: float, duration: float, rate_hz: float):
+ from sensor_msgs.msg import JointState
+
+ idx = FINGER_ROS_IDX[finger]
+ end = time.perf_counter() + duration
+ dt = 1.0 / max(1.0, rate_hz)
+ while time.perf_counter() < end:
+ msg = JointState()
+ msg.position = [255.0, 255.0, 255.0, 255.0, 255.0, 255.0]
+ msg.position[idx] = float(u8)
+ pub.publish(msg)
+ time.sleep(dt)
+
+
+def run_step_sequence(
+ node,
+ *,
+ hand: str = "left",
+ finger: str = "middle",
+ pre_wait: float = DEFAULT_PRE_WAIT,
+ hold: float = DEFAULT_HOLD,
+ rate_hz: float = DEFAULT_RATE_HZ,
+ cmd_topic: str | None = None,
+):
+ from sensor_msgs.msg import JointState
+
+ topic = cmd_topic or f"/cb_{hand}_hand_control_cmd"
+ pub = node.create_publisher(JointState, topic, 10)
+ time.sleep(0.3)
+
+ t0 = time.perf_counter()
+ node.get_logger().info(
+ f"step cmd | finger={finger} | pre={pre_wait}s hold={hold}s | topic={topic}"
+ )
+
+ # 阶段0：保持张开
+ node.get_logger().info(f"[t={time.perf_counter()-t0:.2f}s] baseline open (255)")
+ hold_finger_pose(pub, hand, finger, 255.0, pre_wait, rate_hz)
+
+ # 阶段1：握紧
+ node.get_logger().info(f"[t={time.perf_counter()-t0:.2f}s] CLOSE middle u8=0")
+ hold_finger_pose(pub, hand, finger, 0.0, hold, rate_hz)
+
+ # 阶段2：张开
+ node.get_logger().info(f"[t={time.perf_counter()-t0:.2f}s] OPEN middle u8=255")
+ hold_finger_pose(pub, hand, finger, 255.0, hold, rate_hz)
+
+ node.get_logger().info(f"[t={time.perf_counter()-t0:.2f}s] sequence done")
+ return t0
+
+
+def total_duration(pre_wait: float, hold: float, record_after: float = 0.0) -> float:
+ return pre_wait + hold + hold + record_after + 1.0
+
+
+def main():
+ parser = argparse.ArgumentParser(description="O6 middle finger step command publisher")
+ parser.add_argument("--hand", default="left", choices=["left", "right"])
+ parser.add_argument("--finger", default="middle", choices=list(FINGER_ROS_IDX))
+ parser.add_argument("--pre-wait", type=float, default=DEFAULT_PRE_WAIT)
+ parser.add_argument("--hold", type=float, default=DEFAULT_HOLD)
+ parser.add_argument("--rate", type=float, default=DEFAULT_RATE_HZ)
+ parser.add_argument("--cmd-topic", default="")
+ args = parser.parse_args()
+
+ import rclpy
+ from rclpy.node import Node
+
+ rclpy.init()
+ node = Node("middle_step_cmd_pub")
+ try:
+ run_step_sequence(
+ node,
+ hand=args.hand,
+ finger=args.finger,
+ pre_wait=args.pre_wait,
+ hold=args.hold,
+ rate_hz=args.rate,
+ cmd_topic=args.cmd_topic or None,
+ )
+ finally:
+ node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/tools/plot_real_sim_compare.py b/tools/plot_real_sim_compare.py
new file mode 100644
index 0000000..bcc1061
--- /dev/null
+++ b/tools/plot_real_sim_compare.py
@@ -0,0 +1,158 @@
+#!/usr/bin/env python3
+"""真机 vs 仿真 q/v/力矩(或电流) 对比图。
+
+时间原点 = 首次收到「握紧」控制指令 (cmd_u8: 255→0) 的时刻 − 1 s。
+
+用法:
+ python3 tools/plot_real_sim_compare.py \\
+ reports/O6_middle_ros/ros_real_middle_left_20260716_172221.csv \\
+ reports/O6_middle_ros/ros_sim_middle_left_20260716_114515.csv
+"""
+
+from __future__ import annotations
+
+import argparse
+from pathlib import Path
+
+import numpy as np
+
+
+def load_csv(path: Path):
+ hdr = path.read_text().splitlines()[0].strip().split(",")
+ data = np.loadtxt(path, delimiter=",", skiprows=1)
+ col = {name: i for i, name in enumerate(hdr)}
+ return hdr, col, data
+
+
+def first_close_cmd_time(t: np.ndarray, cmd_u8: np.ndarray) -> float:
+ """首次 cmd 从 255 阶跃到 0（握紧）的时刻。"""
+ diff = np.diff(cmd_u8)
+ idx = np.where(diff < -100)[0]
+ if len(idx) == 0:
+ raise ValueError("未检测到 cmd 255→0 阶跃")
+ return float(t[idx[0] + 1])
+
+
+def align_series(t, *arrays, t_cmd: float, t_pre: float = 1.0):
+ t0 = t_cmd - t_pre
+ t_rel = t - t0
+ return (t_rel, *arrays)
+
+
+def main():
+ parser = argparse.ArgumentParser()
+ parser.add_argument("real_csv", type=Path)
+ parser.add_argument("sim_csv", type=Path)
+ parser.add_argument("-o", "--out", type=Path, default=None)
+ parser.add_argument("--t-pre", type=float, default=1.0)
+ parser.add_argument("--t-max", type=float, default=8.0, help="相对原点后的最大时间(s)")
+ args = parser.parse_args()
+
+ _, cr, dr = load_csv(args.real_csv)
+ _, cs, ds = load_csv(args.sim_csv)
+
+ t_r, cmd_r = dr[:, cr["t_s"]], dr[:, cr["cmd_u8"]]
+ t_s, cmd_s = ds[:, cs["t_s"]], ds[:, cs["cmd_u8"]]
+
+ t_cmd_r = first_close_cmd_time(t_r, cmd_r)
+ t_cmd_s = first_close_cmd_time(t_s, cmd_s)
+
+ q_r = dr[:, cr["q_rad"]]
+ cmd_rad_r = dr[:, cr["cmd_rad"]]
+ v_r = dr[:, cr["v_rad_s"]]
+ q_s = ds[:, cs["q_rad"]]
+ cmd_rad_s = ds[:, cs["cmd_rad"]]
+ v_s = ds[:, cs["v_rad_s"]]
+
+ cur_r = dr[:, cr["current"]] if "current" in cr else np.full_like(q_r, np.nan)
+ tau_s = ds[:, cs["tau_Nm"]] if "tau_Nm" in cs else np.full_like(q_s, np.nan)
+
+ tr, qr, cmdr, vr, cur_r = align_series(
+ t_r, q_r, cmd_rad_r, v_r, cur_r, t_cmd=t_cmd_r, t_pre=args.t_pre
+ )
+ ts, qs, cmds, vs, tau_s = align_series(
+ t_s, q_s, cmd_rad_s, v_s, tau_s, t_cmd=t_cmd_s, t_pre=args.t_pre
+ )
+
+ mask_r = (tr >= -0.05) & (tr <= args.t_max)
+ mask_s = (ts >= -0.05) & (ts <= args.t_max)
+
+ out = args.out
+ if out is None:
+ out = args.real_csv.parent / "ros_real_vs_sim_middle_left_compare_qvt.png"
+
+ import os
+
+ root = Path(__file__).resolve().parents[1]
+ os.environ.setdefault("MPLCONFIGDIR", str(root / ".mplconfig"))
+ Path(os.environ["MPLCONFIGDIR"]).mkdir(parents=True, exist_ok=True)
+
+ import matplotlib
+
+ matplotlib.use("Agg")
+ import matplotlib.pyplot as plt
+
+ fig, axes = plt.subplots(3, 1, figsize=(11, 9), sharex=True)
+ fig.suptitle(
+ "O6 middle_mcp_pitch real vs sim\n"
+ f"t=0 = {args.t_pre:.0f}s before first close command (cmd_u8: 255->0)",
+ fontsize=12,
+ )
+
+ # --- angle ---
+ ax = axes[0]
+ # 控制指令：真机/仿真时序一致，只画一条黑色虚线
+ cmd_t = ts[mask_s] if np.any(mask_s) else tr[mask_r]
+ cmd_y = np.rad2deg(cmds[mask_s]) if np.any(mask_s) else np.rad2deg(cmdr[mask_r])
+ ax.plot(cmd_t, cmd_y, color="black", ls="--", lw=1.2, label="cmd")
+ ax.plot(ts[mask_s], np.rad2deg(qs[mask_s]), color="#1f77b4", lw=1.8, label="sim q")
+ ax.plot(tr[mask_r], np.rad2deg(qr[mask_r]), color="#ff7f0e", lw=1.8, label="real q")
+ ax.axvline(1.0, color="gray", ls=":", lw=1.0, alpha=0.8)
+ ax.set_ylabel("angle (deg)")
+ ax.legend(loc="best", fontsize=9)
+ ax.grid(True, alpha=0.3)
+
+ # --- velocity ---
+ ax = axes[1]
+ ax.plot(ts[mask_s], np.rad2deg(vs[mask_s]), color="#1f77b4", lw=1.8, label="sim v")
+ ax.plot(tr[mask_r], np.rad2deg(vr[mask_r]), color="#ff7f0e", lw=1.8, label="real v")
+ ax.axvline(1.0, color="gray", ls=":", lw=1.0, alpha=0.8)
+ ax.set_ylabel("velocity (deg/s)")
+ ax.legend(loc="best")
+ ax.grid(True, alpha=0.3)
+
+ # --- effort: sim tau vs real current ---
+ ax = axes[2]
+ ax.plot(ts[mask_s], tau_s[mask_s], color="#1f77b4", lw=1.8, label="sim τ (N·m)")
+ if np.any(np.isfinite(cur_r[mask_r])):
+ ax.plot(tr[mask_r], cur_r[mask_r], color="#ff7f0e", lw=1.8, label="real current")
+ ax.set_ylabel("τ (N·m) / current")
+ else:
+ ax.plot([], [], color="#ff7f0e", label="real current (no data)")
+ ax.set_ylabel("torque (N·m)")
+ ax.text(
+ 0.5,
+ 0.5,
+ "real current not recorded",
+ transform=ax.transAxes,
+ ha="center",
+ va="center",
+ fontsize=10,
+ color="#666",
+ )
+ ax.axvline(1.0, color="gray", ls=":", lw=1.0, alpha=0.8)
+ ax.set_xlabel("time (s) [t=0: 1s before first close cmd]")
+ ax.legend(loc="best")
+ ax.grid(True, alpha=0.3)
+
+ fig.tight_layout()
+ fig.savefig(out, dpi=150)
+ plt.close(fig)
+
+ print(f"plot -> {out}")
+ print(f"real cmd↓ at abs t={t_cmd_r:.3f}s -> origin t0={t_cmd_r - args.t_pre:.3f}s")
+ print(f"sim cmd↓ at abs t={t_cmd_s:.3f}s -> origin t0={t_cmd_s - args.t_pre:.3f}s")
+
+
+if __name__ == "__main__":
+ main()
diff --git a/tools/ros_mujoco_record_qvt.py b/tools/ros_mujoco_record_qvt.py
new file mode 100755
index 0000000..a3d5a07
--- /dev/null
+++ b/tools/ros_mujoco_record_qvt.py
@@ -0,0 +1,339 @@
+#!/usr/bin/env python3
+"""ROS 控制指令 → 本地 MuJoCo → 记录并绘制 角度/速度/力矩。
+
+订 /cb__hand_control_cmd（0～255），用与仿真节点相同的 O6 映射写 ctrl，
+从 MuJoCo 读 q / v / actuator_force，Ctrl+C 存 CSV + 三曲线图。
+
+用法：
+ # 终端1（可先不开 run_sim.sh，本脚本自带 MuJoCo）
+ python3 tools/ros_mujoco_record_qvt.py --hand left --finger middle --viewer
+
+ # 终端2 发 ROS 指令，或加 --self-sweep 自动发中指慢扫
+ python3 tools/ros_mujoco_record_qvt.py --hand left --finger middle --viewer --self-sweep
+"""
+
+from __future__ import annotations
+
+import argparse
+import sys
+import threading
+import time
+from datetime import datetime
+from pathlib import Path
+
+import numpy as np
+
+ROOT = Path(__file__).resolve().parents[1]
+PKG = (
+ ROOT
+ / "src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2"
+)
+sys.path.insert(0, str(PKG.parent))
+
+from linker_hand_mujoco_ros2.utils.mapping import ( # noqa: E402
+ O6_JOINT_MAP,
+ O6_MIMIC,
+ apply_mimic,
+ range_to_arc_left,
+ range_to_arc_right,
+)
+
+FINGER_CFG = {
+ "index": {
+ "mcp": "index_mcp_pitch",
+ "mcp_act": "index_mcp_pitch_pos",
+ "ros_idx": 2,
+ },
+ "middle": {
+ "mcp": "middle_mcp_pitch",
+ "mcp_act": "middle_mcp_pitch_pos",
+ "ros_idx": 3,
+ },
+ "ring": {
+ "mcp": "ring_mcp_pitch",
+ "mcp_act": "ring_mcp_pitch_pos",
+ "ros_idx": 4,
+ },
+ "pinky": {
+ "mcp": "pinky_mcp_pitch",
+ "mcp_act": "pinky_mcp_pitch_pos",
+ "ros_idx": 5,
+ },
+}
+
+
+def map_position_array(position, joint_map):
+ mapped = [0.0] * len(joint_map)
+ for target_idx, source_idx in joint_map.items():
+ if source_idx < len(position):
+ mapped[target_idx] = position[source_idx]
+ return mapped
+
+
+def u8_to_ctrl(position_u8, hand_type: str):
+ if hand_type == "left":
+ tmp = range_to_arc_left(position_u8, "O6")
+ else:
+ tmp = range_to_arc_right(position_u8, "O6")
+ res = map_position_array(tmp, O6_JOINT_MAP)
+ from linker_hand_mujoco_ros2.utils.mapping import O6_JOINT_ARC
+
+ res = apply_mimic(res, O6_MIMIC, O6_JOINT_ARC)
+ return res
+
+
+def main():
+ parser = argparse.ArgumentParser()
+ parser.add_argument("--hand", choices=["left", "right"], default="left")
+ parser.add_argument("--finger", choices=list(FINGER_CFG.keys()), default="middle")
+ parser.add_argument("--cmd-topic", default="")
+ parser.add_argument("--out", type=Path, default=None)
+ parser.add_argument("--viewer", action="store_true")
+ parser.add_argument(
+ "--self-sweep",
+ action="store_true",
+ help="本进程自动往控制话题发 255→0→255 慢扫（方便自测）",
+ )
+ parser.add_argument(
+ "--exit-after-sweep",
+ action="store_true",
+ help="配合 --self-sweep：扫完自动结束并画图",
+ )
+ parser.add_argument("--duration", type=float, default=8.0, help="self-sweep 单向时长")
+ parser.add_argument(
+ "--record-duration",
+ type=float,
+ default=0.0,
+ help=">0 时录满该秒数自动结束（配合 middle_step_cmd_pub）",
+ )
+ parser.add_argument("--label", default="ros_sim")
+ args = parser.parse_args()
+ if args.self_sweep and not args.exit_after_sweep:
+ # 自测默认扫完就退出，避免一直挂着
+ args.exit_after_sweep = True
+
+ cfg = FINGER_CFG[args.finger]
+ cmd_topic = args.cmd_topic or f"/cb_{args.hand}_hand_control_cmd"
+ out_dir = args.out or (ROOT / f"reports/O6_{args.finger}_ros")
+ out_dir.mkdir(parents=True, exist_ok=True)
+
+ xml = (
+ PKG
+ / f"urdf/O6/linker_hand_o6_{args.hand}/linker_hand_o6_{args.hand}.xml"
+ )
+
+ import mujoco
+ import mujoco.viewer
+
+ model = mujoco.MjModel.from_xml_path(str(xml))
+ data = mujoco.MjData(model)
+ model.opt.disableflags = 0
+
+ mcp_jnt = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, cfg["mcp"])
+ mcp_act = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, cfg["mcp_act"])
+ if mcp_jnt < 0 or mcp_act < 0:
+ raise RuntimeError(f"joint/actuator not found for {args.finger}")
+ qadr = int(model.jnt_qposadr[mcp_jnt])
+ dadr = int(model.jnt_dofadr[mcp_jnt])
+ mcp_lo, mcp_hi = map(float, model.jnt_range[mcp_jnt])
+
+ ctrl_lock = threading.Lock()
+ ctrl_values = np.zeros(model.nu)
+ last_cmd_u8 = [255.0] * 6
+ running = True
+ sweep_done = threading.Event()
+ logs = {"t": [], "cmd_rad": [], "q": [], "v": [], "tau": [], "cmd_u8": []}
+ t0 = time.perf_counter()
+ t_stop = (
+ t0 + args.record_duration if args.record_duration > 0 else None
+ )
+
+ import signal
+
+ def _stop(*_):
+ nonlocal running
+ running = False
+
+ signal.signal(signal.SIGINT, _stop)
+ signal.signal(signal.SIGTERM, _stop)
+
+ import rclpy
+ from rclpy.node import Node
+ from sensor_msgs.msg import JointState
+
+ class CmdNode(Node):
+ def __init__(self):
+ super().__init__("ros_mujoco_record_qvt")
+ self.create_subscription(JointState, cmd_topic, self.on_cmd, 50)
+ self.get_logger().info(
+ f"MuJoCo+ROS record | topic={cmd_topic} | finger={args.finger} | "
+ f"joint={cfg['mcp']} range=[{mcp_lo:.2f},{mcp_hi:.2f}]"
+ )
+ self.get_logger().info("Ctrl+C to stop and plot")
+
+ def on_cmd(self, msg: JointState):
+ nonlocal last_cmd_u8
+ pos = list(msg.position)
+ if len(pos) < 6:
+ pos = pos + [255.0] * (6 - len(pos))
+ last_cmd_u8 = pos[:6]
+ mapped = u8_to_ctrl(last_cmd_u8, args.hand)
+ with ctrl_lock:
+ n = min(len(mapped), model.nu)
+ ctrl_values[:n] = mapped[:n]
+
+ rclpy.init()
+ node = CmdNode()
+
+ def spin_ros():
+ while running and rclpy.ok():
+ rclpy.spin_once(node, timeout_sec=0.01)
+
+ ros_thread = threading.Thread(target=spin_ros, daemon=True)
+ ros_thread.start()
+
+ pub = None
+ if args.self_sweep:
+ pub = node.create_publisher(JointState, cmd_topic, 10)
+ time.sleep(0.3)
+
+ def sweep_pub():
+ nonlocal running
+ half = args.duration
+ ros_i = cfg["ros_idx"]
+ # 255 -> 0
+ t_start = time.time()
+ while running and time.time() - t_start < half:
+ u = (time.time() - t_start) / half
+ mid = 255.0 * (1.0 - u)
+ m = JointState()
+ m.position = [255.0, 255.0, 255.0, 255.0, 255.0, 255.0]
+ m.position[ros_i] = mid
+ pub.publish(m)
+ time.sleep(0.01)
+ # 0 -> 255
+ t_start = time.time()
+ while running and time.time() - t_start < half:
+ u = (time.time() - t_start) / half
+ mid = 255.0 * u
+ m = JointState()
+ m.position = [255.0, 255.0, 255.0, 255.0, 255.0, 255.0]
+ m.position[ros_i] = mid
+ pub.publish(m)
+ time.sleep(0.01)
+ print("self-sweep finished", flush=True)
+ sweep_done.set()
+ if args.exit_after_sweep:
+ running = False
+
+ threading.Thread(target=sweep_pub, daemon=True).start()
+
+ viewer = None
+ if args.viewer:
+ viewer = mujoco.viewer.launch_passive(model, data)
+
+ try:
+ while running:
+ if viewer is not None and not viewer.is_running():
+ break
+ if t_stop is not None and time.perf_counter() >= t_stop:
+ break
+ with ctrl_lock:
+ data.ctrl[:] = ctrl_values
+ mujoco.mj_step(model, data)
+ if viewer is not None:
+ viewer.sync()
+
+ t = time.perf_counter() - t0
+ q = float(data.qpos[qadr])
+ v = float(data.qvel[dadr])
+ tau = float(data.actuator_force[mcp_act])
+ cmd_rad = float(ctrl_values[mcp_act]) if mcp_act < len(ctrl_values) else 0.0
+ logs["t"].append(t)
+ logs["cmd_rad"].append(cmd_rad)
+ logs["q"].append(q)
+ logs["v"].append(v)
+ logs["tau"].append(tau)
+ logs["cmd_u8"].append(float(last_cmd_u8[cfg["ros_idx"]]))
+ time.sleep(0.001)
+ except KeyboardInterrupt:
+ pass
+ finally:
+ running = False
+ if viewer is not None:
+ viewer.close()
+ node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+ if len(logs["t"]) < 2:
+ print("no samples")
+ return
+
+ for k in logs:
+ logs[k] = np.asarray(logs[k], dtype=float)
+
+ stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
+ stem = f"{args.label}_{args.finger}_{args.hand}_{stamp}"
+ csv_path = out_dir / f"{stem}.csv"
+ png_path = out_dir / f"{stem}_qvt.png"
+
+ np.savetxt(
+ csv_path,
+ np.column_stack(
+ [
+ logs["t"],
+ logs["cmd_u8"],
+ logs["cmd_rad"],
+ logs["q"],
+ logs["v"],
+ logs["tau"],
+ ]
+ ),
+ delimiter=",",
+ header="t_s,cmd_u8,cmd_rad,q_rad,v_rad_s,tau_Nm",
+ comments="",
+ )
+
+ import os
+
+ os.environ.setdefault("MPLCONFIGDIR", str(ROOT / ".mplconfig"))
+ Path(os.environ["MPLCONFIGDIR"]).mkdir(parents=True, exist_ok=True)
+ import matplotlib
+
+ matplotlib.use("Agg")
+ import matplotlib.pyplot as plt
+
+ fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
+ fig.suptitle(
+ f"ROS->MuJoCo {args.hand} {cfg['mcp']} "
+ f"range=[{mcp_lo:.2f}, {mcp_hi:.2f}] rad",
+ fontsize=12,
+ )
+ axes[0].plot(logs["t"], np.rad2deg(logs["cmd_rad"]), "k--", lw=1.2, label="cmd")
+ axes[0].plot(logs["t"], np.rad2deg(logs["q"]), "C0", lw=1.6, label="q")
+ axes[0].set_ylabel("angle (deg)")
+ axes[0].legend(loc="best")
+ axes[0].grid(True, alpha=0.3)
+
+ axes[1].plot(logs["t"], np.rad2deg(logs["v"]), "C1", lw=1.6, label="v")
+ axes[1].set_ylabel("velocity (deg/s)")
+ axes[1].legend(loc="best")
+ axes[1].grid(True, alpha=0.3)
+
+ axes[2].plot(logs["t"], logs["tau"], "C3", lw=1.6, label="tau (actuator_force)")
+ axes[2].set_ylabel("torque (N·m)")
+ axes[2].set_xlabel("time (s)")
+ axes[2].legend(loc="best")
+ axes[2].grid(True, alpha=0.3)
+ fig.tight_layout()
+ fig.savefig(png_path, dpi=140)
+ plt.close(fig)
+
+ print(f"CSV -> {csv_path}")
+ print(f"plot -> {png_path}")
+ print(f"samples={len(logs['t'])}")
+
+
+if __name__ == "__main__":
+ main()
diff --git a/tools/ros_real_record_qvt.py b/tools/ros_real_record_qvt.py
new file mode 100755
index 0000000..a4504c0
--- /dev/null
+++ b/tools/ros_real_record_qvt.py
@@ -0,0 +1,346 @@
+#!/usr/bin/env python3
+"""真机 ROS 录制：订 control_cmd + hand_state，画与仿真相同的三曲线（deg / deg/s / current）。
+
+配合 step 指令（先握中指再张开）：
+ python3 tools/ros_real_record_qvt.py --hand left --finger middle --auto-step
+
+手动：终端1 本脚本；终端2 发 ros2 topic pub ...
+"""
+
+from __future__ import annotations
+
+import argparse
+import json
+import sys
+import threading
+import time
+from datetime import datetime
+from pathlib import Path
+
+import numpy as np
+
+ROOT = Path(__file__).resolve().parents[1]
+PKG = (
+ ROOT
+ / "src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2"
+)
+sys.path.insert(0, str(PKG.parent))
+
+from linker_hand_mujoco_ros2.utils.mapping import ( # noqa: E402
+ range_to_arc_left,
+ range_to_arc_right,
+)
+
+FINGER_CFG = {
+ "index": {"ros_idx": 2, "mcp": "index_mcp_pitch", "lo": 0.0, "hi": 1.60},
+ "middle": {"ros_idx": 3, "mcp": "middle_mcp_pitch", "lo": 0.0, "hi": 1.60},
+ "ring": {"ros_idx": 4, "mcp": "ring_mcp_pitch", "lo": 0.0, "hi": 1.60},
+ "pinky": {"ros_idx": 5, "mcp": "pinky_mcp_pitch", "lo": 0.0, "hi": 1.60},
+}
+
+
+def u8_to_rad(u8: float, hand: str, ros_idx: int) -> float:
+ pos = [255.0] * 6
+ pos[ros_idx] = float(u8)
+ if hand == "left":
+ arc = range_to_arc_left(pos, "O6")
+ else:
+ arc = range_to_arc_right(pos, "O6")
+ return float(arc[ros_idx])
+
+
+def main():
+ parser = argparse.ArgumentParser()
+ parser.add_argument("--hand", choices=["left", "right"], default="left")
+ parser.add_argument("--finger", choices=list(FINGER_CFG.keys()), default="middle")
+ parser.add_argument("--cmd-topic", default="")
+ parser.add_argument("--state-topic", default="")
+ parser.add_argument("--info-topic", default="")
+ parser.add_argument("--out", type=Path, default=None)
+ parser.add_argument("--label", default="ros_real")
+ parser.add_argument("--hz", type=float, default=100.0)
+ parser.add_argument(
+ "--auto-step",
+ action="store_true",
+ help="自动发 255..255,0,255..255 再张开，录完退出",
+ )
+ parser.add_argument("--pre-wait", type=float, default=2.0, help="开录后等待(s)")
+ parser.add_argument("--hold", type=float, default=3.0, help="每步保持(s)")
+ parser.add_argument("--record-after", type=float, default=1.0, help="第二步后多录(s)")
+ parser.add_argument(
+ "--duration",
+ type=float,
+ default=0.0,
+ help=">0 时录满该秒数自动结束（配合外部 middle_step_cmd_pub）",
+ )
+ args = parser.parse_args()
+
+ cfg = FINGER_CFG[args.finger]
+ ros_i = cfg["ros_idx"]
+ cmd_topic = args.cmd_topic or f"/cb_{args.hand}_hand_control_cmd"
+ state_topic = args.state_topic or f"/cb_{args.hand}_hand_state"
+ info_topic = args.info_topic or f"/cb_{args.hand}_hand_info"
+ out_dir = args.out or (ROOT / f"reports/O6_{args.finger}_ros")
+ out_dir.mkdir(parents=True, exist_ok=True)
+
+ import rclpy
+ from rclpy.node import Node
+ from sensor_msgs.msg import JointState
+ from std_msgs.msg import String
+
+ logs = {
+ "t": [],
+ "cmd_u8": [],
+ "joint_u8": [],
+ "cmd_rad": [],
+ "q_rad": [],
+ "v_rad_s": [],
+ "current": [],
+ }
+ t0 = time.perf_counter()
+ running = True
+ state_lock = threading.Lock()
+ last_cmd_u8 = [255.0] * 6
+ last_joint_u8 = [float("nan")] * 6
+ last_current = [float("nan")] * 6
+ have_state = False
+ have_current = False
+
+ class Recorder(Node):
+ def __init__(self):
+ super().__init__("ros_real_record_qvt")
+ self.pub = self.create_publisher(JointState, cmd_topic, 10)
+ self.create_subscription(JointState, cmd_topic, self.on_cmd, 50)
+ self.create_subscription(JointState, state_topic, self.on_state, 50)
+ self.create_subscription(String, info_topic, self.on_info, 10)
+ self.create_timer(1.0 / max(1.0, args.hz), self.on_timer)
+ self.get_logger().info(
+ f"real record | cmd={cmd_topic} | state={state_topic} | "
+ f"finger={args.finger} idx={ros_i}"
+ )
+
+ def on_cmd(self, msg: JointState):
+ nonlocal last_cmd_u8
+ pos = list(msg.position)
+ if len(pos) >= 6:
+ with state_lock:
+ last_cmd_u8 = [float(x) for x in pos[:6]]
+
+ def on_state(self, msg: JointState):
+ nonlocal have_state, last_joint_u8, have_current, last_current
+ with state_lock:
+ pos = list(msg.position)
+ if len(pos) >= 6 and all(float(x) >= 0 for x in pos[:6]):
+ have_state = True
+ last_joint_u8 = [float(x) for x in pos[:6]]
+ if msg.effort and any(abs(float(x)) > 1e-6 for x in msg.effort):
+ have_current = True
+ last_current = [float(x) for x in msg.effort[:6]]
+
+ def on_info(self, msg: String):
+ nonlocal have_current, last_current
+ try:
+ info = json.loads(msg.data)
+ cur = info.get("current")
+ if isinstance(cur, list) and len(cur) >= 6:
+ if any(float(x) >= 0 for x in cur[:6]):
+ with state_lock:
+ have_current = True
+ last_current = [float(x) for x in cur[:6]]
+ except json.JSONDecodeError:
+ pass
+
+ def publish_pose(self, middle_u8: float):
+ nonlocal last_cmd_u8
+ m = JointState()
+ m.position = [255.0, 255.0, 255.0, 255.0, 255.0, 255.0]
+ m.position[ros_i] = float(middle_u8)
+ with state_lock:
+ last_cmd_u8 = list(m.position)
+ self.pub.publish(m)
+
+ def hold_pose(self, middle_u8: float, duration: float, rate_hz: float = 10.0):
+ """持续重发控制帧（SDK 只响应订阅到的 cmd；单发 --once 可能丢）。"""
+ end = time.perf_counter() + duration
+ dt = 1.0 / max(1.0, rate_hz)
+ while time.perf_counter() < end and running:
+ self.publish_pose(middle_u8)
+ time.sleep(dt)
+
+ def on_timer(self):
+ nonlocal running
+ if not running:
+ return
+ with state_lock:
+ joint_u8 = list(last_joint_u8)
+ current = list(last_current)
+ cmd_u8 = float(last_cmd_u8[ros_i])
+ if have_state and not np.isnan(joint_u8[ros_i]):
+ q_u8 = float(joint_u8[ros_i])
+ else:
+ q_u8 = cmd_u8
+ t = time.perf_counter() - t0
+ q_rad = u8_to_rad(q_u8, args.hand, ros_i)
+ cmd_rad = u8_to_rad(cmd_u8, args.hand, ros_i)
+ cur = current[ros_i] if len(current) > ros_i else float("nan")
+ logs["t"].append(t)
+ logs["cmd_u8"].append(cmd_u8)
+ logs["joint_u8"].append(q_u8)
+ logs["cmd_rad"].append(cmd_rad)
+ logs["q_rad"].append(q_rad)
+ logs["current"].append(cur)
+ if len(logs["t"]) >= 2:
+ dt = logs["t"][-1] - logs["t"][-2]
+ if dt > 1e-9:
+ v = (logs["q_rad"][-1] - logs["q_rad"][-2]) / dt
+ else:
+ v = 0.0
+ else:
+ v = 0.0
+ logs["v_rad_s"].append(v)
+
+ rclpy.init()
+ node = Recorder()
+
+ def spin():
+ while running and rclpy.ok():
+ rclpy.spin_once(node, timeout_sec=0.01)
+
+ threading.Thread(target=spin, daemon=True).start()
+
+ if args.auto_step:
+ deadline = time.perf_counter() + max(5.0, args.pre_wait)
+ while time.perf_counter() < deadline and not have_state:
+ time.sleep(0.05)
+ if not have_state:
+ node.get_logger().warn(
+ "仍未收到 hand_state（可能 SDK 未跑 / Domain 不对 / 全是 -1）"
+ )
+ time.sleep(max(0.5, args.pre_wait))
+ node.get_logger().info("step1: middle close (u8=0)")
+ node.hold_pose(0.0, args.hold)
+ node.get_logger().info("step2: middle open (u8=255)")
+ node.hold_pose(255.0, args.hold + args.record_after)
+ running = False
+ else:
+ node.get_logger().info(
+ "listen mode: 外部发令 (middle_step_cmd_pub) 或 Ctrl+C 结束"
+ )
+ t_end = (
+ time.perf_counter() + args.duration if args.duration > 0 else None
+ )
+ try:
+ while running:
+ if t_end is not None and time.perf_counter() >= t_end:
+ break
+ time.sleep(0.05)
+ except KeyboardInterrupt:
+ running = False
+
+ time.sleep(0.2)
+ node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+ if len(logs["t"]) < 2:
+ print("no samples")
+ return 1
+
+ for k in logs:
+ logs[k] = np.asarray(logs[k], dtype=float)
+
+ if len(logs["v_rad_s"]) >= 5:
+ logs["v_rad_s"] = np.convolve(
+ logs["v_rad_s"], np.ones(5) / 5.0, mode="same"
+ )
+
+ stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
+ stem = f"{args.label}_{args.finger}_{args.hand}_{stamp}"
+ csv_path = out_dir / f"{stem}.csv"
+ png_path = out_dir / f"{stem}_qvt.png"
+
+ np.savetxt(
+ csv_path,
+ np.column_stack(
+ [
+ logs["t"],
+ logs["cmd_u8"],
+ logs["joint_u8"],
+ logs["cmd_rad"],
+ logs["q_rad"],
+ logs["v_rad_s"],
+ logs["current"],
+ ]
+ ),
+ delimiter=",",
+ header="t_s,cmd_u8,joint_u8,cmd_rad,q_rad,v_rad_s,current",
+ comments="",
+ )
+
+ import os
+
+ os.environ.setdefault("MPLCONFIGDIR", str(ROOT / ".mplconfig"))
+ Path(os.environ["MPLCONFIGDIR"]).mkdir(parents=True, exist_ok=True)
+ import matplotlib
+
+ matplotlib.use("Agg")
+ import matplotlib.pyplot as plt
+
+ mcp_lo, mcp_hi = cfg["lo"], cfg["hi"]
+ fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
+ fig.suptitle(
+ f"ROS->Real {args.hand} {cfg['mcp']} "
+ f"range=[{mcp_lo:.2f}, {mcp_hi:.2f}] rad "
+ f"(255=open/0deg, 0=close/~92deg)",
+ fontsize=11,
+ )
+ axes[0].plot(logs["t"], np.rad2deg(logs["cmd_rad"]), "k--", lw=1.2, label="cmd")
+ axes[0].plot(logs["t"], np.rad2deg(logs["q_rad"]), "C0", lw=1.6, label="q")
+ axes[0].set_ylabel("angle (deg)")
+ axes[0].legend(loc="best")
+ axes[0].grid(True, alpha=0.3)
+
+ axes[1].plot(logs["t"], np.rad2deg(logs["v_rad_s"]), "C1", lw=1.6, label="v")
+ axes[1].set_ylabel("velocity (deg/s)")
+ axes[1].legend(loc="best")
+ axes[1].grid(True, alpha=0.3)
+
+ if have_current and np.any(np.isfinite(logs["current"])):
+ axes[2].plot(
+ logs["t"], logs["current"], "C3", lw=1.6, label="current (hand_info)"
+ )
+ axes[2].set_ylabel("current")
+ else:
+ axes[2].text(
+ 0.5,
+ 0.5,
+ "no current (subscribe hand_info or state.effort)",
+ ha="center",
+ va="center",
+ transform=axes[2].transAxes,
+ )
+ axes[2].set_ylabel("current")
+ axes[2].set_xlabel("time (s)")
+ axes[2].legend(loc="best")
+ axes[2].grid(True, alpha=0.3)
+ fig.tight_layout()
+ fig.savefig(png_path, dpi=140)
+ plt.close(fig)
+
+ print(f"CSV -> {csv_path}")
+ print(f"plot -> {png_path}")
+ print(f"samples={len(logs['t'])} state={'yes' if have_state else 'NO'}")
+ cmd_span = float(np.max(logs["cmd_u8"]) - np.min(logs["cmd_u8"]))
+ joint_span = float(np.max(logs["joint_u8"]) - np.min(logs["joint_u8"]))
+ print(f"cmd_u8 span={cmd_span:.1f} joint_u8 span={joint_span:.1f}")
+ if cmd_span < 1.0:
+ print("[warn] cmd 全程几乎不变 → 控制指令可能没发出去或未写入录制")
+ if not have_state:
+ print("[warn] 未收到有效 state → q 只能用 cmd 代替，且真机可能没动")
+ if cmd_span >= 1.0 and joint_span < 3.0 and have_state:
+ print("[warn] 有 cmd 变化但 joint 几乎不动 → 查 CAN / SDK / 力矩限制")
+ return 0
+
+
+if __name__ == "__main__":
+ raise SystemExit(main())
diff --git a/tools/run_middle_step_experiment.py b/tools/run_middle_step_experiment.py
new file mode 100755
index 0000000..f185bfb
--- /dev/null
+++ b/tools/run_middle_step_experiment.py
@@ -0,0 +1,231 @@
+#!/usr/bin/env python3
+"""O6 中指 step 实验：真机 / 仿真共用同一套控制时序。
+
+时序（默认，两边完全一致）：
+ 3s 张开 → 3s 握紧 → 3s 张开 → 2s 余量录制
+
+用法：
+ export ROS_DOMAIN_ID=31
+
+ # 真机（需 can0 UP + SDK，或脚本尝试自动启 SDK）
+ python3 tools/run_middle_step_experiment.py real
+
+ # 仿真（自带 MuJoCo，无需 run_sim.sh）
+ python3 tools/run_middle_step_experiment.py sim
+
+ # 两次跑完后出对比图（自动找目录下最新 real/sim csv）
+ python3 tools/run_middle_step_experiment.py compare
+
+ # 指定 csv
+ python3 tools/run_middle_step_experiment.py compare \\
+ --real-csv reports/O6_middle_ros/ros_real_....csv \\
+ --sim-csv reports/O6_middle_ros/ros_sim_....csv
+"""
+
+from __future__ import annotations
+
+import argparse
+import glob
+import os
+import signal
+import subprocess
+import sys
+import threading
+import time
+from pathlib import Path
+
+ROOT = Path(__file__).resolve().parents[1]
+OUT_DIR = ROOT / "reports/O6_middle_ros"
+
+# 与 middle_step_cmd_pub.py 保持一致
+PRE_WAIT = 3.0
+HOLD = 3.0
+RECORD_AFTER = 2.0
+RATE_HZ = 10.0
+
+
+def record_duration() -> float:
+ return PRE_WAIT + HOLD + HOLD + RECORD_AFTER + 1.5
+
+
+def setup_ros_env():
+ os.environ.setdefault("ROS_DOMAIN_ID", "31")
+ os.environ.setdefault("ROS_LOCALHOST_ONLY", "0")
+
+
+def setup_mujoco_pythonpath():
+ venv_site = ROOT / "venv/lib/python3.12/site-packages"
+ if venv_site.is_dir():
+ os.environ["PYTHONPATH"] = f"{venv_site}{os.pathsep}{os.environ.get('PYTHONPATH', '')}"
+
+
+def run_real_experiment(args):
+ setup_ros_env()
+ OUT_DIR.mkdir(parents=True, exist_ok=True)
+ dur = record_duration()
+
+ rec_cmd = [
+ sys.executable,
+ str(ROOT / "tools/ros_real_record_qvt.py"),
+ "--hand",
+ args.hand,
+ "--finger",
+ args.finger,
+ "--label",
+ "ros_real",
+ "--out",
+ str(OUT_DIR),
+ "--duration",
+ str(dur),
+ ]
+ print(f"[real] 启动录制 {dur:.1f}s ...")
+ rec = subprocess.Popen(rec_cmd, cwd=str(ROOT))
+ time.sleep(1.0)
+
+ pub_cmd = [
+ sys.executable,
+ str(ROOT / "tools/middle_step_cmd_pub.py"),
+ "--hand",
+ args.hand,
+ "--finger",
+ args.finger,
+ "--pre-wait",
+ str(PRE_WAIT),
+ "--hold",
+ str(HOLD),
+ "--rate",
+ str(RATE_HZ),
+ ]
+ print("[real] 发送统一 step 指令 ...")
+ pub = subprocess.run(pub_cmd, cwd=str(ROOT))
+ if pub.returncode != 0:
+ rec.send_signal(signal.SIGINT)
+ rec.wait()
+ return pub.returncode
+
+ print("[real] 等待录制结束 ...")
+ try:
+ rec.wait(timeout=dur + 5)
+ except subprocess.TimeoutExpired:
+ rec.send_signal(signal.SIGINT)
+ rec.wait()
+ return rec.returncode
+
+
+def run_sim_experiment(args):
+ setup_ros_env()
+ setup_mujoco_pythonpath()
+ OUT_DIR.mkdir(parents=True, exist_ok=True)
+ dur = record_duration()
+
+ rec_cmd = [
+ sys.executable,
+ str(ROOT / "tools/ros_mujoco_record_qvt.py"),
+ "--hand",
+ args.hand,
+ "--finger",
+ args.finger,
+ "--label",
+ "ros_sim",
+ "--out",
+ str(OUT_DIR),
+ "--record-duration",
+ str(dur),
+ ]
+ if args.viewer:
+ rec_cmd.append("--viewer")
+
+ print(f"[sim] 启动 MuJoCo 录制 {dur:.1f}s ...")
+ rec = subprocess.Popen(rec_cmd, cwd=str(ROOT))
+ time.sleep(1.0)
+
+ pub_cmd = [
+ sys.executable,
+ str(ROOT / "tools/middle_step_cmd_pub.py"),
+ "--hand",
+ args.hand,
+ "--finger",
+ args.finger,
+ "--pre-wait",
+ str(PRE_WAIT),
+ "--hold",
+ str(HOLD),
+ "--rate",
+ str(RATE_HZ),
+ ]
+ print("[sim] 发送统一 step 指令 ...")
+ pub = subprocess.run(pub_cmd, cwd=str(ROOT))
+ if pub.returncode != 0:
+ rec.send_signal(signal.SIGINT)
+ rec.wait()
+ return pub.returncode
+
+ print("[sim] 等待录制结束 ...")
+ try:
+ rec.wait(timeout=dur + 10)
+ except subprocess.TimeoutExpired:
+ rec.send_signal(signal.SIGINT)
+ rec.wait()
+ return rec.returncode
+
+
+def latest_csv(pattern: str) -> Path | None:
+ files = sorted(glob.glob(str(OUT_DIR / pattern)), key=os.path.getmtime)
+ return Path(files[-1]) if files else None
+
+
+def run_compare(args):
+ real_csv = Path(args.real_csv) if args.real_csv else latest_csv("ros_real_*middle*.csv")
+ sim_csv = Path(args.sim_csv) if args.sim_csv else latest_csv("ros_sim_*middle*.csv")
+ if not real_csv or not real_csv.is_file():
+ print("[error] 找不到真机 csv，先跑: python3 tools/run_middle_step_experiment.py real")
+ return 1
+ if not sim_csv or not sim_csv.is_file():
+ print("[error] 找不到仿真 csv，先跑: python3 tools/run_middle_step_experiment.py sim")
+ return 1
+
+ out = OUT_DIR / "ros_real_vs_sim_middle_left_compare_qvt.png"
+ cmd = [
+ sys.executable,
+ str(ROOT / "tools/plot_real_sim_compare.py"),
+ str(real_csv),
+ str(sim_csv),
+ "-o",
+ str(out),
+ ]
+ print(f"[compare] real={real_csv.name}")
+ print(f"[compare] sim ={sim_csv.name}")
+ return subprocess.call(cmd, cwd=str(ROOT))
+
+
+def main():
+ parser = argparse.ArgumentParser(description="O6 middle step experiment (real/sim/compare)")
+ parser.add_argument(
+ "mode",
+ choices=["real", "sim", "compare"],
+ help="real=真机录制 | sim=MuJoCo录制 | compare=对比图",
+ )
+ parser.add_argument("--hand", default="left")
+ parser.add_argument("--finger", default="middle")
+ parser.add_argument("--viewer", action="store_true", help="sim 模式打开 MuJoCo 窗口")
+ parser.add_argument("--real-csv", default="")
+ parser.add_argument("--sim-csv", default="")
+ args = parser.parse_args()
+
+ print("=" * 60)
+ print("O6 middle step 实验时序（真机 / 仿真相同）")
+ print(f" pre_wait={PRE_WAIT}s hold={HOLD}s record_after={RECORD_AFTER}s")
+ print(f" ROS_DOMAIN_ID={os.environ.get('ROS_DOMAIN_ID', '31')}")
+ print("=" * 60)
+
+ if args.mode == "real":
+ print("\n[提示] 真机请先: sudo ip link set can0 up type can bitrate 1000000")
+ print(" 并运行 ./run_real_hand.sh（或本脚本前自行启动 SDK）\n")
+ return run_real_experiment(args) or 0
+ if args.mode == "sim":
+ return run_sim_experiment(args) or 0
+ return run_compare(args)
+
+
+if __name__ == "__main__":
+ raise SystemExit(main())
diff --git a/议题3_仿真负责人行动计划.md b/议题3_仿真负责人行动计划.md
new file mode 100644
index 0000000..a535c38
--- /dev/null
+++ b/议题3_仿真负责人行动计划.md
@@ -0,0 +1,224 @@
+# 议题 3 · 仿真负责人行动计划（O6 摩擦力）
+
+> **角色**：仿真负责人
+> **目标**：用「真机三曲线 vs 仿真三曲线」把 MuJoCo 的 **摩擦 / 阻尼** 调到可 Sim2Real
+> **方法原文**：议题 3《O6 手摩擦力测量方法》
+> **本机仿真现状**：`linker_hand_mujoco_ros2` 已接入 O6 左右手 MJCF，ROS2 话题 `/cb_*_hand_control_cmd`（0～255）
+
+---
+
+## 0. 你负责什么 / 不负责什么
+
+| 你负责（仿真侧） | 他人负责（你要盯着交） |
+|------------------|-------------------------|
+| MuJoCo 模型可稳定重放同一串 `command_u8` | `hand_.json` 角度映射（0～255→度）已验收 |
+| 从真机 CSV 驱动仿真，导出 `q_sim,v_sim,τ/I_sim` | API 同帧 `joint_u8` + `current`（实际电流） |
+| 画出真机↔仿真 **角/速/流** 三叠图 | 真机按 A/B/C 协议采数、CSV 落盘 |
+| 按 §3.4 调 `friction` / `damping`（可+自动优化） | Kt、限流值、供电与工装安全 |
+| 输出 `dynamics_defaults.json`（公版）与验收图归档 | 抽检 3～5 台样机、聚合策略确认 |
+
+**红线（文档强调）：**
+
+1. 映射/终点角不对 → **不准调摩擦**
+2. 电流顶满限流 → **不准加摩擦硬凑角度**
+3. 只拟合角度、不管电流 → **结果作废**
+
+---
+
+## 1. 一周里程碑（建议排期）
+
+### Day 0～1：入口验收（卡死点）
+
+向算法/标定/嵌入式要齐，否则议题 3 **不得开干**：
+
+- [ ] `hand_.json`（含 `joint_u8 → deg`，建议还有 `command → deg`）抽检报告
+- [ ] 电流单位 = A（或已换算），确认为 **实测电流**
+- [ ] Kt（有则更好；暂无可用归一化电流）
+- [ ] 样机 SN、固件版本、限流设定
+- [ ] 关节命名对照表：JSON 关节名 ↔ MuJoCo joint 名 ↔ ROS 6 维通道索引
+
+**你要产出：**《仿真入口检查单》一页（通过/阻塞项）。
+
+### Day 1～2：仿真「同指令重放」能力
+
+只做软件能力，先不动参数：
+
+| 模块 | 你做什么 | 验收标准 |
+|------|----------|----------|
+| 日志解析 | 读 `t_ms,command_u8,joint_u8,current_A` | 对齐时间轴、无丢帧统计 |
+| Lookup | `joint_u8/command` → `q_real/q_cmd`（度→仿真内部 rad） | 与 JSON 抽检一致 |
+| `mj_rollout` | 对**单关节**重放 A/B/C 的 command 轨迹 | 可得到 `q_sim,v_sim`；力矩 `ctrl/actuator force` |
+| 电流代理 | `I_sim ≈ τ_sim / Kt`（或归一化到真机量级） | 能画「仿真电流」虚线 |
+| `plot_three_curves` | 角/速/流三子图，真机实线+仿真虚线 | 出图可直接给人看 §3 |
+
+**对接现有工程建议路径：**
+
+```text
+~/linker_hand_mujoco_ros2/ # ROS2 联调、话题 0～255
+~/mujoco_testwork/o6/ # 已有 scene / validate 脚本，可迁辨识离线工具
+新建例如：
+ tools/friction_id/
+ parse_csv.py
+ apply_lookup.py
+ mj_rollout.py # 优先离线 MjModel，不强制开 GUI
+ plot_three_curves.py
+ identify_friction.py
+ export_dynamics.py
+```
+
+离线辨识用「直接写 `data.ctrl` / 位置执行器目标」即可；**不必**经过 ROS 话题（话题留给联调演示）。
+
+### Day 2～4：单关节闭环调参（先一个指，例如 index 根节）
+
+按文档协议：
+
+1. 真机交付该关节 `A.csv / B.csv / C.csv`
+2. 画真机三曲线 → 按 §3.2 确认「像正常动作」
+3. 现有摩擦/阻尼初值跑仿真叠图
+4. **只用 A+B** 优化 `{friction, damping}`
+5. **段 C** 只验收：三叠图肉眼大致重合 + 未限流
+6. Pass → 写入该关节参数草稿
+
+优化目标（入门）：
+
+```text
+J = w1·‖q_real−q_sim‖² + w2·‖v_real−v_sim‖² + w3·‖I_real−I_sim‖²
+建议起步：w1=1.0, w2=0.3～1.0, w3=0.5～2.0
+```
+
+人手否决用文档 §3.4 七行表；你要在报告里勾选「属于哪一类」。
+
+### Day 4～6：扩到全手主动 DOF
+
+```text
+for joint in O6_主动关节:
+ 锁其它关节 → 收 A/B/C → 拟合 → 段 C 看图 → 通过才入库
+```
+
+拇指多 DOF：**一次只扫一个 DOF**。
+
+### Day 6～7：公版发布
+
+- 同软件/同 URDF 版本抽 **3～5** 只手，同名关节取 **中位数或均值**
+- 输出：
+ - `dynamics_defaults.json`
+ - 全关节段 C 叠图包
+ - 参数变更说明（改了哪些 joint 的 friction/damping）
+- 回写进仓库 MJCF（本仓 `urdf/O6/...xml` 的 `joint damping` / `frictionloss` 或统一 defaults）
+
+---
+
+## 2. 日常调参 SOP（贴显示器）
+
+每次看图只问三句：
+
+1. **角度**：仿真是更快还是更慢？终点对不对？
+2. **电流**：真机是否更高？有没有顶满？
+3. **慢/快差**哪段更大？→ 摩擦还是阻尼？
+
+| 现象 | 动作 |
+|------|------|
+| 仿真快 + 真机电流更高且未限流 | ↑ 摩擦；仍快再 ↑ 阻尼 |
+| 仿真快 + 真机电流顶满 | **停**；查限流/Kt/力矩上限 |
+| 仿真慢 + 真机电流更低 | ↓ 摩擦/阻尼 |
+| 终点角差一截 | **停调摩擦**；查 JSON/指令 |
+| 慢差大多、快还好 | 主调摩擦 |
+| 快差大多、慢还好 | 主调阻尼 |
+
+验收通过定义（仿真负责人签字）：
+
+- [ ] 段 C 角度大致重合
+- [ ] 段 C 电流平台量级接近，且双方均未长期顶满
+- [ ] 速度形状大体一致（允许滤波后略糊）
+- [ ] 参数已写入 JSON +（可选）MJCF，带版本与日期
+
+---
+
+## 3. 与现有 MuJoCo/ROS2 的衔接
+
+| 能力 | 状态 | 仿真负责人接下来 |
+|------|------|------------------|
+| O6 left/right MJCF | 已在包内 `urdf/O6/` | 辨识后回写 damping/friction |
+| ROS 6 维 0～255 控制 | 已通 | 联调演示用；辨识用离线重放更稳 |
+| Joint UI 看仿真角 | 已有 | 示教/演示；正式对比用 matplotlib 叠图 |
+| mimic 比例 | O6 已实现 | 与真机联动 DOF 一致时再比远端 |
+| 力矩→电流 | 未做 | `mj_rollout` 中补 Kt 换算 |
+| CSV→重放流水线 | 未做 | **本周重点** |
+| `dynamics_defaults.json` | 未做 | 调参出口统一格式 |
+
+**建议两套用途拆开：**
+
+- **辨识管线**：离线、可复现、无 GUI、出图+JSON
+- **演示管线**：`./run_sim.sh` + 话题 `/cb_left_hand_control_cmd`（见 `README_启动.md`）
+
+---
+
+## 4. 你要推动的外部接口（写进对接群）
+
+请对方按固定 CSV（文档 §5.3）：
+
+```text
+t_ms,sn,finger,joint_name,command_u8,joint_u8,current_A
+```
+
+目录：`logs/O6///{A,B,C}.csv`
+
+并提供：
+
+```text
+hand_.json
+Kt（或声明暂用归一化）
+限流阈值（A）
+关节名对照表
+```
+
+对方交数后，你回：
+
+```text
+reports/O6///
+ overlay_A.png / overlay_B.png / overlay_C.png
+ fit_summary.json # θ、J、是否限流、是否 Pass
+```
+
+---
+
+## 5. 风险清单（仿真负责人盯防）
+
+| 风险 | 表现 | 对策 |
+|------|------|------|
+| 映射未验收就采数 | 终点角差大 | 入口 checklist 卡死 |
+| 把限流当摩擦 | 电流贴顶还猛加 friction | §3.4 第 2 条一票否决 |
+| 只过角度不过电流 | 「调完感觉对了」 | 验收强制看电流叠图 |
+| ROS 延迟影响辨识 | 跨机丢包、时序漂 | 辨识用离线 ctrl，不跨机 |
+| 一次动多关节 | 耦合辨不准 | 协议：一次一关节 |
+| 左右手/镜像轴 | 左手 yaw 与右手不同 | 左右分别出参数或明确共用规则 |
+
+---
+
+## 6. 本周最小可交差物（答辩用）
+
+若只能交最小包，按优先级：
+
+1. **一个指根**（如 `index` 根节）A/B 拟合 + C 验收三叠图
+2. 对应 `friction/damping` 与前后对比图（调参前 vs 后）
+3. 一页结论：用了映射否、是否限流、参数值、是否建议写公版
+
+有了最小包再铺全手，避免一周空转在工具上却没有「一张能给人看懂的叠图」。
+
+---
+
+## 7. 每日站会三句话（仿真侧）
+
+1. 今天在辨哪只手、哪个关节、到第几步？
+2. 当前卡在映射 / 电流 / 模型 / 优化哪一类？
+3. 有没有触发红线（映射错、限流、只过角度）？
+
+---
+
+## 8. 一句话角色定义
+
+> **仿真负责人** = 把对方给的「同一道 0～255 题」在 MuJoCo 里重做一遍，交出可对上的角/速/流三叠图，并负责任地拧摩擦和阻尼；尺子（角度映射）和电表（真实电流）不对时，拒绝用仿真参数充数。
+
+---
+
+*配套启动与多机联调见：`README_启动.md`*