sunxianghui
0b727785c6
Includes G20 PLAN/cmd_u8 bridge, joint mapping, dual-hand sim scripts, and G20/L20/O6 URDF with USD payloads for Isaac Sim integration. Co-authored-by: Cursor <cursoragent@cursor.com>
283 lines
9.8 KiB
Python
283 lines
9.8 KiB
Python
# SPDX-License-Identifier: Apache-2.0
|
||
"""LinkerHand SDK (0~255) ↔ URDF 关节弧度映射。逻辑与 ROS2 SDK mapping.py 一致。"""
|
||
|
||
from __future__ import annotations
|
||
|
||
# O6
|
||
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_DIR = [-1, -1, -1, -1, -1, -1]
|
||
O6_R_MIN = O6_L_MIN
|
||
O6_R_MAX = O6_L_MAX
|
||
O6_R_DIR = O6_L_DIR
|
||
|
||
# G20（新 linkerhand_g20_left.urdf）限位；SDK 索引语义与 L20 相同
|
||
L20_L_MIN = [0, 0, 0, 0, 0, 0.0, -0.23, -0.23, -0.23, -0.23, 0.0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
|
||
# 通道5 thumb_cmc_roll max：试调 1.47（原 1.4）
|
||
L20_L_MAX = [0.84, 1.22, 1.22, 1.22, 1.22, 1.47, 0.23, 0.23, 0.23, 0.23, 1.57, 0, 0, 0, 0, 1.29, 1.55, 1.55, 1.55, 1.55]
|
||
L20_L_DIR = [-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
|
||
L20_R_MIN = [0, 0, 0, 0, 0, 0.0, -0.23, -0.23, -0.23, -0.23, 0.0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
|
||
L20_R_MAX = [0.84, 1.22, 1.22, 1.22, 1.22, 1.47, 0.23, 0.23, 0.23, 0.23, 1.57, 0, 0, 0, 0, 1.29, 1.55, 1.55, 1.55, 1.55]
|
||
L20_R_DIR = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
|
||
|
||
# 左手 G20：四指侧摆 (SDK 6..9) 与弯曲方向相反 —— SDK0→下限、255→上限（见 g20_mapping）
|
||
G20_L_MIN = L20_L_MIN
|
||
G20_L_MAX = L20_L_MAX
|
||
G20_L_DIR = [-1, -1, -1, -1, -1, -1, 1, 1, 1, 1, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
|
||
G20_R_MIN = L20_R_MIN
|
||
G20_R_MAX = L20_R_MAX
|
||
G20_R_DIR = L20_R_DIR
|
||
|
||
O6_SDK_JOINTS = [
|
||
"thumb_cmc_pitch",
|
||
"thumb_cmc_yaw",
|
||
"index_mcp_pitch",
|
||
"middle_mcp_pitch",
|
||
"ring_mcp_pitch",
|
||
"pinky_mcp_pitch",
|
||
]
|
||
|
||
# SDK 索引 → URDF 关节名（不含 lh_/rh_ 前缀）
|
||
L20_SDK_TO_URDF = {
|
||
0: "thumb_cmc_pitch",
|
||
1: "index_mcp_pitch",
|
||
2: "middle_mcp_pitch",
|
||
3: "ring_mcp_pitch",
|
||
4: "pinky_mcp_pitch",
|
||
5: "thumb_cmc_roll",
|
||
6: "index_mcp_roll",
|
||
7: "middle_mcp_roll",
|
||
8: "ring_mcp_roll",
|
||
9: "pinky_mcp_roll",
|
||
10: "thumb_cmc_yaw",
|
||
15: "thumb_mcp",
|
||
16: "index_pip",
|
||
17: "middle_pip",
|
||
18: "ring_pip",
|
||
19: "pinky_pip",
|
||
}
|
||
|
||
# URDF dip 关节 mimic pip：dip = pip * multiplier（G20 URDF）
|
||
DIP_MIMIC_MULTIPLIER = 0.89
|
||
THUMB_IP_MIMIC_MULTIPLIER = 1.02
|
||
|
||
|
||
def scale_value(original_value: float, a_min: float, a_max: float, b_min: float, b_max: float) -> float:
|
||
if a_max == a_min:
|
||
return b_min
|
||
return (original_value - a_min) * (b_max - b_min) / (a_max - a_min) + b_min
|
||
|
||
|
||
def clamp(value: float, low: float, high: float) -> float:
|
||
return max(low, min(high, value))
|
||
|
||
|
||
def _tables(hand: str, side: str) -> tuple[list[float], list[float], list[int]]:
|
||
if hand == "O6":
|
||
if side == "left":
|
||
return O6_L_MIN, O6_L_MAX, O6_L_DIR
|
||
return O6_R_MIN, O6_R_MAX, O6_R_DIR
|
||
if hand == "G20":
|
||
if side == "left":
|
||
return G20_L_MIN, G20_L_MAX, G20_L_DIR
|
||
return G20_R_MIN, G20_R_MAX, G20_R_DIR
|
||
if side == "left":
|
||
return L20_L_MIN, L20_L_MAX, L20_L_DIR
|
||
return L20_R_MIN, L20_R_MAX, L20_R_DIR
|
||
|
||
|
||
def range_to_arc(values: list[float], hand: str, side: str) -> list[float]:
|
||
"""SDK 0~255 → 弧度（每 DOF 一条）。"""
|
||
j_min, j_max, j_dir = _tables(hand, side)
|
||
num = 6 if hand == "O6" else 20
|
||
arcs = [0.0] * num
|
||
for i in range(num):
|
||
if hand in ("L20", "G20") and 11 <= i <= 14:
|
||
continue
|
||
val = clamp(values[i] if i < len(values) else 0.0, 0.0, 255.0)
|
||
if j_dir[i] == -1:
|
||
arcs[i] = scale_value(val, 0.0, 255.0, j_max[i], j_min[i])
|
||
else:
|
||
arcs[i] = scale_value(val, 0.0, 255.0, j_min[i], j_max[i])
|
||
return arcs
|
||
|
||
|
||
def arc_to_range(arcs: list[float], hand: str, side: str) -> list[int]:
|
||
"""弧度 → SDK 0~255。"""
|
||
j_min, j_max, j_dir = _tables(hand, side)
|
||
num = 6 if hand == "O6" else 20
|
||
ranges = [0] * num
|
||
for i in range(num):
|
||
if hand in ("L20", "G20") and 11 <= i <= 14:
|
||
ranges[i] = 255
|
||
continue
|
||
val = clamp(arcs[i] if i < len(arcs) else 0.0, j_min[i], j_max[i])
|
||
if j_dir[i] == -1:
|
||
ranges[i] = int(round(scale_value(val, j_min[i], j_max[i], 255.0, 0.0)))
|
||
else:
|
||
ranges[i] = int(round(scale_value(val, j_min[i], j_max[i], 0.0, 255.0)))
|
||
return ranges
|
||
|
||
|
||
def mimic_joint_names(hand: str, side: str) -> set[str]:
|
||
"""PhysX/Newton mimic 从动关节，不可施加 position drive。"""
|
||
if hand == "O6":
|
||
prefix = "lh_" if side == "left" else "rh_"
|
||
return {
|
||
f"{prefix}thumb_ip",
|
||
f"{prefix}index_dip",
|
||
f"{prefix}middle_dip",
|
||
f"{prefix}ring_dip",
|
||
f"{prefix}pinky_dip",
|
||
}
|
||
return {"pinky_dip", "ring_dip", "middle_dip", "index_dip", "thumb_ip"}
|
||
|
||
|
||
# G20 URDF / Isaac USD 规范关节名（PLAN 名义弧度桥按名匹配）
|
||
G20_ACTUATED_JOINT_NAMES: tuple[str, ...] = tuple(
|
||
sorted({name for name in L20_SDK_TO_URDF.values()})
|
||
)
|
||
G20_MIMIC_JOINT_NAMES: tuple[str, ...] = (
|
||
"thumb_ip",
|
||
"index_dip",
|
||
"middle_dip",
|
||
"ring_dip",
|
||
"pinky_dip",
|
||
)
|
||
G20_ALL_JOINT_NAMES: tuple[str, ...] = tuple(
|
||
sorted(set(G20_ACTUATED_JOINT_NAMES) | set(G20_MIMIC_JOINT_NAMES))
|
||
)
|
||
|
||
# mimic 从动 ← (主动, 倍率)；与 G20 URDF / Physics mimic 一致
|
||
G20_MIMIC_OF: dict[str, tuple[str, float]] = {
|
||
"thumb_ip": ("thumb_mcp", THUMB_IP_MIMIC_MULTIPLIER),
|
||
"index_dip": ("index_pip", DIP_MIMIC_MULTIPLIER),
|
||
"middle_dip": ("middle_pip", DIP_MIMIC_MULTIPLIER),
|
||
"ring_dip": ("ring_pip", DIP_MIMIC_MULTIPLIER),
|
||
"pinky_dip": ("pinky_pip", DIP_MIMIC_MULTIPLIER),
|
||
}
|
||
|
||
|
||
def g20_required_joint_names() -> frozenset[str]:
|
||
"""启动校验：Articulation 必须包含的全部 G20 关节名。"""
|
||
return frozenset(G20_ALL_JOINT_NAMES)
|
||
|
||
|
||
def g20_actuated_joint_names() -> frozenset[str]:
|
||
return frozenset(G20_ACTUATED_JOINT_NAMES)
|
||
|
||
|
||
def complete_g20_mimic_positions(positions: dict[str, float]) -> dict[str, float]:
|
||
"""补全缺失的 mimic 关节；已给出的 mimic 值优先保留。"""
|
||
out = dict(positions)
|
||
for mimic, (master, ratio) in G20_MIMIC_OF.items():
|
||
if mimic in out:
|
||
continue
|
||
if master in out:
|
||
out[mimic] = float(out[master]) * float(ratio)
|
||
return out
|
||
|
||
|
||
# SDK 张开预设（与 gui_control/config/constants.py 一致）
|
||
L20_OPEN_CMD = [
|
||
255, 255, 255, 255, 255, 255, 10, 100, 180, 240, 245,
|
||
255, 255, 255, 255, 255, 255, 255, 255, 255,
|
||
]
|
||
G20_OPEN_CMD = [
|
||
255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245,
|
||
255, 255, 255, 255, 255, 255, 255, 255, 255,
|
||
]
|
||
|
||
|
||
def open_hand_command(hand: str) -> list[float]:
|
||
if hand == "O6":
|
||
return [255.0] * 6
|
||
if hand == "G20":
|
||
return [float(v) for v in G20_OPEN_CMD]
|
||
return [float(v) for v in L20_OPEN_CMD]
|
||
|
||
|
||
def _joint_prefix(side: str, hand: str) -> str:
|
||
if hand == "O6":
|
||
return "lh_" if side == "left" else "rh_"
|
||
return ""
|
||
|
||
|
||
O6_MIMIC_OF = {
|
||
"thumb_ip": ("thumb_cmc_pitch", 2.29),
|
||
"index_dip": ("index_mcp_pitch", 0.89),
|
||
"middle_dip": ("middle_mcp_pitch", 0.89),
|
||
"ring_dip": ("ring_mcp_pitch", 0.89),
|
||
"pinky_dip": ("pinky_mcp_pitch", 0.89),
|
||
}
|
||
|
||
|
||
def sdk_range_to_urdf_positions(values: list[float], hand: str, side: str) -> dict[str, float]:
|
||
"""将 SDK 指令转为 URDF 关节名 → 弧度。USD 中 mimic 关节由物理引擎跟随。"""
|
||
arcs = range_to_arc(values, hand, side)
|
||
prefix = _joint_prefix(side, hand)
|
||
out: dict[str, float] = {}
|
||
|
||
if hand == "O6":
|
||
for sdk_idx, short_name in enumerate(O6_SDK_JOINTS):
|
||
out[f"{prefix}{short_name}"] = arcs[sdk_idx]
|
||
return out
|
||
|
||
for sdk_idx, short_name in L20_SDK_TO_URDF.items():
|
||
arc = arcs[sdk_idx]
|
||
if sdk_idx == 15:
|
||
# SDK 控制指尖弧度；thumb_ip mimic thumb_mcp
|
||
out["thumb_mcp"] = arc / THUMB_IP_MIMIC_MULTIPLIER
|
||
elif sdk_idx in (16, 17, 18, 19):
|
||
# SDK 控制 DIP 电机弧度；URDF 中 dip mimic pip
|
||
out[short_name] = arc / DIP_MIMIC_MULTIPLIER
|
||
else:
|
||
out[short_name] = arc
|
||
return out
|
||
|
||
|
||
def sdk_range_to_full_urdf_positions(values: list[float], hand: str, side: str) -> dict[str, float]:
|
||
"""主动关节 + 解析 mimic 从动关节，供运动学逐帧写入。"""
|
||
out = sdk_range_to_urdf_positions(values, hand, side)
|
||
prefix = _joint_prefix(side, hand)
|
||
|
||
if hand == "O6":
|
||
for mimic_suffix, (master_suffix, ratio) in O6_MIMIC_OF.items():
|
||
master = out.get(f"{prefix}{master_suffix}")
|
||
if master is not None:
|
||
out[f"{prefix}{mimic_suffix}"] = master * ratio
|
||
return out
|
||
|
||
for finger in ("index", "middle", "ring", "pinky"):
|
||
pip = out.get(f"{finger}_pip")
|
||
if pip is not None:
|
||
out[f"{finger}_dip"] = pip * DIP_MIMIC_MULTIPLIER
|
||
thumb_mcp = out.get("thumb_mcp")
|
||
if thumb_mcp is not None:
|
||
out["thumb_ip"] = thumb_mcp * THUMB_IP_MIMIC_MULTIPLIER
|
||
return out
|
||
|
||
|
||
def urdf_positions_to_sdk_range(joint_positions: dict[str, float], hand: str, side: str) -> list[int]:
|
||
"""从 URDF 主动关节读数反算 SDK 0~255 反馈（不读 mimic 从动关节）。"""
|
||
mapping_hand = hand if hand in ("O6", "G20", "L20") else "L20"
|
||
prefix = _joint_prefix(side, mapping_hand)
|
||
arcs = [0.0] * (6 if mapping_hand == "O6" else 20)
|
||
|
||
if mapping_hand == "O6":
|
||
for sdk_idx, short_name in enumerate(O6_SDK_JOINTS):
|
||
key = f"{prefix}{short_name}"
|
||
arcs[sdk_idx] = joint_positions.get(key, 0.0)
|
||
return arc_to_range(arcs, mapping_hand, side)
|
||
|
||
for sdk_idx, short_name in L20_SDK_TO_URDF.items():
|
||
if sdk_idx == 15:
|
||
thumb_mcp = joint_positions.get("thumb_mcp", 0.0)
|
||
arcs[sdk_idx] = thumb_mcp * THUMB_IP_MIMIC_MULTIPLIER
|
||
elif sdk_idx in (16, 17, 18, 19):
|
||
pip = joint_positions.get(short_name, 0.0)
|
||
arcs[sdk_idx] = pip * DIP_MIMIC_MULTIPLIER
|
||
else:
|
||
arcs[sdk_idx] = joint_positions.get(short_name, 0.0)
|
||
return arc_to_range(arcs, mapping_hand, side)
|