-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathTest_GUI_Velocity.py
More file actions
156 lines (129 loc) · 5.92 KB
/
Test_GUI_Velocity.py
File metadata and controls
156 lines (129 loc) · 5.92 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
# Test_GUI_Velocity.py
import tkinter as tk
from tkinter import ttk
from tkinter import messagebox
import matplotlib.pyplot as plt
from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg
from S7RTT import S7RTT, MotionState
class TestGUI_Velocity:
def __init__(self, root):
self.root = root
self.root.title("S7RTT Velocity Planner GUI")
self.root.geometry("1200x900")
self.planner = S7RTT()
self._init_ui()
self._init_plot()
def _init_ui(self):
control_frame = ttk.LabelFrame(self.root, text="Velocity Mode Parameters")
control_frame.pack(side=tk.TOP, fill=tk.X, padx=10, pady=10)
def create_input(parent, label, row, col, default_val):
ttk.Label(parent, text=label).grid(row=row, column=col, padx=5, pady=5, sticky="e")
var = tk.DoubleVar(value=default_val)
entry = ttk.Entry(parent, textvariable=var, width=12)
entry.grid(row=row, column=col+1, padx=5, pady=5)
return var
# Constraints
ttk.Label(control_frame, text="[Constraints]", foreground="blue").grid(row=0, column=0, sticky="w", padx=5)
self.v_max = create_input(control_frame, "Max Vel:", 0, 1, 10.0)
self.a_max = create_input(control_frame, "Max Acc:", 0, 3, 10.0)
self.j_max = create_input(control_frame, "Max Jerk:", 0, 5, 10.0)
# Start State
ttk.Label(control_frame, text="[Start State]", foreground="green").grid(row=1, column=0, sticky="w", padx=5)
self.p_start = create_input(control_frame, "Start Pos:", 1, 1, 0.0)
self.v_start = create_input(control_frame, "Start Vel:", 1, 3, 20.0)
self.a_start = create_input(control_frame, "Start Acc:", 1, 5, 0.0)
# Target State (Velocity Only)
ttk.Label(control_frame, text="[Target State]", foreground="red").grid(row=2, column=0, sticky="w", padx=5)
# Note: Position target is removed for Velocity Mode
self.v_target = create_input(control_frame, "Target Vel:", 2, 1, 10.0)
# Buttons
btn_frame = ttk.Frame(control_frame)
btn_frame.grid(row=0, column=7, rowspan=3, padx=20, sticky="ns")
ttk.Button(btn_frame, text="GENERATE (Vel)", command=self._on_calculate).pack(fill=tk.X, pady=5)
ttk.Button(btn_frame, text="RESET", command=self._on_reset).pack(fill=tk.X, pady=5)
# Status Label
self.lbl_stats = ttk.Label(control_frame, text="Ready", font=("Arial", 10, "bold"))
self.lbl_stats.grid(row=3, column=0, columnspan=8, pady=5)
def _init_plot(self):
self.fig, self.axs = plt.subplots(4, 1, figsize=(10, 8), sharex=True)
self.fig.subplots_adjust(hspace=0.15, left=0.08, right=0.95, top=0.95, bottom=0.05)
self.titles = ['Position', 'Velocity', 'Acceleration', 'Jerk']
self.colors = ['#1f77b4', '#ff7f0e', '#2ca02c', '#d62728']
self.lines = []
for i, ax in enumerate(self.axs):
ax.set_ylabel(self.titles[i])
ax.grid(True, linestyle=':', alpha=0.6)
line, = ax.plot([], [], color=self.colors[i], lw=2)
self.lines.append(line)
self.canvas = FigureCanvasTkAgg(self.fig, master=self.root)
self.canvas.draw()
self.canvas.get_tk_widget().pack(fill=tk.BOTH, expand=True)
def _generate_plot_data(self, trajectory, sampling_dt=0.01):
"""
Uses at_time to sample the trajectory for plotting.
"""
if not trajectory:
return [], [], [], [], []
total_duration = sum(node.dt for node in trajectory)
plot_duration = total_duration + 1e-5
times = []
pos = []
vel = []
acc = []
jerk = []
t_curr = 0.0
while t_curr <= plot_duration:
state = self.planner.at_time(trajectory, t_curr)
times.append(t_curr)
pos.append(state.p)
vel.append(state.v)
acc.append(state.a)
jerk.append(state.j)
t_curr += sampling_dt
return times, pos, vel, acc, jerk
def _on_calculate(self):
try:
start = MotionState(
dt=0.0,
p=self.p_start.get(),
v=self.v_start.get(),
a=self.a_start.get(),
j=0.0
)
tgt_v = self.v_target.get()
# === CHANGED: Call plan_velocity instead of plan ===
trajectory = self.planner.plan_velocity(
start,
tgt_v,
self.v_max.get(),
self.a_max.get(),
self.j_max.get()
)
ts, ps, vs, as_, js = self._generate_plot_data(trajectory)
data = [ps, vs, as_, js]
for i, line in enumerate(self.lines):
line.set_data(ts, data[i])
self.axs[i].relim()
self.axs[i].autoscale_view()
self.canvas.draw()
if trajectory:
real_total_time = sum(n.dt for n in trajectory)
final_state = self.planner.at_time(trajectory, real_total_time)
self.lbl_stats.config(
text=f"Time: {real_total_time:.3f}s | End V: {final_state.v:.3f} (Target: {tgt_v}) | End Acc: {final_state.a:.3f}"
)
else:
self.lbl_stats.config(text="No movement required or invalid params.")
except Exception as e:
import traceback
traceback.print_exc()
messagebox.showerror("Error", str(e))
def _on_reset(self):
for line in self.lines: line.set_data([], [])
for ax in self.axs: ax.relim(); ax.autoscale_view()
self.canvas.draw()
self.lbl_stats.config(text="Reset.")
if __name__ == "__main__":
root = tk.Tk()
app = TestGUI_Velocity(root)
root.mainloop()