apo/RoboRally
1
0
Fork 0
forked from Telos4/RoboRally
Code Issues Pull Requests Releases Wiki Activity

added test for max speed and prediction step in MPC

Browse Source
This commit is contained in:
Simon Pirkelmann 2019-06-14 10:37:46 +02:00
parent fb12a3c94b
commit 465309ee45
1 changed files with 116 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

147
remote_control/position_controller.py
View File

@ -124,7 +124,7 @@ class RemoteController:
self.ols = OpenLoopSolver() self.ols = OpenLoopSolver()
self.ols.setup() self.ols.setup()
self.target = (0.0, 0.0) self.target = (0.0, 0.0, 0.0)
def ani(self): def ani(self):
self.ani = anim.FuncAnimation(self.fig, init_func=self.ani_init, func=self.ani_update, interval=10, blit=True) 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 tgrid = None
us1 = None us1 = None
us2 = 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") print("starting control")
while True: while True:
@ -225,7 +237,7 @@ class RemoteController:
if keyboard_control: # keyboard controller if keyboard_control: # keyboard controller
events = pygame.event.get() events = pygame.event.get()
speed = 0.5 speed = 1.0
for event in events: for event in events:
if event.type == pygame.KEYDOWN: if event.type == pygame.KEYDOWN:
if event.key == pygame.K_LEFT: if event.key == pygame.K_LEFT:
@ -244,17 +256,17 @@ class RemoteController:
self.u1 = -speed self.u1 = -speed
self.u2 = -speed self.u2 = -speed
#print("forward: ({},{})".format(u1, u2)) #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: elif event.type == pygame.KEYUP:
self.u1 = 0 self.u1 = 0
self.u2 = 0 self.u2 = 0
#print("key released, resetting: ({},{})".format(u1, u2)) #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() tnew = time.time()
dt = tnew - self.t dt = tnew - self.t
r = scipy.integrate.ode(f_ode) 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) #print(self.x0)
if self.x0 is None: if self.x0 is None:
@ -282,18 +294,59 @@ class RemoteController:
events = pygame.event.get() events = pygame.event.get()
for event in events: for event in events:
if event.type == pygame.KEYDOWN: if event.type == pygame.KEYDOWN:
if event.key == pygame.K_LEFT: if event.key == pygame.K_1:
self.speed = self.speed / np.sqrt(np.sqrt(np.sqrt(10.0))) self.controlling = True
elif event.key == pygame.K_RIGHT: forward = True
self.speed = self.speed * np.sqrt(np.sqrt(np.sqrt(10.0))) print("starting test")
elif event.key == pygame.K_UP: self.mutex.acquire()
u1 = self.speed try:
u2 = -self.speed init_pos = copy.deepcopy(self.xms[-1])
elif event.key == pygame.K_DOWN: init_time = copy.deepcopy(self.tms[-1])
u1 = 0.0 finally:
u2 = 0.0 self.mutex.release()
print("speed = {}".format(self.speed)) if event.key == pygame.K_2:
self.rc_socket.send('({},{})\n'.format(u1, u2)) 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: elif pid:
# pid controller # pid controller
@ -351,24 +404,44 @@ class RemoteController:
if event.type == pygame.KEYDOWN: if event.type == pygame.KEYDOWN:
if event.key == pygame.K_UP: if event.key == pygame.K_UP:
self.controlling = True self.controlling = True
self.t = time.time()
elif event.key == pygame.K_DOWN: elif event.key == pygame.K_DOWN:
self.controlling = False self.controlling = False
self.rc_socket.send('(0.1, 0.0,0.0)\n') self.rc_socket.send('(0.1, 0.0,0.0)\n')
elif event.key == pygame.K_0: 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: 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: 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: 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: elif event.key == pygame.K_4:
self.target = (-0.5,0.5) self.target = (-0.5,0.5, 0.0)
if self.controlling: if self.controlling:
tmpc_start = time.time() tmpc_start = time.time()
# get measurement # get measurement
last_measurement = copy.deepcopy(self.xms[-1]) self.mutex.acquire()
res = self.ols.solve(last_measurement, self.target) 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] #tgrid = res[0]
us1 = res[0] us1 = res[0]
us2 = res[1] us2 = res[1]
@ -396,16 +469,28 @@ class RemoteController:
#if event.key == pygame.K_DOWN: #if event.key == pygame.K_DOWN:
# if tgrid is not None: # if tgrid is not None:
tmpc_end = time.time() tmpc_end = time.time()
print("---------------- mpc solution took {} seconds".format(tmpc_end-tmpc_start)) print("---------------- mpc solution took {} seconds".format(tmpc_end - tmpc_start))
for i in range(0, 5): 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] u1 = us1[i]
u2 = us2[i] u2 = us2[i]
dt_mpc = time.time() - self.t self.rc_socket.send('({},{},{})\n'.format(dt,u1, u2))
#if dt_mpc < 0.1:
# time.sleep(0.1 - dt_mpc)
self.rc_socket.send('({},{},{})\n'.format(0.1,u1, u2))
self.t = time.time() self.t = time.time()
time.sleep(0.1) #time.sleep(0.2)
# #