From 826fa4be0fdb04cea42f6d2c36869f259ed2c317 Mon Sep 17 00:00:00 2001 From: spirkelmann Date: Mon, 7 Sep 2020 16:18:35 +0200 Subject: [PATCH] fixed problem with target angle of -pi, pi which was caused by discontinuities in the angle measurements --- remote_control/casadi_opt.py | 27 ++++++++++++++++++++------- remote_control/roborally.py | 13 +++++++------ 2 files changed, 27 insertions(+), 13 deletions(-) diff --git a/remote_control/casadi_opt.py b/remote_control/casadi_opt.py index 87df398..cf13a3c 100644 --- a/remote_control/casadi_opt.py +++ b/remote_control/casadi_opt.py @@ -3,6 +3,9 @@ import time # 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: def __init__(self, N=20, T=4.0): self.T = T @@ -40,7 +43,8 @@ class OpenLoopSolver: # cost functional 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 M = 4 # RK4 steps per interval @@ -138,7 +142,12 @@ class OpenLoopSolver: #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() # alternative solution using multiple shooting (way faster!) self.opti = Opti() # Optimization problem @@ -186,7 +195,11 @@ class OpenLoopSolver: theta)) * omegal * omega_max f3 = -(r / (2 * R) * omegar - r / (2 * R) * omegal) * omega_max 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]) # ---- solve NLP ------ @@ -222,7 +235,7 @@ class OpenLoopSolver: 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 ---------- # 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 tend = time.time() - print("setting up problem took {} seconds".format(tend - tstart)) + #print("setting up problem took {} seconds".format(tend - tstart)) tstart = time.time() if self.use_warmstart and self.opti_x0 is not None: @@ -267,7 +280,7 @@ class OpenLoopSolver: print("could not set warmstart") sol = self.opti.solve() # actual solve tend = time.time() - print("solving the problem took {} seconds".format(tend - tstart)) + #print("solving the problem took {} seconds".format(tend - tstart)) tstart = time.time() self.opti_x0 = sol.value(self.opti.x) @@ -278,7 +291,7 @@ class OpenLoopSolver: u_opt_1 = sol.value(self.U[0,:]) u_opt_2 = sol.value(self.U[1,:]) 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)) diff --git a/remote_control/roborally.py b/remote_control/roborally.py index 6a7f1af..15d32f5 100644 --- a/remote_control/roborally.py +++ b/remote_control/roborally.py @@ -408,7 +408,6 @@ class RemoteController: elif event.key == pygame.K_w: move = 1 turn = 0 - elif event.key == pygame.K_s: move = -1 turn = 0 @@ -419,7 +418,8 @@ class RemoteController: elif event.key == pygame.K_d: turn = -1 move = 0 - integ = 0 + elif event.key == pygame.K_r: + turn = 2 elif event.key == pygame.K_p: self.pid = True elif event.key == pygame.K_SPACE: @@ -459,7 +459,7 @@ class RemoteController: elif turn != 0: new_x = grid_pos[0] 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) print(grid_pos) @@ -473,15 +473,16 @@ class RemoteController: tmpc_start = time.time() error_pos = np.linalg.norm(x_pred[0:2] - self.target[0:2]) - error_ang = np.abs(x_pred[2] - self.target[2]) - print("error pos = ", error_pos, " error_ang = ", error_ang) + angles_unwrapped = np.unwrap([x_pred[2], self.target[2]]) # unwrap angle to avoid jump in data + 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 if error_pos > 0.1 or error_ang > 0.4: # solve mpc open loop problem res = self.ols.solve(x_pred, self.target, [], turning) - #us1 = res[0] #us2 = res[1] us1 = res[0] * control_scaling