|
| 1 | +# optimal_bench.py - benchmarks for optimal control package |
| 2 | +# RMM, 27 Feb 2021 |
| 3 | +# |
| 4 | +# This benchmark tests the timing for the optimal control module |
| 5 | +# (control.optimal) and is intended to be used for helping tune the |
| 6 | +# performance of the functions used for optimization-base control. |
| 7 | + |
| 8 | +import numpy as np |
| 9 | +import math |
| 10 | +import control as ct |
| 11 | +import control.flatsys as fs |
| 12 | +import control.optimal as opt |
| 13 | + |
| 14 | +# |
| 15 | +# Benchmark test parameters |
| 16 | +# |
| 17 | + |
| 18 | +# Define integrator and minimizer methods and options/keywords |
| 19 | +integrator_table = { |
| 20 | + 'default': (None, {}), |
| 21 | + 'RK23': ('RK23', {}), |
| 22 | + 'RK23_sloppy': ('RK23', {'atol': 1e-4, 'rtol': 1e-2}), |
| 23 | + 'RK45': ('RK45', {}), |
| 24 | + 'RK45': ('RK45', {}), |
| 25 | + 'RK45_sloppy': ('RK45', {'atol': 1e-4, 'rtol': 1e-2}), |
| 26 | + 'LSODA': ('LSODA', {}), |
| 27 | +} |
| 28 | + |
| 29 | +minimizer_table = { |
| 30 | + 'default': (None, {}), |
| 31 | + 'trust': ('trust-constr', {}), |
| 32 | + 'trust_bigstep': ('trust-constr', {'finite_diff_rel_step': 0.01}), |
| 33 | + 'SLSQP': ('SLSQP', {}), |
| 34 | + 'SLSQP_bigstep': ('SLSQP', {'eps': 0.01}), |
| 35 | + 'COBYLA': ('COBYLA', {}), |
| 36 | +} |
| 37 | + |
| 38 | +# Utility function to create a basis of a given size |
| 39 | +def get_basis(name, size, Tf): |
| 40 | + if name == 'poly': |
| 41 | + basis = fs.PolyFamily(size, T=Tf) |
| 42 | + elif name == 'bezier': |
| 43 | + basis = fs.BezierFamily(size, T=Tf) |
| 44 | + elif name == 'bspline': |
| 45 | + basis = fs.BSplineFamily([0, Tf/2, Tf], size) |
| 46 | + return basis |
| 47 | + |
| 48 | + |
| 49 | +# |
| 50 | +# Optimal trajectory generation with linear quadratic cost |
| 51 | +# |
| 52 | + |
| 53 | +def time_optimal_lq_basis(basis_name, basis_size, npoints): |
| 54 | + # Create a sufficiently controllable random system to control |
| 55 | + ntrys = 20 |
| 56 | + while ntrys > 0: |
| 57 | + # Create a random system |
| 58 | + sys = ct.rss(2, 2, 2) |
| 59 | + |
| 60 | + # Compute the controllability Gramian |
| 61 | + Wc = ct.gram(sys, 'c') |
| 62 | + |
| 63 | + # Make sure the condition number is reasonable |
| 64 | + if np.linalg.cond(Wc) < 1e6: |
| 65 | + break |
| 66 | + |
| 67 | + ntrys -= 1 |
| 68 | + assert ntrys > 0 # Something wrong if we needed > 20 tries |
| 69 | + |
| 70 | + # Define cost functions |
| 71 | + Q = np.eye(sys.nstates) |
| 72 | + R = np.eye(sys.ninputs) * 10 |
| 73 | + |
| 74 | + # Figure out the LQR solution (for terminal cost) |
| 75 | + K, S, E = ct.lqr(sys, Q, R) |
| 76 | + |
| 77 | + # Define the cost functions |
| 78 | + traj_cost = opt.quadratic_cost(sys, Q, R) |
| 79 | + term_cost = opt.quadratic_cost(sys, S, None) |
| 80 | + constraints = opt.input_range_constraint( |
| 81 | + sys, -np.ones(sys.ninputs), np.ones(sys.ninputs)) |
| 82 | + |
| 83 | + # Define the initial condition, time horizon, and time points |
| 84 | + x0 = np.ones(sys.nstates) |
| 85 | + Tf = 10 |
| 86 | + timepts = np.linspace(0, Tf, npoints) |
| 87 | + |
| 88 | + # Create the basis function to use |
| 89 | + basis = get_basis(basis_name, basis_size, Tf) |
| 90 | + |
| 91 | + res = opt.solve_ocp( |
| 92 | + sys, timepts, x0, traj_cost, constraints, terminal_cost=term_cost, |
| 93 | + ) |
| 94 | + # Only count this as a benchmark if we converged |
| 95 | + assert res.success |
| 96 | + |
| 97 | +# Parameterize the test against different choices of integrator and minimizer |
| 98 | +time_optimal_lq_basis.param_names = ['basis', 'size', 'npoints'] |
| 99 | +time_optimal_lq_basis.params = ( |
| 100 | + ['poly', 'bezier', 'bspline'], [8, 10, 12], [5, 10, 20]) |
| 101 | + |
| 102 | + |
| 103 | +def time_optimal_lq_methods(integrator_name, minimizer_name): |
| 104 | + # Get the integrator and minimizer parameters to use |
| 105 | + integrator = integrator_table[integrator_name] |
| 106 | + minimizer = minimizer_table[minimizer_name] |
| 107 | + |
| 108 | + # Create a random system to control |
| 109 | + sys = ct.rss(2, 1, 1) |
| 110 | + |
| 111 | + # Define cost functions |
| 112 | + Q = np.eye(sys.nstates) |
| 113 | + R = np.eye(sys.ninputs) * 10 |
| 114 | + |
| 115 | + # Figure out the LQR solution (for terminal cost) |
| 116 | + K, S, E = ct.lqr(sys, Q, R) |
| 117 | + |
| 118 | + # Define the cost functions |
| 119 | + traj_cost = opt.quadratic_cost(sys, Q, R) |
| 120 | + term_cost = opt.quadratic_cost(sys, S, None) |
| 121 | + constraints = opt.input_range_constraint( |
| 122 | + sys, -np.ones(sys.ninputs), np.ones(sys.ninputs)) |
| 123 | + |
| 124 | + # Define the initial condition, time horizon, and time points |
| 125 | + x0 = np.ones(sys.nstates) |
| 126 | + Tf = 10 |
| 127 | + timepts = np.linspace(0, Tf, 20) |
| 128 | + |
| 129 | + # Create the basis function to use |
| 130 | + basis = get_basis('poly', 12, Tf) |
| 131 | + |
| 132 | + res = opt.solve_ocp( |
| 133 | + sys, timepts, x0, traj_cost, constraints, terminal_cost=term_cost, |
| 134 | + solve_ivp_method=integrator[0], solve_ivp_kwargs=integrator[1], |
| 135 | + minimize_method=minimizer[0], minimize_options=minimizer[1], |
| 136 | + ) |
| 137 | + # Only count this as a benchmark if we converged |
| 138 | + assert res.success |
| 139 | + |
| 140 | +# Parameterize the test against different choices of integrator and minimizer |
| 141 | +time_optimal_lq_methods.param_names = ['integrator', 'minimizer'] |
| 142 | +time_optimal_lq_methods.params = ( |
| 143 | + ['RK23', 'RK45', 'LSODA'], ['trust', 'SLSQP', 'COBYLA']) |
| 144 | + |
| 145 | + |
| 146 | +def time_optimal_lq_size(nstates, ninputs, npoints): |
| 147 | + # Create a sufficiently controllable random system to control |
| 148 | + ntrys = 20 |
| 149 | + while ntrys > 0: |
| 150 | + # Create a random system |
| 151 | + sys = ct.rss(nstates, ninputs, ninputs) |
| 152 | + |
| 153 | + # Compute the controllability Gramian |
| 154 | + Wc = ct.gram(sys, 'c') |
| 155 | + |
| 156 | + # Make sure the condition number is reasonable |
| 157 | + if np.linalg.cond(Wc) < 1e6: |
| 158 | + break |
| 159 | + |
| 160 | + ntrys -= 1 |
| 161 | + assert ntrys > 0 # Something wrong if we needed > 20 tries |
| 162 | + |
| 163 | + # Define cost functions |
| 164 | + Q = np.eye(sys.nstates) |
| 165 | + R = np.eye(sys.ninputs) * 10 |
| 166 | + |
| 167 | + # Figure out the LQR solution (for terminal cost) |
| 168 | + K, S, E = ct.lqr(sys, Q, R) |
| 169 | + |
| 170 | + # Define the cost functions |
| 171 | + traj_cost = opt.quadratic_cost(sys, Q, R) |
| 172 | + term_cost = opt.quadratic_cost(sys, S, None) |
| 173 | + constraints = opt.input_range_constraint( |
| 174 | + sys, -np.ones(sys.ninputs), np.ones(sys.ninputs)) |
| 175 | + |
| 176 | + # Define the initial condition, time horizon, and time points |
| 177 | + x0 = np.ones(sys.nstates) |
| 178 | + Tf = 10 |
| 179 | + timepts = np.linspace(0, Tf, npoints) |
| 180 | + |
| 181 | + res = opt.solve_ocp( |
| 182 | + sys, timepts, x0, traj_cost, constraints, terminal_cost=term_cost, |
| 183 | + ) |
| 184 | + # Only count this as a benchmark if we converged |
| 185 | + assert res.success |
| 186 | + |
| 187 | +# Parameterize the test against different choices of integrator and minimizer |
| 188 | +time_optimal_lq_size.param_names = ['nstates', 'ninputs', 'npoints'] |
| 189 | +time_optimal_lq_size.params = ([1, 2, 4], [1, 2, 4], [5, 10, 20]) |
| 190 | + |
| 191 | + |
| 192 | +# |
| 193 | +# Aircraft MPC example (from multi-parametric toolbox) |
| 194 | +# |
| 195 | + |
| 196 | +def time_aircraft_mpc(): |
| 197 | + # model of an aircraft discretized with 0.2s sampling time |
| 198 | + # Source: https://www.mpt3.org/UI/RegulationProblem |
| 199 | + A = [[0.99, 0.01, 0.18, -0.09, 0], |
| 200 | + [ 0, 0.94, 0, 0.29, 0], |
| 201 | + [ 0, 0.14, 0.81, -0.9, 0], |
| 202 | + [ 0, -0.2, 0, 0.95, 0], |
| 203 | + [ 0, 0.09, 0, 0, 0.9]] |
| 204 | + B = [[ 0.01, -0.02], |
| 205 | + [-0.14, 0], |
| 206 | + [ 0.05, -0.2], |
| 207 | + [ 0.02, 0], |
| 208 | + [-0.01, 0]] |
| 209 | + C = [[0, 1, 0, 0, -1], |
| 210 | + [0, 0, 1, 0, 0], |
| 211 | + [0, 0, 0, 1, 0], |
| 212 | + [1, 0, 0, 0, 0]] |
| 213 | + model = ct.ss2io(ct.ss(A, B, C, 0, 0.2)) |
| 214 | + |
| 215 | + # For the simulation we need the full state output |
| 216 | + sys = ct.ss2io(ct.ss(A, B, np.eye(5), 0, 0.2)) |
| 217 | + |
| 218 | + # compute the steady state values for a particular value of the input |
| 219 | + ud = np.array([0.8, -0.3]) |
| 220 | + xd = np.linalg.inv(np.eye(5) - A) @ B @ ud |
| 221 | + yd = C @ xd |
| 222 | + |
| 223 | + # provide constraints on the system signals |
| 224 | + constraints = [opt.input_range_constraint(sys, [-5, -6], [5, 6])] |
| 225 | + |
| 226 | + # provide penalties on the system signals |
| 227 | + Q = model.C.transpose() @ np.diag([10, 10, 10, 10]) @ model.C |
| 228 | + R = np.diag([3, 2]) |
| 229 | + cost = opt.quadratic_cost(model, Q, R, x0=xd, u0=ud) |
| 230 | + |
| 231 | + # online MPC controller object is constructed with a horizon 6 |
| 232 | + ctrl = opt.create_mpc_iosystem( |
| 233 | + model, np.arange(0, 6) * 0.2, cost, constraints) |
| 234 | + |
| 235 | + # Define an I/O system implementing model predictive control |
| 236 | + loop = ct.feedback(sys, ctrl, 1) |
| 237 | + |
| 238 | + # Choose a nearby initial condition to speed up computation |
| 239 | + X0 = np.hstack([xd, np.kron(ud, np.ones(6))]) * 0.99 |
| 240 | + |
| 241 | + Nsim = 12 |
| 242 | + tout, xout = ct.input_output_response( |
| 243 | + loop, np.arange(0, Nsim) * 0.2, 0, X0) |
| 244 | + |
| 245 | + # Make sure the system converged to the desired state |
| 246 | + np.testing.assert_allclose( |
| 247 | + xout[0:sys.nstates, -1], xd, atol=0.1, rtol=0.01) |
0 commit comments