#!/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("", 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()