measure postprocessing time

micropython_firmware/l293motor.py

@@ -0,0 +1,29 @@
+import machine
+
+class Motor:
+    def __init__(self, enable, dir1, dir2):
+        self.enable_pin = machine.Pin(enable, machine.Pin.OUT)
+        self.enable_pwm = machine.PWM(self.enable_pin)
+        self.enable_pwm.freq(250)
+
+        self.dir1_pin = machine.Pin(dir1, machine.Pin.OUT)
+        self.dir2_pin = machine.Pin(dir2, machine.Pin.OUT)
+
+        self.direction = 1 # default direction: dir1_pin = HIGH, dir2_pin = LOW
+        self.reverse()
+
+    def reverse(self):
+        self.direction = not self.direction
+        self.dir1_pin.value(self.direction)
+        self.dir2_pin.value(not self.direction)
+
+    def speed(self, value):
+        if value > 0: # forward
+            if not self.direction: # switch direction if necessary
+                self.reverse()
+        else: # backward
+            if self.direction: # switch direction if necessary
+                self.reverse()
+        # apply value as pwm signal
+        self.enable_pwm.duty(int(abs(value)*10.23))
+

micropython_firmware/main.py

@@ -2,43 +2,46 @@ import machine
 import sys
 from machine import I2C, Pin
 
-import d1motor
+i2c = False
+if i2c:
+    import d1motor
+else:
+    import l293motor
 
-import utime
+import time
 import usocket
-import uselect
 import esp
 
 class Robot:
     def __init__(self):
-        print("setting up I2C ...")
-        d1 = Pin(5)
-        d2 = Pin(4)
-        i2c = I2C(scl=d1, sda=d2)
-        i2c_scan = i2c.scan()
-        if len(i2c_scan) > 0:
-            i2c_addr = i2c_scan[0]
-            print("i2c scan = {}".format(i2c_addr))
-            print("setting up motors ...")
-            self.m1 = d1motor.Motor(0, i2c, address=i2c_addr)
-            self.m2 = d1motor.Motor(1, i2c, address=i2c_addr)
-            self.m1.speed(0)
-            self.m2.speed(0)
-            print("motor setup complete!")
+        if i2c:
+            print("setting up I2C ...")
+            #d1 = Pin(5)
+            #d2 = Pin(4)
+            #i2c = I2C(scl=d1, sda=d2)
+            #i2c_scan = i2c.scan()
+            i2c_scan = []
+            if len(i2c_scan) > 0:
+                i2c_addr = i2c_scan[0]
+                print("i2c scan = {}".format(i2c_addr))
+                print("setting up motors ...")
+                self.m1 = d1motor.Motor(0, i2c, address=i2c_addr)
+                self.m2 = d1motor.Motor(1, i2c, address=i2c_addr)
+                self.m1.speed(0)
+                self.m2.speed(0)
+                print("motor setup complete!")
+            else:
+                print("error: no i2c interfaces found!")
+                sys.exit(1) 
         else:
-            print("error: no i2c interfaces found!")
-            sys.exit(1) 
+            print("setting up L293D motor")
+            self.m1 = l293motor.Motor(14, 13, 12)
+            self.m2 = l293motor.Motor(5, 0, 4)
 
         ip = my_ip[0]
         # setup socket for remote control 
         self.addr = usocket.getaddrinfo(ip, 1234)[0][-1]
 
-        self.poller = uselect.poll()
-        self.poller_timeout = 2 # timeout in ms
-
-        self.control_queue = []
-
-
     def remote_control(self):
         while True:
             print("setting up socket communication ...")
@@ -52,124 +55,51 @@ class Robot:
                     socket_setup_complete = True
                 except Exception as e:
                     print("could not create socket. error msg: {}\nwaiting 1 sec and retrying...".format(e))
-                    utime.sleep(1.0)
+                    time.sleep(1.0)
 
             print("waiting for connections on {} ...".format(self.addr))
             socket.listen(1)
             res = socket.accept() # this blocks until someone connects to the socket
             comm_socket = res[0]
-            self.poller.register(comm_socket, uselect.POLLIN)
             print("connected!")
             listening = True
