parameter tuning. works well with control scaling of 0.3

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, :]

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.valid_cmds = ['forward', 'backward', 'turn left', 'turn right']
-        self.track = Track()
 
         # 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 = {}
+        running = True
-        markers_in = self.track.inner.values()
+        while running:
-        markers_out = self.track.outer.values()
+            (clientsocket, address) = self.comm_socket.accept()
 
-        # find targets:
+            connected = True
-        # generate waypoints
+            while connected:
-        # lamb = 0.5
+                try:
-        # j = 0
+                    data = myreceive(clientsocket)
-        # for i in range(0,4):
+                    print(data)
-        #     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
 
+                    try:
+                        robot_id, cmd = data.split(',')
+                        robot_id = int(robot_id)
+                        cmd = cmd.strip()
 
-        # final connection between first and last marker
+                        if robot_id in self.robot_ids and cmd in self.valid_cmds:
-        #mean_in = (np.array(markers_in[3]) + np.array(markers_in[0])) * 0.5
+                            self.mpc_control(robot_id, cmd)
-        #mean_out = (np.array(markers_out[3]) + np.array(markers_out[0])) * 0.5
+                        elif cmd == 'quit':
-        #mean = mean_in + (1.0 - lamb) * (mean_out - mean_in)
+                            clientsocket.close()
-        #targets[j] = (mean[0], mean[1], 0.0)
+                            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)
+                except RuntimeError:
-        target_pos = np.array((0.0, 0.0, 0.0))
+                    print("disconnected")
+                    connected = False
+                    clientsocket.close()
 
-        auto_control = False
+    def mpc_control(self, robot_id, cmd):
-        current_target = 0
+            robot = self.robot_ids[robot_id] # get robot to be controlled
-                
+            grid_pos = robot.grid_pos   # grid position of the robot
-        control_scaling = 0.4
 
-        self.pid = False
+            # compute new grid position and orientation
-        self.mpc = True
+            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:
+            self.target = np.array((0.25 * grid_pos[0], 0.25 * grid_pos[1], grid_pos[2]))
-            # open loop controller
-            events = pygame.event.get()
 
-            move = 0
+            self.pid = False
-            turn = 0
+            self.mpc = True
 
-            for event in events:
+            near_target = 0
-                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
 
-                    # compute new grid position and orientation
+            while near_target < 5:
+                # open loop controller
+                events = pygame.event.get()
 
-                    if move != 0:
+                for event in events:
-                        new_x = grid_pos[0] + move * np.cos(grid_pos[2])
+                    if event.type == pygame.KEYDOWN:
-                        new_y = grid_pos[1] + move * np.sin(grid_pos[2])
+                        if event.key == pygame.K_UP:
-                        new_angle = grid_pos[2]
+                            self.controlling = True
-                        grid_pos = (new_x, new_y, new_angle)
+                            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.35:
-                    if error_pos > 0.1 or error_ang > 0.4:
                         # 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
+                        us1 = res[0] * self.control_scaling
-                        us2 = res[1] * 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):