chenlin/ros2_ws
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initialize FFG left-glove L20 teleoperation workspace with guarded trial modes

Browse Source
This commit is contained in:
cen617-code
2026-09-15 15:14:32 +08:00
commit a9013afdc8
816 changed files with 71500 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitattributes
+2
View File
@@ -0,0 +1,2 @@
*.STL filter=lfs diff=lfs merge=lfs -text
*.stl filter=lfs diff=lfs merge=lfs -text
.gitignore
+37
View File
@@ -0,0 +1,37 @@
# ROS 2 / colcon outputs
/build/
/install/
/log/
# Python environments and caches
/.venv/
/venv/
__pycache__/
*.py[cod]
*.egg-info/
.pytest_cache/
.ruff_cache/
.coverage
htmlcov/
# Local credentials and editor settings
.env
.env.*
!.env.example
*.pem
*.key
.vscode/
.idea/
# Operator-specific calibration and captured hardware data
**/motion/*/tmp/
/diagnostics/
*.bag
*.db3
*.mcap
# Local reference documents (not redistributed)
/*.pdf
/力反馈手套.md
# Temporary files / backups
*.log
*.tmp
*.bak
*~
.DS_Store
.gitmodules
+3
View File
@@ -0,0 +1,3 @@
[submodule "src/Linkerbot_URDF"]
path = src/Linkerbot_URDF
url = https://gitea.robotquan.com/sunxianghui/Linkerbot_URDF.git
README.md
+110
View File
@@ -0,0 +1,110 @@
# FFG 左手套 → L20 左手 ROS 2 工作区
Ubuntu / ROS 2 Jazzy 下，通过 Type-C Linker FFG 左手套控制 CAN L20 左手。
**本项目所测 L20 使用 SDK 的 `G20` 协议及 `g20` 映射，不能据此推广到所有 L20。**
## 当前状态
- 已验证手套读取、个人标定、左手 CAN 版本/位置查询。
- 操作者已确认 ±10 单位试运行、50% 映射、100% 映射、加速跟随档正常。
- 五指触觉矩阵完整，接触响应及移开归零已验证。
- 操作者已确认低速软物抓取/释放正常。
- **非零手套力反馈未开启、未验证。** 协议力值单位、安全上限、释放及断连卸力机制尚待厂家确认。
详情：[进度记录](docs/PROGRESS.md)、[安全边界](docs/SAFETY.md)、[源码来源](docs/UPSTREAM.md)。
## 仓库布局
- `src/linker_hand_ros2_sdk/`：本地修改的硬件 SDK；`trial_guard.py` 为试运行限幅/缓变/超时策略。
- `src/linkerhand_retarget/`：手套采集、标定、位置映射及模型资产。
- `src/Linkerbot_URDF/`：固定提交的上游 Git 子模块。
- 主仓库 STL 网格由 Git LFS 管理；克隆后必须下载实际资产，不能用 LFS 指针文件替代网格。
- `tools/ffg_trial.sh`：隔离 ROS 域的测试入口；勿用原 SDK 默认启动替代。
- `tools/check_*readonly.py`：只读诊断；`tools/test_trial_guard.py`：无硬件策略测试。
- `diagnostics/`、个人标定、原始手册及构建目录仅留本地，不纳入版本控制。
## 克隆和安装
```bash
sudo apt install git-lfs
git lfs install
git clone --recurse-submodules https://gitea.robotquan.com/chenlin/ros2_ws.git
cd ros2_ws
git lfs pull
# 已安装并配置 ROS 2 Jazzy 的 Ubuntu 环境
sudo apt install python3-colcon-common-extensions python3-venv can-utils acl
source /opt/ros/jazzy/setup.bash
/usr/bin/python3 -m venv --system-site-packages .venv
touch .venv/COLCON_IGNORE
.venv/bin/pip install -r requirements.txt
.venv/bin/python -m colcon build --symlink-install \
--packages-select linker_hand_ros2_sdk linkerhand_retarget
```
`requirements-tested.txt` 记录本机直接依赖版本，可用 `pip install -r requirements-tested.txt` 复现 Python 版本组合；它不是 OS/ROS 的完整锁文件。本工作区依赖系统 ROS，不能使用完全隔离、不含系统包的 venv。SDK 与 retarget 需分进程运行，上游顶层 `utils` 导入存在冲突。
## 硬件准备（本机已验证配置）
- FFG 左手：`/dev/ttyUSB0`，2,000,000 baud，固件 2.1.4。
- PCAN-USB：`can0`，1 Mbps，左手标准 CAN ID `0x28`。
- 机械手独立供电；清空夹持区，准备物理断电手段。
```bash
sudo setfacl -m u:$USER:rw /dev/ttyUSB0
# can0 已启用且 bitrate 正确时无需重复设置
sudo ip link set can0 up type can bitrate 1000000
ip -details -statistics link show can0
```
重新插拔串口后 ACL 可能失效。不要在 YAML 中保存 sudo 密码。
## 新操作者必须重新标定
仓库**不包含**本地操作者标定。保持硬件 SDK 退出：
```bash
source /opt/ros/jazzy/setup.bash
source install/setup.bash
ROS_DOMAIN_ID=73 ros2 run linkerhand_retarget handretarget --ros-args \
-p calibration:=true -p ports:='["/dev/ttyUSB0"]' -p baudrate:=2000000
```
按程序完成张手、O 姿态、握拳流程，退出标定后再启动控制。结果保存于
`src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/tmp/jointangle_data.tmp`，该目录已忽略。
## 日常测试入口
终端 1：
```bash
bash tools/ffg_trial.sh glove
```
终端 2（只能运行一个机械手控制进程）：
```bash
bash tools/ffg_trial.sh hand # 小幅验证，输入 ARM
# 逐档确认正常、Ctrl+C 退出上一档后，才能使用下一档：
bash tools/ffg_trial.sh half # 输入 ARM50
bash tools/ffg_trial.sh full # 输入 ARM100
bash tools/ffg_trial.sh follow # 输入 FOLLOW
```
| 档位 | 位置目标 | 电机速度数值 | 目标缓变上限 |
|---|---|---:|---:|
| hand | 相对第一帧手套姿态的变化，限制为硬件启动位置 ±10 单位 | 30 | 10 单位/秒 |
| half | 硬件启动位置 + 0.5 ×（手套映射目标 − 硬件启动位置） | 30 | 10 单位/秒 |
| full | 完整标定位置映射 | 30 | 10 单位/秒 |
| follow | 完整标定位置映射 | 60 | 60 单位/秒 |
位置/速度为 SDK 编码值，不是角度、物理速度或力。50% 指映射跨度，不是机械关节可用角度的一半。所有档位跳过默认启动姿态；收到手套目标后仍可能立即开始运动。脚本使用 ROS 域 74、仅本机发现及专用命令话题。
## 只读触觉检查和离线测试
退出硬件 SDK 后：
```bash
.venv/bin/python tools/check_hand_touch_readonly.py --duration 30
.venv/bin/python tools/test_trial_guard.py
```
触觉工具只查询 type-2 的 12×6 矩阵，缺帧不会复用旧矩阵；结果落在忽略的 `diagnostics/`。
**限速不等于限力；停止发目标不等于急停。** 初次/重新标定后从小幅空载开始，软物抓放使用 `full` 慢速档，不把人体放入夹持区。发生异常立即物理断电。
docs/PROGRESS.md
+36
View File
@@ -0,0 +1,36 @@
# 实验进度（2026-09-15）
## 环境及模型
- 工作区 `/home/cen/ros2_ws`，ROS 2 Jazzy，system-site-packages venv；两个 ROS 包已编译。
- 手套为 Type-C 左 FFG（固件 2.1.4），串口 2 Mbps；21 通道读数有限且有明显活动范围。
- 已完成个人张手/O 姿态/握拳标定，三组数据均为 21 维有限值。
- 用户提供的 L20 LEFT URDF 与 `g20` 关节名、限位一致；已替换 retarget 中 G20 左手 URDF，包含拇指 origin RPY 修正。
- URDF 名称匹配之外，独立完成 CAN 线协议验证：左手 `0x28` 对 C1/C2/C4 及五指 41–45 查询均有真实回复。软件版本原始字节 `01 00 0a`。
## 控制与实机验证
1. 添加 `safe_trial`，跳过 SDK 默认启动速度/力矩/姿态动作，禁用设置话题。
2. 加入初始状态检查、20 维有限值及 0–255 范围检查、目标缓变、300 ms 命令超时锁定。
3. 手套位置包新鲜度门限为 300 ms，防止断连后持续发布缓存姿态。
4. 小幅相对运动、50% 映射、完整映射：操作者均反馈运动正常。
5. 跟随档将目标缓变从 10 提升至 60 单位/秒、电机速度从 30 提升至 60；操作者确认正常。
6. 五指触觉只读验证：75 组数据均完整，补测峰值依次为 108、126、127、124、129，结束均归零。这些是原始强度，不是牛顿或克。
7. 操作者确认慢速软物抓取、释放正常，无明显过度挤压、抖动、异常声音或发热。属于人工观察，不是力学安全认证。
8. 当前策略离线单测 12 项、SDK `test_topic_prefix.py` 的 5 项测试通过；Python 语法、shell 语法及两个包 colcon 构建通过。SDK 测试需先 source ROS 和 install 环境（未 source 的首次采集因找不到包失败，加载环境后通过）。未声称全套上游测试通过。
## 未完成事项
- **非零手套力反馈未测试/未开启**：现有 `max(matrix)*4`、上限 500 缺乏安全单位依据。
- FFG `0x04/A7` 协议单位、安全值、零力释放和固件断连卸力行为需厂家确认。
- 当前抓取为位置控制；无接触力闭环、已验证扭矩限制或硬件急停联锁。
- 尚未测量端到端延迟，也未做长时间耐久、故障注入及硬件断连恢复测试。
## 本地证据（默认不上传）
- `diagnostics/ffg-readonly.json`
- `diagnostics/can-version-query.json`、`diagnostics/can-position-query.json`
- `diagnostics/touch-readonly-1789455774825105359.json`
- `diagnostics/touch-readonly-1789455849390860026.json`
- 标定：`src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/tmp/jointangle_data.tmp`
- 构建日志：`/tmp/ffg-*-build.log`；临时文件不是长期归档。
docs/SAFETY.md
+15
View File
@@ -0,0 +1,15 @@
# 安全边界
这是研究/调试工作区，不是安全认证控制系统。`safe_trial` 名称表示软件试运行策略，不承诺物理安全。
- 原 SDK 默认入口仍保留上游初始化姿态行为；应使用 `tools/ffg_trial.sh`。不要同时运行多个 SDK/遥操作控制器。
- 初始版本、五指位置完整性及数值范围检查不替代持续硬件反馈监控。
- 手套位置数据超过 300 ms 不发布目标；SDK 检查消息时间戳、接收间隔并锁定。锁定后需退出并重启，禁止自动恢复运动。
- 超时只停止发送新目标。机械手可能继续完成最后目标并保持力矩，**不保证松手**。操作系统/进程卡死、CAN 驱动内部重连和固件行为不由此保护覆盖。
- 速度是固件编码值，软件缓变限制的是命令数值，并不保证实际速度、夹持力或碰撞力。
- 试运行不会提高或重设力矩，硬件既有力矩设置仍生效。当前无经标定的抓取力限制或接触自动停止。
- 初始状态与目标不同时，half/full/follow 会在首帧输入后渐进运动。不要误把“无启动姿态命令”当作“不动”。
- 测试保持空载或软泡沫置于支撑面，不用玻璃/金属/易碎物，不夹人体，不提起重物。准备物理断电；Ctrl+C 不是急停。
- 启动器把手套触觉订阅重映射至未使用话题，不接入非零反馈。底层上游代码仍含 `0x04/A7` 输出路径（部分协议可能回复零值）；这不是硬件级禁能，不应手动重映射回触觉话题或发布反馈数据。
- 手套施力前必须取得适配固件的协议：力值单位、安全上限、单指顺序、零力释放、设备看门狗/断连卸力。现有上限 500 只是代码常数。
- 私人标定、原始硬件日志和密码不入库。重建/换操作者必须重新标定。
docs/UPSTREAM.md
+26
View File
@@ -0,0 +1,26 @@
# 上游来源与版本
本仓库是 ROS 2 应用工作区整合，不宣称对上游代码/模型拥有原创版权。
## SDK 与 retarget（vendored 源码）
- 来源：https://gitea.robotquan.com/admin/linkerhand_retarget_ros2.git
- 基线提交：`7953e50ca7d4a38a364ab7d06a2859f0e53b8166`
- 导入目录：`src/linker_hand_ros2_sdk`、`src/linkerhand_retarget`、`src/requirements-retarget.txt`
- 选择直接跟踪源码，以便同一提交复现本工作区的 G20 适配、命令调度、话题隔离、试运行控制和手套新鲜度修改。没有把其他 Git 历史伪装成本仓库历史。
- 保留多型号资产以维持上游模型路径。主仓库的 STL 网格使用 Git LFS，避免首次普通 Git 请求包含约 142 MiB 对象而触发 HTTP 413。单个网格低于 10 MiB，克隆需安装 Git LFS 并下载资产。
- 首次普通 Git 推送失败后，确认远端没有分支，重建未发布的根提交；旧提交 `ec13b93` 保留在本地 `backup/pre-lfs-ec13b93` 分支，不推送该备份分支。
## URDF（Git 子模块）
- 来源：https://gitea.robotquan.com/sunxianghui/Linkerbot_URDF.git
- 固定提交：`afc0ac84a4e7cee4245ba421fb985bab014768a0`
- 路径：`src/Linkerbot_URDF`
- 提供的 `L20/LEFT/linkerhand_g20_left.urdf` 已用于工作区 retarget 的左 G20 模型。
- 克隆需 `--recurse-submodules`；上游若需鉴权，须另行具备访问权限。
## 许可与参考资料
遵守各来源文件自带的版权和许可声明。原上游根目录未发现统一 LICENSE，**本整合不额外授予第三方资产的再分发许可**，也不为整个工作区添加未经确认的开源许可证；进一步公开分发前请核实授权。
操作者提供的 FFG 产品 PDF 与中文操作笔记保留本地、默认不上传；README 已独立整理适用于本工作区的步骤。
requirements-tested.txt
+19
View File
@@ -0,0 +1,19 @@
# Tested direct Python dependencies; excludes ROS/OS and transitive dependency locking.
anytree==2.13.0
colorama==0.4.6
loguru==0.7.3
lxml==5.2.1
numpy==1.26.4
pyqtgraph==0.14.0
pyserial==3.5
PyYAML==6.0.1
scipy==1.11.4
six==1.16.0
tqdm==4.70.1
transforms3d==0.4.2
trimesh==5.1.0
tyro==1.0.16
python-can==4.6.1
python-can-candle==1.2.4
minimalmodbus==2.1.1
pymodbus==3.5.1
requirements.txt
+6
View File
@@ -0,0 +1,6 @@
# Install inside a --system-site-packages venv with ROS 2 Jazzy available.
-r src/requirements-retarget.txt
python-can
python-can-candle
minimalmodbus
pymodbus==3.5.1
src/Linkerbot_URDF
Submodule
+1
Submodule src/Linkerbot_URDF added at afc0ac84a4
src/linker_hand_ros2_sdk/launch/linker_hand.launch.py
+21
View File
@@ -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*权限
}],
),
])
src/linker_hand_ros2_sdk/launch/linker_hand_double.launch.py
+35
View File
@@ -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*
}],
),
])
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/__init__.py
View File
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L10_positions.yaml
+146
View File
@@ -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
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L20_positions.yaml
View File
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L21_positions.yaml
+110
View File
@@ -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
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L25_positions.yaml
+83
View File
@@ -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
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L6_positions.yaml
+26
View File
@@ -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
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L7_positions.yaml
+29
View File
@@ -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
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/O6_positions.yaml
+26
View File
@@ -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
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/__init__.py
View File
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/setting.yaml
+58
View File
@@ -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: L10 # 左手型号 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: L10 # 右手型号 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: "" # 不在仓库保存 sudo 密码；请在终端手动配置 CAN 和串口权限
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/__init__.py
View File
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/__init__.py
View File
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_g20_can.py
+1269
View File
File diff suppressed because it is too large Load Diff
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l10_can.py
+531
View File
@@ -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()
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l20_can.py
+478
View File
@@ -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
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l21_can.py
+822
View File
@@ -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
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l24_can.py
+448
View File
@@ -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
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l25_can.py
+848
View File
@@ -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
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l6_can.py
+426
View File
@@ -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()
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l7_can.py
+419
View File
@@ -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()
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_o6_can.py
+447
View File
@@ -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()
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l10_rs485.py
+345
View File
@@ -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}")
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py
+460
View File
@@ -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}")
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l7_rs485.py
+423
View File
@@ -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}")
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_o6_rs485.py
+671
View File
@@ -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}")
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/linker_hand_api.py
+355
View File
@@ -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")
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/__init__.py
View File
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/color_msg.py
+27
View File
@@ -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)
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/init_linker_hand.py
+81
View File
@@ -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
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/load_write_yaml.py
+101
View File
@@ -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
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/mapping.py
+383
View File
@@ -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))
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/open_can.py
+145
View File
@@ -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
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/__init__.py
View File
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py
+618
View File
@@ -0,0 +1,618 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
'''
编译: colcon build --symlink-install
启动命令:ros2 run linker_hand_ros2_sdk linker_hand_sdk
'''
from linker_hand_ros2_sdk.trial_guard import TrialGuard, valid_pose
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 rclpy.parameter import Parameter
from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
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
COMMAND_QOS = QoSProfile(
history=HistoryPolicy.KEEP_LAST,
depth=1,
reliability=ReliabilityPolicy.RELIABLE,
)
def command_changed(previous, current):
"""Return whether a non-empty command differs from the applied command."""
values = list(current)
if not values:
return False
if previous is None or len(previous) != len(values):
return True
return any(float(old) != float(new) for old, new in zip(previous, values))
def state_poll_due(last_poll_time, now, poll_period):
"""Keep slow CAN state reads off the latency-sensitive command path."""
return last_poll_time is None or now >= last_poll_time + poll_period
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")
# -1 keeps the model's original startup speed. Camera teleoperation can
# set this to a conservative value before the startup pose is sent.
self.declare_parameter('startup_speed', -1)
self.declare_parameter('safe_trial', False)
self.declare_parameter('trial_motion_scale', 0.0)
self.declare_parameter('trial_slew_rate', 10.0)
self.declare_parameter('trial_motor_speed', 30)
self.trial_slew_rate = self.get_parameter('trial_slew_rate').value
self.trial_motor_speed = self.get_parameter('trial_motor_speed').value
if not np.isfinite(self.trial_slew_rate) or not 1 <= self.trial_slew_rate <= 60:
raise ValueError('trial_slew_rate must be in [1, 60]')
if not 1 <= self.trial_motor_speed <= 60:
raise ValueError('trial_motor_speed must be in [1, 60]')
self.trial_motion_scale = self.get_parameter('trial_motion_scale').value
if self.trial_motion_scale not in (0.0, 0.5, 1.0):
raise ValueError('trial_motion_scale must be 0.0, 0.5 or 1.0')
self.safe_trial = self.get_parameter('safe_trial').value
self.trial_guard = None
self.trial_speed_set = False
self.trial_lock_reported = False
# Empty keeps the legacy absolute topics/startup pose. A prefix lets
# two same-side hands coexist without receiving each other's commands.
self.declare_parameter('topic_prefix', '')
self.declare_parameter('startup_pose', Parameter.Type.INTEGER_ARRAY)
# Hardware state reads are synchronous CAN transactions. Keeping them
# below the command rate prevents G20's five-frame reads from starving
# incoming position commands.
self.declare_parameter('state_poll_rate', 60.0)
self.declare_parameter('velocity_poll_rate', 60.0)
# 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.startup_speed = int(self.get_parameter('startup_speed').value)
if self.startup_speed < -1 or self.startup_speed > 255:
raise ValueError('startup_speed must be -1 or in the range [0, 255]')
self.topic_prefix = self.normalize_topic_prefix(
self.get_parameter('topic_prefix').value
)
self.state_poll_rate = float(
self.get_parameter('state_poll_rate').value
)
if self.state_poll_rate <= 0.0:
raise ValueError('state_poll_rate must be greater than zero')
self.state_poll_period = 1.0 / self.state_poll_rate
self.last_state_poll_time = None
self.velocity_poll_rate = float(
self.get_parameter('velocity_poll_rate').value
)
if self.velocity_poll_rate <= 0.0:
raise ValueError('velocity_poll_rate must be greater than zero')
self.velocity_poll_period = 1.0 / self.velocity_poll_rate
self.last_velocity_poll_time = None
configured_startup_pose = self.get_parameter_or(
'startup_pose',
Parameter('startup_pose', Parameter.Type.INTEGER_ARRAY, []),
).value
self.startup_pose = [int(value) for value in configured_startup_pose]
if any(value < 0 or value > 255 for value in self.startup_pose):
raise ValueError('startup_pose values must be in the range [0, 255]')
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.applied_hand_post_cmd = None
self.applied_hand_vel_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, self.topic('/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()
@staticmethod
def normalize_topic_prefix(prefix):
prefix = str(prefix).strip()
if not prefix or prefix == '/':
return ''
if not prefix.startswith('/'):
prefix = '/' + prefix
return prefix.rstrip('/')
def topic(self, absolute_topic):
if not absolute_topic.startswith('/'):
raise ValueError('base topic must be absolute')
return self.topic_prefix + absolute_topic
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,
self.topic(f'/cb_{self.hand_type}_hand_control_cmd'),
self.hand_control_cb,
COMMAND_QOS,
)
self.hand_state_pub = self.create_publisher(JointState, self.topic(f'/cb_{self.hand_type}_hand_state'),10)
self.hand_info_pub = self.create_publisher(String, self.topic(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, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch'), 10)
self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch_pc'), 10)
self.matrix_touch_mass_pub = self.create_publisher(String, self.topic(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, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch'), 10)
self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch_pc'), 10)
self.matrix_touch_mass_pub = self.create_publisher(String, self.topic(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, self.topic(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.safe_trial:
if self.hand_joint != 'G20' or self.hand_type != 'left':
raise ValueError('safe_trial is only validated for left G20 protocol')
# Constructor performs queries only; no startup pose or torque write.
initial = self.api.get_state()
if not valid_pose(initial):
raise ValueError('Cannot start trial without valid hardware state')
if any(len(getattr(self.api.hand, f'x{cmd:02X}')) != 6 for cmd in range(0x41, 0x46)):
raise ValueError('Missing finger position responses')
self.trial_guard = TrialGuard(initial, self.trial_motion_scale, self.trial_slew_rate)
self.applied_hand_post_cmd = list(initial)
mode = ('relative +/-10 units' if self.trial_motion_scale == 0.0
else f'{self.trial_motion_scale:.0%} calibrated position mapping')
self.get_logger().warn(f'SAFE TRIAL: {mode}, speed {self.trial_motor_speed}, slew {self.trial_slew_rate} units/sec; no startup pose command. Incoming targets can cause motion. Timeout latches until restart.')
return
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 == "G20":
# G20 uses a different CAN protocol from L20 even though both
# expose a 20-value ROS command. This is the calibrated startup
# pose also used by linker_hand_advanced_g20.
pose = [255,255,255,255,255,255,193,148,105,42,245,255,255,255,255,255,255,255,255,255]
torque = [255] * 5
speed = [255] * 5
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 self.startup_pose:
if pose is None or len(self.startup_pose) != len(pose):
expected = 0 if pose is None else len(pose)
raise ValueError(
f'startup_pose for {self.hand_joint} must contain {expected} values'
)
pose = list(self.startup_pose)
if self.startup_speed >= 0:
speed = [self.startup_speed] * len(speed)
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 hand_control_cb(self, msg):
if self.safe_trial:
stamp = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
age = self.get_clock().now().nanoseconds * 1e-9 - stamp
if not 0 <= age <= 0.3:
self.trial_guard.locked = True
return
self.trial_guard.accept(list(msg.position), time.monotonic())
return
# The hardware can be slower than the camera. Always replace a
# pending command with the newest sample and never replay an already
# applied sample; this prevents latency from accumulating in software.
position = list(msg.position)
if position:
self.last_hand_post_cmd = (
position
if command_changed(self.applied_hand_post_cmd, position)
else None
)
velocity = list(msg.velocity)
if velocity:
self.last_hand_vel_cmd = (
velocity
if command_changed(self.applied_hand_vel_cmd, velocity)
else None
)
effort = list(msg.effort)
if effort:
self.last_hand_eff_cmd = effort
def _apply_pending_commands(self):
if self.safe_trial:
pose = self.trial_guard.step(time.monotonic())
if self.trial_guard.locked and not self.trial_lock_reported:
self.get_logger().error('TRIAL LOCKED: stale/invalid input; no more targets. Restart required; disconnect power if unsafe.')
self.trial_lock_reported = True
if pose is not None and command_changed(self.applied_hand_post_cmd, pose):
if not self.trial_speed_set:
self.api.set_speed(speed=[self.trial_motor_speed] * 5)
self.trial_speed_set = True
self.api.finger_move(pose=pose)
self.applied_hand_post_cmd = pose
return
if self.cmd_lock:
return
if self.last_hand_post_cmd is not None:
pose = list(self.last_hand_post_cmd)
self.api.finger_move(pose=pose)
self.applied_hand_post_cmd = pose
self.last_hand_post_cmd = None
if self.last_hand_vel_cmd is not None:
vel = list(self.last_hand_vel_cmd)
if not all(x == 0 for x in vel):
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 == "G20" and len(vel) == 20:
# G20 speed is configured per finger, not per one of
# the 20 exposed position slots.
speed = [vel[0],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.applied_hand_vel_cmd = vel
self.last_hand_vel_cmd = None
def _poll_state_if_due(self):
if self.hand_state_pub.get_subscription_count() < 1:
return
now = time.monotonic()
if not state_poll_due(
self.last_state_poll_time, now, self.state_poll_period
):
return
# Record the start time so a slow G20 read cannot immediately trigger
# another read on the following timer callback.
self.last_state_poll_time = now
self.last_hand_state = self.api.get_state()
time.sleep(0.003)
if state_poll_due(
self.last_velocity_poll_time, now, self.velocity_poll_period
):
self.last_velocity_poll_time = now
self.last_hand_vel = self.api.get_joint_speed()
time.sleep(0.002)
def run(self):
if self.sdk_v == 1:
self.sleep_time = 0.009
# Position commands have priority over synchronous state reads.
self._apply_pending_commands()
self._poll_state_if_due()
if self.cmd_lock == False:
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):
if self.safe_trial:
self.get_logger().warn('Settings commands disabled during safe trial')
return
'''控制命令回调'''
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 or []
hand_joint = node.hand_joint.upper()
if len(embedded_version) == 3 or hand_joint in {"O6", "L6", "G20"}:
ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
node.sdk_v = 2
elif len(embedded_version) == 6 and 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("程序已退出。")
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_g20.py
+258
View File
@@ -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("程序已退出。")
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l10.py
+246
View File
@@ -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("程序已退出。")
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l6.py
+251
View File
@@ -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("程序已退出。")
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l7.py
+281
View File
@@ -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("程序已退出。")
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_o6.py
+252
View File
@@ -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("程序已退出。")
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_g20_palm_touch.py
+268
View File
@@ -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("程序已退出。")
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/o6_joint_curve.py
+331
View File
@@ -0,0 +1,331 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""Record O6 joint positions and plot angle/derived-velocity curves.
The O6 ``0x05`` value exposed by the SDK is a configured speed, not a measured
joint velocity. This recorder therefore always derives velocity from the
position samples and their monotonic timestamps.
"""
import argparse
import csv
from datetime import datetime
import json
import math
from pathlib import Path
import sys
import time
import numpy as np
import rclpy
from rclpy.node import Node
from rclpy.utilities import remove_ros_args
from sensor_msgs.msg import JointState
O6_JOINT_NAMES = (
"thumb_cmc_pitch",
"thumb_cmc_yaw",
"index_mcp_pitch",
"middle_mcp_pitch",
"ring_mcp_pitch",
"pinky_mcp_pitch",
)
# Nominal SDK ranges. A hand-specific lookup JSON should be used for formal
# measurements because individual hands can differ from these values.
O6_NOMINAL_MAX_DEG = dict(
zip(O6_JOINT_NAMES, np.rad2deg([0.58, 1.36, 1.6, 1.6, 1.6, 1.6]))
)
class AngleMapper:
"""Convert O6 uint8 positions to degrees using lookup tables or nominal ranges."""
def __init__(self, mapping_json=None):
self.tables = {}
self.source = "SDK nominal range"
if mapping_json:
self._load(Path(mapping_json).expanduser())
def _load(self, path):
with path.open("r", encoding="utf-8") as stream:
data = json.load(stream)
joints = data.get("joints", data)
if not isinstance(joints, dict):
raise ValueError("mapping JSON must contain an object named 'joints'")
for joint_name, values in joints.items():
if not isinstance(values, dict):
continue
joint_u8 = values.get("joint_u8")
joint_real_deg = values.get("joint_real_deg")
if joint_u8 is None or joint_real_deg is None:
continue
if len(joint_u8) != len(joint_real_deg) or len(joint_u8) < 2:
raise ValueError(
f"{joint_name}: joint_u8 and joint_real_deg need equal lengths >= 2"
)
x = np.asarray(joint_u8, dtype=float)
y = np.asarray(joint_real_deg, dtype=float)
if not np.all(np.isfinite(x)) or not np.all(np.isfinite(y)):
raise ValueError(f"{joint_name}: mapping contains a non-finite value")
order = np.argsort(x)
x = x[order]
y = y[order]
if np.any(np.diff(x) <= 0):
raise ValueError(f"{joint_name}: joint_u8 values must be unique")
self.tables[joint_name] = (x, y)
if not self.tables:
raise ValueError(
"mapping JSON has no joint with joint_u8 and joint_real_deg arrays"
)
self.source = str(path)
def to_degrees(self, joint_name, position_u8):
if joint_name in self.tables:
x, y = self.tables[joint_name]
return float(np.interp(position_u8, x, y))
if joint_name not in O6_NOMINAL_MAX_DEG:
raise KeyError(f"no O6 nominal range or lookup table for {joint_name}")
position_u8 = float(np.clip(position_u8, 0.0, 255.0))
return (255.0 - position_u8) * O6_NOMINAL_MAX_DEG[joint_name] / 255.0
def derive_velocity(time_s, angle_deg, cutoff_hz=5.0):
"""Differentiate angle samples and apply a first-order low-pass filter."""
time_s = np.asarray(time_s, dtype=float)
angle_deg = np.asarray(angle_deg, dtype=float)
if len(time_s) != len(angle_deg):
raise ValueError("time and angle arrays must have the same length")
if len(time_s) < 2:
return np.zeros_like(angle_deg)
if np.any(np.diff(time_s) <= 0):
raise ValueError("timestamps must be strictly increasing")
velocity = np.gradient(angle_deg, time_s)
if cutoff_hz <= 0.0:
return velocity
filtered = np.empty_like(velocity)
filtered[0] = velocity[0]
rc = 1.0 / (2.0 * math.pi * cutoff_hz)
for index in range(1, len(velocity)):
dt = time_s[index] - time_s[index - 1]
alpha = dt / (rc + dt)
filtered[index] = filtered[index - 1] + alpha * (
velocity[index] - filtered[index - 1]
)
return filtered
class O6JointCurveRecorder(Node):
def __init__(
self,
hand_type,
duration,
output_dir,
joint_name,
mapping_json,
filter_hz,
):
super().__init__("o6_joint_curve")
self.hand_type = hand_type
self.duration = duration
self.output_dir = Path(output_dir).expanduser()
self.joint_name = joint_name
self.filter_hz = filter_hz
self.mapper = AngleMapper(mapping_json)
self.samples = []
self.start_time = None
self.saved_paths = None
self.finished = False
topic = f"/cb_{hand_type}_hand_state"
self.subscription = self.create_subscription(
JointState, topic, self._state_callback, 100
)
self.stop_timer = self.create_timer(0.05, self._check_duration)
self.get_logger().info(
f"Recording {topic}; duration={duration:.3f}s; angle map={self.mapper.source}"
)
def _state_callback(self, msg):
if self.finished or not msg.position:
return
names = tuple(msg.name) if len(msg.name) == len(msg.position) else O6_JOINT_NAMES
if len(msg.position) != len(names):
self.get_logger().warning(
f"Ignoring JointState with {len(msg.position)} positions and "
f"{len(msg.name)} names"
)
return
now = time.perf_counter()
if self.start_time is None:
self.start_time = now
relative_time = now - self.start_time
for name, position in zip(names, msg.position):
if self.joint_name != "all" and name != self.joint_name:
continue
try:
angle_deg = self.mapper.to_degrees(name, position)
except KeyError as error:
self.get_logger().warning(str(error))
continue
self.samples.append((relative_time, name, float(position), angle_deg))
def _check_duration(self):
if self.finished or self.start_time is None or self.duration <= 0.0:
return
if time.perf_counter() - self.start_time >= self.duration:
self.finish()
rclpy.shutdown()
def finish(self):
if self.finished:
return self.saved_paths
self.finished = True
if not self.samples:
self.get_logger().warning("No joint-state sample received; no files were written")
return None
self.saved_paths = export_curves(
samples=self.samples,
output_dir=self.output_dir,
hand_type=self.hand_type,
mapping_source=self.mapper.source,
cutoff_hz=self.filter_hz,
)
csv_path, plot_path = self.saved_paths
self.get_logger().info(f"Saved samples: {csv_path}")
self.get_logger().info(f"Saved curves: {plot_path}")
return self.saved_paths
def export_curves(samples, output_dir, hand_type, mapping_source, cutoff_hz):
"""Export long-form CSV and a two-panel PNG for all recorded joints."""
import matplotlib
matplotlib.use("Agg")
import matplotlib.pyplot as plt
output_dir = Path(output_dir).expanduser()
output_dir.mkdir(parents=True, exist_ok=True)
stem = f"o6_{hand_type}_{datetime.now().strftime('%Y%m%d_%H%M%S')}"
csv_path = output_dir / f"{stem}.csv"
plot_path = output_dir / f"{stem}.png"
joint_samples = {}
for sample in samples:
joint_samples.setdefault(sample[1], []).append(sample)
processed = {}
for joint_name, values in joint_samples.items():
time_s = np.asarray([value[0] for value in values], dtype=float)
position_u8 = np.asarray([value[2] for value in values], dtype=float)
angle_deg = np.asarray([value[3] for value in values], dtype=float)
velocity_deg_s = derive_velocity(time_s, angle_deg, cutoff_hz)
processed[joint_name] = (time_s, position_u8, angle_deg, velocity_deg_s)
with csv_path.open("w", encoding="utf-8", newline="") as stream:
writer = csv.writer(stream)
writer.writerow(
["t_s", "joint_name", "position_u8", "angle_deg", "velocity_deg_s"]
)
for joint_name, arrays in processed.items():
for row in zip(*arrays):
writer.writerow(
[
f"{row[0]:.9f}",
joint_name,
f"{row[1]:.6f}",
f"{row[2]:.6f}",
f"{row[3]:.6f}",
]
)
figure, axes = plt.subplots(2, 1, sharex=True, figsize=(12, 8))
for joint_name, (time_s, _position_u8, angle_deg, velocity_deg_s) in processed.items():
axes[0].plot(time_s, angle_deg, label=joint_name)
axes[1].plot(time_s, velocity_deg_s, label=joint_name)
axes[0].set_ylabel("Angle (deg)")
axes[0].set_title(f"O6 {hand_type} joint curves\nangle map: {mapping_source}")
axes[1].set_ylabel("Velocity (deg/s)")
axes[1].set_xlabel("Time (s)")
axes[1].set_title(f"Derived velocity, first-order low-pass {cutoff_hz:g} Hz")
for axis in axes:
axis.grid(True, alpha=0.3)
axis.legend(loc="best", fontsize="small")
figure.tight_layout()
figure.savefig(plot_path, dpi=160)
plt.close(figure)
return csv_path, plot_path
def _parse_args(argv):
parser = argparse.ArgumentParser(
description="Record O6 angle and angle-derived velocity curves"
)
parser.add_argument("--hand-type", choices=("left", "right"), default="right")
parser.add_argument(
"--duration",
type=float,
default=10.0,
help="recording seconds; <= 0 records until Ctrl+C (default: 10)",
)
parser.add_argument(
"--output-dir", default="logs/O6/curves", help="CSV/PNG output directory"
)
parser.add_argument(
"--joint",
default="all",
choices=("all",) + O6_JOINT_NAMES,
help="plot one joint or all six (default: all)",
)
parser.add_argument(
"--mapping-json",
help="optional hand_<SN>.json containing joints.<name>.joint_u8 and joint_real_deg",
)
parser.add_argument(
"--filter-hz",
type=float,
default=5.0,
help="velocity low-pass cutoff; <= 0 disables filtering (default: 5)",
)
parsed = parser.parse_args(remove_ros_args(args=argv)[1:])
if parsed.duration == 0.0:
parsed.duration = -1.0
return parsed
def main(args=None):
argv = sys.argv if args is None else [sys.argv[0], *args]
parsed = _parse_args(argv)
rclpy.init(args=args)
node = O6JointCurveRecorder(
hand_type=parsed.hand_type,
duration=parsed.duration,
output_dir=parsed.output_dir,
joint_name=parsed.joint,
mapping_json=parsed.mapping_json,
filter_hz=parsed.filter_hz,
)
try:
rclpy.spin(node)
except KeyboardInterrupt:
node.get_logger().info("Stopping on Ctrl+C")
finally:
node.finish()
node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
if __name__ == "__main__":
main()
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/trial_guard.py
+57
View File
@@ -0,0 +1,57 @@
"""Bounded relative-motion trial policy; values are SDK units, not degrees."""
import math
def valid_pose(values):
return len(values) == 20 and all(math.isfinite(v) and 0 <= v <= 255 for v in values)
class TrialGuard:
def __init__(self, initial, motion_scale=0.0, slew_rate=10.0):
# Zero preserves the original +/-10 relative trial. Other modes
# interpolate from the startup hardware pose to calibrated targets.
if motion_scale not in (0.0, 0.5, 1.0):
raise ValueError('motion_scale must be 0, 0.5 or 1')
if not math.isfinite(slew_rate) or not 1 <= slew_rate <= 60:
raise ValueError('slew_rate must be finite and in [1, 60]')
self.slew_rate = slew_rate
self.motion_scale = motion_scale
if not valid_pose(initial):
raise ValueError('Invalid initial hardware pose')
self.initial = list(initial)
self.output = list(initial)
self.anchor = None
self.target = None
self.received = None
self.tick = None
self.locked = False
def accept(self, values, now):
if self.received is not None and now - self.received > 0.3:
self.locked = True
if not valid_pose(values):
self.locked = True
if self.locked:
return
if self.anchor is None:
self.anchor = list(values)
self.tick = now
if self.motion_scale == 0.0:
self.target = [max(0, min(255, base + max(-10, min(10, v-a))))
for base, v, a in zip(self.initial, values, self.anchor)]
else:
self.target = [base + self.motion_scale * (v - base)
for base, v in zip(self.initial, values)]
self.received = now
def step(self, now):
if self.received is None or self.locked:
return None
if now - self.received > 0.3:
self.locked = True
return None
# Bounded target slew, no catch-up after stalls.
delta = min(max(now - self.tick, 0), 0.05) * self.slew_rate
self.tick = now
self.output = [x + max(-delta, min(delta, y-x)) for x,y in zip(self.output,self.target)]
return [int(round(x)) for x in self.output]
src/linker_hand_ros2_sdk/package.xml
+24
View File
@@ -0,0 +1,24 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>linker_hand_ros2_sdk</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="linker-robot@todo.todo">linker-robot</maintainer>
<license>TODO: License declaration</license>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<exec_depend>rclpy</exec_depend>
<exec_depend>launch</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>python3-numpy</exec_depend>
<exec_depend>python3-matplotlib</exec_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
src/linker_hand_ros2_sdk/pyproject.toml
+3
View File
@@ -0,0 +1,3 @@
[build-system]
requires = ["setuptools>=61.0"]
build-backend = "setuptools.build_meta"
src/linker_hand_ros2_sdk/resource/linker_hand_ros2_sdk
View File
src/linker_hand_ros2_sdk/setup.cfg
+4
View File
@@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/linker_hand_ros2_sdk
[install]
install_scripts=$base/lib/linker_hand_ros2_sdk
src/linker_hand_ros2_sdk/setup.py
+51
View File
@@ -0,0 +1,51 @@
#!/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',
'o6_joint_curve = linker_hand_ros2_sdk.o6_joint_curve: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',
],
},
)
src/linker_hand_ros2_sdk/test/test_copyright.py
+25
View File
@@ -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'
src/linker_hand_ros2_sdk/test/test_flake8.py
+25
View File
@@ -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)
src/linker_hand_ros2_sdk/test/test_pep257.py
+23
View File
@@ -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'
src/linker_hand_ros2_sdk/test/test_topic_prefix.py
+36
View File
@@ -0,0 +1,36 @@
from rclpy.qos import HistoryPolicy
from linker_hand_ros2_sdk.linker_hand import (
COMMAND_QOS,
LinkerHand,
command_changed,
state_poll_due,
)
def test_empty_topic_prefix_preserves_legacy_topics():
assert LinkerHand.normalize_topic_prefix("") == ""
assert LinkerHand.normalize_topic_prefix("/") == ""
def test_topic_prefix_is_absolute_and_has_no_trailing_slash():
assert LinkerHand.normalize_topic_prefix("o6") == "/o6"
assert LinkerHand.normalize_topic_prefix("/g20/") == "/g20"
def test_command_qos_keeps_only_the_latest_sample():
assert COMMAND_QOS.history == HistoryPolicy.KEEP_LAST
assert COMMAND_QOS.depth == 1
def test_identical_commands_are_not_reapplied():
assert command_changed(None, [60, 60])
assert not command_changed([60, 60], [60, 60])
assert command_changed([60, 60], [60, 61])
assert not command_changed([60, 60], [])
def test_state_polling_is_throttled_without_missing_deadline():
assert state_poll_due(None, 10.0, 0.1)
assert not state_poll_due(10.0, 10.09, 0.1)
assert state_poll_due(10.0, 10.1, 0.1)
src/linkerhand_retarget/linkerhand_retarget/__init__.py
View File
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/ReadMe.md
+19
View File
@@ -0,0 +1,19 @@
## v1.0.0.1 update，更新日期2025-03-31
1. 调整linker_hand_l10_left拇指旋转角不当的问题，子版本号变更为1.0.0.1
2. 调整linker_hand_l10_left无名指偏航角的大小，子版本号变更为1.0.0.2
3. 调整linker_hand_l10_left小指偏航角的大小，子版本号变更为1.0.0.3
4. linker_hand_l10_right的拇指旋转角处于异常状态，需结构重新设定，版本封存
5. 调整linker_hand_l20_right拇指旋转角不当的问题，子版本号变更为1.0.0.1
6. 调整linker_hand_l20_right拇指偏航角不当的问题，子版本号变更为1.0.0.2
7. 调整linker_hand_t25_left四指横滚角不当的问题，子版本号变更为1.0.0.1
## v1.0.0.0 create
1. 版本创建
2. 添加linker_hand_l10_left，版本号v1.6.7995.38578
3. 添加linker_hand_l10_right，版本号v1.0.0
4. 添加linker_hand_l20_left，版本号v1.0.0
5. 添加linker_hand_l20_right，版本号v1.0.0
6. 添加linker_hand_t25_left，版本号v1.0.0
7. 添加linker_hand_t25_right，版本号v1.0.0
8. 添加linker_hand_o7_left，版本号v1.0.0
9. 添加linker_hand_o7_right，版本号v1.0.0
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf
+1285
View File
File diff suppressed because it is too large Load Diff
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_l25_left.urdf
Symlink
+1
View File
@@ -0,0 +1 @@
linkerhand_g20_left.urdf
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/hand_base_link.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/index_distal.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/index_metacarpals.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/index_middle.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/index_proximal.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/middle_distal.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/middle_metacarpals.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/middle_middle.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/middle_proximal.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/pinky_distal.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/pinky_metacarpals.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/pinky_middle.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/pinky_proximal.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/ring_distal.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/ring_metacarpals.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/ring_middle.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/ring_proximal.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/thumb_distal.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/thumb_metacarpals.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/thumb_metacarpals_base1.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/thumb_metacarpals_base2.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/thumb_proximal.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right.urdf
+1285
View File
File diff suppressed because it is too large Load Diff
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_l25_right.urdf
Symlink
+1
View File
@@ -0,0 +1 @@
linkerhand_g20_right.urdf
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/hand_base_link.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/index_distal.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/index_metacarpals.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/index_middle.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/index_proximal.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/middle_distal.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/middle_metacarpals.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/middle_middle.STL
LFS
BIN
View File
Binary file not shown.
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/middle_proximal.STL
LFS
BIN
View File
Binary file not shown.

Some files were not shown because too many files have changed in this diff Show More