From c6582c9e6c18ea734a8f829e03b2c090b5fbbb2b Mon Sep 17 00:00:00 2001 From: spirkelmann Date: Thu, 4 Jul 2019 13:48:32 +0200 Subject: [PATCH] added plots for x and y position --- remote_control/casadi_opt.py | 14 ++++- remote_control/position_controller.py | 83 ++++++++++++++++++++++----- 2 files changed, 82 insertions(+), 15 deletions(-) diff --git a/remote_control/casadi_opt.py b/remote_control/casadi_opt.py index 13f69d5..12e86bf 100644 --- a/remote_control/casadi_opt.py +++ b/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, :] diff --git a/remote_control/position_controller.py b/remote_control/position_controller.py index d866866..514d7fe 100644 --- a/remote_control/position_controller.py +++ b/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') - plt.ylabel('y-position') - plt.grid() + self.ax.set_xlabel('x-position') + self.ax.set_ylabel('y-position') + 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