-
-            duration_current = 0 
-            t_current = utime.ticks_ms()
-            duration_next = None
-            stopped = True
-            timeouts = 0 
-            while listening:
-                 elapsed = utime.ticks_ms() 
-                 remaining = duration_current - (elapsed-t_current)
-                 if remaining >= 0:
-                     timeouts = 0
-                     print("start of loop\n I have {} ms until next control needs to be applied".format(remaining))
-                 else:
-                     # current control timed out -> applying next control
-                     if len(self.control_queue) > 0:
-                         print("previous control applied for {} ms too long".format(elapsed - t_current - duration_current))
-                         u_next = self.control_queue.pop(0)
-                         #print("duration of previous control = {}".format((elapsed - t_current)/1000.0))
-                         #print("applying new control (duration, u1, u2) = ({}, {}, {})".format(duration_next, u1_next, u2_next))
-                         # if so, apply it
-                         self.m1.speed(u_next[0])
-                         self.m2.speed(u_next[1])
-
-                         t_current = utime.ticks_ms()
-                         duration_current = duration_next
-
-                         stopped = False
-                     elif not stopped: 
-                         print("previous control applied for {} ms too long".format(elapsed - t_current - duration_current))
-                         #print("duration of previous control = {}".format((elapsed - t_current)/1000.0))
-                         # no new control available -> shutdown
-                         print("no new control available -> stopping")
-
-                         self.m1.speed(0)
-                         self.m2.speed(0)
-                         
-                         t_current = utime.ticks_ms()
-                         duration_current = 0 # as soon as new control will become available we directly want to apply it immediately
-
-                         stopped = True
-
-                 #elif timeouts < 10:
-                 #    print("start of loop\n I have {} ms until next control needs to be applied, timeouts = {}".format(remaining, timeouts))
-                 #    timeouts = timeouts + 1 
-                 
-                 trecv_start = utime.ticks_ms()
-
-                 # expected data: '(t, u1_0, u2_0, u1_1, u2_1, ...)'\n"
-                 # where ui = control for motor i
-                 # ui \in [-1.0, 1.0]
-                 #print("poller waiting..")
-                 poll_res = self.poller.poll(self.poller_timeout) # wait 100 milliseconds for socket data
-                 if poll_res:
-                     print("new data available")
-                     try:
+            try:
+                while listening:
+                     # expected data: '(t, u1, u2)'\n"
+                     # where ui = control for motor i
+                     # ui \in [-1.0, 1.0]
                          data = comm_socket.readline()
                          data_str = data.decode()
-                         #print("Data received: {}".format(data_str))
-                         #print("processing data = {}".format(data_str))
+                         print("Data received: {}".format(data_str))
+                         print("processing data = {}".format(data_str))
                          l = data_str.strip('()\n').split(',')
-                         #print("l = {}".format(l))
-                         duration_next = int(float(l[0])*1000)
-                         #print("duration = {}".format(duration_next))
-                         self.control_queue = []
-                         print("putting data into queue")
-                         for i in range((len(l)-1)/2):
-                             u1_next = int(float(l[2*i+1])*100)
-                             print("u1 = {}".format(u1_next))
-                             u2_next = int(float(l[2*i+2])*100)
-                             print("u2 = {}".format(u2_next))
-                             self.control_queue.append((u1_next, u2_next))
-                     except ValueError:
-                         print("ValueError: Data has wrong format.")
-                         print("Data received: {}".format(data_str))
-                         print("Shutting down ...")
-                         self.control_queue = []
-                         duration_current = 0
-                         listening = False
-                         comm_socket.close()
-                         socket.close()
-                         del comm_socket
-                         del socket
-                         print("disconnected!")
-                     except IndexError:
-                         print("IndexError: Data has wrong format.")
-                         print("Data received: {}".format(data_str))
-                         print("Shutting down ...")
-                         self.control_queue = []
-                         duration_current = 0
-                         listening = False
-                         comm_socket.close()
-                         socket.close()
-                         del comm_socket
-                         del socket
-                         print("disconnected!")
-                     except Exception as e:
-                         print("Some other error occured")
-                         print("Exception: {}".format(e))
-                         print("Shutting down ...")
-                         self.control_queue = []
-                         duration_current = 0
-                         listening = False
-                         comm_socket.close()
-                         socket.close()
-                         del comm_socket
-                         del socket
-                         print("disconnected!")
-                     trecv_end = utime.ticks_ms()
-                     print("communication (incl. polling) took {} ms".format(trecv_end - trecv_start))
+                         print("l = {}".format(l))
+                         u1 = int(float(l[0])*100)
+                         print("u1 = {}".format(u1))
+                         u2 = int(float(l[1])*100)
+                         print("u2 = {}".format(u2))
+                         
+                         self.m1.speed(u1)
+                         self.m2.speed(u2)
+            except ValueError:
+                print("ValueError: Data has wrong format.")
+                print("Data received: {}".format(data_str))
+            except IndexError:
+                print("IndexError: Data has wrong format.")
+                print("Data received: {}".format(data_str))
+            except Exception as e:
+                print("Some other error occured")
+                print("Exception: {}".format(e))
+            finally:    
+                print("Shutting down ...")
+                u1 = u2 = 0
+                self.m1.speed(u1)
+                self.m2.speed(u2)
+                listening = False
+                comm_socket.close()
+                socket.close()
+                del comm_socket
+                del socket
 
 wall_e = Robot()
 wall_e.remote_control()

remote_control/casadi_opt.py

