diff --git a/.gitignore b/.gitignore index 0262ab46f..b95f1730e 100644 --- a/.gitignore +++ b/.gitignore @@ -7,7 +7,7 @@ MANIFEST control/_version.py __conda_*.txt record.txt -build.log +*.log *.egg-info/ .eggs/ .coverage @@ -23,3 +23,6 @@ Untitled*.ipynb # Files created by or for emacs (RMM, 29 Dec 2017) *~ TAGS + +# Files created by or for asv (airspeed velocity) +.asv/ diff --git a/asv.conf.json b/asv.conf.json new file mode 100644 index 000000000..590c24db0 --- /dev/null +++ b/asv.conf.json @@ -0,0 +1,161 @@ +{ + // The version of the config file format. Do not change, unless + // you know what you are doing. + "version": 1, + + // The name of the project being benchmarked + "project": "python-control", + + // The project's homepage + "project_url": "http://python-control.org/", + + // The URL or local path of the source code repository for the + // project being benchmarked + "repo": ".", + + // The Python project's subdirectory in your repo. If missing or + // the empty string, the project is assumed to be located at the root + // of the repository. + // "repo_subdir": ".", + + // Customizable commands for building, installing, and + // uninstalling the project. See asv.conf.json documentation. + // + // "install_command": ["in-dir={env_dir} python -mpip install {wheel_file}"], + // "uninstall_command": ["return-code=any python -mpip uninstall -y {project}"], + "build_command": [ + "python make_version.py", + "python setup.py build", + "PIP_NO_BUILD_ISOLATION=false python -mpip wheel --no-deps --no-index -w {build_cache_dir} {build_dir}" + ], + + // List of branches to benchmark. If not provided, defaults to "master" + // (for git) or "default" (for mercurial). + // "branches": ["master"], // for git + // "branches": ["default"], // for mercurial + + // The DVCS being used. If not set, it will be automatically + // determined from "repo" by looking at the protocol in the URL + // (if remote), or by looking for special directories, such as + // ".git" (if local). + // "dvcs": "git", + + // The tool to use to create environments. May be "conda", + // "virtualenv" or other value depending on the plugins in use. + // If missing or the empty string, the tool will be automatically + // determined by looking for tools on the PATH environment + // variable. + "environment_type": "conda", + + // timeout in seconds for installing any dependencies in environment + // defaults to 10 min + //"install_timeout": 600, + + // the base URL to show a commit for the project. + "show_commit_url": "http://github.com/python-control/python-control/commit/", + + // The Pythons you'd like to test against. If not provided, defaults + // to the current version of Python used to run `asv`. + // "pythons": ["2.7", "3.6"], + + // The list of conda channel names to be searched for benchmark + // dependency packages in the specified order + // "conda_channels": ["conda-forge", "defaults"], + + // The matrix of dependencies to test. Each key is the name of a + // package (in PyPI) and the values are version numbers. An empty + // list or empty string indicates to just test against the default + // (latest) version. null indicates that the package is to not be + // installed. If the package to be tested is only available from + // PyPi, and the 'environment_type' is conda, then you can preface + // the package name by 'pip+', and the package will be installed via + // pip (with all the conda available packages installed first, + // followed by the pip installed packages). + // + // "matrix": { + // "numpy": ["1.6", "1.7"], + // "six": ["", null], // test with and without six installed + // "pip+emcee": [""], // emcee is only available for install with pip. + // }, + + // Combinations of libraries/python versions can be excluded/included + // from the set to test. Each entry is a dictionary containing additional + // key-value pairs to include/exclude. + // + // An exclude entry excludes entries where all values match. The + // values are regexps that should match the whole string. + // + // An include entry adds an environment. Only the packages listed + // are installed. The 'python' key is required. The exclude rules + // do not apply to includes. + // + // In addition to package names, the following keys are available: + // + // - python + // Python version, as in the *pythons* variable above. + // - environment_type + // Environment type, as above. + // - sys_platform + // Platform, as in sys.platform. Possible values for the common + // cases: 'linux2', 'win32', 'cygwin', 'darwin'. + // + // "exclude": [ + // {"python": "3.2", "sys_platform": "win32"}, // skip py3.2 on windows + // {"environment_type": "conda", "six": null}, // don't run without six on conda + // ], + // + // "include": [ + // // additional env for python2.7 + // {"python": "2.7", "numpy": "1.8"}, + // // additional env if run on windows+conda + // {"platform": "win32", "environment_type": "conda", "python": "2.7", "libpython": ""}, + // ], + + // The directory (relative to the current directory) that benchmarks are + // stored in. If not provided, defaults to "benchmarks" + // "benchmark_dir": "benchmarks", + + // The directory (relative to the current directory) to cache the Python + // environments in. If not provided, defaults to "env" + "env_dir": ".asv/env", + + // The directory (relative to the current directory) that raw benchmark + // results are stored in. If not provided, defaults to "results". + "results_dir": ".asv/results", + + // The directory (relative to the current directory) that the html tree + // should be written to. If not provided, defaults to "html". + "html_dir": ".asv/html", + + // The number of characters to retain in the commit hashes. + // "hash_length": 8, + + // `asv` will cache results of the recent builds in each + // environment, making them faster to install next time. This is + // the number of builds to keep, per environment. + // "build_cache_size": 2, + + // The commits after which the regression search in `asv publish` + // should start looking for regressions. Dictionary whose keys are + // regexps matching to benchmark names, and values corresponding to + // the commit (exclusive) after which to start looking for + // regressions. The default is to start from the first commit + // with results. If the commit is `null`, regression detection is + // skipped for the matching benchmark. + // + // "regressions_first_commits": { + // "some_benchmark": "352cdf", // Consider regressions only after this commit + // "another_benchmark": null, // Skip regression detection altogether + // }, + + // The thresholds for relative change in results, after which `asv + // publish` starts reporting regressions. Dictionary of the same + // form as in ``regressions_first_commits``, with values + // indicating the thresholds. If multiple entries match, the + // maximum is taken. If no entry matches, the default is 5%. + // + // "regressions_thresholds": { + // "some_benchmark": 0.01, // Threshold of 1% + // "another_benchmark": 0.5, // Threshold of 50% + // }, +} diff --git a/benchmarks/README b/benchmarks/README new file mode 100644 index 000000000..a10bbfc21 --- /dev/null +++ b/benchmarks/README @@ -0,0 +1,39 @@ +This directory contains various scripts that can be used to measure the +performance of the python-control package. The scripts are intended to be +used with the airspeed velocity package (https://pypi.org/project/asv/) and +are mainly intended for use by developers in identfying potential +improvements to their code. + +Running benchmarks +------------------ +To run the benchmarks listed here against the current (uncommitted) code, +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 + + asv run + +where is a range of commits to benchmark. To check against the HEAD +of the branch that is currently checked out, use + + asv run HEAD^! + +Code profiling +-------------- +You can also use the benchmarks to profile code and look for bottlenecks. +To profile a given test against the current (uncommitted) code use + + PYTHONPATH=`pwd` asv profile --python=python . + +where is the name of one of the files in the benchmark/ subdirectory +and is the name of a test function in that file. + +If you have the `snakeviz` profiling visualization package installed, the +following command will profile a test against the HEAD of the current branch +and open a graphical representation of the profiled code: + + asv profile --gui snakeviz . HEAD + +RMM, 27 Feb 2021 diff --git a/benchmarks/__init__.py b/benchmarks/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/benchmarks/optimal_bench.py b/benchmarks/optimal_bench.py new file mode 100644 index 000000000..4b34ef04d --- /dev/null +++ b/benchmarks/optimal_bench.py @@ -0,0 +1,224 @@ +# optimal_bench.py - benchmarks for optimal control package +# RMM, 27 Feb 2020 +# +# This benchmark tests the timing for the optimal control module +# (control.optimal) and is intended to be used for helping tune the +# performance of the functions used for optimization-base control. + +import numpy as np +import math +import control as ct +import control.flatsys as flat +import control.optimal as opt +import matplotlib.pyplot as plt +import logging +import time +import os + +# 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) + + # Saturate the steering input (use min/max instead of clip for speed) + phi = max(-phimax, min(u[1], phimax)) + + # 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) + ]) + +def vehicle_output(t, x, u, params): + return x # return x, y, theta (full state) + +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 + +# Define the time horizon (and spacing) for the optimization +horizon = np.linspace(0, Tf, 10, endpoint=True) + +# Provide an intial guess (will be extended to entire horizon) +bend_left = [10, 0.01] # slight left veer + +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 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 + +# 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}), +} + + +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 + +# 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 + 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 + +# Reset the timeout value to allow for longer runs +time_steering_bezier_basis.timeout = 120 + +# 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]) + +def time_aircraft_mpc(): + # 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], + [ 0, 0.94, 0, 0.29, 0], + [ 0, 0.14, 0.81, -0.9, 0], + [ 0, -0.2, 0, 0.95, 0], + [ 0, 0.09, 0, 0, 0.9]] + B = [[ 0.01, -0.02], + [-0.14, 0], + [ 0.05, -0.2], + [ 0.02, 0], + [-0.01, 0]] + C = [[0, 1, 0, 0, -1], + [0, 0, 1, 0, 0], + [0, 0, 0, 1, 0], + [1, 0, 0, 0, 0]] + model = ct.ss2io(ct.ss(A, B, C, 0, 0.2)) + + # For the simulation we need the full state output + sys = ct.ss2io(ct.ss(A, B, np.eye(5), 0, 0.2)) + + # compute the steady state values for a particular value of the input + ud = np.array([0.8, -0.3]) + xd = np.linalg.inv(np.eye(5) - A) @ B @ ud + yd = C @ xd + + # provide constraints on the system signals + constraints = [opt.input_range_constraint(sys, [-5, -6], [5, 6])] + + # provide penalties on the system signals + Q = model.C.transpose() @ np.diag([10, 10, 10, 10]) @ model.C + R = np.diag([3, 2]) + cost = opt.quadratic_cost(model, Q, R, x0=xd, u0=ud) + + # online MPC controller object is constructed with a horizon 6 + ctrl = opt.create_mpc_iosystem( + model, np.arange(0, 6) * 0.2, cost, constraints) + + # Define an I/O system implementing model predictive control + loop = ct.feedback(sys, ctrl, 1) + + # Choose a nearby initial condition to speed up computation + X0 = np.hstack([xd, np.kron(ud, np.ones(6))]) * 0.99 + + Nsim = 12 + tout, xout = ct.input_output_response( + loop, np.arange(0, Nsim) * 0.2, 0, X0) + + # 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) diff --git a/control/config.py b/control/config.py index 9bb2dfcf4..2d2cc6248 100644 --- a/control/config.py +++ b/control/config.py @@ -67,7 +67,7 @@ def reset_defaults(): defaults.update(_iosys_defaults) -def _get_param(module, param, argval=None, defval=None, pop=False): +def _get_param(module, param, argval=None, defval=None, pop=False, last=False): """Return the default value for a configuration option. The _get_param() function is a utility function used to get the value of a @@ -91,11 +91,13 @@ def _get_param(module, param, argval=None, defval=None, pop=False): `config.defaults` dictionary. If a dictionary is provided, then `module.param` is used to determine the default value. Defaults to None. - pop : bool + pop : bool, optional If True and if argval is a dict, then pop the remove the parameter entry from the argval dict after retreiving it. This allows the use of a keyword argument list to be passed through to other functions internal to the function being called. + last : bool, optional + If True, check to make sure dictionary is empy after processing. """ @@ -108,7 +110,10 @@ def _get_param(module, param, argval=None, defval=None, pop=False): # If we were passed a dict for the argval, get the param value from there if isinstance(argval, dict): - argval = argval.pop(param, None) if pop else argval.get(param, None) + val = argval.pop(param, None) if pop else argval.get(param, None) + if last and argval: + raise TypeError("unrecognized keywords: " + str(argval)) + argval = val # If we were passed a dict for the defval, get the param value from there if isinstance(defval, dict): diff --git a/control/flatsys/__init__.py b/control/flatsys/__init__.py index 9ff1e2337..0926fa81a 100644 --- a/control/flatsys/__init__.py +++ b/control/flatsys/__init__.py @@ -53,6 +53,7 @@ # Basis function families from .basis import BasisFamily from .poly import PolyFamily +from .bezier import BezierFamily # Classes from .systraj import SystemTrajectory diff --git a/control/flatsys/basis.py b/control/flatsys/basis.py index 83ea89cbd..7592b79a2 100644 --- a/control/flatsys/basis.py +++ b/control/flatsys/basis.py @@ -51,3 +51,10 @@ class BasisFamily: def __init__(self, N): """Create a basis family of order N.""" self.N = N # save number of basis functions + + def __call__(self, i, t): + """Evaluate the ith basis function at a point in time""" + return self.eval_deriv(i, 0, t) + + def eval_deriv(self, i, j, t): + raise NotImplementedError("Internal error; improper basis functions") diff --git a/control/flatsys/bezier.py b/control/flatsys/bezier.py new file mode 100644 index 000000000..1eb7a549f --- /dev/null +++ b/control/flatsys/bezier.py @@ -0,0 +1,69 @@ +# bezier.m - 1D Bezier curve basis functions +# RMM, 24 Feb 2021 +# +# This class implements a set of basis functions based on Bezier curves: +# +# \phi_i(t) = \sum_{i=0}^n {n \choose i} (T - t)^{n-i} t^i +# + +# Copyright (c) 2012 by California Institute of Technology +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# 3. Neither the name of the California Institute of Technology nor +# the names of its contributors may be used to endorse or promote +# products derived from this software without specific prior +# written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL CALTECH +# OR THE CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF +# USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT +# OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF +# SUCH DAMAGE. + +import numpy as np +from scipy.special import binom +from .basis import BasisFamily + +class BezierFamily(BasisFamily): + r"""Polynomial basis functions. + + This class represents the family of polynomials of the form + + .. math:: + \phi_i(t) = \sum_{i=0}^n {n \choose i} (T - t)^{n-i} t^i + + """ + def __init__(self, N, T=1): + """Create a polynomial basis of order N.""" + self.N = N # save number of basis functions + self.T = T # save end of time interval + + # Compute the kth derivative of the ith basis function at time t + def eval_deriv(self, i, k, t): + """Evaluate the kth derivative of the ith basis function at time t.""" + if k > 0: + raise NotImplementedError("Bezier derivatives not yet available") + elif i > self.N: + raise ValueError("Basis function index too high") + + # Return the Bezier basis function (note N = # basis functions) + return binom(self.N - 1, i) * \ + (t/self.T)**i * (1 - t/self.T)**(self.N - i - 1) diff --git a/control/iosys.py b/control/iosys.py index 16ef633b7..7ed4c8b05 100644 --- a/control/iosys.py +++ b/control/iosys.py @@ -1412,9 +1412,10 @@ def __init__(self, io_sys, ss_sys=None): raise TypeError("Second argument must be a state space system.") -def input_output_response(sys, T, U=0., X0=0, params={}, method='RK45', - transpose=False, return_x=False, squeeze=None): - +def input_output_response( + sys, T, U=0., X0=0, params={}, + transpose=False, return_x=False, squeeze=None, + solve_ivp_kwargs={}, **kwargs): """Compute the output response of a system to a given input. Simulate a dynamical system with a given input and return its output @@ -1457,7 +1458,33 @@ def input_output_response(sys, T, U=0., X0=0, params={}, method='RK45', ValueError If time step does not match sampling time (for discrete time systems) + Additional parameters + --------------------- + solve_ivp_method : str, optional + Set the method used by :func:`scipy.integrate.solve_ivp`. Defaults + to 'RK45'. + solve_ivp_kwargs : str, optional + Pass additional keywords to :func:`scipy.integrate.solve_ivp`. + """ + # + # Process keyword arguments + # + + # Allow method as an alternative to solve_ivp_method + if kwargs.get('method', None): + solve_ivp_kwargs['method'] = kwargs.pop('method') + + # Figure out the method to be used + 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'] + + # Set the default method to 'RK45' + if solve_ivp_kwargs.get('method', None) is None: + solve_ivp_kwargs['method'] = 'RK45' + # Sanity checking on the input if not isinstance(sys, InputOutputSystem): raise TypeError("System of type ", type(sys), " not valid") @@ -1495,17 +1522,36 @@ def input_output_response(sys, T, U=0., X0=0, params={}, method='RK45', # Update the parameter values sys._update_params(params) + # + # Define a function to evaluate the input at an arbitrary time + # + # This is equivalent to the function + # + # ufun = sp.interpolate.interp1d(T, U, fill_value='extrapolate') + # + # but has a lot less overhead => simulation runs much faster + def ufun(t): + # Find the value of the index using linear interpolation + idx = np.searchsorted(T, t, side='left') + if idx == 0: + # For consistency in return type, multiple by a float + return U[..., 0] * 1. + else: + dt = (t - T[idx-1]) / (T[idx] - T[idx-1]) + return U[..., idx-1] * (1. - dt) + U[..., idx] * dt + # Create a lambda function for the right hand side - u = sp.interpolate.interp1d(T, U, fill_value="extrapolate") - def ivp_rhs(t, x): return sys._rhs(t, x, u(t)) + def ivp_rhs(t, x): + return sys._rhs(t, x, ufun(t)) # Perform the simulation if isctime(sys): if not hasattr(sp.integrate, 'solve_ivp'): 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, - method=method, vectorized=False) + soln = sp.integrate.solve_ivp( + ivp_rhs, (T0, Tf), X0, t_eval=T, + vectorized=False, **solve_ivp_kwargs) # 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) @@ -1546,10 +1592,10 @@ def ivp_rhs(t, x): return sys._rhs(t, x, u(t)) for i in range(len(T)): # Store the current state and output soln.y.append(x) - y.append(sys._out(T[i], x, u(T[i]))) + y.append(sys._out(T[i], x, ufun(T[i]))) # Update the state for the next iteration - x = sys._rhs(T[i], x, u(T[i])) + x = sys._rhs(T[i], x, ufun(T[i])) # Convert output to numpy arrays soln.y = np.transpose(np.array(soln.y)) diff --git a/control/optimal.py b/control/optimal.py new file mode 100644 index 000000000..9ec25b4fc --- /dev/null +++ b/control/optimal.py @@ -0,0 +1,1334 @@ +# optimal.py - optimization based control module +# +# RMM, 11 Feb 2021 +# + +"""The :mod:`~control.optimal` module provides support for optimization-based +controllers for nonlinear systems with state and input constraints. + +""" + +import numpy as np +import scipy as sp +import scipy.optimize as opt +import control as ct +import warnings +import logging +import time + +from .timeresp import _process_time_response + +__all__ = ['find_optimal_input'] + + +class OptimalControlProblem(): + """Description of a finite horizon, optimal control problem + + The `OptimalControlProblem` class holds all of the information required to + specify and optimal control problem: the system dynamics, cost function, + and constraints. As much as possible, the information used to specify an + optimal control problem matches the notation and terminology of the SciPy + `optimize.minimize` module, with the hope that this makes it easier to + remember how to describe a problem. + + Notes + ----- + This class sets up an optimization over the inputs at each point in + time, using the integral and terminal costs as well as the + trajectory and terminal constraints. The `compute_trajectory` + method sets up an optimization problem that can be solved using + :func:`scipy.optimize.minimize`. + + The `_cost_function` method takes the information computes the cost of the + trajectory generated by the proposed input. It does this by calling a + user-defined function for the integral_cost given the current states and + inputs at each point along the trajetory and then adding the value of a + user-defined terminal cost at the final pint in the trajectory. + + The `_constraint_function` method evaluates the constraint functions along + the trajectory generated by the proposed input. As in the case of the + cost function, the constraints are evaluated at the state and input along + each point on the trjectory. This information is compared against the + constraint upper and lower bounds. The constraint function is processed + in the class initializer, so that it only needs to be computed once. + + If `basis` is specified, then the optimization is done over coefficients + of the basis elements. Otherwise, the optimization is performed over the + values of the input at the specified times (using linear interpolation for + continuous systems). + + """ + def __init__( + self, sys, time_vector, integral_cost, trajectory_constraints=[], + terminal_cost=None, terminal_constraints=[], initial_guess=None, + basis=None, log=False, **kwargs): + """Set up an optimal control problem + + To describe an optimal control problem we need an input/output system, + a time horizon, a cost function, and (optionally) a set of constraints + on the state and/or input, either along the trajectory and at the + terminal time. + + Parameters + ---------- + sys : InputOutputSystem + I/O system for which the optimal input will be computed. + time_vector : 1D array_like + List of times at which the optimal input should be computed. + integral_cost : callable + Function that returns the integral cost given the current state + and input. Called as integral_cost(x, u). + trajectory_constraints : list of tuples, optional + List of constraints that should hold at each point in the time + vector. Each element of the list should consist of a tuple with + first element given by :meth:`~scipy.optimize.LinearConstraint` or + :meth:`~scipy.optimize.NonlinearConstraint` and the remaining + elements of the tuple are the arguments that would be passed to + those functions. The constrains will be applied at each point + along the trajectory. + terminal_cost : callable, optional + Function that returns the terminal cost given the current state + and input. Called as terminal_cost(x, u). + initial_guess : 1D or 2D array_like + Initial inputs to use as a guess for the optimal input. The + inputs should either be a 2D vector of shape (ninputs, horizon) + or a 1D input of shape (ninputs,) that will be broadcast by + extension of the time axis. + log : bool, optional + If `True`, turn on logging messages (using Python logging module). + kwargs : dict, optional + Additional parameters (passed to :func:`scipy.optimal.minimize`). + + Returns + ------- + ocp : OptimalControlProblem + Optimal control problem object, to be used in computing optimal + controllers. + + Additional parameters + --------------------- + solve_ivp_method : str, optional + Set the method used by :func:`scipy.integrate.solve_ivp`. + solve_ivp_kwargs : str, optional + Pass additional keywords to :func:`scipy.integrate.solve_ivp`. + minimize_method : str, optional + Set the method used by :func:`scipy.optimize.minimize`. + minimize_options : str, optional + Set the options keyword used by :func:`scipy.optimize.minimize`. + minimize_kwargs : str, optional + Pass additional keywords to :func:`scipy.optimize.minimize`. + + """ + # Save the basic information for use later + self.system = sys + self.time_vector = time_vector + self.integral_cost = integral_cost + self.terminal_cost = terminal_cost + self.terminal_constraints = terminal_constraints + self.basis = basis + + # 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.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', {})) + + # Process trajectory constraints + if isinstance(trajectory_constraints, tuple): + self.trajectory_constraints = [trajectory_constraints] + elif not isinstance(trajectory_constraints, list): + raise TypeError("trajectory constraints must be a list") + else: + self.trajectory_constraints = trajectory_constraints + + # Process terminal constraints + if isinstance(terminal_constraints, tuple): + self.terminal_constraints = [terminal_constraints] + elif not isinstance(terminal_constraints, list): + raise TypeError("terminal constraints must be a list") + else: + self.terminal_constraints = terminal_constraints + + # + # Compute and store constraints + # + # While the constraints are evaluated during the execution of the + # SciPy optimization method itself, we go ahead and pre-compute the + # `scipy.optimize.NonlinearConstraint` function that will be passed to + # the optimizer on initialization, since it doesn't change. This is + # mainly a matter of computing the lower and upper bound vectors, + # which we need to "stack" to account for the evaluation at each + # trajectory time point plus any terminal constraints (in a way that + # is consistent with the `_constraint_function` that is used at + # evaluation time. + # + constraint_lb, constraint_ub, eqconst_value = [], [], [] + + # Go through each time point and stack the bounds + for t in self.time_vector: + for constraint in self.trajectory_constraints: + type, fun, lb, ub = constraint + if np.all(lb == ub): + # Equality constraint + eqconst_value.append(lb) + else: + # Inequality constraint + constraint_lb.append(lb) + constraint_ub.append(ub) + + # Add on the terminal constraints + for constraint in self.terminal_constraints: + type, fun, lb, ub = constraint + if np.all(lb == ub): + # Equality constraint + eqconst_value.append(lb) + else: + # Inequality constraint + constraint_lb.append(lb) + constraint_ub.append(ub) + + # Turn constraint vectors into 1D arrays + self.constraint_lb = np.hstack(constraint_lb) if constraint_lb else [] + self.constraint_ub = np.hstack(constraint_ub) if constraint_ub else [] + self.eqconst_value = np.hstack(eqconst_value) if eqconst_value else [] + + # Create the constraints (inequality and equality) + self.constraints = [] + if len(self.constraint_lb) != 0: + self.constraints.append(sp.optimize.NonlinearConstraint( + self._constraint_function, self.constraint_lb, + self.constraint_ub)) + if len(self.eqconst_value) != 0: + self.constraints.append(sp.optimize.NonlinearConstraint( + self._eqconst_function, self.eqconst_value, + self.eqconst_value)) + + # Process the initial guess + self.initial_guess = self._process_initial_guess(initial_guess) + + # Store states, input, used later to minimize re-computation + self.last_x = np.full(self.system.nstates, np.nan) + self.last_coeffs = np.full(self.initial_guess.shape, np.nan) + + # Reset run-time statistics + self._reset_statistics(log) + + # Log information + if log: + logging.info("New optimal control problem initailized") + + # + # Cost function + # + # Given the input U = [u[0], ... u[N]], we need to compute the cost of + # the trajectory generated by that input. This means we have to + # simulate the system to get the state trajectory X = [x[0], ..., + # x[N]] and then compute the cost at each point: + # + # cost = sum_k integral_cost(x[k], u[k]) + terminal_cost(x[N], u[N]) + # + # The initial state used for generating the simulation is stored in the + # class parameter `x` prior to calling the optimization algorithm. + # + def _cost_function(self, coeffs): + if self.log: + start_time = time.process_time() + logging.info("_cost_function called at: %g", start_time) + + # Retrieve the initial state and reshape the input vector + x = self.x + coeffs = coeffs.reshape((self.system.ninputs, -1)) + + # Compute time points (if basis present) + if self.basis: + if self.log: + logging.debug("coefficients = " + str(coeffs)) + inputs = self._coeffs_to_inputs(coeffs) + else: + inputs = coeffs + + # See if we already have a simulation for this condition + if np.array_equal(coeffs, self.last_coeffs) and \ + np.array_equal(x, self.last_x): + states = self.last_states + else: + if self.log: + logging.debug("calling input_output_response from state\n" + + str(x)) + logging.debug("initial input[0:3] =\n" + str(inputs[:, 0:3])) + + # Simulate the system to get the state + _, _, states = ct.input_output_response( + self.system, self.time_vector, inputs, x, return_x=True, + solve_ivp_kwargs=self.solve_ivp_kwargs) + self.system_simulations += 1 + self.last_x = x + self.last_coeffs = coeffs + self.last_states = states + + if self.log: + logging.debug("input_output_response returned states\n" + + str(states)) + + # Trajectory cost + # TODO: vectorize + if ct.isctime(self.system): + # Evaluate the costs + costs = [self.integral_cost(states[:, i], inputs[:, i]) for + i in range(self.time_vector.size)] + + # Compute the time intervals + dt = np.diff(self.time_vector) + + # Integrate the cost + cost = 0 + for i in range(self.time_vector.size-1): + # Approximate the integral using trapezoidal rule + cost += 0.5 * (costs[i] + costs[i+1]) * dt[i] + + else: + # Sum the integral cost over the time (second) indices + # cost += self.integral_cost(states[:,i], inputs[:,i]) + cost = sum(map( + self.integral_cost, np.transpose(states), + np.transpose(inputs))) + + # Terminal cost + if self.terminal_cost is not None: + cost += self.terminal_cost(states[:, -1], inputs[:, -1]) + + # Update statistics + self.cost_evaluations += 1 + if self.log: + stop_time = time.process_time() + self.cost_process_time += stop_time - start_time + logging.info( + "_cost_function returning %g; elapsed time: %g", + cost, stop_time - start_time) + + # Return the total cost for this input sequence + return cost + + # + # Constraints + # + # We are given the constraints along the trajectory and the terminal + # constraints, which each take inputs [x, u] and evaluate the + # constraint. How we handle these depends on the type of constraint: + # + # * For linear constraints (LinearConstraint), a combined vector of the + # state and input is multiplied by the polytope A matrix for + # comparison against the upper and lower bounds. + # + # * For nonlinear constraints (NonlinearConstraint), a user-specific + # constraint function having the form + # + # constraint_fun(x, u) TODO: convert from [x, u] to (x, u) + # + # is called at each point along the trajectory and compared against the + # upper and lower bounds. + # + # * If the upper and lower bound for the constraint is identical, then we + # separate out the evaluation into two different constraints, which + # allows the SciPy optimizers to be more efficient (and stops them from + # generating a warning about mixed constraints). This is handled + # through the use of the `_eqconst_function` and `eqconst_value` members. + # + # In both cases, the constraint is specified at a single point, but we + # extend this to apply to each point in the trajectory. This means + # that for N time points with m trajectory constraints and p terminal + # constraints we need to compute N*m + p constraints, each of which + # holds at a specific point in time, and implements the original + # constraint. + # + # To do this, we basically create a function that simulates the system + # dynamics and returns a vector of values corresponding to the value of + # the function at each time. The class initialization methods takes + # care of replicating the upper and lower bounds for each point in time + # so that the SciPy optimization algorithm can do the proper + # evaluation. + # + # In addition, since SciPy's optimization function does not allow us to + # pass arguments to the constraint function, we have to store the initial + # state prior to optimization and retrieve it here. + # + def _constraint_function(self, coeffs): + if self.log: + start_time = time.process_time() + logging.info("_constraint_function called at: %g", start_time) + + # Retrieve the initial state and reshape the input vector + x = self.x + coeffs = coeffs.reshape((self.system.ninputs, -1)) + + # Compute time points (if basis present) + if self.basis: + inputs = self._coeffs_to_inputs(coeffs) + else: + inputs = coeffs + + # See if we already have a simulation for this condition + if np.array_equal(coeffs, self.last_coeffs) \ + and np.array_equal(x, self.last_x): + states = self.last_states + else: + if self.log: + logging.debug("calling input_output_response from state\n" + + str(x)) + logging.debug("initial input[0:3] =\n" + str(inputs[:, 0:3])) + + # Simulate the system to get the state + _, _, states = ct.input_output_response( + self.system, self.time_vector, inputs, x, return_x=True, + solve_ivp_kwargs=self.solve_ivp_kwargs) + self.system_simulations += 1 + self.last_x = x + self.last_coeffs = coeffs + self.last_states = states + + # Evaluate the constraint function along the trajectory + value = [] + for i, t in enumerate(self.time_vector): + for constraint in self.trajectory_constraints: + type, fun, lb, ub = constraint + if np.all(lb == ub): + # Skip equality constraints + continue + elif type == opt.LinearConstraint: + # `fun` is the A matrix associated with the polytope... + value.append( + np.dot(fun, np.hstack([states[:, i], inputs[:, i]]))) + elif type == opt.NonlinearConstraint: + value.append(fun(states[:, i], inputs[:, i])) + else: + raise TypeError("unknown constraint type %s" % + constraint[0]) + + # Evaluate the terminal constraint functions + for constraint in self.terminal_constraints: + type, fun, lb, ub = constraint + if np.all(lb == ub): + # Skip equality constraints + continue + elif type == opt.LinearConstraint: + value.append( + np.dot(fun, np.hstack([states[:, i], inputs[:, i]]))) + elif type == opt.NonlinearConstraint: + value.append(fun(states[:, i], inputs[:, i])) + else: + raise TypeError("unknown constraint type %s" % + constraint[0]) + + # Update statistics + self.constraint_evaluations += 1 + if self.log: + stop_time = time.process_time() + self.constraint_process_time += stop_time - start_time + logging.info( + "_constraint_function elapsed time: %g", + stop_time - start_time) + + # Debugging information + if self.log: + logging.debug( + "constraint values\n" + str(value) + "\n" + + "lb, ub =\n" + str(self.constraint_lb) + "\n" + + str(self.constraint_ub)) + + # Return the value of the constraint function + return np.hstack(value) + + def _eqconst_function(self, coeffs): + if self.log: + start_time = time.process_time() + logging.info("_eqconst_function called at: %g", start_time) + + # Retrieve the initial state and reshape the input vector + x = self.x + coeffs = coeffs.reshape((self.system.ninputs, -1)) + + # Compute time points (if basis present) + if self.basis: + inputs = self._coeffs_to_inputs(coeffs) + else: + inputs = coeffs + + # See if we already have a simulation for this condition + if np.array_equal(coeffs, self.last_coeffs) and \ + np.array_equal(x, self.last_x): + states = self.last_states + else: + if self.log: + logging.debug("calling input_output_response from state\n" + + str(x)) + logging.debug("initial input[0:3] =\n" + str(inputs[:, 0:3])) + + # Simulate the system to get the state + _, _, states = ct.input_output_response( + self.system, self.time_vector, inputs, x, return_x=True, + solve_ivp_kwargs=self.solve_ivp_kwargs) + self.system_simulations += 1 + self.last_x = x + self.last_coeffs = coeffs + self.last_states = states + + if self.log: + logging.debug("input_output_response returned states\n" + + str(states)) + + # Evaluate the constraint function along the trajectory + value = [] + for i, t in enumerate(self.time_vector): + for constraint in self.trajectory_constraints: + type, fun, lb, ub = constraint + if np.any(lb != ub): + # Skip inequality constraints + continue + elif type == opt.LinearConstraint: + # `fun` is the A matrix associated with the polytope... + value.append( + np.dot(fun, np.hstack([states[:, i], inputs[:, i]]))) + elif type == opt.NonlinearConstraint: + value.append(fun(states[:, i], inputs[:, i])) + else: + raise TypeError("unknown constraint type %s" % + constraint[0]) + + # Evaluate the terminal constraint functions + for constraint in self.terminal_constraints: + type, fun, lb, ub = constraint + if np.any(lb != ub): + # Skip inequality constraints + continue + elif type == opt.LinearConstraint: + value.append( + np.dot(fun, np.hstack([states[:, i], inputs[:, i]]))) + elif type == opt.NonlinearConstraint: + value.append(fun(states[:, i], inputs[:, i])) + else: + raise TypeError("unknown constraint type %s" % + constraint[0]) + + # Update statistics + self.eqconst_evaluations += 1 + if self.log: + stop_time = time.process_time() + self.eqconst_process_time += stop_time - start_time + logging.info( + "_eqconst_function elapsed time: %g", stop_time - start_time) + + # Debugging information + if self.log: + logging.debug( + "eqconst values\n" + str(value) + "\n" + + "desired =\n" + str(self.eqconst_value)) + + # Return the value of the constraint function + return np.hstack(value) + + # + # Initial guess + # + # We store an initial guess in case it is not specified later. Note + # that create_mpc_iosystem() will reset the initial guess based on + # the current state of the MPC controller. + # + # Note: the initial guess is passed as the inputs at the given time + # vector. If a basis is specified, this is converted to coefficient + # values (which are generally of smaller dimension). + # + def _process_initial_guess(self, initial_guess): + if initial_guess is not None: + # Convert to a 1D array (or higher) + initial_guess = np.atleast_1d(initial_guess) + + # See whether we got entire guess or just first time point + if len(initial_guess.shape) == 1: + # Broadcast inputs to entire time vector + try: + initial_guess = np.broadcast_to( + initial_guess.reshape(-1, 1), + (self.system.ninputs, self.time_vector.size)) + except ValueError: + raise ValueError("initial guess is the wrong shape") + + elif initial_guess.shape != \ + (self.system.ninputs, self.time_vector.size): + raise ValueError("initial guess is the wrong shape") + + # If we were given a basis, project onto the basis elements + if self.basis is not None: + initial_guess = self._inputs_to_coeffs(initial_guess) + + # Reshape for use by scipy.optimize.minimize() + return initial_guess.reshape(-1) + + # Default is zero + return np.zeros( + self.system.ninputs * + (self.time_vector.size if self.basis is None else self.basis.N)) + + # + # Utility function to convert input vector to coefficient vector + # + # Initially guesses from the user are passed as input vectors as a + # function of time, but internally we store the guess in terms of the + # basis coefficients. We do this by solving a least squares probelm to + # find coefficients that match the input functions at the time points (as + # much as possible, if the problem is under-determined). + # + def _inputs_to_coeffs(self, inputs): + # If there is no basis function, just return inputs as coeffs + if self.basis is None: + return inputs + + # Solve least squares problems (M x = b) for coeffs on each input + coeffs = np.zeros((self.system.ninputs, self.basis.N)) + for i in range(self.system.ninputs): + # Set up the matrices to get inputs + M = np.zeros((self.time_vector.size, self.basis.N)) + b = np.zeros(self.time_vector.size) + + # Evaluate at each time point and for each basis function + # TODO: vectorize + for j, t in enumerate(self.time_vector): + for k in range(self.basis.N): + M[j, k] = self.basis(k, t) + b[j] = inputs[i, j] + + # Solve a least squares problem for the coefficients + alpha, residuals, rank, s = np.linalg.lstsq(M, b, rcond=None) + coeffs[i, :] = alpha + + return coeffs + + # Utility function to convert coefficient vector to input vector + def _coeffs_to_inputs(self, coeffs): + # TODO: vectorize + inputs = np.zeros((self.system.ninputs, self.time_vector.size)) + for i, t in enumerate(self.time_vector): + for k in range(self.basis.N): + phi_k = self.basis(k, t) + for inp in range(self.system.ninputs): + inputs[inp, i] += coeffs[inp, k] * phi_k + return inputs + + # + # Log and statistics + # + # To allow some insight into where time is being spent, we keep track of + # the number of times that various functions are called and (optionally) + # how long we spent inside each function. + # + def _reset_statistics(self, log=False): + """Reset counters for keeping track of statistics""" + self.log = log + self.cost_evaluations, self.cost_process_time = 0, 0 + self.constraint_evaluations, self.constraint_process_time = 0, 0 + self.eqconst_evaluations, self.eqconst_process_time = 0, 0 + self.system_simulations = 0 + + def _print_statistics(self, reset=True): + """Print out summary statistics from last run""" + print("Summary statistics:") + print("* Cost function calls:", self.cost_evaluations) + if self.log: + print("* Cost function process time:", self.cost_process_time) + if self.constraint_evaluations: + print("* Constraint calls:", self.constraint_evaluations) + if self.log: + print( + "* Constraint process time:", self.constraint_process_time) + if self.eqconst_evaluations: + print("* Eqconst calls:", self.eqconst_evaluations) + if self.log: + print( + "* Eqconst process time:", self.eqconst_process_time) + print("* System simulations:", self.system_simulations) + if reset: + self._reset_statistics(self.log) + + # Create an input/output system implementing an MPC controller + def _create_mpc_iosystem(self, dt=True): + """Create an I/O system implementing an MPC controller""" + def _update(t, x, u, params={}): + coeffs = x.reshape((self.system.ninputs, -1)) + if self.basis: + # Keep the coeffecients unchanged + # TODO: could compute input vector, shift, and re-project (?) + self.initial_guess = coeffs + else: + # Shift the basis elements by one time step + self.initial_guess = np.hstack( + [coeffs[:, 1:], coeffs[:, -1:]]).reshape(-1) + res = self.compute_trajectory(u, print_summary=False) + return res.inputs.reshape(-1) + + def _output(t, x, u, params={}): + if self.basis: + # TODO: compute inputs from basis elements + raise NotImplementedError("basis elements not implemented") + else: + inputs = x.reshape((self.system.ninputs, -1)) + return inputs[:, 0] + + return ct.NonlinearIOSystem( + _update, _output, dt=dt, + inputs=self.system.nstates, outputs=self.system.ninputs, + states=self.system.ninputs * + (self.time_vector.size if self.basis is None else self.basis.N)) + + # Compute the optimal trajectory from the current state + def compute_trajectory( + self, x, squeeze=None, transpose=None, return_states=None, + initial_guess=None, print_summary=True, **kwargs): + """Compute the optimal input at state x + + Parameters + ---------- + x : array-like or number, optional + Initial state for the system. + return_states : bool, optional + If True, return the values of the state at each time (default = + False). + squeeze : bool, optional + If True and if the system has a single output, return the system + output as a 1D array rather than a 2D array. If False, return the + system output as a 2D array even if the system is SISO. Default + value set by config.defaults['control.squeeze_time_response']. + transpose : bool, optional + If True, assume that 2D input arrays are transposed from the + standard format. Used to convert MATLAB-style inputs to our + format. + + Returns + ------- + res : OptimalControlResult + Bundle object with the results of the optimal control problem. + res.success: bool + Boolean flag indicating whether the optimization was successful. + res.time : array + Time values of the input. + res.inputs : array + Optimal inputs for the system. If the system is SISO and squeeze + is not True, the array is 1D (indexed by time). If the system is + not SISO or squeeze is False, the array is 2D (indexed by the + output number and time). + res.states : array + Time evolution of the state vector (if return_states=True). + + """ + # Allow 'return_x` as a synonym for 'return_states' + return_states = ct.config._get_param( + 'optimal', 'return_x', kwargs, return_states, pop=True, last=True) + + # Store the initial state (for use in _constraint_function) + self.x = x + + # Allow the initial guess to be overriden + if initial_guess is None: + initial_guess = self.initial_guess + else: + initial_guess = self._process_initial_guess(initial_guess) + + # Call ScipPy optimizer + res = sp.optimize.minimize( + self._cost_function, initial_guess, + constraints=self.constraints, **self.minimize_kwargs) + + # Process and return the results + return OptimalControlResult( + self, res, transpose=transpose, return_states=return_states, + squeeze=squeeze, print_summary=print_summary) + + # Compute the current input to apply from the current state (MPC style) + def compute_mpc(self, x, squeeze=None): + """Compute the optimal input at state x + + This function calls the :meth:`compute_trajectory` method and returns + the input at the first time point. + + Parameters + ---------- + x: array-like or number, optional + Initial state for the system. + squeeze : bool, optional + If True and if the system has a single output, return the system + output as a 1D array rather than a 2D array. If False, return the + system output as a 2D array even if the system is SISO. Default + value set by config.defaults['control.squeeze_time_response']. + + Returns + ------- + input : array + Optimal input for the system at the current time. If the system + is SISO and squeeze is not True, the array is 1D (indexed by + time). If the system is not SISO or squeeze is False, the array + is 2D (indexed by the output number and time). Set to `None` + if the optimization failed. + + """ + res = self.compute_trajectory(x, squeeze=squeeze) + return inputs[:, 0] if res.success else None + + +# Optimal control result +class OptimalControlResult(sp.optimize.OptimizeResult): + """Represents the optimal control result + + This class is a subclass of :class:`sp.optimize.OptimizeResult` with + additional attributes associated with solving optimal control problems. + + Attributes + ---------- + inputs : ndarray + The optimal inputs associated with the optimal control problem. + states : ndarray + If `return_states` was set to true, stores the state trajectory + associated with the optimal input. + success : bool + Whether or not the optimizer exited successful. + problem : OptimalControlProblem + Optimal control problem that generated this solution. + + """ + def __init__( + self, ocp, res, return_states=False, print_summary=False, + transpose=None, squeeze=None): + """Create a OptimalControlResult object""" + + # Copy all of the fields we were sent by sp.optimize.minimize() + for key, val in res.items(): + setattr(self, key, val) + + # Remember the optimal control problem that we solved + self.problem = ocp + + # Reshape and process the input vector + coeffs = res.x.reshape((ocp.system.ninputs, -1)) + + # Compute time points (if basis present) + if ocp.basis: + inputs = ocp._coeffs_to_inputs(coeffs) + else: + inputs = coeffs + + # See if we got an answer + if not res.success: + warnings.warn( + "unable to solve optimal control problem\n" + "scipy.optimize.minimize returned " + res.message, UserWarning) + + # Optionally print summary information + if print_summary: + ocp._print_statistics() + + if return_states and inputs.shape[1] == ocp.time_vector.shape[0]: + # Simulate the system if we need the state back + _, _, states = ct.input_output_response( + ocp.system, ocp.time_vector, inputs, ocp.x, return_x=True, + solve_ivp_kwargs=ocp.solve_ivp_kwargs) + ocp.system_simulations += 1 + else: + states = None + + retval = _process_time_response( + ocp.system, ocp.time_vector, inputs, states, + transpose=transpose, return_x=return_states, squeeze=squeeze) + + self.time = retval[0] + self.inputs = retval[1] + self.states = None if states is None else retval[2] + + +# Compute the input for a nonlinear, (constrained) optimal control problem +def solve_ocp( + sys, horizon, X0, cost, constraints=[], terminal_cost=None, + terminal_constraints=[], initial_guess=None, basis=None, squeeze=None, + transpose=None, return_states=False, log=False, **kwargs): + + """Compute the solution to an optimal control problem + + Parameters + ---------- + sys : InputOutputSystem + I/O system for which the optimal input will be computed. + + horizon : 1D array_like + List of times at which the optimal input should be computed. + + X0: array-like or number, optional + Initial condition (default = 0). + + cost : callable + Function that returns the integral cost given the current state + and input. Called as cost(x, u). + + constraints : list of tuples, optional + List of constraints that should hold at each point in the time vector. + Each element of the list should consist of a tuple with first element + given by :meth:`scipy.optimize.LinearConstraint` or + :meth:`scipy.optimize.NonlinearConstraint` and the remaining + elements of the tuple are the arguments that would be passed to those + functions. The following tuples are supported: + + * (LinearConstraint, A, lb, ub): The matrix A is multiplied by stacked + vector of the state and input at each point on the trajectory for + comparison against the upper and lower bounds. + + * (NonlinearConstraint, fun, lb, ub): a user-specific constraint + function `fun(x, u)` is called at each point along the trajectory + and compared against the upper and lower bounds. + + The constraints are applied at each point along the trajectory. + + terminal_cost : callable, optional + Function that returns the terminal cost given the current state + and input. Called as terminal_cost(x, u). + + terminal_constraint : list of tuples, optional + List of constraints that should hold at the end of the trajectory. + Same format as `constraints`. + + initial_guess : 1D or 2D array_like + Initial inputs to use as a guess for the optimal input. The inputs + should either be a 2D vector of shape (ninputs, horizon) or a 1D + input of shape (ninputs,) that will be broadcast by extension of the + time axis. + + log : bool, optional + If `True`, turn on logging messages (using Python logging module). + + return_states : bool, optional + If True, return the values of the state at each time (default = False). + + squeeze : bool, optional + If True and if the system has a single output, return the system + output as a 1D array rather than a 2D array. If False, return the + system output as a 2D array even if the system is SISO. Default value + set by config.defaults['control.squeeze_time_response']. + + transpose : bool, optional + If True, assume that 2D input arrays are transposed from the standard + format. Used to convert MATLAB-style inputs to our format. + + kwargs : dict, optional + Additional parameters (passed to :func:`scipy.optimal.minimize`). + + Returns + ------- + res : OptimalControlResult + Bundle object with the results of the optimal control problem. + + res.success: bool + Boolean flag indicating whether the optimization was successful. + + res.time : array + Time values of the input. + + res.inputs : array + Optimal inputs for the system. If the system is SISO and squeeze is + not True, the array is 1D (indexed by time). If the system is not + SISO or squeeze is False, the array is 2D (indexed by the output + number and time). + + res.states : array + Time evolution of the state vector (if return_states=True). + + Notes + ----- + Additional keyword parameters can be used to fine tune the behavior of + the underlying optimization and integrations functions. See + :func:`OptimalControlProblem` for more information. + + """ + # Allow 'return_x` as a synonym for 'return_states' + return_states = ct.config._get_param( + 'optimal', 'return_x', kwargs, return_states, pop=True) + + # Set up the optimal control problem + ocp = OptimalControlProblem( + sys, horizon, cost, trajectory_constraints=constraints, + terminal_cost=terminal_cost, terminal_constraints=terminal_constraints, + initial_guess=initial_guess, basis=basis, log=log, **kwargs) + + # Solve for the optimal input from the current state + return ocp.compute_trajectory( + X0, squeeze=squeeze, transpose=transpose, return_states=return_states) + + +# Create a model predictive controller for an optimal control problem +def create_mpc_iosystem( + sys, horizon, cost, constraints=[], terminal_cost=None, + terminal_constraints=[], dt=True, log=False, **kwargs): + """Create a model predictive I/O control system + + This function creates an input/output system that implements a model + predictive control for a system given the time horizon, cost function and + constraints that define the finite-horizon optimization that should be + carried out at each state. + + Parameters + ---------- + sys : InputOutputSystem + I/O system for which the optimal input will be computed. + + horizon : 1D array_like + List of times at which the optimal input should be computed. + + cost : callable + Function that returns the integral cost given the current state + and input. Called as cost(x, u). + + constraints : list of tuples, optional + List of constraints that should hold at each point in the time vector. + See :func:`~control.optimal.solve_ocp` for more details. + + terminal_cost : callable, optional + Function that returns the terminal cost given the current state + and input. Called as terminal_cost(x, u). + + terminal_constraint : list of tuples, optional + List of constraints that should hold at the end of the trajectory. + Same format as `constraints`. + + kwargs : dict, optional + Additional parameters (passed to :func:`scipy.optimal.minimize`). + + Returns + ------- + ctrl : InputOutputSystem + An I/O system taking the currrent state of the model system and + returning the current input to be applied that minimizes the cost + function while satisfying the constraints. + + Notes + ----- + Additional keyword parameters can be used to fine tune the behavior of + the underlying optimization and integrations functions. See + :func:`OptimalControlProblem` for more information. + + """ + + # Set up the optimal control problem + ocp = OptimalControlProblem( + sys, horizon, cost, trajectory_constraints=constraints, + terminal_cost=terminal_cost, terminal_constraints=terminal_constraints, + log=log, **kwargs) + + # Return an I/O system implementing the model predictive controller + return ocp._create_mpc_iosystem(dt=dt) + + +# +# Functions to create cost functions (quadratic cost function) +# +# Since a quadratic function is common as a cost function, we provide a +# function that will take a Q and R matrix and return a callable that +# evaluates to associted quadratic cost. This is compatible with the way that +# the `_cost_function` evaluates the cost at each point in the trajectory. +# +def quadratic_cost(sys, Q, R, x0=0, u0=0): + """Create quadratic cost function + + Returns a quadratic cost function that can be used for an optimal control + problem. The cost function is of the form + + cost = (x - x0)^T Q (x - x0) + (u - u0)^T R (u - u0) + + Parameters + ---------- + sys : InputOutputSystem + I/O system for which the cost function is being defined. + Q : 2D array_like + Weighting matrix for state cost. Dimensions must match system state. + R : 2D array_like + Weighting matrix for input cost. Dimensions must match system input. + x0 : 1D array + Nomimal value of the system state (for which cost should be zero). + u0 : 1D array + Nomimal value of the system input (for which cost should be zero). + + Returns + ------- + cost_fun : callable + Function that can be used to evaluate the cost at a given state and + input. The call signature of the function is cost_fun(x, u). + + """ + # Process the input arguments + if Q is not None: + Q = np.atleast_2d(Q) + if Q.size == 1: # allow scalar weights + Q = np.eye(sys.nstates) * Q.item() + elif Q.shape != (sys.nstates, sys.nstates): + raise ValueError("Q matrix is the wrong shape") + + if R is not None: + R = np.atleast_2d(R) + if R.size == 1: # allow scalar weights + R = np.eye(sys.ninputs) * R.item() + elif R.shape != (sys.ninputs, sys.ninputs): + raise ValueError("R matrix is the wrong shape") + + if Q is None: + return lambda x, u: ((u-u0) @ R @ (u-u0)).item() + + if R is None: + return lambda x, u: ((x-x0) @ Q @ (x-x0)).item() + + # Received both Q and R matrices + return lambda x, u: ((x-x0) @ Q @ (x-x0) + (u-u0) @ R @ (u-u0)).item() + + +# +# Functions to create constraints: either polytopes (A x <= b) or ranges +# (lb # <= x <= ub). +# +# As in the cost function evaluation, the main "trick" in creating a constrain +# on the state or input is to properly evaluate the constraint on the stacked +# state and input vector at the current time point. The constraint itself +# will be called at each poing along the trajectory (or the endpoint) via the +# constrain_function() method. +# +# Note that these functions to not actually evaluate the constraint, they +# simply return the information required to do so. We use the SciPy +# optimization methods LinearConstraint and NonlinearConstraint as "types" to +# keep things consistent with the terminology in scipy.optimize. +# +def state_poly_constraint(sys, A, b): + """Create state constraint from polytope + + Creates a linear constraint on the system state of the form A x <= b that + can be used as an optimal control constraint (trajectory or terminal). + + Parameters + ---------- + sys : InputOutputSystem + I/O system for which the constraint is being defined. + A : 2D array + Constraint matrix + b : 1D array + Upper bound for the constraint + + Returns + ------- + constraint : tuple + A tuple consisting of the constraint type and parameter values. + + """ + # Convert arguments to arrays and make sure dimensions are right + A = np.atleast_2d(A) + b = np.atleast_1d(b) + if len(A.shape) != 2 or A.shape[1] != sys.nstates: + raise ValueError("polytope matrix must match number of states") + elif len(b.shape) != 1 or A.shape[0] != b.shape[0]: + raise ValueError("number of bounds must match number of constraints") + + # Return a linear constraint object based on the polynomial + return (opt.LinearConstraint, + np.hstack([A, np.zeros((A.shape[0], sys.ninputs))]), + np.full(A.shape[0], -np.inf), b) + + +def state_range_constraint(sys, lb, ub): + """Create state constraint from polytope + + Creates a linear constraint on the system state that bounds the range of + the individual states to be between `lb` and `ub`. The upper and lower + bounds can be set of `inf` and `-inf` to indicate there is no constraint + or to the same value to describe an equality constraint. + + Parameters + ---------- + sys : InputOutputSystem + I/O system for which the constraint is being defined. + lb : 1D array + Lower bound for each of the states. + ub : 1D array + Upper bound for each of the states. + + Returns + ------- + constraint : tuple + A tuple consisting of the constraint type and parameter values. + + """ + # Convert bounds to lists and make sure they are the right dimension + lb = np.atleast_1d(lb) + ub = np.atleast_1d(ub) + if lb.shape != (sys.nstates,) or ub.shape != (sys.nstates,): + raise ValueError("state bounds must match number of states") + + # Return a linear constraint object based on the polynomial + return (opt.LinearConstraint, + np.hstack( + [np.eye(sys.nstates), np.zeros((sys.nstates, sys.ninputs))]), + np.array(lb), np.array(ub)) + + +# Create a constraint polytope on the system input +def input_poly_constraint(sys, A, b): + """Create input constraint from polytope + + Creates a linear constraint on the system input of the form A u <= b that + can be used as an optimal control constraint (trajectory or terminal). + + Parameters + ---------- + sys : InputOutputSystem + I/O system for which the constraint is being defined. + A : 2D array + Constraint matrix + b : 1D array + Upper bound for the constraint + + Returns + ------- + constraint : tuple + A tuple consisting of the constraint type and parameter values. + + """ + # Convert arguments to arrays and make sure dimensions are right + A = np.atleast_2d(A) + b = np.atleast_1d(b) + if len(A.shape) != 2 or A.shape[1] != sys.ninputs: + raise ValueError("polytope matrix must match number of inputs") + elif len(b.shape) != 1 or A.shape[0] != b.shape[0]: + raise ValueError("number of bounds must match number of constraints") + + # Return a linear constraint object based on the polynomial + return (opt.LinearConstraint, + np.hstack( + [np.zeros((A.shape[0], sys.nstates)), A]), + np.full(A.shape[0], -np.inf), b) + + +def input_range_constraint(sys, lb, ub): + """Create input constraint from polytope + + Creates a linear constraint on the system input that bounds the range of + the individual states to be between `lb` and `ub`. The upper and lower + bounds can be set of `inf` and `-inf` to indicate there is no constraint + or to the same value to describe an equality constraint. + + Parameters + ---------- + sys : InputOutputSystem + I/O system for which the constraint is being defined. + lb : 1D array + Lower bound for each of the inputs. + ub : 1D array + Upper bound for each of the inputs. + + Returns + ------- + constraint : tuple + A tuple consisting of the constraint type and parameter values. + + """ + # Convert bounds to lists and make sure they are the right dimension + lb = np.atleast_1d(lb) + ub = np.atleast_1d(ub) + if lb.shape != (sys.ninputs,) or ub.shape != (sys.ninputs,): + raise ValueError("input bounds must match number of inputs") + + # Return a linear constraint object based on the polynomial + return (opt.LinearConstraint, + np.hstack( + [np.zeros((sys.ninputs, sys.nstates)), np.eye(sys.ninputs)]), + lb, ub) + + +# +# Create a constraint polytope/range constraint on the system output +# +# Unlike the state and input constraints, for the output constraint we need to +# do a function evaluation before applying the constraints. +# +# TODO: for the special case of an LTI system, we can avoid the extra function +# call by multiplying the state by the C matrix for the system and then +# imposing a linear constraint: +# +# np.hstack( +# [A @ sys.C, np.zeros((A.shape[0], sys.ninputs))]) +# +def output_poly_constraint(sys, A, b): + """Create output constraint from polytope + + Creates a linear constraint on the system ouput of the form A y <= b that + can be used as an optimal control constraint (trajectory or terminal). + + Parameters + ---------- + sys : InputOutputSystem + I/O system for which the constraint is being defined. + A : 2D array + Constraint matrix + b : 1D array + Upper bound for the constraint + + Returns + ------- + constraint : tuple + A tuple consisting of the constraint type and parameter values. + + """ + # Convert arguments to arrays and make sure dimensions are right + A = np.atleast_2d(A) + b = np.atleast_1d(b) + if len(A.shape) != 2 or A.shape[1] != sys.noutputs: + raise ValueError("polytope matrix must match number of outputs") + elif len(b.shape) != 1 or A.shape[0] != b.shape[0]: + raise ValueError("number of bounds must match number of constraints") + + # Function to create the output + def _evaluate_output_poly_constraint(x, u): + return A @ sys._out(0, x, u) + + # Return a nonlinear constraint object based on the polynomial + return (opt.NonlinearConstraint, + _evaluate_output_poly_constraint, + np.full(A.shape[0], -np.inf), b) + + +def output_range_constraint(sys, lb, ub): + """Create output constraint from range + + Creates a linear constraint on the system output that bounds the range of + the individual states to be between `lb` and `ub`. The upper and lower + bounds can be set of `inf` and `-inf` to indicate there is no constraint + or to the same value to describe an equality constraint. + + Parameters + ---------- + sys : InputOutputSystem + I/O system for which the constraint is being defined. + lb : 1D array + Lower bound for each of the outputs. + ub : 1D array + Upper bound for each of the outputs. + + Returns + ------- + constraint : tuple + A tuple consisting of the constraint type and parameter values. + + """ + # Convert bounds to lists and make sure they are the right dimension + lb = np.atleast_1d(lb) + ub = np.atleast_1d(ub) + if lb.shape != (sys.noutputs,) or ub.shape != (sys.noutputs,): + raise ValueError("output bounds must match number of outputs") + + # Function to create the output + def _evaluate_output_range_constraint(x, u): + # Separate the constraint into states and inputs + return sys._out(0, x, u) + + # Return a nonlinear constraint object based on the polynomial + return (opt.NonlinearConstraint, _evaluate_output_range_constraint, lb, ub) diff --git a/control/tests/config_test.py b/control/tests/config_test.py index b36b6b313..c8e4c6cd5 100644 --- a/control/tests/config_test.py +++ b/control/tests/config_test.py @@ -251,3 +251,14 @@ def test_change_default_dt_static(self): assert ct.tf(1, 1).dt is None assert ct.ss(0, 0, 0, 1).dt is None # TODO: add in test for static gain iosys + + def test_get_param_last(self): + """Test _get_param last keyword""" + kwargs = {'first': 1, 'second': 2} + + with pytest.raises(TypeError, match="unrecognized keyword.*second"): + assert ct.config._get_param( + 'config', 'first', kwargs, pop=True, last=True) == 1 + + assert ct.config._get_param( + 'config', 'second', kwargs, pop=True, last=True) == 2 diff --git a/control/tests/optimal_test.py b/control/tests/optimal_test.py new file mode 100644 index 000000000..d4b3fd6ef --- /dev/null +++ b/control/tests/optimal_test.py @@ -0,0 +1,469 @@ +"""optimal_test.py - tests for optimization based control + +RMM, 17 Apr 2019 check the functionality for optimization based control. +RMM, 30 Dec 2020 convert to pytest +""" + +import pytest +import warnings +import numpy as np +import scipy as sp +import math +import control as ct +import control.optimal as opt +import control.flatsys as flat +from control.tests.conftest import slycotonly +from numpy.lib import NumpyVersion + + +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) + + # Convert controller to an explicit form (not implemented yet) + # mpc_explicit = opt.explicit_mpc(); + + # Test explicit controller + # u_explicit = mpc_explicit(x0) + # np.testing.assert_array_almost_equal(u_openloop, u_explicit) + + +# +# Compare to LQR solution +# +# The next unit test is intended to confirm that a finite horizon +# optimal control problem with terminal cost set to LQR "cost to go" +# gives the same answer as LQR. Unfortunately, it requires a discrete +# time LQR function which is not yet availbale => for now this just +# tests the interface a bit. +# +@slycotonly +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 test_mpc_iosystem(): + # 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], + [ 0, 0.94, 0, 0.29, 0], + [ 0, 0.14, 0.81, -0.9, 0], + [ 0, -0.2, 0, 0.95, 0], + [ 0, 0.09, 0, 0, 0.9]] + B = [[ 0.01, -0.02], + [-0.14, 0], + [ 0.05, -0.2], + [ 0.02, 0], + [-0.01, 0]] + C = [[0, 1, 0, 0, -1], + [0, 0, 1, 0, 0], + [0, 0, 0, 1, 0], + [1, 0, 0, 0, 0]] + model = ct.ss2io(ct.ss(A, B, C, 0, 0.2)) + + # For the simulation we need the full state output + sys = ct.ss2io(ct.ss(A, B, np.eye(5), 0, 0.2)) + + # compute the steady state values for a particular value of the input + ud = np.array([0.8, -0.3]) + xd = np.linalg.inv(np.eye(5) - A) @ B @ ud + yd = C @ xd + + # provide constraints on the system signals + constraints = [opt.input_range_constraint(sys, [-5, -6], [5, 6])] + + # provide penalties on the system signals + Q = model.C.transpose() @ np.diag([10, 10, 10, 10]) @ model.C + R = np.diag([3, 2]) + cost = opt.quadratic_cost(model, Q, R, x0=xd, u0=ud) + + # online MPC controller object is constructed with a horizon 6 + ctrl = opt.create_mpc_iosystem( + model, np.arange(0, 6) * 0.2, cost, constraints) + + # Define an I/O system implementing model predictive control + loop = ct.feedback(sys, ctrl, 1) + + # Choose a nearby initial condition to speed up computation + X0 = np.hstack([xd, np.kron(ud, np.ones(6))]) * 0.99 + + Nsim = 12 + tout, xout = ct.input_output_response( + loop, np.arange(0, Nsim) * 0.2, 0, X0) + + # 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) + + +# 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", [ + [(sp.optimize.LinearConstraint, np.eye(3), [-5, -5, -1], [5, 5, 1],)], + [(opt.state_range_constraint, [-5, -5], [5, 5]), + (opt.input_range_constraint, [-1], [1])], + [(opt.state_range_constraint, [-5, -5], [5, 5]), + (opt.input_poly_constraint, np.array([[1], [-1]]), [1, 1])], + [(opt.state_poly_constraint, + np.array([[1, 0], [0, 1], [-1, 0], [0, -1]]), [5, 5, 5, 5]), + (opt.input_poly_constraint, np.array([[1], [-1]]), [1, 1])], + [(opt.output_range_constraint, [-5, -5], [5, 5]), + (opt.input_poly_constraint, np.array([[1], [-1]]), [1, 1])], + [(opt.output_poly_constraint, + np.array([[1, 0], [0, 1], [-1, 0], [0, -1]]), [5, 5, 5, 5]), + (opt.input_poly_constraint, np.array([[1], [-1]]), [1, 1])], + [(sp.optimize.NonlinearConstraint, + lambda x, u: np.array([x[0], x[1], u[0]]), [-5, -5, -1], [5, 5, 1])], +]) +def test_constraint_specification(constraint_list): + sys = ct.ss2io(ct.ss([[1, 1], [0, 1]], [[1], [0.5]], np.eye(2), 0, 1)) + + """Test out different forms of constraints on a simple problem""" + # Parse out the constraint + constraints = [] + for constraint_setup in constraint_list: + if constraint_setup[0] in \ + (sp.optimize.LinearConstraint, sp.optimize.NonlinearConstraint): + # No processing required + constraints.append(constraint_setup) + else: + # Call the function in the first argument to set up the constraint + constraints.append(constraint_setup[0](sys, *constraint_setup[1:])) + + # Quadratic state and input penalty + Q = [[1, 0], [0, 1]] + R = [[1]] + cost = opt.quadratic_cost(sys, Q, R) + + # Create a model predictive controller system + time = np.arange(0, 5, 1) + optctrl = opt.OptimalControlProblem(sys, time, cost, constraints) + + # Compute optimal control and compare against MPT3 solution + x0 = [4, 0] + res = optctrl.compute_trajectory(x0, 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=3) + + +@pytest.mark.parametrize("sys_args", [ + pytest.param( + ([[1, 0], [0, 1]], np.eye(2), np.eye(2), 0, True), + id = "discrete, no timebase"), + pytest.param( + ([[1, 0], [0, 1]], np.eye(2), np.eye(2), 0, 1), + id = "discrete, dt=1"), + pytest.param( + (np.zeros((2,2)), np.eye(2), np.eye(2), 0), + id = "continuous"), +]) +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_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 + + +@pytest.mark.parametrize("fun, args, exception, match", [ + [opt.quadratic_cost, (np.zeros((2, 3)), np.eye(2)), ValueError, + "Q matrix is the wrong shape"], + [opt.quadratic_cost, (np.eye(2), 1), ValueError, + "R matrix is the wrong shape"], +]) +def test_constraint_constructor_errors(fun, args, exception, match): + """Test various error conditions for constraint constructors""" + sys = ct.ss2io(ct.rss(2, 2, 2)) + with pytest.raises(exception, match=match): + fun(sys, *args) + + +@pytest.mark.parametrize("fun, args, exception, match", [ + [opt.input_poly_constraint, (np.zeros((2, 3)), [0, 0]), ValueError, + "polytope matrix must match number of inputs"], + [opt.output_poly_constraint, (np.zeros((2, 3)), [0, 0]), ValueError, + "polytope matrix must match number of outputs"], + [opt.state_poly_constraint, (np.zeros((2, 3)), [0, 0]), ValueError, + "polytope matrix must match number of states"], + [opt.input_poly_constraint, (np.zeros((2, 2)), [0, 0, 0]), ValueError, + "number of bounds must match number of constraints"], + [opt.output_poly_constraint, (np.zeros((2, 2)), [0, 0, 0]), ValueError, + "number of bounds must match number of constraints"], + [opt.state_poly_constraint, (np.zeros((2, 2)), [0, 0, 0]), ValueError, + "number of bounds must match number of constraints"], + [opt.input_poly_constraint, (np.zeros((2, 2)), [[0, 0, 0]]), ValueError, + "number of bounds must match number of constraints"], + [opt.output_poly_constraint, (np.zeros((2, 2)), [[0, 0, 0]]), ValueError, + "number of bounds must match number of constraints"], + [opt.state_poly_constraint, (np.zeros((2, 2)), 0), ValueError, + "number of bounds must match number of constraints"], + [opt.input_range_constraint, ([1, 2, 3], [0, 0]), ValueError, + "input bounds must match"], + [opt.output_range_constraint, ([2, 3], [0, 0, 0]), ValueError, + "output bounds must match"], + [opt.state_range_constraint, ([1, 2, 3], [0, 0, 0]), ValueError, + "state bounds must match"], +]) +def test_constraint_constructor_errors(fun, args, exception, match): + """Test various error conditions for constraint constructors""" + sys = ct.ss2io(ct.rss(2, 2, 2)) + with pytest.raises(exception, match=match): + fun(sys, *args) + + +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_almost_equal(res2.inputs, res1.inputs, decimal=3) + + # 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) diff --git a/doc/classes.rst b/doc/classes.rst index b948f23aa..fdf39a457 100644 --- a/doc/classes.rst +++ b/doc/classes.rst @@ -30,3 +30,14 @@ that allow for linear, nonlinear, and interconnected elements: LinearICSystem LinearIOSystem NonlinearIOSystem + +Additional classes +================== +.. autosummary:: + + flatsys.BasisFamily + flatsys.FlatSystem + flatsys.LinearFlatSystem + flatsys.PolyFamily + flatsys.SystemTrajectory + optimal.OptimalControlProblem diff --git a/doc/examples.rst b/doc/examples.rst index e56d46e70..91476bc9d 100644 --- a/doc/examples.rst +++ b/doc/examples.rst @@ -43,5 +43,6 @@ using running examples in FBS2e. cruise describing_functions + mpc_aircraft steering pvtol-lqr-nested diff --git a/doc/index.rst b/doc/index.rst index 3558b0b30..98b184286 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -30,6 +30,7 @@ implements basic operations for analysis and design of feedback control systems. flatsys iosys descfcn + optimal examples * :ref:`genindex` diff --git a/doc/mpc-overview.png b/doc/mpc-overview.png new file mode 100644 index 000000000..a51b9418a Binary files /dev/null and b/doc/mpc-overview.png differ diff --git a/doc/mpc_aircraft.ipynb b/doc/mpc_aircraft.ipynb new file mode 120000 index 000000000..0a3e4df42 --- /dev/null +++ b/doc/mpc_aircraft.ipynb @@ -0,0 +1 @@ +../examples/mpc_aircraft.ipynb \ No newline at end of file diff --git a/doc/optimal.rst b/doc/optimal.rst new file mode 100644 index 000000000..38bfca0db --- /dev/null +++ b/doc/optimal.rst @@ -0,0 +1,288 @@ +.. _optimal-module: + +*************** +Optimal control +*************** + +.. automodule:: control.optimal + :no-members: + :no-inherited-members: + +Problem setup +============= + +Consider the *optimal control problem*: + +.. math:: + + \min_{u(\cdot)} + \int_0^T L(x,u)\, dt + V \bigl( x(T) \bigr) + +subject to the constraint + +.. math:: + + \dot x = f(x, u), \qquad x\in\mathbb{R}^n,\, u\in\mathbb{R}^m. + +Abstractly, this is a constrained optimization problem where we seek a +*feasible trajectory* :math:`(x(t), u(t))` that minimizes the cost function + +.. math:: + + J(x, u) = \int_0^T L(x, u)\, dt + V \bigl( x(T) \bigr). + +More formally, this problem is equivalent to the "standard" problem of +minimizing a cost function :math:`J(x, u)` where :math:`(x, u) \in L_2[0,T]` +(the set of square integrable functions) and :math:`h(z) = \dot x(t) - +f(x(t), u(t)) = 0` models the dynamics. The term :math:`L(x, u)` is +referred to as the integral (or trajectory) cost and :math:`V(x(T))` is the +final (or terminal) cost. + +It is often convenient to ask that the final value of the trajectory, +denoted :math:`x_\text{f}`, be specified. We can do this by requiring that +:math:`x(T) = x_\text{f}` or by using a more general form of constraint: + +.. math:: + + \psi_i(x(T)) = 0, \qquad i = 1, \dots, q. + +The fully constrained case is obtained by setting :math:`q = n` and defining +:math:`\psi_i(x(T)) = x_i(T) - x_{i,\text{f}}`. For a control problem with +a full set of terminal constraints, :math:`V(x(T))` can be omitted (since +its value is fixed). + +Finally, we may wish to consider optimizations in which either the state or +the inputs are constrained by a set of nonlinear functions of the form + +.. math:: + + \text{lb}_i \leq g_i(x, u) \leq \text{ub}_i, \qquad i = 1, \dots, k. + +where :math:`\text{lb}_i` and :math:`\text{ub}_i` represent lower and upper +bounds on the constraint function :math:`g_i`. Note that these constraints +can be on the input, the state, or combinations of input and state, +depending on the form of :math:`g_i`. Furthermore, these constraints are +intended to hold at all instants in time along the trajectory. + +A common use of optimization-based control techniques is the implementation +of model predictive control (also called receding horizon control). In +model predict control, a finite horizon optimal control problem is solved, +generating open-loop state and control trajectories. The resulting control +trajectory is applied to the system for a fraction of the horizon +length. This process is then repeated, resulting in a sampled data feedback +law. This approach is illustrated in the following figure: + +.. image:: mpc-overview.png + +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 +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. + +In reality, the system will not follow the predicted path exactly, so that +the red (computed) and blue (actual) trajectories will diverge. We thus +recompute the optimal path from the new state at time :math:`t + \Delta T`, +extending our horizon by an additional :math:`\Delta T` units of time. This +approach can be shown to generate stabilizing control laws under suitable +conditions (see, for example, the FBS2e supplement on `Optimization-Based +Control `_. + +Module usage +============ + +The optimal control module provides a means of computing optimal +trajectories for nonlinear systems and implementing optimization-based +controllers, including model predictive control. It follows the basic +problem setup described above, but carries out all computations in *discrete +time* (so that integrals become sums) and over a *finite horizon*. + +To describe an optimal control problem we need an input/output system, a +time horizon, a cost function, and (optionally) a set of constraints on the +state and/or input, either along the trajectory and at the terminal time. +The optimal control module operates by converting the optimal control +problem into a standard optimization problem that can be solved by +:func:`scipy.optimize.minimize`. The optimal control problem can be solved +by using the :func:`~control.obc.solve_ocp` function:: + + res = obc.solve_ocp(sys, horizon, X0, cost, constraints) + +The `sys` parameter should be an :class:`~control.InputOutputSystem` and the +`horizon` parameter should represent a time vector that gives the list of +times at which the cost and constraints should be evaluated. + +The `cost` function has call signature `cost(t, x, u)` and should return the +(incremental) cost at the given time, state, and input. It will be +evaluated at each point in the `horizon` vector. The `terminal_cost` +parameter can be used to specify a cost function for the final point in the +trajectory. + +The `constraints` parameter is a list of constraints similar to that used by +the :func:`scipy.optimize.minimize` function. Each constraint is a tuple of +one of the following forms:: + + (LinearConstraint, A, lb, ub) + (NonlinearConstraint, f, lb, ub) + +For a linear constraint, the 2D array `A` is multiplied by a vector +consisting of the current state `x` and current input `u` stacked +vertically, then compared with the upper and lower bound. This constrain is +satisfied if + +.. code:: python + + lb <= A @ np.hstack([x, u]) <= ub + +A nonlinear constraint is satisfied if + +.. code:: python + + lb <= f(x, u) <= ub + +By default, `constraints` are taken to be trajectory constraints holding at +all points on the trajectory. The `terminal_constraint` parameter can be +used to specify a constraint that only holds at the final point of the +trajectory. + +The return value for :func:`~control.optimal.solve_ocp` is a bundle object +that has the following elements: + + * `res.success`: `True` if the optimization was successfully solved + * `res.inputs`: optimal input + * `res.states`: state trajectory (if `return_x` was `True`) + * `res.time`: copy of the time horizon vector + +In addition, the results from :func:`scipy.optimize.minimize` are also +available. + +To simplify the specification of cost functions and constraints, the +:mod:`~control.ios` module defines a number of utility functions: + +.. autosummary:: + + ~control.optimal.quadratic_cost + ~control.optimal.input_poly_constraint + ~control.optimal.input_range_constraint + ~control.optimal.output_poly_constraint + ~control.optimal.output_range_constraint + ~control.optimal.state_poly_constraint + ~control.optimal.state_range_constraint + +Example +======= + +Consider the vehicle steering example described in FBS2e. The dynamics of +the system can be defined as a nonlinear input/output system using the +following code:: + + import numpy as np + import control as ct + import control.optimal as opt + import matplotlib.pyplot as plt + + 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')) + +We consider an optimal control problem that consists of "changing lanes" by +moving from the point x = 0 m, y = -2 m, :math:`\theta` = 0 to the point x = +100 m, y = 2 m, :math:`\theta` = 0) over a period of 10 seconds and with a +with a starting and ending velocity of 10 m/s:: + + x0 = [0., -2., 0.]; u0 = [10., 0.] + xf = [100., 2., 0.]; uf = [10., 0.] + Tf = 10 + +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) + +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:: + + constraints = [ opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ] + +Finally, we solve for the optimal inputs:: + + horizon = np.linspace(0, Tf, 20, endpoint=True) + bend_left = [10, 0.01] # slight left veer + + 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) + +Plotting the results:: + + # Plot the results + plt.subplot(3, 1, 1) + plt.plot(y[0], y[1]) + plt.plot(x0[0], x0[1], 'ro', xf[0], xf[1], 'ro') + plt.xlabel("x [m]") + plt.ylabel("y [m]") + + 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.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.xlabel("t [sec]") + plt.ylabel("u2 [rad/s]") + + plt.suptitle("Lane change manuever") + plt.tight_layout() + plt.show() + +yields + +.. image:: steering-optimal.png + + +Module classes and functions +============================ +.. autosummary:: + :toctree: generated/ + + ~control.optimal.OptimalControlProblem + ~control.optimal.solve_ocp + ~control.optimal.create_mpc_iosystem + ~control.optimal.input_poly_constraint + ~control.optimal.input_range_constraint + ~control.optimal.output_poly_constraint + ~control.optimal.output_range_constraint + ~control.optimal.state_poly_constraint + ~control.optimal.state_range_constraint diff --git a/doc/steering-optimal.png b/doc/steering-optimal.png new file mode 100644 index 000000000..6ff50c0f4 Binary files /dev/null and b/doc/steering-optimal.png differ diff --git a/examples/mpc_aircraft.ipynb b/examples/mpc_aircraft.ipynb new file mode 100644 index 000000000..5da812eb0 --- /dev/null +++ b/examples/mpc_aircraft.ipynb @@ -0,0 +1,201 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Model Predictive Control: Aircraft Model\n", + "\n", + "RMM, 13 Feb 2021\n", + "\n", + "This example replicates the [MPT3 regulation problem example](https://www.mpt3.org/UI/RegulationProblem)." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "import control as ct\n", + "import numpy as np\n", + "import control.optimal as opt\n", + "import matplotlib.pyplot as plt" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "# model of an aircraft discretized with 0.2s sampling time\n", + "# Source: https://www.mpt3.org/UI/RegulationProblem\n", + "A = [[0.99, 0.01, 0.18, -0.09, 0],\n", + " [ 0, 0.94, 0, 0.29, 0],\n", + " [ 0, 0.14, 0.81, -0.9, 0],\n", + " [ 0, -0.2, 0, 0.95, 0],\n", + " [ 0, 0.09, 0, 0, 0.9]]\n", + "B = [[ 0.01, -0.02],\n", + " [-0.14, 0],\n", + " [ 0.05, -0.2],\n", + " [ 0.02, 0],\n", + " [-0.01, 0]]\n", + "C = [[0, 1, 0, 0, -1],\n", + " [0, 0, 1, 0, 0],\n", + " [0, 0, 0, 1, 0],\n", + " [1, 0, 0, 0, 0]]\n", + "model = ct.ss2io(ct.ss(A, B, C, 0, 0.2))\n", + "\n", + "# For the simulation we need the full state output\n", + "sys = ct.ss2io(ct.ss(A, B, np.eye(5), 0, 0.2))\n", + "\n", + "# compute the steady state values for a particular value of the input\n", + "ud = np.array([0.8, -0.3])\n", + "xd = np.linalg.inv(np.eye(5) - A) @ B @ ud\n", + "yd = C @ xd" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [], + "source": [ + "# computed values will be used as references for the desired\n", + "# steady state which can be added using \"reference\" filter\n", + "# model.u.with('reference');\n", + "# model.u.reference = us;\n", + "# model.y.with('reference');\n", + "# model.y.reference = ys;\n", + "\n", + "# provide constraints on the system signals\n", + "constraints = [opt.input_range_constraint(sys, [-5, -6], [5, 6])]\n", + "\n", + "# provide penalties on the system signals\n", + "Q = model.C.transpose() @ np.diag([10, 10, 10, 10]) @ model.C\n", + "R = np.diag([3, 2])\n", + "cost = opt.quadratic_cost(model, Q, R, x0=xd, u0=ud)\n", + "\n", + "# online MPC controller object is constructed with a horizon 6\n", + "ctrl = opt.create_mpc_iosystem(model, np.arange(0, 6) * 0.2, cost, constraints)" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "System: sys[7]\n", + "Inputs (2): u[0], u[1], \n", + "Outputs (5): y[0], y[1], y[2], y[3], y[4], \n", + "States (17): sys[1]_x[0], sys[1]_x[1], sys[1]_x[2], sys[1]_x[3], sys[1]_x[4], sys[6]_x[0], sys[6]_x[1], sys[6]_x[2], sys[6]_x[3], sys[6]_x[4], sys[6]_x[5], sys[6]_x[6], sys[6]_x[7], sys[6]_x[8], sys[6]_x[9], sys[6]_x[10], sys[6]_x[11], \n" + ] + } + ], + "source": [ + "# Define an I/O system implementing model predictive control\n", + "loop = ct.feedback(sys, ctrl, 1)\n", + "print(loop)" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Computation time = 8.28132 seconds\n" + ] + } + ], + "source": [ + "import time\n", + "\n", + "# loop = ClosedLoop(ctrl, model);\n", + "# x0 = [0, 0, 0, 0, 0]\n", + "Nsim = 60\n", + "\n", + "start = time.time()\n", + "tout, xout = ct.input_output_response(loop, np.arange(0, Nsim) * 0.2, 0, 0)\n", + "end = time.time()\n", + "print(\"Computation time = %g seconds\" % (end-start))" + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "array([-0.15441833, 0.00362039, 0.07760278, 0.00675162, 0.00698118])" + ] + }, + "execution_count": 10, + "metadata": {}, + "output_type": "execute_result" + }, + { + "data": { + "image/png": "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\n", + "text/plain": [ + "
" + ] + }, + "metadata": { + "needs_background": "light" + }, + "output_type": "display_data" + } + ], + "source": [ + "# Plot the results\n", + "# plt.subplot(2, 1, 1)\n", + "for i, y in enumerate(C @ xout):\n", + " plt.plot(tout, y)\n", + " plt.plot(tout, yd[i] * np.ones(tout.shape), 'k--')\n", + "plt.title('outputs')\n", + "\n", + "# plt.subplot(2, 1, 2)\n", + "# plt.plot(t, u);\n", + "# plot(np.range(Nsim), us*ones(1, Nsim), 'k--')\n", + "# plt.title('inputs')\n", + "\n", + "plt.tight_layout()\n", + "\n", + "# Print the final error\n", + "xd - xout[:,-1]" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.5" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/examples/run_examples.sh b/examples/run_examples.sh index 6f04fe12c..48d481aef 100755 --- a/examples/run_examples.sh +++ b/examples/run_examples.sh @@ -18,6 +18,10 @@ for example in *.py; do fi done +# Get rid of the output files +rm *.log + +# List any files that generated errors if [ -n "${example_errors}" ]; then echo These examples had errors: echo "${example_errors}" diff --git a/examples/steering-optimal.py b/examples/steering-optimal.py new file mode 100644 index 000000000..5661e0f38 --- /dev/null +++ b/examples/steering-optimal.py @@ -0,0 +1,256 @@ +# steering-optimal.py - optimal control for vehicle steering +# RMM, 18 Feb 2021 +# +# This file works through an optimal control example for the vehicle +# steering system. It is intended to demonstrate the functionality for +# optimal control module (control.optimal) in the python-control package. + +import numpy as np +import math +import control as ct +import control.optimal as opt +import matplotlib.pyplot as plt +import logging +import time +import os + +# +# Vehicle steering dynamics +# +# The vehicle dynamics are given by a simple bicycle model. We take the state +# of the system as (x, y, theta) where (x, y) is the position of the vehicle +# in the plane and theta is the angle of the vehicle with respect to +# horizontal. The vehicle input is given by (v, phi) where v is the forward +# velocity of the vehicle and phi is the angle of the steering wheel. The +# model includes saturation of the vehicle steering angle. +# +# System state: x, y, theta +# System input: v, phi +# System output: x, y +# System parameters: wheelbase, maxsteer +# +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 (use min/max instead of clip for speed) + phi = max(-phimax, min(u[1], phimax)) + + # 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) + ]) + + +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')) + +# +# Utility function to plot the results +# +def plot_results(t, y, u, figure=None, yf=None): + plt.figure(figure) + + # Plot the xy trajectory + plt.subplot(3, 1, 1) + plt.plot(y[0], y[1]) + plt.xlabel("x [m]") + plt.ylabel("y [m]") + if yf: + plt.plot(yf[0], yf[1], 'ro') + + # Plot the inputs as a function of time + plt.subplot(3, 1, 2) + plt.plot(t, u[0]) + plt.xlabel("t [sec]") + plt.ylabel("velocity [m/s]") + + plt.subplot(3, 1, 3) + plt.plot(t, u[1]) + plt.xlabel("t [sec]") + plt.ylabel("steering [rad/s]") + + plt.suptitle("Lane change manuever") + plt.tight_layout() + plt.show(block=False) + +# +# Optimal control problem +# +# Perform a "lane change" manuever over the course of 10 seconds. +# + +# Initial and final conditions +x0 = [0., -2., 0.]; u0 = [10., 0.] +xf = [100., 2., 0.]; uf = [10., 0.] +Tf = 10 + +# +# Approach 1: standard quadratic cost +# +# We can set up the optimal control problem as trying to minimize the +# distance form the desired final point while at the same time as not +# exerting too much control effort to achieve our goal. +# +print("Approach 1: standard quadratic 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) + +# Define the time horizon (and spacing) for the optimization +horizon = np.linspace(0, Tf, 10, endpoint=True) + +# 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]) + +# +# Approach 2: input cost, input constraints, terminal cost +# +# The previous solution integrates the position error for the entire +# horizon, and so the car changes lanes very quickly (at the cost of larger +# inputs). Instead, we can penalize the final state and impose a higher +# cost on the inputs, resuling in a more graduate lane change. +# +# We also set the solver explicitly. +# +print("Approach 2: input cost and constraints plus terminal cost") + +# Add input constraint, input cost, terminal cost +constraints = [ opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ] +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) + +# Change logging to keep less information +logging.basicConfig( + level=logging.INFO, filename="./steering-terminal_cost.log", + filemode='w', force=True) + +# Compute the optimal control +start_time = time.process_time() +result2 = opt.solve_ocp( + vehicle, horizon, x0, traj_cost, constraints, terminal_cost=term_cost, + initial_guess=bend_left, log=True, + minimize_method='SLSQP', minimize_options={'eps': 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 result2.success + +# Extract and plot the results (+ state trajectory) +t2, u2 = result2.time, result2.inputs +t2, y2 = ct.input_output_response(vehicle, horizon, u2, x0) +plot_results(t2, y2, u2, figure=2, yf=xf[0:2]) + +# +# Approach 3: terminal constraints +# +# We can also remove the cost function on the state and replace it +# with a terminal *constraint* on the state. If a solution is found, +# it guarantees we get to exactly the final state. +# +print("Approach 3: terminal constraints") + +# Input cost and terminal constraints +R = np.diag([1, 1]) # minimize applied inputs +cost3 = 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) ] + +# Reset logging to its default values +logging.basicConfig( + level=logging.DEBUG, filename="./steering-terminal_constraint.log", + filemode='w', force=True) + +# Compute the optimal control +start_time = time.process_time() +result3 = opt.solve_ocp( + vehicle, horizon, x0, cost3, constraints, + terminal_constraints=terminal, initial_guess=bend_left, log=False, + solve_ivp_kwargs={'atol': 1e-3, 'rtol': 1e-2}, + minimize_method='trust-constr', +) +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 result3.success + +# Extract and plot the results (+ state trajectory) +t3, u3 = result3.time, result3.inputs +t3, y3 = ct.input_output_response(vehicle, horizon, u3, x0) +plot_results(t3, y3, u3, figure=3, yf=xf[0:2]) + +# +# Approach 4: terminal constraints w/ basis functions +# +# As a final example, we can use a basis function to reduce the size +# of the problem and get faster answers with more temporal resolution. +# Here we parameterize the input by a set of 4 Bezier curves but solve +# 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) +t4, u4 = result4.time, result4.inputs +t4, y4 = ct.input_output_response(vehicle, horizon, u4, x0) +plot_results(t4, y4, u4, figure=4, yf=xf[0:2]) + +# If we are not running CI tests, display the results +if 'PYCONTROL_TEST_EXAMPLES' not in os.environ: + plt.show()