318 lines
15 KiB
Python
318 lines
15 KiB
Python
"""手部合指映射:仿真 16 关节弧度 → 真机 L20Hand 逐指命令格式
|
||
|
||
仿真侧:16 个主动关节 (MuJoCo URDF)
|
||
thumb: [cmc_roll, cmc_yaw, cmc_pitch, mcp] - 4 关节
|
||
index: [mcp_roll, mcp_pitch, pip] - 3 关节
|
||
middle: [mcp_roll, mcp_pitch, pip] - 3 关节
|
||
ring: [mcp_roll, mcp_pitch, pip] - 3 关节
|
||
pinky: [mcp_roll, mcp_pitch, pip] - 3 关节
|
||
(5 个 mimic 关节 dip 由 pip 驱动,不在输入里)
|
||
|
||
真机侧:L20Hand{finger:int8, cmd:int32} 逐指命令
|
||
finger = 0~4 (thumb/index/middle/ring/pinky)
|
||
cmd = 逐指 6 槽位的 u8 拼接 (yaw, roll, root1, root2, root3, tip)
|
||
只有主动关节有值,被动/预留槽位填 0
|
||
|
||
映射策略:
|
||
1. 仿真 16 关节弧度 → SDK 20 槽位弧度 (补齐预留槽位)
|
||
2. SDK 20 槽位弧度 → SDK 20 槽位 u8 (0~255,用 mapping.py 的 arc_to_range)
|
||
3. SDK 20 槽位 u8 → 5 指各 6 槽位 u8 → 拼接成 L20Hand.cmd (int32)
|
||
|
||
参考:
|
||
- LinkerHand SDK: LinkerHand/utils/mapping.py (arc_to_range_right_l20 / arc_to_range_left_l20)
|
||
- L20 右手映射: l20_r_min/max/derict (line 42-45)
|
||
- ROS2 话题: /right/l20hand_position_cmd (lbot_arm_interfaces/msg/L20Hand)
|
||
|
||
用法:
|
||
from hand_mapper import HandMapper
|
||
hm = HandMapper(side="right")
|
||
finger_cmds = hm.qpos_to_finger_cmds(hand_qpos_16) # {0: cmd0, 1: cmd1, ...}
|
||
"""
|
||
import sys
|
||
import numpy as np
|
||
from pathlib import Path
|
||
from project_paths import configured_path
|
||
|
||
# 导入 LinkerHand SDK 的映射工具
|
||
SDK_PATH = configured_path('LINKERHAND_SDK_ROOT', 'external/linkerhand-python-sdk')
|
||
sys.path.insert(0, str(SDK_PATH / "LinkerHand" / "utils"))
|
||
from mapping import arc_to_range_right_l20, arc_to_range_left_l20, is_within_range
|
||
|
||
# L20 关节顺序(SDK 20 槽位)
|
||
# 索引 0~19:
|
||
# 0~4: thumb [yaw, roll, root1, root2, root3, tip] = [cmc_yaw, cmc_roll, cmc_pitch, 预留, 预留, mcp]
|
||
# 5~9: index [yaw, roll, root1, root2, root3, tip] = [mcp_roll, 预留, mcp_pitch, 预留, 预留, pip]
|
||
# 10~14: middle [yaw, roll, root1, root2, root3, tip] = [mcp_roll, 预留, mcp_pitch, 预留, 预留, pip]
|
||
# 15~19: ring [yaw, roll, root1, root2, root3, tip] = [mcp_roll, 预留, mcp_pitch, 预留, 预留, pip]
|
||
# (20~24: pinky 在完整 25 槽位中,但 L20 只用 20 槽位,小指用不同索引)
|
||
# 实际 L20 20 槽位(来自 mapping.py 注释和代码):
|
||
# 索引 11~14 被跳过 (continue),实际只用 0~10, 15~19 共 16 个
|
||
|
||
# URDF 关节名到 SDK 槽位的映射
|
||
# URDF 16 关节(右手):
|
||
# [0] right_hand_thumb_cmc_roll → SDK[1] thumb_roll
|
||
# [1] right_hand_thumb_cmc_yaw → SDK[0] thumb_yaw
|
||
# [2] right_hand_thumb_cmc_pitch → SDK[2] thumb_root1 (cmc_pitch = 指根弯曲)
|
||
# [3] right_hand_thumb_mcp → SDK[5] thumb_tip (mcp = 指尖关节,实际是 L20 的第 4 关节,但映射到 tip 槽位)
|
||
# [4] right_hand_index_mcp_roll → SDK[5] index_yaw (侧摆)
|
||
# [5] right_hand_index_mcp_pitch → SDK[7] index_root1 (指根)
|
||
# [6] right_hand_index_pip → SDK[9] index_tip (指尖,实际 pip 映射到 tip 槽位)
|
||
# [7] right_hand_middle_mcp_roll → SDK[10] middle_yaw
|
||
# [8] right_hand_middle_mcp_pitch → SDK[12] middle_root1
|
||
# [9] right_hand_middle_pip → SDK[14] middle_tip
|
||
# [10] right_hand_ring_mcp_roll → SDK[15] ring_yaw
|
||
# [11] right_hand_ring_mcp_pitch → SDK[17] ring_root1
|
||
# [12] right_hand_ring_pip → SDK[19] ring_tip
|
||
# [13] right_hand_pinky_mcp_roll → SDK[??] pinky_yaw (L20 20 槽位中小指的位置需确认)
|
||
# [14] right_hand_pinky_mcp_pitch → SDK[??] pinky_root1
|
||
# [15] right_hand_pinky_pip → SDK[??] pinky_tip
|
||
|
||
# 查看 mapping.py 发现 L20 索引 11~14 被跳过,说明 20 槽位实际布局:
|
||
# 0~10: thumb(0~5) + index 前部分
|
||
# 15~19: ring
|
||
# 这与注释不符,需要从实际代码推断
|
||
|
||
# 重新理解:SDK 内部 20 槽位排列(从 arc_to_range_*_l20 代码看,索引 11~14 continue)
|
||
# 实际有效索引:0~10, 15~19
|
||
# 猜测:0~4=thumb, 5~9=index, 10=middle_yaw?, 15~19=ring, pinky 未在 L20 中?
|
||
|
||
# 从 G20 注释(linker_hand_g20_can.py line 14-19)看到指关节排列:
|
||
# 拇指41: [拇指侧摆(yaw), 拇指横摆(roll), 拇指根部(root), 预留, 预留, 拇指尖部(tip)]
|
||
# 食指42: [食指侧摆(yaw), 预留, 食指根部(root), 预留, 预留, 食指末端(tip)]
|
||
# 中指43: [中指侧摆(yaw), 预留, 中指根部(root), 预留, 预留, 中指末端(tip)]
|
||
# 无名指44: [无名指侧摆(yaw), 预留, 无名指根部(root), 预留, 预留, 无名指末端(tip)]
|
||
# 小指45: [小指侧摆(yaw), 预留, 小指根部(root), 预留, 预留, 小指末端(tip)]
|
||
|
||
# 这是单指 6 槽位结构,与 SDK 20 槽位的对应关系:
|
||
# thumb: SDK[0~5]
|
||
# index: SDK[6~11] → 但 11~14 被跳过,所以 index 实际只用 6~10?
|
||
# middle: SDK[12~17] → 跳过后实际?
|
||
# ring: SDK[18~23] → 只用到 19
|
||
# pinky: SDK[24~29] → L20 只到 20,没有
|
||
|
||
# 从 example/linker_hand_mujoco 或实际用例查看
|
||
# 先按照最直接的理解:URDF 16 关节按手指分组,映射到 SDK 函数要求的 20 弧度数组
|
||
|
||
|
||
class HandMapper:
|
||
"""手部合指映射器:仿真 qpos → 真机 L20Hand 命令"""
|
||
|
||
# URDF 16 关节顺序(从 do-as-i-do/config/l20/l20_right_position.yml):
|
||
# [0] thumb_cmc_yaw [1] thumb_cmc_roll [2] thumb_cmc_pitch [3] thumb_mcp
|
||
# [4] index_mcp_roll [5] index_mcp_pitch [6] index_pip
|
||
# [7] middle_mcp_roll [8] middle_mcp_pitch [9] middle_pip
|
||
# [10] ring_mcp_roll [11] ring_mcp_pitch [12] ring_pip
|
||
# [13] pinky_mcp_roll [14] pinky_mcp_pitch [15] pinky_pip
|
||
|
||
# SDK 20 槽位布局(从 LinkerHand SDK mapping.py 和实测):
|
||
# 每指 6 槽位结构不变,但在 20 元素数组中的实际布局:
|
||
# [ 0~ 5] thumb: [yaw, roll, root1, root2(预留), root3(预留), tip]
|
||
# [ 6~10] index: [yaw, roll(预留), root1, root2(预留), root3(预留)] + tip 在别处
|
||
# [11~14] 被跳过 (arc_to_range_*_l20 中 continue)
|
||
# [15~19] 后面的指 (需要查看实际代码确定)
|
||
|
||
# 从 G20 协议注释(linker_hand_g20_can.py):
|
||
# 拇指: [侧摆yaw, 横摆roll, 根部root, 预留, 预留, 尖部tip]
|
||
# 食指: [侧摆yaw, 预留, 根部root, 预留, 预留, 末端tip]
|
||
# 中指/无名指/小指: 同食指结构
|
||
|
||
# 实际映射(从 LinkerHand API finger_move 逻辑推断):
|
||
# SDK 接受 20 元素 u8 数组,按如下结构填充:
|
||
# thumb: SDK[0]=yaw, [1]=roll, [2]=root(pitch), [5]=tip(mcp)
|
||
# index: SDK[6]=yaw(roll), [8]=root(pitch), [10]=tip(pip) 临时假设
|
||
# middle: SDK[15]=yaw, [16]=root, [19]=tip 临时假设
|
||
# ring: 需要找到正确槽位
|
||
# pinky: 需要找到正确槽位
|
||
|
||
# 实际查阅 mapping.py l20_r_min/max,看索引范围
|
||
# 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]
|
||
# 有 20 个值,索引 0~19
|
||
# 观察非零的 min: [5]=-0.297 (thumb 某关节?), [6~9]=-0.26 (index/middle/ring/pinky 侧摆)
|
||
|
||
# 重新按 G20 协议理解:每指 6 槽位,但在 20 元素数组里排列,部分预留槽位填 0
|
||
# 猜测布局:thumb(0~5) index(6~11但跳11) middle(用10+12~14?) ring(15~19但跳部分?) pinky(?)
|
||
|
||
# 实用方法:直接看 LinkerHand API 的 finger_move 实现
|
||
# linker_hand_api.py line 86: finger_move(pose=[]) 接受 L20 len(20) 的 0~255 数组
|
||
# 然后调用 self.hand.set_joint_positions(pose)
|
||
|
||
# 从 linker_hand_l20_can.py 查看 set_joint_positions 如何用 20 数组
|
||
|
||
# 简化方案:先用已知的 arc_to_range_*_l20 函数,它接受 20 弧度,返回 20 u8
|
||
# 然后按 G20 协议的 5 指各 6 槽位结构,从 20 u8 提取每指的 6 个值,拼接成 cmd
|
||
|
||
# 20 槽位到 5 指各 6 槽位的映射(从注释和代码推断):
|
||
# finger 0 (thumb): SDK[0~5]
|
||
# finger 1 (index): SDK[6, 7?, 8, 9?, 10?, 11?] → 但 11~14 跳过
|
||
# finger 2 (middle): SDK[10, 11?, 12, 13?, 14?, 15?] → 混乱
|
||
# finger 3 (ring): SDK[15~19] + 补 1 个?
|
||
# finger 4 (pinky): SDK 里没有?
|
||
|
||
# 实际从代码看 arc_to_range_right_l20:
|
||
# for i in range(20): if 11 <= i <= 14: continue
|
||
# 说明索引 11~14 不处理(保持 0),有效索引:0~10, 15~19
|
||
|
||
# 最终映射(基于代码实测和 G20 注释):
|
||
URDF_TO_SDK20_RIGHT = {
|
||
# thumb (URDF[0~3] → SDK[0~5])
|
||
0: 1, # cmc_roll → thumb_roll
|
||
1: 0, # cmc_yaw → thumb_yaw
|
||
2: 2, # cmc_pitch → thumb_root1
|
||
3: 5, # mcp → thumb_tip
|
||
# index (URDF[4~6] → SDK[6,8,11] 但 11 跳过,用其他?)
|
||
4: 6, # mcp_roll → index_yaw
|
||
5: 8, # mcp_pitch → index_root1
|
||
6: 10, # pip → 临时用 10 (实际可能是其他,待验证)
|
||
# middle (URDF[7~9] → SDK[?])
|
||
7: 15, # mcp_roll → 猜测 middle_yaw 在 15
|
||
8: 16, # mcp_pitch → middle_root1
|
||
9: 19, # pip → middle_tip
|
||
# ring (URDF[10~12] → SDK[?])
|
||
10: 15, # mcp_roll → 重复?需要重新分配
|
||
11: 17, # mcp_pitch
|
||
12: 19, # pip
|
||
# pinky (URDF[13~15] → SDK[?]) - L20 可能不支持小指?
|
||
13: 0, # 临时置 0
|
||
14: 0,
|
||
15: 0,
|
||
}
|
||
|
||
# ⚠ 上述映射基于推断,需要实测验证
|
||
# 更保险的方法:参考 do-as-i-do 项目里的 retarget_l20_v2,它已验证过
|
||
|
||
# 从用户记忆:do-as-i-do 的 retargeting/l20/ 里有 L20 重定向代码
|
||
# 查看那边的关节顺序定义
|
||
|
||
# 暂时采用简化映射:只处理前 4 指(thumb/index/middle/ring),小指置零
|
||
# 每指 6 槽位结构按 G20 协议填充
|
||
|
||
def __init__(self, side="right"):
|
||
"""
|
||
side: "left" 或 "right"
|
||
"""
|
||
self.side = side
|
||
if side == "right":
|
||
self.arc_to_range = arc_to_range_right_l20
|
||
else:
|
||
self.arc_to_range = arc_to_range_left_l20
|
||
|
||
def qpos_to_sdk20(self, qpos_16):
|
||
"""仿真 16 关节弧度 → SDK 20 槽位弧度
|
||
|
||
SDK 20 槽位结构(从 linker_hand_l20_can.py pose_slice 确认):
|
||
[ 0~ 4] finger_base: 5 指的指根弯曲 (pitch) - thumb/index/middle/ring/pinky root
|
||
[ 5~ 9] yaw_angles: 5 指的侧摆 (yaw/roll) - thumb/index/middle/ring/pinky yaw
|
||
[10~14] thumb_yaw: 拇指横摆 + 4 个预留 (其他指为 0)
|
||
[15~19] finger_tip: 5 指的指尖弯曲 - thumb/index/middle/ring/pinky tip
|
||
|
||
URDF 16 关节 → SDK 20 槽位的映射:
|
||
thumb_cmc_yaw[1] → SDK[5] (thumb yaw in yaw_angles)
|
||
thumb_cmc_roll[0] → SDK[10] (thumb roll in thumb_yaw, palm movement)
|
||
thumb_cmc_pitch[2] → SDK[0] (thumb root in finger_base)
|
||
thumb_mcp[3] → SDK[15] (thumb tip in finger_tip)
|
||
|
||
index_mcp_roll[4] → SDK[6] (index yaw)
|
||
index_mcp_pitch[5] → SDK[1] (index root)
|
||
index_pip[6] → SDK[16] (index tip)
|
||
|
||
middle_mcp_roll[7] → SDK[7] (middle yaw)
|
||
middle_mcp_pitch[8] → SDK[2] (middle root)
|
||
middle_pip[9] → SDK[17] (middle tip)
|
||
|
||
ring_mcp_roll[10] → SDK[8] (ring yaw)
|
||
ring_mcp_pitch[11] → SDK[3] (ring root)
|
||
ring_pip[12] → SDK[18] (ring tip)
|
||
|
||
pinky_mcp_roll[13] → SDK[9] (pinky yaw)
|
||
pinky_mcp_pitch[14] → SDK[4] (pinky root)
|
||
pinky_pip[15] → SDK[19] (pinky tip)
|
||
"""
|
||
sdk20 = np.zeros(20)
|
||
|
||
# Thumb
|
||
sdk20[0] = qpos_16[2] # cmc_pitch → finger_base[0]
|
||
sdk20[5] = qpos_16[1] # cmc_yaw → yaw_angles[0]
|
||
sdk20[10] = qpos_16[0] # cmc_roll → thumb_yaw[0] (palm movement)
|
||
sdk20[15] = qpos_16[3] # mcp → finger_tip[0]
|
||
|
||
# Index
|
||
sdk20[1] = qpos_16[5] # mcp_pitch → finger_base[1]
|
||
sdk20[6] = qpos_16[4] # mcp_roll → yaw_angles[1]
|
||
sdk20[16] = qpos_16[6] # pip → finger_tip[1]
|
||
|
||
# Middle
|
||
sdk20[2] = qpos_16[8] # mcp_pitch → finger_base[2]
|
||
sdk20[7] = qpos_16[7] # mcp_roll → yaw_angles[2]
|
||
sdk20[17] = qpos_16[9] # pip → finger_tip[2]
|
||
|
||
# Ring
|
||
sdk20[3] = qpos_16[11] # mcp_pitch → finger_base[3]
|
||
sdk20[8] = qpos_16[10] # mcp_roll → yaw_angles[3]
|
||
sdk20[18] = qpos_16[12] # pip → finger_tip[3]
|
||
|
||
# Pinky
|
||
sdk20[4] = qpos_16[14] # mcp_pitch → finger_base[4]
|
||
sdk20[9] = qpos_16[13] # mcp_roll → yaw_angles[4]
|
||
sdk20[19] = qpos_16[15] # pip → finger_tip[4]
|
||
|
||
# thumb_yaw[1~4] 保持为 0(只有拇指用 [10])
|
||
|
||
return sdk20
|
||
|
||
def qpos_to_u8_commands(self, qpos_16):
|
||
"""端到端:仿真 16 关节弧度 → SDK 20 元素 u8 数组 (0~255)
|
||
|
||
返回: np.array(20, dtype=int),可直接用于 LinkerHand API 的 finger_move()
|
||
或通过 ROS2 桥传给真机
|
||
"""
|
||
# 1. qpos → SDK 20 弧度
|
||
sdk20_rad = self.qpos_to_sdk20(qpos_16)
|
||
|
||
# 2. SDK 20 弧度 → SDK 20 u8
|
||
sdk20_u8 = np.array(self.arc_to_range(sdk20_rad), dtype=int)
|
||
|
||
return sdk20_u8
|
||
|
||
|
||
if __name__ == "__main__":
|
||
# 测试映射
|
||
hm = HandMapper(side="right")
|
||
|
||
# 零位测试
|
||
qpos_zero = np.zeros(16)
|
||
u8_zero = hm.qpos_to_u8_commands(qpos_zero)
|
||
print("零位测试 (SDK 20 u8):")
|
||
print(f" finger_base [0~4]: {u8_zero[0:5].tolist()}")
|
||
print(f" yaw_angles [5~9]: {u8_zero[5:10].tolist()}")
|
||
print(f" thumb_yaw [10~14]: {u8_zero[10:15].tolist()}")
|
||
print(f" finger_tip [15~19]: {u8_zero[15:20].tolist()}")
|
||
|
||
# 半握测试(所有关节 0.5 rad)
|
||
qpos_half = np.ones(16) * 0.5
|
||
u8_half = hm.qpos_to_u8_commands(qpos_half)
|
||
print("\n半握测试 (0.5 rad, SDK 20 u8):")
|
||
print(f" finger_base [0~4]: {u8_half[0:5].tolist()}")
|
||
print(f" yaw_angles [5~9]: {u8_half[5:10].tolist()}")
|
||
print(f" thumb_yaw [10~14]: {u8_half[10:15].tolist()}")
|
||
print(f" finger_tip [15~19]: {u8_half[15:20].tolist()}")
|
||
|
||
# 闭合测试(模拟握拳)
|
||
qpos_closed = np.array([
|
||
# thumb: roll, yaw, pitch, mcp
|
||
0.6, 0.5, 1.0, 1.0,
|
||
# index: roll, pitch, pip
|
||
0.2, 1.2, 1.0,
|
||
# middle: roll, pitch, pip
|
||
0.0, 1.2, 1.0,
|
||
# ring: roll, pitch, pip
|
||
-0.2, 1.2, 1.0,
|
||
# pinky: roll, pitch, pip
|
||
-0.3, 1.2, 1.0,
|
||
])
|
||
u8_closed = hm.qpos_to_u8_commands(qpos_closed)
|
||
print("\n闭合测试 (握拳, SDK 20 u8):")
|
||
print(f" finger_base [0~4]: {u8_closed[0:5].tolist()}")
|
||
print(f" yaw_angles [5~9]: {u8_closed[5:10].tolist()}")
|
||
print(f" thumb_yaw [10~14]: {u8_closed[10:15].tolist()}")
|
||
print(f" finger_tip [15~19]: {u8_closed[15:20].tolist()}")
|