def test_ocp_argument_errors(): 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 time = np.arange(0, 5, 1) x0 = [4, 0] # Trajectory constraints not in the right form with pytest.raises(TypeError, match="constraints must be a list"): res = opt.solve_ocp(sys, time, x0, cost, np.eye(2)) # Terminal constraints not in the right form with pytest.raises(TypeError, match="constraints must be a list"): res = opt.solve_ocp( sys, time, x0, cost, constraints, terminal_constraints=np.eye(2)) # Initial guess in the wrong shape with pytest.raises(ValueError, match="initial guess is the wrong shape"): res = opt.solve_ocp( sys, time, x0, cost, constraints, initial_guess=np.zeros((4,1,1)))
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 test_finite_horizon_simple(): # Define a linear system with constraints # Source: https://www.mpt3.org/UI/RegulationProblem # LTI prediction model 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 time = np.arange(0, 5, 1) x0 = [4, 0] # Retrieve the full open-loop predictions res = opt.solve_ocp( sys, time, x0, cost, constraints, squeeze=True) t, u_openloop = res.time, res.inputs np.testing.assert_almost_equal( u_openloop, [-1, -1, 0.1393, 0.3361, -5.204e-16], decimal=4)
def time_steering_terminal_cost(): # Define cost and constraints traj_cost = opt.quadratic_cost(vehicle, None, np.diag([0.1, 1]), u0=uf) term_cost = opt.quadratic_cost(vehicle, np.diag([1, 10, 10]), None, x0=xf) constraints = [opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1])] res = opt.solve_ocp( vehicle, horizon, x0, traj_cost, constraints, terminal_cost=term_cost, initial_guess=bend_left, print_summary=False, solve_ivp_kwargs={ 'atol': 1e-4, 'rtol': 1e-2 }, # minimize_method='SLSQP', minimize_options={'eps': 0.01} minimize_method='trust-constr', minimize_options={'finite_diff_rel_step': 0.01}, ) # Only count this as a benchmark if we converged assert res.success
def test_optimal_logging(capsys): """Test logging functions (mainly for code coverage)""" sys = ct.ss2io(ct.ss(np.eye(2), np.eye(2), np.eye(2), 0, 1)) # Set up the optimal control problem cost = opt.quadratic_cost(sys, 1, 1) state_constraint = opt.state_range_constraint(sys, [-np.inf, 1], [10, 1]) input_constraint = opt.input_range_constraint(sys, [-100, -100], [100, 100]) time = np.arange(0, 3, 1) x0 = [-1, 1] # Solve it, with logging turned on (with warning due to mixed constraints) with pytest.warns(sp.optimize.optimize.OptimizeWarning, match="Equality and inequality .* same element"): res = opt.solve_ocp(sys, time, x0, cost, input_constraint, terminal_cost=cost, terminal_constraints=state_constraint, log=True) # Make sure the output has info available only with logging turned on captured = capsys.readouterr() assert captured.out.find("process time") != -1
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 time_steering_terminal_constraint(integrator_name, minimizer_name): # Get the integrator and minimizer parameters to use integrator = integrator_table[integrator_name] minimizer = minimizer_table[minimizer_name] # Input cost and terminal constraints R = np.diag([1, 1]) # minimize applied inputs cost = opt.quadratic_cost(vehicle, np.zeros((3, 3)), R, u0=uf) constraints = [opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1])] terminal = [opt.state_range_constraint(vehicle, xf, xf)] res = opt.solve_ocp( vehicle, horizon, x0, cost, constraints, terminal_constraints=terminal, initial_guess=bend_left, log=False, solve_ivp_method=integrator[0], solve_ivp_kwargs=integrator[1], minimize_method=minimizer[0], minimize_options=minimizer[1], ) # Only count this as a benchmark if we converged assert res.success
def test_discrete_lqr(): # oscillator model defined in 2D # Source: https://www.mpt3.org/UI/RegulationProblem A = [[0.5403, -0.8415], [0.8415, 0.5403]] B = [[-0.4597], [0.8415]] C = [[1, 0]] D = [[0]] # Linear discrete-time model with sample time 1 sys = ct.ss2io(ct.ss(A, B, C, D, 1)) # Include weights on states/inputs Q = np.eye(2) R = 1 K, S, E = ct.dlqr(A, B, Q, R) # Compute the integral and terminal cost integral_cost = opt.quadratic_cost(sys, Q, R) terminal_cost = opt.quadratic_cost(sys, S, None) # Solve the LQR problem lqr_sys = ct.ss2io(ct.ss(A - B @ K, B, C, D, 1)) # Generate a simulation of the LQR controller time = np.arange(0, 5, 1) x0 = np.array([1, 1]) _, _, lqr_x = ct.input_output_response(lqr_sys, time, 0, x0, return_x=True) # Use LQR input as initial guess to avoid convergence/precision issues lqr_u = np.array(-K @ lqr_x[0:time.size]) # convert from matrix # Formulate the optimal control problem and compute optimal trajectory optctrl = opt.OptimalControlProblem(sys, time, integral_cost, terminal_cost=terminal_cost, initial_guess=lqr_u) res1 = optctrl.compute_trajectory(x0, return_states=True) # Compare to make sure results are the same np.testing.assert_almost_equal(res1.inputs, lqr_u[0]) np.testing.assert_almost_equal(res1.states, lqr_x) # Add state and input constraints trajectory_constraints = [ (sp.optimize.LinearConstraint, np.eye(3), [-5, -5, -.5], [5, 5, 0.5]), ] # Re-solve res2 = opt.solve_ocp(sys, time, x0, integral_cost, trajectory_constraints, terminal_cost=terminal_cost, initial_guess=lqr_u) # Make sure we got a different solution assert np.any(np.abs(res1.inputs - res2.inputs) > 0.1)
def test_discrete_lqr(): # oscillator model defined in 2D # Source: https://www.mpt3.org/UI/RegulationProblem A = [[0.5403, -0.8415], [0.8415, 0.5403]] B = [[-0.4597], [0.8415]] C = [[1, 0]] D = [[0]] # Linear discrete-time model with sample time 1 sys = ct.ss2io(ct.ss(A, B, C, D, 1)) # Include weights on states/inputs Q = np.eye(2) R = 1 K, S, E = ct.lqr(A, B, Q, R) # note: *continuous* time LQR # Compute the integral and terminal cost integral_cost = opt.quadratic_cost(sys, Q, R) terminal_cost = opt.quadratic_cost(sys, S, None) # Formulate finite horizon MPC problem time = np.arange(0, 5, 1) x0 = np.array([1, 1]) optctrl = opt.OptimalControlProblem(sys, time, integral_cost, terminal_cost=terminal_cost) res1 = optctrl.compute_trajectory(x0, return_states=True) with pytest.xfail("discrete LQR not implemented"): # Result should match LQR K, S, E = ct.dlqr(A, B, Q, R) lqr_sys = ct.ss2io(ct.ss(A - B @ K, B, C, D, 1)) _, _, lqr_x = ct.input_output_response(lqr_sys, time, 0, x0, return_x=True) np.testing.assert_almost_equal(res1.states, lqr_x) # Add state and input constraints trajectory_constraints = [ (sp.optimize.LinearConstraint, np.eye(3), [-10, -10, -1], [10, 10, 1]), ] # Re-solve res2 = opt.solve_ocp(sys, time, x0, integral_cost, constraints, terminal_cost=terminal_cost) # Make sure we got a different solution assert np.any(np.abs(res1.inputs - res2.inputs) > 0.1)
def time_steering_integrated_cost(): # Set up the cost functions Q = np.diag([.1, 10, .1]) # keep lateral error low R = np.diag([.1, 1]) # minimize applied inputs quad_cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf) res = opt.solve_ocp( vehicle, horizon, x0, quad_cost, initial_guess=bend_left, print_summary=False, # solve_ivp_kwargs={'atol': 1e-2, 'rtol': 1e-2}, minimize_method='trust-constr', minimize_options={'finite_diff_rel_step': 0.01}, ) # Only count this as a benchmark if we converged assert res.success
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
def test_optimal_doc(): """Test optimal control problem from documentation""" def vehicle_update(t, x, u, params): # Get the parameters for the model l = params.get('wheelbase', 3.) # vehicle wheelbase phimax = params.get('maxsteer', 0.5) # max steering angle (rad) # Saturate the steering input phi = np.clip(u[1], -phimax, phimax) # Return the derivative of the state return np.array([ np.cos(x[2]) * u[0], # xdot = cos(theta) v np.sin(x[2]) * u[0], # ydot = sin(theta) v (u[0] / l) * np.tan(phi) # thdot = v/l tan(phi) ]) def vehicle_output(t, x, u, params): return x # return x, y, theta (full state) # Define the vehicle steering dynamics as an input/output system vehicle = ct.NonlinearIOSystem(vehicle_update, vehicle_output, states=3, name='vehicle', inputs=('v', 'phi'), outputs=('x', 'y', 'theta')) # Define the initial and final points and time interval x0 = [0., -2., 0.] u0 = [10., 0.] xf = [100., 2., 0.] uf = [10., 0.] Tf = 10 # Define the cost functions Q = np.diag([0, 0, 0.1]) # don't turn too sharply R = np.diag([1, 1]) # keep inputs small P = np.diag([1000, 1000, 1000]) # get close to final point traj_cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf) term_cost = opt.quadratic_cost(vehicle, P, 0, x0=xf) # Define the constraints constraints = [opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1])] # Solve the optimal control problem horizon = np.linspace(0, Tf, 3, endpoint=True) result = opt.solve_ocp(vehicle, horizon, x0, traj_cost, constraints, terminal_cost=term_cost, initial_guess=u0) # Make sure the resulting trajectory generate a good solution resp = ct.input_output_response(vehicle, horizon, result.inputs, x0, t_eval=np.linspace(0, Tf, 10)) t, y = resp assert (y[0, -1] - xf[0]) / xf[0] < 0.01 assert (y[1, -1] - xf[1]) / xf[1] < 0.01 assert y[2, -1] < 0.1
# Provide an intial guess (will be extended to entire horizon) bend_left = [10, 0.01] # slight left veer # Turn on debug level logging so that we can see what the optimizer is doing logging.basicConfig(level=logging.DEBUG, filename="steering-integral_cost.log", filemode='w', force=True) # Compute the optimal control, setting step size for gradient calculation (eps) start_time = time.process_time() result1 = opt.solve_ocp( vehicle, horizon, x0, quad_cost, initial_guess=bend_left, log=True, minimize_method='trust-constr', minimize_options={'finite_diff_rel_step': 0.01}, ) 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 result1.success # Extract and plot the results (+ state trajectory) t1, u1 = result1.time, result1.inputs t1, y1 = ct.input_output_response(vehicle, horizon, u1, x0) plot_results(t1, y1, u1, figure=1, yf=xf[0:2])