added test for max speed and prediction step in MPC
parent
fb12a3c94b
commit
465309ee45
|
|
@ -124,7 +124,7 @@ class RemoteController:
|
|||
self.ols = OpenLoopSolver()
|
||||
self.ols.setup()
|
||||
|
||||
self.target = (0.0, 0.0)
|
||||
self.target = (0.0, 0.0, 0.0)
|
||||
|
||||
def ani(self):
|
||||
self.ani = anim.FuncAnimation(self.fig, init_func=self.ani_init, func=self.ani_update, interval=10, blit=True)
|
||||
|
|
@ -215,6 +215,18 @@ class RemoteController:
|
|||
tgrid = None
|
||||
us1 = None
|
||||
us2 = None
|
||||
u1 = -0.0
|
||||
u2 = 0.0
|
||||
|
||||
r = scipy.integrate.ode(f_ode)
|
||||
|
||||
omega_max = 5.0
|
||||
|
||||
init_pos = None
|
||||
init_time = None
|
||||
final_pos = None
|
||||
final_time = None
|
||||
forward = True
|
||||
print("starting control")
|
||||
while True:
|
||||
|
||||
|
|
@ -225,7 +237,7 @@ class RemoteController:
|
|||
|
||||
if keyboard_control: # keyboard controller
|
||||
events = pygame.event.get()
|
||||
speed = 0.5
|
||||
speed = 1.0
|
||||
for event in events:
|
||||
if event.type == pygame.KEYDOWN:
|
||||
if event.key == pygame.K_LEFT:
|
||||
|
|
@ -244,17 +256,17 @@ class RemoteController:
|
|||
self.u1 = -speed
|
||||
self.u2 = -speed
|
||||
#print("forward: ({},{})".format(u1, u2))
|
||||
self.rc_socket.send('({},{})\n'.format(self.u1, self.u2))
|
||||
self.rc_socket.send('({},{},{})\n'.format(0.1, self.u1, self.u2))
|
||||
elif event.type == pygame.KEYUP:
|
||||
self.u1 = 0
|
||||
self.u2 = 0
|
||||
#print("key released, resetting: ({},{})".format(u1, u2))
|
||||
self.rc_socket.send('({},{})\n'.format(self.u1, self.u2))
|
||||
self.rc_socket.send('({}, {},{})\n'.format(0.1, self.u1, self.u2))
|
||||
|
||||
tnew = time.time()
|
||||
dt = tnew - self.t
|
||||
r = scipy.integrate.ode(f_ode)
|
||||
r.set_f_params(np.array([self.u1 * 13.32, self.u2 * 13.32]))
|
||||
r.set_f_params(np.array([self.u1 * omega_max, self.u2 * omega_max]))
|
||||
|
||||
#print(self.x0)
|
||||
if self.x0 is None:
|
||||
|
|
@ -282,18 +294,59 @@ class RemoteController:
|
|||
events = pygame.event.get()
|
||||
for event in events:
|
||||
if event.type == pygame.KEYDOWN:
|
||||
if event.key == pygame.K_LEFT:
|
||||
self.speed = self.speed / np.sqrt(np.sqrt(np.sqrt(10.0)))
|
||||
elif event.key == pygame.K_RIGHT:
|
||||
self.speed = self.speed * np.sqrt(np.sqrt(np.sqrt(10.0)))
|
||||
elif event.key == pygame.K_UP:
|
||||
u1 = self.speed
|
||||
u2 = -self.speed
|
||||
elif event.key == pygame.K_DOWN:
|
||||
u1 = 0.0
|
||||
u2 = 0.0
|
||||
print("speed = {}".format(self.speed))
|
||||
self.rc_socket.send('({},{})\n'.format(u1, u2))
|
||||
if event.key == pygame.K_1:
|
||||
self.controlling = True
|
||||
forward = True
|
||||
print("starting test")
|
||||
self.mutex.acquire()
|
||||
try:
|
||||
init_pos = copy.deepcopy(self.xms[-1])
|
||||
init_time = copy.deepcopy(self.tms[-1])
|
||||
finally:
|
||||
self.mutex.release()
|
||||
if event.key == pygame.K_2:
|
||||
self.controlling = True
|
||||
forward = False
|
||||
print("starting test")
|
||||
self.mutex.acquire()
|
||||
try:
|
||||
init_pos = copy.deepcopy(self.xms[-1])
|
||||
init_time = copy.deepcopy(self.tms[-1])
|
||||
finally:
|
||||
self.mutex.release()
|
||||
elif event.key == pygame.K_3:
|
||||
self.controlling = False
|
||||
print("stopping test")
|
||||
self.rc_socket.send('(0.1, 0.0,0.0)\n')
|
||||
|
||||
self.mutex.acquire()
|
||||
try:
|
||||
final_pos = copy.deepcopy(self.xms[-1])
|
||||
final_time = copy.deepcopy(self.tms[-1])
|
||||
finally:
|
||||
self.mutex.release()
|
||||
|
||||
print("init_pos = {}".format(init_pos))
|
||||
print("final_pos = {}".format(final_pos))
|
||||
print("distance = {}".format(np.linalg.norm(init_pos[0:2]-final_pos[0:2])))
|
||||
print("dt = {}".format(final_time - init_time))
|
||||
|
||||
d = np.linalg.norm(init_pos[0:2]-final_pos[0:2])
|
||||
t = final_time - init_time
|
||||
r = 0.03
|
||||
|
||||
angular_velocity = d/r/t
|
||||
print("average angular velocity = {}".format(angular_velocity))
|
||||
|
||||
|
||||
if self.controlling:
|
||||
if forward:
|
||||
self.rc_socket.send('(0.1, 1.0,1.0)\n')
|
||||
else:
|
||||
self.rc_socket.send('(0.1, -1.0,-1.0)\n')
|
||||
time.sleep(0.1)
|
||||
#print("speed = {}".format(self.speed))
|
||||
|
||||
|
||||
elif pid:
|
||||
# pid controller
|
||||
|
|
@ -351,24 +404,44 @@ class RemoteController:
|
|||
if event.type == pygame.KEYDOWN:
|
||||
if event.key == pygame.K_UP:
|
||||
self.controlling = True
|
||||
self.t = time.time()
|
||||
elif event.key == pygame.K_DOWN:
|
||||
self.controlling = False
|
||||
self.rc_socket.send('(0.1, 0.0,0.0)\n')
|
||||
elif event.key == pygame.K_0:
|
||||
self.target = (0.0, 0.0)
|
||||
self.target = (0.0, 0.0, 0.0)
|
||||
elif event.key == pygame.K_1:
|
||||
self.target = (0.5,0.5)
|
||||
self.target = (0.5,0.5, -np.pi/2.0)
|
||||
elif event.key == pygame.K_2:
|
||||
self.target = (0.5, -0.5)
|
||||
self.target = (0.5, -0.5, 0.0)
|
||||
elif event.key == pygame.K_3:
|
||||
self.target = (-0.5,-0.5)
|
||||
self.target = (-0.5,-0.5, np.pi/2.0)
|
||||
elif event.key == pygame.K_4:
|
||||
self.target = (-0.5,0.5)
|
||||
self.target = (-0.5,0.5, 0.0)
|
||||
if self.controlling:
|
||||
tmpc_start = time.time()
|
||||
# get measurement
|
||||
last_measurement = copy.deepcopy(self.xms[-1])
|
||||
res = self.ols.solve(last_measurement, self.target)
|
||||
self.mutex.acquire()
|
||||
try:
|
||||
last_measurement = copy.deepcopy(self.xms[-1])
|
||||
last_time = copy.deepcopy(self.tms[-1])
|
||||
finally:
|
||||
self.mutex.release()
|
||||
|
||||
print("current measurement (t, x) = ({}, {})".format(last_time, last_measurement))
|
||||
print("current control (u1, u2) = ({}, {})".format(u1, u2))
|
||||
|
||||
# prediction of state at time the mpc will terminate
|
||||
r.set_f_params(np.array([u1 * omega_max, u2 * omega_max]))
|
||||
|
||||
r.set_initial_value(last_measurement, last_time)
|
||||
dt = self.ols.T/self.ols.N
|
||||
print("integrating for {} seconds".format((dt)))
|
||||
x_pred = r.integrate(r.t + (dt))
|
||||
|
||||
print("predicted initial state x_pred = ({})".format(x_pred))
|
||||
|
||||
res = self.ols.solve(x_pred, self.target)
|
||||
#tgrid = res[0]
|
||||
us1 = res[0]
|
||||
us2 = res[1]
|
||||
|
|
@ -396,16 +469,28 @@ class RemoteController:
|
|||
#if event.key == pygame.K_DOWN:
|
||||
# if tgrid is not None:
|
||||
tmpc_end = time.time()
|
||||
print("---------------- mpc solution took {} seconds".format(tmpc_end-tmpc_start))
|
||||
for i in range(0, 5):
|
||||
print("---------------- mpc solution took {} seconds".format(tmpc_end - tmpc_start))
|
||||
dt_mpc = time.time() - self.t
|
||||
if dt_mpc < dt:
|
||||
print("sleeping for {} seconds...".format(dt - dt_mpc))
|
||||
time.sleep(dt - dt_mpc)
|
||||
|
||||
self.mutex.acquire()
|
||||
try:
|
||||
second_measurement = copy.deepcopy(self.xms[-1])
|
||||
second_time = copy.deepcopy(self.tms[-1])
|
||||
finally:
|
||||
self.mutex.release()
|
||||
|
||||
print("(last_time, second_time, dt) = ({}, {}, {})".format(last_time, second_time, second_time - last_time))
|
||||
print("mismatch between predicted state and measured state: {}\n\n".format(second_measurement - last_measurement))
|
||||
|
||||
for i in range(0, 1):
|
||||
u1 = us1[i]
|
||||
u2 = us2[i]
|
||||
dt_mpc = time.time() - self.t
|
||||
#if dt_mpc < 0.1:
|
||||
# time.sleep(0.1 - dt_mpc)
|
||||
self.rc_socket.send('({},{},{})\n'.format(0.1,u1, u2))
|
||||
self.rc_socket.send('({},{},{})\n'.format(dt,u1, u2))
|
||||
self.t = time.time()
|
||||
time.sleep(0.1)
|
||||
#time.sleep(0.2)
|
||||
#
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue