class gui():
def __init__(self):
self.sys = System()
self.VERBOSITY = 2
# Set the visual paramiters.
self.scale = 600 # scaling factor between simulated system and pixels
boarder = 0.05 # Width of the canvas edge border as a portion of the
# canvas dimensions
self.width = self.sys.el * (1 + boarder) # width of canvas.
self.height = self.sys.el * (1 + boarder) # height of canvas.
self.initial_state = [
-0.1, self.height * 0.8, -np.pi / 4, np.pi, 0, 0, 0, 0
]
self.state = self.initial_state
self.dt = 0.005 # how long to jump the simulation each time step.
# Transform to move from simulation coordinates to gui coordinates.
self.g_cw = self.sys.build_G(
self.sys.build_R(-np.pi),
[self.width / 2, self.height - self.height * boarder * 0.5, 0])
# Vars relating to the closed loop control
self.forces = [0, 0]
self.control_enable = IntVar()
self.inner = PID(0, 0, 0, 0) # These are not safe defaults, change
self.outer = PID(0, 0, 0, 0) # during system bringup.
self.outer2 = PID(0, 0, 0, 0) # during system bringup.
self.outer.output_limits = (-np.pi / 8, np.pi / 8)
self.wind_scale = -0.1
self.plate_goal = 0
self.velocity_goal = 0
self.control_defaults = [1500, 120, -.25, -.04, 0.5]
root = self.create_gui()
stop = threading.Event()
draw = threading.Thread(target=self.animate_canvas, args=(stop, ))
simulate = threading.Thread(target=self.simulate_system, args=(stop, ))
self.run = False # Should the simulation continue
try:
draw.start()
simulate.start()
root.mainloop()
finally:
stop.set()
def compute_control_forces(self, running):
"""! Compute the wind and control forces.
When the controllers are enabled and the system is running the
controllers are updated each timestep in order from outermost to
innermost. When those conditions are not met the controllers are
paused.
@param running, Set to True if the simulation is running.
"""
if running and self.control_enable.get() == 1:
self.log(("velocity goal: {}".format(self.velocity_goal)), 4)
# Handle velocity loop
self.auto_mode = True
try:
self.outer2.Kp = float(self.Kp_o2.get())
except:
self.log("Invalid Input", 2)
self.sample_time = self.dt
self.velocity_goal = self.outer2(self.state[0])
# Handle velocity loop
self.auto_mode = True
self.outer.setpoint = self.velocity_goal
try:
self.outer.Kp = float(self.Kp_o.get())
self.outer.Ki = float(self.Ki_o.get())
except:
self.log("Invalid Input", 2)
self.sample_time = self.dt
self.plate_goal = self.outer(self.state[4])
# Handle inner loop
self.auto_mode = True
self.inner.setpoint = self.plate_goal
try:
self.inner.Kp = float(self.Kp_i.get())
self.inner.Kd = float(self.Kd_i.get())
except:
self.log("Invalid Input", 2)
self.sample_time = self.dt
self.forces[1] = self.inner(self.state[3])
else:
self.auto_mode = False
def simulate_system(self, kill_thread):
"""! The main simulation function/thread.
This function executes the numerical simulation of the system. When the
system is not paused the next state will be computed by numerically
integrating the current system state or by computing a collision update
if appropriate.
If the system is paused the system state will be set to the GUI values.
The state derivatives will not be altered.
This function will throttle if the simulation time begins to run faster
then the real-life time.
"""
t = self.dt
traj = [self.state
] # Trajectory of the system, could saved and graphed
state_old = self.state # Used in computing the impact equation
while (not kill_thread.is_set()):
# Used for throttling
end = time.time() + self.dt
if (self.run):
self.compute_control_forces(True)
# Check Collisions or update sim
col = self.sys.check_collisions(self.state, self.dt)
if col >= 0:
self.log("COLLISION #{}".format(col), 2)
s = self.sys.collision_update(state_old, col)
else:
s = self.sys.integrate(self.state, self.dt, self.forces)
#Record new states
t += self.dt
state_old = self.state
self.state = s
traj.append(s)
self.set_state_gui(s)
self.log(s, 3)
else:
try:
self.state[0:4] = self.get_state_gui()
except:
self.log("invalid input, 2")
self.compute_control_forces(False)
# If we have time left in the loop, sleep till next draw needed.
if end - time.time() - .0001 > 0:
time.sleep(end - time.time())
def get_state_gui(self):
"""! Return the values of the GUI state entrys
@returns an array corresponding to the system state entered/displayed
in the GUI.
"""
s = [
np.float64(self.s0.get()),
np.float64(self.s1.get()),
np.float64(self.s2.get()),
np.float64(self.s3.get())
]
return (s)
def set_state_gui(self, s):
""" Set the state entrys on the GUI
@param s, new state vector to push to the display.
"""
self.s0.set(s[0])
self.s1.set(s[1])
self.s2.set(s[2])
self.s3.set(s[3])
def pause(self):
"""! Pause the simulation
"""
self.run = False
def start(self):
"""! Unpause the simulation.
"""
self.run = True
def reset(self):
"""! Set the system state to it's default.
"""
self.state = self.initial_state
self.set_state_gui(self.state)
self.run = False
def animate_canvas(self, kill_thread):
"""! Thread for drawing the canvas based on the system state.
This function is meant to be called in a seperate thread after the
system and gui have both been set up, but before the tkinter mainloop
has been called.
It will re-draw the canvas to reflect the system state vector at 60hz
or as close to that as the computer's speed allows. This is the thread
that is responsible for actually showing the system's motion on screen.
"""
delay = 1.0 / 60
while (not kill_thread.is_set()):
end = time.time() + delay
ball_new, plate_new, mark_new = self.update_canvas(self.state)
self.canvas.delete(self.ball)
self.canvas.delete(self.plate)
self.canvas.delete(self.mark)
self.ball = ball_new
self.plate = plate_new
self.mark = mark_new
# If we have time left in the loop, sleep till next draw needed.
if end - time.time() - .0001 > 0:
time.sleep(end - time.time())
def update_canvas(self, s):
"""! Create new marker, ball, and plate objects for the new state.
@param s, the new system state for which to create objects.
@returns the ball, plate, and marker objects.
"""
scale = self.scale # Honestly this is just so the text fits
subs = {
self.sys.q[0]: s[0],
self.sys.q[1]: s[1],
self.sys.q[2]: s[2],
self.sys.q[3]: s[3]
}
vec_z = sym.Matrix([0, 0, 0, 1])
# draw the ball.
b0 = (self.g_cw * self.sys.g_wb * self.sys.g_bb0 *
vec_z).subs(subs) * scale
b1 = (self.g_cw * self.sys.g_wb * self.sys.g_bb1 *
vec_z).subs(subs) * scale
b2 = (self.g_cw * self.sys.g_wb * self.sys.g_bb2 *
vec_z).subs(subs) * scale
b3 = (self.g_cw * self.sys.g_wb * self.sys.g_bb3 *
vec_z).subs(subs) * scale
ball_new = self.canvas.create_polygon(b0[0], b0[1], b1[0], b1[1],
b2[0], b2[1], b3[0], b3[1])
self.log("Ball Drawn", 4)
# draw the plate.
p0 = (self.g_cw * self.sys.g_wp * self.sys.g_pp0 *
vec_z).subs(subs) * scale
p1 = (self.g_cw * self.sys.g_wp * self.sys.g_pp1 *
vec_z).subs(subs) * scale
p2 = (self.g_cw * self.sys.g_wp * self.sys.g_pp2 *
vec_z).subs(subs) * scale
p3 = (self.g_cw * self.sys.g_wp * self.sys.g_pp3 *
vec_z).subs(subs) * scale
plate_new = self.canvas.create_polygon(p0[0], p0[1], p1[0], p1[1],
p2[0], p2[1], p3[0], p3[1])
# Draw the marker.
color = 'red'
mark_new = self.canvas.create_line(b0[0],
b0[1],
b3[0],
b3[1],
fill=color)
self.forces[0] = np.float64(self.wind.get()) * self.wind_scale
return (ball_new, plate_new, mark_new)
def draw_canvas(self, s):
"""! This creates a new canvas for the display.
This function should be called once at program start to create the
canvas. Thereafter use update_canvas should be used instead since it
will be much faster.
Pre-condition: Tkinter interface must be set up.
self.sys must be defined.
@param s initial system state vector to display.
@returns A tuple containing the ball, marker, and plate objects.
"""
# Remove old canvas.
self.canvas.delete("all")
scale = self.scale
subs = {
self.sys.q[0]: s[0],
self.sys.q[1]: s[1],
self.sys.q[2]: s[2],
self.sys.q[3]: s[3]
}
vec_z = sym.Matrix([0, 0, 0, 1])
# draw the boarder.
e0 = self.g_cw * self.sys.g_we * self.sys.g_ee0 * vec_z.subs(
subs) * scale
e1 = self.g_cw * self.sys.g_we * self.sys.g_ee1 * vec_z.subs(
subs) * scale
e2 = self.g_cw * self.sys.g_we * self.sys.g_ee2 * vec_z.subs(
subs) * scale
e3 = self.g_cw * self.sys.g_we * self.sys.g_ee3 * vec_z.subs(
subs) * scale
self.canvas.create_line(e0[0],
e0[1],
e1[0],
e1[1],
e2[0],
e2[1],
e3[0],
e3[1],
e0[0],
e0[0],
width=5)
self.log("Boarder Drawn", 4)
ball, plate, mark = self.update_canvas(s)
return (ball, plate, mark)
def create_gui(self):
"""! Creates the GUI objects and assemble them.
This is the obligatory Tkinter blob-function. It sets up all of the
Tkinter widgets and their backing variables.
"""
interface = Frame(root)
display = Frame(root)
# Build Interface
buttons = Frame(interface)
start = Button(buttons, text="start", command=self.start)
stop = Button(buttons, text="pause", command=self.pause)
reset = Button(buttons, text="Reset", command=self.reset)
self.wind = Scale(buttons, from_=-100, to=100, orient=HORIZONTAL)
start.grid(row=0, column=0, padx=5)
stop.grid(row=0, column=1, padx=5)
reset.grid(row=0, column=2, padx=5)
self.wind.grid(row=0, column=3, padx=5)
state = Frame(interface)
self.s0 = StringVar(state)
self.s1 = StringVar(state)
self.s2 = StringVar(state)
self.s3 = StringVar(state)
label0 = Label(state, text="Ball X")
label1 = Label(state, text="Ball Y")
label2 = Label(state, text="Ball Angle")
label3 = Label(state, text="Plate Angle")
s0 = Entry(state, width=10, textvariable=self.s0)
s1 = Entry(state, width=10, textvariable=self.s1)
s2 = Entry(state, width=10, textvariable=self.s2)
s3 = Entry(state, width=10, textvariable=self.s3)
label0.grid(column=0, row=0)
label1.grid(column=1, row=0)
label2.grid(column=2, row=0)
label3.grid(column=3, row=0)
s0.grid(column=0, row=1)
s1.grid(column=1, row=1)
s2.grid(column=2, row=1)
s3.grid(column=3, row=1)
self.set_state_gui(self.state)
# Build Controller Interface
control = Frame(interface)
self.Kp_i = StringVar(control, value=self.control_defaults[0])
self.Kd_i = StringVar(control, value=self.control_defaults[1])
self.Kp_o = StringVar(control, value=self.control_defaults[2])
self.Ki_o = StringVar(control, value=self.control_defaults[3])
self.Kp_o2 = StringVar(control, value=self.control_defaults[4])
label_kpi = Label(control, text="KP - Plate")
label_kdi = Label(control, text="KD - Plate")
label_kpo = Label(control, text="KP - Velocity")
label_kio = Label(control, text="Ki - Velocity")
label_kpo2 = Label(control, text="Kp - Position")
enable = Checkbutton(control,
text="Enable",
variable=self.control_enable)
kpi = Entry(control, width=15, textvariable=self.Kp_i)
kdi = Entry(control, width=15, textvariable=self.Kd_i)
kpo = Entry(control, width=15, textvariable=self.Kp_o)
kio = Entry(control, width=15, textvariable=self.Ki_o)
kpo2 = Entry(control, width=15, textvariable=self.Kp_o2)
label_kpi.grid(column=1, row=0)
label_kdi.grid(column=2, row=0)
label_kpo.grid(column=3, row=0)
label_kio.grid(column=4, row=0)
label_kpo2.grid(column=5, row=0)
enable.grid(column=0, row=1, rowspan=2)
kpi.grid(column=1, row=1)
kdi.grid(column=2, row=1)
kpo.grid(column=3, row=1)
kio.grid(column=4, row=1)
kpo2.grid(column=5, row=1)
state.pack()
control.pack()
buttons.pack()
self.canvas = Canvas(display,
height=self.height * self.scale,
width=self.width * self.scale)
self.canvas.pack()
interface.grid(column=1, row=1)
display.grid(column=1, row=0)
self.ball, self.plate, self.mark = self.draw_canvas(self.state)
return (root)
def log(self, msg, priority=3):
"""! Verbosity aware way to print messages to the terminal.
Messages that are produced each simulation cycle should be rated 3 or
above.
@param msg, the string to be printed.
@param priority, how important the message is. Recommended scale is
ifrom 0 to 3.
"""
if (self.VERBOSITY >= priority):
print(msg)
