forked from Telos4/RoboRally
fixed problem with target angle of -pi, pi which was caused by discontinuities in the angle measurements
This commit is contained in:
parent
6fb3cd5484
commit
826fa4be0f
|
|
@ -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))
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user