orisys/test
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

v 1.2

Browse Source
This commit is contained in:
cheng
2026-06-09 14:29:44 +08:00
parent 74d2bc4853
commit 1f1ee6d8fe
16 changed files with 277 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
gripper_control_02/visualizer.py
+92 -35
View File
@@ -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: