├── .gitignore ├── LICENSE ├── README.md ├── __init__.py ├── animate.py ├── dynamics.py ├── environment.py ├── gifs ├── anim.gif ├── animation.gif └── nine_obs_no_noise.gif ├── img └── five_obs_base.png ├── kinematic_bicycle_linearized_mpc.ipynb ├── optimization.py ├── scenarios.py ├── simulation.py ├── trajectory_gen.py └── vids ├── five_obs_no_noise_01.mp4 └── nine_obs_no_noise.mp4 /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__* 2 | *.ipynb 3 | .ipynb_checkpoints 4 | .python-version 5 | gurobi.log 6 | jupyter_setup.py -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Mike Schoder 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Kinematic Bicycle Model MPC 2 | 3 | Check out the [notebook](./kinematic_bicycle_linearized_mpc.ipynb) to see the models working and a walkthrough of the equations and implementation. 4 | 5 | ![](./gifs/nine_obs_no_noise.gif) 6 | -------------------------------------------------------------------------------- /__init__.py: -------------------------------------------------------------------------------- 1 | from .environment import * 2 | from .scenarios import * 3 | from .trajectory_gen import * 4 | from .animate import * 5 | from .dynamics import * 6 | from .simulation import * 7 | from .optimization import * 8 | -------------------------------------------------------------------------------- /animate.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | from matplotlib import pyplot as plt 4 | from matplotlib import animation, rc, patches 5 | import matplotlib.collections as clt 6 | import environment 7 | import scenarios 8 | import trajectory_gen 9 | 10 | # Set jshtml default mode for notebook use 11 | rc('animation', html='jshtml') 12 | 13 | 14 | def plot_one_step(env, x_ref, x_bar, x_opt, x_next=None, nominals=None): 15 | ''' Plots a single step of trajectory optimization in environment ''' 16 | fig, ax = environment.plot_environment(env, figsize=(16, 10)) 17 | ax.plot(x_ref[0, :], x_ref[1, :], '-o', alpha=0.8, 18 | color='blue', markersize=3, label='reference trajectory') 19 | if nominals is not None: 20 | for nom in nominals: 21 | ax.plot(nom[0, :], nom[1, :], 'r-', label='nominal trajectories') 22 | else: 23 | ax.plot(x_bar[0, :], x_bar[1, :], 'r-', label='nominal trajectory') 24 | ax.plot(x_opt[0, :], x_opt[1, :], '-o', 25 | color='lightseagreen', label='optimal trajectory') 26 | if x_next is not None: 27 | ax.plot(x_next[0], x_next[1], 'o', color='blueviolet', label='next x0') 28 | 29 | handles, labels = plt.gca().get_legend_handles_labels() 30 | labels, ids = np.unique(labels, return_index=True) 31 | handles = [handles[i] for i in ids] 32 | ax.legend(handles, labels, loc='best') 33 | return fig, ax 34 | 35 | 36 | def plot_all_steps(env, x_ref_full, history): 37 | ''' Plots optimization paths in environment at each step over time 38 | course of the full MPC run 39 | ''' 40 | fig, ax = environment.plot_environment(env, figsize=(16, 10)) 41 | ax.plot(x_ref_full[0, :], x_ref_full[1, :], 42 | '-o', color='blue', label='reference trajectory', markersize=3) 43 | for i in range(len(history.keys())): 44 | xi = history[i]['x_opt'] 45 | ax.plot(xi[0, :], xi[1, :], color='lightseagreen', 46 | linewidth=1, label='N-step optimal traj') 47 | x0x = [history[i]['x0'][0] for i in history.keys()] 48 | x0y = [history[i]['x0'][1] for i in history.keys()] 49 | ax.plot(x0x, x0y, '-o', color='blueviolet', label='actual trajectory') 50 | xf_bar = history[len(history.keys())-1]['x_bar'] 51 | # ax.plot(xf_bar[0, :], xf_bar[1, :], 'r', label='xf_bar') 52 | ax.axis('equal') 53 | handles, labels = plt.gca().get_legend_handles_labels() 54 | labels, ids = np.unique(labels, return_index=True) 55 | handles = [handles[i] for i in ids] 56 | ax.legend(handles, labels, loc='best') 57 | return fig, ax 58 | 59 | 60 | def animate(env, ctrl_pts, bc_headings, v, dt, x_ref, history, rect=False): 61 | 62 | fig, ax = environment.plot_environment(env, figsize=(16, 10)) 63 | xs = x_ref[0, :] 64 | ys = x_ref[1, :] 65 | x0s = [history[i]['x0'][0] for i in history.keys()] 66 | y0s = [history[i]['x0'][1] for i in history.keys()] 67 | 68 | # plot reference trajectory 69 | ax.plot(xs, ys, '-o', alpha=0.8, markersize=3, 70 | color='blue', label='reference trajectory') 71 | # optimized trajectory 72 | opt_line, = ax.plot([], [], '-o', lw=3, color='lightseagreen', 73 | label='N-step optimal traj') 74 | nom_line, = ax.plot([], [], color='red', label='nominal trajectory') 75 | act_line, = ax.plot([], [], '-o', lw=3, markersize=6, 76 | color='blueviolet', label='actual trajectory') 77 | 78 | if rect: 79 | ld, wd = 0.5, 0.2 80 | a2 = np.arctan2(wd, ld) 81 | diag = np.sqrt(ld**2 + wd**2) 82 | heading = np.rad2deg(np.arctan2((y0s[1]-y0s[0]), (x0s[1]-x0s[0]))) 83 | car = patches.Rectangle( 84 | (x0s[0]-ld, y0s[0]-wd), 2*ld, 2*wd, angle=-heading, fc='none', lw=1, ec='k') 85 | 86 | def init(): 87 | opt_line.set_data([], []) 88 | act_line.set_data([], []) 89 | nom_line.set_data([], []) 90 | if rect: 91 | ax.add_patch(car) 92 | return opt_line, 93 | 94 | def step(i): 95 | x = history[i]['x_opt'][0] 96 | y = history[i]['x_opt'][1] 97 | xbar = history[i]['x_bar'][0] 98 | ybar = history[i]['x_bar'][1] 99 | opt_line.set_data(x, y) 100 | act_line.set_data(x0s[:i], y0s[:i]) 101 | nom_line.set_data(xbar, ybar) 102 | if rect: 103 | if (len(x) == 1): 104 | heading = bc_headings[1] 105 | else: 106 | heading = np.arctan2((y[1]-y[0]), (x[1]-x[0])) 107 | xoff = diag*np.cos(heading + a2) 108 | yoff = diag*np.sin(heading + a2) 109 | car.set_xy((x[0] - xoff, y[0] - yoff)) 110 | car.angle = np.rad2deg(heading) 111 | 112 | return opt_line, 113 | 114 | anim = animation.FuncAnimation(fig, step, init_func=init, 115 | frames=len(history.keys()), interval=1000*dt*2, blit=True) 116 | 117 | ax.axis('equal') 118 | ax.legend() 119 | plt.close() 120 | return anim 121 | 122 | 123 | if __name__ == "__main__": 124 | 125 | # TEST ANIMATION FUNCTION 126 | 127 | scene = scenarios.five_obstacle 128 | ctrl_pts = scene['control_pts'] 129 | bch = scene['bc_headings'] 130 | env = environment.Environment(scene['obs_list'], 131 | scene['start'], scene['goal']) 132 | env.add_control_points(ctrl_pts) 133 | v = 4 134 | dt = 0.1 135 | xs, ys, psi = trajectory_gen.sample_trajectory(ctrl_pts, bch, v, dt) 136 | nf = len(xs) 137 | x_ref = np.vstack((xs.reshape((1, nf)), 138 | ys.reshape((1, nf)), 139 | psi.reshape((1, nf)), 140 | v*np.ones((1, nf)), 141 | np.zeros((1, nf)))) 142 | 143 | anim = animate(env, ctrl_pts, bch, v, dt, x_ref, None) 144 | plt.show() 145 | -------------------------------------------------------------------------------- /dynamics.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pydrake.symbolic as sym 3 | 4 | 5 | def car_continuous_dynamics(x, u): 6 | # x = [x position, y position, heading, speed, steering angle] 7 | # u = [acceleration, steering velocity] 8 | m = sym if x.dtype == object else np # Check type for autodiff 9 | heading = x[2] 10 | v = x[3] 11 | steer = x[4] 12 | x_d = np.array([ 13 | v*m.cos(heading), 14 | v*m.sin(heading), 15 | v*m.tan(steer), 16 | u[0], 17 | u[1] 18 | ]) 19 | return x_d 20 | 21 | 22 | def discrete_dynamics(x, u, dt): 23 | # forward Euler 24 | x_next = x + dt * car_continuous_dynamics(x, u) 25 | return x_next 26 | 27 | 28 | def rollout(x0, u_trj, n_x, dt): 29 | ''' Returns state trajectory projected in time, x_trj: [N, number of states] ''' 30 | x_trj = np.zeros((n_x, u_trj.shape[1]+1)) 31 | x_trj[:, 0] = x0 32 | for n in range(1, u_trj.shape[1]+1): 33 | x_trj[:, n] = discrete_dynamics(x_trj[:, n-1], u_trj[:, n-1], dt) 34 | return x_trj 35 | 36 | 37 | class derivatives(): 38 | def __init__(self, discrete_dynamics, n_x, n_u): 39 | self.x_sym = np.array([sym.Variable("x_{}".format(i)) 40 | for i in range(n_x)]) 41 | self.u_sym = np.array([sym.Variable("u_{}".format(i)) 42 | for i in range(n_u)]) 43 | x = self.x_sym 44 | u = self.u_sym 45 | f = car_continuous_dynamics(x, u) 46 | self.f_x = sym.Jacobian(f, x) 47 | self.f_u = sym.Jacobian(f, u) 48 | 49 | def get_jacobians(self, x, u): 50 | env = {self.x_sym[i]: x[i] for i in range(x.shape[0])} 51 | env.update({self.u_sym[i]: u[i] for i in range(u.shape[0])}) 52 | f_x = sym.Evaluate(self.f_x, env) 53 | f_u = sym.Evaluate(self.f_u, env) 54 | return f_x, f_u 55 | 56 | 57 | def get_linear_dynamics(derivs, x, u): 58 | ''' Linearize dynamics to take form dx = x[n+1] - x[n] = A*dx + B*du ''' 59 | f_x, f_u = derivs.get_jacobians(x, u) 60 | A = f_x 61 | B = f_u 62 | return A, B 63 | 64 | 65 | def step_car_dynamics(x0, u0, sigmas, dt): 66 | ''' For simulation ''' 67 | n_x = x0.shape[0] 68 | xn = discrete_dynamics(x0, u0, dt) 69 | w = np.random.normal(0, sigmas, n_x) 70 | return xn + w 71 | 72 | 73 | if __name__ == "__main__": 74 | dt = 0.1 75 | n_x = 5 76 | n_u = 2 77 | derivs = derivatives(discrete_dynamics, n_x, n_u) 78 | dfx, dfu = derivs.f_x, derivs.f_u 79 | print(dfx, dfu) 80 | -------------------------------------------------------------------------------- /environment.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | import shapely.geometry as geom 4 | from shapely import affinity 5 | import itertools 6 | from matplotlib import pyplot as plt 7 | from matplotlib import patches 8 | from descartes import PolygonPatch 9 | 10 | 11 | class Environment: 12 | def __init__(self, obstacles, start, goal_region, bounds=None): 13 | self.environment_loaded = False 14 | self.obstacles = obstacles # list of lists of tuples 15 | self.bounds = bounds 16 | self.start = start # (x,y) 17 | self.goal_region = goal_region # list of tuples defining corner points 18 | self.control_points = [] 19 | self.calculate_scene_dimensions() 20 | 21 | def add_obstacles(self, obstacles): 22 | self.obstacles += obstacles 23 | self.calculate_scene_dimensions() 24 | 25 | def set_goal_region(self, goal_region): 26 | self.goal_region = goal_region 27 | self.calculate_scene_dimensions() 28 | 29 | def add_control_points(self, points): 30 | self.control_points += points 31 | self.calculate_scene_dimensions() 32 | 33 | def calculate_scene_dimensions(self): 34 | """Compute scene bounds from obstacles, start, and goal """ 35 | points = [] 36 | for elem in self.obstacles: 37 | points = points + elem 38 | if self.start: 39 | points += [self.start] 40 | if self.goal_region: 41 | points += self.goal_region 42 | if len(self.control_points) > 0: 43 | points += self.control_points 44 | mp = geom.MultiPoint(points) 45 | self.bounds = mp.bounds 46 | 47 | 48 | def plot_ellipse_environment(scene, bounds, figsize): 49 | ''' 50 | scene - dict from scenarios 51 | bounds - [[minx, maxx], [miny, maxy]] 52 | ''' 53 | fig = plt.figure(figsize=figsize) 54 | ax = fig.add_subplot(111) 55 | 56 | for obs in scene['obs_list']: 57 | h, k, a, b, theta = obs 58 | ellipse = patches.Ellipse( 59 | (h, k), a, b, theta, fc='orange', ec='k', alpha=0.5, zorder=5) 60 | ax.add_patch(ellipse) 61 | # start / goal 62 | goal_poly = geom.Polygon(scene['goal']) 63 | ax.add_patch(PolygonPatch(goal_poly, fc='green', 64 | ec='green', alpha=0.5, zorder=1)) 65 | start = geom.Point(scene['start']).buffer(0.2, resolution=3) 66 | ax.add_patch(PolygonPatch(start, fc='red', 67 | ec='black', alpha=0.7, zorder=1)) 68 | plt.xlim(bounds[0]) 69 | plt.ylim(bounds[1]) 70 | ax.set_aspect('equal', adjustable='box') 71 | return ax 72 | 73 | 74 | def plot_environment(env, bounds=None, figsize=None, margin=1.0): 75 | if bounds is None and env.bounds: 76 | minx, miny, maxx, maxy = env.bounds 77 | minx -= margin 78 | miny -= margin 79 | maxx += margin 80 | maxy += margin 81 | elif bounds: 82 | minx, miny, maxx, maxy = bounds 83 | else: 84 | minx, miny, maxx, maxy = (-10, -5, 10, 5) 85 | max_width, max_height = 12, 5.5 86 | if figsize is None: 87 | width, height = max_width, (maxy-miny)*max_width/(maxx-minx) 88 | if height > 5: 89 | width, height = (maxx-minx)*max_height/(maxy-miny), max_height 90 | figsize = (width, height) 91 | 92 | f = plt.figure(figsize=figsize) 93 | ax = f.add_subplot(111) 94 | # obstacles 95 | for i, obs in enumerate(env.obstacles): 96 | poly = geom.Polygon(obs) 97 | patch = PolygonPatch(poly, fc='orange', ec='black', 98 | alpha=0.5, zorder=20) 99 | ax.add_patch(patch) 100 | 101 | # start / goal 102 | goal_poly = geom.Polygon(env.goal_region) 103 | ax.add_patch(PolygonPatch(goal_poly, fc='green', 104 | ec='green', alpha=0.5, zorder=1)) 105 | start = geom.Point(env.start).buffer(0.2, resolution=3) 106 | ax.add_patch(PolygonPatch(start, fc='red', 107 | ec='black', alpha=0.7, zorder=1)) 108 | 109 | # control points 110 | cx = [c[0] for c in env.control_points] 111 | cy = [c[1] for c in env.control_points] 112 | ax.plot(cx, cy, 'ko', markersize=8, alpha=0.8, label='control points') 113 | 114 | plt.xlim([minx, maxx]) 115 | plt.ylim([miny, maxy]) 116 | ax.set_aspect('equal', adjustable='box') 117 | return f, ax 118 | 119 | # Helpers for obstacle constraint handling 120 | 121 | 122 | def centroid(obstacle): 123 | ''' 124 | Averages all vertices in a given obstacle. Average of x's and y's is 125 | guaranteed to lie inside polygon 126 | ''' 127 | x_avg = sum([v[0] for v in obstacle])/len(obstacle) 128 | y_avg = sum([v[1] for v in obstacle])/len(obstacle) 129 | return (x_avg, y_avg) 130 | 131 | 132 | def linear_obstacle_constraints(obs, buffer): 133 | ''' 134 | Given polygonal obstsacle, returns a list of values for a, b, c 135 | Constraints take form: cy <= ax + b - buffer + Mz 136 | Assumes obstacles are given as consecutive ordered list of vertices 137 | ''' 138 | constraints = [] 139 | cent = centroid(obs) 140 | for i, v in enumerate(obs): 141 | v1 = obs[i] 142 | # get next vertex; loop back to first for last constraint 143 | v2 = obs[(i+1) % len(obs)] 144 | dx = v2[0] - v1[0] 145 | dy = v2[1] - v1[1] 146 | 147 | if dx == 0: # vertical constaint case; cy <= ax + b --> x <= b 148 | c = 0 149 | if cent[0] <= v1[0]: # flip constraint 150 | a, b, c = 1, -v1[0] - buffer, 0 151 | 152 | else: 153 | a, b, c = -1, v1[0] - buffer, 0 154 | 155 | else: # non-vertical constraint; cy <= ax + b 156 | a = dy / dx 157 | b = v1[1] - a * v1[0] 158 | if cent[1] < a * cent[0] + b: # flip constraint 159 | a, b, c = -a, -b, -1 160 | a, b, c = calc_offset_coefs((a, b, c), buffer) 161 | else: 162 | a, b, c = a, b, 1 163 | a, b, c = calc_offset_coefs((a, b, c), buffer) 164 | constraints.append((a, b, c)) 165 | return constraints 166 | 167 | 168 | def calc_offset_coefs(coefs, offset): 169 | a, b, c = coefs 170 | b_new = b - offset*np.sqrt(a**2+1) 171 | return a, b_new, c 172 | 173 | 174 | # Test 175 | if __name__ == '__main__': 176 | 177 | import scenarios 178 | 179 | scene = scenarios.two_ellipse 180 | print(scene) 181 | ax = plot_ellipse_environment(scene, [[-1, 15], [-1, 10]], (12, 8)) 182 | ax.plot([1], [2], 'ro') 183 | plt.show() 184 | 185 | # import trajectory_gen as tgen 186 | # import scenarios 187 | 188 | # scene = scenarios.two_obstacle 189 | # control_pts = [(3, 0), (4.5, 1), (5, 2.8), 190 | # (3.5, 3.4), (2, 5.1), (3.7, 6.7), (5.5, 6.3)] 191 | # env = Environment(scene['obs_list'], 192 | # scene['start'], scene['goal']) 193 | # env.add_control_points(control_pts) 194 | # ax = plot_environment(env) 195 | 196 | # xc = [p[0] for p in control_pts] 197 | # yc = [p[1] for p in control_pts] 198 | # v = 5 199 | # dt = 0.1 200 | # bc_headings = (np.pi/8, -np.pi/12) 201 | # xs, ys, psi = tgen.sample_trajectory(xc, yc, bc_headings, v, dt) 202 | 203 | # constraints = linear_obstacle_constraints(env.obstacles[0]) 204 | # print(constraints) 205 | # ax.plot(xs, ys, 'ob', alpha=0.8, markersize=4, color='darkblue') 206 | 207 | # plt.show() 208 | -------------------------------------------------------------------------------- /gifs/anim.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mschoder/vehicle_mpc/6ac3740539fef60224a04899ea5abbde81da6728/gifs/anim.gif -------------------------------------------------------------------------------- /gifs/animation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mschoder/vehicle_mpc/6ac3740539fef60224a04899ea5abbde81da6728/gifs/animation.gif -------------------------------------------------------------------------------- /gifs/nine_obs_no_noise.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mschoder/vehicle_mpc/6ac3740539fef60224a04899ea5abbde81da6728/gifs/nine_obs_no_noise.gif -------------------------------------------------------------------------------- /img/five_obs_base.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mschoder/vehicle_mpc/6ac3740539fef60224a04899ea5abbde81da6728/img/five_obs_base.png -------------------------------------------------------------------------------- /optimization.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from pydrake.all import (Variable, MathematicalProgram, 3 | OsqpSolver, Solve, SnoptSolver, eq) 4 | from pydrake.solvers import branch_and_bound, gurobi 5 | import pydrake.symbolic as sym 6 | import time 7 | import dynamics 8 | import environment 9 | import trajectory_gen 10 | 11 | 12 | # GLOBALS 13 | n_x = 5 14 | n_u = 2 15 | buffer = 0.3 16 | Q = np.diag([1, 1, .05, .1, 0]) 17 | R = np.diag([.1, .01]) 18 | Q_slack = 10 19 | bounds = { 20 | 'accel_max': 10, 21 | 'accel_min': -10, 22 | 'steerv_max': 4, 23 | 'steer_max_deg': 60, 24 | 'v_max': 20, 25 | } 26 | 27 | 28 | ##### Optimization for linearized dynamics ##### 29 | def linear_optimization(env, x_ref, x_bar, u_bar, x0, N, dt): 30 | ''' Builds and solves single step linear optimization problem 31 | with convex polygon obstacles and linearized dynamics 32 | ''' 33 | prog, dvs = initialize_problem(n_x, n_u, N, x0) 34 | prog = linear_dynamics_constraints(prog, dvs, x_bar, u_bar, N, dt) 35 | prog = add_input_state_constraints(prog, dvs, bounds) 36 | prog, dvs = add_polyhedron_obstacle_constraints( 37 | prog, dvs, env.obstacles, buffer, N) 38 | prog = add_costs(prog, dvs, env, x_ref, Q, R, N) 39 | 40 | solver = gurobi.GurobiSolver() 41 | # solver.AcquireLicense() 42 | result = solver.Solve(prog) 43 | # assert(result.is_success), "Optimization Failed" 44 | status = result.get_solver_details().optimization_status 45 | assert(status == 2), "Optimization failed with code: " + str(status) 46 | 47 | x, u, z, v = dvs 48 | x_sol = result.GetSolution(x) 49 | u_sol = result.GetSolution(u) 50 | z_sol = [result.GetSolution(zj) for zj in z] 51 | v_sol = [result.GetSolution(vj) for vj in v] 52 | return result, (x_sol, u_sol, z_sol, v_sol) 53 | 54 | 55 | def iterative_linear_mpc(env, x_ref, x_bar, u_bar, x0, N, dt, max_iters=5): 56 | ''' 57 | Iteratively run optimization for linearized dynamics until input `u` converges 58 | with nominal input `u_bar`, which helps to reduce linearization error 59 | ''' 60 | start = time.perf_counter() 61 | eps = 0.05 62 | 63 | if u_bar is None: 64 | u_opt = np.zeros((n_u, N-1)) 65 | else: 66 | u_opt = u_bar 67 | 68 | if x_bar is None: 69 | x_bar = dynamics.rollout(x0, u_opt, n_x, dt) 70 | 71 | nominals = [x_bar] # container for nominal traj at each iteration 72 | 73 | for i in range(max_iters): 74 | u_prev = u_opt 75 | result, (x_opt, u_opt, z_opt, v_opt) = linear_optimization( 76 | env, x_ref, x_bar, u_opt, x0, N, dt) 77 | u_diff = np.sum(np.abs(u_opt - u_prev)) 78 | if u_diff < eps: 79 | break 80 | 81 | if len(u_opt.shape) == 1: 82 | # handle 1D array output for last iteration 83 | u_opt = u_opt.reshape(u_opt.shape[0], -1) 84 | x_bar = dynamics.rollout(x0, u_opt, n_x, dt) 85 | nominals.append(x_bar) 86 | 87 | end = time.perf_counter() 88 | print('\t', i+1, ' iterations, final control diff: ', np.round(u_diff, 6), 89 | ', Elapsed time (s): ', np.round(end-start, 4)) 90 | return result, (x_opt, u_opt, z_opt, v_opt), x_bar, nominals 91 | 92 | 93 | ##### Nonlinear optimization problem ####### 94 | def nonlinear_optimization(env, x_ref, x_bar, u_bar, x0, N, dt): 95 | ''' Builds and solves single step linear optimization problem 96 | with convex polygon obstacles and linearized dynamics 97 | ''' 98 | prog, dvs = initialize_problem(n_x, n_u, N, x0) 99 | prog = nonlinear_dynamics_constraints(prog, dvs, x_bar, u_bar, N, dt) 100 | prog = add_input_state_constraints(prog, dvs, bounds) 101 | prog, dvs = add_polyhedron_obstacle_constraints( 102 | prog, dvs, env.obstacles, buffer, N) 103 | prog = add_costs(prog, dvs, env, x_ref, Q, R, N) 104 | 105 | # Use SNOPT solver 106 | solver = SnoptSolver() 107 | result = solver.Solve(prog) 108 | assert(result.is_success), "Optimization Failed" 109 | 110 | x, u, z, v = dvs 111 | x_sol = result.GetSolution(x) 112 | u_sol = result.GetSolution(u) 113 | z_sol = [result.GetSolution(zj) for zj in z] 114 | v_sol = [result.GetSolution(vj) for vj in v] 115 | return result, (x_sol, u_sol, z_sol, v_sol), x_bar 116 | 117 | ##### Helper functions ###### 118 | 119 | 120 | def initialize_problem(n_x, n_u, N, x0): 121 | ''' returns base MathematicalProgram object and empty decision vars ''' 122 | prog = MathematicalProgram() 123 | 124 | # Decision Variables 125 | x = prog.NewContinuousVariables(n_x, N, 'x') 126 | u = prog.NewContinuousVariables(n_u, N-1, 'u') 127 | z = [] # placeholder for obstacle binary variables 128 | q = [] # placeholder for obstacle slack buffer variables 129 | 130 | # initial conditions constraints 131 | prog.AddBoundingBoxConstraint(x0, x0, x[:, 0]) 132 | 133 | decision_vars = [x, u, z, q] 134 | return prog, decision_vars 135 | 136 | 137 | def linear_dynamics_constraints(prog, decision_vars, x_bar, u_bar, N, dt): 138 | ''' 139 | Dynamics Constraints -- error state form 140 | x_bar - nominal point; dx = (x - x_bar) 141 | A, B, C = get_linear_dynamics(derivs, x_bar[:,n], u_bar[:,n]) 142 | ''' 143 | x, u, _, _ = decision_vars 144 | derivs = dynamics.derivatives(dynamics.discrete_dynamics, n_x, n_u) 145 | for n in range(N-1): 146 | A, B = dynamics.get_linear_dynamics(derivs, x_bar[:, n], u_bar[:, n]) 147 | dx = x[:, n] - x_bar[:, n] 148 | du = u[:, n] - u_bar[:, n] 149 | dxdt = A@dx + B@du 150 | fxu_bar = dynamics.car_continuous_dynamics(x_bar[:, n], u_bar[:, n]) 151 | for i in range(n_x): 152 | prog.AddConstraint(x[i, n+1] == x[i, n] + 153 | (fxu_bar[i] + dxdt[i])*dt) 154 | return prog 155 | 156 | 157 | def nonlinear_dynamics_constraints(prog, decision_vars, x_bar, u_bar, N, dt): 158 | ''' Add nonlinear dynamics constraints in original form ''' 159 | x, u, _, _ = decision_vars 160 | for n in range(N-1): 161 | psi = x[2, n] 162 | v = x[3, n] 163 | steer = x[4, n] 164 | prog.AddConstraint(x[0, n+1] == x[0, n] + dt*v*sym.cos(psi)) 165 | prog.AddConstraint(x[1, n+1] == x[1, n] + dt*v*sym.sin(psi)) 166 | prog.AddConstraint(x[2, n+1] == x[2, n] + dt*v*sym.tan(steer)) 167 | prog.AddConstraint(x[3, n+1] == x[3, n] + dt*u[0, n]) 168 | prog.AddConstraint(x[4, n+1] == x[4, n] + dt*u[1, n]) 169 | return prog 170 | 171 | 172 | def add_input_state_constraints(prog, decision_vars, bounds): 173 | ''' Docstring ''' 174 | x, u, _, _ = decision_vars 175 | b = bounds 176 | 177 | # input bounds on each timestep n 178 | prog.AddBoundingBoxConstraint( 179 | b['accel_min'], b['accel_max'], u[0, :]) # acceleration 180 | # steering velocity 181 | prog.AddBoundingBoxConstraint(-b['steerv_max'], b['steerv_max'], u[1, :]) 182 | 183 | # Velocity bounds on each timestep (velocity must be positive) 184 | prog.AddBoundingBoxConstraint(0, b['v_max'], x[3, :]) 185 | 186 | # Steer angle 187 | max_steer = np.deg2rad(b['steer_max_deg']) 188 | prog.AddBoundingBoxConstraint(-max_steer, max_steer, x[4, :]) 189 | 190 | return prog 191 | 192 | 193 | def add_polyhedron_obstacle_constraints(prog, decision_vars, obs_list, buffer, N): 194 | M = 1e6 195 | x, u, z, q = decision_vars 196 | z = [[]]*len(obs_list) 197 | q = [[]]*len(obs_list) 198 | for j, ob in enumerate(obs_list): 199 | constraints = environment.linear_obstacle_constraints(ob, buffer) 200 | K = len(constraints) 201 | z[j] = prog.NewBinaryVariables(rows=N, cols=K) 202 | q[j] = prog.NewContinuousVariables(rows=N, cols=K) 203 | prog.AddBoundingBoxConstraint(0, 0, q[j][0, :]) # first one is unused 204 | 205 | for n in range(1, N): # Don't constrain initial position in case already inside obstacle 206 | for k in range(K): 207 | a, b, c = constraints[k] 208 | # format: cy <= ax + b - buffer + Mz 209 | prog.AddConstraint( 210 | c*x[1, n] <= a*x[0, n] + b + M*z[j][n, k] + q[j][n, k]) 211 | prog.AddConstraint(q[j][n, k] >= 0) 212 | prog.AddConstraint(z[j][n, :].sum() <= K-1) 213 | 214 | dvs = [x, u, z, q] 215 | return prog, dvs 216 | 217 | 218 | def add_costs(prog, decision_vars, env, x_ref, Q, R, N): 219 | ''' Quadratic costs of form xTQx + uTRu ''' 220 | x, u, z, q = decision_vars 221 | J = len(env.obstacles) 222 | for n in range(N): 223 | prog.AddQuadraticErrorCost(Q, x_ref[:, n], x[:, n]) 224 | if (n < N - 1): 225 | b = np.zeros(2) 226 | prog.AddQuadraticCost(R, b, u[:, n]) 227 | 228 | slack_cost = Q_slack*np.sum([q[j].sum() for j in range(J)]) 229 | prog.AddCost(slack_cost) 230 | 231 | return prog 232 | 233 | 234 | if __name__ == "__main__": 235 | 236 | # Test 237 | pass 238 | -------------------------------------------------------------------------------- /scenarios.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | two_ellipse = {'start': (0, 0), 4 | 'goal': [(12, 8), (12, 9), (13, 9), (13, 8)], 5 | 'obs_list': [[5.5, 2.0, 4.5, 1.5, -35], 6 | [8, 7, 4.2, 2.0, 65]] 7 | } 8 | 9 | 10 | one_obstacle = {'start': (0, 0), 11 | 'goal': [(5.9, 6.1), (5.9, 7.1), (6.9, 7.1), (6.9, 6.1)], 12 | 'obs_list': [[(3, 2), (2, 6), (4, 5), (6, 3)]], 13 | 14 | } 15 | 16 | 17 | two_obstacle = {'start': (3, 0), 18 | 'goal': [(5, 6), (5, 7), (6, 7), (6, 6)], 19 | 'obs_list': [[(1, 3), (4, 1), (4, 3)], 20 | [(4.5, 6), (4.0, 4), (7.5, 3)]], 21 | 'control_pts': [(3, 0), (4.5, 1), (5, 2.8), 22 | (3.5, 3.8), (2, 5.1), (3.7, 6.7), (5.5, 6.3)], 23 | 'bc_headings': (np.pi/6, -np.pi/6), 24 | 'control_pts_bad': [(3, 0), (3.8, 1.3), (4.5, 2.8), 25 | (4.1, 4.1), (2.5, 5.4), (3.7, 6.7), (5.5, 6.5)], 26 | 'bc_headings_bad': (np.pi/4, -np.pi/6) 27 | } 28 | 29 | quad_boxes = {'start': (3.1, 0), 30 | 'goal': [(3.5, 6.5), (2.5, 6.5), (2.5, 5.5), (3.5, 5.5)], 31 | 'obs_list': [[(1, 1), (1, 3), (3, 3), (3, 1)], 32 | [(1, 3), (1, 5), (3, 5), (3, 3)], 33 | [(3, 1), (3, 3), (5, 3), (5, 1)], 34 | [(3, 3), (3, 5), (5, 5), (5, 3)]], 35 | } 36 | 37 | five_obstacle = {'start': (0, 0), 38 | 'goal': [(12, 12), (12, 14), (14, 14), (14, 12)], 39 | 'obs_list': [[(2, 1), (3, 1), (3, 4), (1, 4)], 40 | [(5, 1), (6, 3), (5, 5), (4, 3)], 41 | [(1, 6), (6, 6), (6, 7), (2, 9)], 42 | [(11, 4), (14, 5), (8, 11), (6, 9)], 43 | [(5, 10), (7, 11), (8, 14), (4, 13)]], 44 | 'control_pts': [(0, 0), (3.5, 1), (4.0, 4.5), (6.4, 6.2), 45 | (5.8, 9.4), (8.4, 11.5), (13, 13)], 46 | 'bc_headings': (np.pi/8, np.pi/8), 47 | 'control_pts_bad': [(0, 0), (4, 1), (4.3, 5), (6.2, 6.9), 48 | (5.5, 9.3), (8.7, 10.7), (13, 13)], 49 | 'bc_headings_bad': (np.pi/8, np.pi/6) 50 | } 51 | 52 | 53 | nine_obstacle = {'start': (0, 0), 54 | 'goal': [(15, 9), (15, 11), (17, 11), (17, 9)], 55 | 'obs_list': [[(1, 1), (2, 1), (2, 4), (0, 4)], 56 | [(5, -1), (6, 1), (5, 3), (4, 1)], 57 | [(1, 6), (6, 6), (6, 7), (2, 9)], 58 | [(11, 0), (14, 1), (10, 7), (8, 5)], 59 | [(7, 7), (9, 8), (10, 11), (6, 10)], 60 | [(6.3, 3.5), (5.8, 4.8), 61 | (6.9, 5.2), (7.3, 3.9)], 62 | [(11.8, 7.2), (10.3, 9), (13, 10), 63 | (14.3, 9.2)], 64 | [(10.5, 12.5), (12, 10.2), 65 | (13.5, 10.5), (14.9, 13)], 66 | [(13.5, 7), (13.5, 5), (15, 7)]], 67 | 'control_pts': [(0, 0), (4.0, 2.6), (7.5, 3), (6, 9), (10.5, 12), 68 | (11.5, 6.0), (16, 10)], 69 | 'bc_headings': (np.pi/8, np.pi/2), 70 | 'control_pts_bad': [(0, 0), (4, 1), (4.3, 5), (6.2, 6.9), 71 | (5.5, 9.3), (8.7, 10.7), (13, 13)], 72 | 'bc_headings_bad': (np.pi/8, np.pi/6) 73 | } 74 | -------------------------------------------------------------------------------- /simulation.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import dynamics 3 | import optimization 4 | 5 | 6 | def simulate_lmpc(env, x_ref_full, noise, x0, N, dt, history={}): 7 | 8 | n_x = x_ref_full.shape[0] 9 | n_u = 2 10 | sigmas = noise 11 | nsteps = x_ref_full.shape[1] 12 | 13 | x_bar = x_ref_full[:, 0:N] 14 | u_bar = u_bar = np.zeros((n_u, N-1)) 15 | 16 | n = N 17 | for s in range(nsteps): 18 | print('Step ', s, '/', nsteps) 19 | 20 | # Get new reference data, stepped by one 21 | end = N + s 22 | if end > x_ref_full.shape[1]: 23 | n = x_ref_full.shape[1] - s 24 | end = x_ref_full.shape[1] 25 | 26 | x_ref = x_ref_full[:, s:end] 27 | result, (x_opt, u_opt, z_opt, v_opt), x_bar, _ = optimization.iterative_linear_mpc( 28 | env, x_ref, x_bar, u_bar, x0, n, dt, max_iters=5) 29 | 30 | if len(x_opt.shape) == 1: # handle 1D array output for last iteration 31 | x_opt = x_opt.reshape(x_opt.shape[0], -1) 32 | 33 | if u_opt.size > 0: 34 | 35 | if len(u_opt.shape) == 1: # handle 1D array output for last iteration 36 | u_opt = u_opt.reshape(u_opt.shape[0], -1) 37 | 38 | # Evolve dynamics and get updated reference and nominal data 39 | u0 = u_opt[:, 0] 40 | x0 = dynamics.step_car_dynamics(x0, u0, sigmas, dt) 41 | 42 | if N + s >= x_ref_full.shape[1]: 43 | u_bar = u_opt[:, 1:] 44 | else: # repeat last control as starting seed 45 | u_bar = np.hstack((u_opt[:, 1:], u_opt[:, -1:])) 46 | 47 | if len(u_opt.shape) == 1: # handle 1D array output for last iteration 48 | u_bar = u_bar.reshape(u_bar.shape[0], -1) 49 | 50 | x_bar = dynamics.rollout(x0, u_bar, n_x, dt) 51 | 52 | # Log results 53 | history[s] = {'x_opt': x_opt, 'u_opt': u_opt, 'z_opt': z_opt, 54 | 'v_opt': v_opt, 'x_bar': x_bar, 'x0': x0, 'result': result} 55 | 56 | return history 57 | -------------------------------------------------------------------------------- /trajectory_gen.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | from scipy.interpolate import CubicSpline 4 | import matplotlib.pyplot as plt 5 | 6 | 7 | def sample_trajectory(ctrl_pts, bc_headings, v, dt): 8 | ''' Given control points [(x,y)], boundary condition headings, fixed velocity v, 9 | return a sampled C2 trajectory every time period dt 10 | ''' 11 | x = [p[0] for p in ctrl_pts] 12 | y = [p[1] for p in ctrl_pts] 13 | cx, cy = calc_c2_traj(x, y, bc_headings) 14 | 15 | total_length = 0 16 | for i in range(cx.c.shape[1]): 17 | coeffs_x = np.flip(cx.c[:, i]) 18 | coeffs_y = np.flip(cy.c[:, i]) 19 | slen = calc_spline_length(coeffs_x, coeffs_y) 20 | total_length += slen 21 | 22 | nsteps = int(total_length/(dt*v)) 23 | # tvec = np.arange(0, len(x)-1+dt, dt) 24 | tvec = np.linspace(0, len(x)-1, nsteps) 25 | xs = cx(tvec) 26 | ys = cy(tvec) 27 | 28 | # calc heading 29 | dxs = cx(tvec, 1) 30 | dys = cy(tvec, 1) 31 | psi = np.arctan2(dys, dxs) 32 | return xs, ys, psi 33 | 34 | 35 | def calc_c2_traj(x, y, bc_headings, eps=0.005): 36 | ''' 37 | Iteratively compute spline coefficients until spline length of first and last segment converges 38 | ''' 39 | 40 | # Start with euclidean dist as slen approx for first and last segments 41 | slen_start = np.sqrt((x[1] - x[0])**2 + (y[1] - y[0])**2) 42 | slen_end = np.sqrt((x[-1] - x[-2])**2 + (y[-1] - y[-2])**2) 43 | 44 | while True: 45 | cx, cy = gen_c2_spline(x, y, bc_headings, slen_start, slen_end) 46 | coeffs_x_start = np.flip(cx.c[:, 0]) 47 | coeffs_y_start = np.flip(cy.c[:, 0]) 48 | coeffs_x_end = np.flip(cx.c[:, -1]) 49 | coeffs_y_end = np.flip(cy.c[:, -1]) 50 | 51 | slen_start_new = calc_spline_length(coeffs_x_start, coeffs_y_start) 52 | slen_end_new = calc_spline_length(coeffs_x_end, coeffs_y_end) 53 | 54 | if abs(slen_start_new - slen_start) < eps and abs(slen_end_new - slen_end) < eps: 55 | break 56 | else: 57 | slen_start = slen_start_new 58 | slen_end = slen_end_new 59 | return cx, cy 60 | 61 | 62 | def gen_c2_spline(x, y, bc_headings, slen_start, slen_end): 63 | ''' 64 | Generates a C2 continuous spline using scipy CubicSpline lib 65 | x: np.array of x-coordinate points 66 | y: np.array of y-coordinate points 67 | ''' 68 | 69 | # define mu, a virtual path variable of length 1 for each spline segment 70 | assert(len(x) == len(y)) 71 | mu = np.arange(0, len(x), 1.0) 72 | 73 | # build splines 74 | cs_x = CubicSpline(mu, x, 75 | bc_type=((1, slen_start * np.cos(bc_headings[0])), 76 | (1, slen_end * np.cos(bc_headings[1])))) 77 | cs_y = CubicSpline(mu, y, 78 | bc_type=((1, slen_start * np.sin(bc_headings[0])), 79 | (1, slen_end * np.sin(bc_headings[1])))) 80 | return cs_x, cs_y 81 | 82 | 83 | def unitvec_from_heading(theta): 84 | x = np.cos(theta) 85 | y = np.sin(theta) 86 | ds = (x**2 + y**2)**0.5 87 | return (x/ds, y/ds) 88 | 89 | 90 | def calc_spline_length(x_coeffs, y_coeffs, n_ips=20): 91 | ''' 92 | Returns numerically computed length along cubic spline 93 | x_coeffs: array of 4 x coefficients 94 | y_coeffs: array of 4 y coefficients 95 | ''' 96 | 97 | t_steps = np.linspace(0.0, 1.0, n_ips) 98 | spl_coords = np.zeros((n_ips, 2)) 99 | 100 | spl_coords[:, 0] = x_coeffs[0] \ 101 | + x_coeffs[1] * t_steps \ 102 | + x_coeffs[2] * np.power(t_steps, 2) \ 103 | + x_coeffs[3] * np.power(t_steps, 3) 104 | spl_coords[:, 1] = y_coeffs[0] \ 105 | + y_coeffs[1] * t_steps \ 106 | + y_coeffs[2] * np.power(t_steps, 2) \ 107 | + y_coeffs[3] * np.power(t_steps, 3) 108 | 109 | slength = np.sum( 110 | np.sqrt(np.sum(np.power(np.diff(spl_coords, axis=0), 2), axis=1))) 111 | return slength 112 | 113 | 114 | def plot_trajectory(x, y, bch, cx, cy, stepsize=0.1): 115 | ''' 116 | Plots x-y coords and cx(t)-cy(t) parametric spline 117 | Plots unit vectors showing the spline headings at boundaries 118 | Generates c1 and c2 plots showing heading and curvature continuity 119 | ''' 120 | 121 | ts = np.arange(0, len(x)-1+stepsize, stepsize) 122 | ts_plus = np.arange(ts[0]-.2, ts[-1]+.3, stepsize) 123 | 124 | # Heading constraint unit vectors 125 | hvec_start = unitvec_from_heading(bch[0]) 126 | hvec_end = unitvec_from_heading(bch[-1]) 127 | 128 | # Plot trajectory 129 | fig, ax = plt.subplots(figsize=(10, 5)) 130 | ax.set_ylim(min(y)-1, max(y)+1) 131 | ax.set_xlim(min(x)-2, max(x)+2) 132 | ax.plot(x, y, 'o', label='nodes') 133 | ax.plot(cx(ts_plus), cy(ts_plus), label='spline') 134 | ax.annotate("", xy=(x[0] + hvec_start[0], y[0] + hvec_start[1]), 135 | xytext=(x[0], y[0]), arrowprops=dict(arrowstyle="->", color="red")) 136 | ax.annotate("", xy=(x[-1] + hvec_end[0], y[-1] + hvec_end[1]), 137 | xytext=(x[-1], y[-1]), arrowprops=dict(arrowstyle="->", color="red")) 138 | ax.set_aspect('equal') 139 | ax.set_title('C2 trajectory') 140 | ax.set_xlabel('x(mu)') 141 | ax.set_ylabel('y(mu)') 142 | 143 | # Plot heading and curvature 144 | fig, ax = plt.subplots(1, 2, figsize=(14, 4)) 145 | ax[0].set_title('X(mu)') 146 | ax[0].plot(ts, cx(ts, 1), label='Heading') 147 | ax[0].plot(ts, cx(ts, 2), label='Curvature') 148 | ax[0].set_xlabel('mu') 149 | ax[0].legend() 150 | ax[1].set_title('Y(mu)') 151 | ax[1].plot(ts, cy(ts, 1), label='Heading') 152 | ax[1].plot(ts, cy(ts, 2), label='Curvature') 153 | ax[1].set_xlabel('mu') 154 | ax[1].legend() 155 | 156 | 157 | ## Test Case ## 158 | if __name__ == "__main__": 159 | 160 | # points defined as (x,y,theta) 161 | bc_headings = (np.pi/8, -np.pi/12) 162 | nodes = [(0, 0, bc_headings[0]), 163 | (2.0, 0.3, None), 164 | (3.0, 0.5, None), 165 | (4.1, -0.2, None), 166 | (5.0, -0.4, None), 167 | (6.0, 0.0, bc_headings[1])] 168 | x = np.array([i[0] for i in nodes]) 169 | y = np.array([i[1] for i in nodes]) 170 | bch = np.array([i[2] for i in nodes]) 171 | theta = np.array([i[2] for i in nodes]) 172 | 173 | # Heading constraint unit vectors 174 | hvec_start = unitvec_from_heading(bch[0]) 175 | hvec_end = unitvec_from_heading(bch[-1]) 176 | 177 | # Plot test case 178 | # fig = plt.figure(figsize=(10, 5)) 179 | # ax = fig.add_subplot(111) 180 | # ax.set_ylim(-1.5, 1.5) 181 | # ax.set_xlim(-1, 7.5) 182 | # ax.scatter(x, y) 183 | # ax.annotate("", xy=(x[0] + hvec_start[0], y[0] + hvec_start[1]), 184 | # xytext=(x[0], y[0]), arrowprops=dict(arrowstyle="->", color="red")) 185 | # ax.annotate("", xy=(x[-1] + hvec_end[0], y[-1] + hvec_end[1]), 186 | # xytext=(x[-1], y[-1]), arrowprops=dict(arrowstyle="->", color="red")) 187 | # ax.set_aspect('equal') 188 | 189 | # Test iterative calc trajectory generation 190 | cx, cy = calc_c2_traj(x, y, bc_headings) 191 | 192 | # test sampling 193 | v = 2.0 194 | dt = 0.1 195 | xs, ys, psi = sample_trajectory(x, y, bc_headings, v, dt) 196 | plt.figure() 197 | plt.plot(xs, ys, 'o') 198 | plt.plot(x[0], y[0], 'ko') 199 | plt.plot(x[-1], y[-1], 'ko') 200 | plt.axis('equal') 201 | plt.show() 202 | 203 | # Plot 204 | plot_trajectory(x, y, bch, cx, cy) 205 | # plt.show() 206 | 207 | # ## Test single spline segment case 208 | # xr = np.array([0,2]) 209 | # yr = np.array([1,4]) 210 | # bcr = np.array([0, np.pi/8]) 211 | # cxr, cyr = calc_c2_traj(xr, yr, bcr) 212 | 213 | # ## Plot 214 | # plot_trajectory(xr, yr, bcr, cxr, cyr) 215 | -------------------------------------------------------------------------------- /vids/five_obs_no_noise_01.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mschoder/vehicle_mpc/6ac3740539fef60224a04899ea5abbde81da6728/vids/five_obs_no_noise_01.mp4 -------------------------------------------------------------------------------- /vids/nine_obs_no_noise.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mschoder/vehicle_mpc/6ac3740539fef60224a04899ea5abbde81da6728/vids/nine_obs_no_noise.mp4 --------------------------------------------------------------------------------