Telos4
/
RoboRally
1
0
Fork 1
Code Issues Pull Requests Releases Wiki Activity

added plots for x and y position

simple_control
Simon Pirkelmann 2019-07-04 13:48:32 +02:00
parent cfa669e498
commit c6582c9e6c
2 changed files with 82 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
remote_control/casadi_opt.py
View File

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

83
remote_control/position_controller.py
View File

@ -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