import argparse import multiprocessing as mp import sys import threading import time import cv2 import numpy as np def _fix_qt_plugin_path(): try: from PyQt5.QtCore import QLibraryInfo import os os.environ["QT_QPA_PLATFORM_PLUGIN_PATH"] = QLibraryInfo.location( QLibraryInfo.PluginsPath ) except Exception: pass _fix_qt_plugin_path() _QT_IMPORT_ERROR = None try: from PyQt5.QtCore import QEasingCurve, QPropertyAnimation, Qt, QTimer from PyQt5.QtGui import QFont, QImage, QPixmap from PyQt5.QtWidgets import ( QApplication, QFrame, QGraphicsOpacityEffect, QGridLayout, QHBoxLayout, QLabel, QMainWindow, QPushButton, QSizePolicy, QSplitter, QVBoxLayout, QWidget, ) except Exception as exc: _QT_IMPORT_ERROR = exc Qt = QTimer = QPropertyAnimation = QEasingCurve = QFont = QImage = QPixmap = QApplication = None QGridLayout = QHBoxLayout = QLabel = QPushButton = QSizePolicy = QSplitter = None QVBoxLayout = QWidget = None QFrame = QGraphicsOpacityEffect = QMainWindow = object try: import pyqtgraph as pg except Exception as exc: pg = None _PYQTGRAPH_IMPORT_ERROR = exc else: _PYQTGRAPH_IMPORT_ERROR = None from .main import build_controller PLOT_POINTS = 180 PLOT_FORCE_Y_MIN_N = 0.0 PLOT_FORCE_Y_MAX_N = 3.0 PLOT_SHEAR_Y_MIN_N = -3.0 PLOT_SHEAR_Y_MAX_N = 3.0 FORCE_DISPLAY_ZERO_THRESHOLD_N = 0.05 CURVE_COLOR = "#00E5FF" def cv2_to_pixmap(img_bgr): if img_bgr is None: return QPixmap() if len(img_bgr.shape) == 2: img_bgr = cv2.cvtColor(img_bgr, cv2.COLOR_GRAY2BGR) rgb = cv2.cvtColor(np.ascontiguousarray(img_bgr), cv2.COLOR_BGR2RGB) h, w, ch = rgb.shape qimg = QImage(rgb.data, w, h, ch * w, QImage.Format_RGB888).copy() return QPixmap.fromImage(qimg) def force_display_value(value): value = float(value) if abs(value) < FORCE_DISPLAY_ZERO_THRESHOLD_N: return 0.0 return value def fmt(value, unit="", width=0, precision=3): if value is None: text = "--" else: value = force_display_value(value) text = "0" if value == 0.0 else f"{value:.{precision}f}" if width: text = text.rjust(width) return f"{text}{unit}" STATE_TEXT = { "open_wait": "等待触发", "closing": "闭合中", "gripping": "夹持加力", "hold_check": "夹持检测", "open_recover": "张开恢复", "manual_close": "手动闭合", "manual_hold": "手动保持", "error": "错误", } ACTION_TEXT = { "wait": "等待", "waiting-for-sensor-samples": "等待传感器数据", "open-wait-stabilizing": "等待稳定基准", "open-wait-armed": "已就绪", "side-trigger-close": "侧边触发闭合", "closing-continue": "继续闭合", "closing-to-grip": "闭合寻找物体", "object-detected-low-force": "检测到物体", "close-timeout-open": "闭合超时张开", "grip-ramp-up": "夹持加力", "grip-hold": "夹持保持", "hold-reference-armed": "记录夹持基准", "adaptive-grip-ramp": "自适应加力", "adaptive-grip-wait": "等待自适应加力", "release-arm-delay": "释放检测延时", "release-reference-armed": "释放检测就绪", "hold-check": "保持检测", "force-change-release-open": "力变化张开", "open-recover-position": "张开位置恢复", "open-recover-position-stabilizing": "张开位置稳定中", "open-ready-position": "张开到位", "open-recover-quiet": "力稳定恢复", "open-recover-stabilizing": "恢复稳定中", "open-recover-wait-force-clear": "等待力恢复", "open-ready": "恢复完成", "manual-open": "手动张开", "manual-close": "手动闭合", "manual-hold": "手动保持", "manual-release": "手动释放", } def label_with_cn(value, mapping): value = str(value or "--") cn = mapping.get(value) if cn is None: for prefix, text in mapping.items(): if value.startswith(f"{prefix} "): cn = text break if cn is None: return value return f"{value}（{cn}）" STYLE = """ QMainWindow { background-color: #000000; } QWidget { background-color: #000000; color: #9a9a9a; font-family: "Microsoft YaHei", "SimHei"; } QLabel#Title { color: #d7d7d7; font-size: 18px; font-weight: 600; } QLabel#Subtle { color: #777777; font-size: 12px; } QLabel#MetricLabel { color: #888888; font-size: 12px; } QLabel#MetricValue { color: #f2f2f2; font-family: "Cascadia Code", "Consolas"; font-size: 22px; font-weight: 600; } QLabel#SmallValue { color: #d8d8d8; font-family: "Cascadia Code", "Consolas"; font-size: 15px; } QLabel#StatusChip { background-color: #101010; border: 1px solid #303030; border-radius: 4px; color: #00e5ff; font-family: "Cascadia Code", "Consolas"; font-size: 18px; font-weight: 600; padding: 8px 12px; } QFrame#StatusPanel { background-color: #101010; border: 1px solid #303030; border-radius: 4px; } QLabel#StatusMain { color: #00e5ff; font-family: "Cascadia Code", "Consolas"; font-size: 17px; font-weight: 600; } QLabel#StatusSub { color: #9a9a9a; font-family: "Cascadia Code", "Consolas"; font-size: 12px; } QLabel#ActionChip { background-color: #0b0b0b; border: 1px solid #262626; border-radius: 4px; color: #c8c8c8; font-family: "Cascadia Code", "Consolas"; font-size: 13px; padding: 7px 10px; } QLabel#ImagePanel { background-color: #0b0b0b; border: 1px solid #262626; border-radius: 4px; } QFrame#Panel { background-color: #050505; border: 1px solid #222222; border-radius: 4px; } QPushButton { background-color: #171717; color: #d8d8d8; border: 1px solid #363636; border-radius: 4px; padding: 7px 16px; font-size: 13px; } QPushButton:hover { background-color: #242424; color: #ffffff; border: 1px solid #5a5a5a; } QPushButton:pressed { background-color: #0f0f0f; border: 1px solid #8a8a8a; padding-top: 8px; padding-bottom: 6px; } QPushButton:disabled { background-color: #101010; color: #606060; border: 1px solid #242424; } QPushButton#StartButton { background-color: #171717; color: #d8d8d8; border: 1px solid #363636; } QPushButton#StopButton { background-color: #171717; color: #d8d8d8; border: 1px solid #363636; } QPushButton#RestartButton { background-color: #171717; color: #d8d8d8; border: 1px solid #363636; } QPushButton#ManualButton { background-color: #171717; color: #d8d8d8; border: 1px solid #363636; } QPushButton#HoldButton { background-color: #171717; color: #d8d8d8; border: 1px solid #363636; } QSplitter::handle { background-color: #151515; } QSplitter::handle:hover { background-color: #2a2a2a; } QSplitter::handle:horizontal { width: 8px; } QSplitter::handle:vertical { height: 8px; } """ if QPushButton is not None: class AnimatedButton(QPushButton): def __init__(self, text=""): super().__init__(text) self._opacity = QGraphicsOpacityEffect(self) self._opacity.setOpacity(1.0) self.setGraphicsEffect(self._opacity) self._animation = QPropertyAnimation(self._opacity, b"opacity", self) self._animation.setEasingCurve(QEasingCurve.OutCubic) def _animate_opacity(self, target, duration): self._animation.stop() self._animation.setDuration(duration) self._animation.setStartValue(self._opacity.opacity()) self._animation.setEndValue(target) self._animation.start() def mousePressEvent(self, event): if self.isEnabled(): self._animate_opacity(0.72, 70) super().mousePressEvent(event) def mouseReleaseEvent(self, event): if self.isEnabled(): self._animate_opacity(1.0, 140) super().mouseReleaseEvent(event) else: AnimatedButton = None class MetricBox(QFrame): def __init__(self, title, value="--"): super().__init__() self.setObjectName("Panel") layout = QVBoxLayout(self) layout.setContentsMargins(10, 8, 10, 8) layout.setSpacing(4) self.title_label = QLabel(title) self.title_label.setObjectName("MetricLabel") self.value_label = QLabel(value) self.value_label.setObjectName("MetricValue") layout.addWidget(self.title_label) layout.addWidget(self.value_label) def set_value(self, value, sub=None): self.value_label.setText(value) class ImageBox(QFrame): def __init__(self, title): super().__init__() self.setObjectName("Panel") layout = QVBoxLayout(self) layout.setContentsMargins(8, 8, 8, 8) layout.setSpacing(6) self.title_label = QLabel(title) self.title_label.setObjectName("MetricLabel") self.image_label = QLabel() self.image_label.setObjectName("ImagePanel") self.image_label.setAlignment(Qt.AlignCenter) self.image_label.setMinimumSize(180, 180) self.image_label.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) self.image_label.setScaledContents(True) layout.addWidget(self.title_label) layout.addWidget(self.image_label, 1) def set_image(self, img): self.image_label.setPixmap(cv2_to_pixmap(img)) class GripperDemo02Viewer(QMainWindow): def __init__(self, argv=None): super().__init__() if _QT_IMPORT_ERROR is not None: raise RuntimeError(f"PyQt5 import failed: {_QT_IMPORT_ERROR}") if pg is None: raise RuntimeError(f"pyqtgraph import failed: {_PYQTGRAPH_IMPORT_ERROR}") self.setWindowTitle("Gripper Demo 02") self.setStyleSheet(STYLE) self.controller = None self.control_thread = None self.latest_status = None self.status_lock = threading.Lock() self.controller_finished = False self.restart_pending = False self.button_state = "idle" self.start_time = time.time() self.force_history = [] self.argv = argv self._build_ui() self._configure_plot() self.timer = QTimer(self) self.timer.timeout.connect(self._refresh) self.timer.start(33) def _build_ui(self): root = QWidget() self.setCentralWidget(root) outer = QVBoxLayout(root) outer.setContentsMargins(8, 8, 8, 8) outer.setSpacing(8) top = QHBoxLayout() top.setSpacing(8) title = QLabel("Orisys 夹爪") title.setObjectName("Title") self.status_panel = QFrame() self.status_panel.setObjectName("StatusPanel") status_layout = QVBoxLayout(self.status_panel) status_layout.setContentsMargins(10, 6, 10, 6) status_layout.setSpacing(1) self.status_chip = QLabel("state=--") self.status_chip.setObjectName("StatusMain") self.action_label = QLabel("action=--") self.action_label.setObjectName("StatusSub") status_layout.addWidget(self.status_chip) status_layout.addWidget(self.action_label) self.start_button = AnimatedButton("启动") self.start_button.setObjectName("StartButton") self.start_button.clicked.connect(self.start_control) self.stop_button = AnimatedButton("停止") self.stop_button.setObjectName("StopButton") self.stop_button.clicked.connect(self.stop_control) self.stop_button.setEnabled(False) self.restart_button = AnimatedButton("重启") self.restart_button.setObjectName("RestartButton") self.restart_button.clicked.connect(self.restart_control) self.manual_open_button = AnimatedButton("夹爪张开") self.manual_open_button.setObjectName("ManualButton") self.manual_open_button.clicked.connect(lambda: self.send_manual_command("open")) self.manual_close_button = AnimatedButton("夹爪闭合") self.manual_close_button.setObjectName("ManualButton") self.manual_close_button.clicked.connect(lambda: self.send_manual_command("close")) self.manual_hold_button = AnimatedButton("保持") self.manual_hold_button.setObjectName("HoldButton") self.manual_hold_button.clicked.connect(self.toggle_manual_hold) self._set_button_state("idle") top.addWidget(title) top.addWidget(self.status_panel) top.addStretch(1) top.addWidget(self.start_button) top.addWidget(self.stop_button) top.addWidget(self.restart_button) top.addWidget(self.manual_open_button) top.addWidget(self.manual_close_button) top.addWidget(self.manual_hold_button) outer.addLayout(top) main_splitter = QSplitter(Qt.Horizontal) main_splitter.setChildrenCollapsible(False) outer.addWidget(main_splitter, 1) flow_splitter = QSplitter(Qt.Vertical) flow_splitter.setChildrenCollapsible(False) self.left_flow = ImageBox("左侧光流") self.right_flow = ImageBox("右侧光流") flow_splitter.addWidget(self.left_flow) flow_splitter.addWidget(self.right_flow) flow_splitter.setStretchFactor(0, 1) flow_splitter.setStretchFactor(1, 1) flow_splitter.setSizes([360, 360]) main_splitter.addWidget(flow_splitter) side_widget = QWidget() side = QVBoxLayout(side_widget) side.setContentsMargins(0, 0, 0, 0) side.setSpacing(8) main_splitter.addWidget(side_widget) main_splitter.setStretchFactor(0, 2) main_splitter.setStretchFactor(1, 4) main_splitter.setSizes([520, 980]) metrics_grid = QGridLayout() metrics_grid.setSpacing(8) side.addLayout(metrics_grid) self.left_force = MetricBox("左法向力", "--") self.right_force = MetricBox("右法向力", "--") self.left_shear_box = MetricBox("左切向力", "--") self.right_shear_box = MetricBox("右切向力", "--") self.force_pct_box = MetricBox("夹爪当前力比例（%）", "--") self.target_force_box = MetricBox("夹爪目标力比例（%）", "--") self.speed_box = MetricBox("速度", "--") self.trigger_box = MetricBox("是否触发/系统是否允许触发", "--") self.contact_box = MetricBox("接触", "--") self.release_box = MetricBox("释放检测", "--") boxes = [ self.left_force, self.right_force, self.left_shear_box, self.right_shear_box, self.force_pct_box, self.target_force_box, self.speed_box, self.trigger_box, self.contact_box, self.release_box, ] for idx, box in enumerate(boxes): metrics_grid.addWidget(box, idx // 2, idx % 2) pg.setConfigOption("background", "#000000") pg.setConfigOption("foreground", "#777777") pg.setConfigOptions(antialias=True) self.plot_widget = pg.GraphicsLayoutWidget() self.plot_widget.setMinimumHeight(240) side.addWidget(self.plot_widget, 1) def _configure_plot(self): self.plots = [] self.curves = {} plot_specs = [ ("left_normal", "左法向力", "#00E5FF", PLOT_FORCE_Y_MIN_N, PLOT_FORCE_Y_MAX_N), ("right_normal", "右法向力", "#ffb000", PLOT_FORCE_Y_MIN_N, PLOT_FORCE_Y_MAX_N), ("left_shear_x", "左切向 X", "#61d394", PLOT_SHEAR_Y_MIN_N, PLOT_SHEAR_Y_MAX_N), ("left_shear_y", "左切向 Y", "#f07178", PLOT_SHEAR_Y_MIN_N, PLOT_SHEAR_Y_MAX_N), ("right_shear_x", "右切向 X", "#8ab4f8", PLOT_SHEAR_Y_MIN_N, PLOT_SHEAR_Y_MAX_N), ("right_shear_y", "右切向 Y", "#d19a66", PLOT_SHEAR_Y_MIN_N, PLOT_SHEAR_Y_MAX_N), ] for idx, (key, title, color, y_min, y_max) in enumerate(plot_specs): row = idx // 2 col = idx % 2 plot = self.plot_widget.addPlot(row=row, col=col) plot.setTitle(title, color="#999999", size="11pt") plot.titleLabel.setFont(QFont("SimHei", 11)) plot.showAxis("top", False) plot.showAxis("right", False) plot.showGrid(x=False, y=True, alpha=0.12) plot.setLabel("bottom", "时间", units="s") plot.setLabel("left", "力", units="N") plot.getAxis("bottom").setTickSpacing(major=1.0, minor=1.0) plot.setYRange(y_min, y_max, padding=0) plot.setLimits(yMin=y_min, yMax=y_max) plot.enableAutoRange(axis=pg.ViewBox.YAxis, enable=False) self.plots.append(plot) self.curves[key] = plot.plot(pen=pg.mkPen(color, width=2)) def _set_button_state(self, state): labels = { "idle": ("启动", "停止", "重启"), "starting": ("启动中", "停止", "重启"), "running": ("启动", "停止", "重启"), "stopping": ("启动", "停止中", "重启"), "restarting": ("启动", "停止", "重启中"), } enabled = { "idle": (True, False, True), "starting": (False, True, False), "running": (False, True, True), "stopping": (False, False, False), "restarting": (False, False, False), } self.button_state = state start_text, stop_text, restart_text = labels[state] start_enabled, stop_enabled, restart_enabled = enabled[state] self.start_button.setText(start_text) self.stop_button.setText(stop_text) self.restart_button.setText(restart_text) self.start_button.setEnabled(start_enabled) self.stop_button.setEnabled(stop_enabled) self.restart_button.setEnabled(restart_enabled) manual_enabled = state == "running" self.manual_open_button.setEnabled(manual_enabled) self.manual_close_button.setEnabled(manual_enabled) self.manual_hold_button.setEnabled(manual_enabled) def send_manual_command(self, command): if self.controller is None or self.control_thread is None or not self.control_thread.is_alive(): return try: self.controller.request_manual_command(command) except Exception as exc: print(f"manual command failed: {exc}") def toggle_manual_hold(self): command = "release" if self.manual_hold_button.text() == "释放" else "hold" self.send_manual_command(command) def start_control(self, from_restart=False): if self.control_thread is not None and self.control_thread.is_alive(): return self.force_history.clear() self.start_time = time.time() self._set_button_state("restarting" if from_restart else "starting") self.controller_finished = False self.controller = build_controller( self.argv, status_callback=self._on_status, include_visuals=True, visual_size=320, ) self.control_thread = threading.Thread( target=self._run_controller, daemon=True, ) self.control_thread.start() def stop_control(self): if self.controller is not None: self.controller.stop() self._set_button_state("stopping") def restart_control(self): if self.control_thread is not None and self.control_thread.is_alive(): self.restart_pending = True self._set_button_state("restarting") if self.controller is not None: self.controller.stop() return self.restart_pending = False self.start_control(from_restart=True) def _run_controller(self): try: self.controller.run() except Exception as exc: self._on_status({"state": "error", "action": str(exc), "timestamp": time.time()}) finally: self.controller_finished = True def _on_status(self, status): with self.status_lock: self.latest_status = status def _refresh(self): with self.status_lock: status = self.latest_status if not status: self._update_thread_buttons() return self._update_thread_buttons() if ( self.control_thread is not None and self.control_thread.is_alive() and not self.restart_pending and self.button_state in ("starting", "restarting") ): self._set_button_state("running") state = status.get("state", "--") action = status.get("action", "--") unit = status.get("normal_unit", "N") shear_unit = status.get("shear_unit", "N") self.status_chip.setText(f"state={label_with_cn(state, STATE_TEXT)}") self.action_label.setText(f"action={label_with_cn(action, ACTION_TEXT)}") self.manual_hold_button.setText( "释放" if bool(status.get("manual_hold_active")) else "保持" ) self.left_force.set_value( fmt(status.get("left_normal"), unit), ) self.right_force.set_value( fmt(status.get("right_normal"), unit), ) self.left_shear_box.set_value( fmt(status.get("left_shear"), shear_unit), ) self.right_shear_box.set_value( fmt(status.get("right_shear"), shear_unit), ) self.force_pct_box.set_value( str(status.get("force_pct", "--")), ) self.target_force_box.set_value( str(status.get("adaptive_target_force", "--")), ) self.speed_box.set_value( str(status.get("speed_pct", "--")), ) self.trigger_box.set_value( f"{int(bool(status.get('trigger')))} / {int(bool(status.get('trigger_armed')))}", ) self.contact_box.set_value( str(int(bool(status.get("grip_contact")))), ) self.release_box.set_value( str(int(bool(status.get("release_armed")))), ) left_sample = status.get("left_sample") or {} right_sample = status.get("right_sample") or {} self.left_flow.set_image(left_sample.get("flow_view")) self.right_flow.set_image(right_sample.get("flow_view")) elapsed = time.time() - self.start_time self.force_history.append( ( elapsed, force_display_value(status.get("left_normal", 0.0) or 0.0), force_display_value(status.get("right_normal", 0.0) or 0.0), force_display_value(status.get("left_shear_x", 0.0) or 0.0), force_display_value(status.get("left_shear_y", 0.0) or 0.0), force_display_value(status.get("right_shear_x", 0.0) or 0.0), force_display_value(status.get("right_shear_y", 0.0) or 0.0), ) ) if len(self.force_history) > PLOT_POINTS: self.force_history = self.force_history[-PLOT_POINTS:] self._update_plot() def _update_plot(self): if not self.force_history: return data = np.asarray(self.force_history, dtype=float) x = data[:, 0] keys = [ "left_normal", "right_normal", "left_shear_x", "left_shear_y", "right_shear_x", "right_shear_y", ] for idx, key in enumerate(keys, start=1): self.curves[key].setData(x, data[:, idx]) if len(x) >= 2: x_max = max(6.0, float(np.ceil(x[-1]))) x_min = max(0.0, x_max - 6.0) for plot in self.plots: plot.setXRange(x_min, x_max, padding=0) def _update_thread_buttons(self): if not self.controller_finished: return self.controller_finished = False if self.control_thread is not None: self.control_thread.join(timeout=0.1) self.control_thread = None if self.restart_pending: self.restart_pending = False self.start_control(from_restart=True) return self._set_button_state("idle") def keyPressEvent(self, event): if event.key() == Qt.Key_Q: self.close() return if event.key() == Qt.Key_Escape: if self.isFullScreen(): self.showNormal() else: self.showFullScreen() return super().keyPressEvent(event) def closeEvent(self, event): self.timer.stop() self.restart_pending = False self.stop_control() if self.control_thread is not None: self.control_thread.join(timeout=3.0) event.accept() def main(argv=None): parser = argparse.ArgumentParser() parser.add_argument("--windowed", action="store_true") ui_args, controller_argv = parser.parse_known_args(argv) if _QT_IMPORT_ERROR is not None: print( "PyQt5 is required for the visualizer. " "Install it in the active environment, for example: pip install PyQt5 pyqtgraph" ) print(f"Import error: {_QT_IMPORT_ERROR}") return 1 if pg is None: print( "pyqtgraph is required for the visualizer. " "Install it in the active environment, for example: pip install pyqtgraph" ) print(f"Import error: {_PYQTGRAPH_IMPORT_ERROR}") return 1 QApplication.setAttribute(Qt.AA_EnableHighDpiScaling, True) QApplication.setAttribute(Qt.AA_UseHighDpiPixmaps, True) app = QApplication(sys.argv[:1]) window = GripperDemo02Viewer(controller_argv) if ui_args.windowed: window.resize(1500, 900) window.show() else: window.showFullScreen() window.start_control() return app.exec_() if __name__ == "__main__": mp.freeze_support() raise SystemExit(main(sys.argv[1:]))