@@ -4,13 +4,15 @@ import time
 # look at: https://github.com/casadi/casadi/blob/master/docs/examples/python/vdp_indirect_multiple_shooting.py
 
 class OpenLoopSolver:
-    def __init__(self, N=10, T=2.0):
+    def __init__(self, N=20, T=4.0):
         self.T = T
         self.N = N
 
         self.opti_x0 = None
         self.opti_lam_g0 = None
 
+        self.use_warmstart = True
+
     def setup(self):
         x = SX.sym('x')
         y = SX.sym('y')
@@ -135,6 +137,9 @@ class OpenLoopSolver:
         #plt.show()
         #return
 
+
+    def solve(self, x0, target, obstacles):
+        tstart = time.time()
         # alternative solution using multiple shooting (way faster!)
         self.opti = Opti() # Optimization problem
 
@@ -143,6 +148,8 @@ class OpenLoopSolver:
         self.Q = self.opti.variable(1,self.N+1) # state trajectory
 
         self.U = self.opti.variable(2,self.N)   # control trajectory (throttle)
+
+        self.slack = self.opti.variable(1,1)
         #T = self.opti.variable()      # final time
 
         # ---- objective          ---------
@@ -157,10 +164,6 @@ class OpenLoopSolver:
         #self.opti.set_initial(speed, 1)
         #self.opti.set_initial(T, 1)
 
-
-    def solve(self, x0, target):
-
-        tstart = time.time()
         x = SX.sym('x')
         y = SX.sym('y')
         theta = SX.sym('theta')
@@ -205,12 +208,12 @@ class OpenLoopSolver:
             q_next = self.Q[:, k] + dt / 6 * (k1_q + 2 * k2_q + 2 * k3_q + k4_q)
             self.opti.subject_to(self.X[:, k + 1] == x_next)  # close the gaps
             self.opti.subject_to(self.Q[:, k + 1] == q_next)  # close the gaps
-        self.opti.minimize(self.Q[:, self.N])
+        self.opti.minimize(self.Q[:, self.N] + 1.0e5 * self.slack**2)
 
         # ---- path constraints -----------
         # limit = lambda pos: 1-sin(2*pi*pos)/2
         # self.opti.subject_to(speed<=limit(pos))   # track speed limit
-        maxcontrol = 0.5
+        maxcontrol = 0.95
         self.opti.subject_to(self.opti.bounded(-maxcontrol, self.U, maxcontrol))  # control is limited
 
         # ---- boundary conditions --------
@@ -227,10 +230,12 @@ class OpenLoopSolver:
         # self.opti.subject_to(X[2,:]>=-2) # Time must be positive
 
         # avoid obstacle
-        # r = 0.25
-        # p = (0.5, 0.5)
-        # for k in range(self.N):
-        #    self.opti.subject_to((X[0,k]-p[0])**2 + (X[1,k]-p[1])**2  > r**2)
+        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)
         #    pass
         posx = self.X[0, :]
         posy = self.X[1, :]
@@ -241,13 +246,17 @@ 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))
 
-        if self.opti_x0 is not None:
+        tstart = time.time()
+        if self.use_warmstart and self.opti_x0 is not None:
             self.opti.set_initial(self.opti.lam_g, self.opti_lam_g0)
             self.opti.set_initial(self.opti.x, self.opti_x0)
         sol = self.opti.solve()   # actual solve
+        tend = time.time()
+        print("solving the problem took {} seconds".format(tend - tstart))
 
+        tstart = time.time()
         self.opti_x0 = sol.value(self.opti.x)
         self.opti_lam_g0 = sol.value(self.opti.lam_g)
 
@@ -255,8 +264,10 @@ class OpenLoopSolver:
         #u_opt_2 = map(lambda x: float(x), [u_opt[i * 2 + 1] for i in range(0, 60)])
         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))
 
-        return (u_opt_1, u_opt_2)
+        return (u_opt_1, u_opt_2, sol.value(posx), sol.value(posy))
 
         #lam_g0 = sol.value(self.opti.lam_g)
 

remote_control/class_control_joystick.py

@@ -0,0 +1,150 @@
+import socket
+import pygame
+import pygame.joystick
+
+
+class robot:  # we have 4 arg for this class, because joysticks get the same (value, axis) events
+    def __init__(self, joy, ip, port):
+        self.joy = joy
+        self.ip = ip
+        self.port = port
+        self.robot0_stopped_1 = True
+        self.robot0_stopped_2 = True
+        self.robot0_stopped_3 = True
+        self.robot0_stopped_4 = True
+        self.rc_socket = socket.socket()
+        try:
+            self.rc_socket.connect((self.ip, self.port))
+        except socket.error():
+            print("couldn't connect to socket")
+
+    def joystick_init(self):  # Joystick's initialisation
+        self.joystick = pygame.joystick.Joystick(self.joy)
+        self.joystick.init()
+        self.axes = self.joystick.get_numaxes()
+
+    def control(self, event):  # the control of two robots with joysticks
+        joy = event.joy
+        value = event.value
+        axis = event.axis
+
+        if joy == self.joy:
+            if axis == 1:
+                if abs(value) > 0.2:
+                    u1 = u2 = -value
+                    self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+                    self.robot0_stopped_1 = False
+
+                elif not self.robot0_stopped_1:
+                    u1 = u2 = 0
+                    self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+                    self.robot0_stopped_1 = True
+            elif axis == 3:
+                if abs(value) > 0.2:
+                    u1 = value
+                    u2 = -value
+                    self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+                    self.robot0_stopped_2 = False
+
+                elif not self.robot0_stopped_2:
+                    u1 = u2 = 0
+                    self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+                    self.robot0_stopped_2 = True
+            elif axis == 2:
+                if value > 0.2:
+                    u1 = value/1.9
+                    u2 = value/1.2
+                    self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+                    self.robot0_stopped_3 = False
+
+                elif not self.robot0_stopped_3:
+                    u1 = u2 = 0
+                    self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+                    self.robot0_stopped_3 = True
+            elif axis == 5:
+                if value > 0.2:
+                    u1 = value/1.2
+                    u2 = value/1.9
+                    self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+                    self.robot0_stopped_4 = False
+
+                elif not self.robot0_stopped_4:
+                    u1 = u2 = 0
+                    self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+                    self.robot0_stopped_4 = True
+
+    def control_keyboard(self, event):  # keyboard control for robot1
+        command_received = False
+        if event.key == pygame.K_LEFT:
+            u1 = -1.0
+            u2 = 1.0
+            command_received = True
+        elif event.key == pygame.K_RIGHT:
+            u1 = 1.0
+            u2 = -1.0
+            command_received = True
+        elif event.key == pygame.K_UP:
+            u1 = -1.0
+            u2 = -1.0
+            command_received = True
+        elif event.key == pygame.K_DOWN:
+            u1 = 1.0
+            u2 = 1.0
+            command_received = True
+        if command_received:
+            self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+
+    def control_keyboard_2(self, event):  # keyboard control for robot2
+
+        command_received = False
+        if event.key == pygame.K_a:
+            u1 = -1.0
+            u2 = 1.0
+            command_received = True
+        elif event.key == pygame.K_d:
+            u1 = 1.0
+            u2 = -1.0
+            command_received = True
+        elif event.key == pygame.K_w:
+            u1 = -1.0
+            u2 = -1.0
+            command_received = True
+        elif event.key == pygame.K_s:
+            u1 = 1.0
+            u2 = 1.0
+            command_received = True
+        if command_received:
+            self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+
+    def control_keyboard_stop(self):  # stop for both robot
+
+        u1 = 0
+        u2 = 0
+        print("key released, resetting: ({},{})".format(u1, u2))
+        self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+
+def main():
+    pygame.init()
+    pygame.display.set_mode((640, 480))
+
+    robot_1 = robot(0, '192.168.1.102', 1234)
+    robot_1.joystick_init()
+    robot_2 = robot(1, '192.168.1.103', 1234)
+    robot_2.joystick_init()
+
+    while True:
+        events = pygame.event.get()
+        for event in events:
+            if event.type == pygame.JOYAXISMOTION:
+                robot_1.control(event)
+                robot_2.control(event)
+            elif event.type == pygame.KEYDOWN:
+                robot_1.control_keyboard(event)
+                robot_2.control_keyboard_2(event)
+            elif event.type == pygame.KEYUP:
+                robot_1.control_keyboard_stop()
+                robot_2.control_keyboard_stop()
+
+
+if __name__ == '__main__':
+    main()

remote_control/keyboard_controller.py

@@ -7,9 +7,9 @@ pygame.display.set_mode((640, 480))
 rc_socket = socket.socket()
 try:
     #rc_socket.connect(('192.168.4.1', 1234)) # connect to robot
-    rc_socket.connect(('192.168.1.101', 1234)) # connect to robot
+    #rc_socket.connect(('192.168.1.101', 1234)) # connect to robot
     #rc_socket.connect(('192.168.1.102', 1234)) # connect to robot
-    #rc_socket.connect(('192.168.1.103', 1234)) # connect to robot
+    rc_socket.connect(('192.168.1.103', 1234)) # connect to robot
 except socket.error:
     print("could not connect to socket")
 
@@ -17,7 +17,7 @@ except socket.error:
 while True:
     u1 = 0
     u2 = 0
-    vmax = 0.5
+    vmax = 1.0
     events = pygame.event.get()
     for event in events:
         if event.type == pygame.KEYDOWN:

remote_control/position_controller.py

@@ -18,6 +18,7 @@ from copy import deepcopy
 
 import matplotlib.pyplot as plt
 import matplotlib.animation as anim
