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() pFad - Phonifier reborn

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

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


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy