diff --git a/benchmarks/README b/benchmarks/README index a10bbfc21..9c788b250 100644 --- a/benchmarks/README +++ b/benchmarks/README @@ -11,7 +11,7 @@ you can use the following command from the root directory of the repository: PYTHONPATH=`pwd` asv run --python=python -You can also run benchmarks against specific commits usuing +You can also run benchmarks against specific commits using asv run diff --git a/benchmarks/flatsys_bench.py b/benchmarks/flatsys_bench.py index 0c0a5e53a..05a2e7066 100644 --- a/benchmarks/flatsys_bench.py +++ b/benchmarks/flatsys_bench.py @@ -11,6 +11,10 @@ import control.flatsys as flat import control.optimal as opt +# +# System setup: vehicle steering (bicycle model) +# + # Vehicle steering dynamics def vehicle_update(t, x, u, params): # Get the parameters for the model @@ -67,11 +71,28 @@ def vehicle_reverse(zflag, params={}): # Define the time points where the cost/constraints will be evaluated timepts = np.linspace(0, Tf, 10, endpoint=True) -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) +# +# Benchmark test parameters +# + +basis_params = (['poly', 'bezier', 'bspline'], [8, 10, 12]) +basis_param_names = ["basis", "size"] + +def get_basis(name, size): + if name == 'poly': + basis = flat.PolyFamily(size, T=Tf) + elif name == 'bezier': + basis = flat.BezierFamily(size, T=Tf) + elif name == 'bspline': + basis = flat.BSplineFamily([0, Tf/2, Tf], size) + return basis + +# +# Benchmarks +# + +def time_point_to_point(basis_name, basis_size): + basis = get_basis(basis_name, basis_size) # Find trajectory between initial and final conditions traj = flat.point_to_point(vehicle, Tf, x0, u0, xf, uf, basis=basis) @@ -80,13 +101,16 @@ def time_steering_point_to_point(basis_name, basis_size): 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]) + np.testing.assert_array_almost_equal(xf, x[:, -1]) + np.testing.assert_array_almost_equal(uf, u[:, -1]) + +time_point_to_point.params = basis_params +time_point_to_point.param_names = basis_param_names -time_steering_point_to_point.params = (['poly', 'bezier'], [6, 8]) -time_steering_point_to_point.param_names = ["basis", "size"] -def time_steering_cost(): +def time_point_to_point_with_cost(basis_name, basis_size): + basis = get_basis(basis_name, basis_size) + # Define cost and constraints traj_cost = opt.quadratic_cost( vehicle, None, np.diag([0.1, 1]), u0=uf) @@ -95,13 +119,47 @@ def time_steering_cost(): traj = flat.point_to_point( vehicle, timepts, x0, u0, xf, uf, - cost=traj_cost, constraints=constraints, basis=flat.PolyFamily(8) + cost=traj_cost, constraints=constraints, 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]) + np.testing.assert_array_almost_equal(xf, x[:, -1]) + np.testing.assert_array_almost_equal(uf, u[:, -1]) + +time_point_to_point_with_cost.params = basis_params +time_point_to_point_with_cost.param_names = basis_param_names + + +def time_solve_flat_ocp_terminal_cost(method, basis_name, basis_size): + basis = get_basis(basis_name, basis_size) + + # 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([1e3, 1e3, 1e3]), None, x0=xf) + constraints = [ + opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ] + + # Initial guess = straight line + initial_guess = np.array( + [x0[i] + (xf[i] - x0[i]) * timepts/Tf for i in (0, 1)]) + + traj = flat.solve_flat_ocp( + vehicle, timepts, x0, u0, basis=basis, initial_guess=initial_guess, + trajectory_cost=traj_cost, constraints=constraints, + terminal_cost=term_cost, minimize_method=method, + ) + + # 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(xf, x[:, -1], decimal=2) +time_solve_flat_ocp_terminal_cost.params = tuple( + [['slsqp', 'trust-constr']] + list(basis_params)) +time_solve_flat_ocp_terminal_cost.param_names = tuple( + ['method'] + basis_param_names) diff --git a/benchmarks/optimal_bench.py b/benchmarks/optimal_bench.py index 21cabef7e..a424c7827 100644 --- a/benchmarks/optimal_bench.py +++ b/benchmarks/optimal_bench.py @@ -8,165 +8,193 @@ import numpy as np import math import control as ct -import control.flatsys as flat +import control.flatsys as fs import control.optimal as opt -# Vehicle steering dynamics -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) +# +# Benchmark test parameters +# - # Saturate the steering input (use min/max instead of clip for speed) - phi = max(-phimax, min(u[1], phimax)) +# Define integrator and minimizer methods and options/keywords +integrator_table = { + 'default': (None, {}), + 'RK23': ('RK23', {}), + 'RK23_sloppy': ('RK23', {'atol': 1e-4, 'rtol': 1e-2}), + 'RK45': ('RK45', {}), + 'RK45': ('RK45', {}), + 'RK45_sloppy': ('RK45', {'atol': 1e-4, 'rtol': 1e-2}), + 'LSODA': ('LSODA', {}), +} - # Return the derivative of the state - return np.array([ - math.cos(x[2]) * u[0], # xdot = cos(theta) v - math.sin(x[2]) * u[0], # ydot = sin(theta) v - (u[0] / l) * math.tan(phi) # thdot = v/l tan(phi) - ]) +minimizer_table = { + 'default': (None, {}), + 'trust': ('trust-constr', {}), + 'trust_bigstep': ('trust-constr', {'finite_diff_rel_step': 0.01}), + 'SLSQP': ('SLSQP', {}), + 'SLSQP_bigstep': ('SLSQP', {'eps': 0.01}), + 'COBYLA': ('COBYLA', {}), +} -def vehicle_output(t, x, u, params): - return x # return x, y, theta (full state) +# Utility function to create a basis of a given size +def get_basis(name, size, Tf): + if name == 'poly': + basis = fs.PolyFamily(size, T=Tf) + elif name == 'bezier': + basis = fs.BezierFamily(size, T=Tf) + elif name == 'bspline': + basis = fs.BSplineFamily([0, Tf/2, Tf], size) + return basis -vehicle = ct.NonlinearIOSystem( - vehicle_update, vehicle_output, states=3, name='vehicle', - inputs=('v', 'phi'), outputs=('x', 'y', 'theta')) -# Initial and final conditions -x0 = [0., -2., 0.]; u0 = [10., 0.] -xf = [100., 2., 0.]; uf = [10., 0.] -Tf = 10 +# +# Optimal trajectory generation with linear quadratic cost +# -# Define the time horizon (and spacing) for the optimization -horizon = np.linspace(0, Tf, 10, endpoint=True) +def time_optimal_lq_basis(basis_name, basis_size, npoints): + # Create a sufficiently controllable random system to control + ntrys = 20 + while ntrys > 0: + # Create a random system + sys = ct.rss(2, 2, 2) -# Provide an intial guess (will be extended to entire horizon) -bend_left = [10, 0.01] # slight left veer + # Compute the controllability Gramian + Wc = ct.gram(sys, 'c') -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) + # Make sure the condition number is reasonable + if np.linalg.cond(Wc) < 1e6: + break - 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}, - ) + ntrys -= 1 + assert ntrys > 0 # Something wrong if we needed > 20 tries - # Only count this as a benchmark if we converged - assert res.success + # Define cost functions + Q = np.eye(sys.nstates) + R = np.eye(sys.ninputs) * 10 + + # Figure out the LQR solution (for terminal cost) + K, S, E = ct.lqr(sys, Q, R) + + # Define the cost functions + traj_cost = opt.quadratic_cost(sys, Q, R) + term_cost = opt.quadratic_cost(sys, S, None) + constraints = opt.input_range_constraint( + sys, -np.ones(sys.ninputs), np.ones(sys.ninputs)) + + # Define the initial condition, time horizon, and time points + x0 = np.ones(sys.nstates) + Tf = 10 + timepts = np.linspace(0, Tf, npoints) -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]) ] + # Create the basis function to use + basis = get_basis(basis_name, basis_size, Tf) 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}, + sys, timepts, x0, traj_cost, constraints, terminal_cost=term_cost, + basis=basis, ) # Only count this as a benchmark if we converged assert res.success -# Define integrator and minimizer methods and options/keywords -integrator_table = { - 'RK23_default': ('RK23', {'atol': 1e-4, 'rtol': 1e-2}), - 'RK23_sloppy': ('RK23', {}), - 'RK45_default': ('RK45', {}), - 'RK45_sloppy': ('RK45', {'atol': 1e-4, 'rtol': 1e-2}), -} - -minimizer_table = { - 'trust_default': ('trust-constr', {}), - 'trust_bigstep': ('trust-constr', {'finite_diff_rel_step': 0.01}), - 'SLSQP_default': ('SLSQP', {}), - 'SLSQP_bigstep': ('SLSQP', {'eps': 0.01}), -} +# Parameterize the test against different choices of integrator and minimizer +time_optimal_lq_basis.param_names = ['basis', 'size', 'npoints'] +time_optimal_lq_basis.params = ( + ['poly', 'bezier', 'bspline'], [8, 10, 12], [5, 10, 20]) -def time_steering_terminal_constraint(integrator_name, minimizer_name): +def time_optimal_lq_methods(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) ] + # Create a random system to control + sys = ct.rss(2, 1, 1) + + # Define cost functions + Q = np.eye(sys.nstates) + R = np.eye(sys.ninputs) * 10 + + # Figure out the LQR solution (for terminal cost) + K, S, E = ct.lqr(sys, Q, R) + + # Define the cost functions + traj_cost = opt.quadratic_cost(sys, Q, R) + term_cost = opt.quadratic_cost(sys, S, None) + constraints = opt.input_range_constraint( + sys, -np.ones(sys.ninputs), np.ones(sys.ninputs)) + + # Define the initial condition, time horizon, and time points + x0 = np.ones(sys.nstates) + Tf = 10 + timepts = np.linspace(0, Tf, 20) + + # Create the basis function to use + basis = get_basis('poly', 12, Tf) res = opt.solve_ocp( - vehicle, horizon, x0, cost, constraints, - terminal_constraints=terminal, initial_guess=bend_left, log=False, + sys, timepts, x0, traj_cost, constraints, terminal_cost=term_cost, 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 -# Reset the timeout value to allow for longer runs -time_steering_terminal_constraint.timeout = 120 - # Parameterize the test against different choices of integrator and minimizer -time_steering_terminal_constraint.param_names = ['integrator', 'minimizer'] -time_steering_terminal_constraint.params = ( - ['RK23_default', 'RK23_sloppy', 'RK45_default', 'RK45_sloppy'], - ['trust_default', 'trust_bigstep', 'SLSQP_default', 'SLSQP_bigstep'] -) - -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 +time_optimal_lq_methods.param_names = ['integrator', 'minimizer'] +time_optimal_lq_methods.params = ( + ['RK23', 'RK45', 'LSODA'], ['trust', 'SLSQP', 'COBYLA']) + + +def time_optimal_lq_size(nstates, ninputs, npoints): + # Create a sufficiently controllable random system to control + ntrys = 20 + while ntrys > 0: + # Create a random system + sys = ct.rss(nstates, ninputs, ninputs) + + # Compute the controllability Gramian + Wc = ct.gram(sys, 'c') + + # Make sure the condition number is reasonable + if np.linalg.cond(Wc) < 1e6: + break + + ntrys -= 1 + assert ntrys > 0 # Something wrong if we needed > 20 tries + + # Define cost functions + Q = np.eye(sys.nstates) + R = np.eye(sys.ninputs) * 10 + + # Figure out the LQR solution (for terminal cost) + K, S, E = ct.lqr(sys, Q, R) + + # Define the cost functions + traj_cost = opt.quadratic_cost(sys, Q, R) + term_cost = opt.quadratic_cost(sys, S, None) + constraints = opt.input_range_constraint( + sys, -np.ones(sys.ninputs), np.ones(sys.ninputs)) + + # Define the initial condition, time horizon, and time points + x0 = np.ones(sys.nstates) + Tf = 10 + timepts = np.linspace(0, Tf, npoints) + 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 + sys, timepts, x0, traj_cost, constraints, terminal_cost=term_cost, ) - t, u, x = res.time, res.inputs, res.states - - # Make sure we found a valid solution + # Only count this as a benchmark if we converged assert res.success -# Reset the timeout value to allow for longer runs -time_steering_bezier_basis.timeout = 120 +# Parameterize the test against different choices of integrator and minimizer +time_optimal_lq_size.param_names = ['nstates', 'ninputs', 'npoints'] +time_optimal_lq_size.params = ([1, 2, 4], [1, 2, 4], [5, 10, 20]) + -# Set the parameter values for the number of times and basis vectors -time_steering_bezier_basis.param_names = ['nbasis', 'ntimes'] -time_steering_bezier_basis.params = ([2, 4, 6], [5, 10, 20]) +# +# Aircraft MPC example (from multi-parametric toolbox) +# -def time_aircraft_mpc(): +def time_discrete_aircraft_mpc(minimizer_name): # model of an aircraft discretized with 0.2s sampling time # Source: https://www.mpt3.org/UI/RegulationProblem A = [[0.99, 0.01, 0.18, -0.09, 0], @@ -201,9 +229,18 @@ def time_aircraft_mpc(): R = np.diag([3, 2]) cost = opt.quadratic_cost(model, Q, R, x0=xd, u0=ud) + # Set the time horizon and time points + Tf = 3 + timepts = np.arange(0, 6) * 0.2 + + # Get the minimizer parameters to use + minimizer = minimizer_table[minimizer_name] + # online MPC controller object is constructed with a horizon 6 ctrl = opt.create_mpc_iosystem( - model, np.arange(0, 6) * 0.2, cost, constraints) + model, timepts, cost, constraints, + minimize_method=minimizer[0], minimize_options=minimizer[1], + ) # Define an I/O system implementing model predictive control loop = ct.feedback(sys, ctrl, 1) @@ -218,3 +255,8 @@ def time_aircraft_mpc(): # Make sure the system converged to the desired state np.testing.assert_allclose( xout[0:sys.nstates, -1], xd, atol=0.1, rtol=0.01) + +# Parameterize the test against different choices of minimizer and basis +time_discrete_aircraft_mpc.param_names = ['minimizer'] +time_discrete_aircraft_mpc.params = ( + ['trust', 'trust_bigstep', 'SLSQP', 'SLSQP_bigstep', 'COBYLA']) diff --git a/control/flatsys/bezier.py b/control/flatsys/bezier.py index 02b4209a6..fcf6201e9 100644 --- a/control/flatsys/bezier.py +++ b/control/flatsys/bezier.py @@ -73,7 +73,7 @@ def eval_deriv(self, i, k, t, var=None): raise ValueError("Basis function index too high") elif k >= self.N: # Higher order derivatives are zero - return np.zeros(t.shape) + return 0 * t # Compute the variables used in Bezier curve formulas n = self.N - 1 diff --git a/control/flatsys/flatsys.py b/control/flatsys/flatsys.py index 849c41c72..04e5c323a 100644 --- a/control/flatsys/flatsys.py +++ b/control/flatsys/flatsys.py @@ -654,6 +654,11 @@ def solve_flat_ocp( * cost : computed cost of the returned trajectory * message : message returned by optimization if success if False + 3. A common failure in solving optimal control problem is that the + default initial guess violates the constraints and the optimizer + can't find a feasible solution. Using the `initial_guess` parameter + can often be used to overcome these errors. + """ # # Make sure the problem is one that we can handle diff --git a/control/flatsys/poly.py b/control/flatsys/poly.py index bfd8de633..f315091aa 100644 --- a/control/flatsys/poly.py +++ b/control/flatsys/poly.py @@ -66,6 +66,6 @@ def __init__(self, N, T=1): # Compute the kth derivative of the ith basis function at time t def eval_deriv(self, i, k, t, var=None): """Evaluate the kth derivative of the ith basis function at time t.""" - if (i < k): return 0; # higher derivative than power + if (i < k): return 0 * t # higher derivative than power return factorial(i)/factorial(i-k) * \ np.power(t/self.T, i-k) / np.power(self.T, k) diff --git a/control/optimal.py b/control/optimal.py index 4913cc341..9650272f7 100644 --- a/control/optimal.py +++ b/control/optimal.py @@ -154,6 +154,14 @@ def __init__( self.minimize_kwargs.update(kwargs.pop( 'minimize_kwargs', config.defaults['optimal.minimize_kwargs'])) + # Check to make sure arguments for discrete-time systems are OK + if sys.isdtime(strict=True): + if self.solve_ivp_kwargs['method'] is not None or \ + len(self.solve_ivp_kwargs) > 1: + raise TypeError( + "solve_ivp method, kwargs not allowed for" + " discrete time systems") + # Make sure there were no extraneous keywords if kwargs: raise TypeError("unrecognized keyword(s): ", str(kwargs)) @@ -889,7 +897,8 @@ def __init__( def solve_ocp( sys, horizon, X0, cost, trajectory_constraints=None, terminal_cost=None, terminal_constraints=[], initial_guess=None, basis=None, squeeze=None, - transpose=None, return_states=True, log=False, **kwargs): + transpose=None, return_states=True, print_summary=True, log=False, + **kwargs): """Compute the solution to an optimal control problem @@ -943,6 +952,9 @@ def solve_ocp( log : bool, optional If `True`, turn on logging messages (using Python logging module). + print_summary : bool, optional + If `True` (default), print a short summary of the computation. + return_states : bool, optional If True, return the values of the state at each time (default = True). @@ -1009,7 +1021,8 @@ def solve_ocp( # Solve for the optimal input from the current state return ocp.compute_trajectory( - X0, squeeze=squeeze, transpose=transpose, return_states=return_states) + X0, squeeze=squeeze, transpose=transpose, print_summary=print_summary, + return_states=return_states) # Create a model predictive controller for an optimal control problem diff --git a/control/tests/flatsys_test.py b/control/tests/flatsys_test.py index 665bfd968..3f308c79b 100644 --- a/control/tests/flatsys_test.py +++ b/control/tests/flatsys_test.py @@ -464,6 +464,26 @@ def test_bezier_basis(self): with pytest.raises(ValueError, match="index too high"): bezier.eval_deriv(4, 0, time) + @pytest.mark.parametrize("basis, degree, T", [ + (fs.PolyFamily(4), 4, 1), + (fs.PolyFamily(4, 100), 4, 100), + (fs.BezierFamily(4), 4, 1), + (fs.BezierFamily(4, 100), 4, 100), + (fs.BSplineFamily([0, 0.5, 1], 4), 3, 1), + (fs.BSplineFamily([0, 50, 100], 4), 3, 100), + ]) + def test_basis_derivs(self, basis, degree, T): + """Make sure that that basis function derivates are correct""" + timepts = np.linspace(0, T, 10000) + dt = timepts[1] - timepts[0] + for i in range(basis.N): + for j in range(degree-1): + # Compare numerical and analytical derivative + np.testing.assert_allclose( + np.diff(basis.eval_deriv(i, j, timepts)) / dt, + basis.eval_deriv(i, j+1, timepts)[0:-1], + atol=1e-2, rtol=1e-4) + def test_point_to_point_errors(self): """Test error and warning conditions in point_to_point()""" # Double integrator system diff --git a/control/tests/optimal_test.py b/control/tests/optimal_test.py index b100e7e14..0e5e35d79 100644 --- a/control/tests/optimal_test.py +++ b/control/tests/optimal_test.py @@ -471,6 +471,15 @@ def test_ocp_argument_errors(): res = opt.solve_ocp( sys, time, x0, cost, terminal_constraints=constraints) + # Discrete time system checks: solve_ivp keywords not allowed + sys = ct.rss(2, 1, 1, dt=True) + with pytest.raises(TypeError, match="solve_ivp method, kwargs not allowed"): + res = opt.solve_ocp( + sys, time, x0, cost, solve_ivp_method='LSODA') + with pytest.raises(TypeError, match="solve_ivp method, kwargs not allowed"): + res = opt.solve_ocp( + sys, time, x0, cost, solve_ivp_kwargs={'eps': 0.1}) + @pytest.mark.parametrize("basis", [ flat.PolyFamily(4), flat.PolyFamily(6),