diff --git a/remote_control/casadi_opt.py b/remote_control/casadi_opt.py index cf13a3c..8c74b55 100644 --- a/remote_control/casadi_opt.py +++ b/remote_control/casadi_opt.py @@ -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, :] diff --git a/remote_control/roborally.py b/remote_control/roborally.py index 15d32f5..def4c70 100644 --- a/remote_control/roborally.py +++ b/remote_control/roborally.py @@ -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):