+import matplotlib.patches as patch
 
 import time
 
@@ -37,6 +38,12 @@ class Robot:
 
         self.ip = ip
 
+class Obstacle:
+    def __init__(self, id, radius):
+        self.id = id
+        self.pos = None
+        self.radius = radius
+
 def f_ode(t, x, u):
     # dynamical model of the two-wheeled robot
     # TODO: find exact values for these parameters
@@ -62,19 +69,28 @@ def f_ode(t, x, u):
 class RemoteController:
     def __init__(self):
 
-        self.robots = [Robot(5)]
+        self.robots = [Robot(3, '192.168.1.103')]
 
         self.robot_ids = {}
         for r in self.robots:
             self.robot_ids[r.id] = r
 
+        obst = [Obstacle(6, 0.175), Obstacle(5, 0.175), Obstacle(8, 0.175)]
+
+        self.obstacles = {}
+        for r in obst:
+            self.obstacles[r.id] = r
+
         # connect to robot
         self.rc_socket = socket.socket()
+        #self.rc_socket = None
         try:
-            pass
-            self.rc_socket.connect(('192.168.1.103', 1234))  # connect to robot
+            for r in self.robots:
+                self.rc_socket.connect((r.ip, 1234))  # connect to robot
         except socket.error:
             print("could not connect to socket")
+            self.rc_socket = None
+
 
         self.t = time.time()
 
@@ -89,23 +105,19 @@ class RemoteController:
         self.tms = None
         self.xms = None
 
+        # variable for mpc open loop
+        self.ol_x = None
+        self.ol_y = None
+
         self.mutex = threading.Lock()
 
+        # ROS subscriber for detected markers
         marker_sub = rospy.Subscriber("/marker_id_pos_angle", id_pos_angle, self.measurement_callback)
 
-
         # pid parameters
-        self.k = 0
-        self.ii = 0.1
-        self.pp = 0.4
-
-        self.inc = 0.0
-
-        self.alphas = []
-
-        self.speed = 1.0
         self.controlling = False
 
+        # currently active control
         self.u1 = 0.0
         self.u2 = 0.0
 
@@ -113,19 +125,34 @@ class RemoteController:
         self.fig = plt.figure()
         self.ax = self.fig.add_subplot(1,1,1)
         self.xdata, self.ydata = [], []
-        self.line, = self.ax.plot([],[])
+        self.line, = self.ax.plot([],[], color='grey', linestyle=':')
         self.line_sim, = self.ax.plot([], [])
+        self.line_ol, = self.ax.plot([],[], color='green', linestyle='--')
         self.dirm, = self.ax.plot([], [])
         self.dirs, = self.ax.plot([], [])
+
+        self.circles = []
+        for o in self.obstacles:
+            self.circles.append(patch.Circle((0.0, 0.0), radius=0.1, fill=False, color='red', linestyle='--'))
+
+        for s in self.circles:
+            self.ax.add_artist(s)
+
         plt.xlabel('x-position')
         plt.ylabel('y-position')
         plt.grid()
 
         self.ols = OpenLoopSolver()
         self.ols.setup()
+        self.dt = self.ols.T / self.ols.N
 
         self.target = (0.0, 0.0, 0.0)
 
+        # integrator
+        self.r = scipy.integrate.ode(f_ode)
+        self.omega_max = 5.0
+
+
     def ani(self):
         self.ani = anim.FuncAnimation(self.fig, init_func=self.ani_init, func=self.ani_update, interval=10, blit=True)
         plt.ion()
@@ -136,7 +163,7 @@ class RemoteController:
         self.ax.set_ylim(-2, 2)
         self.ax.set_aspect('equal', adjustable='box')
 
-        return self.line, self.line_sim, self.dirm, self.dirs,
+        return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol, self.circles[0], self.circles[1],self.circles[2],
 
     def ani_update(self, frame):
         #print("plotting")
@@ -155,8 +182,8 @@ class RemoteController:
                     a = xm_local[-1, 0]
                     b = xm_local[-1, 1]
 
-                    a2 = a + np.cos(xm_local[-1, 2]) * 1.0
-                    b2 = b + np.sin(xm_local[-1, 2]) * 1.0
+                    a2 = a + np.cos(xm_local[-1, 2]) * 0.2
+                    b2 = b + np.sin(xm_local[-1, 2]) * 0.2
 
                     self.dirm.set_data(np.array([a, a2]), np.array([b, b2]))
 
@@ -171,26 +198,42 @@ class RemoteController:
                 a = xs_local[-1, 0]
                 b = xs_local[-1, 1]
 
-                a2 = a + np.cos(xs_local[-1, 2]) * 1.0
-                b2 = b + np.sin(xs_local[-1, 2]) * 1.0
+                a2 = a + np.cos(xs_local[-1, 2]) * 0.2
+                b2 = b + np.sin(xs_local[-1, 2]) * 0.2
 
                 self.dirs.set_data(np.array([a, a2]), np.array([b, b2]))
