diff --git a/control/config.py b/control/config.py index afd7615ca..8acdf28e2 100644 --- a/control/config.py +++ b/control/config.py @@ -103,6 +103,9 @@ def reset_defaults(): from .iosys import _iosys_defaults defaults.update(_iosys_defaults) + from .optimal import _optimal_defaults + defaults.update(_optimal_defaults) + def _get_param(module, param, argval=None, defval=None, pop=False, last=False): """Return the default value for a configuration option. diff --git a/control/iosys.py b/control/iosys.py index 2f63a7e8b..d5a7b755b 100644 --- a/control/iosys.py +++ b/control/iosys.py @@ -1571,7 +1571,7 @@ def __init__(self, io_sys, ss_sys=None): def input_output_response( sys, T, U=0., X0=0, params={}, transpose=False, return_x=False, squeeze=None, - solve_ivp_kwargs={}, **kwargs): + solve_ivp_kwargs={}, t_eval='T', **kwargs): """Compute the output response of a system to a given input. Simulate a dynamical system with a given input and return its output @@ -1654,50 +1654,57 @@ def input_output_response( if kwargs.get('solve_ivp_method', None): if kwargs.get('method', None): raise ValueError("ivp_method specified more than once") - solve_ivp_kwargs['method'] = kwargs['solve_ivp_method'] + solve_ivp_kwargs['method'] = kwargs.pop('solve_ivp_method') # Set the default method to 'RK45' if solve_ivp_kwargs.get('method', None) is None: solve_ivp_kwargs['method'] = 'RK45' + # Make sure all input arguments got parsed + if kwargs: + raise TypeError("unknown parameters %s" % kwargs) + # Sanity checking on the input if not isinstance(sys, InputOutputSystem): raise TypeError("System of type ", type(sys), " not valid") # Compute the time interval and number of steps T0, Tf = T[0], T[-1] - n_steps = len(T) + ntimepts = len(T) + + # Figure out simulation times (t_eval) + if solve_ivp_kwargs.get('t_eval'): + if t_eval == 'T': + # Override the default with the solve_ivp keyword + t_eval = solve_ivp_kwargs.pop('t_eval') + else: + raise ValueError("t_eval specified more than once") + if isinstance(t_eval, str) and t_eval == 'T': + # Use the input time points as the output time points + t_eval = T # Check and convert the input, if needed # TODO: improve MIMO ninputs check (choose from U) if sys.ninputs is None or sys.ninputs == 1: - legal_shapes = [(n_steps,), (1, n_steps)] + legal_shapes = [(ntimepts,), (1, ntimepts)] else: - legal_shapes = [(sys.ninputs, n_steps)] + legal_shapes = [(sys.ninputs, ntimepts)] U = _check_convert_array(U, legal_shapes, 'Parameter ``U``: ', squeeze=False) - U = U.reshape(-1, n_steps) - - # Check to make sure this is not a static function - nstates = _find_size(sys.nstates, X0) - if nstates == 0: - # No states => map input to output - u = U[0] if len(U.shape) == 1 else U[:, 0] - y = np.zeros((np.shape(sys._out(T[0], X0, u))[0], len(T))) - for i in range(len(T)): - u = U[i] if len(U.shape) == 1 else U[:, i] - y[:, i] = sys._out(T[i], [], u) - return TimeResponseData( - T, y, None, U, issiso=sys.issiso(), - output_labels=sys.output_index, input_labels=sys.input_index, - transpose=transpose, return_x=return_x, squeeze=squeeze) + U = U.reshape(-1, ntimepts) + ninputs = U.shape[0] # create X0 if not given, test if X0 has correct shape + nstates = _find_size(sys.nstates, X0) X0 = _check_convert_array(X0, [(nstates,), (nstates, 1)], 'Parameter ``X0``: ', squeeze=True) - # Update the parameter values - sys._update_params(params) + # Figure out the number of outputs + if sys.noutputs is None: + # Evaluate the output function to find number of outputs + noutputs = np.shape(sys._out(T[0], X0, U[:, 0]))[0] + else: + noutputs = sys.noutputs # # Define a function to evaluate the input at an arbitrary time @@ -1714,6 +1721,31 @@ def ufun(t): dt = (t - T[idx-1]) / (T[idx] - T[idx-1]) return U[..., idx-1] * (1. - dt) + U[..., idx] * dt + # Check to make sure this is not a static function + if nstates == 0: # No states => map input to output + # Make sure the user gave a time vector for evaluation (or 'T') + if t_eval is None: + # User overrode t_eval with None, but didn't give us the times... + warn("t_eval set to None, but no dynamics; using T instead") + t_eval = T + + # Allocate space for the inputs and outputs + u = np.zeros((ninputs, len(t_eval))) + y = np.zeros((noutputs, len(t_eval))) + + # Compute the input and output at each point in time + for i, t in enumerate(t_eval): + u[:, i] = ufun(t) + y[:, i] = sys._out(t, [], u[:, i]) + + return TimeResponseData( + t_eval, y, None, u, issiso=sys.issiso(), + output_labels=sys.output_index, input_labels=sys.input_index, + transpose=transpose, return_x=return_x, squeeze=squeeze) + + # Update the parameter values + sys._update_params(params) + # Create a lambda function for the right hand side def ivp_rhs(t, x): return sys._rhs(t, x, ufun(t)) @@ -1724,27 +1756,27 @@ def ivp_rhs(t, x): raise NameError("scipy.integrate.solve_ivp not found; " "use SciPy 1.0 or greater") soln = sp.integrate.solve_ivp( - ivp_rhs, (T0, Tf), X0, t_eval=T, + ivp_rhs, (T0, Tf), X0, t_eval=t_eval, vectorized=False, **solve_ivp_kwargs) + if not soln.success: + raise RuntimeError("solve_ivp failed: " + soln.message) - if not soln.success or soln.status != 0: - # Something went wrong - warn("sp.integrate.solve_ivp failed") - print("Return bunch:", soln) - - # Compute the output associated with the state (and use sys.out to - # figure out the number of outputs just in case it wasn't specified) - u = U[0] if len(U.shape) == 1 else U[:, 0] - y = np.zeros((np.shape(sys._out(T[0], X0, u))[0], len(T))) - for i in range(len(T)): - u = U[i] if len(U.shape) == 1 else U[:, i] - y[:, i] = sys._out(T[i], soln.y[:, i], u) + # Compute inputs and outputs for each time point + u = np.zeros((ninputs, len(soln.t))) + y = np.zeros((noutputs, len(soln.t))) + for i, t in enumerate(soln.t): + u[:, i] = ufun(t) + y[:, i] = sys._out(t, soln.y[:, i], u[:, i]) elif isdtime(sys): + # If t_eval was not specified, use the sampling time + if t_eval is None: + t_eval = np.arange(T[0], T[1] + sys.dt, sys.dt) + # Make sure the time vector is uniformly spaced - dt = T[1] - T[0] - if not np.allclose(T[1:] - T[:-1], dt): - raise ValueError("Parameter ``T``: time values must be " + dt = t_eval[1] - t_eval[0] + if not np.allclose(t_eval[1:] - t_eval[:-1], dt): + raise ValueError("Parameter ``t_eval``: time values must be " "equally spaced.") # Make sure the sample time matches the given time @@ -1764,21 +1796,23 @@ def ivp_rhs(t, x): # Compute the solution soln = sp.optimize.OptimizeResult() - soln.t = T # Store the time vector directly - x = X0 # Initilize state + soln.t = t_eval # Store the time vector directly + x = np.array(X0) # State vector (store as floats) soln.y = [] # Solution, following scipy convention - y = [] # System output - for i in range(len(T)): - # Store the current state and output + u, y = [], [] # System input, output + for t in t_eval: + # Store the current input, state, and output soln.y.append(x) - y.append(sys._out(T[i], x, ufun(T[i]))) + u.append(ufun(t)) + y.append(sys._out(t, x, u[-1])) # Update the state for the next iteration - x = sys._rhs(T[i], x, ufun(T[i])) + x = sys._rhs(t, x, u[-1]) # Convert output to numpy arrays soln.y = np.transpose(np.array(soln.y)) y = np.transpose(np.array(y)) + u = np.transpose(np.array(u)) # Mark solution as successful soln.success = True # No way to fail @@ -1787,7 +1821,7 @@ def ivp_rhs(t, x): raise TypeError("Can't determine system type") return TimeResponseData( - soln.t, y, soln.y, U, issiso=sys.issiso(), + soln.t, y, soln.y, u, issiso=sys.issiso(), output_labels=sys.output_index, input_labels=sys.input_index, state_labels=sys.state_index, transpose=transpose, return_x=return_x, squeeze=squeeze) diff --git a/control/optimal.py b/control/optimal.py index 493b6bc3d..cb9f57ded 100644 --- a/control/optimal.py +++ b/control/optimal.py @@ -16,10 +16,21 @@ import logging import time +from . import config +from .exception import ControlNotImplemented from .timeresp import TimeResponseData __all__ = ['find_optimal_input'] +# Define module default parameter values +_optimal_defaults = { + 'optimal.minimize_method': None, + 'optimal.minimize_options': {}, + 'optimal.minimize_kwargs': {}, + 'optimal.solve_ivp_method': None, + 'optimal.solve_ivp_options': {}, +} + class OptimalControlProblem(): """Description of a finite horizon, optimal control problem. @@ -110,6 +121,10 @@ class OptimalControlProblem(): values of the input at the specified times (using linear interpolation for continuous systems). + The default values for ``minimize_method``, ``minimize_options``, + ``minimize_kwargs``, ``solve_ivp_method``, and ``solve_ivp_options`` can + be set using config.defaults['optimal.']. + """ def __init__( self, sys, timepts, integral_cost, trajectory_constraints=[], @@ -126,17 +141,22 @@ def __init__( # Process keyword arguments self.solve_ivp_kwargs = {} - self.solve_ivp_kwargs['method'] = kwargs.pop('solve_ivp_method', None) - self.solve_ivp_kwargs.update(kwargs.pop('solve_ivp_kwargs', {})) + self.solve_ivp_kwargs['method'] = kwargs.pop( + 'solve_ivp_method', config.defaults['optimal.solve_ivp_method']) + self.solve_ivp_kwargs.update(kwargs.pop( + 'solve_ivp_kwargs', config.defaults['optimal.solve_ivp_options'])) self.minimize_kwargs = {} - self.minimize_kwargs['method'] = kwargs.pop('minimize_method', None) - self.minimize_kwargs['options'] = kwargs.pop('minimize_options', {}) - self.minimize_kwargs.update(kwargs.pop('minimize_kwargs', {})) + self.minimize_kwargs['method'] = kwargs.pop( + 'minimize_method', config.defaults['optimal.minimize_method']) + self.minimize_kwargs['options'] = kwargs.pop( + 'minimize_options', config.defaults['optimal.minimize_options']) + self.minimize_kwargs.update(kwargs.pop( + 'minimize_kwargs', config.defaults['optimal.minimize_kwargs'])) - if len(kwargs) > 0: - raise ValueError( - f'unrecognized keyword(s): {list(kwargs.keys())}') + # Make sure all input arguments got parsed + if kwargs: + raise TypeError("unrecognized keyword(s): ", str(kwargs)) # Process trajectory constraints if isinstance(trajectory_constraints, tuple): @@ -271,9 +291,10 @@ def _cost_function(self, coeffs): logging.debug("initial input[0:3] =\n" + str(inputs[:, 0:3])) # Simulate the system to get the state + # TODO: try calling solve_ivp directly for better speed? _, _, states = ct.input_output_response( self.system, self.timepts, inputs, x, return_x=True, - solve_ivp_kwargs=self.solve_ivp_kwargs) + solve_ivp_kwargs=self.solve_ivp_kwargs, t_eval=self.timepts) self.system_simulations += 1 self.last_x = x self.last_coeffs = coeffs @@ -393,7 +414,7 @@ def _constraint_function(self, coeffs): # Simulate the system to get the state _, _, states = ct.input_output_response( self.system, self.timepts, inputs, x, return_x=True, - solve_ivp_kwargs=self.solve_ivp_kwargs) + solve_ivp_kwargs=self.solve_ivp_kwargs, t_eval=self.timepts) self.system_simulations += 1 self.last_x = x self.last_coeffs = coeffs @@ -475,7 +496,7 @@ def _eqconst_function(self, coeffs): # Simulate the system to get the state _, _, states = ct.input_output_response( self.system, self.timepts, inputs, x, return_x=True, - solve_ivp_kwargs=self.solve_ivp_kwargs) + solve_ivp_kwargs=self.solve_ivp_kwargs, t_eval=self.timepts) self.system_simulations += 1 self.last_x = x self.last_coeffs = coeffs @@ -548,7 +569,7 @@ def _process_initial_guess(self, initial_guess): initial_guess = np.atleast_1d(initial_guess) # See whether we got entire guess or just first time point - if len(initial_guess.shape) == 1: + if initial_guess.ndim == 1: # Broadcast inputs to entire time vector try: initial_guess = np.broadcast_to( @@ -804,6 +825,15 @@ class OptimalControlResult(sp.optimize.OptimizeResult): Whether or not the optimizer exited successful. problem : OptimalControlProblem Optimal control problem that generated this solution. + cost : float + Final cost of the return solution. + system_simulations, {cost, constraint, eqconst}_evaluations : int + Number of system simulations and evaluations of the cost function, + (inequality) constraint function, and equality constraint function + performed during the optimzation. + {cost, constraint, eqconst}_process_time : float + If logging was enabled, the amount of time spent evaluating the cost + and constraint functions. """ def __init__( @@ -833,15 +863,19 @@ def __init__( "unable to solve optimal control problem\n" "scipy.optimize.minimize returned " + res.message, UserWarning) + # Save the final cost + self.cost = res.fun + # Optionally print summary information if print_summary: ocp._print_statistics() + print("* Final cost:", self.cost) if return_states and inputs.shape[1] == ocp.timepts.shape[0]: # Simulate the system if we need the state back _, _, states = ct.input_output_response( ocp.system, ocp.timepts, inputs, ocp.x, return_x=True, - solve_ivp_kwargs=ocp.solve_ivp_kwargs) + solve_ivp_kwargs=ocp.solve_ivp_kwargs, t_eval=ocp.timepts) ocp.system_simulations += 1 else: states = None @@ -858,7 +892,7 @@ def __init__( # Compute the input for a nonlinear, (constrained) optimal control problem def solve_ocp( - sys, horizon, X0, cost, trajectory_constraints=[], terminal_cost=None, + sys, horizon, X0, cost, trajectory_constraints=None, terminal_cost=None, terminal_constraints=[], initial_guess=None, basis=None, squeeze=None, transpose=None, return_states=False, log=False, **kwargs): @@ -960,12 +994,18 @@ def solve_ocp( # Process keyword arguments if trajectory_constraints is None: # Backwards compatibility - trajectory_constraints = kwargs.pop('constraints', None) + trajectory_constraints = kwargs.pop('constraints', []) # Allow 'return_x` as a synonym for 'return_states' return_states = ct.config._get_param( 'optimal', 'return_x', kwargs, return_states, pop=True) + # Process (legacy) method keyword + if kwargs.get('method'): + if kwargs.get('minimize_method'): + raise ValueError("'minimize_method' specified more than once") + kwargs['minimize_method'] = kwargs.pop('method') + # Set up the optimal control problem ocp = OptimalControlProblem( sys, horizon, cost, trajectory_constraints=trajectory_constraints, diff --git a/control/statefbk.py b/control/statefbk.py index a866af725..6598eeeb8 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -734,7 +734,7 @@ def dlqr(*args, **kwargs): The dlqr() function computes the optimal state feedback controller u[n] = - K x[n] that minimizes the quadratic cost - .. math:: J = \\Sum_0^\\infty (x[n]' Q x[n] + u[n]' R u[n] + 2 x[n]' N u[n]) + .. math:: J = \\sum_0^\\infty (x[n]' Q x[n] + u[n]' R u[n] + 2 x[n]' N u[n]) The function can be called with either 3, 4, or 5 arguments: diff --git a/control/tests/iosys_test.py b/control/tests/iosys_test.py index bdc5ba31a..e76a6be55 100644 --- a/control/tests/iosys_test.py +++ b/control/tests/iosys_test.py @@ -1648,7 +1648,9 @@ def test_interconnect_unused_output(): outputs=['u'], name='k') - with pytest.warns(UserWarning, match=r"Unused output\(s\) in InterconnectedSystem:") as record: + with pytest.warns( + UserWarning, + match=r"Unused output\(s\) in InterconnectedSystem:") as record: h = ct.interconnect([g,s,k], inputs=['r'], outputs=['y']) @@ -1679,13 +1681,17 @@ def test_interconnect_unused_output(): pytest.fail(f'Unexpected warning: {r.message}') # warn if explicity ignored output in fact used - with pytest.warns(UserWarning, match=r"Output\(s\) specified as ignored is \(are\) used:"): + with pytest.warns( + UserWarning, + match=r"Output\(s\) specified as ignored is \(are\) used:"): h = ct.interconnect([g,s,k], inputs=['r'], outputs=['y'], ignore_outputs=['dy','u']) - with pytest.warns(UserWarning, match=r"Output\(s\) specified as ignored is \(are\) used:"): + with pytest.warns( + UserWarning, + match=r"Output\(s\) specified as ignored is \(are\) used:"): h = ct.interconnect([g,s,k], inputs=['r'], outputs=['y'], @@ -1697,3 +1703,25 @@ def test_interconnect_unused_output(): inputs=['r'], outputs=['y'], ignore_outputs=['v']) + +def test_nonuniform_timepts(): + """Test non-uniform time points for simulations""" + sys = ct.LinearIOSystem(ct.rss(2, 1, 1)) + + # Start with a uniform set of times + unifpts = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10] + uniform = [1, 2, 3, 2, 1, -1, -3, -5, -7, -3, 1] + t_unif, y_unif = ct.input_output_response(sys, unifpts, uniform) + + # Create a non-uniform set of inputs + noufpts = [0, 2, 4, 8, 10] + nonunif = [1, 3, 1, -7, 1] + t_nouf, y_nouf = ct.input_output_response(sys, noufpts, nonunif) + + # Make sure the outputs agree at common times + np.testing.assert_almost_equal(y_unif[noufpts], y_nouf, decimal=6) + + # Resimulate using a new set of evaluation points + t_even, y_even = ct.input_output_response( + sys, noufpts, nonunif, t_eval=unifpts) + np.testing.assert_almost_equal(y_unif, y_even, decimal=6) diff --git a/control/tests/optimal_test.py b/control/tests/optimal_test.py index f059c4fc6..1aa307b60 100644 --- a/control/tests/optimal_test.py +++ b/control/tests/optimal_test.py @@ -45,6 +45,9 @@ def test_finite_horizon_simple(): np.testing.assert_almost_equal( u_openloop, [-1, -1, 0.1393, 0.3361, -5.204e-16], decimal=4) + # Make sure the final cost is correct + assert math.isclose(res.cost, 32.4898, rel_tol=1e-5) + # Convert controller to an explicit form (not implemented yet) # mpc_explicit = opt.explicit_mpc(); @@ -117,7 +120,7 @@ def test_discrete_lqr(): assert np.any(np.abs(res1.inputs - res2.inputs) > 0.1) -def test_mpc_iosystem(): +def test_mpc_iosystem_aircraft(): # 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], @@ -171,6 +174,21 @@ def test_mpc_iosystem(): xout[0:sys.nstates, -1], xd, atol=0.1, rtol=0.01) +def test_mpc_iosystem_continuous(): + # Create a random state space system + sys = ct.rss(2, 1, 1) + T, _ = ct.step_response(sys) + + # provide penalties on the system signals + Q = np.eye(sys.nstates) + R = np.eye(sys.ninputs) + cost = opt.quadratic_cost(sys, Q, R) + + # Continuous time MPC controller not implemented + with pytest.raises(NotImplementedError): + ctrl = opt.create_mpc_iosystem(sys, T, cost) + + # Test various constraint combinations; need to use a somewhat convoluted # parametrization due to the need to define sys instead the test function @pytest.mark.parametrize("constraint_list", [ @@ -437,7 +455,7 @@ def test_ocp_argument_errors(): sys, time, x0, cost, constraints, initial_guess=np.zeros((4,1,1))) # Unrecognized arguments - with pytest.raises(ValueError, match="unrecognized keyword"): + with pytest.raises(TypeError, match="unrecognized keyword"): res = opt.solve_ocp( sys, time, x0, cost, constraints, terminal_constraint=None) @@ -549,3 +567,59 @@ def final_point_eval(x, u): optctrl = opt.OptimalControlProblem( sys, time, cost, terminal_constraints=final_point) res = optctrl.compute_trajectory(x0, squeeze=True, return_x=True) + + +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 diff --git a/doc/control.rst b/doc/control.rst index 87c1151eb..8bd6f7a32 100644 --- a/doc/control.rst +++ b/doc/control.rst @@ -114,7 +114,7 @@ Control system synthesis lqe mixsyn place - rlocus_pid_designer + rootlocus_pid_designer Model simplification tools ========================== diff --git a/doc/examples.rst b/doc/examples.rst index 91476bc9d..89a2b16a1 100644 --- a/doc/examples.rst +++ b/doc/examples.rst @@ -29,6 +29,7 @@ other sources. robust_mimo cruise-control steering-gainsched + steering-optimal kincar-flatsys Jupyter notebooks diff --git a/doc/optimal.rst b/doc/optimal.rst index e173e430b..8da08e7af 100644 --- a/doc/optimal.rst +++ b/doc/optimal.rst @@ -79,7 +79,7 @@ Every :math:`\Delta T` seconds, an optimal control problem is solved over a :math:`T` second horizon, starting from the current state. The first :math:`\Delta T` seconds of the optimal control :math:`u_T^{\*}(\cdot; x(t))` is then applied to the system. If we let :math:`x_T^{\*}(\cdot; -x(t))` represent the optimal trajectory starting from :math:`x(t)`$ then the +x(t))` represent the optimal trajectory starting from :math:`x(t)` then the system state evolves from :math:`x(t)` at current time :math:`t` to :math:`x_T^{*}(\delta T, x(t))` at the next sample time :math:`t + \Delta T`, assuming no model uncertainty. @@ -219,9 +219,11 @@ with a starting and ending velocity of 10 m/s:: To set up the optimal control problem we design a cost function that penalizes the state and input using quadratic cost functions:: - Q = np.diag([0.1, 10, .1]) # keep lateral error low - R = np.eye(2) * 0.1 - cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf) + 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) We also constraint the maximum turning rate to 0.1 radians (about 6 degees) and constrain the velocity to be in the range of 9 m/s to 11 m/s:: @@ -230,20 +232,19 @@ and constrain the velocity to be in the range of 9 m/s to 11 m/s:: Finally, we solve for the optimal inputs:: - horizon = np.linspace(0, Tf, 20, endpoint=True) - bend_left = [10, 0.01] # slight left veer - + horizon = np.linspace(0, Tf, 3, endpoint=True) result = opt.solve_ocp( - vehicle, horizon, x0, cost, constraints, initial_guess=bend_left, - options={'eps': 0.01}) # set step size for gradient calculation - - # Extract the results - u = result.inputs - t, y = ct.input_output_response(vehicle, horizon, u, x0) + vehicle, horizon, x0, traj_cost, constraints, + terminal_cost=term_cost, initial_guess=u0) Plotting the results:: - # Plot the results + # Simulate the system dynamics (open loop) + resp = ct.input_output_response( + vehicle, horizon, result.inputs, x0, + t_eval=np.linspace(0, Tf, 100)) + t, y, u = resp.time, resp.outputs, resp.inputs + plt.subplot(3, 1, 1) plt.plot(y[0], y[1]) plt.plot(x0[0], x0[1], 'ro', xf[0], xf[1], 'ro') @@ -252,15 +253,13 @@ Plotting the results:: plt.subplot(3, 1, 2) plt.plot(t, u[0]) - plt.axis([0, 10, 8.5, 11.5]) - plt.plot([0, 10], [9, 9], 'k--', [0, 10], [11, 11], 'k--') + plt.axis([0, 10, 9.9, 10.1]) plt.xlabel("t [sec]") plt.ylabel("u1 [m/s]") plt.subplot(3, 1, 3) plt.plot(t, u[1]) - plt.axis([0, 10, -0.15, 0.15]) - plt.plot([0, 10], [-0.1, -0.1], 'k--', [0, 10], [0.1, 0.1], 'k--') + plt.axis([0, 10, -0.01, 0.01]) plt.xlabel("t [sec]") plt.ylabel("u2 [rad/s]") @@ -272,6 +271,47 @@ yields .. image:: steering-optimal.png +Optimization Tips +================= + +The python-control optimization module makes use of the SciPy optimization +toolbox and it can sometimes be tricky to get the optimization to converge. +If you are getting errors when solving optimal control problems or your +solutions do not seem close to optimal, here are a few things to try: + +* Less is more: try using a smaller number of time points in your + optimiation. The default optimal control problem formulation uses the + value of the inputs at each time point as a free variable and this can + generate a large number of parameters quickly. Often you can find very + good solutions with a small number of free variables (the example above + uses 3 time points for 2 inputs, so a total of 6 optimization variables). + Note that you can "resample" the optimal trajectory by running a + simulation of the sytem and using the `t_eval` keyword in + `input_output_response` (as done above). + +* Use a smooth basis: as an alternative to parameterizing the optimal + control inputs using the value of the control at the listed time points, + you can specify a set of basis functions using the `basis` keyword in + :func:`~control.solve_ocp` and then parameterize the controller by linear + combination of the basis functions. The :mod:`!control.flatsys` module + defines several sets of basis functions that can be used. + +* Tweak the optimizer: by using the `minimize_method`, `minimize_options`, + and `minimize_kwargs` keywords in :func:`~control.solve_ocp`, you can + choose the SciPy optimization function that you use and set many + parameters. See :func:`scipy.optimize.minimize` for more information on + the optimzers that are available and the options and keywords that they + accept. + +* Walk before you run: try setting up a simpler version of the optimization, + remove constraints or simplifying the cost to get a simple version of the + problem working and then add complexity. Sometimes this can help you find + the right set of options or identify situations in which you are being too + aggressive in what your are trying to get the system to do. + +See :ref:`steering-optimal` for some examples of different problem +formulations. + Module classes and functions ============================ diff --git a/doc/steering-optimal.png b/doc/steering-optimal.png index 6ff50c0f4..f847923b5 100644 Binary files a/doc/steering-optimal.png and b/doc/steering-optimal.png differ diff --git a/doc/steering-optimal.py b/doc/steering-optimal.py new file mode 120000 index 000000000..506033ec1 --- /dev/null +++ b/doc/steering-optimal.py @@ -0,0 +1 @@ +../examples/steering-optimal.py \ No newline at end of file diff --git a/doc/steering-optimal.rst b/doc/steering-optimal.rst new file mode 100644 index 000000000..777278c1c --- /dev/null +++ b/doc/steering-optimal.rst @@ -0,0 +1,14 @@ +.. _steering-optimal: + +Optimal control for vehicle steeering (lane change) +--------------------------------------------------- + +Code +.... +.. literalinclude:: steering-optimal.py + :language: python + :linenos: + + +Notes +..... pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy