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

fixed problem with target angle of -pi, pi which was caused by discontinuities in the angle measurements

Browse Source
This commit is contained in:
Simon Pirkelmann 2020-09-07 16:18:35 +02:00
parent 6fb3cd5484
commit 826fa4be0f
2 changed files with 27 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
remote_control/casadi_opt.py
View File

@ -3,6 +3,9 @@ import time
# look at: https://github.com/casadi/casadi/blob/master/docs/examples/python/vdp_indirect_multiple_shooting.py # look at: https://github.com/casadi/casadi/blob/master/docs/examples/python/vdp_indirect_multiple_shooting.py
# TODO for roborally
# implement essential movements: forward, backward, turn left, turn right on grid-based level
class OpenLoopSolver: class OpenLoopSolver:
def __init__(self, N=20, T=4.0): def __init__(self, N=20, T=4.0):
self.T = T self.T = T
@ -40,7 +43,8 @@ class OpenLoopSolver:
# cost functional # cost functional
target = (-0.0, 0.0) target = (-0.0, 0.0)
L = (x-target[0]) ** 2 + (y-target[1]) ** 2 + 1e-2 * theta ** 2 + 1e-2 * (omegar ** 2 + omegal ** 2) L = (x-target[0]) ** 2 + (y-target[1]) ** 2 + 1e-2 * (omegar ** 2 + omegal ** 2)
#L = (x-target[0]) ** 2 + (y-target[1]) ** 2 + 1e-2 * theta ** 2 + 1e-2 * (omegar ** 2 + omegal ** 2)
# Fixed step Runge-Kutta 4 integrator # Fixed step Runge-Kutta 4 integrator
M = 4 # RK4 steps per interval M = 4 # RK4 steps per interval
@ -138,7 +142,12 @@ class OpenLoopSolver:
#return #return
def solve(self, x0, target, obstacles, track): def solve(self, x0, target, obstacles, turn=False):
angles_unwrapped = np.unwrap([x0[2], target[2]]) # unwrap angle to avoid jump in data
x0[2] = angles_unwrapped[0]
target[2] = angles_unwrapped[1]
tstart = time.time() tstart = time.time()
# alternative solution using multiple shooting (way faster!) # alternative solution using multiple shooting (way faster!)
self.opti = Opti() # Optimization problem self.opti = Opti() # Optimization problem
@ -186,7 +195,11 @@ class OpenLoopSolver:
theta)) * omegal * omega_max theta)) * omegal * omega_max
f3 = -(r / (2 * R) * omegar - r / (2 * R) * omegal) * omega_max f3 = -(r / (2 * R) * omegar - r / (2 * R) * omegal) * omega_max
xdot = vertcat(f1, f2, f3) xdot = vertcat(f1, f2, f3)
L = (x - target[0]) ** 2 + (y - target[1]) ** 2 + 1e-2 * (theta - target[2]) ** 2 + 1e-2 * (omegar ** 2 + omegal ** 2)
#L = 100 * (x - target[0]) ** 2 + 100 * (y - target[1]) ** 2 + 1e-1 * (theta - target[2])**2 + 1e-2 * (omegar ** 2 + omegal ** 2)
L = 100 * (x - target[0]) ** 2 + 100 * (y - target[1]) ** 2 + 1e-1 * (target[2] - theta) ** 2 + 1e-2 * (
omegar ** 2 + omegal ** 2)
#L = (x - target[0]) ** 2 + (y - target[1]) ** 2 + 1e-2 * (theta - target[2]) ** 2 + 1e-2 * (omegar ** 2 + omegal ** 2)
self.f = Function('f', [state, control], [xdot, L]) self.f = Function('f', [state, control], [xdot, L])
# ---- solve NLP ------ # ---- solve NLP ------
@ -222,7 +235,7 @@ class OpenLoopSolver:
self.opti.subject_to(self.Q[:, 0] == 0.0) self.opti.subject_to(self.Q[:, 0] == 0.0)
solver = self.opti.solver("ipopt", {}, {"print_level": 0}) solver = self.opti.solver("ipopt", {'print_time': False}, {"print_level": 0})
# ---- misc. constraints ---------- # ---- misc. constraints ----------
# self.opti.subject_to(X[1,:]>=0) # Time must be positive # self.opti.subject_to(X[1,:]>=0) # Time must be positive
@ -256,7 +269,7 @@ class OpenLoopSolver:
self.opti.subject_to(angle[0] == x0[2]) # finish line at position 1 self.opti.subject_to(angle[0] == x0[2]) # finish line at position 1
tend = time.time() tend = time.time()
print("setting up problem took {} seconds".format(tend - tstart)) #print("setting up problem took {} seconds".format(tend - tstart))
tstart = time.time() tstart = time.time()
if self.use_warmstart and self.opti_x0 is not None: if self.use_warmstart and self.opti_x0 is not None:
@ -267,7 +280,7 @@ class OpenLoopSolver:
print("could not set warmstart") print("could not set warmstart")
sol = self.opti.solve() # actual solve sol = self.opti.solve() # actual solve
tend = time.time() tend = time.time()
print("solving the problem took {} seconds".format(tend - tstart)) #print("solving the problem took {} seconds".format(tend - tstart))
tstart = time.time() tstart = time.time()
self.opti_x0 = sol.value(self.opti.x) self.opti_x0 = sol.value(self.opti.x)
@ -278,7 +291,7 @@ class OpenLoopSolver:
u_opt_1 = sol.value(self.U[0,:]) u_opt_1 = sol.value(self.U[0,:])
u_opt_2 = sol.value(self.U[1,:]) u_opt_2 = sol.value(self.U[1,:])
tend = time.time() tend = time.time()
print("postprocessing took {} seconds".format(tend - tstart)) #print("postprocessing took {} seconds".format(tend - tstart))
return (u_opt_1, u_opt_2, sol.value(posx), sol.value(posy)) return (u_opt_1, u_opt_2, sol.value(posx), sol.value(posy))

13
remote_control/roborally.py
View File

@ -408,7 +408,6 @@ class RemoteController:
elif event.key == pygame.K_w: elif event.key == pygame.K_w:
move = 1 move = 1
turn = 0 turn = 0
elif event.key == pygame.K_s: elif event.key == pygame.K_s:
move = -1 move = -1
turn = 0 turn = 0
@ -419,7 +418,8 @@ class RemoteController:
elif event.key == pygame.K_d: elif event.key == pygame.K_d:
turn = -1 turn = -1
move = 0 move = 0
integ = 0 elif event.key == pygame.K_r:
turn = 2
elif event.key == pygame.K_p: elif event.key == pygame.K_p:
self.pid = True self.pid = True
elif event.key == pygame.K_SPACE: elif event.key == pygame.K_SPACE:
@ -459,7 +459,7 @@ class RemoteController:
elif turn != 0: elif turn != 0:
new_x = grid_pos[0] new_x = grid_pos[0]
new_y = grid_pos[1] new_y = grid_pos[1]
new_angle = grid_pos[2] + turn * np.pi/2 new_angle = np.unwrap([0, grid_pos[2] + turn * np.pi/2])[1]
grid_pos = (new_x, new_y, new_angle) grid_pos = (new_x, new_y, new_angle)
print(grid_pos) print(grid_pos)
@ -473,15 +473,16 @@ class RemoteController:
tmpc_start = time.time() tmpc_start = time.time()
error_pos = np.linalg.norm(x_pred[0:2] - self.target[0:2]) error_pos = np.linalg.norm(x_pred[0:2] - self.target[0:2])
error_ang = np.abs(x_pred[2] - self.target[2]) angles_unwrapped = np.unwrap([x_pred[2], self.target[2]]) # unwrap angle to avoid jump in data
print("error pos = ", error_pos, " error_ang = ", error_ang) error_ang = np.abs(angles_unwrapped[0] - angles_unwrapped[1])
#print("error pos = ", error_pos)
print(" error_ang = {}, target = {}, angle = {}".format(error_ang, self.target[2], x_pred[2]))
turning = turn != 0 turning = turn != 0
if error_pos > 0.1 or error_ang > 0.4: if error_pos > 0.1 or error_ang > 0.4:
# solve mpc open loop problem # solve mpc open loop problem
res = self.ols.solve(x_pred, self.target, [], turning) res = self.ols.solve(x_pred, self.target, [], turning)
#us1 = res[0] #us1 = res[0]
#us2 = res[1] #us2 = res[1]
us1 = res[0] * control_scaling us1 = res[0] * control_scaling