+
+            ol_x_local = deepcopy(self.ol_x)
+            ol_y_local = deepcopy(self.ol_y)
+
+            if ol_x_local is not None:
+                self.line_ol.set_data(ol_x_local, ol_y_local)
+            else:
+                self.line_ol.set_data([],[])
+
+            i = 0
+            obst_keys = self.obstacles.keys()
+            for s in self.circles:
+                o = self.obstacles[obst_keys[i]]
+                i = i + 1
+
+                if o.pos is not None:
+                    s.center = o.pos
+                    s.radius = o.radius
+
         finally:
             self.mutex.release()
 
-        return self.line, self.line_sim, self.dirm, self.dirs,
+        return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol, self.circles[0], self.circles[1],self.circles[2],
 
     def measurement_callback(self, data):
-        #print("data = {}".format(data))
+        # detect robots
         if data.id in self.robot_ids:
             r = self.robot_ids[data.id]
 
             r.pos = (data.x, data.y) # only x and y component are important for us
             r.euler = data.angle
 
-            #print("r.pos = {}".format(r.pos))
-            #print("r.angle = {}".format(r.euler))
-
             # save measured position and angle for plotting
             measurement = np.array([r.pos[0], r.pos[1], r.euler])
             if self.tms_0 is None:
@@ -211,290 +254,89 @@ class RemoteController:
                 finally:
                     self.mutex.release()
 
+        # detect obstacles
+        if data.id in self.obstacles.keys():
+            obst = (data.x, data.y)
+            self.obstacles[data.id].pos = obst
+
     def controller(self):
-        tgrid = None
-        us1 = None
-        us2 = None
-        u1 = -0.0
-        u2 = 0.0
-
-        r = scipy.integrate.ode(f_ode)
-
-        omega_max = 5.0
-
-        init_pos = None
-        init_time = None
-        final_pos = None
-        final_time = None
-        forward = True
         print("starting control")
         while True:
 
-            keyboard_control = False
-            keyboard_control_speed_test = False
-            pid = False
-            open_loop_solve = True
+            # open loop controller
+            events = pygame.event.get()
+            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 = (0.0, 0.0, 0.0)
+                    elif event.key == pygame.K_1:
+                        self.target = (0.5,0.5, -np.pi/2.0)
+                    elif event.key == pygame.K_2:
+                        self.target = (0.5, -0.5, 0.0)
+                    elif event.key == pygame.K_3:
+                        self.target = (-0.5,-0.5, np.pi/2.0)
+                    elif event.key == pygame.K_4:
+                        self.target = (-0.5,0.5, 0.0)
 
-            if keyboard_control: # keyboard controller
-                events = pygame.event.get()
-                speed = 1.0
-                for event in events:
-                    if event.type == pygame.KEYDOWN:
-                        if event.key == pygame.K_LEFT:
-                            self.u1 = -speed
-                            self.u2 = speed
-                            #print("turn left: ({},{})".format(u1, u2))
-                        elif event.key == pygame.K_RIGHT:
-                            self.u1 = speed
-                            self.u2 = -speed
-                            #print("turn right: ({},{})".format(u1, u2))
-                        elif event.key == pygame.K_UP:
-                            self.u1 = speed
-                            self.u2 = speed
-                            #print("forward: ({},{})".format(self.u1, self.u2))
-                        elif event.key == pygame.K_DOWN:
-                            self.u1 = -speed
-                            self.u2 = -speed
-                            #print("forward: ({},{})".format(u1, u2))
-                        self.rc_socket.send('({},{},{})\n'.format(0.1, self.u1, self.u2))
-                    elif event.type == pygame.KEYUP:
-                        self.u1 = 0
-                        self.u2 = 0
-                        #print("key released, resetting: ({},{})".format(u1, u2))
-                        self.rc_socket.send('({}, {},{})\n'.format(0.1, self.u1, self.u2))
+            if self.controlling:
+                x_pred = self.get_measurement_prediction()
 
-                tnew = time.time()
-                dt = tnew - self.t
-                r = scipy.integrate.ode(f_ode)
-                r.set_f_params(np.array([self.u1 * omega_max, self.u2 * omega_max]))
+                tmpc_start = time.time()
 
-                #print(self.x0)
-                if self.x0 is None:
-                    if self.xm_0 is not None:
-                        self.x0 = self.xm_0
-                        self.xs = self.x0
-                    else:
-                        print("error: no measurement available to initialize simulation")
-                x = self.x0
-                r.set_initial_value(x, self.t)
-                xnew = r.integrate(r.t + dt)
+                # solve mpc open loop problem
+                res = self.ols.solve(x_pred, self.target, self.obstacles)
 
