def initialize_simulation(self):
self.pendulum1 = pendulum(self.ui.m1_SpinBox.value(),
self.ui.l1_SpinBox.value(),
math.radians(self.ui.theta1_SpinBox.value()),
math.radians(self.ui.vel1_SpinBox.value()))
self.pendulum2 = pendulum(self.ui.m2_SpinBox.value(),
self.ui.l2_SpinBox.value(),
math.radians(self.ui.theta2_SpinBox.value()),
math.radians(self.ui.vel2_SpinBox.value()))
self.double_pendulum = double_pendulum(self.pendulum1, self.pendulum2,
self.ui.g_SpinBox.value())
self.ui_pendulum.draw_pendulum(self.double_pendulum)
def test_pendulum():
values = [4, 6, 8, 7, 5]
assert pendulum(values) == [8, 6, 4, 5, 7]
values = [19, 30, 16, 19, 28, 26, 28, 17, 21, 17]
assert pendulum(values) == [28, 26, 19, 17, 16, 17, 19, 21, 28, 30]
values = [-9, -2, -10, -6]
assert pendulum(values) == [-6, -10, -9, -2]
values = [-19, -9, -5, -6, -15, -16, -5, -12]
assert pendulum(values) == [-5, -9, -15, -19, -16, -12, -6, -5]
values = [11, -16, -18, 13, -11, -12, 3, 18]
assert pendulum(values) == [13, 3, -12, -18, -16, -11, 11, 18]
def pr1d(file_prefix):
drive_freq = 7.4246
pfunc2 = pendulum.pendulum(0.1, 0.1, 0.25, ampl=1, freq=drive_freq)
ts3, xs3 = rungekutta.rk4(
pfunc2,
0.0,
numpy.array([3.0, 0.1], dtype=numpy.float64),
0.02,
250000
)
xs3[0,:] = numpy.array(
[pendulum.mod2pi(x) for x in xs3[0,:]],
dtype=numpy.float64
)
points3 = xs3.transpose()
ps4 = poincare.section(ts3, points3, interval=2*numpy.pi/drive_freq)
plot.mod2pi(
ps4[:,0],
ps4[:,1],
'k.',
xlabel=r'$\theta$',
ylabel=r'$\omega$',
markersize=0.6,
title='Poincare section, increased step size',
file_prefix=_suffixed(file_prefix, '_1d')
)
def pr1c(file_prefix):
drive_freq = 7.4246
pfunc2 = pendulum.pendulum(0.1, 0.1, 0.25, ampl=1, freq=drive_freq)
ts2, xs2 = rungekutta.rk4(
pfunc2,
0.0,
numpy.array([3.0, 0.1], dtype=numpy.float64),
0.005,
1000000
)
xs2[0,:] = numpy.array(
[pendulum.mod2pi(x) for x in xs2[0,:]],
dtype=numpy.float64
)
points2 = xs2.transpose()
ps3 = poincare.section(ts2, points2, interval=2*numpy.pi/drive_freq)
plot.mod2pi(
ps3[:,0],
ps3[:,1],
'k.',
xlabel=r'$\theta$',
ylabel=r'$\omega$',
markersize=0.6,
title='Poincare Section (Chaotic Trajectory)',
file_prefix=_suffixed(file_prefix, '_1c')
)
def pr2x(file_prefix, step, nsteps, title, suffix):
drive_freq = 7.4246
pfunc = pendulum.pendulum(0.1, 0.1, 0.25, ampl=1.0, freq=drive_freq)
ts, xs = rungekutta.rk4(
pfunc,
0.0,
numpy.array([3.0, 0.1], dtype=numpy.float64),
step,
nsteps
)
xs[0,:] = numpy.array(
[pendulum.mod2pi(x) for x in xs[0,:]],
dtype=numpy.float64
)
points = xs.transpose()
ps = poincare.linear(ts, points, interval=2*numpy.pi/drive_freq)
plot.mod2pi(
ps[:,0],
ps[:,1],
'k.',
xlabel=r'$\theta$',
ylabel=r'$\omega$',
markersize=0.6,
title=title,
file_prefix=_suffixed(file_prefix, suffix)
)
def rk4(double_pendulum):
dt = 0.02
condition = np.array([
double_pendulum.pendulum1.theta, double_pendulum.pendulum2.theta,
double_pendulum.pendulum1.vel, double_pendulum.pendulum2.vel
])
k1 = function(condition, double_pendulum) * dt
k2 = function(condition + k1 / 2, double_pendulum) * dt
k3 = function(condition + k2 / 2, double_pendulum) * dt
k4 = function(condition + k3, double_pendulum) * dt
condition += (k1 + 2 * k2 + 2 * k3 + k4) / 6
return pendulum.double_pendulum(
pendulum.pendulum(double_pendulum.pendulum1.mass,
double_pendulum.pendulum1.length, condition[0],
condition[2]),
pendulum.pendulum(double_pendulum.pendulum2.mass,
double_pendulum.pendulum2.length, condition[1],
condition[3]), double_pendulum.gravity)
def pr1d(file_prefix):
drive_freq = 7.4246
pfunc2 = pendulum.pendulum(0.1, 0.1, 0.25, ampl=1, freq=drive_freq)
ts3, xs3 = rungekutta.rk4(pfunc2, 0.0,
numpy.array([3.0, 0.1], dtype=numpy.float64),
0.02, 250000)
xs3[0, :] = numpy.array([pendulum.mod2pi(x) for x in xs3[0, :]],
dtype=numpy.float64)
points3 = xs3.transpose()
ps4 = poincare.section(ts3, points3, interval=2 * numpy.pi / drive_freq)
plot.mod2pi(ps4[:, 0],
ps4[:, 1],
'k.',
xlabel=r'$\theta$',
ylabel=r'$\omega$',
markersize=0.6,
title='Poincare section, increased step size',
file_prefix=_suffixed(file_prefix, '_1d'))
def pr1c(file_prefix):
drive_freq = 7.4246
pfunc2 = pendulum.pendulum(0.1, 0.1, 0.25, ampl=1, freq=drive_freq)
ts2, xs2 = rungekutta.rk4(pfunc2, 0.0,
numpy.array([3.0, 0.1], dtype=numpy.float64),
0.005, 1000000)
xs2[0, :] = numpy.array([pendulum.mod2pi(x) for x in xs2[0, :]],
dtype=numpy.float64)
points2 = xs2.transpose()
ps3 = poincare.section(ts2, points2, interval=2 * numpy.pi / drive_freq)
plot.mod2pi(ps3[:, 0],
ps3[:, 1],
'k.',
xlabel=r'$\theta$',
ylabel=r'$\omega$',
markersize=0.6,
title='Poincare Section (Chaotic Trajectory)',
file_prefix=_suffixed(file_prefix, '_1c'))
def pr1a(file_prefix):
pfunc = pendulum.pendulum(0.1, 0.1, 0)
ts, xs = rungekutta.rk4(pfunc, 0.0,
numpy.array([0.01, 0.0], dtype=numpy.float64),
0.005, 80000)
points = xs.transpose()
ps = poincare.section(ts, points, interval=0.6346975625940523)
plot.render(ps[:, 0],
ps[:, 1],
'k.',
xbound=(-0.015, 0.015),
ybound=(-0.15, 0.15),
xlabel=r'$\theta$',
ylabel=r'$\omega$',
markersize=0.6,
title='Poincare Section (Natural Frequency)',
file_prefix=_suffixed(file_prefix, '_1a'))
return ts, points
def pr2x(file_prefix, step, nsteps, title, suffix):
drive_freq = 7.4246
pfunc = pendulum.pendulum(0.1, 0.1, 0.25, ampl=1.0, freq=drive_freq)
ts, xs = rungekutta.rk4(pfunc, 0.0,
numpy.array([3.0, 0.1], dtype=numpy.float64), step,
nsteps)
xs[0, :] = numpy.array([pendulum.mod2pi(x) for x in xs[0, :]],
dtype=numpy.float64)
points = xs.transpose()
ps = poincare.linear(ts, points, interval=2 * numpy.pi / drive_freq)
plot.mod2pi(ps[:, 0],
ps[:, 1],
'k.',
xlabel=r'$\theta$',
ylabel=r'$\omega$',
markersize=0.6,
title=title,
file_prefix=_suffixed(file_prefix, suffix))
def pr1a(file_prefix):
pfunc = pendulum.pendulum(0.1, 0.1, 0)
ts, xs = rungekutta.rk4(
pfunc,
0.0,
numpy.array([0.01, 0.0], dtype=numpy.float64),
0.005,
80000
)
points = xs.transpose()
ps = poincare.section(ts, points, interval=0.6346975625940523)
plot.render(
ps[:,0],
ps[:,1],
'k.',
xbound=(-0.015, 0.015),
ybound=(-0.15, 0.15),
xlabel=r'$\theta$',
ylabel=r'$\omega$',
markersize=0.6,
title='Poincare Section (Natural Frequency)',
file_prefix=_suffixed(file_prefix, '_1a')
)
return ts, points
Problem 3) Pendulum
For this problem the aim is to use either Q-Learning or SARSA for the pendulum environment. You should be able to
reuse most of the Q-Learning and SARSA algorithms form the preveous problems, but you may have to make two main
changes:
1) The states of the pendulum environment are continous, you must crteate a discretization of the state so
that it can be stored in a Q-table.
2) The state of the pendulum has multiple elements (s = [theta, theta_dot]) you must change the code so that
Q-table is able to store this. The simplest way ofdoing this is to create a three dimensional Q-Table in
the following way: Q[theta, theta_dot, action]
"""
# Create instance of pendulum environment
env = pendulum()
fig = env.render()
gamma = 0.99 # Discount rate
alpha = 0.2 # Learning rate
epsilon = 0.9 # Probability of taking greedy action
episodes = 5000 # Number of episodes
num_d = 80
Q = np.zeros([num_d + 1, num_d + 1, 3])
list_theta = np.arange(-np.pi, np.pi, 2 * np.pi / num_d)
list_theta_dot = np.arange(-10, 10, 2 * 10 / num_d)
for i in range(episodes):
s, r, done = env.reset()
while True:
def pid_sim(pid_param, plot_en=False):
"""
Inverted pendulum simulation
@param pid_param - the pid controller parameter
# kp_x, ki_x, kd_x, kp_t, ki_t, kd_t
@param plot_en - plot record data or not
#note plot is blocking, you should close the figure manually and the program will continue
"""
force = 0 # Force last calculate
# Create inverted pendulum system, signal set to origin point
plant = pendulum.pendulum(M=const.M,
m=const.m,
L=const.L,
mu_c=const.mu_c,
mu_p=const.mu_p,
signal=signal_const)
plant.initial(const.theta_init, const.pos_init)
# Create PID variable
e_x = d_x = i_x = 0 # Position error term
prev_ex = 0 # Previous position error
i_x_bound = 10 # Prevent intergral overshoot
e_t = d_t = i_t = 0 # Angle error term
prev_et = 0 # Previous angle error
i_t_bound = 3 # Prevent intergral overshoot
pid_outmax = 80
t = 0
record_theta = []
record_x = []
desire_x = []
record_x_e = []
record_force = []
for t in range(const.TUNE_LIMIT):
# Calculate error for record
x_d = plant.signal(t)
error_x = plant.pos[0] - x_d
# Judge if reach the controller sample time
if t % const.REACT_T == 0:
""" Control sample """
e_x = plant.pos[0] - x_d # position error
d_x = e_x - prev_ex # position error differential
i_x = i_x + e_x # position error integrate
prev_ex = e_x # update previous position error
e_t = plant.theta[0] - 0 # angle error
d_t = e_t - prev_et # angle error differential
i_t = i_t + e_t # angle error integrate
prev_et = e_t # update previous angle error
# Prevent position integral overshoot
if i_x > i_x_bound:
i_x = i_x_bound
elif i_x < -1 * i_x_bound:
i_x = -1 * i_x_bound
# Prevent angle integral overshoot
if i_t > i_t_bound:
i_t = i_t_bound
elif i_t < -1 * i_t_bound:
i_t = -1 * i_t_bound
pid_output = -1*(pid_param[0] * e_x + 0.01*pid_param[1] * i_x + pid_param[2] * d_x) +\
(pid_param[3] * e_t + 0.01*pid_param[4] * i_t + pid_param[5] * d_t)
if pid_output > pid_outmax:
pid_output = pid_outmax
elif pid_output < -1 * pid_outmax:
pid_output = -1 * pid_outmax
force = pid_output
# Terminate condition, if we are training, no ploting
if not plot_en:
# theta out of limit
if abs(plant.theta[0]) >= const.theta_limit:
t = const.TUNE_LIMIT
break
# x out of limit
if abs(error_x) >= const.x_limit:
t = const.TUNE_LIMIT
break
# Theta is stable in a range of time
len_record = len(record_theta)
avg_theta = 0
if len_record > const.window:
sample_data = record_theta[len_record -
int(const.window /
2):len_record].copy()
sample_data = np.abs(sample_data)
avg_theta = np.average(sample_data)
if avg_theta < const.stable_theta:
break
# Run model
# print(force)
plant.add_force(force)
record_theta.append(plant.theta[0])
record_x.append(plant.pos[0])
desire_x.append(x_d)
record_x_e.append(error_x)
record_force.append(force)
len_record = len(record_theta)
sample_data = 0
sample_x_e = 0
if len_record >= const.window:
sample_data = record_theta[len_record - const.window:len_record].copy()
sample_x_e = record_x_e[len_record - const.window:len_record].copy()
else:
sample_data = record_theta[:len_record].copy()
sample_x_e = record_x_e[:len_record].copy()
sample_data = np.abs(sample_data)
sample_x_e = np.abs(sample_x_e)
avg_theta = np.average(sample_data)
avg_x_e = np.average(sample_x_e)
fit = fitness(avg_theta, avg_x_e, t)
if plot_en:
fig, axs = plt.subplots(3, 1, constrained_layout=True)
axs[0].plot(record_x, '--', desire_x, '-')
axs[0].set_title('x')
axs[1].plot(record_theta, '--')
axs[1].set_title('theta')
axs[2].plot(record_force, '--')
axs[2].set_title('force')
plt.show()
return fit
'''
Set constant position
'''
def signal_const(t):
return -4
if __name__ == "__main__":
# Create inverted pendulum system, signal set to origin point
# sys = pendulum.pendulum(M=const.M, m=const.m, L=const.L, mu_c=const.mu_c, mu_p=const.mu_p)
# Create inverted pendulum system, signal set to constant desire position
# sys = pendulum.pendulum(M=const.M, m=const.m, L=const.L, mu_c=const.mu_c, mu_p=const.mu_p,signal=signal_const)
# Create inverted pendulum system, signal set square function
sys = pendulum.pendulum(M=const.M, m=const.m, L=const.L, mu_c=const.mu_c, mu_p=const.mu_p,signal=signal_sqrt)
sys.initial(const.theta_init, const.pos_init)
# fit = simulate([11.88827976, 1.97008765, 14.13742648, 15.72209416, 1.38708104, 0.12893926]) # Test results
# Initial ABC algorithm
algo = abc_py.ABC (dim=dim, num=const.num, max_iter=const.max_iter, u_bound=const.p_range[1], l_bound=const.p_range[0], func=fuzzy_sim, end_thres=const.end_thres, end_sample=const.end_sample, fit_max=const.fitness_max)
# Initial particles
algo.abc_init()
# Run iteration
algo.abc_iterator()
# Extract best solution
C = algo.bestx.copy()
import os
os.environ['SDL_VIDEO_WINDOW_POS'] = "50,50"
import pygame as pg
from pendulum import pendulum
from ball import ball
from math import pi
pg.init()
h,w=600,400
scr=pg.display.set_mode((h,w))
pg.draw.rect(scr,(0,0,0),[0,0,600,600])
p1 = pendulum(50,100,1,1,0,0,50)
p2 = pendulum(50,100,1,1,-0,-0,550)
b= ball(160,38,0.6)
clc = pg.time.Clock()
while True:
p1.checkCollision(b,scr)
p2.checkCollision(b,scr)
b.collideWalls(h-1,w-1)
p1.move()
p2.move()
b.move()
pg.draw.rect(scr,(0,0,0),[0,0,600,600])
