Files

318 lines
15 KiB
Python
Raw Permalink Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
"""手部合指映射:仿真 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()}")