-                self.t = tnew
-                self.x0 = xnew
+                us1 = res[0]
+                us2 = res[1]
 
+                # save open loop trajectories for plotting
                 self.mutex.acquire()
                 try:
-                    self.ts = np.append(self.ts, tnew)
-                    self.xs = np.vstack((self.xs, xnew))
+                    self.ol_x = res[2]
+                    self.ol_y = res[3]
                 finally:
                     self.mutex.release()
 
+                tmpc_end = time.time()
+                print("---------------- mpc solution took {} seconds".format(tmpc_end - tmpc_start))
+                dt_mpc = time.time() - self.t
+                if dt_mpc < self.dt: # wait until next control can be applied
+                    print("sleeping for {} seconds...".format(self.dt - dt_mpc))
+                    time.sleep(self.dt - dt_mpc)
 
-            elif keyboard_control_speed_test:
-                events = pygame.event.get()
-                for event in events:
-                    if event.type == pygame.KEYDOWN:
-                        if event.key == pygame.K_1:
-                            self.controlling = True
-                            forward = True
-                            print("starting test")
-                            self.mutex.acquire()
-                            try:
-                                init_pos = copy.deepcopy(self.xms[-1])
-                                init_time = copy.deepcopy(self.tms[-1])
-                            finally:
-                                self.mutex.release()
-                        if event.key == pygame.K_2:
-                            self.controlling = True
-                            forward = False
-                            print("starting test")
-                            self.mutex.acquire()
-                            try:
-                                init_pos = copy.deepcopy(self.xms[-1])
-                                init_time = copy.deepcopy(self.tms[-1])
-                            finally:
-                                self.mutex.release()
-                        elif event.key == pygame.K_3:
-                            self.controlling = False
-                            print("stopping test")
-                            self.rc_socket.send('(0.1, 0.0,0.0)\n')
+                # send controls to the robot
+                for i in range(0, 1): # option to use multistep mpc if len(range) > 1
+                    u1 = us1[i]
+                    u2 = us2[i]
+                    if self.rc_socket:
+                        self.rc_socket.send('({},{})\n'.format(u1, u2))
+                    self.t = time.time() # save time the most recent control was applied
 
-                            self.mutex.acquire()
-                            try:
-                                final_pos = copy.deepcopy(self.xms[-1])
-                                final_time = copy.deepcopy(self.tms[-1])
-                            finally:
-                                self.mutex.release()
+    def get_measurement_prediction(self):
+        # get measurement
+        self.mutex.acquire()
+        try:
+            last_measurement = copy.deepcopy(self.xms[-1])
+            last_time = copy.deepcopy(self.tms[-1])
+        finally:
+            self.mutex.release()
 
-                            print("init_pos = {}".format(init_pos))
-                            print("final_pos = {}".format(final_pos))
-                            print("distance = {}".format(np.linalg.norm(init_pos[0:2]-final_pos[0:2])))
-                            print("dt = {}".format(final_time - init_time))
+        # prediction of state at time the mpc will terminate
+        self.r.set_f_params(np.array([self.u1 * self.omega_max, self.u2 * self.omega_max]))
 
-                            d = np.linalg.norm(init_pos[0:2]-final_pos[0:2])
-                            t = final_time - init_time
-                            r = 0.03
+        self.r.set_initial_value(last_measurement, last_time)
 
-                            angular_velocity = d/r/t
-                            print("average angular velocity = {}".format(angular_velocity))
+        x_pred = self.r.integrate(self.r.t + self.dt)
 
+        return x_pred
 
