def test_optimal_basis_simple(): sys = ct.ss2io(ct.ss([[1, 1], [0, 1]], [[1], [0.5]], np.eye(2), 0, 1)) # State and input constraints constraints = [ (sp.optimize.LinearConstraint, np.eye(3), [-5, -5, -1], [5, 5, 1]), ] # Quadratic state and input penalty Q = [[1, 0], [0, 1]] R = [[1]] cost = opt.quadratic_cost(sys, Q, R) # Set up the optimal control problem Tf = 5 time = np.arange(0, Tf, 1) x0 = [4, 0] # Basic optimal control problem res1 = opt.solve_ocp(sys, time, x0, cost, constraints, basis=flat.BezierFamily(4, Tf), return_x=True) assert res1.success # Make sure the constraints were satisfied np.testing.assert_array_less(np.abs(res1.states[0]), 5 + 1e-6) np.testing.assert_array_less(np.abs(res1.states[1]), 5 + 1e-6) np.testing.assert_array_less(np.abs(res1.inputs[0]), 1 + 1e-6) # Pass an initial guess and rerun res2 = opt.solve_ocp(sys, time, x0, cost, constraints, initial_guess=0.99 * res1.inputs, basis=flat.BezierFamily(4, Tf), return_x=True) assert res2.success np.testing.assert_allclose(res2.inputs, res1.inputs, atol=0.01, rtol=0.01) # Run with logging turned on for code coverage res3 = opt.solve_ocp(sys, time, x0, cost, constraints, basis=flat.BezierFamily(4, Tf), return_x=True, log=True) assert res3.success np.testing.assert_almost_equal(res3.inputs, res1.inputs, decimal=3)
def time_steering_bezier_basis(nbasis, ntimes): # Set up costs and constriants Q = np.diag([.1, 10, .1]) # keep lateral error low R = np.diag([1, 1]) # minimize applied inputs cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf) constraints = [opt.input_range_constraint(vehicle, [0, -0.1], [20, 0.1])] terminal = [opt.state_range_constraint(vehicle, xf, xf)] # Set up horizon horizon = np.linspace(0, Tf, ntimes, endpoint=True) # Set up the optimal control problem res = opt.solve_ocp( vehicle, horizon, x0, cost, constraints, terminal_constraints=terminal, initial_guess=bend_left, basis=flat.BezierFamily(nbasis, T=Tf), # solve_ivp_kwargs={'atol': 1e-4, 'rtol': 1e-2}, minimize_method='trust-constr', minimize_options={'finite_diff_rel_step': 0.01}, # minimize_method='SLSQP', minimize_options={'eps': 0.01}, return_states=True, print_summary=False) t, u, x = res.time, res.inputs, res.states # Make sure we found a valid solution assert res.success
def test_bezier_basis(self): bezier = fs.BezierFamily(4) time = np.linspace(0, 1, 100) # Sum of the Bezier curves should be one np.testing.assert_almost_equal( 1, sum([bezier(i, time) for i in range(4)])) # Sum of derivatives should be zero for k in range(1, 5): np.testing.assert_almost_equal( 0, sum([bezier.eval_deriv(i, k, time) for i in range(4)])) # Compare derivatives to formulas np.testing.assert_almost_equal(bezier.eval_deriv(1, 0, time), 3 * time - 6 * time**2 + 3 * time**3) np.testing.assert_almost_equal(bezier.eval_deriv(1, 1, time), 3 - 12 * time + 9 * time**2) np.testing.assert_almost_equal(bezier.eval_deriv(1, 2, time), -12 + 18 * time) # Make sure that the second derivative integrates to the first time = np.linspace(0, 1, 1000) dt = np.diff(time) for N in range(5): bezier = fs.BezierFamily(N) for i in range(N): for j in range(1, N + 1): np.testing.assert_allclose( np.diff(bezier.eval_deriv(i, j - 1, time)) / dt, bezier.eval_deriv(i, j, time)[0:-1], atol=0.01, rtol=0.01) # Exception check with pytest.raises(ValueError, match="index too high"): bezier.eval_deriv(4, 0, time)
def time_steering_point_to_point(basis_name, basis_size): if basis_name == 'poly': basis = flat.PolyFamily(basis_size) elif basis_name == 'bezier': basis = flat.BezierFamily(basis_size) # Find trajectory between initial and final conditions traj = flat.point_to_point(vehicle, Tf, x0, u0, xf, uf, basis=basis) # Verify that the trajectory computation is correct x, u = traj.eval([0, Tf]) np.testing.assert_array_almost_equal(x0, x[:, 0]) np.testing.assert_array_almost_equal(u0, u[:, 0]) np.testing.assert_array_almost_equal(xf, x[:, 1]) np.testing.assert_array_almost_equal(uf, u[:, 1])
class TestFlatSys: """Test differential flat systems""" @pytest.mark.parametrize("xf, uf, Tf", [([1, 0], [0], 2), ([0, 1], [0], 3), ([1, 1], [1], 4)]) def test_double_integrator(self, xf, uf, Tf): # Define a second order integrator sys = ct.StateSpace([[-1, 1], [0, -2]], [[0], [1]], [[1, 0]], 0) flatsys = fs.LinearFlatSystem(sys) # Define the basis set poly = fs.PolyFamily(6) x1, u1, = [0, 0], [0] traj = fs.point_to_point(flatsys, Tf, x1, u1, xf, uf, basis=poly) # Verify that the trajectory computation is correct x, u = traj.eval([0, Tf]) np.testing.assert_array_almost_equal(x1, x[:, 0]) np.testing.assert_array_almost_equal(u1, u[:, 0]) np.testing.assert_array_almost_equal(xf, x[:, 1]) np.testing.assert_array_almost_equal(uf, u[:, 1]) # Simulate the system and make sure we stay close to desired traj T = np.linspace(0, Tf, 100) xd, ud = traj.eval(T) t, y, x = ct.forced_response(sys, T, ud, x1, return_x=True) np.testing.assert_array_almost_equal(x, xd, decimal=3) @pytest.fixture def vehicle_flat(self): """Differential flatness for a kinematic car""" def vehicle_flat_forward(x, u, params={}): b = params.get('wheelbase', 3.) # get parameter values zflag = [np.zeros(3), np.zeros(3)] # list for flag arrays zflag[0][0] = x[0] # flat outputs zflag[1][0] = x[1] zflag[0][1] = u[0] * np.cos(x[2]) # first derivatives zflag[1][1] = u[0] * np.sin(x[2]) thdot = (u[0] / b) * np.tan(u[1]) # dtheta/dt zflag[0][2] = -u[0] * thdot * np.sin(x[2]) # second derivatives zflag[1][2] = u[0] * thdot * np.cos(x[2]) return zflag def vehicle_flat_reverse(zflag, params={}): b = params.get('wheelbase', 3.) # get parameter values x = np.zeros(3) u = np.zeros(2) # vectors to store x, u x[0] = zflag[0][0] # x position x[1] = zflag[1][0] # y position x[2] = np.arctan2(zflag[1][1], zflag[0][1]) # angle u[0] = zflag[0][1] * np.cos(x[2]) + zflag[1][1] * np.sin(x[2]) thdot_v = zflag[1][2] * np.cos(x[2]) - zflag[0][2] * np.sin(x[2]) u[1] = np.arctan2(thdot_v, u[0]**2 / b) return x, u def vehicle_update(t, x, u, params): b = params.get('wheelbase', 3.) # get parameter values dx = np.array([ np.cos(x[2]) * u[0], np.sin(x[2]) * u[0], (u[0] / b) * np.tan(u[1]) ]) return dx def vehicle_output(t, x, u, params): return x # Create differentially flat input/output system return fs.FlatSystem(vehicle_flat_forward, vehicle_flat_reverse, vehicle_update, vehicle_output, inputs=('v', 'delta'), outputs=('x', 'y', 'theta'), states=('x', 'y', 'theta')) @pytest.mark.parametrize( "poly", [fs.PolyFamily(6), fs.PolyFamily(8), fs.BezierFamily(6)]) def test_kinematic_car(self, vehicle_flat, poly): # Define the endpoints of the trajectory x0 = [0., -2., 0.] u0 = [10., 0.] xf = [100., 2., 0.] uf = [10., 0.] Tf = 10 # Find trajectory between initial and final conditions traj = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=poly) # Verify that the trajectory computation is correct x, u = traj.eval([0, Tf]) np.testing.assert_array_almost_equal(x0, x[:, 0]) np.testing.assert_array_almost_equal(u0, u[:, 0]) np.testing.assert_array_almost_equal(xf, x[:, 1]) np.testing.assert_array_almost_equal(uf, u[:, 1]) # Simulate the system and make sure we stay close to desired traj T = np.linspace(0, Tf, 500) xd, ud = traj.eval(T) # For SciPy 1.0+, integrate equations and compare to desired if StrictVersion(sp.__version__) >= "1.0": t, y, x = ct.input_output_response(vehicle_flat, T, ud, x0, return_x=True) np.testing.assert_allclose(x, xd, atol=0.01, rtol=0.01) def test_flat_cost_constr(self): # Double integrator system sys = ct.ss([[0, 1], [0, 0]], [[0], [1]], [[1, 0]], 0) flat_sys = fs.LinearFlatSystem(sys) # Define the endpoints of the trajectory x0 = [1, 0] u0 = [0] xf = [0, 0] uf = [0] Tf = 10 T = np.linspace(0, Tf, 500) # Find trajectory between initial and final conditions traj = fs.point_to_point(flat_sys, Tf, x0, u0, xf, uf, basis=fs.PolyFamily(8)) x, u = traj.eval(T) np.testing.assert_array_almost_equal(x0, x[:, 0]) np.testing.assert_array_almost_equal(u0, u[:, 0]) np.testing.assert_array_almost_equal(xf, x[:, -1]) np.testing.assert_array_almost_equal(uf, u[:, -1]) # Solve with a cost function timepts = np.linspace(0, Tf, 10) cost_fcn = opt.quadratic_cost(flat_sys, np.diag([0, 0]), 1, x0=xf, u0=uf) traj_cost = fs.point_to_point( flat_sys, timepts, x0, u0, xf, uf, cost=cost_fcn, basis=fs.PolyFamily(8), # initial_guess='lstsq', # minimize_kwargs={'method': 'trust-constr'} ) # Verify that the trajectory computation is correct x_cost, u_cost = traj_cost.eval(T) np.testing.assert_array_almost_equal(x0, x_cost[:, 0]) np.testing.assert_array_almost_equal(u0, u_cost[:, 0]) np.testing.assert_array_almost_equal(xf, x_cost[:, -1]) np.testing.assert_array_almost_equal(uf, u_cost[:, -1]) # Make sure that we got a different answer than before assert np.any(np.abs(x - x_cost) > 0.1) # Re-solve with constraint on the y deviation lb, ub = [-2, -0.1], [2, 0] lb, ub = [-2, np.min(x_cost[1]) * 0.95], [2, 1] constraints = [opt.state_range_constraint(flat_sys, lb, ub)] # Make sure that the previous solution violated at least one constraint assert np.any(x_cost[0, :] < lb[0]) or np.any(x_cost[0, :] > ub[0]) \ or np.any(x_cost[1, :] < lb[1]) or np.any(x_cost[1, :] > ub[1]) traj_const = fs.point_to_point( flat_sys, timepts, x0, u0, xf, uf, cost=cost_fcn, constraints=constraints, basis=fs.PolyFamily(8), ) # Verify that the trajectory computation is correct x_const, u_const = traj_const.eval(T) np.testing.assert_array_almost_equal(x0, x_const[:, 0]) np.testing.assert_array_almost_equal(u0, u_const[:, 0]) np.testing.assert_array_almost_equal(xf, x_const[:, -1]) np.testing.assert_array_almost_equal(uf, u_const[:, -1]) # Make sure that the solution respects the bounds (with some slop) for i in range(x_const.shape[0]): assert np.all(x_const[i] >= lb[i] * 1.02) assert np.all(x_const[i] <= ub[i] * 1.02) # Solve the same problem with a nonlinear constraint type nl_constraints = [(sp.optimize.NonlinearConstraint, lambda x, u: x, lb, ub)] traj_nlconst = fs.point_to_point( flat_sys, timepts, x0, u0, xf, uf, cost=cost_fcn, constraints=nl_constraints, basis=fs.PolyFamily(8), ) x_nlconst, u_nlconst = traj_nlconst.eval(T) np.testing.assert_almost_equal(x_const, x_nlconst) np.testing.assert_almost_equal(u_const, u_nlconst) def test_bezier_basis(self): bezier = fs.BezierFamily(4) time = np.linspace(0, 1, 100) # Sum of the Bezier curves should be one np.testing.assert_almost_equal( 1, sum([bezier(i, time) for i in range(4)])) # Sum of derivatives should be zero for k in range(1, 5): np.testing.assert_almost_equal( 0, sum([bezier.eval_deriv(i, k, time) for i in range(4)])) # Compare derivatives to formulas np.testing.assert_almost_equal(bezier.eval_deriv(1, 0, time), 3 * time - 6 * time**2 + 3 * time**3) np.testing.assert_almost_equal(bezier.eval_deriv(1, 1, time), 3 - 12 * time + 9 * time**2) np.testing.assert_almost_equal(bezier.eval_deriv(1, 2, time), -12 + 18 * time) # Make sure that the second derivative integrates to the first time = np.linspace(0, 1, 1000) dt = np.diff(time) for N in range(5): bezier = fs.BezierFamily(N) for i in range(N): for j in range(1, N + 1): np.testing.assert_allclose( np.diff(bezier.eval_deriv(i, j - 1, time)) / dt, bezier.eval_deriv(i, j, time)[0:-1], atol=0.01, rtol=0.01) # Exception check with pytest.raises(ValueError, match="index too high"): bezier.eval_deriv(4, 0, time) def test_point_to_point_errors(self): """Test error and warning conditions in point_to_point()""" # Double integrator system sys = ct.ss([[0, 1], [0, 0]], [[0], [1]], [[1, 0]], 0) flat_sys = fs.LinearFlatSystem(sys) # Define the endpoints of the trajectory x0 = [1, 0] u0 = [0] xf = [0, 0] uf = [0] Tf = 10 T = np.linspace(0, Tf, 500) # Cost function timepts = np.linspace(0, Tf, 10) cost_fcn = opt.quadratic_cost(flat_sys, np.diag([1, 1]), 1, x0=xf, u0=uf) # Solving without basis specified should be OK traj = fs.point_to_point(flat_sys, timepts, x0, u0, xf, uf) x, u = traj.eval(timepts) np.testing.assert_array_almost_equal(x0, x[:, 0]) np.testing.assert_array_almost_equal(u0, u[:, 0]) np.testing.assert_array_almost_equal(xf, x[:, -1]) np.testing.assert_array_almost_equal(uf, u[:, -1]) # Adding a cost function generates a warning with pytest.warns(UserWarning, match="optimization not possible"): traj = fs.point_to_point(flat_sys, timepts, x0, u0, xf, uf, cost=cost_fcn) # Make sure we still solved the problem x, u = traj.eval(timepts) np.testing.assert_array_almost_equal(x0, x[:, 0]) np.testing.assert_array_almost_equal(u0, u[:, 0]) np.testing.assert_array_almost_equal(xf, x[:, -1]) np.testing.assert_array_almost_equal(uf, u[:, -1]) # Try to optimize with insufficient degrees of freedom with pytest.warns(UserWarning, match="optimization not possible"): traj = fs.point_to_point(flat_sys, timepts, x0, u0, xf, uf, cost=cost_fcn, basis=fs.PolyFamily(6)) # Make sure we still solved the problem x, u = traj.eval(timepts) np.testing.assert_array_almost_equal(x0, x[:, 0]) np.testing.assert_array_almost_equal(u0, u[:, 0]) np.testing.assert_array_almost_equal(xf, x[:, -1]) np.testing.assert_array_almost_equal(uf, u[:, -1]) # Solve with the errors in the various input arguments with pytest.raises(ValueError, match="Initial state: Wrong shape"): traj = fs.point_to_point(flat_sys, timepts, np.zeros(3), u0, xf, uf) with pytest.raises(ValueError, match="Initial input: Wrong shape"): traj = fs.point_to_point(flat_sys, timepts, x0, np.zeros(3), xf, uf) with pytest.raises(ValueError, match="Final state: Wrong shape"): traj = fs.point_to_point(flat_sys, timepts, x0, u0, np.zeros(3), uf) with pytest.raises(ValueError, match="Final input: Wrong shape"): traj = fs.point_to_point(flat_sys, timepts, x0, u0, xf, np.zeros(3)) # Different ways of describing constraints constraint = opt.input_range_constraint(flat_sys, -100, 100) with pytest.warns(UserWarning, match="optimization not possible"): traj = fs.point_to_point(flat_sys, timepts, x0, u0, xf, uf, constraints=constraint, basis=fs.PolyFamily(6)) x, u = traj.eval(timepts) np.testing.assert_array_almost_equal(x0, x[:, 0]) np.testing.assert_array_almost_equal(u0, u[:, 0]) np.testing.assert_array_almost_equal(xf, x[:, -1]) np.testing.assert_array_almost_equal(uf, u[:, -1]) # Constraint that isn't a constraint with pytest.raises(TypeError, match="must be a list"): traj = fs.point_to_point(flat_sys, timepts, x0, u0, xf, uf, constraints=np.eye(2), basis=fs.PolyFamily(8)) # Unknown constraint type with pytest.raises(TypeError, match="unknown constraint type"): traj = fs.point_to_point(flat_sys, timepts, x0, u0, xf, uf, constraints=[(None, 0, 0, 0)], basis=fs.PolyFamily(8)) # Unsolvable optimization constraint = [opt.input_range_constraint(flat_sys, -0.01, 0.01)] with pytest.raises(RuntimeError, match="Unable to solve optimal"): traj = fs.point_to_point(flat_sys, timepts, x0, u0, xf, uf, constraints=constraint, basis=fs.PolyFamily(8)) # Method arguments, parameters traj_method = fs.point_to_point(flat_sys, timepts, x0, u0, xf, uf, cost=cost_fcn, basis=fs.PolyFamily(8), minimize_method='slsqp') traj_kwarg = fs.point_to_point(flat_sys, timepts, x0, u0, xf, uf, cost=cost_fcn, basis=fs.PolyFamily(8), minimize_kwargs={'method': 'slsqp'}) np.testing.assert_allclose(traj_method.eval(timepts)[0], traj_kwarg.eval(timepts)[0], atol=1e-5) # Unrecognized keywords with pytest.raises(TypeError, match="unrecognized keyword"): traj_method = fs.point_to_point(flat_sys, timepts, x0, u0, xf, uf, solve_ivp_method=None)
xf = [75, -2., 0.] uf = [15, 0.] Tf = xf[0] / uf[0] # Define a set of basis functions to use for the trajectories poly = fs.PolyFamily(8) # Find a trajectory between the initial condition and the final condition traj1 = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=poly) plot_vehicle_lanechange(traj1) # ### Change of basis function # In[ ]: bezier = fs.BezierFamily(8) traj2 = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=bezier) plot_vehicle_lanechange(traj2) # ### Added cost function # In[ ]: timepts = np.linspace(0, Tf, 12) poly = fs.PolyFamily(8) traj_cost = opt.quadratic_cost(vehicle_flat, np.diag([0, 0.1, 0]), np.diag([0.1, 10]), x0=xf, u0=uf) constraints = [opt.input_range_constraint(vehicle_flat, [8, -0.1], [12, 0.1])]
def test_terminal_constraints(sys_args): """Test out the ability to handle terminal constraints""" # Create the system sys = ct.ss2io(ct.ss(*sys_args)) # Shortest path to a point is a line Q = np.zeros((2, 2)) R = np.eye(2) cost = opt.quadratic_cost(sys, Q, R) # Set up the terminal constraint to be the origin final_point = [opt.state_range_constraint(sys, [0, 0], [0, 0])] # Create the optimal control problem time = np.arange(0, 3, 1) optctrl = opt.OptimalControlProblem( sys, time, cost, terminal_constraints=final_point) # Find a path to the origin x0 = np.array([4, 3]) res = optctrl.compute_trajectory(x0, squeeze=True, return_x=True) t, u1, x1 = res.time, res.inputs, res.states # Bug prior to SciPy 1.6 will result in incorrect results if NumpyVersion(sp.__version__) < '1.6.0': pytest.xfail("SciPy 1.6 or higher required") np.testing.assert_almost_equal(x1[:,-1], 0, decimal=4) # Make sure it is a straight line Tf = time[-1] if ct.isctime(sys): # Continuous time is not that accurate on the input, so just skip test pass else: # Final point doesn't affect cost => don't need to test np.testing.assert_almost_equal( u1[:, 0:-1], np.kron((-x0/Tf).reshape((2, 1)), np.ones(time.shape))[:, 0:-1]) np.testing.assert_allclose( x1, np.kron(x0.reshape((2, 1)), time[::-1]/Tf), atol=0.1, rtol=0.01) # Re-run using initial guess = optional and make sure nothing changes res = optctrl.compute_trajectory(x0, initial_guess=u1) np.testing.assert_almost_equal(res.inputs, u1) # Re-run using a basis function and see if we get the same answer res = opt.solve_ocp(sys, time, x0, cost, terminal_constraints=final_point, basis=flat.BezierFamily(4, Tf)) np.testing.assert_almost_equal(res.inputs, u1, decimal=2) # Impose some cost on the state, which should change the path Q = np.eye(2) R = np.eye(2) * 0.1 cost = opt.quadratic_cost(sys, Q, R) optctrl = opt.OptimalControlProblem( sys, time, cost, terminal_constraints=final_point) # Turn off warning messages, since we sometimes don't get convergence with warnings.catch_warnings(): warnings.filterwarnings( "ignore", message="unable to solve", category=UserWarning) # Find a path to the origin res = optctrl.compute_trajectory( x0, squeeze=True, return_x=True, initial_guess=u1) t, u2, x2 = res.time, res.inputs, res.states # Not all configurations are able to converge (?) if res.success: np.testing.assert_almost_equal(x2[:,-1], 0) # Make sure that it is *not* a straight line path assert np.any(np.abs(x2 - x1) > 0.1) assert np.any(np.abs(u2) > 1) # Make sure next test is useful # Add some bounds on the inputs constraints = [opt.input_range_constraint(sys, [-1, -1], [1, 1])] optctrl = opt.OptimalControlProblem( sys, time, cost, constraints, terminal_constraints=final_point) res = optctrl.compute_trajectory(x0, squeeze=True, return_x=True) t, u3, x3 = res.time, res.inputs, res.states # Check the answers only if we converged if res.success: np.testing.assert_almost_equal(x3[:,-1], 0, decimal=4) # Make sure we got a new path and didn't violate the constraints assert np.any(np.abs(x3 - x1) > 0.1) np.testing.assert_array_less(np.abs(u3), 1 + 1e-6) # Make sure that infeasible problems are handled sensibly x0 = np.array([10, 3]) with pytest.warns(UserWarning, match="unable to solve"): res = optctrl.compute_trajectory(x0, squeeze=True, return_x=True) assert not res.success
# for a much more time resolved set of inputs. print("Approach 4: Bezier basis") import control.flatsys as flat # Compute the optimal control start_time = time.process_time() result4 = opt.solve_ocp( vehicle, horizon, x0, quad_cost, constraints, terminal_constraints=terminal, initial_guess=bend_left, basis=flat.BezierFamily(4, T=Tf), # solve_ivp_kwargs={'method': 'RK45', 'atol': 1e-2, 'rtol': 1e-2}, solve_ivp_kwargs={ 'atol': 1e-3, 'rtol': 1e-2 }, minimize_method='trust-constr', minimize_options={'disp': True}, log=False) print("* Total time = %5g seconds\n" % (time.process_time() - start_time)) # If we are running CI tests, make sure we succeeded if 'PYCONTROL_TEST_EXAMPLES' in os.environ: assert result4.success # Extract and plot the results (+ state trajectory)
x0 = [0., 2., 0.] u0 = [15, 0.] xf = [75, -2., 0.] uf = [15, 0.] Tf = xf[0] / uf[0] # Define a set of basis functions to use for the trajectories poly = fs.PolyFamily(10) # Find a trajectory between the initial condition and the final condition traj1 = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=poly) plot_vehicle_lanechange(traj1) # ## Change of basis function # ## bezier = fs.BezierFamily(8) # 创建阶数8的多项式基础 # Find a trajectory between the initial condition and the final condition traj2 = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=bezier) plot_vehicle_lanechange(traj2) # ### Added cost function # ### timepts = np.linspace(0, Tf, 12) poly = fs.PolyFamily(8) # Create quadratic cost function # Returns a quadratic cost function that can be used for an optimal control problem. traj_cost = opt.quadratic_cost(vehicle_flat, np.diag([0, 0.1, 0]), np.diag([0.1, 10]), x0=xf,