added plots for x and y position

remote_control/casadi_opt.py

@@ -213,7 +213,7 @@ class OpenLoopSolver:
         # ---- path constraints -----------
         # limit = lambda pos: 1-sin(2*pi*pos)/2
         # self.opti.subject_to(speed<=limit(pos))   # track speed limit
-        maxcontrol = 0.95
+        maxcontrol = 0.3
         self.opti.subject_to(self.opti.bounded(-maxcontrol, self.U, maxcontrol))  # control is limited
 
         # ---- boundary conditions --------
@@ -236,7 +236,17 @@ class OpenLoopSolver:
             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
+        # 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/position_controller.py

@@ -124,13 +124,14 @@ def f_ode(t, x, u):
 class RemoteController:
     def __init__(self):
 
-        self.robots = [Robot(15, '192.168.1.103')]
+        #self.robots = [Robot(15, '192.168.1.103')]
+        self.robots = [Robot(14, '192.168.1.102')]
 
         self.robot_ids = {}
         for r in self.robots:
             self.robot_ids[r.id] = r
 
-        obst = [Obstacle(1, 0.175), Obstacle(5, 0.175), Obstacle(7, 0.175)]
+        obst = [Obstacle(12, 0.25), Obstacle(10, 0.25), Obstacle(13, 0.25)]
 
         self.obstacles = {}
         for r in obst:
@@ -146,7 +147,7 @@ class RemoteController:
                 self.rc_socket.connect((r.ip, 1234))  # connect to robot
         except socket.error:
             print("could not connect to socket")
-            self.rc_socket = None
+            sys.exit(1)
 
 
         self.t = time.time()
@@ -180,7 +181,10 @@ class RemoteController:
 
         # animation
         self.fig = plt.figure()
-        self.ax = self.fig.add_subplot(1,1,1)
+        self.ax = self.fig.add_subplot(2,2,1)
+        self.ax2 = self.fig.add_subplot(2, 2, 2)
+        self.ax3 = self.fig.add_subplot(2, 2, 4)
+
         self.xdata, self.ydata = [], []
         self.line, = self.ax.plot([],[], color='grey', linestyle=':')
         self.line_sim, = self.ax.plot([], [])
@@ -188,6 +192,9 @@ class RemoteController:
         self.dirm, = self.ax.plot([], [])
         self.dirs, = self.ax.plot([], [])
 
+        self.line_x, = self.ax2.plot([],[])
+        self.line_y, = self.ax3.plot([], [])
+
         self.track_line_inner, = self.ax.plot([], [])
         self.track_line_outer, = self.ax.plot([], [])
 
@@ -198,9 +205,17 @@ class RemoteController:
         for s in self.circles:
             self.ax.add_artist(s)
 
-        plt.xlabel('x-position')
+        self.ax.set_xlabel('x-position')
-        plt.ylabel('y-position')
+        self.ax.set_ylabel('y-position')
-        plt.grid()
+        self.ax.grid()
+
+        self.ax2.set_xlabel('Zeit t')
+        self.ax2.set_ylabel('x-position')
+        self.ax2.grid()
+
+        self.ax3.set_xlabel('Zeit t')
+        self.ax3.set_ylabel('y-position')
+        self.ax3.grid()
 
         self.ols = OpenLoopSolver()
         self.ols.setup()
@@ -224,11 +239,17 @@ class RemoteController:
         self.ax.set_ylim(-2, 2)
         self.ax.set_aspect('equal', adjustable='box')
 
+        self.ax2.set_ylim(-2, 2)
+        self.ax2.set_xlim(0, 10)
+
+        self.ax3.set_ylim(-2, 2)
+        self.ax3.set_xlim(0, 10)
+
         self.track_line_inner.set_data(self.track.inner_poly.exterior.xy)
         self.track_line_outer.set_data(self.track.outer_poly.exterior.xy)
 
         return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol,\
-               self.track_line_inner, self.track_line_outer, self.circles[0], self.circles[1],self.circles[2],
+               self.track_line_inner, self.track_line_outer, self.line_x,self.line_y, self.circles[0], self.circles[1],self.circles[2],
 
     def ani_update(self, frame):
         #print("plotting")
@@ -252,6 +273,12 @@ class RemoteController:
 
                     self.dirm.set_data(np.array([a, a2]), np.array([b, b2]))
 
+                    n_plot = 300
+                    if len(tm_local) > n_plot:
+                        # plot x and y coordinate
+                        self.line_x.set_data(tm_local[-n_plot:] - (tm_local[-1] - 10), xm_local[-n_plot:,0])
+                        self.line_y.set_data(tm_local[-n_plot:] - (tm_local[-1] - 10), xm_local[-n_plot:, 1])
+
             ts_local = deepcopy(self.ts)
             xs_local = deepcopy(self.xs)
 
@@ -289,7 +316,8 @@ class RemoteController:
         finally:
             self.mutex.release()
 
-        return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol, self.track_line_inner, self.track_line_outer, self.circles[0], self.circles[1],self.circles[2],
+        return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol, self.track_line_inner, self.track_line_outer,\
+               self.line_x, self.line_y, self.circles[0], self.circles[1],self.circles[2],
 
     def measurement_callback(self, data):
         #print(data)
@@ -332,6 +360,19 @@ class RemoteController:
 
     def controller(self):
         print("starting control")
+
+        targets = {}
+        markers_in = self.track.inner.values()
+        markers_out = self.track.outer.values()
+
+        # find targets:
+        for i in range(0,4):
+            p = (np.array(markers_in[i]) + np.array(markers_out[i]))/2.0
+            targets[i] = (p[0],p[1], 0.0)
+
+        auto_control = False
+        current_target = 0
+
         while True:
 
             # open loop controller
@@ -348,17 +389,33 @@ class RemoteController:
                     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)
+                        #self.target = (0.5,0.5, -np.pi/2.0)
+                        self.target = targets[0]
                     elif event.key == pygame.K_2:
-                        self.target = (0.5, -0.5, 0.0)
+                        #self.target = (0.5, -0.5, 0.0)
+                        self.target = targets[1]
                     elif event.key == pygame.K_3:
-                        self.target = (-0.5,-0.5, np.pi/2.0)
+                        #self.target = (-0.5,-0.5, np.pi/2.0)
+                        self.target = targets[2]
                     elif event.key == pygame.K_4:
-                        self.target = (-0.5,0.5, 0.0)
+                        #self.target = (-0.5,0.5, 0.0)
+                        self.target = targets[3]
+                    elif event.key == pygame.K_5:
+                        auto_control = not auto_control
+                        if auto_control:
+                            self.target = targets[current_target]
 
             if self.controlling:
+
                 x_pred = self.get_measurement_prediction()
 
+                if auto_control:
+                    if np.linalg.norm(x_pred[0:2]-np.array(self.target[0:2])) < 0.2:
+                        print("close to target!")
+                        current_target = (current_target + 1) % 4
+                        self.target = targets[current_target]
+                        print("new target = {}".format(current_target))
+
                 tmpc_start = time.time()
 
                 # solve mpc open loop problem