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>
284 lines
10 KiB
Python
Executable File
284 lines
10 KiB
Python
Executable File
#!/usr/bin/env python3
|
||
# SPDX-License-Identifier: Apache-2.0
|
||
"""LinkerHand O6 / G20 简易关节控制 UI。
|
||
|
||
向 Isaac Sim 桥接话题发布 sensor_msgs/JointState（position 为 SDK 0~255）。
|
||
依赖系统 ROS2 + tkinter（不要用 Isaac 的 python.sh 启动）。
|
||
|
||
用法:
|
||
source /opt/ros/jazzy/setup.bash
|
||
python3 linkerhand/hand_control_ui.py
|
||
# 或
|
||
bash linkerhand/run_ui.sh
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import threading
|
||
import time
|
||
import tkinter as tk
|
||
from tkinter import ttk
|
||
|
||
import rclpy
|
||
from rclpy.node import Node
|
||
from sensor_msgs.msg import JointState
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# 手型配置（与仿真话题 / SDK 一致）
|
||
# ---------------------------------------------------------------------------
|
||
HANDS = {
|
||
"O6": {
|
||
"cmd": "/o6/cb_left_hand_control_cmd",
|
||
"state": "/o6/cb_left_hand_state",
|
||
"labels": [
|
||
"大拇指弯曲",
|
||
"大拇指横摆",
|
||
"食指弯曲",
|
||
"中指弯曲",
|
||
"无名指弯曲",
|
||
"小拇指弯曲",
|
||
],
|
||
"init": [250, 250, 250, 250, 250, 250],
|
||
"presets": {
|
||
"张开": [250, 250, 250, 250, 250, 250],
|
||
"握拳": [102, 18, 0, 0, 0, 0],
|
||
"点赞": [250, 79, 0, 0, 0, 0],
|
||
"OK": [139, 91, 103, 250, 250, 250],
|
||
},
|
||
},
|
||
"G20": {
|
||
"cmd": "/cb_left_hand_control_cmd",
|
||
"state": "/cb_left_hand_state",
|
||
"labels": [
|
||
"拇指根部",
|
||
"食指根部",
|
||
"中指根部",
|
||
"无名指根部",
|
||
"小指根部",
|
||
"拇指侧摆",
|
||
"食指侧摆",
|
||
"中指侧摆",
|
||
"无名指侧摆",
|
||
"小指侧摆",
|
||
"拇指横摆",
|
||
"预留11",
|
||
"预留12",
|
||
"预留13",
|
||
"预留14",
|
||
"拇指尖部",
|
||
"食指末端",
|
||
"中指末端",
|
||
"无名指末端",
|
||
"小指末端",
|
||
],
|
||
"init": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
|
||
"presets": {
|
||
"张开": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
|
||
"握拳": [96, 0, 0, 0, 0, 0, 193, 158, 128, 91, 132, 255, 255, 255, 255, 144, 0, 0, 0, 0],
|
||
"点赞": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0],
|
||
"OK": [148, 110, 255, 255, 255, 44, 164, 100, 114, 127, 178, 255, 255, 255, 255, 94, 71, 255, 255, 255],
|
||
},
|
||
# 预留电机不可控
|
||
"disabled": {11, 12, 13, 14},
|
||
},
|
||
}
|
||
|
||
PUBLISH_HZ = 20.0
|
||
|
||
|
||
class HandControlNode(Node):
|
||
def __init__(self) -> None:
|
||
super().__init__("linker_hand_control_ui")
|
||
self._pubs = {
|
||
name: self.create_publisher(JointState, cfg["cmd"], 10) for name, cfg in HANDS.items()
|
||
}
|
||
self._latest_cmd: dict[str, list[float]] = {
|
||
name: list(cfg["init"]) for name, cfg in HANDS.items()
|
||
}
|
||
self._latest_state: dict[str, list[float]] = {
|
||
name: list(cfg["init"]) for name, cfg in HANDS.items()
|
||
}
|
||
for name, cfg in HANDS.items():
|
||
self.create_subscription(
|
||
JointState,
|
||
cfg["state"],
|
||
lambda msg, n=name: self._on_state(n, msg),
|
||
10,
|
||
)
|
||
self.create_timer(1.0 / PUBLISH_HZ, self._publish_all)
|
||
|
||
def _on_state(self, hand: str, msg: JointState) -> None:
|
||
if not msg.position:
|
||
return
|
||
n = len(self._latest_state[hand])
|
||
self._latest_state[hand] = [float(v) for v in msg.position[:n]]
|
||
if len(msg.position) < n:
|
||
self._latest_state[hand].extend([0.0] * (n - len(msg.position)))
|
||
|
||
def set_cmd(self, hand: str, values: list[float]) -> None:
|
||
n = len(self._latest_cmd[hand])
|
||
self._latest_cmd[hand] = [float(max(0, min(255, v))) for v in values[:n]]
|
||
|
||
def set_joint(self, hand: str, index: int, value: float) -> None:
|
||
if 0 <= index < len(self._latest_cmd[hand]):
|
||
self._latest_cmd[hand][index] = float(max(0, min(255, value)))
|
||
|
||
def get_state(self, hand: str) -> list[float]:
|
||
return list(self._latest_state[hand])
|
||
|
||
def _publish_all(self) -> None:
|
||
now = self.get_clock().now().to_msg()
|
||
for name, pub in self._pubs.items():
|
||
msg = JointState()
|
||
msg.header.stamp = now
|
||
msg.name = [f"motor_{i}" for i in range(len(self._latest_cmd[name]))]
|
||
msg.position = list(self._latest_cmd[name])
|
||
pub.publish(msg)
|
||
|
||
|
||
class HandTab(ttk.Frame):
|
||
def __init__(self, master: tk.Misc, hand: str, cfg: dict, node: HandControlNode) -> None:
|
||
super().__init__(master)
|
||
self.hand = hand
|
||
self.cfg = cfg
|
||
self.node = node
|
||
self.vars: list[tk.IntVar] = []
|
||
self.value_labels: list[ttk.Label] = []
|
||
self.state_labels: list[ttk.Label] = []
|
||
self._building = True
|
||
|
||
top = ttk.Frame(self)
|
||
top.pack(fill=tk.X, padx=8, pady=6)
|
||
ttk.Label(top, text=f"话题: {cfg['cmd']}", font=("Sans", 9)).pack(side=tk.LEFT)
|
||
self.status = ttk.Label(top, text="状态: —", foreground="#555")
|
||
self.status.pack(side=tk.RIGHT)
|
||
|
||
preset_row = ttk.Frame(self)
|
||
preset_row.pack(fill=tk.X, padx=8, pady=4)
|
||
ttk.Label(preset_row, text="预设:").pack(side=tk.LEFT)
|
||
for name, pose in cfg.get("presets", {}).items():
|
||
ttk.Button(preset_row, text=name, command=lambda p=pose: self.apply_preset(p)).pack(
|
||
side=tk.LEFT, padx=3
|
||
)
|
||
|
||
canvas = tk.Canvas(self, borderwidth=0, highlightthickness=0)
|
||
scroll = ttk.Scrollbar(self, orient=tk.VERTICAL, command=canvas.yview)
|
||
inner = ttk.Frame(canvas)
|
||
inner.bind("<Configure>", lambda e: canvas.configure(scrollregion=canvas.bbox("all")))
|
||
canvas.create_window((0, 0), window=inner, anchor="nw")
|
||
canvas.configure(yscrollcommand=scroll.set)
|
||
canvas.pack(side=tk.LEFT, fill=tk.BOTH, expand=True, padx=(8, 0), pady=4)
|
||
scroll.pack(side=tk.RIGHT, fill=tk.Y, padx=(0, 8), pady=4)
|
||
|
||
disabled = cfg.get("disabled", set())
|
||
header = ttk.Frame(inner)
|
||
header.pack(fill=tk.X, pady=(0, 4))
|
||
ttk.Label(header, text="关节", width=14).pack(side=tk.LEFT)
|
||
ttk.Label(header, text="指令", width=6).pack(side=tk.LEFT)
|
||
ttk.Label(header, text="反馈", width=6).pack(side=tk.LEFT)
|
||
|
||
for i, label in enumerate(cfg["labels"]):
|
||
row = ttk.Frame(inner)
|
||
row.pack(fill=tk.X, pady=1)
|
||
ttk.Label(row, text=f"{i:02d} {label}", width=14).pack(side=tk.LEFT)
|
||
var = tk.IntVar(value=int(cfg["init"][i]))
|
||
self.vars.append(var)
|
||
val_lbl = ttk.Label(row, text=f"{var.get():3d}", width=4)
|
||
val_lbl.pack(side=tk.LEFT)
|
||
self.value_labels.append(val_lbl)
|
||
state_lbl = ttk.Label(row, text="—", width=4, foreground="#06c")
|
||
state_lbl.pack(side=tk.LEFT)
|
||
self.state_labels.append(state_lbl)
|
||
scale = ttk.Scale(
|
||
row,
|
||
from_=0,
|
||
to=255,
|
||
orient=tk.HORIZONTAL,
|
||
variable=var,
|
||
command=lambda _v, idx=i: self._on_slide(idx),
|
||
)
|
||
scale.pack(side=tk.LEFT, fill=tk.X, expand=True, padx=4)
|
||
if i in disabled:
|
||
scale.state(["disabled"])
|
||
val_lbl.configure(foreground="#999")
|
||
|
||
self._building = False
|
||
self.after(200, self._refresh_state)
|
||
|
||
def _on_slide(self, index: int) -> None:
|
||
if self._building:
|
||
return
|
||
value = int(self.vars[index].get())
|
||
self.value_labels[index].configure(text=f"{value:3d}")
|
||
self.node.set_joint(self.hand, index, value)
|
||
|
||
def apply_preset(self, pose: list[int]) -> None:
|
||
self._building = True
|
||
for i, v in enumerate(pose):
|
||
if i >= len(self.vars):
|
||
break
|
||
if i in self.cfg.get("disabled", set()):
|
||
continue
|
||
self.vars[i].set(int(v))
|
||
self.value_labels[i].configure(text=f"{int(v):3d}")
|
||
self._building = False
|
||
self.node.set_cmd(self.hand, [float(v.get()) for v in self.vars])
|
||
|
||
def _refresh_state(self) -> None:
|
||
state = self.node.get_state(self.hand)
|
||
for i, lbl in enumerate(self.state_labels):
|
||
if i < len(state):
|
||
lbl.configure(text=f"{int(round(state[i])):3d}")
|
||
self.status.configure(text=f"状态: 已收 {len(state)} DOF @ {PUBLISH_HZ:.0f}Hz")
|
||
self.after(200, self._refresh_state)
|
||
|
||
|
||
class App(tk.Tk):
|
||
def __init__(self, node: HandControlNode) -> None:
|
||
super().__init__()
|
||
self.node = node
|
||
self.title("LinkerHand 关节控制 (默认 G20)")
|
||
self.geometry("720x780")
|
||
self.minsize(560, 480)
|
||
|
||
nb = ttk.Notebook(self)
|
||
nb.pack(fill=tk.BOTH, expand=True, padx=6, pady=6)
|
||
# 默认只展示 G20；需要 O6 时可改 HANDS 顺序或加回 O6
|
||
for hand in ("G20",):
|
||
tab = HandTab(nb, hand, HANDS[hand], node)
|
||
nb.add(tab, text=hand)
|
||
|
||
tip = ttk.Label(
|
||
self,
|
||
text="先启动仿真: bash linkerhand/run_sim.sh\n"
|
||
"（默认位置驱动：稳，拇指可达；可见轻微穿模属正常）\n"
|
||
"拖动滑条即发布；0≈弯曲，255≈张开",
|
||
justify=tk.LEFT,
|
||
foreground="#444",
|
||
)
|
||
tip.pack(fill=tk.X, padx=10, pady=(0, 8))
|
||
|
||
self.protocol("WM_DELETE_WINDOW", self._on_close)
|
||
|
||
def _on_close(self) -> None:
|
||
self.destroy()
|
||
|
||
|
||
def main() -> None:
|
||
rclpy.init()
|
||
node = HandControlNode()
|
||
spin_thread = threading.Thread(target=lambda: rclpy.spin(node), daemon=True)
|
||
spin_thread.start()
|
||
|
||
app = App(node)
|
||
try:
|
||
app.mainloop()
|
||
finally:
|
||
node.destroy_node()
|
||
if rclpy.ok():
|
||
rclpy.shutdown()
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|