mpc.set_cost_weights(Q, R, T)
mpc.Y_prime = np.matrix(np.zeros(shape=(2, mpc_horizon)))
mpc.Y_prime[0, :] = 0.7
mpc.C = np.matrix(np.eye(2))
mpc.set_body(spring_damper)
Xs = np.zeros(shape=(mpc.body.get_state().shape[0], time.shape[0]))
Us = np.zeros(shape=(mpc.body.u.shape[0], time.shape[0]))
Ys = np.zeros(shape=Xs.shape)
Y_desired = np.zeros(shape=Xs.shape)
for i, t in enumerate(time):
if i % mpc_horizon == 0:
mpc.calc_control()
Xs[:, i] = np.ravel(mpc.body.get_state())
Ys[:, i] = np.ravel(mpc.Xs[:, i % mpc_horizon])
u = mpc.U[:, i % mpc_horizon]
mpc.body.simulate(dt, u)
Us[:, i] = np.ravel(u)
Y_desired[:, i] = np.ravel(mpc.Y_prime[:, i % mpc_horizon])
fig, axarr = plt.subplots(Xs.shape[0] + 1, sharex=True)
for i in range(Xs.shape[0]):
axarr[i].plot(time, Xs[i, :], label='Actual')
axarr[i].plot(time, Ys[i, :], label='Predicted')
axarr[i].plot(time, Y_desired[i, :], 'r--', label='Desired')
mpc.set_cost_weights(Q, R, T)
mpc.Y_prime = np.matrix(np.zeros(shape=(2, mpc_horizon)))
mpc.Y_prime[0, :] = 0.7
mpc.C = np.matrix(np.eye(2))
mpc.set_body(spring_damper)
Xs = np.zeros(shape=(mpc.body.get_state().shape[0], time.shape[0]))
Us = np.zeros(shape=(mpc.body.u.shape[0],time.shape[0]))
Ys = np.zeros(shape=Xs.shape)
Y_desired = np.zeros(shape=Xs.shape)
for i, t in enumerate(time):
if i % mpc_horizon == 0:
mpc.calc_control()
Xs[:, i] = np.ravel(mpc.body.get_state())
Ys[:, i] = np.ravel(mpc.Xs[:, i % mpc_horizon])
u = mpc.U[:, i % mpc_horizon]
mpc.body.simulate(dt, u)
Us[:, i] = np.ravel(u)
Y_desired[:, i] = np.ravel(mpc.Y_prime[:, i % mpc_horizon])
fig, axarr = plt.subplots(Xs.shape[0] + 1, sharex=True)
for i in range(Xs.shape[0]):
axarr[i].plot(time, Xs[i, :], label='Actual')
axarr[i].plot(time, Ys[i, :], label='Predicted')
axarr[i].plot(time, Y_desired[i, :], 'r--', label='Desired')
class System(object):
def __init__(self, window, dt=0.01):
self.use_mpc = True
self.clock = sf.Clock()
self.window = window
self.dt = dt
self.time = 0
self.mpc_horizon = 10
self.counter = -1
spring_damper = SpringDamper(window, pos_x=0.9, pos_y=0.5)
spring_damper.set_properties(k=10., m=1., c=0.1, x0=0.5)
spring_damper.integration_type = spring_damper.RUNGE_KUTTA
pendulum = Pendulum(window)
pendulum.set_properties(m=1.0, g=-9.8, L=0.25, k=0.1)
pendulum.set_position(0.5, 0.5)
pendulum.set_rotation(np.pi * 0.95)
pendulum.integration_type = pendulum.RUNGE_KUTTA
pendulum.nonlinear_mode = True
cart = CartPole(window)
cart.set_properties(m=0.2, M=0.5, I=0.006, g=-9.8, l=0.25, b=0.5)
cart.set_position(0.5, 0.5)
cart.set_rotation(np.pi * 0.99)
cart.nonlinear_mode = True
cart.integration_type = cart.RUNGE_KUTTA
self.mpc = MPC(dt, self.mpc_horizon)
# This works for springdamper and pendulum
R = np.matrix(np.eye(2) * 1e-5)
Q = np.matrix(np.matrix([[100, 0], [0.0, 1.0]]))
T = np.matrix(np.eye(2) * 1e-5)
self.mpc.set_cost_weights(Q, R, T)
self.mpc.Y_prime = np.matrix(np.zeros(shape=(2, self.mpc_horizon)))
self.mpc.Y_prime[0, :] = 0.7
self.mpc.C = np.matrix(np.eye(2))
# cartpole
# R = np.matrix(np.eye(4)*0.001)
# Q = np.matrix([[100, 0, 0, 0],
# [0, 0, 0, 0],
# [0, 0, 100, 0],
# [0, 0, 0, 0]])
#
# self.mpc.set_cost_weights(Q, R)
# self.mpc.set_constraints(cart.cons)
#
# self.mpc.Y_prime = np.matrix(np.zeros(shape=(4, self.mpc_horizon)))
# self.mpc.Y_prime[0,:] = np.pi
# self.mpc.Y_prime[2,:] = 0.5
# self.C = np.matrix([[1,0, 1, 0]])
# self.bodies = [spring_damper, pendulum]
self.bodies = [spring_damper]
self.mpc.set_body(self.bodies[0])
def simulate(self):
self.counter += 1
if self.use_mpc:
if self.counter % self.mpc_horizon == 0:
self.counter = 0
self.mpc.calc_control()
u = self.mpc.U[:, self.counter]
self.mpc.body.simulate(self.dt, np.matrix(u).transpose())
else:
for body in self.bodies:
body.simulate(self.dt)
def render(self):
self.window.clear(sf.Color.WHITE)
for body in self.bodies:
body.render()
add_time(self.window, self.time)
self.window.display()
def step(self):
if self.clock.elapsed_time.milliseconds >= self.dt * 1000:
self.clock.restart()
self.time += self.dt
self.simulate()
self.render()
