128 lines
4.6 KiB
Python
128 lines
4.6 KiB
Python
"""GUI control panel for the wheeled-legged robot."""
|
|
|
|
import tkinter as tk
|
|
from tkinter import ttk
|
|
|
|
|
|
class GUI:
|
|
"""Tkinter control panel: sliders + gait buttons + status display."""
|
|
|
|
def __init__(self, controller):
|
|
self.ctrl = controller
|
|
self.root = tk.Tk()
|
|
self.root.title("WheelLeg Control")
|
|
self.root.geometry("400x500")
|
|
self.root.protocol("WM_DELETE_WINDOW", self._on_close)
|
|
self._closed = False
|
|
|
|
self._build()
|
|
|
|
def _build(self):
|
|
# Mode buttons
|
|
mf = ttk.LabelFrame(self.root, text="Mode")
|
|
mf.pack(fill="x", padx=8, pady=4)
|
|
for mode in ("wheel", "trot", "mpc"):
|
|
ttk.Button(mf, text=mode.upper(),
|
|
command=lambda m=mode: self._set_mode(m)
|
|
).pack(side="left", padx=4, expand=True)
|
|
ttk.Button(mf, text="PRONE/STAND",
|
|
command=self._toggle_prone).pack(side="left", padx=4, expand=True)
|
|
|
|
# Command sliders
|
|
cf = ttk.LabelFrame(self.root, text="Commands")
|
|
cf.pack(fill="x", padx=8, pady=4)
|
|
|
|
self.vel_x_var = tk.DoubleVar(value=0.0)
|
|
self.vel_y_var = tk.DoubleVar(value=0.0)
|
|
self.yaw_var = tk.DoubleVar(value=0.0)
|
|
self.height_var = tk.DoubleVar(value=self.ctrl.height)
|
|
|
|
self._slider(cf, "Vel X", self.vel_x_var, -1.5, 1.5)
|
|
self._slider(cf, "Vel Y*", self.vel_y_var, -0.5, 0.5)
|
|
self._slider(cf, "Yaw", self.yaw_var, -2.0, 2.0)
|
|
self._slider(cf, "Height", self.height_var, 0.16, 0.45)
|
|
|
|
ttk.Label(cf, text="* Vel Y: trot mode only (diff-drive can't sidestep)",
|
|
font=("", 8)).pack(anchor="w", padx=8)
|
|
|
|
ttk.Button(cf, text="Reset", command=self._reset).pack(pady=4)
|
|
|
|
# Status display
|
|
sf = ttk.LabelFrame(self.root, text="Status")
|
|
sf.pack(fill="both", expand=True, padx=8, pady=4)
|
|
self.status_text = tk.Text(sf, height=12, width=45, font=("Consolas", 9))
|
|
self.status_text.pack(fill="both", expand=True, padx=4, pady=4)
|
|
|
|
def _slider(self, parent, label, var, lo, hi):
|
|
f = ttk.Frame(parent)
|
|
f.pack(fill="x", padx=4, pady=2)
|
|
ttk.Label(f, text=label, width=7).pack(side="left")
|
|
ttk.Scale(f, from_=lo, to=hi, variable=var,
|
|
command=lambda *_: self._sync()).pack(side="left", fill="x", expand=True)
|
|
lbl = ttk.Label(f, text="0.00", width=6)
|
|
lbl.pack(side="left")
|
|
var.trace_add("write", lambda *_, v=var, l=lbl: l.config(text=f"{v.get():.2f}"))
|
|
|
|
def _set_mode(self, mode):
|
|
self.ctrl.mode = mode
|
|
self.ctrl.prone = False
|
|
|
|
def _toggle_prone(self):
|
|
self.ctrl.prone = not self.ctrl.prone
|
|
|
|
def _sync(self):
|
|
self.ctrl.vel_x = self.vel_x_var.get()
|
|
self.ctrl.vel_y = self.vel_y_var.get()
|
|
self.ctrl.yaw_rate = self.yaw_var.get()
|
|
self.ctrl.height = self.height_var.get()
|
|
|
|
def _reset(self):
|
|
self.vel_x_var.set(0.0)
|
|
self.vel_y_var.set(0.0)
|
|
self.yaw_var.set(0.0)
|
|
self._sync()
|
|
|
|
def _on_close(self):
|
|
self._closed = True
|
|
self.root.destroy()
|
|
|
|
@property
|
|
def closed(self):
|
|
return self._closed
|
|
|
|
def update_status(self, state, step):
|
|
"""Update status text with current robot state."""
|
|
txt = (
|
|
f"Mode: {self.ctrl.mode} Step: {step}\n"
|
|
f"Pos: x={state.pos[0]:.3f} y={state.pos[1]:.3f} z={state.pos[2]:.3f}\n"
|
|
f"RPY: r={np.degrees(state.rpy[0]):.1f}° p={np.degrees(state.rpy[1]):.1f}° "
|
|
f"y={np.degrees(state.rpy[2]):.1f}°\n"
|
|
f"Vel: vx={state.lin_vel[0]:.3f} vy={state.lin_vel[1]:.3f} vz={state.lin_vel[2]:.3f}\n"
|
|
f"Cmd: vx={self.ctrl.vel_x:.2f} yaw={self.ctrl.yaw_rate:.2f} h={self.ctrl.height:.3f}\n"
|
|
f"─────────────────────────────────\n"
|
|
)
|
|
# Joint angles (compact)
|
|
for i, leg in enumerate(("FL", "FR", "RL", "RR")):
|
|
q = state.joint_pos[i*4:i*4+3]
|
|
w = state.joint_vel[i*4+3]
|
|
txt += f"{leg}: [{q[0]:+.2f} {q[1]:+.2f} {q[2]:+.2f}] w={w:+.1f}\n"
|
|
|
|
self.status_text.delete("1.0", tk.END)
|
|
self.status_text.insert(tk.END, txt)
|
|
|
|
def tick(self):
|
|
"""Process GUI events. Returns False if window closed."""
|
|
if self._closed:
|
|
return False
|
|
try:
|
|
self.root.update_idletasks()
|
|
self.root.update()
|
|
return True
|
|
except tk.TclError:
|
|
self._closed = True
|
|
return False
|
|
|
|
|
|
# Need numpy for degrees conversion in update_status
|
|
import numpy as np
|