diff --git a/config/compound_eye.png b/config/compound_eye.png index a7240e2..88b0139 100644 Binary files a/config/compound_eye.png and b/config/compound_eye.png differ diff --git a/gripper_control_02/README.md b/gripper_control_02/README.md index 61ddd82..9bf352d 100644 --- a/gripper_control_02/README.md +++ b/gripper_control_02/README.md @@ -111,9 +111,10 @@ gripper_control_02\config\gripper_demo_02.py - 左右两边的光流箭头图。 - 左右两边的光流幅值图。 -- 左右法向力、最大切向力、左右差值。 +- 左右法向力、左切向力、右切向力。 - 当前 `state`、`action`、`trigger/armed`、`grip_contact`。 - 当前 `force_pct`、自适应目标力、速度 `speed_pct`、`hold_pos`。 +- 顶部有 `重启` 按钮，会先停止当前控制线程，释放相机与夹爪后再重新启动。 - 当前 `release_armed`，用于判断系统是在补夹紧力，还是已经开始检测人取物松开。 - 折线图带图例，默认固定 y 轴为 `0~3 N`，范围在 `visualizer.py` 顶部的 `PLOT_FORCE_Y_MIN_N / PLOT_FORCE_Y_MAX_N` 调整。 - 左右传感器 FPS、接触标志、光流均值/最大值。 diff --git a/gripper_control_02/__pycache__/visualizer.cpython-311.pyc b/gripper_control_02/__pycache__/visualizer.cpython-311.pyc index c602aa4..0f4667f 100644 Binary files a/gripper_control_02/__pycache__/visualizer.cpython-311.pyc and b/gripper_control_02/__pycache__/visualizer.cpython-311.pyc differ diff --git a/gripper_control_02/__pycache__/visualizer.cpython-313.pyc b/gripper_control_02/__pycache__/visualizer.cpython-313.pyc index fe49cd3..ea81068 100644 Binary files a/gripper_control_02/__pycache__/visualizer.cpython-313.pyc and b/gripper_control_02/__pycache__/visualizer.cpython-313.pyc differ diff --git a/gripper_control_02/config/gripper_demo_02.py b/gripper_control_02/config/gripper_demo_02.py index b6f2268..67bceee 100644 --- a/gripper_control_02/config/gripper_demo_02.py +++ b/gripper_control_02/config/gripper_demo_02.py @@ -153,9 +153,8 @@ SHEAR_FORCE_CALIBRATION = { "clamp_output_min": 0.0, "points": [ {"raw": 1000.0, "force_n": 0.0}, - {"raw": 10000.0, "force_n": 0.377}, - {"raw": 30000.0, "force_n": 1.377}, - {"raw": 62000.0, "force_n": 2.377}, + {"raw": 36000.0, "force_n": 1.19}, + {"raw": 68000.0, "force_n": 2.06}, ], } diff --git a/gripper_control_02/visualizer.py b/gripper_control_02/visualizer.py index 68d12b3..0001502 100644 --- a/gripper_control_02/visualizer.py +++ b/gripper_control_02/visualizer.py @@ -60,6 +60,7 @@ from .main import build_controller PLOT_POINTS = 180 PLOT_FORCE_Y_MIN_N = 0.0 PLOT_FORCE_Y_MAX_N = 3.0 +FORCE_DISPLAY_ZERO_THRESHOLD_N = 0.05 CURVE_COLOR = "#00E5FF" @@ -74,11 +75,19 @@ def cv2_to_pixmap(img_bgr): 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: - text = f"{float(value):.{precision}f}" + 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}" @@ -169,6 +178,11 @@ QPushButton#StopButton { color: #f07070; border: 1px solid #4d1a1a; } +QPushButton#RestartButton { + background-color: #1d1d12; + color: #e0c96a; + border: 1px solid #4d4520; +} """ @@ -232,6 +246,8 @@ class GripperDemo02Viewer(QMainWindow): 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 @@ -265,11 +281,16 @@ class GripperDemo02Viewer(QMainWindow): self.stop_button.setObjectName("StopButton") self.stop_button.clicked.connect(self.stop_control) self.stop_button.setEnabled(False) + self.restart_button = QPushButton("重启") + self.restart_button.setObjectName("RestartButton") + self.restart_button.clicked.connect(self.restart_control) + self._set_button_state("idle") top.addWidget(title) top.addWidget(self.status_chip) top.addWidget(self.action_chip, 1) top.addWidget(self.start_button) top.addWidget(self.stop_button) + top.addWidget(self.restart_button) outer.addLayout(top) main = QGridLayout() @@ -294,10 +315,10 @@ class GripperDemo02Viewer(QMainWindow): side.addLayout(metrics_grid) self.left_force = MetricBox("左法向力", "--") self.right_force = MetricBox("右法向力", "--") - self.shear_force = MetricBox("最大切向力", "--") - self.force_diff = MetricBox("左右差值", "--") - self.force_pct_box = MetricBox("force_pct", "--") - self.target_force_box = 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("接触", "--") @@ -305,8 +326,8 @@ class GripperDemo02Viewer(QMainWindow): boxes = [ self.left_force, self.right_force, - self.shear_force, - self.force_diff, + self.left_shear_box, + self.right_shear_box, self.force_pct_box, self.target_force_box, self.speed_box, @@ -342,7 +363,7 @@ class GripperDemo02Viewer(QMainWindow): def _configure_plot(self): self.plot = self.plot_widget.addPlot() - self.plot.setTitle("左右法向力与最大切向力", color="#999999", size="13pt") + self.plot.setTitle("左右法向力", color="#999999", size="13pt") self.plot.titleLabel.setFont(QFont("SimHei", 13)) self.plot.showAxis("top", False) self.plot.showAxis("right", False) @@ -361,20 +382,43 @@ class GripperDemo02Viewer(QMainWindow): pen=pg.mkPen("#ffb000", width=2), name="右法向 (N)", ) - self.shear_curve = self.plot.plot( - pen=pg.mkPen("#7ddc8a", width=2), - name="最大切向 (N)", - ) - def start_control(self): + 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) + + 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.start_button.setEnabled(False) - self.stop_button.setEnabled(True) + self._set_button_state("restarting" if from_restart else "starting") self.controller_finished = False - self.info_label.setText("启动中：正在连接夹爪与两个触觉传感器") + if from_restart: + self.info_label.setText("重启中：正在重新连接夹爪与两个触觉传感器") + else: + self.info_label.setText("启动中：正在连接夹爪与两个触觉传感器") self.controller = build_controller( self.argv, @@ -391,9 +435,20 @@ class GripperDemo02Viewer(QMainWindow): def stop_control(self): if self.controller is not None: self.controller.stop() - self.stop_button.setEnabled(False) + self._set_button_state("stopping") self.info_label.setText("停止中：正在释放相机与夹爪") + 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() + self.info_label.setText("重启中：正在停止当前控制线程") + return + self.restart_pending = False + self.start_control(from_restart=True) + def _run_controller(self): try: self.controller.run() @@ -413,6 +468,13 @@ class GripperDemo02Viewer(QMainWindow): 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", "--") @@ -429,13 +491,13 @@ class GripperDemo02Viewer(QMainWindow): fmt(status.get("right_normal"), unit), f"raw FPS {self._sample_fps(status, 'right_sample')}", ) - self.shear_force.set_value( - fmt(status.get("max_shear"), shear_unit), - f"dShear {fmt(status.get('shear_change'), shear_unit)}", + self.left_shear_box.set_value( + fmt(status.get("left_shear"), shear_unit), + "left_shear", ) - self.force_diff.set_value( - fmt(status.get("normal_diff"), unit), - f"dN {fmt(status.get('normal_change'), unit)}", + self.right_shear_box.set_value( + fmt(status.get("right_shear"), shear_unit), + "right_shear", ) self.force_pct_box.set_value( str(status.get("force_pct", "--")), @@ -473,9 +535,8 @@ class GripperDemo02Viewer(QMainWindow): self.force_history.append( ( elapsed, - float(status.get("left_normal", 0.0) or 0.0), - float(status.get("right_normal", 0.0) or 0.0), - float(status.get("max_shear", 0.0) or 0.0), + force_display_value(status.get("left_normal", 0.0) or 0.0), + force_display_value(status.get("right_normal", 0.0) or 0.0), ) ) if len(self.force_history) > PLOT_POINTS: @@ -503,7 +564,6 @@ class GripperDemo02Viewer(QMainWindow): x = data[:, 0] self.left_curve.setData(x, data[:, 1]) self.right_curve.setData(x, data[:, 2]) - self.shear_curve.setData(x, data[:, 3]) self.plot.setYRange(PLOT_FORCE_Y_MIN_N, PLOT_FORCE_Y_MAX_N, padding=0) if len(x) >= 2: self.plot.setXRange(max(0.0, x[-1] - 6.0), max(6.0, x[-1]), padding=0) @@ -512,8 +572,15 @@ class GripperDemo02Viewer(QMainWindow): if not self.controller_finished: return self.controller_finished = False - self.start_button.setEnabled(True) - self.stop_button.setEnabled(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.info_label.setText("重启中：正在重新启动") + self.start_control(from_restart=True) + return + self._set_button_state("idle") if self.controller is not None: self.info_label.setText("已停止") @@ -548,6 +615,7 @@ class GripperDemo02Viewer(QMainWindow): 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)