diff --git a/N2.jpg b/N2.jpg new file mode 100644 index 0000000..61591b5 Binary files /dev/null and b/N2.jpg differ diff --git a/config/compound_eye.png b/config/compound_eye.png index 88b0139..87f1b92 100644 Binary files a/config/compound_eye.png and b/config/compound_eye.png differ diff --git a/gripper_control/__pycache__/calibration.cpython-311.pyc b/gripper_control/__pycache__/calibration.cpython-311.pyc index d555bf1..15ff164 100644 Binary files a/gripper_control/__pycache__/calibration.cpython-311.pyc and b/gripper_control/__pycache__/calibration.cpython-311.pyc differ diff --git a/gripper_control/__pycache__/calibration.cpython-313.pyc b/gripper_control/__pycache__/calibration.cpython-313.pyc index f52676e..91afd97 100644 Binary files a/gripper_control/__pycache__/calibration.cpython-313.pyc and b/gripper_control/__pycache__/calibration.cpython-313.pyc differ diff --git a/gripper_control/__pycache__/feedback.cpython-311.pyc b/gripper_control/__pycache__/feedback.cpython-311.pyc index 33e63cf..2a74235 100644 Binary files a/gripper_control/__pycache__/feedback.cpython-311.pyc and b/gripper_control/__pycache__/feedback.cpython-311.pyc differ diff --git a/gripper_control/__pycache__/feedback.cpython-313.pyc b/gripper_control/__pycache__/feedback.cpython-313.pyc index c70cde5..204a56f 100644 Binary files a/gripper_control/__pycache__/feedback.cpython-313.pyc and b/gripper_control/__pycache__/feedback.cpython-313.pyc differ diff --git a/gripper_control/calibration.py b/gripper_control/calibration.py index 0605bc8..99ac605 100644 --- a/gripper_control/calibration.py +++ b/gripper_control/calibration.py @@ -47,6 +47,8 @@ class ForceConverter: enabled: bool = True extrapolate: bool = True clamp_output_min: float | None = 0.0 + input_mode: str = "magnitude" + signed: bool = False @classmethod def from_config(cls, calibration, config_name): @@ -71,6 +73,8 @@ class ForceConverter: enabled=True, extrapolate=bool(calibration.get("extrapolate", True)), clamp_output_min=calibration.get("clamp_output_min", 0.0), + input_mode=str(calibration.get("input", "magnitude")).lower(), + signed=bool(calibration.get("signed", False)), ) def convert(self, raw): @@ -104,8 +108,14 @@ class ForceConverter: value = n0 + ratio * (n1 - n0) return self._clamp(value) + def convert_component(self, raw): + if not self.signed: + return self.convert(raw) + raw = float(raw) + sign = -1.0 if raw < 0.0 else 1.0 + return sign * self.convert(abs(raw)) + def _clamp(self, value): if self.clamp_output_min is not None: value = max(float(self.clamp_output_min), value) return value - diff --git a/gripper_control/feedback.py b/gripper_control/feedback.py index d217553..b0ceffc 100644 --- a/gripper_control/feedback.py +++ b/gripper_control/feedback.py @@ -8,12 +8,20 @@ def shear_magnitude(sample): return (float(sample["fshearx"]) ** 2 + float(sample["fsheary"]) ** 2) ** 0.5 +def _component_shear_enabled(converter): + return getattr(converter, "input_mode", "magnitude") == "components" + + @dataclass class ForceBaseline: left_normal: float = 0.0 right_normal: float = 0.0 left_shear: float = 0.0 right_shear: float = 0.0 + left_shear_x: float = 0.0 + left_shear_y: float = 0.0 + right_shear_x: float = 0.0 + right_shear_y: float = 0.0 @dataclass @@ -27,6 +35,10 @@ class ForceFeedback: right_shear: float max_shear: float shear_diff: float + left_shear_x: float = 0.0 + left_shear_y: float = 0.0 + right_shear_x: float = 0.0 + right_shear_y: float = 0.0 class TactileFeedbackProcessor: @@ -42,6 +54,17 @@ class TactileFeedbackProcessor: self.right_normal_filter = ExponentialFilter(self.config.filter_alpha) self.left_shear_filter = ExponentialFilter(self.config.shear_filter_alpha) self.right_shear_filter = ExponentialFilter(self.config.shear_filter_alpha) + self.left_shear_x_filter = ExponentialFilter(self.config.shear_filter_alpha) + self.left_shear_y_filter = ExponentialFilter(self.config.shear_filter_alpha) + self.right_shear_x_filter = ExponentialFilter(self.config.shear_filter_alpha) + self.right_shear_y_filter = ExponentialFilter(self.config.shear_filter_alpha) + + def _convert_shear(self, sample): + if _component_shear_enabled(self.shear_converter): + shear_x = self.shear_converter.convert_component(sample["fshearx"]) + shear_y = self.shear_converter.convert_component(sample["fsheary"]) + return (shear_x ** 2 + shear_y ** 2) ** 0.5, shear_x, shear_y + return self.shear_converter.convert(shear_magnitude(sample)), 0.0, 0.0 def calibrate_baseline(self, reader): seconds = self.config.baseline_seconds @@ -53,15 +76,25 @@ class TactileFeedbackProcessor: right_values = [] left_shear_values = [] right_shear_values = [] + left_shear_x_values = [] + left_shear_y_values = [] + right_shear_x_values = [] + right_shear_y_values = [] deadline = time.perf_counter() + seconds while time.perf_counter() < deadline: left, right = reader.get_samples(timeout=0.2) if left is not None: left_values.append(self.normal_converter.convert(left["fnormal"])) - left_shear_values.append(self.shear_converter.convert(shear_magnitude(left))) + shear, shear_x, shear_y = self._convert_shear(left) + left_shear_values.append(shear) + left_shear_x_values.append(shear_x) + left_shear_y_values.append(shear_y) if right is not None: right_values.append(self.normal_converter.convert(right["fnormal"])) - right_shear_values.append(self.shear_converter.convert(shear_magnitude(right))) + shear, shear_x, shear_y = self._convert_shear(right) + right_shear_values.append(shear) + right_shear_x_values.append(shear_x) + right_shear_y_values.append(shear_y) time.sleep(0.01) self.baseline = ForceBaseline( @@ -69,6 +102,10 @@ class TactileFeedbackProcessor: right_normal=sum(right_values) / len(right_values) if right_values else 0.0, left_shear=sum(left_shear_values) / len(left_shear_values) if left_shear_values else 0.0, right_shear=sum(right_shear_values) / len(right_shear_values) if right_shear_values else 0.0, + left_shear_x=sum(left_shear_x_values) / len(left_shear_x_values) if left_shear_x_values else 0.0, + left_shear_y=sum(left_shear_y_values) / len(left_shear_y_values) if left_shear_y_values else 0.0, + right_shear_x=sum(right_shear_x_values) / len(right_shear_x_values) if right_shear_x_values else 0.0, + right_shear_y=sum(right_shear_y_values) / len(right_shear_y_values) if right_shear_y_values else 0.0, ) return self.baseline @@ -85,19 +122,34 @@ class TactileFeedbackProcessor: 0.0, self.normal_converter.convert(right["fnormal"]) - self.baseline.right_normal, ) - left_shear = max( - 0.0, - self.shear_converter.convert(shear_magnitude(left)) - self.baseline.left_shear, - ) - right_shear = max( - 0.0, - self.shear_converter.convert(shear_magnitude(right)) - self.baseline.right_shear, - ) + if _component_shear_enabled(self.shear_converter): + _, raw_left_shear_x, raw_left_shear_y = self._convert_shear(left) + _, raw_right_shear_x, raw_right_shear_y = self._convert_shear(right) + left_shear_x = raw_left_shear_x - self.baseline.left_shear_x + left_shear_y = raw_left_shear_y - self.baseline.left_shear_y + right_shear_x = raw_right_shear_x - self.baseline.right_shear_x + right_shear_y = raw_right_shear_y - self.baseline.right_shear_y + left_shear = (left_shear_x ** 2 + left_shear_y ** 2) ** 0.5 + right_shear = (right_shear_x ** 2 + right_shear_y ** 2) ** 0.5 + else: + left_shear_x = left_shear_y = right_shear_x = right_shear_y = 0.0 + left_shear = max( + 0.0, + self.shear_converter.convert(shear_magnitude(left)) - self.baseline.left_shear, + ) + right_shear = max( + 0.0, + self.shear_converter.convert(shear_magnitude(right)) - self.baseline.right_shear, + ) left_normal = self.left_normal_filter.update(left_force) right_normal = self.right_normal_filter.update(right_force) left_shear_filtered = self.left_shear_filter.update(left_shear) right_shear_filtered = self.right_shear_filter.update(right_shear) + left_shear_x_filtered = self.left_shear_x_filter.update(left_shear_x) + left_shear_y_filtered = self.left_shear_y_filter.update(left_shear_y) + right_shear_x_filtered = self.right_shear_x_filter.update(right_shear_x) + right_shear_y_filtered = self.right_shear_y_filter.update(right_shear_y) return ForceFeedback( left_normal=left_normal, @@ -109,5 +161,8 @@ class TactileFeedbackProcessor: right_shear=right_shear_filtered, max_shear=max(left_shear_filtered, right_shear_filtered), shear_diff=left_shear_filtered - right_shear_filtered, + left_shear_x=left_shear_x_filtered, + left_shear_y=left_shear_y_filtered, + right_shear_x=right_shear_x_filtered, + right_shear_y=right_shear_y_filtered, ) - diff --git a/gripper_control_02/__pycache__/controller.cpython-311.pyc b/gripper_control_02/__pycache__/controller.cpython-311.pyc index 724f042..230a339 100644 Binary files a/gripper_control_02/__pycache__/controller.cpython-311.pyc and b/gripper_control_02/__pycache__/controller.cpython-311.pyc differ diff --git a/gripper_control_02/__pycache__/controller.cpython-313.pyc b/gripper_control_02/__pycache__/controller.cpython-313.pyc index bb75513..c698d9f 100644 Binary files a/gripper_control_02/__pycache__/controller.cpython-313.pyc and b/gripper_control_02/__pycache__/controller.cpython-313.pyc differ diff --git a/gripper_control_02/__pycache__/visualizer.cpython-311.pyc b/gripper_control_02/__pycache__/visualizer.cpython-311.pyc index 0f4667f..8a21bf8 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 ea81068..26822ed 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/__pycache__/gripper_demo_02.cpython-311.pyc b/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-311.pyc index 6b1c886..d21cdd7 100644 Binary files a/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-311.pyc and b/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-311.pyc differ diff --git a/gripper_control_02/config/gripper_demo_02.py b/gripper_control_02/config/gripper_demo_02.py index 67bceee..aa2a1a7 100644 --- a/gripper_control_02/config/gripper_demo_02.py +++ b/gripper_control_02/config/gripper_demo_02.py @@ -144,11 +144,13 @@ NORMAL_FORCE_CALIBRATION = { } -# 切向力标定：sqrt(FSHEARX^2 + FSHEARY^2) 原始幅值 -> N。 -# 目前临时沿用 N_.jpg 的法向标定；有切向力标定后替换 points。 +# 切向力标定：FSHEARX / FSHEARY 原始分量 -> N。 +# X/Y 分量分别按同一条曲线标定；控制逻辑再使用标定后分量的合力。 SHEAR_FORCE_CALIBRATION = { "enabled": True, "method": "piecewise_linear", + "input": "components", + "signed": True, "extrapolate": True, "clamp_output_min": 0.0, "points": [ diff --git a/gripper_control_02/controller.py b/gripper_control_02/controller.py index f3c4e20..06b86a9 100644 --- a/gripper_control_02/controller.py +++ b/gripper_control_02/controller.py @@ -21,10 +21,24 @@ class SideTriggerGripController: self.shear_unit = feedback_processor.shear_converter.unit self.status_callback = status_callback self._stop_event = threading.Event() + self._manual_lock = threading.Lock() + self._manual_command = None def stop(self): self._stop_event.set() + def request_manual_command(self, command): + if command not in {"open", "close", "hold", "release"}: + raise ValueError(f"unsupported manual command: {command}") + with self._manual_lock: + self._manual_command = command + + def _pop_manual_command(self): + with self._manual_lock: + command = self._manual_command + self._manual_command = None + return command + def run(self): self._stop_event.clear() self.reader.start() @@ -122,19 +136,92 @@ class SideTriggerGripController: while not self._stop_event.is_set(): tick_start = time.perf_counter() now = time.perf_counter() + manual_action = None + + manual_command = self._pop_manual_command() + if manual_command == "open": + force_pct = int(clamp( + config.open_force, + config.force_min, + config.force_max, + )) + self.gripper.open(force_pct, "manual-open") + self.feedback_processor.reset_filters() + state = "open_recover" + state_start = now + last_open_position_check_time = 0.0 + last_open_position = None + last_open_position_ready = False + recover_quiet_since = None + recover_position_since = None + trigger_clear_since = None + trigger_armed = False + release_armed = False + hold_pos = None + reference = None + manual_action = "manual-open" + elif manual_command == "close": + force_pct = int(clamp( + config.close_force, + config.force_min, + config.force_max, + )) + self.gripper.close(force_pct, "manual-close") + state = "manual_close" + state_start = now + last_force_ramp_time = now + last_open_position_check_time = 0.0 + last_open_position = None + last_open_position_ready = False + recover_quiet_since = None + recover_position_since = None + trigger_clear_since = None + trigger_armed = False + release_armed = False + hold_pos = None + reference = None + manual_action = "manual-close" + elif manual_command == "hold": + force_pct = int(clamp( + config.hold_force, + config.force_min, + config.force_max, + )) + self.gripper.set_force(force_pct) + hold_pos = self.gripper.hold_current_position(force_pct) + state = "manual_hold" + state_start = now + last_force_ramp_time = now + trigger_clear_since = None + trigger_armed = False + release_armed = False + reference = None + manual_action = "manual-hold" + elif manual_command == "release": + if state == "manual_hold": + state = "open_wait" + state_start = now + trigger_clear_since = None + trigger_armed = False + release_armed = False + hold_pos = None + reference = None + manual_action = "manual-release" feedback = self.feedback_processor.read(self.reader, timeout=0.3) if feedback is None: - print(f"state={state} action=waiting-for-sensor-samples") + action = manual_action or "waiting-for-sensor-samples" + print(f"state={state} action={action}") self._emit_status( { "timestamp": time.time(), "state": state, - "action": "waiting-for-sensor-samples", + "action": action, "force_pct": force_pct, "adaptive_target_force": adaptive_target_force, "adaptive_grip_enabled": config.adaptive_grip_enabled, "release_armed": release_armed, + "manual_hold_active": state == "manual_hold", "speed_pct": config.speed, "normal_unit": self.normal_unit, "shear_unit": self.shear_unit, @@ -147,9 +234,11 @@ class SideTriggerGripController: grip_contact = self._grip_contact(feedback) normal_change = 0.0 shear_change = 0.0 - action = "wait" + action = manual_action or "wait" - if state == "open_wait": + if manual_action is not None: + pass + elif state == "open_wait": trigger_edge = trigger and not last_trigger if not trigger: if trigger_clear_since is None: @@ -387,6 +476,12 @@ class SideTriggerGripController: recover_position_since = None action = "open-recover-wait-force-clear" + elif state == "manual_hold": + action = "manual-hold" + + elif state == "manual_close": + action = "manual-close" + self._log_tick( feedback=feedback, state=state, @@ -577,6 +672,10 @@ class SideTriggerGripController: "normal_diff": feedback.normal_diff, "left_shear": feedback.left_shear, "right_shear": feedback.right_shear, + "left_shear_x": feedback.left_shear_x, + "left_shear_y": feedback.left_shear_y, + "right_shear_x": feedback.right_shear_x, + "right_shear_y": feedback.right_shear_y, "max_shear": feedback.max_shear, "shear_diff": feedback.shear_diff, "normal_change": normal_change, @@ -586,6 +685,7 @@ class SideTriggerGripController: "adaptive_target_force": adaptive_target_force, "adaptive_grip_enabled": self.config.adaptive_grip_enabled, "release_armed": bool(release_armed), + "manual_hold_active": state == "manual_hold", "speed_pct": speed_pct, "normal_unit": self.normal_unit, "shear_unit": self.shear_unit, diff --git a/gripper_control_02/visualizer.py b/gripper_control_02/visualizer.py index 0001502..d94d186 100644 --- a/gripper_control_02/visualizer.py +++ b/gripper_control_02/visualizer.py @@ -60,6 +60,8 @@ 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" @@ -183,6 +185,16 @@ QPushButton#RestartButton { color: #e0c96a; border: 1px solid #4d4520; } +QPushButton#ManualButton { + background-color: #101822; + color: #9cc8ff; + border: 1px solid #23405f; +} +QPushButton#HoldButton { + background-color: #102019; + color: #7ee0a2; + border: 1px solid #24573a; +} """ @@ -284,6 +296,15 @@ class GripperDemo02Viewer(QMainWindow): self.restart_button = QPushButton("重启") self.restart_button.setObjectName("RestartButton") self.restart_button.clicked.connect(self.restart_control) + self.manual_open_button = QPushButton("夹爪张开") + self.manual_open_button.setObjectName("ManualButton") + self.manual_open_button.clicked.connect(lambda: self.send_manual_command("open")) + self.manual_close_button = QPushButton("夹爪闭合") + self.manual_close_button.setObjectName("ManualButton") + self.manual_close_button.clicked.connect(lambda: self.send_manual_command("close")) + self.manual_hold_button = QPushButton("保持") + 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_chip) @@ -291,6 +312,9 @@ class GripperDemo02Viewer(QMainWindow): 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 = QGridLayout() @@ -298,17 +322,13 @@ class GripperDemo02Viewer(QMainWindow): outer.addLayout(main, 1) self.left_flow = ImageBox("左侧光流") - self.left_mag = ImageBox("左侧幅值图") self.right_flow = ImageBox("右侧光流") - self.right_mag = ImageBox("右侧幅值图") main.addWidget(self.left_flow, 0, 0) - main.addWidget(self.left_mag, 0, 1) main.addWidget(self.right_flow, 1, 0) - main.addWidget(self.right_mag, 1, 1) side = QVBoxLayout() side.setSpacing(8) - main.addLayout(side, 0, 2, 2, 1) + main.addLayout(side, 0, 1, 2, 1) metrics_grid = QGridLayout() metrics_grid.setSpacing(8) @@ -356,32 +376,37 @@ class GripperDemo02Viewer(QMainWindow): side.addWidget(self.info_box) main.setColumnStretch(0, 2) - main.setColumnStretch(1, 2) - main.setColumnStretch(2, 3) + main.setColumnStretch(1, 4) main.setRowStretch(0, 1) main.setRowStretch(1, 1) def _configure_plot(self): - self.plot = self.plot_widget.addPlot() - self.plot.setTitle("左右法向力", color="#999999", size="13pt") - self.plot.titleLabel.setFont(QFont("SimHei", 13)) - self.plot.showAxis("top", False) - self.plot.showAxis("right", False) - self.plot.showGrid(x=False, y=True, alpha=0.12) - self.plot.setLabel("bottom", "时间", units="s") - self.plot.setLabel("left", "力", units="N") - self.plot.setYRange(PLOT_FORCE_Y_MIN_N, PLOT_FORCE_Y_MAX_N, padding=0) - self.plot.setLimits(yMin=PLOT_FORCE_Y_MIN_N, yMax=PLOT_FORCE_Y_MAX_N) - self.plot.enableAutoRange(axis=pg.ViewBox.YAxis, enable=False) - self.plot.addLegend(offset=(-12, 12), labelTextColor="#d0d0d0") - self.left_curve = self.plot.plot( - pen=pg.mkPen(CURVE_COLOR, width=2), - name="左法向 (N)", - ) - self.right_curve = self.plot.plot( - pen=pg.mkPen("#ffb000", width=2), - name="右法向 (N)", - ) + 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.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 = { @@ -407,6 +432,23 @@ class GripperDemo02Viewer(QMainWindow): 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(): + self.info_label.setText("请先启动控制器") + return + try: + self.controller.request_manual_command(command) + except Exception as exc: + self.info_label.setText(f"手动命令失败：{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(): @@ -482,6 +524,9 @@ class GripperDemo02Viewer(QMainWindow): shear_unit = status.get("shear_unit", "N") self.status_chip.setText(f"state={state}") self.action_chip.setText(f"action={action}") + self.manual_hold_button.setText( + "释放" if bool(status.get("manual_hold_active")) else "保持" + ) self.left_force.set_value( fmt(status.get("left_normal"), unit), @@ -493,11 +538,11 @@ class GripperDemo02Viewer(QMainWindow): ) self.left_shear_box.set_value( fmt(status.get("left_shear"), shear_unit), - "left_shear", + f"x={fmt(status.get('left_shear_x'), shear_unit)} y={fmt(status.get('left_shear_y'), shear_unit)}", ) self.right_shear_box.set_value( fmt(status.get("right_shear"), shear_unit), - "right_shear", + f"x={fmt(status.get('right_shear_x'), shear_unit)} y={fmt(status.get('right_shear_y'), shear_unit)}", ) self.force_pct_box.set_value( str(status.get("force_pct", "--")), @@ -527,9 +572,7 @@ class GripperDemo02Viewer(QMainWindow): 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.left_mag.set_image(left_sample.get("magnitude_view")) self.right_flow.set_image(right_sample.get("flow_view")) - self.right_mag.set_image(right_sample.get("magnitude_view")) elapsed = time.time() - self.start_time self.force_history.append( @@ -537,6 +580,10 @@ class GripperDemo02Viewer(QMainWindow): 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: @@ -562,11 +609,21 @@ class GripperDemo02Viewer(QMainWindow): return data = np.asarray(self.force_history, dtype=float) x = data[:, 0] - self.left_curve.setData(x, data[:, 1]) - self.right_curve.setData(x, data[:, 2]) - self.plot.setYRange(PLOT_FORCE_Y_MIN_N, PLOT_FORCE_Y_MAX_N, padding=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: - self.plot.setXRange(max(0.0, x[-1] - 6.0), max(6.0, x[-1]), padding=0) + x_min = max(0.0, x[-1] - 6.0) + x_max = max(6.0, x[-1]) + for plot in self.plots: + plot.setXRange(x_min, x_max, padding=0) def _update_thread_buttons(self): if not self.controller_finished: