784 lines
29 KiB
Python
784 lines
29 KiB
Python
import sys
|
|
import time
|
|
from collections import deque
|
|
from pathlib import Path
|
|
from typing import Deque, Dict, Optional, Tuple
|
|
|
|
import grpc
|
|
import numpy as np
|
|
from PyQt5 import QtCore, QtWidgets
|
|
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
|
|
from matplotlib.backends.backend_qt5agg import NavigationToolbar2QT as NavigationToolbar
|
|
from matplotlib.figure import Figure
|
|
try:
|
|
from yourdfpy import URDF
|
|
except Exception: # pragma: no cover - fallback if yourdfpy not available
|
|
from urdfpy import URDF
|
|
|
|
from clients._path_setup import ensure_paths
|
|
|
|
ensure_paths()
|
|
|
|
from google.protobuf import timestamp_pb2
|
|
|
|
from cmvr.api import humanoid_robot_command_pb2 as pb
|
|
from cmvr.api import humanoid_robot_service_pb2_grpc as rpc
|
|
|
|
DEFAULT_JOINT_ORDER = [
|
|
"L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y",
|
|
"L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R",
|
|
"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
|
|
"R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R",
|
|
"HEAD_P", "HEAD_Y", "HEAD_R", "WAIST_Y", "WAIST_R",
|
|
]
|
|
|
|
URDF_PATH = Path(__file__).resolve().parents[1] / "robot_model" / "xiaoyan_description" / "dual_arm.urdf"
|
|
|
|
|
|
class JointStateWorker(QtCore.QThread):
|
|
updated = QtCore.pyqtSignal(dict)
|
|
log = QtCore.pyqtSignal(str)
|
|
|
|
def __init__(
|
|
self,
|
|
address: str,
|
|
device_id: str,
|
|
interval: float,
|
|
joint_names: list[str],
|
|
timeout: float = 2.0,
|
|
) -> None:
|
|
super().__init__()
|
|
self.address = address
|
|
self.device_id = device_id
|
|
self.interval = interval
|
|
self.joint_names = joint_names
|
|
self.timeout = timeout
|
|
self._stop = False
|
|
self._channel: Optional[grpc.Channel] = None
|
|
|
|
def stop(self) -> None:
|
|
self._stop = True
|
|
|
|
def run(self) -> None:
|
|
try:
|
|
self._channel = grpc.insecure_channel(self.address)
|
|
stub = rpc.HumanoidRobotServiceStub(self._channel)
|
|
except Exception as exc:
|
|
self.log.emit(f"[error] connect failed: {exc}")
|
|
return
|
|
|
|
while not self._stop:
|
|
req = pb.JointRequest()
|
|
req.header.device_id = self.device_id
|
|
ts = timestamp_pb2.Timestamp()
|
|
ts.GetCurrentTime()
|
|
req.header.timestamp.CopyFrom(ts)
|
|
try:
|
|
resp = stub.getJointState(req, timeout=10)
|
|
except Exception as exc:
|
|
self.log.emit(f"[error] getJointState failed: {exc}")
|
|
time.sleep(self.interval)
|
|
continue
|
|
|
|
joint_pos: Dict[str, float] = {}
|
|
joint_vel: Dict[str, float] = {}
|
|
for state in resp.state:
|
|
for name, pos, vel in zip(state.name, state.position, state.velocity):
|
|
joint_pos[name] = pos
|
|
joint_vel[name] = vel
|
|
|
|
payload: Dict[str, Tuple[float, float]] = {}
|
|
for name in self.joint_names:
|
|
payload[name] = (joint_pos.get(name, 0.0), joint_vel.get(name, 0.0))
|
|
|
|
self.updated.emit(payload)
|
|
time.sleep(self.interval)
|
|
if self._channel:
|
|
self._channel.close()
|
|
|
|
|
|
class PoseWorker(QtCore.QThread):
|
|
updated = QtCore.pyqtSignal(dict)
|
|
log = QtCore.pyqtSignal(str)
|
|
|
|
def __init__(
|
|
self,
|
|
address: str,
|
|
device_id: str,
|
|
interval: float,
|
|
base_link: str,
|
|
ee_link: str,
|
|
timeout: float = 2.0,
|
|
) -> None:
|
|
super().__init__()
|
|
self.address = address
|
|
self.device_id = device_id
|
|
self.interval = interval
|
|
self.base_link = base_link
|
|
self.ee_link = ee_link
|
|
self.timeout = timeout
|
|
self._stop = False
|
|
self._channel: Optional[grpc.Channel] = None
|
|
|
|
def stop(self) -> None:
|
|
self._stop = True
|
|
|
|
def run(self) -> None:
|
|
try:
|
|
self._channel = grpc.insecure_channel(self.address)
|
|
stub = rpc.HumanoidRobotServiceStub(self._channel)
|
|
except Exception as exc:
|
|
self.log.emit(f"[error] connect failed: {exc}")
|
|
return
|
|
|
|
while not self._stop:
|
|
req = pb.GetPose.Request()
|
|
req.header.device_id = self.device_id
|
|
req.base_link = self.base_link
|
|
req.ee_link = self.ee_link
|
|
ts = timestamp_pb2.Timestamp()
|
|
ts.GetCurrentTime()
|
|
req.header.timestamp.CopyFrom(ts)
|
|
try:
|
|
resp = stub.getPose(req, timeout=10)
|
|
except Exception as exc:
|
|
self.log.emit(f"[error] getPose failed: {exc}")
|
|
time.sleep(self.interval)
|
|
continue
|
|
|
|
pose = resp.pose
|
|
payload = {
|
|
"x": pose.x,
|
|
"y": pose.y,
|
|
"z": pose.z,
|
|
"rx": pose.rx,
|
|
"ry": pose.ry,
|
|
"rz": pose.rz,
|
|
}
|
|
self.updated.emit(payload)
|
|
time.sleep(self.interval)
|
|
if self._channel:
|
|
self._channel.close()
|
|
|
|
|
|
class MainWindow(QtWidgets.QMainWindow):
|
|
def __init__(self) -> None:
|
|
super().__init__()
|
|
self.setWindowTitle("gRPC Client UI")
|
|
self.resize(900, 600)
|
|
|
|
self._pose_process: Optional[QtCore.QProcess] = None
|
|
self._joint_worker: Optional[JointStateWorker] = None
|
|
self._pose_worker: Optional[PoseWorker] = None
|
|
self._plot_selected: set[str] = set()
|
|
self._last_data: Dict[str, Tuple[float, float]] = {}
|
|
self._updating_movej_table = False
|
|
self.urdf: Optional[URDF] = None
|
|
self.urdf_error: Optional[str] = None
|
|
self.joint_names: list[str] = DEFAULT_JOINT_ORDER.copy()
|
|
self.link_names: list[str] = []
|
|
self._warned_no_joint_match = False
|
|
|
|
self._load_urdf()
|
|
|
|
central = self._build_center_panel()
|
|
self.setCentralWidget(central)
|
|
self._build_docks()
|
|
self.statusBar().showMessage("Ready")
|
|
|
|
def _load_urdf(self) -> None:
|
|
try:
|
|
self.urdf = URDF.load(str(URDF_PATH))
|
|
self.link_names = [link.name for link in self._urdf_links()]
|
|
self.joint_names = [
|
|
joint.name for joint in self._urdf_joints() if getattr(joint, "joint_type", "") != "fixed"
|
|
]
|
|
except Exception as exc:
|
|
self.urdf_error = str(exc)
|
|
self.urdf = None
|
|
self.link_names = []
|
|
self.joint_names = DEFAULT_JOINT_ORDER.copy()
|
|
|
|
def _urdf_links(self) -> list:
|
|
if not self.urdf:
|
|
return []
|
|
if hasattr(self.urdf, "links"):
|
|
return list(self.urdf.links)
|
|
if hasattr(self.urdf, "link_map"):
|
|
return list(self.urdf.link_map.values())
|
|
return []
|
|
|
|
def _urdf_joints(self) -> list:
|
|
if not self.urdf:
|
|
return []
|
|
if hasattr(self.urdf, "joints"):
|
|
return list(self.urdf.joints)
|
|
if hasattr(self.urdf, "joint_map"):
|
|
return list(self.urdf.joint_map.values())
|
|
return []
|
|
|
|
def _build_center_panel(self) -> QtWidgets.QWidget:
|
|
panel = QtWidgets.QWidget()
|
|
layout = QtWidgets.QVBoxLayout(panel)
|
|
layout.setContentsMargins(0, 0, 0, 0)
|
|
layout.setSpacing(0)
|
|
self.center_tabs = QtWidgets.QTabWidget()
|
|
self.center_tabs.addTab(self._build_live_panel(), "Plot")
|
|
self.center_tabs.setCurrentIndex(0)
|
|
self.center_tabs.currentChanged.connect(self._on_center_tab_changed)
|
|
layout.addWidget(self.center_tabs)
|
|
return panel
|
|
|
|
def _build_docks(self) -> None:
|
|
control = QtWidgets.QDockWidget("Control")
|
|
control.setWidget(self._build_control_panel())
|
|
control.setMinimumWidth(550)
|
|
control.setFeatures(QtWidgets.QDockWidget.AllDockWidgetFeatures)
|
|
self.addDockWidget(QtCore.Qt.LeftDockWidgetArea, control)
|
|
|
|
status = QtWidgets.QDockWidget("Joint Status")
|
|
status.setWidget(self._build_status_panel())
|
|
status.setMinimumWidth(650)
|
|
status.setFeatures(QtWidgets.QDockWidget.AllDockWidgetFeatures)
|
|
self.addDockWidget(QtCore.Qt.RightDockWidgetArea, status)
|
|
|
|
log_dock = QtWidgets.QDockWidget("Log")
|
|
log_dock.setWidget(self._build_log_panel())
|
|
log_dock.setMinimumHeight(140)
|
|
log_dock.setMaximumHeight(220)
|
|
log_dock.setFeatures(QtWidgets.QDockWidget.AllDockWidgetFeatures)
|
|
self.addDockWidget(QtCore.Qt.BottomDockWidgetArea, log_dock)
|
|
|
|
def _build_status_panel(self) -> QtWidgets.QWidget:
|
|
panel = QtWidgets.QWidget()
|
|
layout = QtWidgets.QVBoxLayout(panel)
|
|
splitter = QtWidgets.QSplitter()
|
|
splitter.setOrientation(QtCore.Qt.Vertical)
|
|
splitter.addWidget(self._build_table())
|
|
splitter.addWidget(self._build_pose_panel())
|
|
splitter.setStretchFactor(0, 3)
|
|
splitter.setStretchFactor(1, 1)
|
|
layout.addWidget(splitter)
|
|
return panel
|
|
|
|
def _build_control_panel(self) -> QtWidgets.QWidget:
|
|
panel = QtWidgets.QWidget()
|
|
layout = QtWidgets.QVBoxLayout(panel)
|
|
layout.addWidget(self._build_settings_group())
|
|
layout.addWidget(self._build_movej_group())
|
|
layout.addWidget(self._build_actions_group())
|
|
layout.addStretch(1)
|
|
return panel
|
|
|
|
def _build_settings_group(self) -> QtWidgets.QGroupBox:
|
|
group = QtWidgets.QGroupBox("Settings")
|
|
layout = QtWidgets.QVBoxLayout(group)
|
|
layout.addLayout(self._build_form())
|
|
return group
|
|
|
|
def _build_movej_group(self) -> QtWidgets.QGroupBox:
|
|
group = QtWidgets.QGroupBox("MoveJ")
|
|
layout = QtWidgets.QVBoxLayout(group)
|
|
params = QtWidgets.QFormLayout()
|
|
params.addRow("MoveJ Vel", self.vel_edit)
|
|
params.addRow("MoveJ Acc", self.acc_edit)
|
|
layout.addLayout(params)
|
|
layout.addWidget(self._build_movej_table())
|
|
self.movej_btn = QtWidgets.QPushButton("Send MoveJ")
|
|
self.movej_btn.clicked.connect(self._on_movej)
|
|
layout.addWidget(self.movej_btn)
|
|
return group
|
|
|
|
def _build_actions_group(self) -> QtWidgets.QGroupBox:
|
|
group = QtWidgets.QGroupBox("Actions")
|
|
layout = QtWidgets.QVBoxLayout(group)
|
|
self.jointstate_toggle = self._build_switch_row("JointState")
|
|
self.torque_on_btn = QtWidgets.QPushButton("Torque On")
|
|
self.torque_off_btn = QtWidgets.QPushButton("Torque Off")
|
|
self.jointstate_toggle.toggled.connect(self._on_jointstate_toggle)
|
|
self.torque_on_btn.clicked.connect(self._on_torque_on)
|
|
self.torque_off_btn.clicked.connect(self._on_torque_off)
|
|
layout.addLayout(self._wrap_switch_row(self.jointstate_toggle, "Start JointState"))
|
|
layout.addWidget(self.torque_on_btn)
|
|
layout.addWidget(self.torque_off_btn)
|
|
return group
|
|
|
|
def _build_switch_row(self, name: str) -> QtWidgets.QCheckBox:
|
|
toggle = QtWidgets.QCheckBox()
|
|
toggle.setChecked(False)
|
|
toggle.setFixedSize(56, 28)
|
|
toggle.setStyleSheet(
|
|
"""
|
|
QCheckBox {
|
|
background-color: #bfc5cf;
|
|
border-radius: 14px;
|
|
padding: 0px;
|
|
}
|
|
QCheckBox:checked {
|
|
background-color: #2f6fed;
|
|
}
|
|
QCheckBox::indicator {
|
|
width: 22px;
|
|
height: 22px;
|
|
border-radius: 11px;
|
|
background: #ffffff;
|
|
margin-left: 3px;
|
|
}
|
|
QCheckBox::indicator:checked {
|
|
margin-left: 31px;
|
|
}
|
|
"""
|
|
)
|
|
toggle.setTristate(False)
|
|
toggle.setObjectName(f"{name}_toggle")
|
|
return toggle
|
|
|
|
def _wrap_switch_row(self, toggle: QtWidgets.QCheckBox, label: str) -> QtWidgets.QHBoxLayout:
|
|
row = QtWidgets.QHBoxLayout()
|
|
row.addWidget(toggle)
|
|
row.addWidget(QtWidgets.QLabel(label))
|
|
row.addStretch(1)
|
|
return row
|
|
def _build_form(self) -> QtWidgets.QLayout:
|
|
form = QtWidgets.QFormLayout()
|
|
|
|
self.host_edit = QtWidgets.QLineEdit("192.168.0.222")
|
|
self.port_edit = QtWidgets.QSpinBox()
|
|
self.port_edit.setRange(1, 65535)
|
|
self.port_edit.setValue(50052)
|
|
self.device_edit = QtWidgets.QLineEdit("hc01")
|
|
self.interval_edit = QtWidgets.QDoubleSpinBox()
|
|
self.interval_edit.setDecimals(3)
|
|
self.interval_edit.setRange(0.001, 10.0)
|
|
self.interval_edit.setValue(0.05)
|
|
self.vel_edit = QtWidgets.QDoubleSpinBox()
|
|
self.vel_edit.setRange(0.1, 10.0)
|
|
self.vel_edit.setValue(1.0)
|
|
self.acc_edit = QtWidgets.QDoubleSpinBox()
|
|
self.acc_edit.setRange(0.1, 10.0)
|
|
self.acc_edit.setValue(0.5)
|
|
|
|
form.addRow("Host", self.host_edit)
|
|
form.addRow("Port", self.port_edit)
|
|
form.addRow("Device ID", self.device_edit)
|
|
form.addRow("Interval (s)", self.interval_edit)
|
|
|
|
return form
|
|
|
|
def _build_log(self) -> QtWidgets.QWidget:
|
|
self.log = QtWidgets.QTextEdit()
|
|
self.log.setReadOnly(True)
|
|
return self.log
|
|
|
|
def _build_log_panel(self) -> QtWidgets.QWidget:
|
|
panel = QtWidgets.QWidget()
|
|
layout = QtWidgets.QVBoxLayout(panel)
|
|
header = QtWidgets.QHBoxLayout()
|
|
header.addWidget(QtWidgets.QLabel("Runtime logs"))
|
|
header.addStretch(1)
|
|
log_widget = self._build_log()
|
|
self.clear_log_btn = QtWidgets.QPushButton("Clear")
|
|
self.clear_log_btn.clicked.connect(log_widget.clear)
|
|
header.addWidget(self.clear_log_btn)
|
|
layout.addLayout(header)
|
|
layout.addWidget(log_widget)
|
|
return panel
|
|
|
|
def _build_pose_panel(self) -> QtWidgets.QGroupBox:
|
|
group = QtWidgets.QGroupBox("GetPose")
|
|
layout = QtWidgets.QVBoxLayout(group)
|
|
|
|
self.getpose_toggle = self._build_switch_row("GetPose")
|
|
self.getpose_toggle.toggled.connect(self._on_getpose_toggle)
|
|
layout.addLayout(self._wrap_switch_row(self.getpose_toggle, "Start GetPose"))
|
|
|
|
form = QtWidgets.QFormLayout()
|
|
self.base_link_edit = QtWidgets.QComboBox()
|
|
self.ee_link_edit = QtWidgets.QComboBox()
|
|
if self.link_names:
|
|
self.base_link_edit.addItems(self.link_names)
|
|
self.ee_link_edit.addItems(self.link_names)
|
|
if "PELVIS_S" in self.link_names:
|
|
self.base_link_edit.setCurrentText("PELVIS_S")
|
|
if "R_FINGER_TIP" in self.link_names:
|
|
self.ee_link_edit.setCurrentText("R_FINGER_TIP")
|
|
else:
|
|
self.base_link_edit.addItem("PELVIS_S")
|
|
self.ee_link_edit.addItem("R_FINGER_TIP")
|
|
form.addRow("Base Link", self.base_link_edit)
|
|
form.addRow("EE Link", self.ee_link_edit)
|
|
layout.addLayout(form)
|
|
|
|
self.pose_labels = {}
|
|
grid = QtWidgets.QGridLayout()
|
|
for i, key in enumerate(["x", "y", "z", "rx", "ry", "rz"]):
|
|
label = QtWidgets.QLabel(f"{key}: 0.000000")
|
|
self.pose_labels[key] = label
|
|
grid.addWidget(label, i // 3, i % 3)
|
|
layout.addLayout(grid)
|
|
return group
|
|
|
|
def _build_live_panel(self) -> QtWidgets.QWidget:
|
|
tab = QtWidgets.QWidget()
|
|
layout = QtWidgets.QVBoxLayout(tab)
|
|
|
|
layout.setContentsMargins(0, 0, 0, 0)
|
|
layout.setSpacing(4)
|
|
|
|
self.figure = Figure(figsize=(5, 4))
|
|
self.canvas = FigureCanvas(self.figure)
|
|
self.ax_pos = self.figure.add_subplot(2, 1, 1)
|
|
self.ax_vel = self.figure.add_subplot(2, 1, 2)
|
|
self.ax_pos.set_title("Position (rad)")
|
|
self.ax_vel.set_title("Velocity (rad/s)")
|
|
self.ax_pos.grid(True, alpha=0.3)
|
|
self.ax_vel.grid(True, alpha=0.3)
|
|
self.pos_lines: Dict[str, object] = {}
|
|
self.vel_lines: Dict[str, object] = {}
|
|
|
|
self.time_hist: Deque[float] = deque(maxlen=300)
|
|
self._last_plot_time: Optional[float] = None
|
|
self.pos_hist: Dict[str, Deque[float]] = {name: deque(maxlen=300) for name in self.joint_names}
|
|
self.vel_hist: Dict[str, Deque[float]] = {name: deque(maxlen=300) for name in self.joint_names}
|
|
self._plot_start = time.time()
|
|
|
|
self.canvas.setSizePolicy(
|
|
QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Expanding
|
|
)
|
|
self.toolbar = NavigationToolbar(self.canvas, self)
|
|
layout.addWidget(self.toolbar)
|
|
layout.addWidget(self.canvas)
|
|
return tab
|
|
|
|
def _build_table(self) -> QtWidgets.QWidget:
|
|
self.table = QtWidgets.QTableWidget(len(self.joint_names), 4)
|
|
self.table.setHorizontalHeaderLabels(
|
|
["Joint", "Position (rad)", "Position (deg)", "Velocity (rad/s)"]
|
|
)
|
|
self.table.verticalHeader().setVisible(False)
|
|
self.table.setEditTriggers(QtWidgets.QAbstractItemView.NoEditTriggers)
|
|
self.table.setSelectionMode(QtWidgets.QAbstractItemView.ExtendedSelection)
|
|
self.table.setSelectionBehavior(QtWidgets.QAbstractItemView.SelectRows)
|
|
for row, name in enumerate(self.joint_names):
|
|
self.table.setItem(row, 0, QtWidgets.QTableWidgetItem(name))
|
|
self.table.setItem(row, 1, QtWidgets.QTableWidgetItem("0.0"))
|
|
self.table.setItem(row, 2, QtWidgets.QTableWidgetItem("0.0"))
|
|
self.table.setItem(row, 3, QtWidgets.QTableWidgetItem("0.0"))
|
|
self.table.horizontalHeader().setStretchLastSection(True)
|
|
self.table.itemSelectionChanged.connect(self._on_status_selection_changed)
|
|
return self.table
|
|
|
|
def _build_movej_table(self) -> QtWidgets.QWidget:
|
|
self.movej_table = QtWidgets.QTableWidget(len(self.joint_names), 4)
|
|
self.movej_table.setHorizontalHeaderLabels(["Use", "Joint", "Deg", "Rad"])
|
|
self.movej_table.verticalHeader().setVisible(False)
|
|
self.movej_table.setSizePolicy(
|
|
QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Expanding
|
|
)
|
|
for row, name in enumerate(self.joint_names):
|
|
use_item = QtWidgets.QTableWidgetItem()
|
|
use_item.setCheckState(QtCore.Qt.Unchecked)
|
|
self.movej_table.setItem(row, 0, use_item)
|
|
self.movej_table.setItem(row, 1, QtWidgets.QTableWidgetItem(name))
|
|
self.movej_table.setItem(row, 2, QtWidgets.QTableWidgetItem("0.0"))
|
|
self.movej_table.setItem(row, 3, QtWidgets.QTableWidgetItem("0.0"))
|
|
self.movej_table.horizontalHeader().setStretchLastSection(True)
|
|
self._fit_table_height(self.movej_table)
|
|
self.movej_table.itemChanged.connect(self._on_movej_angle_changed)
|
|
return self.movej_table
|
|
|
|
def _fit_table_height(self, table: QtWidgets.QTableWidget) -> None:
|
|
header_height = table.horizontalHeader().height()
|
|
row_height = table.verticalHeader().defaultSectionSize()
|
|
total = header_height + row_height * table.rowCount() + 2
|
|
table.setMinimumHeight(total)
|
|
|
|
def _append_log(self, text: str) -> None:
|
|
if hasattr(self, "log") and self.log is not None:
|
|
self.log.append(text.rstrip())
|
|
|
|
def _base_args(self) -> list[str]:
|
|
return [
|
|
"-m",
|
|
"ui.run_action",
|
|
"--host",
|
|
self.host_edit.text().strip(),
|
|
"--port",
|
|
str(self.port_edit.value()),
|
|
"--device-id",
|
|
self.device_edit.text().strip(),
|
|
]
|
|
|
|
def _start_process(self, args: list[str], keep: bool = False) -> QtCore.QProcess:
|
|
proc = QtCore.QProcess(self)
|
|
proc.setProgram(sys.executable)
|
|
proc.setArguments(args)
|
|
proc.setWorkingDirectory(str(Path(__file__).resolve().parents[1]))
|
|
proc.setProcessChannelMode(QtCore.QProcess.MergedChannels)
|
|
proc.readyReadStandardOutput.connect(
|
|
lambda p=proc: self._append_log(p.readAllStandardOutput().data().decode("utf-8", "ignore"))
|
|
)
|
|
proc.finished.connect(
|
|
lambda code, status: self._append_log(f"[exit] code={code} status={int(status)}")
|
|
)
|
|
proc.start()
|
|
if keep:
|
|
return proc
|
|
proc.finished.connect(proc.deleteLater)
|
|
return proc
|
|
|
|
def _on_movej(self) -> None:
|
|
joint_list = self._get_selected_movej_joints()
|
|
|
|
if not joint_list:
|
|
self._append_log("[warn] No joints selected for MoveJ.")
|
|
return
|
|
|
|
args = self._base_args() + [
|
|
"movej",
|
|
"--vel",
|
|
str(self.vel_edit.value()),
|
|
"--acc",
|
|
str(self.acc_edit.value()),
|
|
]
|
|
self._start_process(args + ["--joint-list", self._encode_joint_list(joint_list)])
|
|
|
|
def _on_get_pose(self) -> None:
|
|
if self._joint_worker and self._joint_worker.isRunning():
|
|
self._append_log("[info] JointState already running.")
|
|
return
|
|
address = f"{self.host_edit.text().strip()}:{self.port_edit.value()}"
|
|
self._joint_worker = JointStateWorker(
|
|
address=address,
|
|
device_id=self.device_edit.text().strip(),
|
|
interval=float(self.interval_edit.value()),
|
|
joint_names=self.joint_names,
|
|
)
|
|
self._joint_worker.updated.connect(self._update_joint_table)
|
|
self._joint_worker.log.connect(self._append_log)
|
|
self._joint_worker.start()
|
|
|
|
def _on_stop_pose(self) -> None:
|
|
if self._joint_worker:
|
|
self._append_log("[info] Stopping JointState...")
|
|
self._joint_worker.stop()
|
|
self._joint_worker = None
|
|
|
|
def _on_torque_on(self) -> None:
|
|
args = self._base_args() + ["torque_on"]
|
|
self._start_process(args)
|
|
|
|
def _on_torque_off(self) -> None:
|
|
args = self._base_args() + ["torque_off"]
|
|
self._start_process(args)
|
|
|
|
def _on_jointstate_toggle(self, checked: bool) -> None:
|
|
if checked:
|
|
self._on_get_pose()
|
|
else:
|
|
self._on_stop_pose()
|
|
|
|
def _on_getpose_toggle(self, checked: bool) -> None:
|
|
if checked:
|
|
self._on_start_get_pose()
|
|
else:
|
|
self._on_stop_get_pose()
|
|
|
|
def _update_joint_table(self, data: Dict[str, Tuple[float, float]]) -> None:
|
|
self._last_data = data
|
|
self._render_joint_table_and_plot()
|
|
|
|
def _append_plot(self, name: str, pos: float, vel: float) -> None:
|
|
t = time.time() - self._plot_start
|
|
if self._last_plot_time is None or t > self._last_plot_time:
|
|
self.time_hist.append(t)
|
|
self._last_plot_time = t
|
|
self.pos_hist[name].append(pos)
|
|
self.vel_hist[name].append(vel)
|
|
|
|
def _refresh_plot(self, selected: set) -> None:
|
|
for name in list(self.pos_lines.keys()):
|
|
if name not in selected:
|
|
self.pos_lines[name].remove()
|
|
self.vel_lines[name].remove()
|
|
del self.pos_lines[name]
|
|
del self.vel_lines[name]
|
|
|
|
for name in selected:
|
|
if name not in self.pos_lines:
|
|
(pos_line,) = self.ax_pos.plot([], [], label=name)
|
|
(vel_line,) = self.ax_vel.plot([], [], label=name)
|
|
self.pos_lines[name] = pos_line
|
|
self.vel_lines[name] = vel_line
|
|
x_data = list(self.time_hist)
|
|
y_pos_full = list(self.pos_hist[name])
|
|
y_vel_full = list(self.vel_hist[name])
|
|
n_pos = min(len(x_data), len(y_pos_full))
|
|
n_vel = min(len(x_data), len(y_vel_full))
|
|
if n_pos:
|
|
self.pos_lines[name].set_data(x_data[-n_pos:], y_pos_full[-n_pos:])
|
|
else:
|
|
self.pos_lines[name].set_data([], [])
|
|
if n_vel:
|
|
self.vel_lines[name].set_data(x_data[-n_vel:], y_vel_full[-n_vel:])
|
|
else:
|
|
self.vel_lines[name].set_data([], [])
|
|
|
|
if len(self.time_hist) > 1 and selected:
|
|
self.ax_pos.set_xlim(self.time_hist[0], self.time_hist[-1])
|
|
self.ax_vel.set_xlim(self.time_hist[0], self.time_hist[-1])
|
|
self.ax_pos.relim()
|
|
self.ax_pos.autoscale_view(scalex=False, scaley=True)
|
|
self.ax_vel.relim()
|
|
self.ax_vel.autoscale_view(scalex=False, scaley=True)
|
|
if selected:
|
|
self.ax_pos.legend(loc="upper right", fontsize=8, ncol=2)
|
|
self.ax_vel.legend(loc="upper right", fontsize=8, ncol=2)
|
|
self.canvas.draw_idle()
|
|
|
|
def _reset_plot_data(self) -> None:
|
|
self.time_hist.clear()
|
|
for name in self.joint_names:
|
|
self.pos_hist[name].clear()
|
|
self.vel_hist[name].clear()
|
|
self._plot_start = time.time()
|
|
self._last_plot_time = None
|
|
for line in self.pos_lines.values():
|
|
line.set_data([], [])
|
|
for line in self.vel_lines.values():
|
|
line.set_data([], [])
|
|
self.canvas.draw_idle()
|
|
|
|
def _encode_joint_list(self, joint_list: list[dict]) -> str:
|
|
return ";".join(f"{j['joint_name']}:{j['rad']}" for j in joint_list)
|
|
|
|
def _get_selected_movej_joints(self) -> list[dict]:
|
|
joint_list = []
|
|
for row, name in enumerate(self.joint_names):
|
|
use_item = self.movej_table.item(row, 0)
|
|
if use_item and use_item.checkState() == QtCore.Qt.Checked:
|
|
rad_item = self.movej_table.item(row, 3)
|
|
try:
|
|
rad = float(rad_item.text()) if rad_item else 0.0
|
|
except ValueError:
|
|
rad = 0.0
|
|
joint_list.append({"joint_name": name, "rad": rad})
|
|
return joint_list
|
|
|
|
def _on_movej_angle_changed(self, item: QtWidgets.QTableWidgetItem) -> None:
|
|
if self._updating_movej_table:
|
|
return
|
|
col = item.column()
|
|
if col not in (2, 3):
|
|
return
|
|
row = item.row()
|
|
self._updating_movej_table = True
|
|
try:
|
|
if col == 2:
|
|
try:
|
|
deg = float(item.text())
|
|
except ValueError:
|
|
deg = 0.0
|
|
rad = deg * 3.141592653589793 / 180.0
|
|
rad_item = self.movej_table.item(row, 3)
|
|
if rad_item is None:
|
|
rad_item = QtWidgets.QTableWidgetItem("0.0")
|
|
self.movej_table.setItem(row, 3, rad_item)
|
|
rad_item.setText(f"{rad:.6f}")
|
|
elif col == 3:
|
|
try:
|
|
rad = float(item.text())
|
|
except ValueError:
|
|
rad = 0.0
|
|
deg = rad * 180.0 / 3.141592653589793
|
|
deg_item = self.movej_table.item(row, 2)
|
|
if deg_item is None:
|
|
deg_item = QtWidgets.QTableWidgetItem("0.0")
|
|
self.movej_table.setItem(row, 2, deg_item)
|
|
deg_item.setText(f"{deg:.3f}")
|
|
finally:
|
|
self._updating_movej_table = False
|
|
|
|
def _apply_plot_selection(self) -> None:
|
|
# Deprecated: plot selection now follows right-side Joint Status selection.
|
|
pass
|
|
|
|
def _on_status_selection_changed(self) -> None:
|
|
selected_rows = {idx.row() for idx in self.table.selectionModel().selectedRows()}
|
|
self._plot_selected = {self.joint_names[row] for row in selected_rows}
|
|
self.statusBar().showMessage(f"Selected joints: {len(self._plot_selected)}")
|
|
self._render_joint_table_and_plot()
|
|
|
|
def _on_start_get_pose(self) -> None:
|
|
if self._pose_worker and self._pose_worker.isRunning():
|
|
self._append_log("[info] GetPose already running.")
|
|
return
|
|
address = f"{self.host_edit.text().strip()}:{self.port_edit.value()}"
|
|
self._pose_worker = PoseWorker(
|
|
address=address,
|
|
device_id=self.device_edit.text().strip(),
|
|
interval=float(self.interval_edit.value()),
|
|
base_link=self.base_link_edit.currentText().strip(),
|
|
ee_link=self.ee_link_edit.currentText().strip(),
|
|
)
|
|
self._pose_worker.updated.connect(self._update_pose)
|
|
self._pose_worker.log.connect(self._append_log)
|
|
self._pose_worker.start()
|
|
|
|
def _on_stop_get_pose(self) -> None:
|
|
if self._pose_worker:
|
|
self._append_log("[info] Stopping GetPose...")
|
|
self._pose_worker.stop()
|
|
self._pose_worker = None
|
|
|
|
def _update_pose(self, data: Dict[str, float]) -> None:
|
|
for key, value in data.items():
|
|
if key in self.pose_labels:
|
|
self.pose_labels[key].setText(f"{key}: {value:.6f}")
|
|
return
|
|
|
|
def _render_joint_table_and_plot(self) -> None:
|
|
selected = set(self._plot_selected)
|
|
if not self._last_data:
|
|
self._last_data = {name: (0.0, 0.0) for name in self.joint_names}
|
|
|
|
selected_lines = []
|
|
for row, name in enumerate(self.joint_names):
|
|
pos, vel = self._last_data.get(name, (0.0, 0.0))
|
|
self.table.item(row, 1).setText(f"{pos:.6f}")
|
|
self.table.item(row, 2).setText(f"{pos * 180.0 / 3.141592653589793:.3f}")
|
|
self.table.item(row, 3).setText(f"{vel:.6f}")
|
|
|
|
if name in selected:
|
|
selected_lines.append(f"{name}: {pos:.4f} rad, {vel:.4f} rad/s")
|
|
self._append_plot(name, pos, vel)
|
|
|
|
if selected_lines:
|
|
self.statusBar().showMessage(
|
|
f"Selected joints: {len(selected)} | " + " | ".join(selected_lines)
|
|
)
|
|
else:
|
|
self.statusBar().showMessage(f"Selected joints: {len(selected)}")
|
|
|
|
if self._is_plot_visible():
|
|
self._refresh_plot(selected)
|
|
return
|
|
|
|
def _is_plot_visible(self) -> bool:
|
|
return hasattr(self, "center_tabs") and self.center_tabs.currentIndex() == 0
|
|
|
|
def _on_center_tab_changed(self, index: int) -> None:
|
|
if index == 0:
|
|
self._render_joint_table_and_plot()
|
|
|
|
|
|
def main() -> int:
|
|
app = QtWidgets.QApplication(sys.argv)
|
|
window = MainWindow()
|
|
window.show()
|
|
return app.exec_()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
raise SystemExit(main())
|