parameter tuning. works well with control scaling of 0.3
parent
826fa4be0f
commit
280aee75ee
|
|
@ -142,7 +142,7 @@ class OpenLoopSolver:
|
|||
#return
|
||||
|
||||
|
||||
def solve(self, x0, target, obstacles, turn=False):
|
||||
def solve(self, x0, target):
|
||||
|
||||
angles_unwrapped = np.unwrap([x0[2], target[2]]) # unwrap angle to avoid jump in data
|
||||
x0[2] = angles_unwrapped[0]
|
||||
|
|
@ -242,24 +242,6 @@ class OpenLoopSolver:
|
|||
# self.opti.subject_to(X[2,:]<=4) # Time must be positive
|
||||
# self.opti.subject_to(X[2,:]>=-2) # Time must be positive
|
||||
|
||||
# avoid obstacle
|
||||
for o in obstacles:
|
||||
p = obstacles[o].pos
|
||||
r = obstacles[o].radius
|
||||
if p is not None:
|
||||
for k in range(1,self.N):
|
||||
self.opti.subject_to((self.X[0,k]-p[0])**2 + (self.X[1,k]-p[1])**2 + self.slack > r**2)
|
||||
# keep inside track
|
||||
# TODO
|
||||
# track_ids = track.inner.keys()
|
||||
# a = track.outer[track_ids[0]]
|
||||
# b = track.outer[track_ids[1]]
|
||||
# c = track.outer[track_ids[2]]
|
||||
# d = track.outer[track_ids[3]]
|
||||
#
|
||||
# for k in range(1, self.N):
|
||||
# self.opti.subject_to(self.opti.subject_to(self.X))
|
||||
|
||||
posx = self.X[0, :]
|
||||
posy = self.X[1, :]
|
||||
angle = self.X[2, :]
|
||||
|
|
|
|||
|
|
@ -32,12 +32,28 @@ from collections import OrderedDict
|
|||
|
||||
from argparse import ArgumentParser
|
||||
|
||||
MSGLEN = 32
|
||||
def myreceive(sock):
|
||||
chunks = []
|
||||
bytes_recd = 0
|
||||
while bytes_recd < MSGLEN:
|
||||
chunk = sock.recv(min(MSGLEN - bytes_recd, 2048))
|
||||
if chunk == b'':
|
||||
raise RuntimeError("socket connection broken")
|
||||
chunks.append(chunk)
|
||||
bytes_recd = bytes_recd + len(chunk)
|
||||
|
||||
if chunk[-1] == '\n':
|
||||
break
|
||||
|
||||
return b''.join(chunks)
|
||||
|
||||
|
||||
class Robot:
|
||||
def __init__(self, id, ip):
|
||||
self.pos = None
|
||||
self.orient = None
|
||||
self.grid_pos = (0,0,0)
|
||||
|
||||
self.id = id
|
||||
|
||||
|
|
@ -46,63 +62,6 @@ class Robot:
|
|||
|
||||
self.ip = ip
|
||||
|
||||
class Obstacle:
|
||||
def __init__(self, id, radius):
|
||||
self.id = id
|
||||
self.pos = None
|
||||
self.radius = radius
|
||||
|
||||
class Track:
|
||||
def __init__(self):
|
||||
# ids in clockwise direction
|
||||
self.inner = OrderedDict()
|
||||
self.inner[0] = None
|
||||
self.inner[1] = None
|
||||
self.inner[2] = None
|
||||
self.inner[3] = None
|
||||
|
||||
self.outer = OrderedDict()
|
||||
self.outer[4] = None
|
||||
self.outer[5] = None
|
||||
self.outer[6] = None
|
||||
self.outer[7] = None
|
||||
|
||||
self.track_complete = False
|
||||
|
||||
self.inner_poly = None
|
||||
self.outer_poly = None
|
||||
|
||||
def set_id(self, d):
|
||||
if not self.track_complete:
|
||||
if d.id in self.inner:
|
||||
print("Detected marker {} at pos {}".format(d.id, (d.x,d.y)))
|
||||
self.inner[d.id] = (d.x, d.y)
|
||||
elif d.id in self.outer:
|
||||
print("Detected marker {} at pos {}".format(d.id, (d.x, d.y)))
|
||||
self.outer[d.id] = (d.x, d.y)
|
||||
else:
|
||||
print("Unknown marker!")
|
||||
else:
|
||||
return
|
||||
|
||||
if not None in self.inner.values() and not None in self.outer.values():
|
||||
print("Track marker positions detected!")
|
||||
self.track_complete = True
|
||||
|
||||
self.inner_poly = Polygon(self.inner.values())
|
||||
self.outer_poly = Polygon(self.outer.values())
|
||||
|
||||
def plot_track(self):
|
||||
if self.track_complete:
|
||||
plt.figure(2)
|
||||
x_in, y_in = self.inner_poly.exterior.xy
|
||||
x_out, y_out = self.outer_poly.exterior.xy
|
||||
plt.plot(x_in, y_in)
|
||||
plt.plot(x_out, y_out)
|
||||
else:
|
||||
print("plot is not complete yet!")
|
||||
|
||||
|
||||
def f_ode(t, x, u):
|
||||
# dynamical model of the two-wheeled robot
|
||||
# TODO: find exact values for these parameters
|
||||
|
|
@ -138,8 +97,7 @@ class RemoteController:
|
|||
for r in self.robots:
|
||||
self.robot_ids[r.id] = r
|
||||
|
||||
|
||||
self.track = Track()
|
||||
self.valid_cmds = ['forward', 'backward', 'turn left', 'turn right']
|
||||
|
||||
# connect to robot
|
||||
self.rc_socket = socket.socket()
|
||||
|
|
@ -151,6 +109,13 @@ class RemoteController:
|
|||
print("could not connect to socket")
|
||||
sys.exit(1)
|
||||
|
||||
# socket for movement commands
|
||||
self.comm_socket = socket.socket()
|
||||
self.comm_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
||||
|
||||
#self.comm_socket.bind((socket.gethostname(), 1337))
|
||||
self.comm_socket.bind(('', 1337))
|
||||
self.comm_socket.listen(5)
|
||||
|
||||
self.t = time.time()
|
||||
|
||||
|
|
@ -222,6 +187,7 @@ class RemoteController:
|
|||
# integrator
|
||||
self.r = scipy.integrate.ode(f_ode)
|
||||
self.omega_max = 5.0
|
||||
self.control_scaling = 0.2
|
||||
#self.omega_max = 13.32
|
||||
|
||||
|
||||
|
|
@ -341,132 +307,114 @@ class RemoteController:
|
|||
finally:
|
||||
self.mutex.release()
|
||||
|
||||
# detect track
|
||||
if data.id in self.track.inner.keys() or data.id in self.track.outer.keys():
|
||||
self.track.set_id(data)
|
||||
|
||||
def controller(self):
|
||||
print("starting control")
|
||||
|
||||
targets = {}
|
||||
markers_in = self.track.inner.values()
|
||||
markers_out = self.track.outer.values()
|
||||
running = True
|
||||
while running:
|
||||
(clientsocket, address) = self.comm_socket.accept()
|
||||
|
||||
# find targets:
|
||||
# generate waypoints
|
||||
# lamb = 0.5
|
||||
# j = 0
|
||||
# for i in range(0,4):
|
||||
# p = np.array(markers_in[i]) + lamb * (np.array(markers_out[i]) - np.array(markers_in[i]))
|
||||
# targets[j] = (p[0],p[1], 0.0)
|
||||
# j += 1
|
||||
# if i < 3:
|
||||
# mean_in = (np.array(markers_in[i]) + np.array(markers_in[i+1])) * 0.5
|
||||
# mean_out = (np.array(markers_out[i]) + np.array(markers_out[i+1])) * 0.5
|
||||
# mean = mean_in + (1.0 - lamb) * (mean_out - mean_in)
|
||||
# targets[j] = (mean[0], mean[1], 0.0)
|
||||
# j += 1
|
||||
connected = True
|
||||
while connected:
|
||||
try:
|
||||
data = myreceive(clientsocket)
|
||||
print(data)
|
||||
|
||||
try:
|
||||
robot_id, cmd = data.split(',')
|
||||
robot_id = int(robot_id)
|
||||
cmd = cmd.strip()
|
||||
|
||||
# final connection between first and last marker
|
||||
#mean_in = (np.array(markers_in[3]) + np.array(markers_in[0])) * 0.5
|
||||
#mean_out = (np.array(markers_out[3]) + np.array(markers_out[0])) * 0.5
|
||||
#mean = mean_in + (1.0 - lamb) * (mean_out - mean_in)
|
||||
#targets[j] = (mean[0], mean[1], 0.0)
|
||||
if robot_id in self.robot_ids and cmd in self.valid_cmds:
|
||||
self.mpc_control(robot_id, cmd)
|
||||
elif cmd == 'quit':
|
||||
clientsocket.close()
|
||||
self.comm_socket.close()
|
||||
connected = False
|
||||
running = False
|
||||
else:
|
||||
print("invalid command or robot id!")
|
||||
except ValueError:
|
||||
print("could not process command!")
|
||||
|
||||
grid_pos = (0,0, 0)
|
||||
target_pos = np.array((0.0, 0.0, 0.0))
|
||||
except RuntimeError:
|
||||
print("disconnected")
|
||||
connected = False
|
||||
clientsocket.close()
|
||||
|
||||
auto_control = False
|
||||
current_target = 0
|
||||
|
||||
control_scaling = 0.4
|
||||
def mpc_control(self, robot_id, cmd):
|
||||
robot = self.robot_ids[robot_id] # get robot to be controlled
|
||||
grid_pos = robot.grid_pos # grid position of the robot
|
||||
|
||||
self.pid = False
|
||||
self.mpc = True
|
||||
# compute new grid position and orientation
|
||||
if cmd == 'forward':
|
||||
new_x = grid_pos[0] + 1 * np.cos(grid_pos[2])
|
||||
new_y = grid_pos[1] + 1 * np.sin(grid_pos[2])
|
||||
new_angle = grid_pos[2]
|
||||
elif cmd == 'backward':
|
||||
new_x = grid_pos[0] - 1 * np.cos(grid_pos[2])
|
||||
new_y = grid_pos[1] - 1 * np.sin(grid_pos[2])
|
||||
new_angle = grid_pos[2]
|
||||
elif cmd == 'turn left':
|
||||
new_x = grid_pos[0]
|
||||
new_y = grid_pos[1]
|
||||
new_angle = np.unwrap([0, grid_pos[2] + np.pi / 2])[1]
|
||||
elif cmd == 'turn right':
|
||||
new_x = grid_pos[0]
|
||||
new_y = grid_pos[1]
|
||||
new_angle = np.unwrap([0, grid_pos[2] - np.pi / 2])[1]
|
||||
else:
|
||||
print("unknown command!")
|
||||
sys.exit(1)
|
||||
|
||||
integ = 0.0
|
||||
grid_pos = (new_x, new_y, new_angle)
|
||||
print("new grid pos for robot {}: {}".format(robot_id, grid_pos))
|
||||
|
||||
while True:
|
||||
# open loop controller
|
||||
events = pygame.event.get()
|
||||
self.target = np.array((0.25 * grid_pos[0], 0.25 * grid_pos[1], grid_pos[2]))
|
||||
|
||||
move = 0
|
||||
turn = 0
|
||||
self.pid = False
|
||||
self.mpc = True
|
||||
|
||||
for event in events:
|
||||
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
|
||||
if self.rc_socket:
|
||||
self.rc_socket.send('(0.0,0.0)\n')
|
||||
elif event.key == pygame.K_0:
|
||||
grid_pos = (0,0, 0)
|
||||
elif event.key == pygame.K_w:
|
||||
move = 1
|
||||
turn = 0
|
||||
elif event.key == pygame.K_s:
|
||||
move = -1
|
||||
turn = 0
|
||||
elif event.key == pygame.K_a:
|
||||
turn = 1
|
||||
move = 0
|
||||
integ = 0
|
||||
elif event.key == pygame.K_d:
|
||||
turn = -1
|
||||
move = 0
|
||||
elif event.key == pygame.K_r:
|
||||
turn = 2
|
||||
elif event.key == pygame.K_p:
|
||||
self.pid = True
|
||||
elif event.key == pygame.K_SPACE:
|
||||
auto_control = not auto_control
|
||||
if auto_control:
|
||||
self.target = targets[current_target]
|
||||
elif event.key == pygame.K_PLUS:
|
||||
control_scaling += 0.1
|
||||
control_scaling = min(control_scaling, 1.0)
|
||||
elif event.key == pygame.K_MINUS:
|
||||
control_scaling -= 0.1
|
||||
control_scaling = max(control_scaling, 0.3)
|
||||
elif event.key == pygame.K_ESCAPE:
|
||||
print("quit!")
|
||||
self.controlling = False
|
||||
if self.rc_socket:
|
||||
self.rc_socket.send('(0.0,0.0)\n')
|
||||
self.anim_stopped = True
|
||||
return
|
||||
elif event.key == pygame.QUIT:
|
||||
print("quit!")
|
||||
self.controlling = False
|
||||
if self.rc_socket:
|
||||
self.rc_socket.send('(0.0,0.0)\n')
|
||||
self.anim_stopped = True
|
||||
return
|
||||
near_target = 0
|
||||
|
||||
# compute new grid position and orientation
|
||||
while near_target < 5:
|
||||
# open loop controller
|
||||
events = pygame.event.get()
|
||||
|
||||
if move != 0:
|
||||
new_x = grid_pos[0] + move * np.cos(grid_pos[2])
|
||||
new_y = grid_pos[1] + move * np.sin(grid_pos[2])
|
||||
new_angle = grid_pos[2]
|
||||
grid_pos = (new_x, new_y, new_angle)
|
||||
for event in events:
|
||||
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
|
||||
if self.rc_socket:
|
||||
self.rc_socket.send('(0.0,0.0)\n')
|
||||
elif event.key == pygame.K_0:
|
||||
self.target = np.array([0,0,0])
|
||||
elif event.key == pygame.K_PLUS:
|
||||
self.control_scaling += 0.1
|
||||
self.control_scaling = min(self.control_scaling, 1.0)
|
||||
print("control scaling = ", self.control_scaling)
|
||||
elif event.key == pygame.K_MINUS:
|
||||
self.control_scaling -= 0.1
|
||||
self.control_scaling = max(self.control_scaling, 0.1)
|
||||
print("control scaling = ", self.control_scaling)
|
||||
elif event.key == pygame.K_ESCAPE:
|
||||
print("quit!")
|
||||
self.controlling = False
|
||||
if self.rc_socket:
|
||||
self.rc_socket.send('(0.0,0.0)\n')
|
||||
self.anim_stopped = True
|
||||
return
|
||||
elif event.key == pygame.QUIT:
|
||||
print("quit!")
|
||||
self.controlling = False
|
||||
if self.rc_socket:
|
||||
self.rc_socket.send('(0.0,0.0)\n')
|
||||
self.anim_stopped = True
|
||||
return
|
||||
|
||||
print(grid_pos)
|
||||
elif turn != 0:
|
||||
new_x = grid_pos[0]
|
||||
new_y = grid_pos[1]
|
||||
new_angle = np.unwrap([0, grid_pos[2] + turn * np.pi/2])[1]
|
||||
grid_pos = (new_x, new_y, new_angle)
|
||||
|
||||
print(grid_pos)
|
||||
|
||||
self.target = np.array((0.25*grid_pos[0], 0.25*grid_pos[1], grid_pos[2]))
|
||||
|
||||
if self.controlling:
|
||||
if self.mpc:
|
||||
x_pred = self.get_measurement_prediction()
|
||||
|
||||
|
|
@ -478,15 +426,14 @@ class RemoteController:
|
|||
#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:
|
||||
if error_pos > 0.1 or error_ang > 0.35:
|
||||
# solve mpc open loop problem
|
||||
res = self.ols.solve(x_pred, self.target, [], turning)
|
||||
res = self.ols.solve(x_pred, self.target)
|
||||
|
||||
#us1 = res[0]
|
||||
#us2 = res[1]
|
||||
us1 = res[0] * control_scaling
|
||||
us2 = res[1] * control_scaling
|
||||
us1 = res[0] * self.control_scaling
|
||||
us2 = res[1] * self.control_scaling
|
||||
#print("u = {}", (us1, us2))
|
||||
|
||||
# save open loop trajectories for plotting
|
||||
|
|
@ -507,6 +454,9 @@ class RemoteController:
|
|||
us1 = [0] * self.mstep
|
||||
us2 = [0] * self.mstep
|
||||
|
||||
near_target += 1
|
||||
robot.grid_pos = grid_pos
|
||||
|
||||
# send controls to the robot
|
||||
for i in range(0, self.mstep): # option to use multistep mpc if len(range) > 1
|
||||
u1 = us1[i]
|
||||
|
|
@ -516,36 +466,6 @@ class RemoteController:
|
|||
if i < self.mstep:
|
||||
time.sleep(self.dt)
|
||||
self.t = time.time() # save time the most recent control was applied
|
||||
else:
|
||||
if self.pid:
|
||||
x_pred = self.get_measurement()
|
||||
|
||||
# compute angle difference
|
||||
d_angle = x_pred[2] - self.target[2]
|
||||
dt = time.time() - self.t
|
||||
integ += d_angle * dt
|
||||
|
||||
#print(d_angle)
|
||||
|
||||
K = 0.2
|
||||
I = 0.15
|
||||
pp = d_angle * K
|
||||
ii = integ * I
|
||||
u1 = pp + ii
|
||||
u2 = -u1
|
||||
|
||||
print("e = {}, dt = {}, P = {}, I = {}, u = {}".format(d_angle, dt, pp, ii, (u1,u2)))
|
||||
|
||||
self.t = time.time()
|
||||
|
||||
self.rc_socket.send('({},{})\n'.format(u1, u2))
|
||||
time.sleep(0.025)
|
||||
#self.rc_socket.send('({},{})\n'.format(0, 0))
|
||||
#time.sleep(0.1)
|
||||
|
||||
|
||||
|
||||
#self.pid = False
|
||||
|
||||
|
||||
def get_measurement_prediction(self):
|
||||
|
|
|
|||
Loading…
Reference in New Issue