From d25de2e0c263edf56cbc9157b6c044a5c0c69043 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Sun, 8 Dec 2019 14:57:35 +0100 Subject: [PATCH] Add attribute dynamic_border_cells to reference path. --- MPC.py | 11 +++-- reference_path.py | 98 +++++++++++++++++++++++---------------- simulation.py | 43 +++++++++-------- spatial_bicycle_models.py | 2 +- 4 files changed, 88 insertions(+), 66 deletions(-) diff --git a/MPC.py b/MPC.py index 1eafe2b..6c86b21 100644 --- a/MPC.py +++ b/MPC.py @@ -112,10 +112,13 @@ class MPC: umax_dyn[self.nu*n] = vmax_dyn # Compute dynamic constraints on e_y - ub, lb, cells = self.model.reference_path.update_path_constraints( - self.model.wp_id, self.N+1, 2*self.model.safety_margin[1], 0.05) - xmin_dyn[::self.nx] = lb - xmax_dyn[::self.nx] = ub + ub, lb = self.model.reference_path.update_path_constraints( + self.model.wp_id+1, self.N, 2*self.model.safety_margin[1], + self.model.safety_margin[1]) + xmin_dyn[0] = self.model.spatial_state.e_y + xmax_dyn[0] = self.model.spatial_state.e_y + xmin_dyn[self.nx::self.nx] = lb + xmax_dyn[self.nx::self.nx] = ub # Get equality matrix Ax = sparse.kron(sparse.eye(self.N + 1), diff --git a/reference_path.py b/reference_path.py index 3394579..38a9f6e 100644 --- a/reference_path.py +++ b/reference_path.py @@ -11,7 +11,7 @@ import osqp DRIVABLE_AREA = '#BDC3C7' WAYPOINTS = '#D0D3D4' OBSTACLE = '#2E4053' - +PATH_CONSTRAINTS = '#F5B041' ############ # Waypoint # @@ -40,7 +40,8 @@ class Waypoint: # waypoint orientation self.lb = None self.ub = None - self.border_cells = None + self.static_border_cells = None + self.dynamic_border_cells = None def __sub__(self, other): """ @@ -264,7 +265,8 @@ class ReferencePath: wp.lb = -1 * width_info[2] # minus can be assumed as waypoints # represent center-line of the path # Set border cells of waypoint - wp.border_cells = (width_info[1], width_info[3]) + wp.static_border_cells = (width_info[1], width_info[3]) + wp.dynamic_border_cells = (width_info[1], width_info[3]) def _get_min_width(self, wp, t_x, t_y, max_width): """ @@ -316,6 +318,8 @@ class ReferencePath: """ Compute a speed profile for the path. Assign a reference velocity to each waypoint based on its curvature. + :param Constraints: constraints on acceleration and velocity + curvature of the path """ # Set optimization horizon @@ -336,8 +340,6 @@ class ReferencePath: # Iterate over horizon for i in range(N): - look_ahead = 30 - # Get information about current waypoint current_waypoint = self.get_waypoint(i) next_waypoint = self.get_waypoint(i+1) @@ -345,9 +347,6 @@ class ReferencePath: li = next_waypoint - current_waypoint # curvature of waypoint ki = current_waypoint.kappa - if np.abs(ki) <= 0.1: - kis = [wp.kappa for wp in self.waypoints[i:i+look_ahead]] - ki = np.mean(kis) # Fill operator matrix # dynamics of acceleration @@ -436,8 +435,8 @@ class ReferencePath: plt.yticks([]) # Plot map in gray-scale and set extent to match world coordinates - #canvas = np.ones(self.map.data.shape) - canvas = np.flipud(self.map.data) + canvas = np.ones(self.map.data.shape) + #canvas = np.flipud(self.map.data) plt.imshow(canvas, cmap='gray', extent=[self.map.origin[0], self.map.origin[0] + self.map.width * self.map.resolution, @@ -450,10 +449,10 @@ class ReferencePath: wp_y = np.array([wp.y for wp in self.waypoints]) # Get x and y locations of border cells for upper and lower bound - wp_ub_x = np.array([wp.border_cells[0][0] for wp in self.waypoints]) - wp_ub_y = np.array([wp.border_cells[0][1] for wp in self.waypoints]) - wp_lb_x = np.array([wp.border_cells[1][0] for wp in self.waypoints]) - wp_lb_y = np.array([wp.border_cells[1][1] for wp in self.waypoints]) + wp_ub_x = np.array([wp.static_border_cells[0][0] for wp in self.waypoints]) + wp_ub_y = np.array([wp.static_border_cells[0][1] for wp in self.waypoints]) + wp_lb_x = np.array([wp.static_border_cells[1][0] for wp in self.waypoints]) + wp_lb_y = np.array([wp.static_border_cells[1][1] for wp in self.waypoints]) # Plot waypoints plt.scatter(wp_x, wp_y, color=WAYPOINTS, s=10) @@ -468,18 +467,18 @@ class ReferencePath: headwidth=1, headlength=0) # Plot border of path - bl_x = np.array([wp.border_cells[0][0] for wp in + bl_x = np.array([wp.static_border_cells[0][0] for wp in self.waypoints] + - [self.waypoints[0].border_cells[0][0]]) - bl_y = np.array([wp.border_cells[0][1] for wp in + [self.waypoints[0].static_border_cells[0][0]]) + bl_y = np.array([wp.static_border_cells[0][1] for wp in self.waypoints] + - [self.waypoints[0].border_cells[0][1]]) - br_x = np.array([wp.border_cells[1][0] for wp in + [self.waypoints[0].static_border_cells[0][1]]) + br_x = np.array([wp.static_border_cells[1][0] for wp in self.waypoints] + - [self.waypoints[0].border_cells[1][0]]) - br_y = np.array([wp.border_cells[1][1] for wp in + [self.waypoints[0].static_border_cells[1][0]]) + br_y = np.array([wp.static_border_cells[1][1] for wp in self.waypoints] + - [self.waypoints[0].border_cells[1][1]]) + [self.waypoints[0].static_border_cells[1][1]]) # If circular path, connect start and end point if self.circular: @@ -492,6 +491,19 @@ class ReferencePath: plt.plot((bl_x[-2], br_x[-2]), (bl_y[-2], br_y[-2]), color=OBSTACLE) plt.plot((bl_x[0], br_x[0]), (bl_y[0], br_y[0]), color=OBSTACLE) + # Plot dynamic path constraints + # Get x and y locations of border cells for upper and lower bound + wp_ub_x = np.array( + [wp.dynamic_border_cells[0][0] for wp in self.waypoints]) + wp_ub_y = np.array( + [wp.dynamic_border_cells[0][1] for wp in self.waypoints]) + wp_lb_x = np.array( + [wp.dynamic_border_cells[1][0] for wp in self.waypoints]) + wp_lb_y = np.array( + [wp.dynamic_border_cells[1][1] for wp in self.waypoints]) + plt.plot(wp_ub_x, wp_ub_y, color=PATH_CONSTRAINTS) + plt.plot(wp_lb_x, wp_lb_y, color=PATH_CONSTRAINTS) + # Plot obstacles for obstacle in self.obstacles: obstacle.show() @@ -508,10 +520,10 @@ class ReferencePath: free_segments = [] # Get waypoint's border cells in map coordinates - ub_p = self.map.w2m(wp.border_cells[0][0], - wp.border_cells[0][1]) - lb_p = self.map.w2m(wp.border_cells[1][0], - wp.border_cells[1][1]) + ub_p = self.map.w2m(wp.static_border_cells[0][0], + wp.static_border_cells[0][1]) + lb_p = self.map.w2m(wp.static_border_cells[1][0], + wp.static_border_cells[1][1]) # Compute path from left border cell to right border cell x_list, y_list = line(ub_p[0], ub_p[1], lb_p[0], lb_p[1]) @@ -562,12 +574,13 @@ class ReferencePath: ub_hor = [] lb_hor = [] border_cells_hor = [] + border_cells_hor_sm = [] # Iterate over horizon for n in range(N): # get corresponding waypoint - wp = self.waypoints[wp_id+n] + wp = self.get_waypoint(wp_id+n) # Get list of free segments free_segments = self._compute_free_segments(wp, min_width) @@ -586,7 +599,7 @@ class ReferencePath: ub_pw, lb_pw = list(ub_pw), list(lb_pw) # Project border cells onto new waypoint in path direction - wp_prev = self.waypoints[wp_id+n-1] + wp_prev = self.get_waypoint(wp_id+n-1) delta_s = wp_prev - wp ub_pw[0] += delta_s * np.cos(wp_prev.psi) ub_pw[1] += delta_s * np.cos(wp_prev.psi) @@ -660,14 +673,24 @@ class ReferencePath: angle_ub) lb_ls = wp.x - lb * np.cos(angle_lb), wp.y - lb * np.sin( angle_lb) + bound_cells_sm = (ub_ls, lb_ls) + # Compute cell on bound for computed distance ub and lb + ub_ls = wp.x + (ub + safety_margin) * np.cos(angle_ub), wp.y + (ub + safety_margin) * np.sin( + angle_ub) + lb_ls = wp.x - (lb - safety_margin) * np.cos(angle_lb), wp.y - (lb - safety_margin) * np.sin( + angle_lb) bound_cells = (ub_ls, lb_ls) # Append results ub_hor.append(ub) lb_hor.append(lb) border_cells_hor.append(list(bound_cells)) + border_cells_hor_sm.append(list(bound_cells_sm)) - return np.array(ub_hor), np.array(lb_hor), border_cells_hor + # Assign dynamic border cells to waypoints + wp.dynamic_border_cells = bound_cells_sm + + return np.array(ub_hor), np.array(lb_hor) if __name__ == '__main__': @@ -711,29 +734,22 @@ if __name__ == '__main__': circular=False) obs1 = Obstacle(cx=-6.3, cy=-11.1, radius=0.20) obs2 = Obstacle(cx=-2.2, cy=-6.8, radius=0.25) - obs3 = Obstacle(cx=1.7, cy=-1.0, radius=0.15) - obs4 = Obstacle(cx=2.0, cy=-1.2, radius=0.25) - obs5 = Obstacle(cx=2.2, cy=-0.86, radius=0.1) - obs6 = Obstacle(cx=2.33, cy=-0.7, radius=0.1) - obs7 = Obstacle(cx=2.67, cy=-0.73, radius=0.1) - obs8 = Obstacle(cx=6.42, cy=3.97, radius=0.3) + obs4 = Obstacle(cx=2.0, cy=-0.2, radius=0.25) + obs8 = Obstacle(cx=6.0, cy=5.0, radius=0.3) obs9 = Obstacle(cx=7.42, cy=4.97, radius=0.3) - obs10 = Obstacle(cx=7.14, cy=5.7, radius=0.1) - reference_path.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7, obs8, obs9, obs10]) + reference_path.add_obstacles([obs1, obs2, obs4, obs8, obs9]) else: reference_path = None print('Invalid path!') exit(1) - ub, lb, border_cells = reference_path.update_path_constraints(0, reference_path.n_waypoints, 0.02, 0.01) + ub, lb, border_cells = reference_path.update_path_constraints(0, + reference_path.n_waypoints, 0.02, 0.01) # Get x and y locations of border cells for upper and lower bound for wp_id in range(reference_path.n_waypoints): - if ub[wp_id] > 0.0 and lb[wp_id] > 0.0: - print(wp_id) reference_path.waypoints[wp_id].border_cells = border_cells[wp_id] reference_path.show() - plt.show() diff --git a/simulation.py b/simulation.py index c7062ee..d98f663 100644 --- a/simulation.py +++ b/simulation.py @@ -26,6 +26,17 @@ if __name__ == '__main__': reference_path = ReferencePath(map, wp_x, wp_y, path_resolution, smoothing_distance=5, max_width=0.23, circular=True) + # Add obstacles + obs1 = Obstacle(cx=0.0, cy=0.0, radius=0.05) + obs2 = Obstacle(cx=-0.8, cy=-0.5, radius=0.05) + obs3 = Obstacle(cx=-0.7, cy=-1.5, radius=0.05) + obs4 = Obstacle(cx=-0.3, cy=-1.0, radius=0.05) + obs5 = Obstacle(cx=0.3, cy=-1.0, radius=0.05) + obs6 = Obstacle(cx=0.75, cy=-1.5, radius=0.05) + obs7 = Obstacle(cx=0.7, cy=-0.9, radius=0.05) + obs8 = Obstacle(cx=1.2, cy=0.0, radius=0.05) + reference_path.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7, + obs8]) elif sim_mode == 'Q': map = Map(file_path='map_floor2.png') wp_x = [-9.169, 11.9, 7.3, -6.95] @@ -36,25 +47,18 @@ if __name__ == '__main__': reference_path = ReferencePath(map, wp_x, wp_y, path_resolution, smoothing_distance=5, max_width=1.50, circular=False) + obs1 = Obstacle(cx=-6.3, cy=-11.1, radius=0.20) + obs2 = Obstacle(cx=-2.2, cy=-6.8, radius=0.25) + obs4 = Obstacle(cx=2.0, cy=-0.2, radius=0.25) + obs8 = Obstacle(cx=6.0, cy=5.0, radius=0.3) + obs9 = Obstacle(cx=7.42, cy=4.97, radius=0.3) + reference_path.add_obstacles([obs1, obs2, obs4, obs8, obs9]) else: print('Invalid Simulation Mode!') map, wp_x, wp_y, path_resolution, reference_path \ = None, None, None, None, None exit(1) - obs1 = Obstacle(cx=0.0, cy=0.0, radius=0.05) - obs2 = Obstacle(cx=-0.8, cy=-0.5, radius=0.05) - obs3 = Obstacle(cx=-0.7, cy=-1.5, radius=0.07) - obs4 = Obstacle(cx=-0.3, cy=-1.0, radius=0.07) - obs5 = Obstacle(cx=0.3, cy=-1.0, radius=0.05) - obs6 = Obstacle(cx=0.75, cy=-1.5, radius=0.07) - obs7 = Obstacle(cx=0.7, cy=-0.9, radius=0.08) - obs8 = Obstacle(cx=1.2, cy=0.0, radius=0.08) - obs9 = Obstacle(cx=0.7, cy=-0.1, radius=0.05) - obs10 = Obstacle(cx=1.1, cy=-0.6, radius=0.07) - reference_path.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7, - obs8, obs9, obs10]) - ################ # Motion Model # ################ @@ -65,7 +69,7 @@ if __name__ == '__main__': t_0 = 0.0 V_MAX = 2.5 - car = BicycleModel(length=0.12, width=0.06, reference_path=reference_path, + car = BicycleModel(length=0.56, width=0.33, reference_path=reference_path, e_y=e_y_0, e_psi=e_psi_0, t=t_0) ############## @@ -83,7 +87,7 @@ if __name__ == '__main__': mpc = MPC(car, N, Q, R, QN, StateConstraints, InputConstraints) # Compute speed profile - SpeedProfileConstraints = {'a_min': -1.0, 'a_max': 1.0, + SpeedProfileConstraints = {'a_min': -0.05, 'a_max': 0.5, 'v_min': 0, 'v_max': V_MAX, 'ay_max': 1.0} car.reference_path.compute_speed_profile(SpeedProfileConstraints) @@ -92,7 +96,7 @@ if __name__ == '__main__': ############## # Sampling time - Ts = 0.20 + Ts = 0.05 t = 0 car.set_sampling_time(Ts) @@ -121,8 +125,8 @@ if __name__ == '__main__': # Plot path and drivable area reference_path.show() - plt.scatter(x_log, y_log, c=v_log, s=10) - plt.colorbar() + #plt.scatter(x_log, y_log, c=v_log, s=10) + #plt.colorbar() # Plot MPC prediction mpc.show_prediction() @@ -137,6 +141,5 @@ if __name__ == '__main__': plt.title('MPC Simulation: v(t): {:.2f}, delta(t): {:.2f}, Duration: ' '{:.2f} s'.format(u[0], u[1], t)) - plt.pause(0.00001) - print(min(v_log)) + plt.pause(0.000001) plt.show() diff --git a/spatial_bicycle_models.py b/spatial_bicycle_models.py index 85be2cb..ef6be94 100644 --- a/spatial_bicycle_models.py +++ b/spatial_bicycle_models.py @@ -240,7 +240,7 @@ class SpatialBicycleModel(ABC): # Model ellipsoid around the car length = self.l / np.sqrt(2) - width = self.w / np.sqrt(2) + width = self.w / np.sqrt(2) + 0.02 return length, width