-                if self.controlling:
-                    if forward:
-                        self.rc_socket.send('(0.1, 1.0,1.0)\n')
-                    else:
-                        self.rc_socket.send('(0.1, -1.0,-1.0)\n')
-                    time.sleep(0.1)
-                    #print("speed = {}".format(self.speed))
-
-
-            elif pid:
-                # pid controller
-
-                events = pygame.event.get()
-                for event in events:
-                    if event.type == pygame.KEYDOWN:
-                        if event.key == pygame.K_LEFT:
-                            self.ii = self.ii / np.sqrt(np.sqrt(np.sqrt(10.0)))
-                            print("ii = {}".format(self.pp))
-                        elif event.key == pygame.K_RIGHT:
-                            self.ii = self.ii * np.sqrt(np.sqrt(np.sqrt(10.0)))
-                            print("ii = {}".format(self.pp))
-                        elif event.key == pygame.K_UP:
-                            self.controlling = True
-                        elif event.key == pygame.K_DOWN:
-                            self.controlling = False
-                            self.rc_socket.send('({},{})\n'.format(0, 0))
-
-                dt = 0.05
-
-                if self.controlling:
-                    # test: turn robot such that angle is zero
-                    for r in self.robots:
-                        if r.euler is not None:
-                            self.k = self.k + 1
-
-                            alpha = r.euler
-                            self.alphas.append(alpha)
-
-                            # compute error
-                            e = alpha - 0
-
-                            # compute integral of error (approximately)
-                            self.inc += e * dt
-
-                            # PID
-                            p = self.pp * e
-                            i = self.ii * self.inc
-                            d = 0.0
-
-                            # compute controls for robot from PID
-                            u1 = p + i + d
-                            u2 = - p - i - d
-                            print("alpha = {}, u = ({}, {})".format(alpha, u1, u2))
-                            self.rc_socket.send('({},{})\n'.format(u1, u2))
-
-                            time.sleep(dt)
-
-            elif open_loop_solve:
-                # open loop controller
-
-                events = pygame.event.get()
-                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
-                            self.rc_socket.send('(0.1, 0.0,0.0)\n')
-                        elif event.key == pygame.K_0:
-                            self.target = (0.0, 0.0, 0.0)
-                        elif event.key == pygame.K_1:
-                            self.target = (0.5,0.5, -np.pi/2.0)
-                        elif event.key == pygame.K_2:
-                            self.target = (0.5, -0.5, 0.0)
-                        elif event.key == pygame.K_3:
-                            self.target = (-0.5,-0.5, np.pi/2.0)
-                        elif event.key == pygame.K_4:
-                            self.target = (-0.5,0.5, 0.0)
-                if self.controlling:
-                    tmpc_start = time.time()
-                    # get measurement
-                    self.mutex.acquire()
-                    try:
-                        last_measurement = copy.deepcopy(self.xms[-1])
-                        last_time = copy.deepcopy(self.tms[-1])
-                    finally:
-                        self.mutex.release()
-
-                    print("current measurement (t, x) = ({}, {})".format(last_time, last_measurement))
-                    print("current control (u1, u2) = ({}, {})".format(u1, u2))
-
-                    # prediction of state at time the mpc will terminate
-                    r.set_f_params(np.array([u1 * omega_max, u2 * omega_max]))
-
-                    r.set_initial_value(last_measurement, last_time)
-                    dt = self.ols.T/self.ols.N
-                    print("integrating for {} seconds".format((dt)))
-                    x_pred = r.integrate(r.t + (dt))
-
-                    print("predicted initial state x_pred = ({})".format(x_pred))
-
-                    res = self.ols.solve(x_pred, self.target)
-                    #tgrid = res[0]
-                    us1 = res[0]
-                    us2 = res[1]
-
-                    # tt = 0
-                    # x = last_measurement
-                    # t_ol = np.array([tt])
-                    # x_ol = np.array([x])
-                    # # compute open loop prediction
-                    # for i in range(len(us1)):
-                    #     r = scipy.integrate.ode(f_ode)
-                    #     r.set_f_params(np.array([us1[i] * 13.32, us2[i] * 13.32]))
-                    #     r.set_initial_value(x, tt)
-                    #
-                    #     tt = tt + 0.1
-                    #     x = r.integrate(tt)
-                    #
-                    #     t_ol = np.vstack((t_ol, tt))
-                    #     x_ol = np.vstack((x_ol, x))
-
-                    #plt.figure(4)
-                    #plt.plot(x_ol[:,0], x_ol[:,1])
-
-
-                #if event.key == pygame.K_DOWN:
-                #    if tgrid is not None:
-                    tmpc_end = time.time()
-                    print("---------------- mpc solution took {} seconds".format(tmpc_end - tmpc_start))
-                    dt_mpc = time.time() - self.t
-                    if dt_mpc < dt:
-                        print("sleeping for {} seconds...".format(dt - dt_mpc))
-                        time.sleep(dt - dt_mpc)
-
-                    self.mutex.acquire()
-                    try:
-                        second_measurement = copy.deepcopy(self.xms[-1])
-                        second_time = copy.deepcopy(self.tms[-1])
-                    finally:
-                        self.mutex.release()
-
-                    print("(last_time, second_time, dt) = ({}, {}, {})".format(last_time, second_time, second_time - last_time))
-                    print("mismatch between predicted state and measured state: {}\n\n".format(second_measurement - last_measurement))
-
-                    for i in range(0, 1):
-                        u1 = us1[i]
-                        u2 = us2[i]
-                        self.rc_socket.send('({},{},{})\n'.format(dt,u1, u2))
-                        self.t = time.time()
-                        #time.sleep(0.2)
-                    #
-
-
-                    pass
 
 def main(args):
     rospy.init_node('controller_node', anonymous=True)
@@ -505,8 +347,9 @@ def main(args):
 
     screenheight = 480
     screenwidth = 640
-    screen = pygame.display.set_mode([screenwidth, screenheight])
+    pygame.display.set_mode([screenwidth, screenheight])
 
+    #threading.Thread(target=rc.input_handling).start()
     threading.Thread(target=rc.controller).start()
 
     rc.ani()