diff --git a/.github/conda-env/build-env.yml b/.github/conda-env/build-env.yml new file mode 100644 index 000000000..f75973640 --- /dev/null +++ b/.github/conda-env/build-env.yml @@ -0,0 +1,4 @@ +name: build-env +dependencies: + - boa + - numpy !=1.23.0 diff --git a/.github/conda-env/test-env.yml b/.github/conda-env/test-env.yml new file mode 100644 index 000000000..1c28589a4 --- /dev/null +++ b/.github/conda-env/test-env.yml @@ -0,0 +1,13 @@ +name: test-env +dependencies: + - conda-build # for conda index + - pip + - coverage + - coveralls + - pytest + - pytest-cov + - pytest-timeout + - pytest-xvfb + - numpy + - matplotlib + - scipy diff --git a/.github/scripts/set-conda-test-matrix.py b/.github/scripts/set-conda-test-matrix.py new file mode 100644 index 000000000..954480cb0 --- /dev/null +++ b/.github/scripts/set-conda-test-matrix.py @@ -0,0 +1,34 @@ +""" set-conda-test-matrix.py + +Create test matrix for conda packages +""" +import json, re +from pathlib import Path + +osmap = {'linux': 'ubuntu', + 'osx': 'macos', + 'win': 'windows', + } + +blas_implementations = ['unset', 'Generic', 'OpenBLAS', 'Intel10_64lp'] + +combinations = {'ubuntu': blas_implementations, + 'macos': blas_implementations, + 'windows': ['unset', 'Intel10_64lp'], + } + +conda_jobs = [] +for conda_pkg_file in Path("slycot-conda-pkgs").glob("*/*.tar.bz2"): + cos = osmap[conda_pkg_file.parent.name.split("-")[0]] + m = re.search(r'py(\d)(\d+)_', conda_pkg_file.name) + pymajor, pyminor = int(m[1]), int(m[2]) + cpy = f'{pymajor}.{pyminor}' + for cbl in combinations[cos]: + cjob = {'packagekey': f'{cos}-{cpy}', + 'os': cos, + 'python': cpy, + 'blas_lib': cbl} + conda_jobs.append(cjob) + +matrix = { 'include': conda_jobs } +print(json.dumps(matrix)) diff --git a/.github/scripts/set-pip-test-matrix.py b/.github/scripts/set-pip-test-matrix.py new file mode 100644 index 000000000..ed18239d0 --- /dev/null +++ b/.github/scripts/set-pip-test-matrix.py @@ -0,0 +1,28 @@ +""" set-pip-test-matrix.py + +Create test matrix for pip wheels +""" +import json +from pathlib import Path + +system_opt_blas_libs = {'ubuntu': ['OpenBLAS'], + 'macos' : ['OpenBLAS', 'Apple']} + +wheel_jobs = [] +for wkey in Path("slycot-wheels").iterdir(): + wos, wpy, wbl = wkey.name.split("-") + wheel_jobs.append({'packagekey': wkey.name, + 'os': wos, + 'python': wpy, + 'blas_lib': wbl, + }) + if wbl == "Generic": + for bl in system_opt_blas_libs[wos]: + wheel_jobs.append({ 'packagekey': wkey.name, + 'os': wos, + 'python': wpy, + 'blas_lib': bl, + }) + +matrix = { 'include': wheel_jobs } +print(json.dumps(matrix)) \ No newline at end of file diff --git a/.github/workflows/control-slycot-src.yml b/.github/workflows/control-slycot-src.yml index 13a66e426..811a89216 100644 --- a/.github/workflows/control-slycot-src.yml +++ b/.github/workflows/control-slycot-src.yml @@ -7,35 +7,37 @@ jobs: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v2 + - name: Checkout python-control + uses: actions/checkout@v3 + with: + path: python-control - name: Set up Python - uses: actions/setup-python@v2 - - name: Install Python dependencies - run: | - # Set up conda - echo $CONDA/bin >> $GITHUB_PATH - - # Set up (virtual) X11 - sudo apt install -y xvfb - - # Install test tools - conda install pip pytest pytest-timeout - - # Install python-control dependencies - conda install numpy matplotlib scipy - + uses: actions/setup-python@v4 + with: + python-version: '3.11' + - name: Install Python dependencies and test tools + run: pip install -v './python-control[test]' + + - name: Checkout Slycot + uses: actions/checkout@v3 + with: + repository: python-control/Slycot + submodules: recursive + fetch-depth: 0 + path: slycot - name: Install slycot from source + env: + BLA_VENDOR: Generic + CMAKE_GENERATOR: Unix Makefiles + working-directory: slycot run: | # Install compilers, libraries, and development environment sudo apt-get -y install gfortran cmake --fix-missing sudo apt-get -y install libblas-dev liblapack-dev - conda install -c conda-forge scikit-build; # Compile and install slycot - git clone https://github.com/python-control/Slycot.git slycot - cd slycot - git submodule update --init - python setup.py build_ext install -DBLA_VENDOR=Generic + pip install -v . - name: Test with pytest - run: xvfb-run --auto-servernum pytest control/tests + working-directory: python-control + run: pytest -v control/tests diff --git a/.github/workflows/install_examples.yml b/.github/workflows/install_examples.yml index b36ff3e7f..d50f8fda6 100644 --- a/.github/workflows/install_examples.yml +++ b/.github/workflows/install_examples.yml @@ -1,32 +1,29 @@ -name: setup.py, examples +name: Setup, Examples, Notebooks on: [push, pull_request] jobs: - build-linux: + install-examples: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v2 - - name: Set up Python - uses: actions/setup-python@v2 - - name: Install Python dependencies + - uses: actions/checkout@v3 + - name: Install Python dependencies from conda-forge run: | - # Set up conda + # Set up conda using the preinstalled GHA Miniconda environment echo $CONDA/bin >> $GITHUB_PATH + conda config --add channels conda-forge + conda config --set channel_priority strict - # Set up (virtual) X11 - sudo apt install -y xvfb + # Install build tools + conda install pip setuptools setuptools-scm - # Install test tools - conda install pip pytest + # Install python-control dependencies and extras + conda install numpy matplotlib scipy + conda install slycot pmw jupyter - # Install python-control dependencies - conda install numpy matplotlib scipy jupyter - conda install -c conda-forge slycot pmw - - - name: Install with setup.py - run: python setup.py install + - name: Install from source + run: pip install . - name: Run examples run: | diff --git a/.github/workflows/os-blas-test-matrix.yml b/.github/workflows/os-blas-test-matrix.yml new file mode 100644 index 000000000..4470e2454 --- /dev/null +++ b/.github/workflows/os-blas-test-matrix.yml @@ -0,0 +1,325 @@ +name: OS/BLAS test matrix + +on: + workflow_dispatch: + pull_request: + paths: + - .github/workflows/os-blas-test-matrix.yml + - .github/scripts/set-conda-test-matrix.py + - .github/scripts/set-conda-pip-matrix.py + - .github/conda-env/build-env.yml + - .github/conda-env/test-env.yml + +jobs: + build-pip: + name: Build pip Py${{ matrix.python }}, ${{ matrix.os }}, ${{ matrix.bla_vendor}} BLA_VENDOR + runs-on: ${{ matrix.os }}-latest + strategy: + fail-fast: false + matrix: + os: + - 'ubuntu' + - 'macos' + python: + - '3.8' + - '3.11' + bla_vendor: [ 'unset' ] + include: + - os: 'ubuntu' + python: '3.11' + bla_vendor: 'Generic' + - os: 'ubuntu' + python: '3.11' + bla_vendor: 'OpenBLAS' + - os: 'macos' + python: '3.11' + bla_vendor: 'Apple' + - os: 'macos' + python: '3.11' + bla_vendor: 'Generic' + - os: 'macos' + python: '3.11' + bla_vendor: 'OpenBLAS' + + steps: + - name: Set up Python + uses: actions/setup-python@v4 + with: + python-version: ${{ matrix.python }} + - name: Checkout Slycot + uses: actions/checkout@v3 + with: + repository: python-control/Slycot + fetch-depth: 0 + submodules: 'recursive' + - name: Setup Ubuntu + if: matrix.os == 'ubuntu' + run: | + sudo apt-get -y update + sudo apt-get -y install gfortran cmake --fix-missing + case ${{ matrix.bla_vendor }} in + unset | Generic ) sudo apt-get -y install libblas-dev liblapack-dev ;; + OpenBLAS ) sudo apt-get -y install libopenblas-dev ;; + *) + echo "bla_vendor option ${{ matrix.bla_vendor }} not supported" + exit 1 ;; + esac + - name: Setup macOS + if: matrix.os == 'macos' + run: | + case ${{ matrix.bla_vendor }} in + unset | Generic | Apple ) ;; # Found in system + OpenBLAS ) + brew install openblas + echo "BLAS_ROOT=/usr/local/opt/openblas/" >> $GITHUB_ENV + echo "LAPACK_ROOT=/usr/local/opt/openblas/" >> $GITHUB_ENV + ;; + *) + echo "bla_vendor option ${{ matrix.bla_vendor }} not supported" + exit 1 ;; + esac + echo "FC=gfortran-11" >> $GITHUB_ENV + - name: Build wheel + env: + BLA_VENDOR: ${{ matrix.bla_vendor }} + CMAKE_GENERATOR: Unix Makefiles + run: | + if [[ $BLA_VENDOR = unset ]]; then unset BLA_VENDOR; fi + python -m pip install --upgrade pip + pip wheel -v -w . . + wheeldir=slycot-wheels/${{ matrix.os }}-${{ matrix.python }}-${{ matrix.bla_vendor }} + mkdir -p ${wheeldir} + cp ./slycot*.whl ${wheeldir}/ + - name: Save wheel + uses: actions/upload-artifact@v3 + with: + name: slycot-wheels + path: slycot-wheels + + + build-conda: + name: Build conda Py${{ matrix.python }}, ${{ matrix.os }} + runs-on: ${{ matrix.os }}-latest + strategy: + fail-fast: false + matrix: + os: + - 'ubuntu' + - 'macos' + - 'windows' + python: + - '3.9' + - '3.11' + + steps: + - name: Checkout Slycot + uses: actions/checkout@v3 + with: + repository: python-control/Slycot + fetch-depth: 0 + submodules: 'recursive' + - name: Setup Conda + uses: conda-incubator/setup-miniconda@v2 + with: + python-version: ${{ matrix.python }} + activate-environment: build-env + environment-file: .github/conda-env/build-env.yml + miniforge-version: latest + miniforge-variant: Mambaforge + channel-priority: strict + auto-update-conda: false + auto-activate-base: false + - name: Conda build + shell: bash -l {0} + run: | + set -e + numpyversion=$(python -c 'import numpy; print(numpy.version.version)') + conda mambabuild --python "${{ matrix.python }}" --numpy $numpyversion conda-recipe + # preserve directory structure for custom conda channel + find "${CONDA_PREFIX}/conda-bld" -maxdepth 2 -name 'slycot*.tar.bz2' | while read -r conda_pkg; do + conda_platform=$(basename $(dirname "${conda_pkg}")) + mkdir -p "slycot-conda-pkgs/${conda_platform}" + cp "${conda_pkg}" "slycot-conda-pkgs/${conda_platform}/" + done + - name: Save to local conda pkg channel + uses: actions/upload-artifact@v3 + with: + name: slycot-conda-pkgs + path: slycot-conda-pkgs + + + create-wheel-test-matrix: + name: Create wheel test matrix + runs-on: ubuntu-latest + needs: build-pip + if: always() # run tests for all successful builds, even if others failed + outputs: + matrix: ${{ steps.set-matrix.outputs.matrix }} + steps: + - name: Checkout python-control + uses: actions/checkout@v3 + - name: Download wheels (if any) + uses: actions/download-artifact@v3 + with: + name: slycot-wheels + path: slycot-wheels + - id: set-matrix + run: echo "matrix=$(python3 .github/scripts/set-pip-test-matrix.py)" >> $GITHUB_OUTPUT + + + create-conda-test-matrix: + name: Create conda test matrix + runs-on: ubuntu-latest + needs: build-conda + if: always() # run tests for all successful builds, even if others failed + outputs: + matrix: ${{ steps.set-matrix.outputs.matrix }} + steps: + - name: Checkout python-control + uses: actions/checkout@v3 + - name: Download conda packages + uses: actions/download-artifact@v3 + with: + name: slycot-conda-pkgs + path: slycot-conda-pkgs + - id: set-matrix + run: echo "matrix=$(python3 .github/scripts/set-conda-test-matrix.py)" >> $GITHUB_OUTPUT + + + test-wheel: + name: Test wheel ${{ matrix.packagekey }}, ${{matrix.blas_lib}} BLAS lib ${{ matrix.failok }} + needs: create-wheel-test-matrix + runs-on: ${{ matrix.os }}-latest + continue-on-error: ${{ matrix.failok == 'FAILOK' }} + + strategy: + fail-fast: false + matrix: ${{ fromJSON(needs.create-wheel-test-matrix.outputs.matrix) }} + + steps: + - name: Checkout Slycot + uses: actions/checkout@v3 + with: + repository: 'python-control/Slycot' + path: slycot-src + - name: Checkout python-control + uses: actions/checkout@v3 + with: + repository: 'python-control/python-control' + - name: Setup Python + uses: actions/setup-python@v4 + with: + python-version: ${{ matrix.python }} + - name: Setup Ubuntu + if: matrix.os == 'ubuntu' + run: | + set -xe + sudo apt-get -y update + case ${{ matrix.blas_lib }} in + Generic ) sudo apt-get -y install libblas3 liblapack3 ;; + unset | OpenBLAS ) sudo apt-get -y install libopenblas-base ;; + *) + echo "BLAS ${{ matrix.blas_lib }} not supported for wheels on Ubuntu" + exit 1 ;; + esac + update-alternatives --display libblas.so.3-x86_64-linux-gnu + update-alternatives --display liblapack.so.3-x86_64-linux-gnu + - name: Setup macOS + if: matrix.os == 'macos' + run: | + set -xe + brew install coreutils + case ${{ matrix.blas_lib }} in + unset | Generic | Apple ) ;; # system provided (Uses Apple Accelerate Framework) + OpenBLAS ) + brew install openblas + echo "DYLIB_LIBRARY_PATH=/usr/local/opt/openblas/lib" >> $GITHUB_ENV + ;; + *) + echo "BLAS option ${{ matrix.blas_lib }} not supported for wheels on MacOS" + exit 1 ;; + esac + - name: Download wheels + uses: actions/download-artifact@v3 + with: + name: slycot-wheels + path: slycot-wheels + - name: Install Wheel + run: | + python -m pip install --upgrade pip + pip install matplotlib scipy pytest pytest-cov pytest-timeout coverage coveralls + pip install slycot-wheels/${{ matrix.packagekey }}/slycot*.whl + pip show slycot + - name: Test with pytest + run: JOBNAME="$JOBNAME" pytest control/tests + env: + JOBNAME: wheel ${{ matrix.packagekey }} ${{ matrix.blas_lib }} + + + test-conda: + name: Test conda ${{ matrix.packagekey }}, ${{matrix.blas_lib}} BLAS lib ${{ matrix.failok }} + needs: create-conda-test-matrix + runs-on: ${{ matrix.os }}-latest + continue-on-error: ${{ matrix.failok == 'FAILOK' }} + + strategy: + fail-fast: false + matrix: ${{ fromJSON(needs.create-conda-test-matrix.outputs.matrix) }} + + defaults: + run: + shell: bash -l {0} + + steps: + - name: Checkout Slycot + uses: actions/checkout@v3 + with: + repository: 'python-control/Slycot' + path: slycot-src + - name: Checkout python-control + uses: actions/checkout@v3 + - name: Setup macOS + if: matrix.os == 'macos' + run: brew install coreutils + - name: Setup Conda + uses: conda-incubator/setup-miniconda@v2 + with: + python-version: ${{ matrix.python }} + miniforge-version: latest + miniforge-variant: Mambaforge + activate-environment: test-env + environment-file: .github/conda-env/test-env.yml + channel-priority: strict + auto-activate-base: false + - name: Download conda packages + uses: actions/download-artifact@v3 + with: + name: slycot-conda-pkgs + path: slycot-conda-pkgs + - name: Install Conda package + run: | + set -e + case ${{ matrix.blas_lib }} in + unset ) # the conda-forge default (os dependent) + mamba install libblas libcblas liblapack + ;; + Generic ) + mamba install 'libblas=*=*netlib' 'libcblas=*=*netlib' 'liblapack=*=*netlib' + echo "libblas * *netlib" >> $CONDA_PREFIX/conda-meta/pinned + ;; + OpenBLAS ) + mamba install 'libblas=*=*openblas' openblas + echo "libblas * *openblas" >> $CONDA_PREFIX/conda-meta/pinned + ;; + Intel10_64lp ) + mamba install 'libblas=*=*mkl' mkl + echo "libblas * *mkl" >> $CONDA_PREFIX/conda-meta/pinned + ;; + esac + conda index --no-progress ./slycot-conda-pkgs + mamba install -c ./slycot-conda-pkgs slycot + conda list + - name: Test with pytest + run: JOBNAME="$JOBNAME" pytest control/tests + env: + JOBNAME: conda ${{ matrix.packagekey }} ${{ matrix.blas_lib }} diff --git a/.github/workflows/python-package-conda.yml b/.github/workflows/python-package-conda.yml index 0744906a7..cea5e542f 100644 --- a/.github/workflows/python-package-conda.yml +++ b/.github/workflows/python-package-conda.yml @@ -3,58 +3,69 @@ name: Conda-based pytest on: [push, pull_request] jobs: - test-linux: - name: Python ${{ matrix.python-version }}${{ matrix.slycot && format(' with Slycot from {0}', matrix.slycot) || ' without Slycot' }}${{ matrix.pandas && ', with pandas' || '' }}${{ matrix.array-and-matrix == 1 && ', array and matrix' || '' }} + test-linux-conda: + name: > + Py${{ matrix.python-version }}; + ${{ matrix.slycot || 'no' }} Slycot; + ${{ matrix.pandas || 'no' }} Pandas; + ${{ matrix.cvxopt || 'no' }} CVXOPT + ${{ matrix.array-and-matrix == 1 && '; array and matrix' || '' }} + ${{ matrix.mplbackend && format('; {0}', matrix.mplbackend) }} runs-on: ubuntu-latest strategy: max-parallel: 5 + fail-fast: false matrix: - python-version: [3.7, 3.9] + python-version: ['3.8', '3.11'] slycot: ["", "conda"] pandas: [""] + cvxopt: ["", "conda"] + mplbackend: [""] array-and-matrix: [0] include: - - python-version: 3.9 + - python-version: '3.11' slycot: conda pandas: conda + cvxopt: conda + mplbackend: QtAgg array-and-matrix: 1 steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 - - name: Install dependencies - run: | - # Set up conda - echo $CONDA/bin >> $GITHUB_PATH - conda create -q -n test-environment python=${{matrix.python-version}} - source $CONDA/bin/activate test-environment - - # Set up (virtual) X11 - sudo apt install -y xvfb - - # Install test tools - conda install pip coverage pytest pytest-timeout - pip install coveralls + - name: Setup Conda + uses: conda-incubator/setup-miniconda@v2 + with: + python-version: ${{ matrix.python-version }} + activate-environment: test-env + environment-file: .github/conda-env/test-env.yml + miniforge-version: latest + miniforge-variant: Mambaforge + channels: conda-forge + channel-priority: strict + auto-update-conda: false + auto-activate-base: false - # Install python-control dependencies - conda install numpy matplotlib scipy + - name: Install optional dependencies + shell: bash -l {0} + run: | + if [[ '${{matrix.cvxopt}}' == 'conda' ]]; then + mamba install cvxopt + fi if [[ '${{matrix.slycot}}' == 'conda' ]]; then - conda install -c conda-forge slycot + mamba install slycot fi if [[ '${{matrix.pandas}}' == 'conda' ]]; then - conda install pandas + mamba install pandas fi - name: Test with pytest + shell: bash -l {0} env: PYTHON_CONTROL_ARRAY_AND_MATRIX: ${{ matrix.array-and-matrix }} - run: | - source $CONDA/bin/activate test-environment - # Use xvfb-run instead of pytest-xvfb to get proper mpl backend - # Use coverage instead of pytest-cov to get .coverage file - # See https://github.com/python-control/python-control/pull/504 - xvfb-run --auto-servernum coverage run -m pytest control/tests + MPLBACKEND: ${{ matrix.mplbackend }} + run: pytest -v --cov=control --cov-config=.coveragerc control/tests - name: Coveralls parallel # https://github.com/coverallsapp/github-action @@ -64,7 +75,7 @@ jobs: coveralls: name: coveralls completion - needs: test-linux + needs: test-linux-conda runs-on: ubuntu-latest steps: - name: Coveralls Finished diff --git a/.readthedocs.yaml b/.readthedocs.yaml new file mode 100644 index 000000000..dca7c8bc4 --- /dev/null +++ b/.readthedocs.yaml @@ -0,0 +1,21 @@ +# .readthedocs.yaml +# Read the Docs configuration file +# See https://docs.readthedocs.io/en/stable/config-file/v2.html for details + +# Required +version: 2 + +# Set the version of Python and other tools you might need +build: + os: ubuntu-22.04 + tools: + python: "3.9" + +# Build documentation in the docs/ directory with Sphinx +sphinx: + configuration: doc/conf.py + +# Optionally declare the Python requirements required to build your docs +python: + install: + - requirements: doc/requirements.txt diff --git a/MANIFEST.in b/MANIFEST.in index 1edd5f83f..5e06ec2d1 100644 --- a/MANIFEST.in +++ b/MANIFEST.in @@ -1,5 +1,4 @@ include examples/*.py -include tests/*.py include README.rst include ChangeLog include Pending diff --git a/README.rst b/README.rst index f1feda7c5..f3e3a13ff 100644 --- a/README.rst +++ b/README.rst @@ -31,20 +31,17 @@ Try out the examples in the examples folder using the binder service. :target: https://mybinder.org/v2/gh/python-control/python-control/HEAD - - - Features -------- - Linear input/output systems in state-space and frequency domain -- Block diagram algebra: serial, parallel, and feedback interconnections +- Block diagram algebra: serial, parallel, feedback, and other interconnections - Time response: initial, step, impulse -- Frequency response: Bode and Nyquist plots -- Control analysis: stability, reachability, observability, stability margins -- Control design: eigenvalue placement, linear quadratic regulator +- Frequency response: Bode, Nyquist, and Nichols plots +- Control analysis: stability, reachability, observability, stability margins, root locus +- Control design: eigenvalue placement, linear quadratic regulator, sisotool, hinfsyn, rootlocus_pid_designer - Estimator design: linear quadratic estimator (Kalman filter) - +- Nonlinear systems: optimization-based control, describing functions, differential flatness Links ===== @@ -97,17 +94,30 @@ To install using pip:: If you install Slycot using pip you'll need a development environment (e.g., Python development files, C and Fortran compilers). -Distutils ---------- +Installing from source +---------------------- + +To install from source, get the source code of the desired branch or release +from the github repository or archive, unpack, and run from within the +toplevel `python-control` directory:: -To install in your home directory, use:: + pip install . + +Article and Citation Information +================================ - python setup.py install --user +An `article `_ about the library is available on IEEE Explore. If the Python Control Systems Library helped you in your research, please cite:: -To install for all users (on Linux or Mac OS):: + @inproceedings{python-control2021, + title={The Python Control Systems Library (python-control)}, + author={Fuller, Sawyer and Greiner, Ben and Moore, Jason and Murray, Richard and van Paassen, Ren{\'e} and Yorke, Rory}, + booktitle={60th IEEE Conference on Decision and Control (CDC)}, + pages={4875--4881}, + year={2021}, + organization={IEEE} + } - python setup.py build - sudo python setup.py install +or the GitHub site: https://github.com/python-control/python-control Development diff --git a/benchmarks/README b/benchmarks/README index a10bbfc21..9c788b250 100644 --- a/benchmarks/README +++ b/benchmarks/README @@ -11,7 +11,7 @@ you can use the following command from the root directory of the repository: PYTHONPATH=`pwd` asv run --python=python -You can also run benchmarks against specific commits usuing +You can also run benchmarks against specific commits using asv run diff --git a/benchmarks/flatsys_bench.py b/benchmarks/flatsys_bench.py index 0c0a5e53a..05a2e7066 100644 --- a/benchmarks/flatsys_bench.py +++ b/benchmarks/flatsys_bench.py @@ -11,6 +11,10 @@ import control.flatsys as flat import control.optimal as opt +# +# System setup: vehicle steering (bicycle model) +# + # Vehicle steering dynamics def vehicle_update(t, x, u, params): # Get the parameters for the model @@ -67,11 +71,28 @@ def vehicle_reverse(zflag, params={}): # Define the time points where the cost/constraints will be evaluated timepts = np.linspace(0, Tf, 10, endpoint=True) -def time_steering_point_to_point(basis_name, basis_size): - if basis_name == 'poly': - basis = flat.PolyFamily(basis_size) - elif basis_name == 'bezier': - basis = flat.BezierFamily(basis_size) +# +# Benchmark test parameters +# + +basis_params = (['poly', 'bezier', 'bspline'], [8, 10, 12]) +basis_param_names = ["basis", "size"] + +def get_basis(name, size): + if name == 'poly': + basis = flat.PolyFamily(size, T=Tf) + elif name == 'bezier': + basis = flat.BezierFamily(size, T=Tf) + elif name == 'bspline': + basis = flat.BSplineFamily([0, Tf/2, Tf], size) + return basis + +# +# Benchmarks +# + +def time_point_to_point(basis_name, basis_size): + basis = get_basis(basis_name, basis_size) # Find trajectory between initial and final conditions traj = flat.point_to_point(vehicle, Tf, x0, u0, xf, uf, basis=basis) @@ -80,13 +101,16 @@ def time_steering_point_to_point(basis_name, basis_size): x, u = traj.eval([0, Tf]) np.testing.assert_array_almost_equal(x0, x[:, 0]) np.testing.assert_array_almost_equal(u0, u[:, 0]) - np.testing.assert_array_almost_equal(xf, x[:, 1]) - np.testing.assert_array_almost_equal(uf, u[:, 1]) + np.testing.assert_array_almost_equal(xf, x[:, -1]) + np.testing.assert_array_almost_equal(uf, u[:, -1]) + +time_point_to_point.params = basis_params +time_point_to_point.param_names = basis_param_names -time_steering_point_to_point.params = (['poly', 'bezier'], [6, 8]) -time_steering_point_to_point.param_names = ["basis", "size"] -def time_steering_cost(): +def time_point_to_point_with_cost(basis_name, basis_size): + basis = get_basis(basis_name, basis_size) + # Define cost and constraints traj_cost = opt.quadratic_cost( vehicle, None, np.diag([0.1, 1]), u0=uf) @@ -95,13 +119,47 @@ def time_steering_cost(): traj = flat.point_to_point( vehicle, timepts, x0, u0, xf, uf, - cost=traj_cost, constraints=constraints, basis=flat.PolyFamily(8) + cost=traj_cost, constraints=constraints, basis=basis, ) # Verify that the trajectory computation is correct x, u = traj.eval([0, Tf]) np.testing.assert_array_almost_equal(x0, x[:, 0]) np.testing.assert_array_almost_equal(u0, u[:, 0]) - np.testing.assert_array_almost_equal(xf, x[:, 1]) - np.testing.assert_array_almost_equal(uf, u[:, 1]) + np.testing.assert_array_almost_equal(xf, x[:, -1]) + np.testing.assert_array_almost_equal(uf, u[:, -1]) + +time_point_to_point_with_cost.params = basis_params +time_point_to_point_with_cost.param_names = basis_param_names + + +def time_solve_flat_ocp_terminal_cost(method, basis_name, basis_size): + basis = get_basis(basis_name, basis_size) + + # Define cost and constraints + traj_cost = opt.quadratic_cost( + vehicle, None, np.diag([0.1, 1]), u0=uf) + term_cost = opt.quadratic_cost( + vehicle, np.diag([1e3, 1e3, 1e3]), None, x0=xf) + constraints = [ + opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ] + + # Initial guess = straight line + initial_guess = np.array( + [x0[i] + (xf[i] - x0[i]) * timepts/Tf for i in (0, 1)]) + + traj = flat.solve_flat_ocp( + vehicle, timepts, x0, u0, basis=basis, initial_guess=initial_guess, + trajectory_cost=traj_cost, constraints=constraints, + terminal_cost=term_cost, minimize_method=method, + ) + + # Verify that the trajectory computation is correct + x, u = traj.eval([0, Tf]) + np.testing.assert_array_almost_equal(x0, x[:, 0]) + np.testing.assert_array_almost_equal(xf, x[:, -1], decimal=2) +time_solve_flat_ocp_terminal_cost.params = tuple( + [['slsqp', 'trust-constr']] + list(basis_params)) +time_solve_flat_ocp_terminal_cost.param_names = tuple( + ['method'] + basis_param_names) diff --git a/benchmarks/optimal_bench.py b/benchmarks/optimal_bench.py index 21cabef7e..997b5a241 100644 --- a/benchmarks/optimal_bench.py +++ b/benchmarks/optimal_bench.py @@ -8,165 +8,197 @@ import numpy as np import math import control as ct -import control.flatsys as flat +import control.flatsys as fs import control.optimal as opt -# Vehicle steering dynamics -def vehicle_update(t, x, u, params): - # Get the parameters for the model - l = params.get('wheelbase', 3.) # vehicle wheelbase - phimax = params.get('maxsteer', 0.5) # max steering angle (rad) +# +# Benchmark test parameters +# - # Saturate the steering input (use min/max instead of clip for speed) - phi = max(-phimax, min(u[1], phimax)) +# Define integrator and minimizer methods and options/keywords +integrator_table = { + 'default': (None, {}), + 'RK23': ('RK23', {}), + 'RK23_sloppy': ('RK23', {'atol': 1e-4, 'rtol': 1e-2}), + 'RK45': ('RK45', {}), + 'RK45': ('RK45', {}), + 'RK45_sloppy': ('RK45', {'atol': 1e-4, 'rtol': 1e-2}), + 'LSODA': ('LSODA', {}), +} - # Return the derivative of the state - return np.array([ - math.cos(x[2]) * u[0], # xdot = cos(theta) v - math.sin(x[2]) * u[0], # ydot = sin(theta) v - (u[0] / l) * math.tan(phi) # thdot = v/l tan(phi) - ]) +minimizer_table = { + 'default': (None, {}), + 'trust': ('trust-constr', {}), + 'trust_bigstep': ('trust-constr', {'finite_diff_rel_step': 0.01}), + 'SLSQP': ('SLSQP', {}), + 'SLSQP_bigstep': ('SLSQP', {'eps': 0.01}), + 'COBYLA': ('COBYLA', {}), +} -def vehicle_output(t, x, u, params): - return x # return x, y, theta (full state) -vehicle = ct.NonlinearIOSystem( - vehicle_update, vehicle_output, states=3, name='vehicle', - inputs=('v', 'phi'), outputs=('x', 'y', 'theta')) +# Utility function to create a basis of a given size +def get_basis(name, size, Tf): + if name == 'poly': + basis = fs.PolyFamily(size, T=Tf) + elif name == 'bezier': + basis = fs.BezierFamily(size, T=Tf) + elif name == 'bspline': + basis = fs.BSplineFamily([0, Tf/2, Tf], size) + else: + basis = None + return basis -# 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) +# Assess performance as a function of basis type and size +def time_optimal_lq_basis(basis_name, basis_size, npoints, method): + # Create a sufficiently controllable random system to control + ntrys = 20 + while ntrys > 0: + # Create a random system + sys = ct.rss(2, 2, 2) -# Provide an intial guess (will be extended to entire horizon) -bend_left = [10, 0.01] # slight left veer + # Compute the controllability Gramian + Wc = ct.gram(sys, 'c') -def time_steering_integrated_cost(): - # Set up the cost functions - Q = np.diag([.1, 10, .1]) # keep lateral error low - R = np.diag([.1, 1]) # minimize applied inputs - quad_cost = opt.quadratic_cost( - vehicle, Q, R, x0=xf, u0=uf) + # Make sure the condition number is reasonable + if np.linalg.cond(Wc) < 1e6: + break - res = opt.solve_ocp( - vehicle, horizon, x0, quad_cost, - initial_guess=bend_left, print_summary=False, - # solve_ivp_kwargs={'atol': 1e-2, 'rtol': 1e-2}, - minimize_method='trust-constr', - minimize_options={'finite_diff_rel_step': 0.01}, - ) + ntrys -= 1 + assert ntrys > 0 # Something wrong if we needed > 20 tries - # Only count this as a benchmark if we converged - assert res.success + # Define cost functions + Q = np.eye(sys.nstates) + R = np.eye(sys.ninputs) * 10 + + # Figure out the LQR solution (for terminal cost) + K, S, E = ct.lqr(sys, Q, R) + + # Define the cost functions + traj_cost = opt.quadratic_cost(sys, Q, R) + term_cost = opt.quadratic_cost(sys, S, None) + constraints = opt.input_range_constraint( + sys, -np.ones(sys.ninputs), np.ones(sys.ninputs)) + + # Define the initial condition, time horizon, and time points + x0 = np.ones(sys.nstates) + Tf = 10 + timepts = np.linspace(0, Tf, npoints) -def time_steering_terminal_cost(): - # Define cost and constraints - traj_cost = opt.quadratic_cost( - vehicle, None, np.diag([0.1, 1]), u0=uf) - term_cost = opt.quadratic_cost( - vehicle, np.diag([1, 10, 10]), None, x0=xf) - constraints = [ - opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ] + # Create the basis function to use + basis = get_basis(basis_name, basis_size, Tf) res = opt.solve_ocp( - vehicle, horizon, x0, traj_cost, constraints, - terminal_cost=term_cost, initial_guess=bend_left, print_summary=False, - solve_ivp_kwargs={'atol': 1e-4, 'rtol': 1e-2}, - # minimize_method='SLSQP', minimize_options={'eps': 0.01} - minimize_method='trust-constr', - minimize_options={'finite_diff_rel_step': 0.01}, + sys, timepts, x0, traj_cost, constraints, terminal_cost=term_cost, + basis=basis, trajectory_method=method, ) # Only count this as a benchmark if we converged assert res.success -# Define integrator and minimizer methods and options/keywords -integrator_table = { - 'RK23_default': ('RK23', {'atol': 1e-4, 'rtol': 1e-2}), - 'RK23_sloppy': ('RK23', {}), - 'RK45_default': ('RK45', {}), - 'RK45_sloppy': ('RK45', {'atol': 1e-4, 'rtol': 1e-2}), -} - -minimizer_table = { - 'trust_default': ('trust-constr', {}), - 'trust_bigstep': ('trust-constr', {'finite_diff_rel_step': 0.01}), - 'SLSQP_default': ('SLSQP', {}), - 'SLSQP_bigstep': ('SLSQP', {'eps': 0.01}), -} +# Parameterize the test against different choices of integrator and minimizer +time_optimal_lq_basis.param_names = ['basis', 'size', 'npoints', 'method'] +time_optimal_lq_basis.params = ( + [None, 'poly', 'bezier', 'bspline'], + [4, 8], [5, 10], ['shooting', 'collocation']) -def time_steering_terminal_constraint(integrator_name, minimizer_name): +# Assess performance as a function of optimization and integration methods +def time_optimal_lq_methods(integrator_name, minimizer_name, method): # Get the integrator and minimizer parameters to use integrator = integrator_table[integrator_name] minimizer = minimizer_table[minimizer_name] - # Input cost and terminal constraints - R = np.diag([1, 1]) # minimize applied inputs - cost = opt.quadratic_cost(vehicle, np.zeros((3,3)), R, u0=uf) - constraints = [ - opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ] - terminal = [ opt.state_range_constraint(vehicle, xf, xf) ] + # Create a random system to control + sys = ct.rss(2, 1, 1) + + # Define cost functions + Q = np.eye(sys.nstates) + R = np.eye(sys.ninputs) * 10 + + # Figure out the LQR solution (for terminal cost) + K, S, E = ct.lqr(sys, Q, R) + + # Define the cost functions + traj_cost = opt.quadratic_cost(sys, Q, R) + term_cost = opt.quadratic_cost(sys, S, None) + constraints = opt.input_range_constraint( + sys, -np.ones(sys.ninputs), np.ones(sys.ninputs)) + + # Define the initial condition, time horizon, and time points + x0 = np.ones(sys.nstates) + Tf = 10 + timepts = np.linspace(0, Tf, 20) + + # Create the basis function to use + basis = get_basis('poly', 12, Tf) res = opt.solve_ocp( - vehicle, horizon, x0, cost, constraints, - terminal_constraints=terminal, initial_guess=bend_left, log=False, + sys, timepts, x0, traj_cost, constraints, terminal_cost=term_cost, solve_ivp_method=integrator[0], solve_ivp_kwargs=integrator[1], minimize_method=minimizer[0], minimize_options=minimizer[1], + trajectory_method=method, ) # Only count this as a benchmark if we converged assert res.success -# Reset the timeout value to allow for longer runs -time_steering_terminal_constraint.timeout = 120 - # Parameterize the test against different choices of integrator and minimizer -time_steering_terminal_constraint.param_names = ['integrator', 'minimizer'] -time_steering_terminal_constraint.params = ( - ['RK23_default', 'RK23_sloppy', 'RK45_default', 'RK45_sloppy'], - ['trust_default', 'trust_bigstep', 'SLSQP_default', 'SLSQP_bigstep'] -) - -def time_steering_bezier_basis(nbasis, ntimes): - # Set up costs and constriants - Q = np.diag([.1, 10, .1]) # keep lateral error low - R = np.diag([1, 1]) # minimize applied inputs - cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf) - constraints = [ opt.input_range_constraint(vehicle, [0, -0.1], [20, 0.1]) ] - terminal = [ opt.state_range_constraint(vehicle, xf, xf) ] - - # Set up horizon - horizon = np.linspace(0, Tf, ntimes, endpoint=True) - - # Set up the optimal control problem +time_optimal_lq_methods.param_names = ['integrator', 'minimizer', 'method'] +time_optimal_lq_methods.params = ( + ['RK23', 'RK45', 'LSODA'], ['trust', 'SLSQP', 'COBYLA'], + ['shooting', 'collocation']) + + +# Assess performance as a function system size +def time_optimal_lq_size(nstates, ninputs, npoints, method): + # Create a sufficiently controllable random system to control + ntrys = 20 + while ntrys > 0: + # Create a random system + sys = ct.rss(nstates, ninputs, ninputs) + + # Compute the controllability Gramian + Wc = ct.gram(sys, 'c') + + # Make sure the condition number is reasonable + if np.linalg.cond(Wc) < 1e6: + break + + ntrys -= 1 + assert ntrys > 0 # Something wrong if we needed > 20 tries + + # Define cost functions + Q = np.eye(sys.nstates) + R = np.eye(sys.ninputs) * 10 + + # Figure out the LQR solution (for terminal cost) + K, S, E = ct.lqr(sys, Q, R) + + # Define the cost functions + traj_cost = opt.quadratic_cost(sys, Q, R) + term_cost = opt.quadratic_cost(sys, S, None) + constraints = opt.input_range_constraint( + sys, -np.ones(sys.ninputs), np.ones(sys.ninputs)) + + # Define the initial condition, time horizon, and time points + x0 = np.ones(sys.nstates) + Tf = 10 + timepts = np.linspace(0, Tf, npoints) + res = opt.solve_ocp( - vehicle, horizon, x0, cost, - constraints, - terminal_constraints=terminal, - initial_guess=bend_left, - basis=flat.BezierFamily(nbasis, T=Tf), - # solve_ivp_kwargs={'atol': 1e-4, 'rtol': 1e-2}, - minimize_method='trust-constr', - minimize_options={'finite_diff_rel_step': 0.01}, - # minimize_method='SLSQP', minimize_options={'eps': 0.01}, - return_states=True, print_summary=False + sys, timepts, x0, traj_cost, constraints, terminal_cost=term_cost, + trajectory_method=method, ) - t, u, x = res.time, res.inputs, res.states - - # Make sure we found a valid solution + # Only count this as a benchmark if we converged assert res.success -# Reset the timeout value to allow for longer runs -time_steering_bezier_basis.timeout = 120 +# Parameterize the test against different choices of integrator and minimizer +time_optimal_lq_size.param_names = ['nstates', 'ninputs', 'npoints', 'method'] +time_optimal_lq_size.params = ( + [2, 4], [2, 4], [10, 20], ['shooting', 'collocation']) -# 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(): +# Aircraft MPC example (from multi-parametric toolbox) +def time_discrete_aircraft_mpc(minimizer_name): # model of an aircraft discretized with 0.2s sampling time # Source: https://www.mpt3.org/UI/RegulationProblem A = [[0.99, 0.01, 0.18, -0.09, 0], @@ -201,9 +233,18 @@ def time_aircraft_mpc(): R = np.diag([3, 2]) cost = opt.quadratic_cost(model, Q, R, x0=xd, u0=ud) + # Set the time horizon and time points + Tf = 3 + timepts = np.arange(0, 6) * 0.2 + + # Get the minimizer parameters to use + minimizer = minimizer_table[minimizer_name] + # online MPC controller object is constructed with a horizon 6 ctrl = opt.create_mpc_iosystem( - model, np.arange(0, 6) * 0.2, cost, constraints) + model, timepts, cost, constraints, + minimize_method=minimizer[0], minimize_options=minimizer[1], + ) # Define an I/O system implementing model predictive control loop = ct.feedback(sys, ctrl, 1) @@ -218,3 +259,8 @@ def time_aircraft_mpc(): # Make sure the system converged to the desired state np.testing.assert_allclose( xout[0:sys.nstates, -1], xd, atol=0.1, rtol=0.01) + +# Parameterize the test against different choices of minimizer and basis +time_discrete_aircraft_mpc.param_names = ['minimizer'] +time_discrete_aircraft_mpc.params = ( + ['trust', 'trust_bigstep', 'SLSQP', 'SLSQP_bigstep', 'COBYLA']) diff --git a/control/__init__.py b/control/__init__.py index ad2685273..e0edc96a2 100644 --- a/control/__init__.py +++ b/control/__init__.py @@ -72,6 +72,7 @@ from .config import * from .sisotool import * from .iosys import * +from .passivity import * # Exceptions from .exception import * diff --git a/control/config.py b/control/config.py index 32f5f2eef..37763a6b8 100644 --- a/control/config.py +++ b/control/config.py @@ -97,6 +97,12 @@ def reset_defaults(): from .rlocus import _rlocus_defaults defaults.update(_rlocus_defaults) + from .sisotool import _sisotool_defaults + defaults.update(_sisotool_defaults) + + from .namedio import _namedio_defaults + defaults.update(_namedio_defaults) + from .xferfcn import _xferfcn_defaults defaults.update(_xferfcn_defaults) @@ -285,7 +291,7 @@ def use_legacy_defaults(version): set_defaults('control', default_dt=None) # changed iosys naming conventions - set_defaults('iosys', state_name_delim='.', + set_defaults('namedio', state_name_delim='.', duplicate_system_name_prefix='copy of ', duplicate_system_name_suffix='', linearized_system_name_prefix='', diff --git a/control/descfcn.py b/control/descfcn.py index 149db1bd2..41d273f38 100644 --- a/control/descfcn.py +++ b/control/descfcn.py @@ -74,7 +74,7 @@ def _f(self, x): def describing_function( F, A, num_points=100, zero_check=True, try_method=True): - """Numerical compute the describing function of a nonlinear function + """Numerically compute the describing function of a nonlinear function The describing function of a nonlinearity is given by magnitude and phase of the first harmonic of the function when evaluated along a sinusoidal @@ -236,8 +236,8 @@ def describing_function_plot( given by the first value of the tuple and frequency given by the second value. - Example - ------- + Examples + -------- >>> H_simple = ct.tf([8], [1, 2, 2, 1]) >>> F_saturation = ct.descfcn.saturation_nonlinearity(1) >>> amp = np.linspace(1, 4, 10) diff --git a/control/dtime.py b/control/dtime.py index b05d22b96..724eafb76 100644 --- a/control/dtime.py +++ b/control/dtime.py @@ -53,7 +53,8 @@ __all__ = ['sample_system', 'c2d'] # Sample a continuous time system -def sample_system(sysc, Ts, method='zoh', alpha=None, prewarp_frequency=None): +def sample_system(sysc, Ts, method='zoh', alpha=None, prewarp_frequency=None, + name=None, copy_names=True, **kwargs): """ Convert a continuous time system to discrete time by sampling @@ -72,12 +73,35 @@ def sample_system(sysc, Ts, method='zoh', alpha=None, prewarp_frequency=None): prewarp_frequency : float within [0, infinity) The frequency [rad/s] at which to match with the input continuous- time system's magnitude and phase (only valid for method='bilinear') + name : string, optional + Set the name of the sampled system. If not specified and + if `copy_names` is `False`, a generic name is generated + with a unique integer id. If `copy_names` is `True`, the new system + name is determined by adding the prefix and suffix strings in + config.defaults['namedio.sampled_system_name_prefix'] and + config.defaults['namedio.sampled_system_name_suffix'], with the + default being to add the suffix '$sampled'. + copy_names : bool, Optional + If True, copy the names of the input signals, output + signals, and states to the sampled system. Returns ------- sysd : linsys Discrete time system, with sampling rate Ts + Other Parameters + ---------------- + inputs : int, list of str or None, optional + Description of the system inputs. If not specified, the origional + system inputs are used. See :class:`InputOutputSystem` for more + information. + outputs : int, list of str or None, optional + Description of the system outputs. Same format as `inputs`. + states : int, list of str, or None, optional + Description of the system states. Same format as `inputs`. Only + available if the system is :class:`StateSpace`. + Notes ----- See :meth:`StateSpace.sample` or :meth:`TransferFunction.sample` for @@ -94,7 +118,8 @@ def sample_system(sysc, Ts, method='zoh', alpha=None, prewarp_frequency=None): raise ValueError("First argument must be continuous time system") return sysc.sample(Ts, - method=method, alpha=alpha, prewarp_frequency=prewarp_frequency) + method=method, alpha=alpha, prewarp_frequency=prewarp_frequency, + name=name, copy_names=copy_names, **kwargs) def c2d(sysc, Ts, method='zoh', prewarp_frequency=None): diff --git a/control/exception.py b/control/exception.py index f66eb7f30..575c78c0a 100644 --- a/control/exception.py +++ b/control/exception.py @@ -84,3 +84,15 @@ def pandas_check(): except: pandas_installed = False return pandas_installed + +# Utility function to see if cvxopt is installed +cvxopt_installed = None +def cvxopt_check(): + global cvxopt_installed + if cvxopt_installed is None: + try: + import cvxopt + cvxopt_installed = True + except: + cvxopt_installed = False + return cvxopt_installed diff --git a/control/flatsys/__init__.py b/control/flatsys/__init__.py index 0926fa81a..157800073 100644 --- a/control/flatsys/__init__.py +++ b/control/flatsys/__init__.py @@ -46,7 +46,9 @@ defined using the :class:`~control.flatsys.BasisFamily` class. The resulting trajectory is return as a :class:`~control.flatsys.SystemTrajectory` object and can be evaluated using the :func:`~control.flatsys.SystemTrajectory.eval` -member function. +member function. Alternatively, the :func:`~control.flatsys.solve_flat_ocp` +function can be used to solve an optimal control problem with trajectory and +final costs or constraints. """ @@ -54,6 +56,7 @@ from .basis import BasisFamily from .poly import PolyFamily from .bezier import BezierFamily +from .bspline import BSplineFamily # Classes from .systraj import SystemTrajectory @@ -61,4 +64,4 @@ from .linflat import LinearFlatSystem # Package functions -from .flatsys import point_to_point +from .flatsys import point_to_point, solve_flat_ocp diff --git a/control/flatsys/basis.py b/control/flatsys/basis.py index 1ea957f52..04abce88a 100644 --- a/control/flatsys/basis.py +++ b/control/flatsys/basis.py @@ -36,6 +36,8 @@ # OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF # SUCH DAMAGE. +import numpy as np + # Basis family class (for use as a base class) class BasisFamily: @@ -47,7 +49,11 @@ class BasisFamily: :math:`z_i^{(q)}(t)` = basis.eval_deriv(self, i, j, t) - Parameters + A basis set can either consist of a single variable that is used for + each flat output (nvars = None) or a different variable for different + flat outputs (nvars > 0). + + Attributes ---------- N : int Order of the basis set. @@ -56,10 +62,52 @@ class BasisFamily: def __init__(self, N): """Create a basis family of order N.""" self.N = N # save number of basis functions + self.nvars = None # default number of variables + self.coef_offset = [0] # coefficient offset for each variable + self.coef_length = [N] # coefficient length for each variable - def __call__(self, i, t): + def __repr__(self): + return f'<{self.__class__.__name__}: nvars={self.nvars}, ' + \ + f'N={self.N}>' + + def __call__(self, i, t, var=None): """Evaluate the ith basis function at a point in time""" - return self.eval_deriv(i, 0, t) + return self.eval_deriv(i, 0, t, var=var) + + def var_ncoefs(self, var): + """Get the number of coefficients for a variable""" + return self.N if self.nvars is None else self.coef_length[var] + + def eval(self, coeffs, tlist, var=None): + """Compute function values given the coefficients and time points.""" + if self.nvars is None and var != None: + raise SystemError("multi-variable call to a scalar basis") + + elif self.nvars is None: + # Single variable basis + return [ + sum([coeffs[i] * self(i, t) for i in range(self.N)]) + for t in tlist] + + elif var is None: + # Multi-variable basis with single list of coefficients + values = np.empty((self.nvars, tlist.size)) + offset = 0 + for j in range(self.nvars): + coef_len = self.var_ncoefs(j) + values[j] = np.array([ + sum([coeffs[offset + i] * self(i, t, var=j) + for i in range(coef_len)]) + for t in tlist]) + offset += coef_len + return values + + else: + return np.array([ + sum([coeffs[i] * self(i, t, var=var) + for i in range(self.var_ncoefs(var))]) + for t in tlist]) - def eval_deriv(self, i, j, t): + def eval_deriv(self, i, j, t, var=None): + """Evaluate the kth derivative of the ith basis function at time t.""" raise NotImplementedError("Internal error; improper basis functions") diff --git a/control/flatsys/bezier.py b/control/flatsys/bezier.py index 7e41c546e..fcf6201e9 100644 --- a/control/flatsys/bezier.py +++ b/control/flatsys/bezier.py @@ -48,24 +48,32 @@ class BezierFamily(BasisFamily): This class represents the family of polynomials of the form .. math:: - \phi_i(t) = \sum_{i=0}^n {n \choose i} - \left( \frac{t}{T_\text{f}} - t \right)^{n-i} - \left( \frac{t}{T_f} \right)^i + \phi_i(t) = \sum_{i=0}^N {N \choose i} + \left( \frac{t}{T} - t \right)^{N-i} + \left( \frac{t}{T} \right)^i + + Parameters + ---------- + N : int + Degree of the Bezier curve. + + T : float + Final time (used for rescaling). """ def __init__(self, N, T=1): """Create a polynomial basis of order N.""" super(BezierFamily, self).__init__(N) - self.T = T # save end of time interval + self.T = float(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): + def eval_deriv(self, i, k, t, var=None): """Evaluate the kth derivative of the ith basis function at time t.""" if i >= self.N: raise ValueError("Basis function index too high") elif k >= self.N: # Higher order derivatives are zero - return np.zeros(t.shape) + return 0 * t # Compute the variables used in Bezier curve formulas n = self.N - 1 @@ -78,6 +86,7 @@ def eval_deriv(self, i, k, t): # Return the kth derivative of the ith Bezier basis function return binom(n, i) * sum([ (-1)**(j-i) * - binom(n-i, j-i) * factorial(j)/factorial(j-k) * np.power(u, j-k) + binom(n-i, j-i) * factorial(j)/factorial(j-k) * \ + np.power(u, j-k) / np.power(self.T, k) for j in range(max(i, k), n+1) ]) diff --git a/control/flatsys/bspline.py b/control/flatsys/bspline.py new file mode 100644 index 000000000..c771beb59 --- /dev/null +++ b/control/flatsys/bspline.py @@ -0,0 +1,196 @@ +# bspline.py - B-spline basis functions +# RMM, 2 Aug 2022 +# +# This class implements a set of B-spline basis functions that implement a +# piecewise polynomial at a set of breakpoints t0, ..., tn with given orders +# and smoothness. +# + +import numpy as np +from .basis import BasisFamily +from scipy.interpolate import BSpline, splev + +class BSplineFamily(BasisFamily): + """B-spline basis functions. + + This class represents a B-spline basis for piecewise polynomials defined + across a set of breakpoints with given degree and smoothness. On each + interval between two breakpoints, we have a polynomial of a given degree + and the spline is continuous up to a given smoothness at interior + breakpoints. + + Parameters + ---------- + breakpoints : 1D array or 2D array of float + The breakpoints for the spline(s). + + degree : int or list of ints + For each spline variable, the degree of the polynomial between + breakpoints. If a single number is given and more than one spline + variable is specified, the same degree is used for each spline + variable. + + smoothness : int or list of ints + For each spline variable, the smoothness at breakpoints (number of + derivatives that should match). + + vars : None or int, optional + The number of spline variables. If specified as None (default), + then the spline basis describes a single variable, with no indexing. + If the number of spine variables is > 0, then the spline basis is + index using the `var` keyword. + + """ + def __init__(self, breakpoints, degree, smoothness=None, vars=None): + """Create a B-spline basis for piecewise smooth polynomials.""" + # Process the breakpoints for the spline */ + breakpoints = np.array(breakpoints, dtype=float) + if breakpoints.ndim == 2: + raise NotImplementedError( + "breakpoints for each spline variable not yet supported") + elif breakpoints.ndim != 1: + raise ValueError("breakpoints must be convertable to a 1D array") + elif breakpoints.size < 2: + raise ValueError("break point vector must have at least 2 values") + elif np.any(np.diff(breakpoints) <= 0): + raise ValueError("break points must be strictly increasing values") + + # Decide on the number of spline variables + if vars is None: + nvars = 1 + self.nvars = None # track as single variable + elif not isinstance(vars, int): + raise TypeError("vars must be an integer") + else: + nvars = vars + self.nvars = nvars + + # + # Process B-spline parameters (degree, smoothness) + # + # B-splines are defined on a set of intervals separated by + # breakpoints. On each interval we have a polynomial of a certain + # degree and the spline is continuous up to a given smoothness at + # breakpoints. The code in this section allows some flexibility in + # the way that all of this information is supplied, including using + # scalar values for parameters (which are then broadcast to each + # output) and inferring values and dimensions from other + # information, when possible. + # + + # Utility function for broadcasting spline params (degree, smoothness) + def process_spline_parameters( + values, length, allowed_types, minimum=0, + default=None, name='unknown'): + + # Preprocessing + if values is None and default is None: + return None + elif values is None: + values = default + elif isinstance(values, np.ndarray): + # Convert ndarray to list + values = values.tolist() + + # Figure out what type of object we were passed + if isinstance(values, allowed_types): + # Single number of an allowed type => broadcast to list + values = [values for i in range(length)] + elif all([isinstance(v, allowed_types) for v in values]): + # List of values => make sure it is the right size + if len(values) != length: + raise ValueError(f"length of '{name}' does not match" + f" number of variables") + else: + raise ValueError(f"could not parse '{name}' keyword") + + # Check to make sure the values are OK + if values is not None and any([val < minimum for val in values]): + raise ValueError( + f"invalid value for '{name}'; must be at least {minimum}") + + return values + + # Degree of polynomial + degree = process_spline_parameters( + degree, nvars, (int), name='degree', minimum=1) + + # Smoothness at breakpoints; set default to degree - 1 (max possible) + smoothness = process_spline_parameters( + smoothness, nvars, (int), name='smoothness', minimum=0, + default=[d - 1 for d in degree]) + + # Make sure degree is sufficent for the level of smoothness + if any([degree[i] - smoothness[i] < 1 for i in range(nvars)]): + raise ValueError("degree must be greater than smoothness") + + # Store the parameters for the spline (self.nvars already stored) + self.breakpoints = breakpoints + self.degree = degree + self.smoothness = smoothness + + # + # Compute parameters for a SciPy BSpline object + # + # To create a B-spline, we need to compute the knotpoints, keeping + # track of the use of repeated knotpoints at the initial knot and + # final knot as well as repeated knots at intermediate points + # depending on the desired smoothness. + # + + # Store the coefficients for each output (useful later) + self.coef_offset, self.coef_length, offset = [], [], 0 + for i in range(nvars): + # Compute number of coefficients for the piecewise polynomial + ncoefs = (self.degree[i] + 1) * (len(self.breakpoints) - 1) - \ + (self.smoothness[i] + 1) * (len(self.breakpoints) - 2) + + self.coef_offset.append(offset) + self.coef_length.append(ncoefs) + offset += ncoefs + self.N = offset # save the total number of coefficients + + # Create knotpoints for each spline variable + # TODO: extend to multi-dimensional breakpoints + self.knotpoints = [] + for i in range(nvars): + # Allocate space for the knotpoints + self.knotpoints.append(np.empty( + (self.degree[i] + 1) + (len(self.breakpoints) - 2) * \ + (self.degree[i] - self.smoothness[i]) + (self.degree[i] + 1))) + + # Initial knotpoints (multiplicity = order) + self.knotpoints[i][0:self.degree[i] + 1] = self.breakpoints[0] + offset = self.degree[i] + 1 + + # Interior knotpoints (multiplicity = degree - smoothness) + nknots = self.degree[i] - self.smoothness[i] + assert nknots > 0 # just in case + for j in range(1, self.breakpoints.size - 1): + self.knotpoints[i][offset:offset+nknots] = self.breakpoints[j] + offset += nknots + + # Final knotpoint (multiplicity = order) + self.knotpoints[i][offset:offset + self.degree[i] + 1] = \ + self.breakpoints[-1] + + def __repr__(self): + return f'<{self.__class__.__name__}: nvars={self.nvars}, ' + \ + f'degree={self.degree}, smoothness={self.smoothness}>' + + # Compute the kth derivative of the ith basis function at time t + def eval_deriv(self, i, k, t, var=None): + """Evaluate the kth derivative of the ith basis function at time t.""" + if self.nvars is None or (self.nvars == 1 and var is None): + # Use same variable for all requests + var = 0 + elif self.nvars > 1 and var is None: + raise SystemError( + "scalar variable call to multi-variable splines") + + # Create a coefficient vector for this spline + coefs = np.zeros(self.coef_length[var]); coefs[i] = 1 + + # Evaluate the derivative of the spline at the desired point in time + return BSpline(self.knotpoints[var], coefs, + self.degree[var]).derivative(k)(t) diff --git a/control/flatsys/flatsys.py b/control/flatsys/flatsys.py index c01eb9127..5741a9bd3 100644 --- a/control/flatsys/flatsys.py +++ b/control/flatsys/flatsys.py @@ -61,8 +61,10 @@ class FlatSystem(NonlinearIOSystem): ---------- forward : callable A function to compute the flat flag given the states and input. + reverse : callable A function to compute the states and input given the flat flag. + updfcn : callable, optional Function returning the state update function @@ -73,6 +75,7 @@ class FlatSystem(NonlinearIOSystem): time, and `param` is an optional dict containing the values of parameters used by the function. If not specified, the state space update will be computed using the flat system coordinates. + outfcn : callable Function returning the output at the given state @@ -80,6 +83,7 @@ class FlatSystem(NonlinearIOSystem): where the arguments are the same as for `upfcn`. If not specified, the output will be the flat outputs. + inputs : int, list of str, or None Description of the system inputs. This can be given as an integer count or as a list of strings that name the individual signals. @@ -88,19 +92,24 @@ class FlatSystem(NonlinearIOSystem): this parameter is not given or given as `None`, the relevant quantity will be determined when possible based on other information provided to functions using the system. + outputs : int, list of str, or None Description of the system outputs. Same format as `inputs`. + states : int, list of str, or None Description of the system states. Same format as `inputs`. + dt : None, True or float, optional System timebase. None (default) indicates continuous time, True indicates discrete time with undefined sampling time, positive number is discrete time with specified sampling time. + params : dict, optional Parameter values for the systems. Passed to the evaluation functions for the system as default values, overriding internal defaults. + name : string, optional System name (used for specifying signals) @@ -133,7 +142,7 @@ def __init__(self, forward, reverse, # flat system updfcn=None, outfcn=None, # I/O system inputs=None, outputs=None, - states=None, params={}, dt=None, name=None): + states=None, params=None, dt=None, name=None): """Create a differentially flat I/O system. The FlatIOSystem constructor is used to create an input/output system @@ -155,13 +164,14 @@ def __init__(self, if reverse is not None: self.reverse = reverse # Save the length of the flat flag + # TODO: missing def __str__(self): return f"{NonlinearIOSystem.__str__(self)}\n\n" \ + f"Forward: {self.forward}\n" \ + f"Reverse: {self.reverse}" - def forward(self, x, u, params={}): + def forward(self, x, u, params=None): """Compute the flat flag given the states and input. @@ -190,7 +200,7 @@ def forward(self, x, u, params={}): """ raise NotImplementedError("internal error; forward method not defined") - def reverse(self, zflag, params={}): + def reverse(self, zflag, params=None): """Compute the states and input given the flat flag. Parameters @@ -214,18 +224,18 @@ def reverse(self, zflag, params={}): """ raise NotImplementedError("internal error; reverse method not defined") - def _flat_updfcn(self, t, x, u, params={}): + def _flat_updfcn(self, t, x, u, params=None): # TODO: implement state space update using flat coordinates raise NotImplementedError("update function for flat system not given") - def _flat_outfcn(self, t, x, u, params={}): + def _flat_outfcn(self, t, x, u, params=None): # Return the flat output zflag = self.forward(x, u, params) return np.array([zflag[i][0] for i in range(len(zflag))]) # Utility function to compute flag matrix given a basis -def _basis_flag_matrix(sys, basis, flag, t, params={}): +def _basis_flag_matrix(sys, basis, flag, t): """Compute the matrix of basis functions and their derivatives This function computes the matrix ``M`` that is used to solve for the @@ -233,17 +243,19 @@ def _basis_flag_matrix(sys, basis, flag, t, params={}): column of the matrix corresponds to a basis function and each row is a derivative, with the derivatives (flag) for each output stacked on top of each other. - +l """ flagshape = [len(f) for f in flag] - M = np.zeros((sum(flagshape), basis.N * sys.ninputs)) + M = np.zeros((sum(flagshape), + sum([basis.var_ncoefs(i) for i in range(sys.ninputs)]))) flag_off = 0 - coeff_off = 0 + coef_off = 0 for i, flag_len in enumerate(flagshape): - for j, k in itertools.product(range(basis.N), range(flag_len)): - M[flag_off + k, coeff_off + j] = basis.eval_deriv(j, k, t) + coef_len = basis.var_ncoefs(i) + for j, k in itertools.product(range(coef_len), range(flag_len)): + M[flag_off + k, coef_off + j] = basis.eval_deriv(j, k, t, var=i) flag_off += flag_len - coeff_off += basis.N + coef_off += coef_len return M @@ -362,11 +374,16 @@ def point_to_point( if basis is None: basis = PolyFamily(2 * (sys.nstates + sys.ninputs)) + # If a multivariable basis was given, make sure the size is correct + if basis.nvars is not None and basis.nvars != sys.ninputs: + raise ValueError("size of basis does not match flat system size") + # Make sure we have enough basis functions to solve the problem - if basis.N * sys.ninputs < 2 * (sys.nstates + sys.ninputs): + ncoefs = sum([basis.var_ncoefs(i) for i in range(sys.ninputs)]) + if ncoefs < 2 * (sys.nstates + sys.ninputs): raise ValueError("basis set is too small") elif (cost is not None or trajectory_constraints is not None) and \ - basis.N * sys.ninputs == 2 * (sys.nstates + sys.ninputs): + ncoefs == 2 * (sys.nstates + sys.ninputs): warnings.warn("minimal basis specified; optimization not possible") cost = None trajectory_constraints = None @@ -403,7 +420,7 @@ def point_to_point( # Solve for the coefficients of the flat outputs # # At this point, we need to solve the equation M alpha = zflag, where M - # is the matrix constrains for initial and final conditions and zflag = + # is the matrix constraints for initial and final conditions and zflag = # [zflag_T0; zflag_tf]. # # If there are no constraints, then we just need to solve a linear @@ -413,12 +430,20 @@ def point_to_point( # # Start by solving the least squares problem + # TODO: add warning if rank is too small alpha, residuals, rank, s = np.linalg.lstsq(M, Z, rcond=None) + if rank < Z.size: + warnings.warn("basis too small; solution may not exist") if cost is not None or trajectory_constraints is not None: # Search over the null space to minimize cost/satisfy constraints N = sp.linalg.null_space(M) + # Precompute the collocation matrix the defines the flag at timepts + Mt_list = [] + for t in timepts[1:-1]: + Mt_list.append(_basis_flag_matrix(sys, basis, zflag_T0, t)) + # Define a function to evaluate the cost along a trajectory def traj_cost(null_coeffs): # Add this to the existing solution @@ -426,8 +451,8 @@ def traj_cost(null_coeffs): # Evaluate the costs at the listed time points costval = 0 - for t in timepts: - M_t = _basis_flag_matrix(sys, basis, zflag_T0, t) + for i, t in enumerate(timepts[1:-1]): + M_t = Mt_list[i] # Compute flag at this time point zflag = (M_t @ coeffs).reshape(sys.ninputs, -1) @@ -436,7 +461,7 @@ def traj_cost(null_coeffs): x, u = sys.reverse(zflag, params) # Evaluate the cost at this time point - costval += cost(x, u) + costval += cost(x, u) * (timepts[i+1] - timepts[i]) return costval # If no cost given, override with magnitude of the coefficients @@ -463,9 +488,9 @@ def traj_const(null_coeffs): # Evaluate the constraints at the listed time points values = [] - for i, t in enumerate(timepts): + for i, t in enumerate(timepts[1:-1]): # Calculate the states and inputs for the flat output - M_t = _basis_flag_matrix(sys, basis, zflag_T0, t) + M_t = Mt_list[i] # Compute flag at this time point zflag = (M_t @ coeffs).reshape(sys.ninputs, -1) @@ -487,7 +512,7 @@ def traj_const(null_coeffs): # Store upper and lower bounds const_lb, const_ub = [], [] - for t in timepts: + for t in timepts[1:-1]: for type, fun, lb, ub in traj_constraints: const_lb.append(lb) const_ub.append(ub) @@ -498,9 +523,6 @@ def traj_const(null_coeffs): minimize_constraints = [sp.optimize.NonlinearConstraint( traj_const, const_lb, const_ub)] - # Add initial and terminal constraints - # minimize_constraints += [sp.optimize.LinearConstraint(M, Z, Z)] - # Process the initial condition if initial_guess is None: initial_guess = np.zeros(M.shape[1] - M.shape[0]) @@ -511,12 +533,349 @@ def traj_const(null_coeffs): res = sp.optimize.minimize( traj_cost, initial_guess, constraints=minimize_constraints, **minimize_kwargs) - if res.success: - alpha += N @ res.x - else: - raise RuntimeError( - "Unable to solve optimal control problem\n" + - "scipy.optimize.minimize returned " + res.message) + alpha += N @ res.x + + # See if we got an answer + if not res.success: + warnings.warn( + "unable to solve optimal control problem\n" + f"scipy.optimize.minimize: '{res.message}'", UserWarning) + + # + # Transform the trajectory from flat outputs to states and inputs + # + + # Create a trajectory object to store the result + systraj = SystemTrajectory(sys, basis, params=params) + if cost is not None or trajectory_constraints is not None: + # Store the result of the optimization + systraj.cost = res.fun + systraj.success = res.success + systraj.message = res.message + + # Store the flag lengths and coefficients + # TODO: make this more pythonic + coef_off = 0 + for i in range(sys.ninputs): + # Grab the coefficients corresponding to this flat output + coef_len = basis.var_ncoefs(i) + systraj.coeffs.append(alpha[coef_off:coef_off + coef_len]) + coef_off += coef_len + + # Keep track of the length of the flat flag for this output + systraj.flaglen.append(len(zflag_T0[i])) + + # Return a function that computes inputs and states as a function of time + return systraj + + +# Solve a point to point trajectory generation problem for a flat system +def solve_flat_ocp( + sys, timepts, x0=0, u0=0, trajectory_cost=None, basis=None, + terminal_cost=None, trajectory_constraints=None, + initial_guess=None, params=None, **kwargs): + """Compute trajectory between an initial and final conditions. + + Compute an optimial trajectory for a differentially flat system starting + from an initial state and input value. + + Parameters + ---------- + flatsys : FlatSystem object + Description of the differentially flat system. This object must + define a function `flatsys.forward()` that takes the system state and + produceds the flag of flat outputs and a system `flatsys.reverse()` + that takes the flag of the flat output and prodes the state and + input. + + timepts : float or 1D array_like + The list of points for evaluating cost and constraints, as well as + the time horizon. If given as a float, indicates the final time for + the trajectory (corresponding to xf) + + x0, u0 : 1D arrays + Define the initial conditions for the system. If either of the + values are given as None, they are replaced by a vector of zeros of + the appropriate dimension. + + basis : :class:`~control.flatsys.BasisFamily` object, optional + The basis functions to use for generating the trajectory. If not + specified, the :class:`~control.flatsys.PolyFamily` basis family + will be used, with the minimal number of elements required to find a + feasible trajectory (twice the number of system states) + + trajectory_cost : callable + Function that returns the integral cost given the current state + and input. Called as `cost(x, u)`. + + terminal_cost : callable + Function that returns the terminal cost given the state and input. + Called as `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 :class:`scipy.optimize.LinearConstraint` or + :class:`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 time point along the trajectory. + + initial_guess : 2D array_like, optional + Initial guess for the optimal trajectory of the flat outputs. + + minimize_kwargs : str, optional + Pass additional keywords to :func:`scipy.optimize.minimize`. + + Returns + ------- + traj : :class:`~control.flatsys.SystemTrajectory` object + The system trajectory is returned as an object that implements the + `eval()` function, we can be used to compute the value of the state + and input and a given time t. + + Notes + ----- + 1. Additional keyword parameters can be used to fine tune the behavior + of the underlying optimization function. See `minimize_*` keywords + in :func:`~control.optimal.OptimalControlProblem` for more information. + + 2. The return data structure includes the following additional attributes: + * success : bool indicating whether the optimization succeeded + * cost : computed cost of the returned trajectory + * message : message returned by optimization if success if False + + 3. A common failure in solving optimal control problem is that the + default initial guess violates the constraints and the optimizer + can't find a feasible solution. Using the `initial_guess` parameter + can often be used to overcome these errors. + + """ + # + # Make sure the problem is one that we can handle + # + x0 = _check_convert_array(x0, [(sys.nstates,), (sys.nstates, 1)], + 'Initial state: ', squeeze=True) + u0 = _check_convert_array(u0, [(sys.ninputs,), (sys.ninputs, 1)], + 'Initial input: ', squeeze=True) + + # Process final time + timepts = np.atleast_1d(timepts) + Tf = timepts[-1] + T0 = timepts[0] if len(timepts) > 1 else T0 + + # Process keyword arguments + if trajectory_constraints is None: + # Backwards compatibility + trajectory_constraints = kwargs.pop('constraints', None) + if trajectory_cost is None: + # Compatibility with point_to_point + trajectory_cost = kwargs.pop('cost', None) + + minimize_kwargs = {} + minimize_kwargs['method'] = kwargs.pop('minimize_method', None) + minimize_kwargs['options'] = kwargs.pop('minimize_options', {}) + minimize_kwargs.update(kwargs.pop('minimize_kwargs', {})) + + if trajectory_cost is None and terminal_cost is None: + raise TypeError("need trajectory and/or terminal cost required") + + if kwargs: + raise TypeError("unrecognized keywords: ", str(kwargs)) + + # + # Determine the basis function set to use and make sure it is big enough + # + + # If no basis set was specified, use a polynomial basis (poor choice...) + if basis is None: + basis = PolyFamily(2 * (sys.nstates + sys.ninputs)) + + # If a multivariable basis was given, make sure the size is correct + if basis.nvars is not None and basis.nvars != sys.ninputs: + raise ValueError("size of basis does not match flat system size") + + # Make sure we have enough basis functions to solve the problem + ncoefs = sum([basis.var_ncoefs(i) for i in range(sys.ninputs)]) + if ncoefs <= sys.nstates + sys.ninputs: + raise ValueError("basis set is too small") + + # Figure out the parameters to use, if any + params = sys.params if params is None else params + + # + # Map the initial and conditions to flat output conditions + # + # We need to compute the output "flag": [z(t), z'(t), z''(t), ...] + # and then evaluate this at the initial and final condition. + # + + zflag_T0 = sys.forward(x0, u0, params) + Z_T0 = np.hstack(zflag_T0) + + # + # Compute the matrix constraints for initial conditions + # + # This computation depends on the basis function we are using. It + # essentially amounts to evaluating the basis functions and their + # derivatives at the initial conditions. + + # Compute the flag for the initial state + M_T0 = _basis_flag_matrix(sys, basis, zflag_T0, T0) + + # + # Solve for the coefficients of the flat outputs + # + # At this point, we need to solve the equation M_T0 alpha = zflag_T0. + # + # If there are no additional constraints, then we just need to solve a + # linear system of equations => use least squares. Otherwise, we have a + # nonlinear optimal control problem with equality constraints => use + # scipy.optimize.minimize(). + # + + # Start by solving the least squares problem + alpha, residuals, rank, s = np.linalg.lstsq(M_T0, Z_T0, rcond=None) + if rank < Z_T0.size: + warnings.warn("basis too small; solution may not exist") + + # Precompute the collocation matrix the defines the flag at timepts + # TODO: only compute if we have trajectory cost/constraints + Mt_list = [] + for t in timepts: + Mt_list.append(_basis_flag_matrix(sys, basis, zflag_T0, t)) + + # Search over the null space to minimize cost/satisfy constraints + N = sp.linalg.null_space(M_T0) + + # Define a function to evaluate the cost along a trajectory + def traj_cost(null_coeffs): + # Add this to the existing solution + coeffs = alpha + N @ null_coeffs + costval = 0 + + # Evaluate the trajectory costs at the listed time points + if trajectory_cost is not None: + for i, t in enumerate(timepts[0:-1]): + M_t = Mt_list[i] + + # Compute flag at this time point + zflag = (M_t @ coeffs).reshape(sys.ninputs, -1) + + # Find states and inputs at the time points + x, u = sys.reverse(zflag, params) + + # Evaluate the cost at this time point + # TODO: make use of time interval + costval += trajectory_cost(x, u) * (timepts[i+1] - timepts[i]) + + # Evaluate the terminal_cost + if terminal_cost is not None: + M_t = Mt_list[-1] + zflag = (M_t @ coeffs).reshape(sys.ninputs, -1) + x, u = sys.reverse(zflag, params) + costval += terminal_cost(x, u) + + return costval + + # Process the constraints we were given + traj_constraints = trajectory_constraints + if traj_constraints is None: + traj_constraints = [] + elif isinstance(traj_constraints, tuple): + # TODO: Check to make sure this is really a constraint + traj_constraints = [traj_constraints] + elif not isinstance(traj_constraints, list): + raise TypeError("trajectory constraints must be a list") + + # Process constraints + minimize_constraints = [] + if len(traj_constraints) > 0: + # Set up a nonlinear function to evaluate the constraints + def traj_const(null_coeffs): + # Add this to the existing solution + coeffs = alpha + N @ null_coeffs + + # Evaluate the constraints at the listed time points + values = [] + for i, t in enumerate(timepts): + # Calculate the states and inputs for the flat output + M_t = Mt_list[i] + + # Compute flag at this time point + zflag = (M_t @ coeffs).reshape(sys.ninputs, -1) + + # Find states and inputs at the time points + states, inputs = sys.reverse(zflag, params) + + # Evaluate the constraint function along the trajectory + for type, fun, lb, ub in traj_constraints: + if type == sp.optimize.LinearConstraint: + # `fun` is A matrix associated with polytope... + values.append(fun @ np.hstack([states, inputs])) + elif type == sp.optimize.NonlinearConstraint: + values.append(fun(states, inputs)) + else: + raise TypeError( + "unknown constraint type %s" % type) + return np.array(values).flatten() + + # Store upper and lower bounds + const_lb, const_ub = [], [] + for t in timepts: + for type, fun, lb, ub in traj_constraints: + const_lb.append(lb) + const_ub.append(ub) + const_lb = np.array(const_lb).flatten() + const_ub = np.array(const_ub).flatten() + + # Store the constraint as a nonlinear constraint + minimize_constraints = [sp.optimize.NonlinearConstraint( + traj_const, const_lb, const_ub)] + + # Add initial and terminal constraints + # minimize_constraints += [sp.optimize.LinearConstraint(M, Z, Z)] + + # Process the initial guess + if initial_guess is None: + initial_coefs = np.ones(M_T0.shape[1] - M_T0.shape[0]) + else: + # Compute the map from coefficients to flat outputs + initial_coefs = [] + for i in range(sys.ninputs): + M_z = np.array([ + basis.eval_deriv(j, 0, timepts, var=i) + for j in range(basis.var_ncoefs(i))]).transpose() + + # Compute the parameters that give the best least squares fit + coefs, _, _, _ = np.linalg.lstsq( + M_z, initial_guess[i], rcond=None) + initial_coefs.append(coefs) + initial_coefs = np.hstack(initial_coefs) + + # Project the parameters onto the independent variables + initial_coefs, _, _, _ = np.linalg.lstsq(N, initial_coefs, rcond=None) + + # Find the optimal solution + res = sp.optimize.minimize( + traj_cost, initial_coefs, constraints=minimize_constraints, + **minimize_kwargs) + alpha += N @ res.x + + # See if we got an answer + if not res.success: + warnings.warn( + "unable to solve optimal control problem\n" + f"scipy.optimize.minimize: '{res.message}'", UserWarning) # # Transform the trajectory from flat outputs to states and inputs @@ -524,14 +883,18 @@ def traj_const(null_coeffs): # Create a trajectory object to store the result systraj = SystemTrajectory(sys, basis, params=params) + systraj.cost = res.fun + systraj.success = res.success + systraj.message = res.message # Store the flag lengths and coefficients # TODO: make this more pythonic - coeff_off = 0 + coef_off = 0 for i in range(sys.ninputs): # Grab the coefficients corresponding to this flat output - systraj.coeffs.append(alpha[coeff_off:coeff_off + basis.N]) - coeff_off += basis.N + coef_len = basis.var_ncoefs(i) + systraj.coeffs.append(alpha[coef_off:coef_off + coef_len]) + coef_off += coef_len # Keep track of the length of the flat flag for this output systraj.flaglen.append(len(zflag_T0[i])) diff --git a/control/flatsys/linflat.py b/control/flatsys/linflat.py index e4a31c6de..8e6c23604 100644 --- a/control/flatsys/linflat.py +++ b/control/flatsys/linflat.py @@ -142,11 +142,11 @@ def reverse(self, zflag, params): return np.reshape(x, self.nstates), np.reshape(u, self.ninputs) # Update function - def _rhs(self, t, x, u, params={}): + def _rhs(self, t, x, u): # Use LinearIOSystem._rhs instead of default (MRO) NonlinearIOSystem return LinearIOSystem._rhs(self, t, x, u) # output function - def _out(self, t, x, u, params={}): + def _out(self, t, x, u): # Use LinearIOSystem._out instead of default (MRO) NonlinearIOSystem return LinearIOSystem._out(self, t, x, u) diff --git a/control/flatsys/poly.py b/control/flatsys/poly.py index 08dcfb1c9..f315091aa 100644 --- a/control/flatsys/poly.py +++ b/control/flatsys/poly.py @@ -47,15 +47,25 @@ class PolyFamily(BasisFamily): This class represents the family of polynomials of the form .. math:: - \phi_i(t) = t^i + \phi_i(t) = \left( \frac{t}{T} \right)^i + + Parameters + ---------- + N : int + Degree of the Bezier curve. + + T : float + Final time (used for rescaling). """ - def __init__(self, N): + def __init__(self, N, T=1): """Create a polynomial basis of order N.""" super(PolyFamily, self).__init__(N) + self.T = float(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): + def eval_deriv(self, i, k, t, var=None): """Evaluate the kth derivative of the ith basis function at time t.""" - if (i < k): return 0; # higher derivative than power - return factorial(i)/factorial(i-k) * np.power(t, i-k) + if (i < k): return 0 * t # higher derivative than power + return factorial(i)/factorial(i-k) * \ + np.power(t/self.T, i-k) / np.power(self.T, k) diff --git a/control/flatsys/systraj.py b/control/flatsys/systraj.py index 9d425295b..c9bde6d7a 100644 --- a/control/flatsys/systraj.py +++ b/control/flatsys/systraj.py @@ -106,11 +106,11 @@ def eval(self, tlist): for i in range(self.ninputs): flag_len = self.flaglen[i] zflag.append(np.zeros(flag_len)) - for j in range(self.basis.N): + for j in range(self.basis.var_ncoefs(i)): for k in range(flag_len): #! TODO: rewrite eval_deriv to take in time vector zflag[i][k] += self.coeffs[i][j] * \ - self.basis.eval_deriv(j, k, t) + self.basis.eval_deriv(j, k, t, var=i) # Now copy the states and inputs # TODO: revisit order of list arguments diff --git a/control/frdata.py b/control/frdata.py index a33775afb..c78607a07 100644 --- a/control/frdata.py +++ b/control/frdata.py @@ -408,10 +408,6 @@ def __truediv__(self, other): smooth=(self.ifunc is not None) and (other.ifunc is not None)) - # TODO: Remove when transition to python3 complete - def __div__(self, other): - return self.__truediv__(other) - # TODO: Division of MIMO transfer function objects is not written yet. def __rtruediv__(self, other): """Right divide two LTI objects.""" @@ -429,10 +425,6 @@ def __rtruediv__(self, other): return other / self - # TODO: Remove when transition to python3 complete - def __rdiv__(self, other): - return self.__rtruediv__(other) - def __pow__(self, other): if not type(other) == int: raise ValueError("Exponent must be an integer") diff --git a/control/freqplot.py b/control/freqplot.py index 05ae9da55..d34906855 100644 --- a/control/freqplot.py +++ b/control/freqplot.py @@ -589,8 +589,8 @@ def nyquist_plot( if `return_contour` is Tue. To obtain the Nyquist curve values, evaluate system(s) along contour. - Additional Parameters - --------------------- + Other Parameters + ---------------- arrows : int or 1D/2D array of floats, optional Specify the number of arrows to plot on the Nyquist curve. If an integer is passed. that number of equally spaced arrows will be diff --git a/control/iosys.py b/control/iosys.py index e3719614b..c9e2351ed 100644 --- a/control/iosys.py +++ b/control/iosys.py @@ -47,13 +47,7 @@ 'interconnect', 'summing_junction'] # Define module default parameter values -_iosys_defaults = { - 'iosys.state_name_delim': '_', - 'iosys.duplicate_system_name_prefix': '', - 'iosys.duplicate_system_name_suffix': '$copy', - 'iosys.linearized_system_name_prefix': '', - 'iosys.linearized_system_name_suffix': '$linearized' -} +_iosys_defaults = {} class InputOutputSystem(NamedIOSystem): @@ -126,7 +120,7 @@ class for a set of subclasses that are used to implement specific # Allow ndarray * InputOutputSystem to give IOSystem._rmul_() priority __array_priority__ = 12 # override ndarray, matrix, SS types - def __init__(self, params={}, **kwargs): + def __init__(self, params=None, **kwargs): """Create an input/output system. The InputOutputSystem constructor is used to create an input/output @@ -148,7 +142,7 @@ def __init__(self, params={}, **kwargs): states=states, name=name, dt=dt) # default parameters - self.params = params.copy() + self.params = {} if params is None else params.copy() def __mul__(sys2, sys1): """Multiply two input/output systems (series interconnection)""" @@ -352,12 +346,25 @@ def __neg__(sys): # Return the newly created system return newsys + def __truediv__(sys2, sys1): + """Division of input/output systems + + Only division by scalars and arrays of scalars is supported""" + # Note: order of arguments is flipped so that self = sys2, + # corresponding to the ordering convention of sys2 * sys1 + + if not isinstance(sys1, (LTI, NamedIOSystem)): + return sys2 * (1/sys1) + else: + return NotImplemented + + # Update parameters used for _rhs, _out (used by subclasses) def _update_params(self, params, warning=False): if warning: warn("Parameters passed to InputOutputSystem ignored.") - def _rhs(self, t, x, u, params={}): + def _rhs(self, t, x, u): """Evaluate right hand side of a differential or difference equation. Private function used to compute the right hand side of an @@ -366,26 +373,27 @@ def _rhs(self, t, x, u, params={}): you may want to use :meth:`dynamics`. """ - NotImplemented("Evaluation not implemented for system of type ", - type(self)) + raise NotImplementedError("Evaluation not implemented for system of type ", + type(self)) - def dynamics(self, t, x, u): + def dynamics(self, t, x, u, params=None): """Compute the dynamics of a differential or difference equation. Given time `t`, input `u` and state `x`, returns the value of the right hand side of the dynamical system. If the system is continuous, returns the time derivative - dx/dt = f(t, x, u) + dx/dt = f(t, x, u[, params]) where `f` is the system's (possibly nonlinear) dynamics function. If the system is discrete-time, returns the next value of `x`: - x[t+dt] = f(t, x[t], u[t]) + x[t+dt] = f(t, x[t], u[t][, params]) - Where `t` is a scalar. + where `t` is a scalar. - The inputs `x` and `u` must be of the correct length. + The inputs `x` and `u` must be of the correct length. The `params` + argument is an optional dictionary of parameter values. Parameters ---------- @@ -395,14 +403,17 @@ def dynamics(self, t, x, u): current state u : array_like input + params : dict (optional) + system parameter values Returns ------- dx/dt or x[t+dt] : ndarray """ + self._update_params(params) return self._rhs(t, x, u) - def _out(self, t, x, u, params={}): + def _out(self, t, x, u): """Evaluate the output of a system at a given state, input, and time Private function used to compute the output of of an input/output @@ -414,13 +425,13 @@ def _out(self, t, x, u, params={}): # If no output function was defined in subclass, return state return x - def output(self, t, x, u): + def output(self, t, x, u, params=None): """Compute the output of the system Given time `t`, input `u` and state `x`, returns the output of the system: - y = g(t, x, u) + y = g(t, x, u[, params]) The inputs `x` and `u` must be of the correct length. @@ -432,14 +443,17 @@ def output(self, t, x, u): current state u : array_like input + params : dict (optional) + system parameter values Returns ------- y : ndarray """ + self._update_params(params) return self._out(t, x, u) - def feedback(self, other=1, sign=-1, params={}): + def feedback(self, other=1, sign=-1, params=None): """Feedback interconnection between two input/output systems Parameters @@ -507,8 +521,8 @@ def feedback(self, other=1, sign=-1, params={}): # Return the newly created system return newsys - def linearize(self, x0, u0, t=0, params={}, eps=1e-6, - name=None, copy=False, **kwargs): + def linearize(self, x0, u0, t=0, params=None, eps=1e-6, + name=None, copy_names=False, **kwargs): """Linearize an input/output system at a given state and input. Return the linearization of an input/output system at a given state @@ -522,6 +536,10 @@ def linearize(self, x0, u0, t=0, params={}, eps=1e-6, # functions. # + # If x0 and u0 are specified as lists, concatenate the elements + x0 = _concatenate_list_elements(x0, 'x0') + u0 = _concatenate_list_elements(u0, 'u0') + # Figure out dimensions if they were not specified. nstates = _find_size(self.nstates, x0) ninputs = _find_size(self.ninputs, u0) @@ -564,25 +582,26 @@ def linearize(self, x0, u0, t=0, params={}, eps=1e-6, # Create the state space system linsys = LinearIOSystem( - StateSpace(A, B, C, D, self.dt, remove_useless_states=False), - name=name, **kwargs) + StateSpace(A, B, C, D, self.dt, remove_useless_states=False)) + + # Set the system name, inputs, outputs, and states + if 'copy' in kwargs: + copy_names = kwargs.pop('copy') + warn("keyword 'copy' is deprecated. please use 'copy_names'", + DeprecationWarning) - # Set the names the system, inputs, outputs, and states - if copy: + if copy_names: + linsys._copy_names(self) if name is None: linsys.name = \ - config.defaults['iosys.linearized_system_name_prefix'] + \ - self.name + \ - config.defaults['iosys.linearized_system_name_suffix'] - linsys.ninputs, linsys.input_index = self.ninputs, \ - self.input_index.copy() - linsys.noutputs, linsys.output_index = \ - self.noutputs, self.output_index.copy() - linsys.nstates, linsys.state_index = \ - self.nstates, self.state_index.copy() - - return linsys + config.defaults['namedio.linearized_system_name_prefix']+\ + linsys.name+\ + config.defaults['namedio.linearized_system_name_suffix'] + else: + linsys.name = name + # re-init to include desired signal names if names were provided + return LinearIOSystem(linsys, **kwargs) class LinearIOSystem(InputOutputSystem, StateSpace): """Input/output representation of a linear (state space) system. @@ -651,7 +670,7 @@ def __init__(self, linsys, **kwargs): # Note: don't use super() to override StateSpace MRO InputOutputSystem.__init__( self, inputs=inputs, outputs=outputs, states=states, - params={}, dt=dt, name=name) + params=None, dt=dt, name=name) # Initalize additional state space variables StateSpace.__init__( @@ -668,7 +687,7 @@ def __init__(self, linsys, **kwargs): #: number of states, use :attr:`nstates`. states = property(StateSpace._get_states, StateSpace._set_states) - def _update_params(self, params={}, warning=True): + def _update_params(self, params=None, warning=True): # Parameters not supported; issue a warning if params and warning: warn("Parameters passed to LinearIOSystems are ignored.") @@ -756,7 +775,7 @@ class NonlinearIOSystem(InputOutputSystem): defaults. """ - def __init__(self, updfcn, outfcn=None, params={}, **kwargs): + def __init__(self, updfcn, outfcn=None, params=None, **kwargs): """Create a nonlinear I/O system given update and output functions.""" # Process keyword arguments name, inputs, outputs, states, dt = _process_namedio_keywords( @@ -791,7 +810,7 @@ def __init__(self, updfcn, outfcn=None, params={}, **kwargs): "(and nstates not known).") # Initialize current parameters to default parameters - self._current_params = params.copy() + self._current_params = {} if params is None else params.copy() def __str__(self): return f"{InputOutputSystem.__str__(self)}\n\n" + \ @@ -838,7 +857,8 @@ def __call__(sys, u, params=None, squeeze=None): def _update_params(self, params, warning=False): # Update the current parameter values self._current_params = self.params.copy() - self._current_params.update(params) + if params: + self._current_params.update(params) def _rhs(self, t, x, u): xdot = self.updfcn(t, x, u, self._current_params) \ @@ -862,22 +882,27 @@ class InterconnectedSystem(InputOutputSystem): See :func:`~control.interconnect` for a list of parameters. """ - def __init__(self, syslist, connections=[], inplist=[], outlist=[], - params={}, warn_duplicate=None, **kwargs): + def __init__(self, syslist, connections=None, inplist=None, outlist=None, + params=None, warn_duplicate=None, **kwargs): """Create an I/O system from a list of systems + connection info.""" # Convert input and output names to lists if they aren't already - if not isinstance(inplist, (list, tuple)): + if inplist is not None and not isinstance(inplist, (list, tuple)): inplist = [inplist] - if not isinstance(outlist, (list, tuple)): + if outlist is not None and not isinstance(outlist, (list, tuple)): outlist = [outlist] - # Process keyword arguments - defaults = {'inputs': len(inplist), 'outputs': len(outlist)} - name, inputs, outputs, states, dt = _process_namedio_keywords( + # Check if dt argument was given; if not, pull from systems + dt = kwargs.pop('dt', None) + + # Process keyword arguments (except dt) + defaults = { + 'inputs': len(inplist or []), + 'outputs': len(outlist or [])} + name, inputs, outputs, states, _ = _process_namedio_keywords( kwargs, defaults, end=True) # Initialize the system list and index - self.syslist = syslist + self.syslist = list(syslist) # insure modifications can be made self.syslist_index = {} # Initialize the input, output, and state counts, indices @@ -890,7 +915,13 @@ def __init__(self, syslist, connections=[], inplist=[], outlist=[], sysname_count_dct = {} # Go through the system list and keep track of counts, offsets - for sysidx, sys in enumerate(syslist): + for sysidx, sys in enumerate(self.syslist): + # If we were passed a SS or TF system, convert to LinearIOSystem + if isinstance(sys, (StateSpace, TransferFunction)) and \ + not isinstance(sys, LinearIOSystem): + sys = LinearIOSystem(sys, name=sys.name) + self.syslist[sysidx] = sys + # Make sure time bases are consistent dt = common_timebase(dt, sys.dt) @@ -947,7 +978,7 @@ def __init__(self, syslist, connections=[], inplist=[], outlist=[], if states is None: states = [] - state_name_delim = config.defaults['iosys.state_name_delim'] + state_name_delim = config.defaults['namedio.state_name_delim'] for sys, sysname in sysobj_name_dct.items(): states += [sysname + state_name_delim + statename for statename in sys.state_index.keys()] @@ -966,7 +997,7 @@ def __init__(self, syslist, connections=[], inplist=[], outlist=[], # Convert the list of interconnections to a connection map (matrix) self.connect_map = np.zeros((ninputs, noutputs)) - for connection in connections: + for connection in connections or []: input_index = self._parse_input_spec(connection[0]) for output_spec in connection[1:]: output_index, gain = self._parse_output_spec(output_spec) @@ -977,7 +1008,7 @@ def __init__(self, syslist, connections=[], inplist=[], outlist=[], # Convert the input list to a matrix: maps system to subsystems self.input_map = np.zeros((ninputs, self.ninputs)) - for index, inpspec in enumerate(inplist): + for index, inpspec in enumerate(inplist or []): if isinstance(inpspec, (int, str, tuple)): inpspec = [inpspec] if not isinstance(inpspec, list): @@ -992,7 +1023,7 @@ def __init__(self, syslist, connections=[], inplist=[], outlist=[], # Convert the output list to a matrix: maps subsystems to system self.output_map = np.zeros((self.noutputs, noutputs + ninputs)) - for index, outspec in enumerate(outlist): + for index, outspec in enumerate(outlist or []): if isinstance(outspec, (int, str, tuple)): outspec = [outspec] if not isinstance(outspec, list): @@ -1006,13 +1037,14 @@ def __init__(self, syslist, connections=[], inplist=[], outlist=[], self.output_map[index, ylist_index] += gain # Save the parameters for the system - self.params = params.copy() + self.params = {} if params is None else params.copy() def _update_params(self, params, warning=False): for sys in self.syslist: local = sys.params.copy() # start with system parameters local.update(self.params) # update with global params - local.update(params) # update with locally passed parameters + if params: + local.update(params) # update with locally passed parameters sys._update_params(local, warning=warning) def _rhs(self, t, x, u): @@ -1562,9 +1594,9 @@ def __init__(self, io_sys, ss_sys=None): def input_output_response( - sys, T, U=0., X0=0, params={}, + sys, T, U=0., X0=0, params=None, transpose=False, return_x=False, squeeze=None, - solve_ivp_kwargs={}, t_eval='T', **kwargs): + solve_ivp_kwargs=None, t_eval='T', **kwargs): """Compute the output response of a system to a given input. Simulate a dynamical system with a given input and return its output @@ -1590,6 +1622,10 @@ def input_output_response( number of states in the system, the initial condition will be padded with zeros. + t_eval : array-list, optional + List of times at which the time response should be computed. + Defaults to ``T``. + return_x : bool, optional If True, return the state vector when assigning to a tuple (default = False). See :func:`forced_response` for more details. @@ -1630,7 +1666,7 @@ def input_output_response( solve_ivp_method : str, optional Set the method used by :func:`scipy.integrate.solve_ivp`. Defaults to 'RK45'. - solve_ivp_kwargs : str, optional + solve_ivp_kwargs : dict, optional Pass additional keywords to :func:`scipy.integrate.solve_ivp`. Raises @@ -1656,6 +1692,7 @@ def input_output_response( # # Figure out the method to be used + solve_ivp_kwargs = solve_ivp_kwargs.copy() if solve_ivp_kwargs else {} if kwargs.get('solve_ivp_method', None): if kwargs.get('method', None): raise ValueError("ivp_method specified more than once") @@ -1729,14 +1766,7 @@ def input_output_response( ninputs = U.shape[0] # If we were passed a list of initial states, concatenate them - if isinstance(X0, (tuple, list)): - X0_list = [] - for i, x0 in enumerate(X0): - x0 = np.array(x0).reshape(-1) # convert everyting to 1D array - X0_list += x0.tolist() # add elements to initial state - - # Save the newly created input vector - X0 = np.array(X0_list) + X0 = _concatenate_list_elements(X0, 'X0') # If the initial state is too short, make it longer (NB: sys.nstates # could be None if nstates comes from size of initial condition) @@ -1762,19 +1792,8 @@ def input_output_response( warn("initial state too short; padding with zeros") X0 = np.hstack([X0, np.zeros(sys.nstates - X0.size)]) - # Check to make sure this is not a static function + # Compute the number of states nstates = _find_size(sys.nstates, X0) - if nstates == 0: - # No states => map input to output - u = U[0] if len(U.shape) == 1 else U[:, 0] - y = np.zeros((np.shape(sys._out(T[0], X0, u))[0], len(T))) - for i in range(len(T)): - u = U[i] if len(U.shape) == 1 else U[:, i] - y[:, i] = sys._out(T[i], [], u) - return TimeResponseData( - T, y, None, U, issiso=sys.issiso(), - output_labels=sys.output_index, input_labels=sys.input_index, - transpose=transpose, return_x=return_x, squeeze=squeeze) # create X0 if not given, test if X0 has correct shape X0 = _check_convert_array(X0, [(nstates,), (nstates, 1)], @@ -1787,6 +1806,9 @@ def input_output_response( else: noutputs = sys.noutputs + # Update the parameter values + sys._update_params(params) + # # Define a function to evaluate the input at an arbitrary time # @@ -1824,9 +1846,6 @@ def ufun(t): output_labels=sys.output_index, input_labels=sys.input_index, transpose=transpose, return_x=return_x, squeeze=squeeze) - # Update the parameter values - sys._update_params(params) - # Create a lambda function for the right hand side def ivp_rhs(t, x): return sys._rhs(t, x, ufun(t)) @@ -1908,7 +1927,7 @@ def ivp_rhs(t, x): transpose=transpose, return_x=return_x, squeeze=squeeze) -def find_eqpt(sys, x0, u0=[], y0=None, t=0, params={}, +def find_eqpt(sys, x0, u0=None, y0=None, t=0, params=None, iu=None, iy=None, ix=None, idx=None, dx0=None, return_y=False, return_result=False): """Find the equilibrium point for an input/output system. @@ -1976,6 +1995,14 @@ def find_eqpt(sys, x0, u0=[], y0=None, t=0, params={}, If `return_result` is True, returns the `result` from the :func:`scipy.optimize.root` function. + Notes + ----- + For continuous time systems, equilibrium points are defined as points for + which the right hand side of the differential equation is zero: + :math:`f(t, x_e, u_e) = 0`. For discrete time systems, equilibrium points + are defined as points for which the right hand side of the difference + equation returns the current state: :math:`f(t, x_e, u_e) = x_e`. + """ from scipy.optimize import root @@ -1992,11 +2019,6 @@ def find_eqpt(sys, x0, u0=[], y0=None, t=0, params={}, if np.isscalar(y0): y0 = np.ones((ninputs,)) * y0 - # Discrete-time not yet supported - if isdtime(sys, strict=True): - raise NotImplementedError( - "Discrete time systems are not yet supported.") - # Make sure the input arguments match the sizes of the system if len(x0) != nstates or \ (u0 is not None and len(u0) != ninputs) or \ @@ -2012,18 +2034,28 @@ def find_eqpt(sys, x0, u0=[], y0=None, t=0, params={}, # Special cases: either inputs or outputs are constrained if y0 is None: # Take u0 as fixed and minimize over x - # TODO: update to allow discrete time systems - def ode_rhs(z): return sys._rhs(t, z, u0) - result = root(ode_rhs, x0) + if sys.isdtime(strict=True): + def state_rhs(z): return sys._rhs(t, z, u0) - z + else: + def state_rhs(z): return sys._rhs(t, z, u0) + + result = root(state_rhs, x0) z = (result.x, u0, sys._out(t, result.x, u0)) + else: # Take y0 as fixed and minimize over x and u - def rootfun(z): - # Split z into x and u - x, u = np.split(z, [nstates]) - # TODO: update to allow discrete time systems - return np.concatenate( - (sys._rhs(t, x, u), sys._out(t, x, u) - y0), axis=0) + if sys.isdtime(strict=True): + def rootfun(z): + x, u = np.split(z, [nstates]) + return np.concatenate( + (sys._rhs(t, x, u) - x, sys._out(t, x, u) - y0), + axis=0) + else: + def rootfun(z): + x, u = np.split(z, [nstates]) + return np.concatenate( + (sys._rhs(t, x, u), sys._out(t, x, u) - y0), axis=0) + z0 = np.concatenate((x0, u0), axis=0) # Put variables together result = root(rootfun, z0) # Find the eq point x, u = np.split(result.x, [nstates]) # Split result back in two @@ -2117,7 +2149,6 @@ def rootfun(z): # Keep track of the number of states in the set of free variables nstate_vars = len(state_vars) - dtime = isdtime(sys, strict=True) def rootfun(z): # Map the vector of values into the states and inputs @@ -2126,12 +2157,17 @@ def rootfun(z): # Compute the update and output maps dx = sys._rhs(t, x, u) - dx0 - if dtime: - dx -= x # TODO: check - dy = sys._out(t, x, u) - y0 + if sys.isdtime(strict=True): + dx -= x - # Map the results into the constrained variables - return np.concatenate((dx[deriv_vars], dy[output_vars]), axis=0) + # If no y0 is given, don't evaluate the output function + if y0 is None: + return dx[deriv_vars] + else: + dy = sys._out(t, x, u) - y0 + + # Map the results into the constrained variables + return np.concatenate((dx[deriv_vars], dy[output_vars]), axis=0) # Set the initial condition for the root finding algorithm z0 = np.concatenate((x[state_vars], u[input_vars]), axis=0) @@ -2159,7 +2195,7 @@ def rootfun(z): # Linearize an input/output system -def linearize(sys, xeq, ueq=[], t=0, params={}, **kw): +def linearize(sys, xeq, ueq=None, t=0, params=None, **kw): """Linearize an input/output system at a given state and input. This function computes the linearization of an input/output system at a @@ -2182,19 +2218,17 @@ def linearize(sys, xeq, ueq=[], t=0, params={}, **kw): params : dict, optional Parameter values for the systems. Passed to the evaluation functions for the system as default values, overriding internal defaults. - copy : bool, Optional - If `copy` is True, copy the names of the input signals, output signals, - and states to the linearized system. If `name` is not specified, - the system name is set to the input system name with the string - '_linearized' appended. name : string, optional Set the name of the linearized system. If not specified and - if `copy` is `False`, a generic name is generated - with a unique integer id. If `copy` is `True`, the new system + if `copy_names` is `False`, a generic name is generated + with a unique integer id. If `copy_names` is `True`, the new system name is determined by adding the prefix and suffix strings in - config.defaults['iosys.linearized_system_name_prefix'] and - config.defaults['iosys.linearized_system_name_suffix'], with the + config.defaults['namedio.linearized_system_name_prefix'] and + config.defaults['namedio.linearized_system_name_suffix'], with the default being to add the suffix '$linearized'. + copy_names : bool, Optional + If True, Copy the names of the input signals, output signals, and + states to the linearized system. Returns ------- @@ -2202,8 +2236,8 @@ def linearize(sys, xeq, ueq=[], t=0, params={}, **kw): The linearization of the system, as a :class:`~control.LinearIOSystem` object (which is also a :class:`~control.StateSpace` object. - Additional Parameters - --------------------- + Other Parameters + ---------------- inputs : int, list of str or None, optional Description of the system inputs. If not specified, the origional system inputs are used. See :class:`InputOutputSystem` for more @@ -2212,7 +2246,6 @@ def linearize(sys, xeq, ueq=[], t=0, params={}, **kw): Description of the system outputs. Same format as `inputs`. states : int, list of str, or None, optional Description of the system states. Same format as `inputs`. - """ if not isinstance(sys, InputOutputSystem): raise TypeError("Can only linearize InputOutputSystem types") @@ -2240,7 +2273,7 @@ def _find_size(sysval, vecval): # Define a state space object that is an I/O system def ss(*args, **kwargs): - """ss(A, B, C, D[, dt]) + r"""ss(A, B, C, D[, dt]) Create a state space system. @@ -2250,7 +2283,7 @@ def ss(*args, **kwargs): Convert a linear system into space system form. Always creates a new system, even if sys is already a state space system. - ``ss(updfcn, outfucn)``` + ``ss(updfcn, outfcn)`` Create a nonlinear input/output system with update function ``updfcn`` and output function ``outfcn``. See :class:`NonlinearIOSystem` for more information. @@ -2260,25 +2293,24 @@ def ss(*args, **kwargs): output equations: .. math:: - \\dot x = A \\cdot x + B \\cdot u - y = C \\cdot x + D \\cdot u + dx/dt &= A x + B u \\ + y &= C x + D u ``ss(A, B, C, D, dt)`` Create a discrete-time state space system from the matrices of its state and output equations: .. math:: - x[k+1] = A \\cdot x[k] + B \\cdot u[k] - y[k] = C \\cdot x[k] + D \\cdot u[ki] + x[k+1] &= A x[k] + B u[k] \\ + y[k] &= C x[k] + D u[k] The matrices can be given as *array like* data types or strings. Everything that the constructor of :class:`numpy.matrix` accepts is permissible here too. - ``ss(args, inputs=['u1', ..., 'up'], outputs=['y1', ..., 'yq'], - states=['x1', ..., 'xn']) + ``ss(args, inputs=['u1', ..., 'up'], outputs=['y1', ..., 'yq'], states=['x1', ..., 'xn'])`` Create a system with named input, output, and state signals. Parameters @@ -2358,6 +2390,19 @@ def ss(*args, **kwargs): return sys +# Utility function to allow lists states, inputs +def _concatenate_list_elements(X, name='X'): + # If we were passed a list, concatenate the elements together + if isinstance(X, (tuple, list)): + X_list = [] + for i, x in enumerate(X): + x = np.array(x).reshape(-1) # convert everyting to 1D array + X_list += x.tolist() # add elements to initial state + return np.array(X_list) + + # Otherwise, do nothing + return X + def rss(states=1, outputs=1, inputs=1, strictly_proper=False, **kwargs): """Create a stable random state space object. @@ -2422,7 +2467,10 @@ def rss(states=1, outputs=1, inputs=1, strictly_proper=False, **kwargs): def drss(*args, **kwargs): - """Create a stable, discrete-time, random state space system + """ + drss([states, outputs, inputs, strictly_proper]) + + Create a stable, discrete-time, random state space system Create a stable *discrete time* random state space object. This function calls :func:`rss` using either the `dt` keyword provided by @@ -2460,7 +2508,7 @@ def ss2io(*args, **kwargs): # Convert a transfer function into an input/output system (wrapper) def tf2io(*args, **kwargs): - """tf2io(sys) + """tf2io(sys[, ...]) Convert a transfer function into an I/O system @@ -2532,9 +2580,9 @@ def tf2io(*args, **kwargs): # Function to create an interconnected system -def interconnect(syslist, connections=None, inplist=[], outlist=[], params={}, - check_unused=True, ignore_inputs=None, ignore_outputs=None, - warn_duplicate=None, **kwargs): +def interconnect(syslist, connections=None, inplist=None, outlist=None, + params=None, check_unused=True, ignore_inputs=None, + ignore_outputs=None, warn_duplicate=None, **kwargs): """Interconnect a set of input/output systems. This function creates a new system that is an interconnection of a set of @@ -2691,8 +2739,8 @@ def interconnect(syslist, connections=None, inplist=[], outlist=[], params={}, generated and if `True` then warnings are always generated. - Example - ------- + Examples + -------- >>> P = control.LinearIOSystem( >>> control.rss(2, 2, 2, strictly_proper=True), name='P') >>> C = control.LinearIOSystem(control.rss(2, 2, 2), name='C') @@ -2709,8 +2757,8 @@ def interconnect(syslist, connections=None, inplist=[], outlist=[], params={}, :func:`~control.summing_block` function and the ability to automatically interconnect signals with the same names: - >>> P = control.tf2io(control.tf(1, [1, 0]), inputs='u', outputs='y') - >>> C = control.tf2io(control.tf(10, [1, 1]), inputs='e', outputs='u') + >>> P = control.tf(1, [1, 0], inputs='u', outputs='y') + >>> C = control.tf(10, [1, 1], inputs='e', outputs='u') >>> sumblk = control.summing_junction(inputs=['r', '-y'], output='e') >>> T = control.interconnect([P, C, sumblk], inputs='r', outputs='y') @@ -2719,8 +2767,8 @@ def interconnect(syslist, connections=None, inplist=[], outlist=[], params={}, If a system is duplicated in the list of systems to be connected, a warning is generated and a copy of the system is created with the name of the new system determined by adding the prefix and suffix - strings in config.defaults['iosys.linearized_system_name_prefix'] - and config.defaults['iosys.linearized_system_name_suffix'], with the + strings in config.defaults['namedio.linearized_system_name_prefix'] + and config.defaults['namedio.linearized_system_name_suffix'], with the default being to add the suffix '$copy'$ to the system name. It is possible to replace lists in most of arguments with tuples instead, @@ -2776,10 +2824,10 @@ def interconnect(syslist, connections=None, inplist=[], outlist=[], params={}, connections = [] # If inplist/outlist is not present, try using inputs/outputs instead - if not inplist and inputs is not None: - inplist = list(inputs) - if not outlist and outputs is not None: - outlist = list(outputs) + if inplist is None: + inplist = list(inputs or []) + if outlist is None: + outlist = list(outputs or []) # Process input list if not isinstance(inplist, (list, tuple)): @@ -2842,12 +2890,12 @@ def interconnect(syslist, connections=None, inplist=[], outlist=[], params={}, inputs=inputs, outputs=outputs, states=states, params=params, dt=dt, name=name, warn_duplicate=warn_duplicate) - # check for implicity dropped signals + # check for implicitly dropped signals if check_unused: newsys.check_unused_signals(ignore_inputs, ignore_outputs) # If all subsystems are linear systems, maintain linear structure - if all([isinstance(sys, LinearIOSystem) for sys in syslist]): + if all([isinstance(sys, LinearIOSystem) for sys in newsys.syslist]): return LinearICSystem(newsys, None) return newsys @@ -2892,8 +2940,8 @@ def summing_junction( Linear input/output system object with no states and only a direct term that implements the summing junction. - Example - ------- + Examples + -------- >>> P = control.tf2io(ct.tf(1, [1, 0]), inputs='u', outputs='y') >>> C = control.tf2io(ct.tf(10, [1, 1]), inputs='e', outputs='u') >>> sumblk = control.summing_junction(inputs=['r', '-y'], output='e') diff --git a/control/lti.py b/control/lti.py index fdb4946cd..1bc08229d 100644 --- a/control/lti.py +++ b/control/lti.py @@ -13,6 +13,7 @@ """ import numpy as np + from numpy import absolute, real, angle, abs from warnings import warn from . import config @@ -21,6 +22,7 @@ __all__ = ['poles', 'zeros', 'damp', 'evalfr', 'frequency_response', 'freqresp', 'dcgain', 'pole', 'zero'] + class LTI(NamedIOSystem): """LTI is a parent class to linear time-invariant (LTI) system objects. @@ -44,6 +46,7 @@ class LTI(NamedIOSystem): Note: dt processing has been moved to the NamedIOSystem class. """ + def __init__(self, inputs=1, outputs=1, states=None, name=None, **kwargs): """Assign the LTI object's numbers of inputs and ouputs.""" super().__init__( @@ -71,8 +74,7 @@ def _set_inputs(self, value): #: Deprecated inputs = property( - _get_inputs, _set_inputs, doc= - """ + _get_inputs, _set_inputs, doc=""" Deprecated attribute; use :attr:`ninputs` instead. The ``inputs`` attribute was used to store the number of system @@ -94,8 +96,7 @@ def _set_outputs(self, value): #: Deprecated outputs = property( - _get_outputs, _set_outputs, doc= - """ + _get_outputs, _set_outputs, doc=""" Deprecated attribute; use :attr:`noutputs` instead. The ``outputs`` attribute was used to store the number of system @@ -201,6 +202,11 @@ def _dcgain(self, warn_infinite): else: return zeroresp + def ispassive(self): + # importing here prevents circular dependancy + from control.passivity import ispassive + return ispassive(self) + # # Deprecated functions # @@ -298,8 +304,12 @@ def damp(sys, doprint=True): poles: array Pole locations - Algorithm - --------- + See Also + -------- + pole + + Notes + ----- If the system is continuous, wn = abs(poles) Z = -real(poles)/poles. @@ -314,14 +324,11 @@ def damp(sys, doprint=True): wn = abs(s) Z = -real(s)/wn. - See Also - -------- - pole """ wn, damping, poles = sys.damp() if doprint: print('_____Eigenvalue______ Damping___ Frequency_') - for p, d, w in zip(poles, damping, wn) : + for p, d, w in zip(poles, damping, wn): if abs(p.imag) < 1e-12: print("%10.4g %10.4g %10.4g" % (p.real, 1.0, -p.real)) @@ -330,6 +337,7 @@ def damp(sys, doprint=True): (p.real, p.imag, d, w)) return wn, damping, poles + def evalfr(sys, x, squeeze=None): """Evaluate the transfer function of an LTI system for complex frequency x. @@ -388,6 +396,7 @@ def evalfr(sys, x, squeeze=None): """ return sys.__call__(x, squeeze=squeeze) + def frequency_response(sys, omega, squeeze=None): """Frequency response of an LTI system at multiple angular frequencies. diff --git a/control/mateqn.py b/control/mateqn.py index 23ae1e64e..1cf2e65d9 100644 --- a/control/mateqn.py +++ b/control/mateqn.py @@ -88,7 +88,9 @@ def sb03md(n, C, A, U, dico, job='X', fact='N', trana='N', ldwork=None): def lyap(A, Q, C=None, E=None, method=None): - """X = lyap(A, Q) solves the continuous-time Lyapunov equation + """Solves the continuous-time Lyapunov equation + + X = lyap(A, Q) solves :math:`A X + X A^T + Q = 0` @@ -217,7 +219,9 @@ def lyap(A, Q, C=None, E=None, method=None): def dlyap(A, Q, C=None, E=None, method=None): - """dlyap(A, Q) solves the discrete-time Lyapunov equation + """Solves the discrete-time Lyapunov equation + + X = dlyap(A, Q) solves :math:`A X A^T - X + Q = 0` @@ -348,8 +352,9 @@ def dlyap(A, Q, C=None, E=None, method=None): def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None, A_s="A", B_s="B", Q_s="Q", R_s="R", S_s="S", E_s="E"): - """X, L, G = care(A, B, Q, R=None) solves the continuous-time - algebraic Riccati equation + """Solves the continuous-time algebraic Riccati equation + + X, L, G = care(A, B, Q, R=None) solves :math:`A^T X + X A - X B R^{-1} B^T X + Q = 0` @@ -505,9 +510,11 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None, def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None, A_s="A", B_s="B", Q_s="Q", R_s="R", S_s="S", E_s="E"): - """X, L, G = dare(A, B, Q, R) solves the discrete-time algebraic Riccati + """Solves the discrete-time algebraic Riccati equation + X, L, G = dare(A, B, Q, R) solves + :math:`A^T X A - X - A^T X B (B^T X B + R)^{-1} B^T X A + Q = 0` where A and Q are square matrices of the same dimension. Further, Q diff --git a/control/matlab/__init__.py b/control/matlab/__init__.py index 80f2a0a65..1a524b33f 100644 --- a/control/matlab/__init__.py +++ b/control/matlab/__init__.py @@ -84,6 +84,7 @@ from ..rlocus import rlocus from ..dtime import c2d from ..sisotool import sisotool +from ..stochsys import lqe, dlqe # Functions that are renamed in MATLAB pole, zero = poles, zeros @@ -114,7 +115,7 @@ == ========================== ============================================ \* :func:`tf` create transfer function (TF) models -\ zpk create zero/pole/gain (ZPK) models. +\* :func:`zpk` create zero/pole/gain (ZPK) models. \* :func:`ss` create state-space (SS) models \ dss create descriptor state-space models \ delayss create state-space models with delayed terms diff --git a/control/matlab/timeresp.py b/control/matlab/timeresp.py index b1fa24bb0..58b5e589d 100644 --- a/control/matlab/timeresp.py +++ b/control/matlab/timeresp.py @@ -7,8 +7,7 @@ __all__ = ['step', 'stepinfo', 'impulse', 'initial', 'lsim'] def step(sys, T=None, X0=0., input=0, output=None, return_x=False): - ''' - Step response of a linear system + '''Step response of a linear system If the system has multiple inputs or outputs (MIMO), one input has to be selected for the simulation. Optionally, one output may be @@ -20,19 +19,14 @@ def step(sys, T=None, X0=0., input=0, output=None, return_x=False): ---------- sys: StateSpace, or TransferFunction LTI system to simulate - T: array-like or number, optional Time vector, or simulation time duration if a number (time vector is autocomputed if not given) - X0: array-like or number, optional Initial condition (default = 0) - Numbers are converted to constant arrays with the correct shape. - input: int Index of the input that will be used in this simulation. - output: int If given, index of the output that is returned by this simulation. @@ -40,15 +34,12 @@ def step(sys, T=None, X0=0., input=0, output=None, return_x=False): ------- yout: array Response of the system - T: array Time values of the output - xout: array (if selected) Individual response of each x variable - See Also -------- lsim, initial, impulse @@ -67,8 +58,7 @@ def step(sys, T=None, X0=0., input=0, output=None, return_x=False): def stepinfo(sysdata, T=None, yfinal=None, SettlingTimeThreshold=0.02, RiseTimeLimits=(0.1, 0.9)): - """ - Step response characteristics (Rise time, Settling Time, Peak and others). + """Step response characteristics (Rise time, Settling Time, Peak and others) Parameters ---------- @@ -137,8 +127,7 @@ def stepinfo(sysdata, T=None, yfinal=None, SettlingTimeThreshold=0.02, return S def impulse(sys, T=None, X0=0., input=0, output=None, return_x=False): - ''' - Impulse response of a linear system + '''Impulse response of a linear system If the system has multiple inputs or outputs (MIMO), one input has to be selected for the simulation. Optionally, one output may be @@ -150,19 +139,15 @@ def impulse(sys, T=None, X0=0., input=0, output=None, return_x=False): ---------- sys: StateSpace, TransferFunction LTI system to simulate - T: array-like or number, optional Time vector, or simulation time duration if a number (time vector is autocomputed if not given) - X0: array-like or number, optional Initial condition (default = 0) Numbers are converted to constant arrays with the correct shape. - input: int Index of the input that will be used in this simulation. - output: int Index of the output that will be used in this simulation. @@ -170,10 +155,8 @@ def impulse(sys, T=None, X0=0., input=0, output=None, return_x=False): ------- yout: array Response of the system - T: array Time values of the output - xout: array (if selected) Individual response of each x variable @@ -193,8 +176,7 @@ def impulse(sys, T=None, X0=0., input=0, output=None, return_x=False): return (out[1], out[0], out[2]) if return_x else (out[1], out[0]) def initial(sys, T=None, X0=0., input=None, output=None, return_x=False): - ''' - Initial condition response of a linear system + '''Initial condition response of a linear system If the system has multiple outputs (?IMO), optionally, one output may be selected. If no selection is made for the output, all @@ -204,20 +186,16 @@ def initial(sys, T=None, X0=0., input=None, output=None, return_x=False): ---------- sys: StateSpace, or TransferFunction LTI system to simulate - T: array-like or number, optional Time vector, or simulation time duration if a number (time vector is autocomputed if not given) - X0: array-like object or number, optional Initial condition (default = 0) Numbers are converted to constant arrays with the correct shape. - input: int This input is ignored, but present for compatibility with step and impulse. - output: int If given, index of the output that is returned by this simulation. @@ -225,10 +203,8 @@ def initial(sys, T=None, X0=0., input=None, output=None, return_x=False): ------- yout: array Response of the system - T: array Time values of the output - xout: array (if selected) Individual response of each x variable @@ -250,8 +226,7 @@ def initial(sys, T=None, X0=0., input=None, output=None, return_x=False): def lsim(sys, U=0., T=None, X0=0.): - ''' - Simulate the output of a linear system. + '''Simulate the output of a linear system As a convenience for parameters `U`, `X0`: Numbers (scalars) are converted to constant arrays with the correct shape. @@ -261,16 +236,13 @@ def lsim(sys, U=0., T=None, X0=0.): ---------- sys: LTI (StateSpace, or TransferFunction) LTI system to simulate - U: array-like or number, optional Input array giving input at each time `T` (default = 0). If `U` is ``None`` or ``0``, a special algorithm is used. This special algorithm is faster than the general algorithm, which is used otherwise. - T: array-like, optional for discrete LTI `sys` Time steps at which the input is defined; values must be evenly spaced. - X0: array-like or number, optional Initial condition (default = 0). diff --git a/control/matlab/wrappers.py b/control/matlab/wrappers.py index 8eafdaad2..22ec95f39 100644 --- a/control/matlab/wrappers.py +++ b/control/matlab/wrappers.py @@ -117,7 +117,7 @@ def nyquist(*args, **kwargs): def _parse_freqplot_args(*args): """Parse arguments to frequency plot routines (bode, nyquist)""" syslist, plotstyle, omega, other = [], [], None, {} - i = 0; + i = 0 while i < len(args): # Check to see if this is a system of some sort if issys(args[i]): @@ -178,8 +178,7 @@ def ngrid(): def dcgain(*args): - ''' - Compute the gain of the system in steady state. + '''Compute the gain of the system in steady state The function takes either 1, 2, 3, or 4 parameters: diff --git a/control/modelsimp.py b/control/modelsimp.py index 432b76b96..b1c1ae31c 100644 --- a/control/modelsimp.py +++ b/control/modelsimp.py @@ -476,7 +476,7 @@ def markov(Y, U, m=None, transpose=False): # Make sure there is enough data to compute parameters if m > n: - warn.warning("Not enough data for requested number of parameters") + warnings.warn("Not enough data for requested number of parameters") # # Original algorithm (with mapping to standard order) diff --git a/control/namedio.py b/control/namedio.py index 254f310ff..a94d1a9f5 100644 --- a/control/namedio.py +++ b/control/namedio.py @@ -6,13 +6,24 @@ # and other similar classes to allow naming of signals. import numpy as np -from copy import copy +from copy import deepcopy from warnings import warn from . import config __all__ = ['issiso', 'timebase', 'common_timebase', 'timebaseEqual', 'isdtime', 'isctime'] - +# Define module default parameter values +_namedio_defaults = { + 'namedio.state_name_delim': '_', + 'namedio.duplicate_system_name_prefix': '', + 'namedio.duplicate_system_name_suffix': '$copy', + 'namedio.linearized_system_name_prefix': '', + 'namedio.linearized_system_name_suffix': '$linearized', + 'namedio.sampled_system_name_prefix': '', + 'namedio.sampled_system_name_suffix': '$sampled' +} + + class NamedIOSystem(object): def __init__( self, name=None, inputs=None, outputs=None, states=None, **kwargs): @@ -88,26 +99,37 @@ def __str__(self): def _find_signal(self, name, sigdict): return sigdict.get(name, None) + def _copy_names(self, sys): + """copy the signal and system name of sys. Name is given as a keyword + in case a specific name (e.g. append 'linearized') is desired. """ + self.name = sys.name + self.ninputs, self.input_index = \ + sys.ninputs, sys.input_index.copy() + self.noutputs, self.output_index = \ + sys.noutputs, sys.output_index.copy() + self.nstates, self.state_index = \ + sys.nstates, sys.state_index.copy() + def copy(self, name=None, use_prefix_suffix=True): """Make a copy of an input/output system A copy of the system is made, with a new name. The `name` keyword can be used to specify a specific name for the system. If no name is given and `use_prefix_suffix` is True, the name is constructed - by prepending config.defaults['iosys.duplicate_system_name_prefix'] - and appending config.defaults['iosys.duplicate_system_name_suffix']. + by prepending config.defaults['namedio.duplicate_system_name_prefix'] + and appending config.defaults['namedio.duplicate_system_name_suffix']. Otherwise, a generic system name of the form `sys[]` is used, where `` is based on an internal counter. """ # Create a copy of the system - newsys = copy(self) + newsys = deepcopy(self) # Update the system name if name is None and use_prefix_suffix: # Get the default prefix and suffix to use - dup_prefix = config.defaults['iosys.duplicate_system_name_prefix'] - dup_suffix = config.defaults['iosys.duplicate_system_name_suffix'] + dup_prefix = config.defaults['namedio.duplicate_system_name_prefix'] + dup_suffix = config.defaults['namedio.duplicate_system_name_suffix'] newsys.name = self._name_or_default( dup_prefix + self.name + dup_suffix) else: diff --git a/control/optimal.py b/control/optimal.py index da1bdcb8e..377a6972e 100644 --- a/control/optimal.py +++ b/control/optimal.py @@ -20,9 +20,8 @@ from .exception import ControlNotImplemented from .timeresp import TimeResponseData -__all__ = ['find_optimal_input'] - # Define module default parameter values +_optimal_trajectory_methods = {'shooting', 'collocation'} _optimal_defaults = { 'optimal.minimize_method': None, 'optimal.minimize_options': {}, @@ -51,27 +50,33 @@ class OptimalControlProblem(): 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 + trajectory_constraints : list of constraints, 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 constraints will be applied at each time - point along the trajectory. + vector. Each element of the list should be an object of type + :class:`~scipy.optimize.LinearConstraint` with arguments `(A, lb, + ub)` or :class:`~scipy.optimize.NonlinearConstraint` with arguments + `(fun, lb, ub)`. The constraints will be applied at each time 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. + trajectory_method : string, optional + Method to use for carrying out the optimization. Currently supported + methods are 'shooting' and 'collocation' (continuous time only). The + default value is 'shooting' for discrete time systems and + 'collocation' for continuous time systems + initial_guess : (tuple of) 1D or 2D array_like + Initial states and/or inputs to use as a guess for the optimal + trajectory. For shooting methods, an array of inputs for each time + point should be specified. For collocation methods, the initial + guess is either the input vector or a tuple consisting guesses for + the state and the input. Guess should either be a 2D vector of + shape (ninputs, ntimepts) 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). - Use ``logging.basicConfig`` to enable logging output (e.g., to a file). - kwargs : dict, optional - Additional parameters (passed to :func:`scipy.optimal.minimize`). + Use :py:func:`logging.basicConfig` to enable logging output + (e.g., to a file). Returns ------- @@ -79,8 +84,14 @@ class OptimalControlProblem(): Optimal control problem object, to be used in computing optimal controllers. - Additional parameters - --------------------- + Other Parameters + ---------------- + basis : BasisFamily, optional + Use the given set of basis functions for the inputs instead of + setting the value of the input at each point in the timepts vector. + terminal_constraints : list of constraints, optional + List of constraints that should hold at the terminal point in time, + in the same form as `trajectory_constraints`. solve_ivp_method : str, optional Set the method used by :func:`scipy.integrate.solve_ivp`. solve_ivp_kwargs : str, optional @@ -129,7 +140,7 @@ class OptimalControlProblem(): def __init__( self, sys, timepts, integral_cost, trajectory_constraints=[], terminal_cost=None, terminal_constraints=[], initial_guess=None, - basis=None, log=False, **kwargs): + trajectory_method=None, basis=None, log=False, **kwargs): """Set up an optimal control problem.""" # Save the basic information for use later self.system = sys @@ -139,6 +150,15 @@ def __init__( self.terminal_constraints = terminal_constraints self.basis = basis + # Keep track of what type of trajectory method we are using + if trajectory_method is None: + trajectory_method = 'collocation' if sys.isctime() else 'shooting' + elif trajectory_method not in _optimal_trajectory_methods: + raise NotImplementedError(f"Unkown method {method}") + + self.shooting = trajectory_method in {'shooting'} + self.collocation = trajectory_method in {'collocation'} + # Process keyword arguments self.solve_ivp_kwargs = {} self.solve_ivp_kwargs['method'] = kwargs.pop( @@ -154,35 +174,22 @@ def __init__( self.minimize_kwargs.update(kwargs.pop( 'minimize_kwargs', config.defaults['optimal.minimize_kwargs'])) + # Check to make sure arguments for discrete-time systems are OK + if sys.isdtime(strict=True): + if self.solve_ivp_kwargs['method'] is not None or \ + len(self.solve_ivp_kwargs) > 1: + raise TypeError( + "solve_ivp method, kwargs not allowed for" + " discrete time systems") + # Make sure there were no extraneous keywords if kwargs: raise TypeError("unrecognized keyword(s): ", str(kwargs)) - # 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 - - # Make sure that we recognize all of the constraint types - for ctype, fun, lb, ub in self.trajectory_constraints: - if not ctype in [opt.LinearConstraint, opt.NonlinearConstraint]: - raise TypeError(f"unknown constraint type {ctype}") - - # 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 - - # Make sure that we recognize all of the constraint types - for ctype, fun, lb, ub in self.terminal_constraints: - if not ctype in [opt.LinearConstraint, opt.NonlinearConstraint]: - raise TypeError(f"unknown constraint type {ctype}") + self.trajectory_constraints = _process_constraints( + trajectory_constraints, "trajectory") + self.terminal_constraints = _process_constraints( + terminal_constraints, "terminal") # # Compute and store constraints @@ -197,6 +204,10 @@ def __init__( # is consistent with the `_constraint_function` that is used at # evaluation time. # + # TODO: when using the collocation method, linear constraints on the + # states and inputs can potentially maintain their linear structure + # rather than being converted to nonlinear constraints. + # constraint_lb, constraint_ub, eqconst_value = [], [], [] # Go through each time point and stack the bounds @@ -226,26 +237,36 @@ def __init__( self.eqconst_value = np.hstack(eqconst_value) if eqconst_value else [] # Create the constraints (inequality and equality) + # TODO: for collocation method, keep track of linear vs nonlinear 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)) + if self.collocation: + # Add the collocation constraints + colloc_zeros = np.zeros(sys.nstates * self.timepts.size) + self.colloc_vals = np.zeros((sys.nstates, self.timepts.size)) + self.constraints.append(sp.optimize.NonlinearConstraint( + self._collocation_constraint, colloc_zeros, colloc_zeros)) + + # Initialize run-time statistics + self._reset_statistics(log) + # Process the initial guess self.initial_guess = self._process_initial_guess(initial_guess) - # Store states, input, used later to minimize re-computation + # 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") @@ -253,10 +274,14 @@ def __init__( # # 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: + # For collocation methods we are given the states and inputs at each + # time point and we use a trapezoidal approximation to compute the + # integral cost, then add on the terminal cost. + # + # For shooting methods, 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]) # @@ -268,41 +293,8 @@ def _cost_function(self, coeffs): 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 - # TODO: try calling solve_ivp directly for better speed? - _, _, states = ct.input_output_response( - self.system, self.timepts, inputs, x, return_x=True, - solve_ivp_kwargs=self.solve_ivp_kwargs, t_eval=self.timepts) - 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)) + # Compute the states and inputs + states, inputs = self._compute_states_inputs(coeffs) # Trajectory cost if ct.isctime(self.system): @@ -362,11 +354,12 @@ def _cost_function(self, coeffs): # 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 are 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. + # * If the upper and lower bound for the constraint are 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 @@ -375,52 +368,27 @@ def _cost_function(self, coeffs): # 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. + # For collocation methods, we can directly evaluate the constraints at + # the collocation points. # - # 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. + # For shooting methods, we do this by creating 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. # 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.timepts, inputs, x, return_x=True, - solve_ivp_kwargs=self.solve_ivp_kwargs, t_eval=self.timepts) - self.system_simulations += 1 - self.last_x = x - self.last_coeffs = coeffs - self.last_states = states + # Compute the states and inputs + states, inputs = self._compute_states_inputs(coeffs) + # # Evaluate the constraint function along the trajectory + # value = [] for i, t in enumerate(self.timepts): for ctype, fun, lb, ub in self.trajectory_constraints: @@ -442,9 +410,9 @@ def _constraint_function(self, coeffs): # Skip equality constraints continue elif ctype == opt.LinearConstraint: - value.append(fun @ np.hstack([states[:, i], inputs[:, i]])) + value.append(fun @ np.hstack([states[:, -1], inputs[:, -1]])) elif ctype == opt.NonlinearConstraint: - value.append(fun(states[:, i], inputs[:, i])) + value.append(fun(states[:, -1], inputs[:, -1])) else: # pragma: no cover # Checked above => we should never get here raise TypeError(f"unknown constraint type {ctype}") @@ -458,8 +426,7 @@ def _constraint_function(self, coeffs): "_constraint_function elapsed time: %g", stop_time - start_time) - # Debugging information - if self.log: + # Debugging information logging.debug( "constraint values\n" + str(value) + "\n" + "lb, ub =\n" + str(self.constraint_lb) + "\n" + @@ -473,38 +440,8 @@ def _eqconst_function(self, coeffs): 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.timepts, inputs, x, return_x=True, - solve_ivp_kwargs=self.solve_ivp_kwargs, t_eval=self.timepts) - 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)) + # Compute the states and inputs + states, inputs = self._compute_states_inputs(coeffs) # Evaluate the constraint function along the trajectory value = [] @@ -528,9 +465,9 @@ def _eqconst_function(self, coeffs): # Skip inequality constraints continue elif ctype == opt.LinearConstraint: - value.append(fun @ np.hstack([states[:, i], inputs[:, i]])) + value.append(fun @ np.hstack([states[:, -1], inputs[:, -1]])) elif ctype == opt.NonlinearConstraint: - value.append(fun(states[:, i], inputs[:, i])) + value.append(fun(states[:, -1], inputs[:, -1])) else: # pragma: no cover # Checked above => we should never get here raise TypeError("unknown constraint type {ctype}") @@ -543,8 +480,7 @@ def _eqconst_function(self, coeffs): logging.info( "_eqconst_function elapsed time: %g", stop_time - start_time) - # Debugging information - if self.log: + # Debugging information logging.debug( "eqconst values\n" + str(value) + "\n" + "desired =\n" + str(self.eqconst_value)) @@ -552,56 +488,117 @@ def _eqconst_function(self, coeffs): # Return the value of the constraint function return np.hstack(value) + def _collocation_constraint(self, coeffs): + # Compute the states and inputs + states, inputs = self._compute_states_inputs(coeffs) + + if self.system.isctime(): + # Compute the collocation constraints + for i, t in enumerate(self.timepts): + if i == 0: + # Initial condition constraint (self.x = initial point) + self.colloc_vals[:, 0] = states[:, 0] - self.x + fk = self.system._rhs( + self.timepts[0], states[:, 0], inputs[:, 0]) + continue + + # From M. Kelly, SIAM Review (2017), equation (3.2), i = k+1 + # x[k+1] - x[k] = 0.5 hk (f(x[k+1], u[k+1] + f(x[k], u[k])) + fkp1 = self.system._rhs(t, states[:, i], inputs[:, i]) + self.colloc_vals[:, i] = states[:, i] - states[:, i-1] - \ + 0.5 * (self.timepts[i] - self.timepts[i-1]) * (fkp1 + fk) + fk = fkp1 + else: + raise NotImplementedError( + "collocation not yet implemented for discrete time systems") + + # Return the value of the constraint function + return self.colloc_vals.reshape(-1) + # - # Initial guess + # Initial guess processing # # 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 + # The functions below are used to process the initial guess, which can + # either consist of an input only (for shooting methods) or an input + # and/or state trajectory (for collocaiton methods). + # + # Note: The initial input 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 initial_guess.ndim == 1: - # Broadcast inputs to entire time vector - try: - initial_guess = np.broadcast_to( - initial_guess.reshape(-1, 1), - (self.system.ninputs, self.timepts.size)) - except ValueError: - raise ValueError("initial guess is the wrong shape") - - elif initial_guess.shape != \ - (self.system.ninputs, self.timepts.size): - raise ValueError("initial guess is the wrong shape") + # Sort out the input guess and the state guess + if self.collocation and initial_guess is not None and \ + isinstance(initial_guess, tuple): + state_guess, input_guess = initial_guess + else: + state_guess, input_guess = None, initial_guess + + # Process the input guess + if input_guess is not None: + input_guess = self._broadcast_initial_guess( + input_guess, (self.system.ninputs, self.timepts.size)) # 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) + input_guess = self._inputs_to_coeffs(input_guess) + else: + input_guess = np.zeros( + self.system.ninputs * + (self.timepts.size if self.basis is None else self.basis.N)) + # Process the state guess + if self.collocation: + if state_guess is None: + # Run a simulation to get the initial guess + if self.basis: + inputs = self._coeffs_to_inputs(input_guess) + else: + inputs = input_guess.reshape(self.system.ninputs, -1) + state_guess = self._simulate_states( + np.zeros(self.system.nstates), inputs) + else: + state_guess = self._broadcast_initial_guess( + state_guess, (self.system.nstates, self.timepts.size)) + + # Reshape for use by scipy.optimize.minimize() + return np.hstack([ + input_guess.reshape(-1), state_guess.reshape(-1)]) + else: # Reshape for use by scipy.optimize.minimize() - return initial_guess.reshape(-1) + return input_guess.reshape(-1) + + # Utility function to broadcast an initial guess to the right shape + def _broadcast_initial_guess(self, initial_guess, shape): + # 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 initial_guess.ndim == 1: + # Broadcast inputs to entire time vector + try: + initial_guess = np.broadcast_to( + initial_guess.reshape(-1, 1), shape) + except ValueError: + raise ValueError("initial guess is the wrong shape") - # Default is zero - return np.zeros( - self.system.ninputs * - (self.timepts.size if self.basis is None else self.basis.N)) + elif initial_guess.shape != shape: + raise ValueError("initial guess is the wrong shape") + + return initial_guess # # Utility function to convert input vector to coefficient vector # - # Initially guesses from the user are passed as input vectors as a + # Initial 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 problem to - # find coefficients that match the input functions at the time points (as - # much as possible, if the problem is under-determined). + # 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 @@ -609,34 +606,37 @@ def _inputs_to_coeffs(self, inputs): return inputs # Solve least squares problems (M x = b) for coeffs on each input - coeffs = np.zeros((self.system.ninputs, self.basis.N)) + coeffs = [] for i in range(self.system.ninputs): # Set up the matrices to get inputs - M = np.zeros((self.timepts.size, self.basis.N)) + M = np.zeros((self.timepts.size, self.basis.var_ncoefs(i))) b = np.zeros(self.timepts.size) # Evaluate at each time point and for each basis function # TODO: vectorize for j, t in enumerate(self.timepts): - for k in range(self.basis.N): + for k in range(self.basis.var_ncoefs(i)): M[j, k] = self.basis(k, t) - b[j] = inputs[i, j] + 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 + coeffs.append(alpha) - return coeffs + return np.hstack(coeffs) # Utility function to convert coefficient vector to input vector def _coeffs_to_inputs(self, coeffs): # TODO: vectorize + # TODO: use BasisFamily eval() method (if more efficient)? inputs = np.zeros((self.system.ninputs, self.timepts.size)) - for i, t in enumerate(self.timepts): - 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 + offset = 0 + for i in range(self.system.ninputs): + length = self.basis.var_ncoefs(i) + for j, t in enumerate(self.timepts): + for k in range(length): + inputs[i, j] += coeffs[offset + k] * self.basis(k, t) + offset += length return inputs # @@ -674,13 +674,71 @@ def _print_statistics(self, reset=True): if reset: self._reset_statistics(self.log) + # + # Compute the states and inputs from the coefficient vector + # + # These internal functions return the states and inputs at the + # collocation points given the ceofficient (optimizer state) vector. + # They keep track of whether a shooting method is being used or not and + # simulate the dynamics if needed. + # + + # Compute the states and inputs from the coefficients + def _compute_states_inputs(self, coeffs): + # + # Compute out the states and inputs + # + if self.collocation: + # States are appended to end of (input) coefficients + states = coeffs[-self.system.nstates * self.timepts.size:].reshape( + self.system.nstates, -1) + coeffs = coeffs[:-self.system.nstates * self.timepts.size] + + # Compute input at time points + if self.basis: + inputs = self._coeffs_to_inputs(coeffs) + else: + inputs = coeffs.reshape((self.system.ninputs, -1)) + + if self.shooting: + # See if we already have a simulation for this condition + if np.array_equal(coeffs, self.last_coeffs) \ + and np.array_equal(self.x, self.last_x): + states = self.last_states + else: + states = self._simulate_states(self.x, inputs) + self.last_x = self.x + self.last_states = states + self.last_coeffs = coeffs + + return states, inputs + + # Simulate the system dynamis to retrieve the state + def _simulate_states(self, x0, inputs): + if self.log: + logging.debug( + "calling input_output_response from state\n" + str(x0)) + logging.debug("input =\n" + str(inputs)) + + # Simulate the system to get the state + _, _, states = ct.input_output_response( + self.system, self.timepts, inputs, x0, return_x=True, + solve_ivp_kwargs=self.solve_ivp_kwargs, t_eval=self.timepts) + self.system_simulations += 1 + + if self.log: + logging.debug( + "input_output_response returned states\n" + str(states)) + + return states + # # Optimal control computations # # Compute the optimal trajectory from the current state def compute_trajectory( - self, x, squeeze=None, transpose=None, return_states=None, + self, x, squeeze=None, transpose=None, return_states=True, initial_guess=None, print_summary=True, **kwargs): """Compute the optimal input at state x @@ -689,8 +747,7 @@ def compute_trajectory( 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). + If True (default), return the values of the state at each time. 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 @@ -837,7 +894,7 @@ class OptimalControlResult(sp.optimize.OptimizeResult): """ def __init__( - self, ocp, res, return_states=False, print_summary=False, + self, ocp, res, return_states=True, print_summary=False, transpose=None, squeeze=None): """Create a OptimalControlResult object""" @@ -848,14 +905,8 @@ def __init__( # 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 + # Parse the optimization variables into states and inputs + states, inputs = ocp._compute_states_inputs(res.x) # See if we got an answer if not res.success: @@ -871,15 +922,6 @@ def __init__( ocp._print_statistics() print("* Final cost:", self.cost) - if return_states and inputs.shape[1] == ocp.timepts.shape[0]: - # Simulate the system if we need the state back - _, _, states = ct.input_output_response( - ocp.system, ocp.timepts, inputs, ocp.x, return_x=True, - solve_ivp_kwargs=ocp.solve_ivp_kwargs, t_eval=ocp.timepts) - ocp.system_simulations += 1 - else: - states = None - # Process data as a time response (with "outputs" = inputs) response = TimeResponseData( ocp.timepts, inputs, states, issiso=ocp.system.issiso(), @@ -894,7 +936,8 @@ def __init__( def solve_ocp( sys, horizon, X0, cost, trajectory_constraints=None, terminal_cost=None, terminal_constraints=[], initial_guess=None, basis=None, squeeze=None, - transpose=None, return_states=False, log=False, **kwargs): + transpose=None, return_states=True, print_summary=True, log=False, + **kwargs): """Compute the solution to an optimal control problem @@ -948,8 +991,11 @@ def solve_ocp( log : bool, optional If `True`, turn on logging messages (using Python logging module). + print_summary : bool, optional + If `True` (default), print a short summary of the computation. + return_states : bool, optional - If True, return the values of the state at each time (default = False). + If True, return the values of the state at each time (default = True). squeeze : bool, optional If True and if the system has a single output, return the system @@ -961,9 +1007,6 @@ def solve_ocp( 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 @@ -987,7 +1030,7 @@ def solve_ocp( Notes ----- Additional keyword parameters can be used to fine tune the behavior of - the underlying optimization and integrations functions. See + the underlying optimization and integration functions. See :func:`OptimalControlProblem` for more information. """ @@ -1002,9 +1045,21 @@ def solve_ocp( # Process (legacy) method keyword if kwargs.get('method'): - if kwargs.get('minimize_method'): - raise ValueError("'minimize_method' specified more than once") - kwargs['minimize_method'] = kwargs.pop('method') + method = kwargs.pop('method') + if method not in optimal_methods: + if kwargs.get('minimize_method'): + raise ValueError("'minimize_method' specified more than once") + warnings.warn( + "'method' parameter is deprecated; assuming minimize_method", + DeprecationWarning) + kwargs['minimize_method'] = method + else: + if kwargs.get('trajectory_method'): + raise ValueError("'trajectory_method' specified more than once") + warnings.warn( + "'method' parameter is deprecated; assuming trajectory_method", + DeprecationWarning) + kwargs['trajectory_method'] = method # Set up the optimal control problem ocp = OptimalControlProblem( @@ -1014,7 +1069,8 @@ def solve_ocp( # Solve for the optimal input from the current state return ocp.compute_trajectory( - X0, squeeze=squeeze, transpose=transpose, return_states=return_states) + X0, squeeze=squeeze, transpose=transpose, print_summary=print_summary, + return_states=return_states) # Create a model predictive controller for an optimal control problem @@ -1387,3 +1443,45 @@ def _evaluate_output_range_constraint(x, u): # Return a nonlinear constraint object based on the polynomial return (opt.NonlinearConstraint, _evaluate_output_range_constraint, lb, ub) + +# +# Utility functions +# + +# +# Process trajectory constraints +# +# Constraints were originally specified as a tuple with the type of +# constraint followed by the arguments. However, they are now specified +# directly as SciPy constraint objects. +# +# The _process_constraints() function will covert everything to a consistent +# internal representation (currently a tuple with the constraint type as the +# first element. +# +def _process_constraints(clist, name): + if isinstance( + clist, (tuple, opt.LinearConstraint, opt.NonlinearConstraint)): + clist = [clist] + elif not isinstance(clist, list): + raise TypeError(f"{name} constraints must be a list") + + # Process individual list elements + constraint_list = [] + for constraint in clist: + if isinstance(constraint, tuple): + # Original style of constraint + ctype, fun, lb, ub = constraint + if not ctype in [opt.LinearConstraint, opt.NonlinearConstraint]: + raise TypeError(f"unknown {name} constraint type {ctype}") + constraint_list.append(constraint) + elif isinstance(constraint, opt.LinearConstraint): + constraint_list.append( + (opt.LinearConstraint, constraint.A, + constraint.lb, constraint.ub)) + elif isinstance(constraint, opt.NonlinearConstraint): + constraint_list.append( + (opt.NonlinearConstraint, constraint.fun, + constraint.lb, constraint.ub)) + + return constraint_list diff --git a/control/passivity.py b/control/passivity.py new file mode 100644 index 000000000..0f4104186 --- /dev/null +++ b/control/passivity.py @@ -0,0 +1,295 @@ +""" +Functions for passive control. + +Author: Mark Yeatman +Date: July 17, 2022 +""" + +import numpy as np +from control import statesp +from control.exception import ControlArgument, ControlDimension + +try: + import cvxopt as cvx +except ImportError: + cvx = None + + +__all__ = ["get_output_fb_index", "get_input_ff_index", "ispassive", + "solve_passivity_LMI"] + + +def solve_passivity_LMI(sys, rho=None, nu=None): + """Compute passivity indices and/or solves feasiblity via a LMI. + + Constructs a linear matrix inequality (LMI) such that if a solution exists + and the last element of the solution is positive, the system `sys` is + passive. Inputs of None for `rho` or `nu` indicate that the function should + solve for that index (they are mutually exclusive, they can't both be + None, otherwise you're trying to solve a nonconvex bilinear matrix + inequality.) The last element of the output `solution` is either the output or input + passivity index, for `rho` = None and `nu` = None respectively. + + The sources for the algorithm are: + + McCourt, Michael J., and Panos J. Antsaklis + "Demonstrating passivity and dissipativity using computational + methods." + + Nicholas Kottenstette and Panos J. Antsaklis + "Relationships Between Positive Real, Passive Dissipative, & Positive + Systems", equation 36. + + Parameters + ---------- + sys : LTI + System to be checked + rho : float or None + Output feedback passivity index + nu : float or None + Input feedforward passivity index + + Returns + ------- + solution : ndarray + The LMI solution + """ + if cvx is None: + raise ModuleNotFoundError("cvxopt required for passivity module") + + if sys.ninputs != sys.noutputs: + raise ControlDimension( + "The number of system inputs must be the same as the number of " + "system outputs.") + + if rho is None and nu is None: + raise ControlArgument("rho or nu must be given a numerical value.") + + sys = statesp._convert_to_statespace(sys) + + A = sys.A + B = sys.B + C = sys.C + D = sys.D + + # account for strictly proper systems + [_, m] = D.shape + [n, _] = A.shape + + def make_LMI_matrix(P, rho, nu, one): + q = sys.noutputs + Q = -rho*np.eye(q, q) + S = 1.0/2.0*(one+rho*nu)*np.eye(q) + R = -nu*np.eye(m) + if sys.isctime(): + off_diag = P@B - (C.T@S + C.T@Q@D) + return np.vstack(( + np.hstack((A.T @ P + P@A - C.T@Q@C, off_diag)), + np.hstack((off_diag.T, -(D.T@Q@D + D.T@S + S.T@D + R))) + )) + else: + off_diag = A.T@P@B - (C.T@S + C.T@Q@D) + return np.vstack(( + np.hstack((A.T @ P @ A - P - C.T@Q@C, off_diag)), + np.hstack((off_diag.T, B.T@P@B-(D.T@Q@D + D.T@S + S.T@D + R))) + )) + + def make_P_basis_matrices(n, rho, nu): + """Make list of matrix constraints for passivity LMI. + + Utility function to make basis matrices for a LMI from a + symmetric matrix P of size n by n representing a parametrized symbolic + matrix + """ + matrix_list = [] + for i in range(0, n): + for j in range(0, n): + if j <= i: + P = np.zeros((n, n)) + P[i, j] = 1 + P[j, i] = 1 + matrix_list.append(make_LMI_matrix(P, 0, 0, 0).flatten()) + zeros = 0.0*np.eye(n) + if rho is None: + matrix_list.append(make_LMI_matrix(zeros, 1, 0, 0).flatten()) + elif nu is None: + matrix_list.append(make_LMI_matrix(zeros, 0, 1, 0).flatten()) + return matrix_list + + + def P_pos_def_constraint(n): + """Make a list of matrix constraints for P >= 0. + + Utility function to make basis matrices for a LMI that ensures + parametrized symbolic matrix of size n by n is positive definite + """ + matrix_list = [] + for i in range(0, n): + for j in range(0, n): + if j <= i: + P = np.zeros((n, n)) + P[i, j] = -1 + P[j, i] = -1 + matrix_list.append(P.flatten()) + if rho is None or nu is None: + matrix_list.append(np.zeros((n, n)).flatten()) + return matrix_list + + n = sys.nstates + + # coefficents for passivity indices and feasibility matrix + sys_matrix_list = make_P_basis_matrices(n, rho, nu) + + # get constants for numerical values of rho and nu + sys_constants = list() + if rho is not None and nu is not None: + sys_constants = -make_LMI_matrix(np.zeros_like(A), rho, nu, 1.0) + elif rho is not None: + sys_constants = -make_LMI_matrix(np.zeros_like(A), rho, 0.0, 1.0) + elif nu is not None: + sys_constants = -make_LMI_matrix(np.zeros_like(A), 0.0, nu, 1.0) + + sys_coefficents = np.vstack(sys_matrix_list).T + + # LMI to ensure P is positive definite + P_matrix_list = P_pos_def_constraint(n) + P_coefficents = np.vstack(P_matrix_list).T + P_constants = np.zeros((n, n)) + + # cost function + number_of_opt_vars = int( + (n**2-n)/2 + n) + c = cvx.matrix(0.0, (number_of_opt_vars, 1)) + + #we're maximizing a passivity index, include it in the cost function + if rho is None or nu is None: + c = cvx.matrix(np.append(np.array(c), -1.0)) + + Gs = [cvx.matrix(sys_coefficents)] + [cvx.matrix(P_coefficents)] + hs = [cvx.matrix(sys_constants)] + [cvx.matrix(P_constants)] + + # crunch feasibility solution + cvx.solvers.options['show_progress'] = False + try: + sol = cvx.solvers.sdp(c, Gs=Gs, hs=hs) + return sol["x"] + + except ZeroDivisionError as e: + raise ValueError("The system is probably ill conditioned. " + "Consider perturbing the system matrices by a small amount." + ) from e + + + +def get_output_fb_index(sys): + """Return the output feedback passivity (OFP) index for the system. + + The OFP is the largest gain that can be placed in positive feedback + with a system such that the new interconnected system is passive. + + Parameters + ---------- + sys : LTI + System to be checked + + Returns + ------- + float + The OFP index + """ + sol = solve_passivity_LMI(sys, nu=0.0) + if sol is None: + raise RuntimeError("LMI passivity problem is infeasible") + else: + return sol[-1] + + +def get_input_ff_index(sys): + """Return the input feedforward passivity (IFP) index for the system. + + The IFP is the largest gain that can be placed in negative parallel + interconnection with a system such that the new interconnected system is + passive. + + Parameters + ---------- + sys : LTI + System to be checked. + + Returns + ------- + float + The IFP index + """ + sol = solve_passivity_LMI(sys, rho=0.0) + if sol is None: + raise RuntimeError("LMI passivity problem is infeasible") + else: + return sol[-1] + + +def get_relative_index(sys): + """Return the relative passivity index for the system. + + (not implemented yet) + """ + raise NotImplementedError("Relative passivity index not implemented") + + +def get_combined_io_index(sys): + """Return the combined I/O passivity index for the system. + + (not implemented yet) + """ + raise NotImplementedError("Combined I/O passivity index not implemented") + + +def get_directional_index(sys): + """Return the directional passivity index for the system. + + (not implemented yet) + """ + raise NotImplementedError("Directional passivity index not implemented") + + +def ispassive(sys, ofp_index=0, ifp_index=0): + r"""Indicate if a linear time invariant (LTI) system is passive. + + Checks if system is passive with the given output feedback (OFP) and input + feedforward (IFP) passivity indices. + + Parameters + ---------- + sys : LTI + System to be checked + ofp_index : float + Output feedback passivity index + ifp_index : float + Input feedforward passivity index + + Returns + ------- + bool + The system is passive. + + Notes + ----- + Querying if the system is passive in the sense of + + .. math:: V(x) >= 0 \land \dot{V}(x) <= y^T u + + is equivalent to the default case of `ofp_index` = 0 and `ifp_index` = 0. + Note that computing the `ofp_index` and `ifp_index` for a system, then + using both values simultaneously as inputs to this function is not + guaranteed to have an output of True (the system might not be passive with + both indices at the same time). + + For more details, see [1]_. + + References + ---------- + .. [1] McCourt, Michael J., and Panos J. Antsaklis + "Demonstrating passivity and dissipativity using computational + methods." + """ + return solve_passivity_LMI(sys, rho=ofp_index, nu=ifp_index) is not None diff --git a/control/rlocus.py b/control/rlocus.py index 9d531de94..53c5c9031 100644 --- a/control/rlocus.py +++ b/control/rlocus.py @@ -61,6 +61,7 @@ from .sisotool import _SisotoolUpdate from .grid import sgrid, zgrid from . import config +import warnings __all__ = ['root_locus', 'rlocus'] @@ -76,7 +77,7 @@ # Main function: compute a root locus diagram def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr=None, plot=True, print_gain=None, grid=None, ax=None, - **kwargs): + initial_gain=None, **kwargs): """Root locus plot @@ -88,8 +89,8 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, ---------- sys : LTI object Linear input/output systems (SISO only, for now). - kvect : list or ndarray, optional - List of gains to use in computing diagram. + kvect : array_like, optional + Gains to use in computing plot of closed-loop poles. xlim : tuple or list, optional Set limits of x axis, normally with tuple (see :doc:`matplotlib:api/axes_api`). @@ -107,13 +108,16 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, If True plot omega-damping grid. Default is False. ax : :class:`matplotlib.axes.Axes` Axes on which to create root locus plot + initial_gain : float, optional + Used by :func:`sisotool` to indicate initial gain. Returns ------- - rlist : ndarray - Computed root locations, given as a 2D array - klist : ndarray or list - Gains used. Same as klist keyword argument if provided. + roots : ndarray + Closed-loop root locations, arranged in which each row corresponds + to a gain in gains + gains : ndarray + Gains used. Same as kvect keyword argument if provided. Notes ----- @@ -125,7 +129,6 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, """ # Check to see if legacy 'Plot' keyword was used if 'Plot' in kwargs: - import warnings warnings.warn("'Plot' keyword is deprecated in root_locus; " "use 'plot'", FutureWarning) # Map 'Plot' keyword to 'plot' keyword @@ -133,7 +136,6 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, # Check to see if legacy 'PrintGain' keyword was used if 'PrintGain' in kwargs: - import warnings warnings.warn("'PrintGain' keyword is deprecated in root_locus; " "use 'print_gain'", FutureWarning) # Map 'PrintGain' keyword to 'print_gain' keyword @@ -145,8 +147,15 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, print_gain = config._get_param( 'rlocus', 'print_gain', print_gain, _rlocus_defaults) - sys_loop = sys if sys.issiso() else sys[0, 0] + # Check for sisotool mode + sisotool = kwargs.get('sisotool', False) + + # make sure siso. sisotool has different requirements + if not sys.issiso() and not sisotool: + raise ControlMIMONotImplemented( + 'sys must be single-input single-output (SISO)') + sys_loop = sys[0,0] # Convert numerator and denominator to polynomials if they aren't (nump, denp) = _systopoly1d(sys_loop) @@ -158,15 +167,16 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, xlim = (-1.3, 1.3) if kvect is None: - start_mat = _RLFindRoots(nump, denp, [1]) - kvect, mymat, xlim, ylim = _default_gains(nump, denp, xlim, ylim) + kvect, root_array, xlim, ylim = _default_gains(nump, denp, xlim, ylim) + recompute_on_zoom = True else: - start_mat = _RLFindRoots(nump, denp, [kvect[0]]) - mymat = _RLFindRoots(nump, denp, kvect) - mymat = _RLSortRoots(mymat) + kvect = np.atleast_1d(kvect) + root_array = _RLFindRoots(nump, denp, kvect) + root_array = _RLSortRoots(root_array) + recompute_on_zoom = False - # Check for sisotool mode - sisotool = False if 'sisotool' not in kwargs else True + if sisotool: + start_roots = _RLFindRoots(nump, denp, initial_gain) # Make sure there were no extraneous keywords if not sisotool and kwargs: @@ -190,17 +200,17 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, ax_rlocus=fig.axes[0], plotstr=plotstr)) elif sisotool: fig.axes[1].plot( - [root.real for root in start_mat], - [root.imag for root in start_mat], + [root.real for root in start_roots], + [root.imag for root in start_roots], marker='s', markersize=6, zorder=20, color='k', label='gain_point') - s = start_mat[0][0] + s = start_roots[0][0] if isdtime(sys, strict=True): zeta = -np.cos(np.angle(np.log(s))) else: zeta = -1 * s.real / abs(s) fig.suptitle( "Clicked at: %10.4g%+10.4gj gain: %10.4g damp: %10.4g" % - (s.real, s.imag, kvect[0], zeta), + (s.real, s.imag, initial_gain, zeta), fontsize=12 if int(mpl.__version__[0]) == 1 else 10) fig.canvas.mpl_connect( 'button_release_event', @@ -210,14 +220,16 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, bode_plot_params=kwargs['bode_plot_params'], tvect=kwargs['tvect'])) - # zoom update on xlim/ylim changed, only then data on new limits - # is available, i.e., cannot combine with _RLClickDispatcher - dpfun = partial( - _RLZoomDispatcher, sys=sys, ax_rlocus=ax, plotstr=plotstr) - # TODO: the next too lines seem to take a long time to execute - # TODO: is there a way to speed them up? (RMM, 6 Jun 2019) - ax.callbacks.connect('xlim_changed', dpfun) - ax.callbacks.connect('ylim_changed', dpfun) + + if recompute_on_zoom: + # update gains and roots when xlim/ylim change. Only then are + # data on available. I.e., cannot combine with _RLClickDispatcher + dpfun = partial( + _RLZoomDispatcher, sys=sys, ax_rlocus=ax, plotstr=plotstr) + # TODO: the next too lines seem to take a long time to execute + # TODO: is there a way to speed them up? (RMM, 6 Jun 2019) + ax.callbacks.connect('xlim_changed', dpfun) + ax.callbacks.connect('ylim_changed', dpfun) # plot open loop poles poles = array(denp.r) @@ -229,7 +241,7 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, ax.plot(real(zeros), imag(zeros), 'o') # Now plot the loci - for index, col in enumerate(mymat.T): + for index, col in enumerate(root_array.T): ax.plot(real(col), imag(col), plotstr, label='rootlocus') # Set up plot axes and labels @@ -257,7 +269,7 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, (0, 0), radius=1.0, linestyle=':', edgecolor='k', linewidth=0.75, fill=False, zorder=-20)) - return mymat, kvect + return root_array, kvect def _default_gains(num, den, xlim, ylim, zoom_xlim=None, zoom_ylim=None): @@ -274,8 +286,8 @@ def _default_gains(num, den, xlim, ylim, zoom_xlim=None, zoom_ylim=None): kvect = np.hstack((np.linspace(0, kmax, 50), np.real(k_break))) kvect.sort() - mymat = _RLFindRoots(num, den, kvect) - mymat = _RLSortRoots(mymat) + root_array = _RLFindRoots(num, den, kvect) + root_array = _RLSortRoots(root_array) open_loop_poles = den.roots open_loop_zeros = num.roots @@ -285,13 +297,13 @@ def _default_gains(num, den, xlim, ylim, zoom_xlim=None, zoom_ylim=None): open_loop_zeros, np.ones(open_loop_poles.size - open_loop_zeros.size) * open_loop_zeros[-1]) - mymat_xl = np.append(mymat, open_loop_zeros_xl) + root_array_xl = np.append(root_array, open_loop_zeros_xl) else: - mymat_xl = mymat + root_array_xl = root_array singular_points = np.concatenate((num.roots, den.roots), axis=0) important_points = np.concatenate((singular_points, real_break), axis=0) important_points = np.concatenate((important_points, np.zeros(2)), axis=0) - mymat_xl = np.append(mymat_xl, important_points) + root_array_xl = np.append(root_array_xl, important_points) false_gain = float(den.coeffs[0]) / float(num.coeffs[0]) if false_gain < 0 and not den.order > num.order: @@ -300,27 +312,27 @@ def _default_gains(num, den, xlim, ylim, zoom_xlim=None, zoom_ylim=None): "with equal order of numerator and denominator.") if xlim is None and false_gain > 0: - x_tolerance = 0.05 * (np.max(np.real(mymat_xl)) - - np.min(np.real(mymat_xl))) - xlim = _ax_lim(mymat_xl) + x_tolerance = 0.05 * (np.max(np.real(root_array_xl)) + - np.min(np.real(root_array_xl))) + xlim = _ax_lim(root_array_xl) elif xlim is None and false_gain < 0: axmin = np.min(np.real(important_points)) \ - (np.max(np.real(important_points)) - np.min(np.real(important_points))) - axmin = np.min(np.array([axmin, np.min(np.real(mymat_xl))])) + axmin = np.min(np.array([axmin, np.min(np.real(root_array_xl))])) axmax = np.max(np.real(important_points)) \ + np.max(np.real(important_points)) \ - np.min(np.real(important_points)) - axmax = np.max(np.array([axmax, np.max(np.real(mymat_xl))])) + axmax = np.max(np.array([axmax, np.max(np.real(root_array_xl))])) xlim = [axmin, axmax] x_tolerance = 0.05 * (axmax - axmin) else: x_tolerance = 0.05 * (xlim[1] - xlim[0]) if ylim is None: - y_tolerance = 0.05 * (np.max(np.imag(mymat_xl)) - - np.min(np.imag(mymat_xl))) - ylim = _ax_lim(mymat_xl * 1j) + y_tolerance = 0.05 * (np.max(np.imag(root_array_xl)) + - np.min(np.imag(root_array_xl))) + ylim = _ax_lim(root_array_xl * 1j) else: y_tolerance = 0.05 * (ylim[1] - ylim[0]) @@ -333,7 +345,7 @@ def _default_gains(num, den, xlim, ylim, zoom_xlim=None, zoom_ylim=None): tolerance = x_tolerance else: tolerance = np.min([x_tolerance, y_tolerance]) - indexes_too_far = _indexes_filt(mymat, tolerance, zoom_xlim, zoom_ylim) + indexes_too_far = _indexes_filt(root_array, tolerance, zoom_xlim, zoom_ylim) # Add more points into the root locus for points that are too far apart while len(indexes_too_far) > 0 and kvect.size < 5000: @@ -342,27 +354,27 @@ def _default_gains(num, den, xlim, ylim, zoom_xlim=None, zoom_ylim=None): new_gains = np.linspace(kvect[index], kvect[index + 1], 5) new_points = _RLFindRoots(num, den, new_gains[1:4]) kvect = np.insert(kvect, index + 1, new_gains[1:4]) - mymat = np.insert(mymat, index + 1, new_points, axis=0) + root_array = np.insert(root_array, index + 1, new_points, axis=0) - mymat = _RLSortRoots(mymat) - indexes_too_far = _indexes_filt(mymat, tolerance, zoom_xlim, zoom_ylim) + root_array = _RLSortRoots(root_array) + indexes_too_far = _indexes_filt(root_array, tolerance, zoom_xlim, zoom_ylim) new_gains = kvect[-1] * np.hstack((np.logspace(0, 3, 4))) new_points = _RLFindRoots(num, den, new_gains[1:4]) kvect = np.append(kvect, new_gains[1:4]) - mymat = np.concatenate((mymat, new_points), axis=0) - mymat = _RLSortRoots(mymat) - return kvect, mymat, xlim, ylim + root_array = np.concatenate((root_array, new_points), axis=0) + root_array = _RLSortRoots(root_array) + return kvect, root_array, xlim, ylim -def _indexes_filt(mymat, tolerance, zoom_xlim=None, zoom_ylim=None): +def _indexes_filt(root_array, tolerance, zoom_xlim=None, zoom_ylim=None): """Calculate the distance between points and return the indexes. Filter the indexes so only the resolution of points within the xlim and ylim is improved when zoom is used. """ - distance_points = np.abs(np.diff(mymat, axis=0)) + distance_points = np.abs(np.diff(root_array, axis=0)) indexes_too_far = list(np.unique(np.where(distance_points > tolerance)[0])) if zoom_xlim is not None and zoom_ylim is not None: @@ -374,23 +386,23 @@ def _indexes_filt(mymat, tolerance, zoom_xlim=None, zoom_ylim=None): indexes_too_far_filtered = [] for index in indexes_too_far_zoom: - for point in mymat[index]: + for point in root_array[index]: if (zoom_xlim[0] <= point.real <= zoom_xlim[1]) and \ (zoom_ylim[0] <= point.imag <= zoom_ylim[1]): indexes_too_far_filtered.append(index) break # Check if zoom box is not overshot & insert points where neccessary - if len(indexes_too_far_filtered) == 0 and len(mymat) < 500: + if len(indexes_too_far_filtered) == 0 and len(root_array) < 500: limits = [zoom_xlim[0], zoom_xlim[1], zoom_ylim[0], zoom_ylim[1]] for index, limit in enumerate(limits): if index <= 1: - asign = np.sign(real(mymat)-limit) + asign = np.sign(real(root_array)-limit) else: - asign = np.sign(imag(mymat) - limit) + asign = np.sign(imag(root_array) - limit) signchange = ((np.roll(asign, 1, axis=0) - asign) != 0).astype(int) - signchange[0] = np.zeros((len(mymat[0]))) + signchange[0] = np.zeros((len(root_array[0]))) if len(np.where(signchange == 1)[0]) > 0: indexes_too_far_filtered.append( np.where(signchange == 1)[0][0]-1) @@ -399,7 +411,7 @@ def _indexes_filt(mymat, tolerance, zoom_xlim=None, zoom_ylim=None): if indexes_too_far_filtered[0] != 0: indexes_too_far_filtered.insert( 0, indexes_too_far_filtered[0]-1) - if not indexes_too_far_filtered[-1] + 1 >= len(mymat) - 2: + if not indexes_too_far_filtered[-1] + 1 >= len(root_array) - 2: indexes_too_far_filtered.append( indexes_too_far_filtered[-1] + 1) @@ -429,10 +441,10 @@ def _break_points(num, den): return k_break, real_break_pts -def _ax_lim(mymat): +def _ax_lim(root_array): """Utility to get the axis limits""" - axmin = np.min(np.real(mymat)) - axmax = np.max(np.real(mymat)) + axmin = np.min(np.real(root_array)) + axmax = np.max(np.real(root_array)) if axmax != axmin: deltax = (axmax - axmin) * 0.02 else: @@ -509,28 +521,27 @@ def _RLFindRoots(nump, denp, kvect): """Find the roots for the root locus.""" # Convert numerator and denominator to polynomials if they aren't roots = [] - for k in np.array(kvect, ndmin=1): + for k in np.atleast_1d(kvect): curpoly = denp + k * nump curroots = curpoly.r if len(curroots) < denp.order: # if I have fewer poles than open loop, it is because i have # one at infinity - curroots = np.insert(curroots, len(curroots), np.inf) + curroots = np.append(curroots, np.inf) curroots.sort() roots.append(curroots) - mymat = row_stack(roots) - return mymat + return row_stack(roots) -def _RLSortRoots(mymat): - """Sort the roots from sys._RLFindRoots, so that the root +def _RLSortRoots(roots): + """Sort the roots from _RLFindRoots, so that the root locus doesn't show weird pseudo-branches as roots jump from one branch to another.""" - sorted = zeros_like(mymat) - for n, row in enumerate(mymat): + sorted = zeros_like(roots) + for n, row in enumerate(roots): if n == 0: sorted[n, :] = row else: @@ -539,7 +550,7 @@ def _RLSortRoots(mymat): # previous row available = list(range(len(prevrow))) for elem in row: - evect = elem-prevrow[available] + evect = elem - prevrow[available] ind1 = abs(evect).argmin() ind = available.pop(ind1) sorted[n, ind] = elem @@ -549,16 +560,15 @@ def _RLSortRoots(mymat): def _RLZoomDispatcher(event, sys, ax_rlocus, plotstr): """Rootlocus plot zoom dispatcher""" - sys_loop = sys if sys.issiso() else sys[0,0] - + sys_loop = sys[0,0] nump, denp = _systopoly1d(sys_loop) xlim, ylim = ax_rlocus.get_xlim(), ax_rlocus.get_ylim() - kvect, mymat, xlim, ylim = _default_gains( + kvect, root_array, xlim, ylim = _default_gains( nump, denp, xlim=None, ylim=None, zoom_xlim=xlim, zoom_ylim=ylim) _removeLine('rootlocus', ax_rlocus) - for i, col in enumerate(mymat.T): + for i, col in enumerate(root_array.T): ax_rlocus.plot(real(col), imag(col), plotstr, label='rootlocus', scalex=False, scaley=False) @@ -583,8 +593,7 @@ def _RLClickDispatcher(event, sys, fig, ax_rlocus, plotstr, sisotool=False, def _RLFeedbackClicksPoint(event, sys, fig, ax_rlocus, sisotool=False): """Display root-locus gain feedback point for clicks on root-locus plot""" - sys_loop = sys if sys.issiso() else sys[0,0] - + sys_loop = sys[0,0] (nump, denp) = _systopoly1d(sys_loop) xlim = ax_rlocus.get_xlim() @@ -631,10 +640,10 @@ def _RLFeedbackClicksPoint(event, sys, fig, ax_rlocus, sisotool=False): # Visualise clicked point, display all roots for sisotool mode if sisotool: - mymat = _RLFindRoots(nump, denp, K.real) + root_array = _RLFindRoots(nump, denp, K.real) ax_rlocus.plot( - [root.real for root in mymat], - [root.imag for root in mymat], + [root.real for root in root_array], + [root.imag for root in root_array], marker='s', markersize=6, zorder=20, label='gain_point', color='k') else: ax_rlocus.plot(s.real, s.imag, 'k.', marker='s', markersize=8, diff --git a/control/sisotool.py b/control/sisotool.py index 52c061249..2b735c0af 100644 --- a/control/sisotool.py +++ b/control/sisotool.py @@ -9,13 +9,19 @@ from .bdalg import append, connect from .iosys import tf2io, ss2io, summing_junction, interconnect from control.statesp import _convert_to_statespace, StateSpace +from . import config +import numpy as np import matplotlib.pyplot as plt import warnings -def sisotool(sys, kvect=None, xlim_rlocus=None, ylim_rlocus=None, +_sisotool_defaults = { + 'sisotool.initial_gain': 1 +} + +def sisotool(sys, initial_gain=None, xlim_rlocus=None, ylim_rlocus=None, plotstr_rlocus='C0', rlocus_grid=False, omega=None, dB=None, Hz=None, deg=None, omega_limits=None, omega_num=None, - margins_bode=True, tvect=None): + margins_bode=True, tvect=None, kvect=None): """ Sisotool style collection of plots inspired by MATLAB's sisotool. The left two plots contain the bode magnitude and phase diagrams. @@ -28,16 +34,22 @@ def sisotool(sys, kvect=None, xlim_rlocus=None, ylim_rlocus=None, sys : LTI object Linear input/output systems. If sys is SISO, use the same system for the root locus and step response. If it is desired to - see a different step response than feedback(K*loop,1), sys can be - provided as a two-input, two-output system (e.g. by using - :func:`bdgalg.connect' or :func:`iosys.interconnect`). Sisotool - inserts the negative of the selected gain K between the first output - and first input and uses the second input and output for computing - the step response. This allows you to see the step responses of more - complex systems, for example, systems with a feedforward path into the - plant or in which the gain appears in the feedback path. - kvect : list or ndarray, optional - List of gains to use for plotting root locus + see a different step response than feedback(K*sys,1), such as a + disturbance response, sys can be provided as a two-input, two-output + system (e.g. by using :func:`bdgalg.connect' or + :func:`iosys.interconnect`). For two-input, two-output + system, sisotool inserts the negative of the selected gain K between + the first output and first input and uses the second input and output + for computing the step response. To see the disturbance response, + configure your plant to have as its second input the disturbance input. + To view the step response with a feedforward controller, give your + plant two identical inputs, and sum your feedback controller and your + feedforward controller and multiply them into your plant's second + input. It is also possible to accomodate a system with a gain in the + feedback. + initial_gain : float, optional + Initial gain to use for plotting root locus. Defaults to 1 + (config.defaults['sisotool.initial_gain']). xlim_rlocus : tuple or list, optional control of x-axis range, normally with tuple (see :doc:`matplotlib:api/axes_api`). @@ -101,12 +113,19 @@ def sisotool(sys, kvect=None, xlim_rlocus=None, ylim_rlocus=None, 'margins': margins_bode } + # Check to see if legacy 'PrintGain' keyword was used + if kvect is not None: + warnings.warn("'kvect' keyword is deprecated in sisotool; " + "use 'initial_gain' instead", FutureWarning) + initial_gain = np.atleast_1d(kvect)[0] + initial_gain = config._get_param('sisotool', 'initial_gain', + initial_gain, _sisotool_defaults) + # First time call to setup the bode and step response plots - _SisotoolUpdate(sys, fig, - 1 if kvect is None else kvect[0], bode_plot_params) + _SisotoolUpdate(sys, fig, initial_gain, bode_plot_params) # Setup the root-locus plot window - root_locus(sys, kvect=kvect, xlim=xlim_rlocus, + root_locus(sys, initial_gain=initial_gain, xlim=xlim_rlocus, ylim=ylim_rlocus, plotstr=plotstr_rlocus, grid=rlocus_grid, fig=fig, bode_plot_params=bode_plot_params, tvect=tvect, sisotool=True) @@ -209,9 +228,7 @@ def rootlocus_pid_designer(plant, gain='P', sign=+1, input_signal='r', C_f = Kp + Ki/s + Kd*s/(tau*s + 1). - If `plant` is a discrete-time system, then the proportional, integral, and - derivative terms are given instead by Kp, Ki*dt/2*(z+1)/(z-1), and - Kd/dt*(z-1)/z, respectively. + :: ------> C_ff ------ d | | | @@ -222,6 +239,10 @@ def rootlocus_pid_designer(plant, gain='P', sign=+1, input_signal='r', | ----- C_b <-------| --------------------------------- + If `plant` is a discrete-time system, then the proportional, integral, and + derivative terms are given instead by Kp, Ki*dt/2*(z+1)/(z-1), and + Kd/dt*(z-1)/z, respectively. + It is also possible to move the derivative term into the feedback path `C_b` using `derivative_in_feedback_path=True`. This may be desired to avoid that the plant is subject to an impulse function when the reference @@ -264,7 +285,7 @@ def rootlocus_pid_designer(plant, gain='P', sign=+1, input_signal='r', Whether to create Sisotool interactive plot. Returns - ---------- + ------- closedloop : class:`StateSpace` system The closed-loop system using initial gains. diff --git a/control/statefbk.py b/control/statefbk.py index 97f314da5..1f61134a6 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -41,6 +41,7 @@ # External packages and modules import numpy as np +import scipy as sp from . import statesp from .mateqn import care, dare, _check_shape @@ -71,7 +72,7 @@ def sb03md(n, C, A, U, dico, job='X',fact='N',trana='N',ldwork=None): sb03od = None -__all__ = ['ctrb', 'obsv', 'gram', 'place', 'place_varga', 'lqr', +__all__ = ['ctrb', 'obsv', 'gram', 'place', 'place_varga', 'lqr', 'dlqr', 'acker', 'create_statefbk_iosystem'] @@ -126,10 +127,6 @@ def place(A, B, p): -------- place_varga, acker - Notes - ----- - The return type for 2D arrays depends on the default class set for - state space operations. See :func:`~control.use_numpy_matrix`. """ from scipy.signal import place_poles @@ -600,8 +597,10 @@ def dlqr(*args, **kwargs): # Function to create an I/O sytems representing a state feedback controller def create_statefbk_iosystem( - sys, K, integral_action=None, xd_labels='xd[{i}]', ud_labels='ud[{i}]', - estimator=None, type='linear'): + sys, gain, integral_action=None, estimator=None, type=None, + xd_labels='xd[{i}]', ud_labels='ud[{i}]', gainsched_indices=None, + gainsched_method='linear', name=None, inputs=None, outputs=None, + states=None): """Create an I/O system using a (full) state feedback controller This function creates an input/output system that implements a @@ -617,31 +616,47 @@ def create_statefbk_iosystem( feedback gain (eg, from LQR). The function returns the controller ``ctrl`` and the closed loop systems ``clsys``, both as I/O systems. + A gain scheduled controller can also be created, by passing a list of + gains and a corresponding list of values of a set of scheduling + variables. In this case, the controller has the form + + u = ud - K_p(mu) (x - xd) - K_i(mu) integral(C x - C x_d) + + where mu represents the scheduling variable. + Parameters ---------- sys : InputOutputSystem The I/O system that represents the process dynamics. If no estimator is given, the output of this system should represent the full state. - K : ndarray - The state feedback gain. This matrix defines the gains to be - applied to the system. If ``integral_action`` is None, then the - dimensions of this array should be (sys.ninputs, sys.nstates). If - `integral action` is set to a matrix or a function, then additional - columns represent the gains of the integral states of the - controller. + gain : ndarray or tuple + If a array is give, it represents the state feedback gain (K). + This matrix defines the gains to be applied to the system. If + ``integral_action`` is None, then the dimensions of this array + should be (sys.ninputs, sys.nstates). If `integral action` is + set to a matrix or a function, then additional columns + represent the gains of the integral states of the controller. + + If a tuple is given, then it specifies a gain schedule. The tuple + should be of the form ``(gains, points)`` where gains is a list of + gains :math:`K_j` and points is a list of values :math:`\\mu_j` at + which the gains are computed. The `gainsched_indices` parameter + should be used to specify the scheduling variables. xd_labels, ud_labels : str or list of str, optional Set the name of the signals to use for the desired state and inputs. If a single string is specified, it should be a format string using - the variable ``i`` as an index. Otherwise, a list of strings matching - the size of xd and ud, respectively, should be used. Default is - ``'xd[{i}]'`` for xd_labels and ``'xd[{i}]'`` for ud_labels. + the variable ``i`` as an index. Otherwise, a list of strings + matching the size of xd and ud, respectively, should be used. + Default is ``'xd[{i}]'`` for xd_labels and ``'ud[{i}]'`` for + ud_labels. These settings can also be overriden using the `inputs` + keyword. integral_action : None, ndarray, or func, optional If this keyword is specified, the controller can include integral - action in addition to state feedback. If ``integral_action`` is an - ndarray, it will be multiplied by the current and desired state to + action in addition to state feedback. If ``integral_action`` is a + matrix, it will be multiplied by the current and desired state to generate the error for the internal integrator states of the control law. If ``integral_action`` is a function ``h``, that function will be called with the signature h(t, x, u, params) to obtain the @@ -650,30 +665,63 @@ def create_statefbk_iosystem( ``K`` matrix. estimator : InputOutputSystem, optional - If an estimator is provided, using the states of the estimator as + If an estimator is provided, use the states of the estimator as the system inputs for the controller. - type : 'nonlinear' or 'linear', optional - Set the type of controller to create. The default is a linear - controller implementing the LQR regulator. If the type is 'nonlinear', - a :class:NonlinearIOSystem is created instead, with the gain ``K`` as - a parameter (allowing modifications of the gain at runtime). + gainsched_indices : list of int or str, optional + If a gain scheduled controller is specified, specify the indices of + the controller input to use for scheduling the gain. The input to + the controller is the desired state xd, the desired input ud, and + the system state x (or state estimate xhat, if an estimator is + given). The indices can either be specified as integer offsets into + the input vector or as strings matching the signal names of the + input vector. The default is to use the desire state xd. + + gainsched_method : str, optional + The method to use for gain scheduling. Possible values are 'linear' + (default), 'nearest', and 'cubic'. More information is available in + :func:`scipy.interpolate.griddata`. For points outside of the convex + hull of the scheduling points, the gain at the nearest point is + used. + + type : 'linear' or 'nonlinear', optional + Set the type of controller to create. The default for a linear gain + is a linear controller implementing the LQR regulator. If the type + is 'nonlinear', a :class:NonlinearIOSystem is created instead, with + the gain ``K`` as a parameter (allowing modifications of the gain at + runtime). If the gain parameter is a tuple, then a nonlinear, + gain-scheduled controller is created. Returns ------- ctrl : InputOutputSystem Input/output system representing the controller. This system takes - as inputs the desired state xd, the desired input ud, and the system - state x. It outputs the controller action u according to the - formula u = ud - K(x - xd). If the keyword `integral_action` is - specified, then an additional set of integrators is included in the - control system (with the gain matrix K having the integral gains - appended after the state gains). + as inputs the desired state ``xd``, the desired input ``ud``, and + either the system state ``x`` or the estimated state ``xhat``. It + outputs the controller action u according to the formula :math:`u = + u_d - K(x - x_d)`. If the keyword ``integral_action`` is specified, + then an additional set of integrators is included in the control + system (with the gain matrix ``K`` having the integral gains + appended after the state gains). If a gain scheduled controller is + specified, the gain (proportional and integral) are evaluated using + the scheduling variables specified by ``gainsched_indices``. clsys : InputOutputSystem Input/output system representing the closed loop system. This - systems takes as inputs the desired trajectory (xd, ud) and outputs - the system state x and the applied input u (vertically stacked). + systems takes as inputs the desired trajectory ``(xd, ud)`` and + outputs the system state ``x`` and the applied input ``u`` + (vertically stacked). + + Other Parameters + ---------------- + inputs, outputs : str, or list of str, optional + List of strings that name the individual signals of the transformed + system. If not given, the inputs and outputs are the same as the + original system. + + name : string, optional + System name. If unspecified, a generic name is generated + with a unique integer id. """ # Make sure that we were passed an I/O system as an input @@ -709,53 +757,127 @@ def create_statefbk_iosystem( C = np.zeros((0, sys.nstates)) # Check to make sure that state feedback has the right shape - if not isinstance(K, np.ndarray) or \ - K.shape != (sys.ninputs, estimator.noutputs + nintegrators): + if isinstance(gain, np.ndarray): + K = gain + if K.shape != (sys.ninputs, estimator.noutputs + nintegrators): + raise ControlArgument( + f'Control gain must be an array of size {sys.ninputs}' + f'x {sys.nstates}' + + (f'+{nintegrators}' if nintegrators > 0 else '')) + gainsched = False + + elif isinstance(gain, tuple): + # Check for gain scheduled controller + if len(gain) != 2: + raise ControlArgument("gain must be a 2-tuple for gain scheduling") + gains, points = gain[0:2] + + # Stack gains and points if past as a list + gains = np.stack(gains) + points = np.stack(points) + gainsched=True + + else: + raise ControlArgument("gain must be an array or a tuple") + + # Decide on the type of system to create + if gainsched and type == 'linear': raise ControlArgument( - f'Control gain must be an array of size {sys.ninputs}' - f'x {sys.nstates}' + - (f'+{nintegrators}' if nintegrators > 0 else '')) + "type 'linear' not allowed for gain scheduled controller") + elif type is None: + type = 'nonlinear' if gainsched else 'linear' + elif type not in {'linear', 'nonlinear'}: + raise ControlArgument(f"unknown type '{type}'") # Figure out the labels to use if isinstance(xd_labels, str): - # Gnerate the list of labels using the argument as a format string + # Generate the list of labels using the argument as a format string xd_labels = [xd_labels.format(i=i) for i in range(sys.nstates)] if isinstance(ud_labels, str): - # Gnerate the list of labels using the argument as a format string + # Generate the list of labels using the argument as a format string ud_labels = [ud_labels.format(i=i) for i in range(sys.ninputs)] + # Create the signal and system names + if inputs is None: + inputs = xd_labels + ud_labels + estimator.output_labels + if outputs is None: + outputs = list(sys.input_index.keys()) + if states is None: + states = nintegrators + + # Process gainscheduling variables, if present + if gainsched: + # Create a copy of the scheduling variable indices (default = xd) + gainsched_indices = range(sys.nstates) if gainsched_indices is None \ + else list(gainsched_indices) + + # Make sure the scheduling variable indices are the right length + if len(gainsched_indices) != points.shape[1]: + raise ControlArgument( + "length of gainsched_indices must match dimension of" + " scheduling variables") + + # Process scheduling variables + for i, idx in enumerate(gainsched_indices): + if isinstance(idx, str): + gainsched_indices[i] = inputs.index(gainsched_indices[i]) + + # Create interpolating function + if gainsched_method == 'nearest': + _interp = sp.interpolate.NearestNDInterpolator(points, gains) + def _nearest(mu): + raise SystemError(f"could not find nearest gain at mu = {mu}") + elif gainsched_method == 'linear': + _interp = sp.interpolate.LinearNDInterpolator(points, gains) + _nearest = sp.interpolate.NearestNDInterpolator(points, gains) + elif gainsched_method == 'cubic': + _interp = sp.interpolate.CloughTocher2DInterpolator(points, gains) + _nearest = sp.interpolate.NearestNDInterpolator(points, gains) + else: + raise ControlArgument( + f"unknown gain scheduling method '{gainsched_method}'") + + def _compute_gain(mu): + K = _interp(mu) + if np.isnan(K).any(): + K = _nearest(mu) + return K + # Define the controller system if type == 'nonlinear': # Create an I/O system for the state feedback gains - def _control_update(t, x, inputs, params): + def _control_update(t, states, inputs, params): # Split input into desired state, nominal input, and current state xd_vec = inputs[0:sys.nstates] x_vec = inputs[-estimator.nstates:] # Compute the integral error in the xy coordinates - return C @ x_vec - C @ xd_vec + return C @ (x_vec - xd_vec) - def _control_output(t, e, z, params): - K = params.get('K') + def _control_output(t, states, inputs, params): + if gainsched: + mu = inputs[gainsched_indices] + K_ = _compute_gain(mu) + else: + K_ = params.get('K') # Split input into desired state, nominal input, and current state - xd_vec = z[0:sys.nstates] - ud_vec = z[sys.nstates:sys.nstates + sys.ninputs] - x_vec = z[-sys.nstates:] + xd_vec = inputs[0:sys.nstates] + ud_vec = inputs[sys.nstates:sys.nstates + sys.ninputs] + x_vec = inputs[-sys.nstates:] # Compute the control law - u = ud_vec - K[:, 0:sys.nstates] @ (x_vec - xd_vec) + u = ud_vec - K_[:, 0:sys.nstates] @ (x_vec - xd_vec) if nintegrators > 0: - u -= K[:, sys.nstates:] @ e + u -= K_[:, sys.nstates:] @ states return u + params = {} if gainsched else {'K': K} ctrl = NonlinearIOSystem( - _control_update, _control_output, name='control', - inputs=xd_labels + ud_labels + estimator.output_labels, - outputs=list(sys.input_index.keys()), params={'K': K}, - states=nintegrators) + _control_update, _control_output, name=name, inputs=inputs, + outputs=outputs, states=states, params=params) elif type == 'linear' or type is None: # Create the matrices implementing the controller @@ -772,9 +894,8 @@ def _control_output(t, e, z, params): ]) ctrl = ss( - A_lqr, B_lqr, C_lqr, D_lqr, dt=sys.dt, name='control', - inputs=xd_labels + ud_labels + estimator.output_labels, - outputs=list(sys.input_index.keys()), states=nintegrators) + A_lqr, B_lqr, C_lqr, D_lqr, dt=sys.dt, name=name, + inputs=inputs, outputs=outputs, states=states) else: raise ControlArgument(f"unknown type '{type}'") @@ -783,7 +904,7 @@ def _control_output(t, e, z, params): closed = interconnect( [sys, ctrl] if estimator == sys else [sys, ctrl, estimator], name=sys.name + "_" + ctrl.name, - inplist=xd_labels + ud_labels, inputs=xd_labels + ud_labels, + inplist=inputs[:-sys.nstates], inputs=inputs[:-sys.nstates], outlist=sys.output_labels + sys.input_labels, outputs=sys.output_labels + sys.input_labels ) diff --git a/control/statesp.py b/control/statesp.py index 98d4a1633..9fff28d27 100644 --- a/control/statesp.py +++ b/control/statesp.py @@ -59,10 +59,12 @@ from scipy.signal import cont2discrete from scipy.signal import StateSpace as signalStateSpace from warnings import warn + +from .exception import ControlSlycot from .frdata import FrequencyResponseData from .lti import LTI, _process_frequency_response -from .namedio import common_timebase, isdtime -from .namedio import _process_namedio_keywords +from .namedio import common_timebase, isdtime, _process_namedio_keywords, \ + _process_dt_keyword, NamedIOSystem from . import config from copy import deepcopy @@ -162,19 +164,19 @@ def _f2s(f): class StateSpace(LTI): - """StateSpace(A, B, C, D[, dt]) + r"""StateSpace(A, B, C, D[, dt]) A class for representing state-space models. The StateSpace class is used to represent state-space realizations of linear time-invariant (LTI) systems: - + .. math:: - dx/dt = A x + B u + + dx/dt &= A x + B u \\ + y &= C x + D u - y = C x + D u - - where u is the input, y is the output, and x is the state. + where `u` is the input, `y` is the output, and `x` is the state. Parameters ---------- @@ -326,7 +328,7 @@ def __init__(self, *args, init_namedio=True, **kwargs): D = np.zeros((C.shape[0], B.shape[1])) D = _ssmatrix(D) - # Matrices definining the linear system + # Matrices defining the linear system self.A = A self.B = B self.C = C @@ -346,9 +348,8 @@ def __init__(self, *args, init_namedio=True, **kwargs): defaults = args[0] if len(args) == 1 else \ {'inputs': D.shape[1], 'outputs': D.shape[0], 'states': A.shape[0]} - static = (A.size == 0) name, inputs, outputs, states, dt = _process_namedio_keywords( - kwargs, defaults, static=static, end=True) + kwargs, defaults, static=(A.size == 0), end=True) # Initialize LTI (NamedIOSystem) object super().__init__( @@ -358,7 +359,7 @@ def __init__(self, *args, init_namedio=True, **kwargs): raise TypeError("unrecognized keyword(s): ", str(kwargs)) # Reset shapes (may not be needed once np.matrix support is removed) - if 0 == self.nstates: + if self._isstatic(): # static gain # matrix's default "empty" shape is 1x0 A.shape = (0, 0) @@ -519,7 +520,7 @@ def _latex_partitioned_stateless(self): s : string with LaTeX representation of model """ lines = [ - r'\[', + r'$$', (r'\left(' + r'\begin{array}' + r'{' + 'rll' * self.ninputs + '}') @@ -533,7 +534,7 @@ def _latex_partitioned_stateless(self): r'\end{array}' r'\right)' + self._latex_dt(), - r'\]']) + r'$$']) return '\n'.join(lines) @@ -551,7 +552,7 @@ def _latex_partitioned(self): return self._latex_partitioned_stateless() lines = [ - r'\[', + r'$$', (r'\left(' + r'\begin{array}' + r'{' + 'rll' * self.nstates + '|' + 'rll' * self.ninputs + '}') @@ -571,7 +572,7 @@ def _latex_partitioned(self): r'\end{array}' + r'\right)' + self._latex_dt(), - r'\]']) + r'$$']) return '\n'.join(lines) @@ -585,7 +586,7 @@ def _latex_separate(self): s : string with LaTeX representation of model """ lines = [ - r'\[', + r'$$', r'\begin{array}{ll}', ] @@ -615,7 +616,7 @@ def fmt_matrix(matrix, name): lines.extend([ r'\end{array}' + self._latex_dt(), - r'\]']) + r'$$']) return '\n'.join(lines) @@ -793,20 +794,21 @@ def __rmul__(self, other): pass raise TypeError("can't interconnect systems") - # TODO: __div__ and __rdiv__ are not written yet. - def __div__(self, other): - """Divide two LTI systems.""" - - raise NotImplementedError("StateSpace.__div__ is not implemented yet.") + # TODO: general __truediv__, and __rtruediv__; requires descriptor system support + def __truediv__(self, other): + """Division of StateSpace systems - def __rdiv__(self, other): - """Right divide two LTI systems.""" + Only division by TFs, FRDs, scalars, and arrays of scalars is + supported. + """ + if not isinstance(other, (LTI, NamedIOSystem)): + return self * (1/other) + else: + return NotImplemented - raise NotImplementedError( - "StateSpace.__rdiv__ is not implemented yet.") def __call__(self, x, squeeze=None, warn_infinite=True): - """Evaluate system's transfer function at complex frequency. + """Evaluate system's frequency response at complex frequencies. Returns the complex frequency response `sys(x)` where `x` is `s` for continuous-time systems and `z` for discrete-time systems. @@ -1297,7 +1299,8 @@ def __getitem__(self, indices): return StateSpace(self.A, self.B[:, j], self.C[i, :], self.D[i, j], self.dt) - def sample(self, Ts, method='zoh', alpha=None, prewarp_frequency=None): + def sample(self, Ts, method='zoh', alpha=None, prewarp_frequency=None, + name=None, copy_names=True, **kwargs): """Convert a continuous time system to discrete time Creates a discrete-time system from a continuous-time system by @@ -1316,22 +1319,42 @@ def sample(self, Ts, method='zoh', alpha=None, prewarp_frequency=None): alpha=0) * backward_diff: Backwards differencing ("gbt" with alpha=1.0) * zoh: zero-order hold (default) - alpha : float within [0, 1] The generalized bilinear transformation weighting parameter, which should only be specified with method="gbt", and is ignored otherwise - prewarp_frequency : float within [0, infinity) The frequency [rad/s] at which to match with the input continuous- time system's magnitude and phase (the gain=1 crossover frequency, for example). Should only be specified with method='bilinear' or 'gbt' with alpha=0.5 and ignored otherwise. + name : string, optional + Set the name of the sampled system. If not specified and + if `copy_names` is `False`, a generic name is generated + with a unique integer id. If `copy_names` is `True`, the new system + name is determined by adding the prefix and suffix strings in + config.defaults['namedio.sampled_system_name_prefix'] and + config.defaults['namedio.sampled_system_name_suffix'], with the + default being to add the suffix '$sampled'. + copy_names : bool, Optional + If True, copy the names of the input signals, output + signals, and states to the sampled system. Returns ------- sysd : StateSpace - Discrete time system, with sampling rate Ts + Discrete-time system, with sampling rate Ts + + Other Parameters + ---------------- + inputs : int, list of str or None, optional + Description of the system inputs. If not specified, the origional + system inputs are used. See :class:`InputOutputSystem` for more + information. + outputs : int, list of str or None, optional + Description of the system outputs. Same format as `inputs`. + states : int, list of str, or None, optional + Description of the system states. Same format as `inputs`. Notes ----- @@ -1346,14 +1369,26 @@ def sample(self, Ts, method='zoh', alpha=None, prewarp_frequency=None): if not self.isctime(): raise ValueError("System must be continuous time system") - sys = (self.A, self.B, self.C, self.D) if (method == 'bilinear' or (method == 'gbt' and alpha == 0.5)) and \ prewarp_frequency is not None: Twarp = 2 * np.tan(prewarp_frequency * Ts/2)/prewarp_frequency else: Twarp = Ts + sys = (self.A, self.B, self.C, self.D) Ad, Bd, C, D, _ = cont2discrete(sys, Twarp, method, alpha) - return StateSpace(Ad, Bd, C, D, Ts) + sysd = StateSpace(Ad, Bd, C, D, Ts) + # copy over the system name, inputs, outputs, and states + if copy_names: + sysd._copy_names(self) + if name is None: + sysd.name = \ + config.defaults['namedio.sampled_system_name_prefix'] +\ + sysd.name + \ + config.defaults['namedio.sampled_system_name_suffix'] + else: + sysd.name = name + # pass desired signal names if names were provided + return StateSpace(sysd, **kwargs) def dcgain(self, warn_infinite=False): """Return the zero-frequency gain @@ -1389,7 +1424,7 @@ def dcgain(self, warn_infinite=False): """ return self._dcgain(warn_infinite) - def dynamics(self, t, x, u=None): + def dynamics(self, t, x, u=None, params=None): """Compute the dynamics of the system Given input `u` and state `x`, returns the dynamics of the state-space @@ -1423,6 +1458,9 @@ def dynamics(self, t, x, u=None): dx/dt or x[t+dt] : ndarray """ + if params is not None: + warn("params keyword ignored for StateSpace object") + x = np.reshape(x, (-1, 1)) # force to a column in case matrix if np.size(x) != self.nstates: raise ValueError("len(x) must be equal to number of states") @@ -1435,7 +1473,7 @@ def dynamics(self, t, x, u=None): return (self.A @ x).reshape((-1,)) \ + (self.B @ u).reshape((-1,)) # return as row vector - def output(self, t, x, u=None): + def output(self, t, x, u=None, params=None): """Compute the output of the system Given input `u` and state `x`, returns the output `y` of the @@ -1465,6 +1503,9 @@ def output(self, t, x, u=None): ------- y : ndarray """ + if params is not None: + warn("params keyword ignored for StateSpace object") + x = np.reshape(x, (-1, 1)) # force to a column in case matrix if np.size(x) != self.nstates: raise ValueError("len(x) must be equal to number of states") @@ -1478,11 +1519,6 @@ def output(self, t, x, u=None): return (self.C @ x).reshape((-1,)) \ + (self.D @ u).reshape((-1,)) # return as row vector - def _isstatic(self): - """True if and only if the system has no dynamics, that is, - if A and B are zero. """ - return not np.any(self.A) and not np.any(self.B) - # TODO: add discrete time check def _convert_to_statespace(sys): diff --git a/control/stochsys.py b/control/stochsys.py index 2b8233070..90768a222 100644 --- a/control/stochsys.py +++ b/control/stochsys.py @@ -20,7 +20,7 @@ import scipy as sp from math import sqrt -from .iosys import InputOutputSystem, NonlinearIOSystem +from .iosys import InputOutputSystem, LinearIOSystem, NonlinearIOSystem from .lti import LTI from .namedio import isctime, isdtime from .mateqn import care, dare, _check_shape @@ -311,22 +311,32 @@ def create_estimator_iosystem( sys, QN, RN, P0=None, G=None, C=None, state_labels='xhat[{i}]', output_labels='xhat[{i}]', covariance_labels='P[{i},{j}]', sensor_labels=None): - """Create an I/O system implementing a linqear quadratic estimator + r"""Create an I/O system implementing a linear quadratic estimator This function creates an input/output system that implements a - state estimator of the form + continuous time state estimator of the form - xhat[k + 1] = A x[k] + B u[k] - L (C xhat[k] - y[k]) - P[k + 1] = A P A^T + F QN F^T - A P C^T Reps^{-1} C P A - L = A P C^T Reps^{-1} + .. math:: + + d \hat{x}/dt &= A \hat{x} + B u - L (C \hat{x} - y) \\ + dP/dt &= A P + P A^T + F Q_N F^T - P C^T R_N^{-1} C P \\ + L &= P C^T R_N^{-1} + + or a discrete time state estimator of the form + + .. math:: + + \hat{x}[k+1] &= A \hat{x}[k] + B u[k] - L (C \hat{x}[k] - y[k]) \\ + P[k+1] &= A P A^T + F Q_N F^T - A P C^T R_e^{-1} C P A \\ + L &= A P C^T R_e^{-1} - where Reps = RN + C P C^T. It can be called in the form + where :math:`R_e = R_N + C P C^T`. It can be called in the form:: estim = ct.create_estimator_iosystem(sys, QN, RN) - where ``sys`` is the process dynamics and QN and RN are the covariance + where `sys` is the process dynamics and `QN` and `RN` are the covariance of the disturbance noise and sensor noise. The function returns the - estimator ``estim`` as I/O system with a parameter ``correct`` that can + estimator `estim` as I/O system with a parameter `correct` that can be used to turn off the correction term in the estimation (for forward predictions). @@ -350,8 +360,8 @@ def create_estimator_iosystem( {state, covariance, sensor, output}_labels : str or list of str, optional Set the name of the signals to use for the internal state, covariance, sensors, and outputs (state estimate). If a single string is - specified, it should be a format string using the variable ``i`` as an - index (or ``i`` and ``j`` for covariance). Otherwise, a list of + specified, it should be a format string using the variable `i` as an + index (or `i` and `j` for covariance). Otherwise, a list of strings matching the size of the respective signal should be used. Default is ``'xhat[{i}]'`` for state and output labels, ``'y[{i}]'`` for output labels and ``'P[{i},{j}]'`` for covariance labels. @@ -359,24 +369,25 @@ def create_estimator_iosystem( Returns ------- estim : InputOutputSystem - Input/output system representing the estimator. This system takes the - system input and output and generates the estimated state. + Input/output system representing the estimator. This system takes + the system output y and input u and generates the estimated state + xhat. Notes ----- This function can be used with the ``create_statefbk_iosystem()`` function - to create a closed loop, output-feedback, state space controller: + to create a closed loop, output-feedback, state space controller:: K, _, _ = ct.lqr(sys, Q, R) est = ct.create_estimator_iosystem(sys, QN, RN, P0) ctrl, clsys = ct.create_statefbk_iosystem(sys, K, estimator=est) - The estimator can also be run on its own to process a noisy signal: + The estimator can also be run on its own to process a noisy signal:: resp = ct.input_output_response(est, T, [Y, U], [X0, P0]) If desired, the ``correct`` parameter can be set to ``False`` to allow - prediction with no additional sensor information: + prediction with no additional sensor information:: resp = ct.input_output_response( est, T, 0, [X0, P0], param={'correct': False) @@ -384,8 +395,8 @@ def create_estimator_iosystem( """ # Make sure that we were passed an I/O system as an input - if not isinstance(sys, InputOutputSystem): - raise ControlArgument("Input system must be I/O system") + if not isinstance(sys, LinearIOSystem): + raise ControlArgument("Input system must be a linear I/O system") # Extract the matrices that we need for easy reference A, B = sys.A, sys.B @@ -409,7 +420,7 @@ def create_estimator_iosystem( # Initialize the covariance matrix if P0 is None: # Initalize P0 to the steady state value - _, P0, _ = lqe(A, G, C, QN, RN) + L0, P0, _ = lqe(A, G, C, QN, RN) # Figure out the labels to use if isinstance(state_labels, str): @@ -432,16 +443,46 @@ def create_estimator_iosystem( sensor_labels = [sensor_labels.format(i=i) for i in range(C.shape[0])] if isctime(sys): - raise NotImplementedError("continuous time not yet implemented") - - else: # Create an I/O system for the state feedback gains # Note: reshape vectors into column vectors for legacy np.matrix + + R_inv = np.linalg.inv(RN) + Reps_inv = C.T @ R_inv @ C + + def _estim_update(t, x, u, params): + # See if we are estimating or predicting + correct = params.get('correct', True) + + # Get the state of the estimator + xhat = x[0:sys.nstates].reshape(-1, 1) + P = x[sys.nstates:].reshape(sys.nstates, sys.nstates) + + # Extract the inputs to the estimator + y = u[0:C.shape[0]].reshape(-1, 1) + u = u[C.shape[0]:].reshape(-1, 1) + + # Compute the optimal gain + L = P @ C.T @ R_inv + + # Update the state estimate + dxhat = A @ xhat + B @ u # prediction + if correct: + dxhat -= L @ (C @ xhat - y) # correction + + # Update the covariance + dP = A @ P + P @ A.T + G @ QN @ G.T + if correct: + dP -= P @ Reps_inv @ P + + # Return the update + return np.hstack([dxhat.reshape(-1), dP.reshape(-1)]) + + else: def _estim_update(t, x, u, params): # See if we are estimating or predicting correct = params.get('correct', True) - # Get the state of the estimator + # Get the state of the estimator xhat = x[0:sys.nstates].reshape(-1, 1) P = x[sys.nstates:].reshape(sys.nstates, sys.nstates) @@ -449,7 +490,7 @@ def _estim_update(t, x, u, params): y = u[0:C.shape[0]].reshape(-1, 1) u = u[C.shape[0]:].reshape(-1, 1) - # Compute the optimal again + # Compute the optimal gain Reps_inv = np.linalg.inv(RN + C @ P @ C.T) L = A @ P @ C.T @ Reps_inv @@ -466,8 +507,8 @@ def _estim_update(t, x, u, params): # Return the update return np.hstack([dxhat.reshape(-1), dP.reshape(-1)]) - def _estim_output(t, x, u, params): - return x[0:sys.nstates] + def _estim_output(t, x, u, params): + return x[0:sys.nstates] # Define the estimator system return NonlinearIOSystem( diff --git a/control/tests/bspline_test.py b/control/tests/bspline_test.py new file mode 100644 index 000000000..0ac59094d --- /dev/null +++ b/control/tests/bspline_test.py @@ -0,0 +1,221 @@ +"""bspline_test.py - test bsplines and their use in flat system + +RMM, 2 Aug 2022 + +This test suite checks to make sure that the bspline basic functions +supporting differential flat systetms are functioning. It doesn't do +exhaustive testing of operations on flat systems. Separate unit tests +should be created for that purpose. + +""" + +import numpy as np +import pytest +import scipy as sp + +import control as ct +import control.flatsys as fs +import control.optimal as opt + +def test_bspline_basis(): + Tf = 10 + degree = 5 + maxderiv = 4 + bspline = fs.BSplineFamily([0, Tf/3, Tf/2, Tf], degree, maxderiv) + time = np.linspace(0, Tf, 100) + + # Make sure that the knotpoint vector looks right + np.testing.assert_equal( + bspline.knotpoints, + [np.array([0, 0, 0, 0, 0, 0, + Tf/3, Tf/2, + Tf, Tf, Tf, Tf, Tf, Tf])]) + + # Repeat with default smoothness + bspline = fs.BSplineFamily([0, Tf/3, Tf/2, Tf], degree) + np.testing.assert_equal( + bspline.knotpoints, + [np.array([0, 0, 0, 0, 0, 0, + Tf/3, Tf/2, + Tf, Tf, Tf, Tf, Tf, Tf])]) + + # Sum of the B-spline curves should be one + np.testing.assert_almost_equal( + 1, sum([bspline(i, time) for i in range(bspline.N)])) + + # Sum of derivatives should be zero + for k in range(1, maxderiv): + np.testing.assert_almost_equal( + 0, sum([bspline.eval_deriv(i, k, time) + for i in range(0, bspline.N)])) + + # Make sure that the second derivative integrates to the first + time = np.linspace(0, Tf, 1000) + dt = time[1] - time[0] + for i in range(bspline.N): + for j in range(1, maxderiv): + np.testing.assert_allclose( + np.diff(bspline.eval_deriv(i, j-1, time)) / dt, + bspline.eval_deriv(i, j, time)[0:-1], + atol=0.01, rtol=0.01) + + # Make sure that ndarrays are processed the same as integer lists + degree = np.array(degree) + bspline2 = fs.BSplineFamily([0, Tf/3, Tf/2, Tf], degree, maxderiv) + np.testing.assert_equal(bspline(0, time), bspline2(0, time)) + + # Exception check + with pytest.raises(IndexError, match="out of bounds"): + bspline.eval_deriv(bspline.N, 0, time) + + +@pytest.mark.parametrize( + "xf, uf, Tf", + [([1, 0], [0], 2), + ([0, 1], [0], 3), + ([1, 1], [1], 4)]) +def test_double_integrator(xf, uf, Tf): + # Define a second order integrator + sys = ct.StateSpace([[-1, 1], [0, -2]], [[0], [1]], [[1, 0]], 0) + flatsys = fs.LinearFlatSystem(sys) + + # Define the basis set + bspline = fs.BSplineFamily([0, Tf/2, Tf], 4, 2) + + x0, u0, = [0, 0], [0] + traj = fs.point_to_point(flatsys, Tf, x0, u0, xf, uf, basis=bspline) + + # Verify that the trajectory computation is correct + x, u = traj.eval([0, Tf]) + np.testing.assert_array_almost_equal(x0, x[:, 0]) + np.testing.assert_array_almost_equal(u0, u[:, 0]) + np.testing.assert_array_almost_equal(xf, x[:, 1]) + np.testing.assert_array_almost_equal(uf, u[:, 1]) + + # Simulate the system and make sure we stay close to desired traj + T = np.linspace(0, Tf, 200) + xd, ud = traj.eval(T) + + t, y, x = ct.forced_response(sys, T, ud, x0, return_x=True) + np.testing.assert_array_almost_equal(x, xd, decimal=3) + + +# Bicycle model +def vehicle_flat_forward(x, u, params={}): + b = params.get('wheelbase', 3.) # get parameter values + zflag = [np.zeros(3), np.zeros(3)] # list for flag arrays + zflag[0][0] = x[0] # flat outputs + zflag[1][0] = x[1] + zflag[0][1] = u[0] * np.cos(x[2]) # first derivatives + zflag[1][1] = u[0] * np.sin(x[2]) + thdot = (u[0]/b) * np.tan(u[1]) # dtheta/dt + zflag[0][2] = -u[0] * thdot * np.sin(x[2]) # second derivatives + zflag[1][2] = u[0] * thdot * np.cos(x[2]) + return zflag + +def vehicle_flat_reverse(zflag, params={}): + b = params.get('wheelbase', 3.) # get parameter values + x = np.zeros(3); u = np.zeros(2) # vectors to store x, u + x[0] = zflag[0][0] # x position + x[1] = zflag[1][0] # y position + x[2] = np.arctan2(zflag[1][1], zflag[0][1]) # angle + u[0] = zflag[0][1] * np.cos(x[2]) + zflag[1][1] * np.sin(x[2]) + thdot_v = zflag[1][2] * np.cos(x[2]) - zflag[0][2] * np.sin(x[2]) + u[1] = np.arctan2(thdot_v, u[0]**2 / b) + return x, u + +def vehicle_update(t, x, u, params): + b = params.get('wheelbase', 3.) # get parameter values + dx = np.array([ + np.cos(x[2]) * u[0], + np.sin(x[2]) * u[0], + (u[0]/b) * np.tan(u[1]) + ]) + return dx + +def vehicle_output(t, x, u, params): return x + +# Create differentially flat input/output system +vehicle_flat = fs.FlatSystem( + vehicle_flat_forward, vehicle_flat_reverse, vehicle_update, + vehicle_output, inputs=('v', 'delta'), outputs=('x', 'y', 'theta'), + states=('x', 'y', 'theta')) + +def test_kinematic_car(): + # Define the endpoints of the trajectory + x0 = [0., -2., 0.]; u0 = [10., 0.] + xf = [100., 2., 0.]; uf = [10., 0.] + Tf = 10 + + # Set up a basis vector + bspline = fs.BSplineFamily([0, Tf/2, Tf], 5, 3) + + # Find trajectory between initial and final conditions + traj = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=bspline) + + # Verify that the trajectory computation is correct + x, u = traj.eval([0, Tf]) + np.testing.assert_array_almost_equal(x0, x[:, 0]) + np.testing.assert_array_almost_equal(u0, u[:, 0]) + np.testing.assert_array_almost_equal(xf, x[:, 1]) + np.testing.assert_array_almost_equal(uf, u[:, 1]) + +def test_kinematic_car_multivar(): + # Define the endpoints of the trajectory + x0 = [0., -2., 0.]; u0 = [10., 0.] + xf = [100., 2., 0.]; uf = [10., 0.] + Tf = 10 + + # Set up a basis vector + bspline = fs.BSplineFamily([0, Tf/2, Tf], [5, 6], [3, 4], vars=2) + + # Find trajectory between initial and final conditions + traj = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=bspline) + + # Verify that the trajectory computation is correct + x, u = traj.eval([0, Tf]) + np.testing.assert_array_almost_equal(x0, x[:, 0]) + np.testing.assert_array_almost_equal(u0, u[:, 0]) + np.testing.assert_array_almost_equal(xf, x[:, 1]) + np.testing.assert_array_almost_equal(uf, u[:, 1]) + +def test_bspline_errors(): + # Breakpoints must be a 1D array, in increasing order + with pytest.raises(NotImplementedError, match="not yet supported"): + basis = fs.BSplineFamily([[0, 1, 3], [0, 2, 3]], [3, 3]) + + with pytest.raises(ValueError, + match="breakpoints must be convertable to a 1D array"): + basis = fs.BSplineFamily([[[0, 1], [0, 1]], [[0, 1], [0, 1]]], [3, 3]) + + with pytest.raises(ValueError, match="must have at least 2 values"): + basis = fs.BSplineFamily([10], 2) + + with pytest.raises(ValueError, match="must be strictly increasing"): + basis = fs.BSplineFamily([1, 3, 2], 2) + + # Smoothness can't be more than dimension of splines + basis = fs.BSplineFamily([0, 1], 4, 3) # OK + with pytest.raises(ValueError, match="degree must be greater"): + basis = fs.BSplineFamily([0, 1], 4, 4) # not OK + + # nvars must be an integer + with pytest.raises(TypeError, match="vars must be an integer"): + basis = fs.BSplineFamily([0, 1], 4, 3, vars=['x1', 'x2']) + + # degree, smoothness must match nvars + with pytest.raises(ValueError, match="length of 'degree' does not match"): + basis = fs.BSplineFamily([0, 1], [4, 4, 4], 3, vars=2) + + # degree, smoothness must be list of ints + basis = fs.BSplineFamily([0, 1], [4, 4], 3, vars=2) # OK + with pytest.raises(ValueError, match="could not parse 'degree'"): + basis = fs.BSplineFamily([0, 1], [4, '4'], 3, vars=2) + + # degree must be strictly positive + with pytest.raises(ValueError, match="'degree'; must be at least 1"): + basis = fs.BSplineFamily([0, 1], 0, 1) + + # smoothness must be non-negative + with pytest.raises(ValueError, match="'smoothness'; must be at least 0"): + basis = fs.BSplineFamily([0, 1], 2, -1) diff --git a/control/tests/config_test.py b/control/tests/config_test.py index 295c68bdd..c36f67280 100644 --- a/control/tests/config_test.py +++ b/control/tests/config_test.py @@ -8,7 +8,6 @@ from math import pi, log10 import matplotlib.pyplot as plt -from matplotlib.testing.decorators import cleanup as mplcleanup import numpy as np import pytest @@ -18,7 +17,6 @@ @pytest.mark.usefixtures("editsdefaults") # makes sure to reset the defaults # to the test configuration class TestConfig: - # Create a simple second order system to use for testing sys = ct.tf([10], [1, 2, 1]) @@ -28,8 +26,7 @@ def test_set_defaults(self): assert ct.config.defaults['freqplot.deg'] == 2 assert ct.config.defaults['freqplot.Hz'] is None - @mplcleanup - def test_get_param(self): + def test_get_param(self, mplcleanup): assert ct.config._get_param('freqplot', 'dB')\ == ct.config.defaults['freqplot.dB'] assert ct.config._get_param('freqplot', 'dB', 1) == 1 @@ -92,8 +89,7 @@ def test_default_deprecation(self): assert ct.config.defaults['bode.Hz'] \ == ct.config.defaults['freqplot.Hz'] - @mplcleanup - def test_fbs_bode(self): + def test_fbs_bode(self, mplcleanup): ct.use_fbs_defaults() # Generate a Bode plot @@ -137,8 +133,7 @@ def test_fbs_bode(self): phase_x, phase_y = (((plt.gcf().axes[1]).get_lines())[0]).get_data() np.testing.assert_almost_equal(phase_y[-1], -pi, decimal=2) - @mplcleanup - def test_matlab_bode(self): + def test_matlab_bode(self, mplcleanup): ct.use_matlab_defaults() # Generate a Bode plot @@ -182,8 +177,7 @@ def test_matlab_bode(self): phase_x, phase_y = (((plt.gcf().axes[1]).get_lines())[0]).get_data() np.testing.assert_almost_equal(phase_y[-1], -pi, decimal=2) - @mplcleanup - def test_custom_bode_default(self): + def test_custom_bode_default(self, mplcleanup): ct.config.defaults['freqplot.dB'] = True ct.config.defaults['freqplot.deg'] = True ct.config.defaults['freqplot.Hz'] = True @@ -204,8 +198,7 @@ def test_custom_bode_default(self): np.testing.assert_almost_equal(mag_y[0], 20*log10(10), decimal=3) np.testing.assert_almost_equal(phase_y[-1], -pi, decimal=2) - @mplcleanup - def test_bode_number_of_samples(self): + def test_bode_number_of_samples(self, mplcleanup): # Set the number of samples (default is 50, from np.logspace) mag_ret, phase_ret, omega_ret = ct.bode_plot(self.sys, omega_num=87) assert len(mag_ret) == 87 @@ -219,8 +212,7 @@ def test_bode_number_of_samples(self): mag_ret, phase_ret, omega_ret = ct.bode_plot(self.sys, omega_num=87) assert len(mag_ret) == 87 - @mplcleanup - def test_bode_feature_periphery_decade(self): + def test_bode_feature_periphery_decade(self, mplcleanup): # Generate a sample Bode plot to figure out the range it uses ct.reset_defaults() # Make sure starting state is correct mag_ret, phase_ret, omega_ret = ct.bode_plot(self.sys, Hz=False) diff --git a/control/tests/conftest.py b/control/tests/conftest.py index b67ef3674..3f798f26c 100644 --- a/control/tests/conftest.py +++ b/control/tests/conftest.py @@ -1,13 +1,10 @@ """conftest.py - pytest local plugins and fixtures""" -from contextlib import contextmanager -from distutils.version import StrictVersion import os -import sys +from contextlib import contextmanager import matplotlib as mpl import numpy as np -import scipy as sp import pytest import control @@ -18,10 +15,8 @@ # pytest.param(marks=) slycotonly = pytest.mark.skipif(not control.exception.slycot_check(), reason="slycot not installed") -noscipy0 = pytest.mark.skipif(StrictVersion(sp.__version__) < "1.0", - reason="requires SciPy 1.0 or greater") -nopython2 = pytest.mark.skipif(sys.version_info < (3, 0), - reason="requires Python 3+") +cvxoptonly = pytest.mark.skipif(not control.exception.cvxopt_check(), + reason="cvxopt not installed") matrixfilter = pytest.mark.filterwarnings("ignore:.*matrix subclass:" "PendingDeprecationWarning") matrixerrorfilter = pytest.mark.filterwarnings("error:.*matrix subclass:" @@ -43,11 +38,12 @@ def control_defaults(): # assert that nothing changed it without reverting assert control.config.defaults == the_defaults + @pytest.fixture(scope="function", autouse=TEST_MATRIX_AND_ARRAY, params=[pytest.param("arrayout", marks=matrixerrorfilter), pytest.param("matrixout", marks=matrixfilter)]) def matarrayout(request): - """Switch the config to use np.ndarray and np.matrix as returns""" + """Switch the config to use np.ndarray and np.matrix as returns.""" restore = control.config.defaults['statesp.use_numpy_matrix'] control.use_numpy_matrix(request.param == "matrixout", warn=False) yield @@ -55,7 +51,7 @@ def matarrayout(request): def ismatarrayout(obj): - """Test if the returned object has the correct type as configured + """Test if the returned object has the correct type as configured. note that isinstance(np.matrix(obj), np.ndarray) is True """ @@ -65,7 +61,7 @@ def ismatarrayout(obj): def asmatarrayout(obj): - """Return a object according to the configured default""" + """Return a object according to the configured default.""" use_matrix = control.config.defaults['statesp.use_numpy_matrix'] matarray = np.asmatrix if use_matrix else np.asarray return matarray(obj) @@ -73,7 +69,7 @@ def asmatarrayout(obj): @contextmanager def check_deprecated_matrix(): - """Check that a call produces a deprecation warning because of np.matrix""" + """Check that a call produces a deprecation warning because of np.matrix.""" use_matrix = control.config.defaults['statesp.use_numpy_matrix'] if use_matrix: with pytest.deprecated_call(): @@ -96,13 +92,13 @@ def check_deprecated_matrix(): False)] if usebydefault or TEST_MATRIX_AND_ARRAY]) def matarrayin(request): - """Use array and matrix to construct input data in tests""" + """Use array and matrix to construct input data in tests.""" return request.param @pytest.fixture(scope="function") def editsdefaults(): - """Make sure any changes to the defaults only last during a test""" + """Make sure any changes to the defaults only last during a test.""" restore = control.config.defaults.copy() yield control.config.defaults = restore.copy() @@ -110,10 +106,10 @@ def editsdefaults(): @pytest.fixture(scope="function") def mplcleanup(): - """Workaround for python2 + """Clean up any plots and changes a test may have made to matplotlib. - python 2 does not like to mix the original mpl decorator with pytest - fixtures. So we roll our own. + compare matplotlib.testing.decorators.cleanup() but as a fixture instead + of a decorator. """ save = mpl.units.registry.copy() try: diff --git a/control/tests/delay_test.py b/control/tests/delay_test.py index 25f37eeb5..24263c3b8 100644 --- a/control/tests/delay_test.py +++ b/control/tests/delay_test.py @@ -52,8 +52,8 @@ def testTvalues(self, T, dendeg, numdeg, baseden, basenum): refnum /= refden[0] refden /= refden[0] num, den = pade(T, dendeg, numdeg) - np.testing.assert_array_almost_equal_nulp(refden, den, nulp=2) - np.testing.assert_array_almost_equal_nulp(refnum, num, nulp=2) + np.testing.assert_array_almost_equal_nulp(refden, den, nulp=4) + np.testing.assert_array_almost_equal_nulp(refnum, num, nulp=4) def testErrors(self): "ValueError raised for invalid arguments" diff --git a/control/tests/discrete_test.py b/control/tests/discrete_test.py index cb0ce3c76..4cf28a21b 100644 --- a/control/tests/discrete_test.py +++ b/control/tests/discrete_test.py @@ -446,3 +446,59 @@ def test_discrete_bode(self, tsys): np.testing.assert_array_almost_equal(omega, omega_out) np.testing.assert_array_almost_equal(mag_out, np.absolute(H_z)) np.testing.assert_array_almost_equal(phase_out, np.angle(H_z)) + + def test_signal_names(self, tsys): + "test that signal names are preserved in conversion to discrete-time" + ssc = StateSpace(tsys.siso_ss1c, + inputs='u', outputs='y', states=['a', 'b', 'c']) + ssd = ssc.sample(0.1) + tfc = TransferFunction(tsys.siso_tf1c, inputs='u', outputs='y') + tfd = tfc.sample(0.1) + assert ssd.input_labels == ['u'] + assert ssd.state_labels == ['a', 'b', 'c'] + assert ssd.output_labels == ['y'] + assert tfd.input_labels == ['u'] + assert tfd.output_labels == ['y'] + + ssd = sample_system(ssc, 0.1) + tfd = sample_system(tfc, 0.1) + assert ssd.input_labels == ['u'] + assert ssd.state_labels == ['a', 'b', 'c'] + assert ssd.output_labels == ['y'] + assert tfd.input_labels == ['u'] + assert tfd.output_labels == ['y'] + + # system names and signal name override + sysc = StateSpace(1.1, 1, 1, 1, inputs='u', outputs='y', states='a') + + sysd = sample_system(sysc, 0.1, name='sampled') + assert sysd.name == 'sampled' + assert sysd.find_input('u') == 0 + assert sysd.find_output('y') == 0 + assert sysd.find_state('a') == 0 + + # If we copy signal names w/out a system name, append '$sampled' + sysd = sample_system(sysc, 0.1) + assert sysd.name == sysc.name + '$sampled' + + # If copy is False, signal names should not be copied + sysd_nocopy = sample_system(sysc, 0.1, copy_names=False) + assert sysd_nocopy.find_input('u') is None + assert sysd_nocopy.find_output('y') is None + assert sysd_nocopy.find_state('a') is None + + # if signal names are provided, they should override those of sysc + sysd_newnames = sample_system(sysc, 0.1, + inputs='v', outputs='x', states='b') + assert sysd_newnames.find_input('v') == 0 + assert sysd_newnames.find_input('u') is None + assert sysd_newnames.find_output('x') == 0 + assert sysd_newnames.find_output('y') is None + assert sysd_newnames.find_state('b') == 0 + assert sysd_newnames.find_state('a') is None + # test just one name + sysd_newnames = sample_system(sysc, 0.1, inputs='v') + assert sysd_newnames.find_input('v') == 0 + assert sysd_newnames.find_input('u') is None + assert sysd_newnames.find_output('y') == 0 + assert sysd_newnames.find_output('x') is None diff --git a/control/tests/flatsys_test.py b/control/tests/flatsys_test.py index a12852759..95fb8cf7c 100644 --- a/control/tests/flatsys_test.py +++ b/control/tests/flatsys_test.py @@ -8,34 +8,41 @@ created for that purpose. """ -from distutils.version import StrictVersion - import numpy as np import pytest import scipy as sp +import re +import warnings +import os +import platform import control as ct import control.flatsys as fs import control.optimal as opt +# Set tolerances for lower/upper bound tests +atol = 1e-4 +rtol = 1e-4 + class TestFlatSys: """Test differential flat systems""" @pytest.mark.parametrize( - "xf, uf, Tf", - [([1, 0], [0], 2), - ([0, 1], [0], 3), - ([1, 1], [1], 4)]) - def test_double_integrator(self, xf, uf, Tf): + " xf, uf, Tf, basis", + [([1, 0], [0], 2, fs.PolyFamily(6)), + ([0, 1], [0], 3, fs.PolyFamily(6)), + ([0, 1], [0], 3, fs.BezierFamily(6)), + ([0, 1], [0], 3, fs.BSplineFamily([0, 1.5, 3], 4)), + ([1, 1], [1], 4, fs.PolyFamily(6)), + ([1, 1], [1], 4, fs.BezierFamily(6)), + ([1, 1], [1], 4, fs.BSplineFamily([0, 1.5, 3], 4))]) + def test_double_integrator(self, xf, uf, Tf, basis): # Define a second order integrator sys = ct.StateSpace([[-1, 1], [0, -2]], [[0], [1]], [[1, 0]], 0) flatsys = fs.LinearFlatSystem(sys) - # Define the basis set - poly = fs.PolyFamily(6) - x1, u1, = [0, 0], [0] - traj = fs.point_to_point(flatsys, Tf, x1, u1, xf, uf, basis=poly) + traj = fs.point_to_point(flatsys, Tf, x1, u1, xf, uf, basis=basis) # Verify that the trajectory computation is correct x, u = traj.eval([0, Tf]) @@ -94,16 +101,19 @@ def vehicle_output(t, x, u, params): return x vehicle_output, inputs=('v', 'delta'), outputs=('x', 'y', 'theta'), states=('x', 'y', 'theta')) - @pytest.mark.parametrize("poly", [ - fs.PolyFamily(6), fs.PolyFamily(8), fs.BezierFamily(6)]) - def test_kinematic_car(self, vehicle_flat, poly): + @pytest.mark.parametrize("basis", [ + fs.PolyFamily(6), fs.PolyFamily(8), fs.BezierFamily(6), + fs.BSplineFamily([0, 10], 8), + fs.BSplineFamily([0, 5, 10], 4) + ]) + def test_kinematic_car(self, vehicle_flat, basis): # Define the endpoints of the trajectory x0 = [0., -2., 0.]; u0 = [10., 0.] xf = [100., 2., 0.]; uf = [10., 0.] Tf = 10 # Find trajectory between initial and final conditions - traj = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=poly) + traj = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=basis) # Verify that the trajectory computation is correct x, u = traj.eval([0, Tf]) @@ -113,16 +123,116 @@ def test_kinematic_car(self, vehicle_flat, poly): np.testing.assert_array_almost_equal(uf, u[:, 1]) # Simulate the system and make sure we stay close to desired traj + # Note: this can sometimes fail since system is open loop unstable T = np.linspace(0, Tf, 100) xd, ud = traj.eval(T) resp = ct.input_output_response(vehicle_flat, T, ud, x0) - np.testing.assert_array_almost_equal(resp.states, xd, decimal=2) + if not np.allclose(resp.states, xd, atol=1e-2, rtol=1e-2): + pytest.xfail("system is open loop unstable => errors can build") + + # integrate equations and compare to desired + t, y, x = ct.input_output_response( + vehicle_flat, T, ud, x0, return_x=True) + np.testing.assert_allclose(x, xd, atol=0.01, rtol=0.01) + + @pytest.mark.parametrize( + "basis, guess, constraints, method", [ + (fs.PolyFamily(8, T=10), 'prev', None, None), + (fs.BezierFamily(8, T=10), 'linear', None, None), + (fs.BSplineFamily([0, 10], 8), None, None, None), + (fs.BSplineFamily([0, 10], 8), 'prev', None, 'trust-constr'), + (fs.BSplineFamily([0, 10], [6, 8], vars=2), 'prev', None, None), + (fs.BSplineFamily([0, 5, 10], 5), 'linear', None, 'slsqp'), + (fs.BSplineFamily([0, 10], 8), None, ([8, -0.1], [12, 0.1]), None), + (fs.BSplineFamily([0, 5, 10], 5, 3), None, None, None), + ]) + def test_kinematic_car_ocp( + self, vehicle_flat, basis, guess, constraints, method): + + # Define the endpoints of the trajectory + x0 = [0., -2., 0.]; u0 = [10., 0.] + xf = [40., 2., 0.]; uf = [10., 0.] + Tf = 4 + timepts = np.linspace(0, Tf, 10) + + # Find trajectory between initial and final conditions + traj_p2p = fs.point_to_point( + vehicle_flat, Tf, x0, u0, xf, uf, basis=basis) - # For SciPy 1.0+, integrate equations and compare to desired - if StrictVersion(sp.__version__) >= "1.0": - t, y, x = ct.input_output_response( - vehicle_flat, T, ud, x0, return_x=True) - np.testing.assert_allclose(x, xd, atol=0.01, rtol=0.01) + # Verify that the trajectory computation is correct + x, u = traj_p2p.eval(timepts) + np.testing.assert_array_almost_equal(x0, x[:, 0]) + np.testing.assert_array_almost_equal(u0, u[:, 0]) + np.testing.assert_array_almost_equal(xf, x[:, -1]) + np.testing.assert_array_almost_equal(uf, u[:, -1]) + + # + # Re-solve as optimal control problem + # + + # Define the cost function (mainly penalize steering angle) + traj_cost = opt.quadratic_cost( + vehicle_flat, None, np.diag([0.1, 10]), x0=xf, u0=uf) + + # Set terminal cost to bring us close to xf + terminal_cost = opt.quadratic_cost( + vehicle_flat, 1e3 * np.eye(3), None, x0=xf) + + # Implement terminal constraints if specified + if constraints: + input_constraints = opt.input_range_constraint( + vehicle_flat, *constraints) + else: + input_constraints = None + + # Use a straight line as an initial guess for the trajectory + if guess == 'prev': + initial_guess = traj_p2p.eval(timepts)[0][0:2] + elif guess == 'linear': + initial_guess = np.array( + [x0[i] + (xf[i] - x0[i]) * timepts/Tf for i in (0, 1)]) + else: + initial_guess = None + + # Solve the optimal trajectory + traj_ocp = fs.solve_flat_ocp( + vehicle_flat, timepts, x0, u0, + cost=traj_cost, constraints=input_constraints, + terminal_cost=terminal_cost, basis=basis, + initial_guess=initial_guess, + minimize_kwargs={'method': method}, + ) + xd, ud = traj_ocp.eval(timepts) + + if not traj_ocp.success: + # Known failure cases + if re.match(".*precision loss.*", traj_ocp.message): + pytest.xfail("precision loss in some configurations") + + elif re.match("Iteration limit.*", traj_ocp.message) and \ + re.match( + "conda ubuntu-3.* Generic", os.getenv('JOBNAME', '')) and \ + re.match("1.24.[01]", np.__version__): + pytest.xfail("gh820: iteration limit exceeded") + + else: + # Dump out information to allow creation of an exception + print("Message:", traj_ocp.message) + print("Platform:", platform.platform()) + print("Python:", platform.python_version()) + print("NumPy version:", np.__version__) + np.show_config() + print("JOBNAME:", os.getenv('JOBNAME')) + + pytest.fail( + "unknown failure; view output to identify configuration") + + # Make sure the constraints are satisfied + if input_constraints: + _, _, lb, ub = input_constraints + for i in range(ud.shape[0]): + assert all(lb[i] - ud[i] < rtol * abs(lb[i]) + atol) + assert all(ud[i] - ub[i] < rtol * abs(ub[i]) + atol) def test_flat_default_output(self, vehicle_flat): # Construct a flat system with the default outputs @@ -137,9 +247,9 @@ def test_flat_default_output(self, vehicle_flat): Tf = 10 # Find trajectory between initial and final conditions - poly = fs.PolyFamily(6) - traj1 = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=poly) - traj2 = fs.point_to_point(flatsys, Tf, x0, u0, xf, uf, basis=poly) + basis = fs.PolyFamily(6) + traj1 = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=basis) + traj2 = fs.point_to_point(flatsys, Tf, x0, u0, xf, uf, basis=basis) # Verify that the trajectory computation is correct T = np.linspace(0, Tf, 10) @@ -153,7 +263,12 @@ def test_flat_default_output(self, vehicle_flat): resp2 = ct.input_output_response(flatsys, T, u1, x0) np.testing.assert_array_almost_equal(resp1.outputs[0:2], resp2.outputs) - def test_flat_cost_constr(self): + @pytest.mark.parametrize("basis", [ + fs.PolyFamily(8), + fs.BSplineFamily([0, 5, 10], 6), + fs.BSplineFamily([0, 3, 7, 10], 4, 2) + ]) + def test_flat_cost_constr(self, basis): # Double integrator system sys = ct.ss([[0, 1], [0, 0]], [[0], [1]], [[1, 0]], 0) flat_sys = fs.LinearFlatSystem(sys) @@ -162,11 +277,11 @@ def test_flat_cost_constr(self): x0 = [1, 0]; u0 = [0] xf = [0, 0]; uf = [0] Tf = 10 - T = np.linspace(0, Tf, 500) + T = np.linspace(0, Tf, 100) # Find trajectory between initial and final conditions traj = fs.point_to_point( - flat_sys, Tf, x0, u0, xf, uf, basis=fs.PolyFamily(8)) + flat_sys, Tf, x0, u0, xf, uf, basis=basis) x, u = traj.eval(T) np.testing.assert_array_almost_equal(x0, x[:, 0]) @@ -181,7 +296,7 @@ def test_flat_cost_constr(self): traj_cost = fs.point_to_point( flat_sys, timepts, x0, u0, xf, uf, cost=cost_fcn, - basis=fs.PolyFamily(8), + basis=basis, # initial_guess='lstsq', # minimize_kwargs={'method': 'trust-constr'} ) @@ -207,11 +322,14 @@ def test_flat_cost_constr(self): traj_const = fs.point_to_point( flat_sys, timepts, x0, u0, xf, uf, cost=cost_fcn, - constraints=constraints, basis=fs.PolyFamily(8), + constraints=constraints, basis=basis, + # minimize_kwargs={'method': 'trust-constr'} ) + assert traj_const.success # Verify that the trajectory computation is correct - x_const, u_const = traj_const.eval(T) + x_cost, u_cost = traj_cost.eval(timepts) # re-eval on timepts + x_const, u_const = traj_const.eval(timepts) np.testing.assert_array_almost_equal(x0, x_const[:, 0]) np.testing.assert_array_almost_equal(u0, u_const[:, 0]) np.testing.assert_array_almost_equal(xf, x_const[:, -1]) @@ -219,17 +337,115 @@ def test_flat_cost_constr(self): # Make sure that the solution respects the bounds (with some slop) for i in range(x_const.shape[0]): - assert np.all(x_const[i] >= lb[i] * 1.02) - assert np.all(x_const[i] <= ub[i] * 1.02) + assert all(lb[i] - x_const[i] < rtol * abs(lb[i]) + atol) + assert all(x_const[i] - ub[i] < rtol * abs(ub[i]) + atol) # Solve the same problem with a nonlinear constraint type nl_constraints = [ (sp.optimize.NonlinearConstraint, lambda x, u: x, lb, ub)] traj_nlconst = fs.point_to_point( flat_sys, timepts, x0, u0, xf, uf, cost=cost_fcn, - constraints=nl_constraints, basis=fs.PolyFamily(8), + constraints=nl_constraints, basis=basis, + ) + x_nlconst, u_nlconst = traj_nlconst.eval(timepts) + np.testing.assert_almost_equal(x_const, x_nlconst, decimal=2) + np.testing.assert_almost_equal(u_const, u_nlconst, decimal=2) + + @pytest.mark.parametrize("basis", [ + # fs.PolyFamily(8), + fs.BSplineFamily([0, 3, 7, 10], 5, 2)]) + def test_flat_solve_ocp(self, basis): + # Double integrator system + sys = ct.ss([[0, 1], [0, 0]], [[0], [1]], [[1, 0]], 0) + flat_sys = fs.LinearFlatSystem(sys) + + # Define the endpoints of the trajectory + x0 = [1, 0]; u0 = [0] + xf = [-1, 0]; uf = [0] + Tf = 10 + T = np.linspace(0, Tf, 100) + + # Find trajectory between initial and final conditions + traj = fs.point_to_point( + flat_sys, Tf, x0, u0, xf, uf, basis=basis) + x, u = traj.eval(T) + + np.testing.assert_array_almost_equal(x0, x[:, 0]) + np.testing.assert_array_almost_equal(u0, u[:, 0]) + np.testing.assert_array_almost_equal(xf, x[:, -1]) + np.testing.assert_array_almost_equal(uf, u[:, -1]) + + # Solve with a terminal cost function + timepts = np.linspace(0, Tf, 10) + terminal_cost = opt.quadratic_cost( + flat_sys, 1e3, 1e3, x0=xf, u0=uf) + + traj_cost = fs.solve_flat_ocp( + flat_sys, timepts, x0, u0, + terminal_cost=terminal_cost, basis=basis) + + # Verify that the trajectory computation is correct + x_cost, u_cost = traj_cost.eval(T) + np.testing.assert_array_almost_equal(x0, x_cost[:, 0]) + np.testing.assert_array_almost_equal(u0, u_cost[:, 0]) + np.testing.assert_array_almost_equal(xf, x_cost[:, -1]) + np.testing.assert_array_almost_equal(uf, u_cost[:, -1]) + + # Solve with trajectory and terminal cost functions + trajectory_cost = opt.quadratic_cost(flat_sys, 0, 1, x0=xf, u0=uf) + + traj_cost = fs.solve_flat_ocp( + flat_sys, timepts, x0, u0, terminal_cost=terminal_cost, + trajectory_cost=trajectory_cost, basis=basis) + + # Verify that the trajectory computation is correct + x_cost, u_cost = traj_cost.eval(T) + np.testing.assert_array_almost_equal(x0, x_cost[:, 0]) + np.testing.assert_array_almost_equal(u0, u_cost[:, 0]) + + # Make sure we got close on the terminal condition + assert all(np.abs(x_cost[:, -1] - xf) < 0.1) + + # Make sure that we got a different answer than before + assert np.any(np.abs(x - x_cost) > 0.1) + + # Re-solve with constraint on the y deviation + lb, ub = [-2, np.min(x_cost[1])*0.95], [2, 1] + constraints = [opt.state_range_constraint(flat_sys, lb, ub)] + + # Make sure that the previous solution violated at least one constraint + assert np.any(x_cost[0, :] < lb[0]) or np.any(x_cost[0, :] > ub[0]) \ + or np.any(x_cost[1, :] < lb[1]) or np.any(x_cost[1, :] > ub[1]) + + traj_const = fs.solve_flat_ocp( + flat_sys, timepts, x0, u0, + terminal_cost=terminal_cost, trajectory_cost=trajectory_cost, + trajectory_constraints=constraints, basis=basis, + ) + + # Verify that the trajectory computation is correct + x_const, u_const = traj_const.eval(timepts) + np.testing.assert_array_almost_equal(x0, x_const[:, 0]) + np.testing.assert_array_almost_equal(u0, u_const[:, 0]) + + # Make sure we got close on the terminal condition + assert all(np.abs(x_cost[:, -1] - xf) < 0.1) + + # Make sure that the solution respects the bounds (with some slop) + for i in range(x_const.shape[0]): + assert all(lb[i] - x_const[i] < rtol * abs(lb[i]) + atol) + assert all(x_const[i] - ub[i] < rtol * abs(ub[i]) + atol) + + # Solve the same problem with a nonlinear constraint type + # Use alternative keywords as well + nl_constraints = [ + (sp.optimize.NonlinearConstraint, lambda x, u: x, lb, ub)] + traj_nlconst = fs.solve_flat_ocp( + flat_sys, timepts, x0, u0, + cost=trajectory_cost, terminal_cost=terminal_cost, + constraints=nl_constraints, basis=basis, ) - x_nlconst, u_nlconst = traj_nlconst.eval(T) + x_nlconst, u_nlconst = traj_nlconst.eval(timepts) np.testing.assert_almost_equal(x_const, x_nlconst) np.testing.assert_almost_equal(u_const, u_nlconst) @@ -270,6 +486,26 @@ def test_bezier_basis(self): with pytest.raises(ValueError, match="index too high"): bezier.eval_deriv(4, 0, time) + @pytest.mark.parametrize("basis, degree, T", [ + (fs.PolyFamily(4), 4, 1), + (fs.PolyFamily(4, 100), 4, 100), + (fs.BezierFamily(4), 4, 1), + (fs.BezierFamily(4, 100), 4, 100), + (fs.BSplineFamily([0, 0.5, 1], 4), 3, 1), + (fs.BSplineFamily([0, 50, 100], 4), 3, 100), + ]) + def test_basis_derivs(self, basis, degree, T): + """Make sure that that basis function derivates are correct""" + timepts = np.linspace(0, T, 10000) + dt = timepts[1] - timepts[0] + for i in range(basis.N): + for j in range(degree-1): + # Compare numerical and analytical derivative + np.testing.assert_allclose( + np.diff(basis.eval_deriv(i, j, timepts)) / dt, + basis.eval_deriv(i, j+1, timepts)[0:-1], + atol=1e-2, rtol=1e-4) + def test_point_to_point_errors(self): """Test error and warning conditions in point_to_point()""" # Double integrator system @@ -358,10 +594,11 @@ def test_point_to_point_errors(self): # Unsolvable optimization constraint = [opt.input_range_constraint(flat_sys, -0.01, 0.01)] - with pytest.raises(RuntimeError, match="Unable to solve optimal"): + with pytest.warns(UserWarning, match="unable to solve"): traj = fs.point_to_point( flat_sys, timepts, x0, u0, xf, uf, constraints=constraint, basis=fs.PolyFamily(8)) + assert not traj.success # Method arguments, parameters traj_method = fs.point_to_point( @@ -379,6 +616,83 @@ def test_point_to_point_errors(self): traj_method = fs.point_to_point( flat_sys, timepts, x0, u0, xf, uf, solve_ivp_method=None) + def test_solve_flat_ocp_errors(self): + """Test error and warning conditions in point_to_point()""" + # Double integrator system + sys = ct.ss([[0, 1], [0, 0]], [[0], [1]], [[1, 0]], 0) + flat_sys = fs.LinearFlatSystem(sys) + + # Define the endpoints of the trajectory + x0 = [1, 0]; u0 = [0] + xf = [0, 0]; uf = [0] + Tf = 10 + T = np.linspace(0, Tf, 500) + + # Cost function + timepts = np.linspace(0, Tf, 10) + cost_fcn = opt.quadratic_cost( + flat_sys, np.diag([1, 1]), 1, x0=xf, u0=uf) + + # Solving without basis specified should be OK (may generate warning) + with warnings.catch_warnings(): + warnings.simplefilter("ignore") + traj = fs.solve_flat_ocp(flat_sys, timepts, x0, u0, cost_fcn) + x, u = traj.eval(timepts) + np.testing.assert_array_almost_equal(x0, x[:, 0]) + if not traj.success: + # If unsuccessful, make sure the error is just about precision + assert re.match(".* precision loss.*", traj.message) is not None + + x, u = traj.eval(timepts) + np.testing.assert_array_almost_equal(x0, x[:, 0]) + np.testing.assert_array_almost_equal(u0, u[:, 0]) + + # Solving without a cost function generates an error + with pytest.raises(TypeError, match="cost required"): + traj = fs.solve_flat_ocp(flat_sys, timepts, x0, u0) + + # Try to optimize with insufficient degrees of freedom + with pytest.raises(ValueError, match="basis set is too small"): + traj = fs.solve_flat_ocp( + flat_sys, timepts, x0, u0, cost=cost_fcn, + basis=fs.PolyFamily(2)) + + # Solve with the errors in the various input arguments + with pytest.raises(ValueError, match="Initial state: Wrong shape"): + traj = fs.solve_flat_ocp( + flat_sys, timepts, np.zeros(3), u0, cost_fcn) + with pytest.raises(ValueError, match="Initial input: Wrong shape"): + traj = fs.solve_flat_ocp( + flat_sys, timepts, x0, np.zeros(3), cost_fcn) + + # Constraint that isn't a constraint + with pytest.raises(TypeError, match="must be a list"): + traj = fs.solve_flat_ocp( + flat_sys, timepts, x0, u0, cost_fcn, + constraints=np.eye(2), basis=fs.PolyFamily(8)) + + # Unknown constraint type + with pytest.raises(TypeError, match="unknown constraint type"): + traj = fs.solve_flat_ocp( + flat_sys, timepts, x0, u0, cost_fcn, + constraints=[(None, 0, 0, 0)], basis=fs.PolyFamily(8)) + + # Method arguments, parameters + traj_method = fs.solve_flat_ocp( + flat_sys, timepts, x0, u0, cost=cost_fcn, + basis=fs.PolyFamily(6), minimize_method='slsqp') + traj_kwarg = fs.solve_flat_ocp( + flat_sys, timepts, x0, u0, cost=cost_fcn, + basis=fs.PolyFamily(6), minimize_kwargs={'method': 'slsqp'}) + np.testing.assert_allclose( + traj_method.eval(timepts)[0], traj_kwarg.eval(timepts)[0], + atol=1e-5) + + # Unrecognized keywords + with pytest.raises(TypeError, match="unrecognized keyword"): + traj_method = fs.solve_flat_ocp( + flat_sys, timepts, x0, u0, cost_fcn, solve_ivp_method=None) + @pytest.mark.parametrize( "xf, uf, Tf", [([1, 0], [0], 2), @@ -390,10 +704,10 @@ def test_response(self, xf, uf, Tf): flatsys = fs.LinearFlatSystem(sys) # Define the basis set - poly = fs.PolyFamily(6) + basis = fs.PolyFamily(6) x1, u1, = [0, 0], [0] - traj = fs.point_to_point(flatsys, Tf, x1, u1, xf, uf, basis=poly) + traj = fs.point_to_point(flatsys, Tf, x1, u1, xf, uf, basis=basis) # Compute the response the regular way T = np.linspace(0, Tf, 10) @@ -401,6 +715,46 @@ def test_response(self, xf, uf, Tf): # Recompute using response() response = traj.response(T, squeeze=False) - np.testing.assert_equal(T, response.time) - np.testing.assert_equal(u, response.inputs) - np.testing.assert_equal(x, response.states) + np.testing.assert_array_almost_equal(T, response.time) + np.testing.assert_array_almost_equal(u, response.inputs) + np.testing.assert_array_almost_equal(x, response.states) + + @pytest.mark.parametrize( + "basis", + [fs.PolyFamily(4), + fs.BezierFamily(4), + fs.BSplineFamily([0, 1], 4), + fs.BSplineFamily([0, 1], 4, vars=2), + fs.BSplineFamily([0, 1], [4, 3], [2, 1], vars=2), + ]) + def test_basis_class(self, basis): + timepts = np.linspace(0, 1, 10) + + if basis.nvars is None: + # Evaluate function on basis vectors + for j in range(basis.N): + coefs = np.zeros(basis.N) + coefs[j] = 1 + np.testing.assert_array_almost_equal( + basis.eval(coefs, timepts), + basis.eval_deriv(j, 0, timepts)) + else: + # Evaluate each variable on basis vectors + for i in range(basis.nvars): + for j in range(basis.var_ncoefs(i)): + coefs = np.zeros(basis.var_ncoefs(i)) + coefs[j] = 1 + np.testing.assert_array_almost_equal( + basis.eval(coefs, timepts, var=i), + basis.eval_deriv(j, 0, timepts, var=i)) + + # Evaluate multi-variable output + offset = 0 + for i in range(basis.nvars): + for j in range(basis.var_ncoefs(i)): + coefs = np.zeros(basis.N) + coefs[offset] = 1 + np.testing.assert_array_almost_equal( + basis.eval(coefs, timepts)[i], + basis.eval_deriv(j, 0, timepts, var=i)) + offset += 1 diff --git a/control/tests/frd_test.py b/control/tests/frd_test.py index ff88c3dea..1a383c2a7 100644 --- a/control/tests/frd_test.py +++ b/control/tests/frd_test.py @@ -51,72 +51,74 @@ def testOperators(self): h1 = TransferFunction([1], [1, 2, 2]) h2 = TransferFunction([1], [0.1, 1]) omega = np.logspace(-1, 2, 10) + chkpts = omega[::3] f1 = FRD(h1, omega) f2 = FRD(h2, omega) np.testing.assert_array_almost_equal( - (f1 + f2).frequency_response([0.1, 1.0, 10])[0], - (h1 + h2).frequency_response([0.1, 1.0, 10])[0]) + (f1 + f2).frequency_response(chkpts)[0], + (h1 + h2).frequency_response(chkpts)[0]) np.testing.assert_array_almost_equal( - (f1 + f2).frequency_response([0.1, 1.0, 10])[1], - (h1 + h2).frequency_response([0.1, 1.0, 10])[1]) + (f1 + f2).frequency_response(chkpts)[1], + (h1 + h2).frequency_response(chkpts)[1]) np.testing.assert_array_almost_equal( - (f1 - f2).frequency_response([0.1, 1.0, 10])[0], - (h1 - h2).frequency_response([0.1, 1.0, 10])[0]) + (f1 - f2).frequency_response(chkpts)[0], + (h1 - h2).frequency_response(chkpts)[0]) np.testing.assert_array_almost_equal( - (f1 - f2).frequency_response([0.1, 1.0, 10])[1], - (h1 - h2).frequency_response([0.1, 1.0, 10])[1]) + (f1 - f2).frequency_response(chkpts)[1], + (h1 - h2).frequency_response(chkpts)[1]) # multiplication and division np.testing.assert_array_almost_equal( - (f1 * f2).frequency_response([0.1, 1.0, 10])[1], - (h1 * h2).frequency_response([0.1, 1.0, 10])[1]) + (f1 * f2).frequency_response(chkpts)[1], + (h1 * h2).frequency_response(chkpts)[1]) np.testing.assert_array_almost_equal( - (f1 / f2).frequency_response([0.1, 1.0, 10])[1], - (h1 / h2).frequency_response([0.1, 1.0, 10])[1]) + (f1 / f2).frequency_response(chkpts)[1], + (h1 / h2).frequency_response(chkpts)[1]) # with default conversion from scalar np.testing.assert_array_almost_equal( - (f1 * 1.5).frequency_response([0.1, 1.0, 10])[1], - (h1 * 1.5).frequency_response([0.1, 1.0, 10])[1]) + (f1 * 1.5).frequency_response(chkpts)[1], + (h1 * 1.5).frequency_response(chkpts)[1]) np.testing.assert_array_almost_equal( - (f1 / 1.7).frequency_response([0.1, 1.0, 10])[1], - (h1 / 1.7).frequency_response([0.1, 1.0, 10])[1]) + (f1 / 1.7).frequency_response(chkpts)[1], + (h1 / 1.7).frequency_response(chkpts)[1]) np.testing.assert_array_almost_equal( - (2.2 * f2).frequency_response([0.1, 1.0, 10])[1], - (2.2 * h2).frequency_response([0.1, 1.0, 10])[1]) + (2.2 * f2).frequency_response(chkpts)[1], + (2.2 * h2).frequency_response(chkpts)[1]) np.testing.assert_array_almost_equal( - (1.3 / f2).frequency_response([0.1, 1.0, 10])[1], - (1.3 / h2).frequency_response([0.1, 1.0, 10])[1]) + (1.3 / f2).frequency_response(chkpts)[1], + (1.3 / h2).frequency_response(chkpts)[1]) def testOperatorsTf(self): # get two SISO transfer functions h1 = TransferFunction([1], [1, 2, 2]) h2 = TransferFunction([1], [0.1, 1]) omega = np.logspace(-1, 2, 10) + chkpts = omega[::3] f1 = FRD(h1, omega) f2 = FRD(h2, omega) f2 # reference to avoid pyflakes error np.testing.assert_array_almost_equal( - (f1 + h2).frequency_response([0.1, 1.0, 10])[0], - (h1 + h2).frequency_response([0.1, 1.0, 10])[0]) + (f1 + h2).frequency_response(chkpts)[0], + (h1 + h2).frequency_response(chkpts)[0]) np.testing.assert_array_almost_equal( - (f1 + h2).frequency_response([0.1, 1.0, 10])[1], - (h1 + h2).frequency_response([0.1, 1.0, 10])[1]) + (f1 + h2).frequency_response(chkpts)[1], + (h1 + h2).frequency_response(chkpts)[1]) np.testing.assert_array_almost_equal( - (f1 - h2).frequency_response([0.1, 1.0, 10])[0], - (h1 - h2).frequency_response([0.1, 1.0, 10])[0]) + (f1 - h2).frequency_response(chkpts)[0], + (h1 - h2).frequency_response(chkpts)[0]) np.testing.assert_array_almost_equal( - (f1 - h2).frequency_response([0.1, 1.0, 10])[1], - (h1 - h2).frequency_response([0.1, 1.0, 10])[1]) + (f1 - h2).frequency_response(chkpts)[1], + (h1 - h2).frequency_response(chkpts)[1]) # multiplication and division np.testing.assert_array_almost_equal( - (f1 * h2).frequency_response([0.1, 1.0, 10])[1], - (h1 * h2).frequency_response([0.1, 1.0, 10])[1]) + (f1 * h2).frequency_response(chkpts)[1], + (h1 * h2).frequency_response(chkpts)[1]) np.testing.assert_array_almost_equal( - (f1 / h2).frequency_response([0.1, 1.0, 10])[1], - (h1 / h2).frequency_response([0.1, 1.0, 10])[1]) + (f1 / h2).frequency_response(chkpts)[1], + (h1 / h2).frequency_response(chkpts)[1]) # the reverse does not work def testbdalg(self): @@ -124,49 +126,51 @@ def testbdalg(self): h1 = TransferFunction([1], [1, 2, 2]) h2 = TransferFunction([1], [0.1, 1]) omega = np.logspace(-1, 2, 10) + chkpts = omega[::3] f1 = FRD(h1, omega) f2 = FRD(h2, omega) np.testing.assert_array_almost_equal( - (bdalg.series(f1, f2)).frequency_response([0.1, 1.0, 10])[0], - (bdalg.series(h1, h2)).frequency_response([0.1, 1.0, 10])[0]) + (bdalg.series(f1, f2)).frequency_response(chkpts)[0], + (bdalg.series(h1, h2)).frequency_response(chkpts)[0]) np.testing.assert_array_almost_equal( - (bdalg.parallel(f1, f2)).frequency_response([0.1, 1.0, 10])[0], - (bdalg.parallel(h1, h2)).frequency_response([0.1, 1.0, 10])[0]) + (bdalg.parallel(f1, f2)).frequency_response(chkpts)[0], + (bdalg.parallel(h1, h2)).frequency_response(chkpts)[0]) np.testing.assert_array_almost_equal( - (bdalg.feedback(f1, f2)).frequency_response([0.1, 1.0, 10])[0], - (bdalg.feedback(h1, h2)).frequency_response([0.1, 1.0, 10])[0]) + (bdalg.feedback(f1, f2)).frequency_response(chkpts)[0], + (bdalg.feedback(h1, h2)).frequency_response(chkpts)[0]) np.testing.assert_array_almost_equal( - (bdalg.negate(f1)).frequency_response([0.1, 1.0, 10])[0], - (bdalg.negate(h1)).frequency_response([0.1, 1.0, 10])[0]) + (bdalg.negate(f1)).frequency_response(chkpts)[0], + (bdalg.negate(h1)).frequency_response(chkpts)[0]) # append() and connect() not implemented for FRD objects # np.testing.assert_array_almost_equal( -# (bdalg.append(f1, f2)).frequency_response([0.1, 1.0, 10])[0], -# (bdalg.append(h1, h2)).frequency_response([0.1, 1.0, 10])[0]) +# (bdalg.append(f1, f2)).frequency_response(chkpts)[0], +# (bdalg.append(h1, h2)).frequency_response(chkpts)[0]) # # f3 = bdalg.append(f1, f2, f2) # h3 = bdalg.append(h1, h2, h2) # Q = np.mat([ [1, 2], [2, -1] ]) # np.testing.assert_array_almost_equal( -# (bdalg.connect(f3, Q, [2], [1])).frequency_response([0.1, 1.0, 10])[0], -# (bdalg.connect(h3, Q, [2], [1])).frequency_response([0.1, 1.0, 10])[0]) +# (bdalg.connect(f3, Q, [2], [1])).frequency_response(chkpts)[0], +# (bdalg.connect(h3, Q, [2], [1])).frequency_response(chkpts)[0]) def testFeedback(self): h1 = TransferFunction([1], [1, 2, 2]) omega = np.logspace(-1, 2, 10) + chkpts = omega[::3] f1 = FRD(h1, omega) np.testing.assert_array_almost_equal( - f1.feedback(1).frequency_response([0.1, 1.0, 10])[0], - h1.feedback(1).frequency_response([0.1, 1.0, 10])[0]) + f1.feedback(1).frequency_response(chkpts)[0], + h1.feedback(1).frequency_response(chkpts)[0]) # Make sure default argument also works np.testing.assert_array_almost_equal( - f1.feedback().frequency_response([0.1, 1.0, 10])[0], - h1.feedback().frequency_response([0.1, 1.0, 10])[0]) + f1.feedback().frequency_response(chkpts)[0], + h1.feedback().frequency_response(chkpts)[0]) def testFeedback2(self): h2 = StateSpace([[-1.0, 0], [0, -2.0]], [[0.4], [0.1]], @@ -197,13 +201,14 @@ def testMIMO(self): [[1.0, 0.0], [0.0, 1.0]], [[0.0, 0.0], [0.0, 0.0]]) omega = np.logspace(-1, 2, 10) + chkpts = omega[::3] f1 = FRD(sys, omega) np.testing.assert_array_almost_equal( - sys.frequency_response([0.1, 1.0, 10])[0], - f1.frequency_response([0.1, 1.0, 10])[0]) + sys.frequency_response(chkpts)[0], + f1.frequency_response(chkpts)[0]) np.testing.assert_array_almost_equal( - sys.frequency_response([0.1, 1.0, 10])[1], - f1.frequency_response([0.1, 1.0, 10])[1]) + sys.frequency_response(chkpts)[1], + f1.frequency_response(chkpts)[1]) @slycotonly def testMIMOfb(self): @@ -212,14 +217,15 @@ def testMIMOfb(self): [[1.0, 0.0], [0.0, 1.0]], [[0.0, 0.0], [0.0, 0.0]]) omega = np.logspace(-1, 2, 10) + chkpts = omega[::3] f1 = FRD(sys, omega).feedback([[0.1, 0.3], [0.0, 1.0]]) f2 = FRD(sys.feedback([[0.1, 0.3], [0.0, 1.0]]), omega) np.testing.assert_array_almost_equal( - f1.frequency_response([0.1, 1.0, 10])[0], - f2.frequency_response([0.1, 1.0, 10])[0]) + f1.frequency_response(chkpts)[0], + f2.frequency_response(chkpts)[0]) np.testing.assert_array_almost_equal( - f1.frequency_response([0.1, 1.0, 10])[1], - f2.frequency_response([0.1, 1.0, 10])[1]) + f1.frequency_response(chkpts)[1], + f2.frequency_response(chkpts)[1]) @slycotonly def testMIMOfb2(self): @@ -229,15 +235,16 @@ def testMIMOfb2(self): np.array([[1.0, 0], [0, 0], [0, 1]]), np.eye(3), np.zeros((3, 2))) omega = np.logspace(-1, 2, 10) + chkpts = omega[::3] K = np.array([[1, 0.3, 0], [0.1, 0, 0]]) f1 = FRD(sys, omega).feedback(K) f2 = FRD(sys.feedback(K), omega) np.testing.assert_array_almost_equal( - f1.frequency_response([0.1, 1.0, 10])[0], - f2.frequency_response([0.1, 1.0, 10])[0]) + f1.frequency_response(chkpts)[0], + f2.frequency_response(chkpts)[0]) np.testing.assert_array_almost_equal( - f1.frequency_response([0.1, 1.0, 10])[1], - f2.frequency_response([0.1, 1.0, 10])[1]) + f1.frequency_response(chkpts)[1], + f2.frequency_response(chkpts)[1]) @slycotonly def testMIMOMult(self): @@ -246,14 +253,15 @@ def testMIMOMult(self): [[1.0, 0.0], [0.0, 1.0]], [[0.0, 0.0], [0.0, 0.0]]) omega = np.logspace(-1, 2, 10) + chkpts = omega[::3] f1 = FRD(sys, omega) f2 = FRD(sys, omega) np.testing.assert_array_almost_equal( - (f1*f2).frequency_response([0.1, 1.0, 10])[0], - (sys*sys).frequency_response([0.1, 1.0, 10])[0]) + (f1*f2).frequency_response(chkpts)[0], + (sys*sys).frequency_response(chkpts)[0]) np.testing.assert_array_almost_equal( - (f1*f2).frequency_response([0.1, 1.0, 10])[1], - (sys*sys).frequency_response([0.1, 1.0, 10])[1]) + (f1*f2).frequency_response(chkpts)[1], + (sys*sys).frequency_response(chkpts)[1]) @slycotonly def testMIMOSmooth(self): @@ -263,17 +271,18 @@ def testMIMOSmooth(self): [[0.0, 0.0], [0.0, 0.0], [0.0, 0.0]]) sys2 = np.array([[1, 0, 0], [0, 1, 0]]) * sys omega = np.logspace(-1, 2, 10) + chkpts = omega[::3] f1 = FRD(sys, omega, smooth=True) f2 = FRD(sys2, omega, smooth=True) np.testing.assert_array_almost_equal( - (f1*f2).frequency_response([0.1, 1.0, 10])[0], - (sys*sys2).frequency_response([0.1, 1.0, 10])[0]) + (f1*f2).frequency_response(chkpts)[0], + (sys*sys2).frequency_response(chkpts)[0]) np.testing.assert_array_almost_equal( - (f1*f2).frequency_response([0.1, 1.0, 10])[1], - (sys*sys2).frequency_response([0.1, 1.0, 10])[1]) + (f1*f2).frequency_response(chkpts)[1], + (sys*sys2).frequency_response(chkpts)[1]) np.testing.assert_array_almost_equal( - (f1*f2).frequency_response([0.1, 1.0, 10])[2], - (sys*sys2).frequency_response([0.1, 1.0, 10])[2]) + (f1*f2).frequency_response(chkpts)[2], + (sys*sys2).frequency_response(chkpts)[2]) def testAgainstOctave(self): # with data from octave: @@ -284,6 +293,7 @@ def testAgainstOctave(self): np.array([[1.0, 0], [0, 0], [0, 1]]), np.eye(3), np.zeros((3, 2))) omega = np.logspace(-1, 2, 10) + chkpts = omega[::3] f1 = FRD(sys, omega) np.testing.assert_array_almost_equal( (f1.frequency_response([1.0])[0] * diff --git a/control/tests/interconnect_test.py b/control/tests/interconnect_test.py index 3b99adc6e..2c29aeaca 100644 --- a/control/tests/interconnect_test.py +++ b/control/tests/interconnect_test.py @@ -230,3 +230,25 @@ def test_string_inputoutput(): P_s2 = ct.interconnect([P1_iosys, P2_iosys], inputs=['u1'], output='y2') assert P_s2.output_index == {'y2' : 0} + +def test_linear_interconnect(): + tf_ctrl = ct.tf(1, (10.1, 1), inputs='e', outputs='u') + tf_plant = ct.tf(1, (10.1, 1), inputs='u', outputs='y') + ss_ctrl = ct.ss(1, 2, 1, 2, inputs='e', outputs='u') + ss_plant = ct.ss(1, 2, 1, 2, inputs='u', outputs='y') + nl_ctrl = ct.NonlinearIOSystem( + lambda t, x, u, params: x*x, + lambda t, x, u, params: u*x, states=1, inputs='e', outputs='u') + nl_plant = ct.NonlinearIOSystem( + lambda t, x, u, params: x*x, + lambda t, x, u, params: u*x, states=1, inputs='u', outputs='y') + + assert isinstance(ct.interconnect((tf_ctrl, tf_plant), inputs='e', outputs='y'), ct.LinearIOSystem) + assert isinstance(ct.interconnect((ss_ctrl, ss_plant), inputs='e', outputs='y'), ct.LinearIOSystem) + assert isinstance(ct.interconnect((tf_ctrl, ss_plant), inputs='e', outputs='y'), ct.LinearIOSystem) + assert isinstance(ct.interconnect((ss_ctrl, tf_plant), inputs='e', outputs='y'), ct.LinearIOSystem) + + assert ~isinstance(ct.interconnect((nl_ctrl, ss_plant), inputs='e', outputs='y'), ct.LinearIOSystem) + assert ~isinstance(ct.interconnect((nl_ctrl, tf_plant), inputs='e', outputs='y'), ct.LinearIOSystem) + assert ~isinstance(ct.interconnect((ss_ctrl, nl_plant), inputs='e', outputs='y'), ct.LinearIOSystem) + assert ~isinstance(ct.interconnect((tf_ctrl, nl_plant), inputs='e', outputs='y'), ct.LinearIOSystem) \ No newline at end of file diff --git a/control/tests/iosys_test.py b/control/tests/iosys_test.py index ecb30c316..fa26ded43 100644 --- a/control/tests/iosys_test.py +++ b/control/tests/iosys_test.py @@ -9,13 +9,16 @@ """ import re +import warnings +import pytest import numpy as np -import pytest +from math import sqrt import control as ct from control import iosys as ios -from control.tests.conftest import noscipy0, matrixfilter +from control.tests.conftest import matrixfilter + class TestIOSys: @@ -39,6 +42,9 @@ class TSys: [[-1, 1], [0, -2]], [[0, 1], [1, 0]], [[1, 0], [0, 1]], np.zeros((2, 2))) + # Create a static gain linear system + T.staticgain = ct.StateSpace([], [], [], 1) + # Create simulation parameters T.T = np.linspace(0, 10, 100) T.U = np.sin(T.T) @@ -46,11 +52,10 @@ class TSys: return T - @noscipy0 def test_linear_iosys(self, tsys): # Create an input/output system from the linear system linsys = tsys.siso_linsys - iosys = ios.LinearIOSystem(linsys) + iosys = ios.LinearIOSystem(linsys).copy() # Make sure that the right hand side matches linear system for x, u in (([0, 0], 0), ([1, 0], 0), ([0, 1], 0), ([0, 0], 1)): @@ -65,7 +70,11 @@ def test_linear_iosys(self, tsys): np.testing.assert_array_almost_equal(lti_t, ios_t) np.testing.assert_allclose(lti_y, ios_y, atol=0.002, rtol=0.) - @noscipy0 + # Make sure that a static linear system has dt=None + # and otherwise dt is as specified + assert ios.LinearIOSystem(tsys.staticgain).dt is None + assert ios.LinearIOSystem(tsys.staticgain, dt=.1).dt == .1 + def test_tf2io(self, tsys): # Create a transfer function from the state space system linsys = tsys.siso_linsys @@ -129,7 +138,6 @@ def test_iosys_print(self, tsys, capsys): ios_linearized = ios.linearize(ios_unspecified, [0, 0], [0]) print(ios_linearized) - @noscipy0 @pytest.mark.parametrize("ss", [ios.NonlinearIOSystem, ct.ss]) def test_nonlinear_iosys(self, tsys, ss): # Create a simple nonlinear I/O system @@ -209,7 +217,7 @@ def test_linearize(self, tsys, kincar): @pytest.mark.usefixtures("editsdefaults") def test_linearize_named_signals(self, kincar): # Full form of the call - linearized = kincar.linearize([0, 0, 0], [0, 0], copy=True, + linearized = kincar.linearize([0, 0, 0], [0, 0], copy_names=True, name='linearized') assert linearized.name == 'linearized' assert linearized.find_input('v') == 0 @@ -221,22 +229,35 @@ def test_linearize_named_signals(self, kincar): assert linearized.find_state('theta') == 2 # If we copy signal names w/out a system name, append '$linearized' - linearized = kincar.linearize([0, 0, 0], [0, 0], copy=True) + linearized = kincar.linearize([0, 0, 0], [0, 0], copy_names=True) assert linearized.name == kincar.name + '$linearized' - # Test legacy version as well - ct.use_legacy_defaults('0.8.4') - ct.config.use_numpy_matrix(False) # np.matrix deprecated - linearized = kincar.linearize([0, 0, 0], [0, 0], copy=True) - assert linearized.name == kincar.name + '_linearized' - # If copy is False, signal names should not be copied - lin_nocopy = kincar.linearize(0, 0, copy=False) + lin_nocopy = kincar.linearize(0, 0, copy_names=False) assert lin_nocopy.find_input('v') is None assert lin_nocopy.find_output('x') is None assert lin_nocopy.find_state('x') is None - @noscipy0 + # if signal names are provided, they should override those of kincar + linearized_newnames = kincar.linearize([0, 0, 0], [0, 0], + name='linearized', + copy_names=True, inputs=['v2', 'phi2'], outputs=['x2','y2']) + assert linearized_newnames.name == 'linearized' + assert linearized_newnames.find_input('v2') == 0 + assert linearized_newnames.find_input('phi2') == 1 + assert linearized_newnames.find_input('v') is None + assert linearized_newnames.find_input('phi') is None + assert linearized_newnames.find_output('x2') == 0 + assert linearized_newnames.find_output('y2') == 1 + assert linearized_newnames.find_output('x') is None + assert linearized_newnames.find_output('y') is None + + # Test legacy version as well + ct.use_legacy_defaults('0.8.4') + ct.config.use_numpy_matrix(False) # np.matrix deprecated + linearized = kincar.linearize([0, 0, 0], [0, 0], copy_names=True) + assert linearized.name == kincar.name + '_linearized' + def test_connect(self, tsys): # Define a couple of (linear) systems to interconnection linsys1 = tsys.siso_linsys @@ -294,7 +315,6 @@ def test_connect(self, tsys): np.testing.assert_array_almost_equal(lti_t, ios_t) np.testing.assert_allclose(lti_y, ios_y,atol=0.002,rtol=0.) - @noscipy0 @pytest.mark.parametrize( "connections, inplist, outlist", [pytest.param([[(1, 0), (0, 0, 1)]], [[(0, 0, 1)]], [[(1, 0, 1)]], @@ -338,7 +358,6 @@ def test_connect_spec_variants(self, tsys, connections, inplist, outlist): np.testing.assert_array_almost_equal(lti_t, ios_t) np.testing.assert_allclose(lti_y, ios_y, atol=0.002, rtol=0.) - @noscipy0 @pytest.mark.parametrize( "connections, inplist, outlist", [pytest.param([['sys2.u[0]', 'sys1.y[0]']], @@ -375,7 +394,6 @@ def test_connect_spec_warnings(self, tsys, connections, inplist, outlist): np.testing.assert_array_almost_equal(lti_t, ios_t) np.testing.assert_allclose(lti_y, ios_y, atol=0.002, rtol=0.) - @noscipy0 def test_static_nonlinearity(self, tsys): # Linear dynamical system linsys = tsys.siso_linsys @@ -400,7 +418,6 @@ def test_static_nonlinearity(self, tsys): np.testing.assert_array_almost_equal(lti_y, ios_y, decimal=2) - @noscipy0 @pytest.mark.filterwarnings("ignore:Duplicate name::control.iosys") def test_algebraic_loop(self, tsys): # Create some linear and nonlinear systems to play with @@ -470,7 +487,6 @@ def test_algebraic_loop(self, tsys): with pytest.raises(RuntimeError): ios.input_output_response(*args) - @noscipy0 def test_summer(self, tsys): # Construct a MIMO system for testing linsys = tsys.mimo_linsys1 @@ -489,7 +505,6 @@ def test_summer(self, tsys): ios_t, ios_y = ios.input_output_response(iosys_parallel, T, U, X0) np.testing.assert_allclose(ios_y, lin_y,atol=0.002,rtol=0.) - @noscipy0 def test_rmul(self, tsys): # Test right multiplication # TODO: replace with better tests when conversions are implemented @@ -510,7 +525,6 @@ def test_rmul(self, tsys): lti_t, lti_y = ct.forced_response(ioslin, T, U*U, X0) np.testing.assert_array_almost_equal(ios_y, lti_y*lti_y, decimal=3) - @noscipy0 def test_neg(self, tsys): """Test negation of a system""" @@ -533,7 +547,6 @@ def test_neg(self, tsys): lti_t, lti_y = ct.forced_response(ioslin, T, U*U, X0) np.testing.assert_array_almost_equal(ios_y, -lti_y, decimal=3) - @noscipy0 def test_feedback(self, tsys): # Set up parameters for simulation T, U, X0 = tsys.T, tsys.U, tsys.X0 @@ -549,7 +562,6 @@ def test_feedback(self, tsys): lti_t, lti_y = ct.forced_response(linsys, T, U, X0) np.testing.assert_allclose(ios_y, lti_y,atol=0.002,rtol=0.) - @noscipy0 def test_bdalg_functions(self, tsys): """Test block diagram functions algebra on I/O systems""" # Set up parameters for simulation @@ -596,7 +608,6 @@ def test_bdalg_functions(self, tsys): ios_t, ios_y = ios.input_output_response(iosys_feedback, T, U, X0) np.testing.assert_allclose(ios_y, lin_y,atol=0.002,rtol=0.) - @noscipy0 def test_algebraic_functions(self, tsys): """Test algebraic operations on I/O systems""" # Set up parameters for simulation @@ -648,7 +659,6 @@ def test_algebraic_functions(self, tsys): ios_t, ios_y = ios.input_output_response(iosys_negate, T, U, X0) np.testing.assert_allclose(ios_y, lin_y,atol=0.002,rtol=0.) - @noscipy0 def test_nonsquare_bdalg(self, tsys): # Set up parameters for simulation T = tsys.T @@ -699,7 +709,6 @@ def test_nonsquare_bdalg(self, tsys): with pytest.raises(ValueError): ct.series(*args) - @noscipy0 def test_discrete(self, tsys): """Test discrete time functionality""" # Create some linear and nonlinear systems to play with @@ -758,8 +767,8 @@ def nlsys_output(t, x, u, params): np.testing.assert_allclose(ios_t, lin_t,atol=0.002,rtol=0.) np.testing.assert_allclose(ios_y, lin_y,atol=0.002,rtol=0.) - def test_find_eqpts(self, tsys): - """Test find_eqpt function""" + def test_find_eqpts_dfan(self, tsys): + """Test find_eqpt function on dfan example""" # Simple equilibrium point with no inputs nlsys = ios.NonlinearIOSystem(predprey) xeq, ueq, result = ios.find_eqpt( @@ -828,6 +837,17 @@ def test_find_eqpts(self, tsys): np.testing.assert_array_almost_equal( nlsys_full._rhs(0, xeq, ueq)[-4:], np.zeros((4,)), decimal=5) + # Same test as before, but now all constraints are in the state vector + nlsys_full = ios.NonlinearIOSystem(pvtol_full, None) + xeq, ueq, result = ios.find_eqpt( + nlsys_full, [0, 0, 0.1, 0.1, 0, 0], [0.01, 4*9.8], + idx=[2, 3, 4, 5], ix=[0, 1, 2, 3], return_result=True) + assert result.success + np.testing.assert_array_almost_equal( + nlsys_full._out(0, xeq, ueq)[[2, 3]], [0.1, 0.1], decimal=5) + np.testing.assert_array_almost_equal( + nlsys_full._rhs(0, xeq, ueq)[-4:], np.zeros((4,)), decimal=5) + # Fix one input and vary the other nlsys_full = ios.NonlinearIOSystem(pvtol_full, None) xeq, ueq, result = ios.find_eqpt( @@ -868,7 +888,6 @@ def test_find_eqpts(self, tsys): assert xeq is None assert ueq is None - @noscipy0 def test_params(self, tsys): # Start with the default set of parameters ios_secord_default = ios.NonlinearIOSystem( @@ -1169,7 +1188,7 @@ def test_signals_naming_convention_0_8_4(self, tsys): assert "copy of namedsys.x0" in same_name_series.state_index def test_named_signals_linearize_inconsistent(self, tsys): - """Mare sure that providing inputs or outputs not consistent with + """Make sure that providing inputs or outputs not consistent with updfcn or outfcn fail """ @@ -1214,6 +1233,17 @@ def outfcn(t, x, u, params): with pytest.raises(ValueError): sys2.linearize(x0, u0) + def test_linearize_concatenation(self, kincar): + # Create a simple nonlinear system to check (kinematic car) + iosys = kincar + linearized = iosys.linearize([0, np.array([0, 0])], [0, 0]) + np.testing.assert_array_almost_equal(linearized.A, np.zeros((3,3))) + np.testing.assert_array_almost_equal( + linearized.B, [[1, 0], [0, 0], [0, 1]]) + np.testing.assert_array_almost_equal( + linearized.C, [[1, 0, 0], [0, 1, 0]]) + np.testing.assert_array_almost_equal(linearized.D, np.zeros((2,2))) + def test_lineariosys_statespace(self, tsys): """Make sure that a LinearIOSystem is also a StateSpace object""" iosys_siso = ct.LinearIOSystem(tsys.siso_linsys, name='siso') @@ -1433,11 +1463,10 @@ def test_duplicates(self, tsys): nlios2 = ios.NonlinearIOSystem(None, lambda t, x, u, params: u * u, inputs=1, outputs=1, name="nlios2") - with pytest.warns(None) as record: + with warnings.catch_warnings(): + warnings.simplefilter("error") ct.InterconnectedSystem([nlios1, iosys_siso, nlios2], inputs=0, outputs=0, states=0) - if record: - pytest.fail("Warning not expected: " + record[0].message) def test_linear_interconnection(): @@ -1462,6 +1491,11 @@ def test_linear_interconnection(): inputs = ('u[0]', 'u[1]'), outputs = ('y[0]', 'y[1]'), name = 'sys2') + tf_siso = ct.tf(1, [0.1, 1]) + ss_siso = ct.ss(1, 2, 1, 1) + nl_siso = ios.NonlinearIOSystem( + lambda t, x, u, params: x*x, + lambda t, x, u, params: u*x, states=1, inputs=1, outputs=1) # Create a "regular" InterconnectedSystem nl_connect = ios.interconnect( @@ -1508,6 +1542,18 @@ def test_linear_interconnection(): np.testing.assert_array_almost_equal(io_connect.C, ss_connect.C) np.testing.assert_array_almost_equal(io_connect.D, ss_connect.D) + # make sure interconnections of linear systems are linear and + # if a nonlinear system is included then system is nonlinear + assert isinstance(ss_siso*ss_siso, ios.LinearIOSystem) + assert isinstance(tf_siso*ss_siso, ios.LinearIOSystem) + assert isinstance(ss_siso*tf_siso, ios.LinearIOSystem) + assert ~isinstance(ss_siso*nl_siso, ios.LinearIOSystem) + assert ~isinstance(nl_siso*ss_siso, ios.LinearIOSystem) + assert ~isinstance(nl_siso*nl_siso, ios.LinearIOSystem) + assert ~isinstance(tf_siso*nl_siso, ios.LinearIOSystem) + assert ~isinstance(nl_siso*tf_siso, ios.LinearIOSystem) + assert ~isinstance(nl_siso*nl_siso, ios.LinearIOSystem) + def predprey(t, x, u, params={}): """Predator prey dynamics""" @@ -1527,7 +1573,7 @@ def predprey(t, x, u, params={}): def pvtol(t, x, u, params={}): """Reduced planar vertical takeoff and landing dynamics""" - from math import sin, cos + from math import cos, sin m = params.get('m', 4.) # kg, system mass J = params.get('J', 0.0475) # kg m^2, system inertia r = params.get('r', 0.25) # m, thrust offset @@ -1543,7 +1589,7 @@ def pvtol(t, x, u, params={}): def pvtol_full(t, x, u, params={}): - from math import sin, cos + from math import cos, sin m = params.get('m', 4.) # kg, system mass J = params.get('J', 0.0475) # kg m^2, system inertia r = params.get('r', 0.25) # m, thrust offset @@ -1596,8 +1642,12 @@ def test_interconnect_unused_input(): inputs=['r'], outputs=['y']) - with pytest.warns(None) as record: + with warnings.catch_warnings(): # no warning if output explicitly ignored, various argument forms + warnings.simplefilter("error") + # strip out matrix warnings + warnings.filterwarnings("ignore", "the matrix subclass", + category=PendingDeprecationWarning) h = ct.interconnect([g,s,k], inputs=['r'], outputs=['y'], @@ -1612,14 +1662,6 @@ def test_interconnect_unused_input(): h = ct.interconnect([g,s,k], connections=False) - #https://docs.pytest.org/en/6.2.x/warnings.html#recwarn - for r in record: - # strip out matrix warnings - if re.match(r'.*matrix subclass', str(r.message)): - continue - print(r.message) - pytest.fail(f'Unexpected warning: {r.message}') - # warn if explicity ignored input in fact used with pytest.warns( @@ -1674,7 +1716,11 @@ def test_interconnect_unused_output(): # no warning if output explicitly ignored - with pytest.warns(None) as record: + with warnings.catch_warnings(): + warnings.simplefilter("error") + # strip out matrix warnings + warnings.filterwarnings("ignore", "the matrix subclass", + category=PendingDeprecationWarning) h = ct.interconnect([g,s,k], inputs=['r'], outputs=['y'], @@ -1689,14 +1735,6 @@ def test_interconnect_unused_output(): h = ct.interconnect([g,s,k], connections=False) - #https://docs.pytest.org/en/6.2.x/warnings.html#recwarn - for r in record: - # strip out matrix warnings - if re.match(r'.*matrix subclass', str(r.message)): - continue - print(r.message) - pytest.fail(f'Unexpected warning: {r.message}') - # warn if explicity ignored output in fact used with pytest.warns( UserWarning, @@ -1763,27 +1801,40 @@ def test_input_output_broadcasting(): resp_bad = ct.input_output_response( sys, T, (U[0, :], U[:2, :-1]), [X0, P0]) - -def test_nonuniform_timepts(): +@pytest.mark.parametrize("nstates, ninputs, noutputs", [ + [2, 1, 1], + [4, 2, 3], + [0, 1, 1], # static function + [0, 3, 2], # static function +]) +def test_nonuniform_timepts(nstates, noutputs, ninputs): """Test non-uniform time points for simulations""" - sys = ct.LinearIOSystem(ct.rss(2, 1, 1)) + if nstates: + sys = ct.rss(nstates, noutputs, ninputs) + else: + sys = ct.ss( + [], np.zeros((0, ninputs)), np.zeros((noutputs, 0)), + np.random.rand(noutputs, ninputs)) # Start with a uniform set of times unifpts = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10] - uniform = [1, 2, 3, 2, 1, -1, -3, -5, -7, -3, 1] - t_unif, y_unif = ct.input_output_response(sys, unifpts, uniform) + uniform = np.outer( + np.ones(ninputs), [1, 2, 3, 2, 1, -1, -3, -5, -7, -3, 1]) + t_unif, y_unif = ct.input_output_response( + sys, unifpts, uniform, squeeze=False) # Create a non-uniform set of inputs noufpts = [0, 2, 4, 8, 10] - nonunif = [1, 3, 1, -7, 1] - t_nouf, y_nouf = ct.input_output_response(sys, noufpts, nonunif) + nonunif = np.outer(np.ones(ninputs), [1, 3, 1, -7, 1]) + t_nouf, y_nouf = ct.input_output_response( + sys, noufpts, nonunif, squeeze=False) # Make sure the outputs agree at common times - np.testing.assert_almost_equal(y_unif[noufpts], y_nouf, decimal=6) + np.testing.assert_almost_equal(y_unif[:, noufpts], y_nouf, decimal=6) # Resimulate using a new set of evaluation points t_even, y_even = ct.input_output_response( - sys, noufpts, nonunif, t_eval=unifpts) + sys, noufpts, nonunif, t_eval=unifpts, squeeze=False) np.testing.assert_almost_equal(y_unif, y_even, decimal=6) @@ -1859,3 +1910,73 @@ def test_rss(): with pytest.warns(UserWarning, match="may be interpreted as continuous"): sys = ct.drss(2, 1, 1, dt=None) assert np.all(np.abs(sys.poles()) < 1) + + +def eqpt_rhs(t, x, u, params): + return np.array([x[0]/2 + u[0], x[0] - x[1]**2 + u[1], x[1] - x[2]]) + +def eqpt_out(t, x, u, params): + return np.array([x[0], x[1] + u[1]]) + +@pytest.mark.parametrize( + "x0, ix, u0, iu, y0, iy, dx0, idx, dt, x_expect, u_expect", [ + # Equilibrium points with input given + (0, None, 0, None, None, None, None, None, 0, [0, 0, 0], [0, 0]), + (0, None, 0, None, None, None, None, None, None, [0, 0, 0], [0, 0]), + ([0.9, 0.9, 0.9], None, [-1, 0], None, None, None, None, None, 0, + [2, sqrt(2), sqrt(2)], [-1, 0]), + ([0.9, -0.9, 0.9], None, [-1, 0], None, None, None, None, None, 0, + [2, -sqrt(2), -sqrt(2)], [-1, 0]), # same input, different eqpt + (0, None, 0, None, None, None, None, None, 1, [0, 0, 0], [0, 0]), #DT + (0, None, [-1, 0], None, None, None, None, None, 1, None, None), #DT + ([0, -0.1, 0], None, [0, -0.25], None, None, None, None, None, 1, #DT + [0, -0.5, -0.25], [0, -0.25]), + + # Equilibrium points with output given + ([0.9, 0.9, 0.9], None, [-0.9, 0], None, [2, sqrt(2)], None, None, + None, 0, [2, sqrt(2), sqrt(2)], [-1, 0]), + (0, None, [0, -0.25], None, [0, -0.75], None, None, None, 1, #DT + [0, -0.5, -0.25], [0, -0.25]), + + # Equilibrium points with mixture of inputs and outputs given + ([0.9, 0.9, 0.9], None, [-1, 0], [0], [2, sqrt(2)], [1], None, + None, 0, [2, sqrt(2), sqrt(2)], [-1, 0]), + (0, None, [0, -0.22], [0], [0, -0.75], [1], None, None, 1, #DT + [0, -0.5, -0.25], [0, -0.25]), + ]) + +def test_find_eqpt(x0, ix, u0, iu, y0, iy, dx0, idx, dt, x_expect, u_expect): + sys = ct.NonlinearIOSystem( + eqpt_rhs, eqpt_out, dt=dt, states=3, inputs=2, outputs=2) + + xeq, ueq = ct.find_eqpt( + sys, x0, u0, y0, ix=ix, iu=iu, iy=iy, dx0=dx0, idx=idx) + + # If no equilibrium points, skip remaining tests + if x_expect is None: + assert xeq is None + assert ueq is None + return + + # Make sure we are at an appropriate equilibrium point + if dt is None or dt == 0: + # Continuous time system + np.testing.assert_allclose(eqpt_rhs(0, xeq, ueq, {}), 0, atol=1e-6) + if y0 is not None: + y0 = np.array(y0) + iy = np.s_[:] if iy is None else np.array(iy) + np.testing.assert_allclose( + eqpt_out(0, xeq, ueq, {})[iy], y0[iy], atol=1e-6) + + else: + # Discrete time system + np.testing.assert_allclose(eqpt_rhs(0, xeq, ueq, {}), xeq, atol=1e-6) + if y0 is not None: + y0 = np.array(y0) + iy = np.s_[:] if iy is None else np.array(iy) + np.testing.assert_allclose( + eqpt_out(0, xeq, ueq, {})[iy], y0[iy], atol=1e-6) + + # Check that we got the expected result as well + np.testing.assert_allclose(np.array(xeq), x_expect, atol=1e-6) + np.testing.assert_allclose(np.array(ueq), u_expect, atol=1e-6) diff --git a/control/tests/kwargs_test.py b/control/tests/kwargs_test.py index ada16a46a..8116f013a 100644 --- a/control/tests/kwargs_test.py +++ b/control/tests/kwargs_test.py @@ -2,7 +2,7 @@ # RMM, 20 Mar 2022 # # Allowing unrecognized keywords to be passed to a function without -# generating and error message can generate annoying bugs, since you +# generating an error message can generate annoying bugs, since you # sometimes think you are telling the function to do something and actually # you have a misspelling or other error and your input is being ignored. # @@ -38,6 +38,10 @@ def test_kwarg_search(module, prefix): # Skip anything that isn't part of the control package continue + # Look for classes and then check member functions + if inspect.isclass(obj): + test_kwarg_search(obj, prefix + obj.__name__ + '.') + # Only look for functions with keyword arguments if not inspect.isfunction(obj): continue @@ -70,84 +74,76 @@ def test_kwarg_search(module, prefix): f"'unrecognized keyword' not found in unit test " f"for {name}") - # Look for classes and then check member functions - if inspect.isclass(obj): - test_kwarg_search(obj, prefix + obj.__name__ + '.') +@pytest.mark.parametrize( + "function, nsssys, ntfsys, moreargs, kwargs", + [(control.dlqe, 1, 0, ([[1]], [[1]]), {}), + (control.dlqr, 1, 0, ([[1, 0], [0, 1]], [[1]]), {}), + (control.drss, 0, 0, (2, 1, 1), {}), + (control.input_output_response, 1, 0, ([0, 1, 2], [1, 1, 1]), {}), + (control.lqe, 1, 0, ([[1]], [[1]]), {}), + (control.lqr, 1, 0, ([[1, 0], [0, 1]], [[1]]), {}), + (control.linearize, 1, 0, (0, 0), {}), + (control.pzmap, 1, 0, (), {}), + (control.rlocus, 0, 1, ( ), {}), + (control.root_locus, 0, 1, ( ), {}), + (control.rss, 0, 0, (2, 1, 1), {}), + (control.set_defaults, 0, 0, ('control',), {'default_dt': True}), + (control.ss, 0, 0, (0, 0, 0, 0), {'dt': 1}), + (control.ss2io, 1, 0, (), {}), + (control.ss2tf, 1, 0, (), {}), + (control.summing_junction, 0, 0, (2,), {}), + (control.tf, 0, 0, ([1], [1, 1]), {}), + (control.tf2io, 0, 1, (), {}), + (control.tf2ss, 0, 1, (), {}), + (control.zpk, 0, 0, ([1], [2, 3], 4), {}), + (control.InputOutputSystem, 0, 0, (), + {'inputs': 1, 'outputs': 1, 'states': 1}), + (control.InputOutputSystem.linearize, 1, 0, (0, 0), {}), + (control.StateSpace, 0, 0, ([[-1, 0], [0, -1]], [[1], [1]], [[1, 1]], 0), {}), + (control.TransferFunction, 0, 0, ([1], [1, 1]), {})] +) +def test_unrecognized_kwargs(function, nsssys, ntfsys, moreargs, kwargs, + mplcleanup, editsdefaults): + # Create SISO systems for use in parameterized tests + sssys = control.ss([[-1, 1], [0, -1]], [[0], [1]], [[1, 0]], 0, dt=None) + tfsys = control.tf([1], [1, 1]) + + args = (sssys, )*nsssys + (tfsys, )*ntfsys + moreargs + + # Call the function normally and make sure it works + function(*args, **kwargs) + + # Now add an unrecognized keyword and make sure there is an error + with pytest.raises(TypeError, match="unrecognized keyword"): + function(*args, **kwargs, unknown=None) -@pytest.mark.usefixtures('editsdefaults') -def test_unrecognized_kwargs(): - # Create a SISO system for use in parameterized tests - sys = control.ss([[-1, 1], [0, -1]], [[0], [1]], [[1, 0]], 0, dt=None) - table = [ - [control.dlqe, (sys, [[1]], [[1]]), {}], - [control.dlqr, (sys, [[1, 0], [0, 1]], [[1]]), {}], - [control.drss, (2, 1, 1), {}], - [control.input_output_response, (sys, [0, 1, 2], [1, 1, 1]), {}], - [control.lqe, (sys, [[1]], [[1]]), {}], - [control.lqr, (sys, [[1, 0], [0, 1]], [[1]]), {}], - [control.linearize, (sys, 0, 0), {}], - [control.pzmap, (sys,), {}], - [control.rlocus, (control.tf([1], [1, 1]), ), {}], - [control.root_locus, (control.tf([1], [1, 1]), ), {}], - [control.rss, (2, 1, 1), {}], - [control.set_defaults, ('control',), {'default_dt': True}], - [control.ss, (0, 0, 0, 0), {'dt': 1}], - [control.ss2io, (sys,), {}], - [control.ss2tf, (sys,), {}], - [control.summing_junction, (2,), {}], - [control.tf, ([1], [1, 1]), {}], - [control.tf2io, (control.tf([1], [1, 1]),), {}], - [control.tf2ss, (control.tf([1], [1, 1]),), {}], - [control.InputOutputSystem, (), - {'inputs': 1, 'outputs': 1, 'states': 1}], - [control.InputOutputSystem.linearize, (sys, 0, 0), {}], - [control.StateSpace, ([[-1, 0], [0, -1]], [[1], [1]], [[1, 1]], 0), {}], - [control.TransferFunction, ([1], [1, 1]), {}], - ] - - for function, args, kwargs in table: - # Call the function normally and make sure it works - function(*args, **kwargs) - - # Now add an unrecognized keyword and make sure there is an error - with pytest.raises(TypeError, match="unrecognized keyword"): - function(*args, **kwargs, unknown=None) - - # If we opened any figures, close them to avoid matplotlib warnings - if plt.gca(): - plt.close('all') - - -def test_matplotlib_kwargs(): +@pytest.mark.parametrize( + "function, nsysargs, moreargs, kwargs", + [(control.bode, 1, (), {}), + (control.bode_plot, 1, (), {}), + (control.describing_function_plot, 1, + (control.descfcn.saturation_nonlinearity(1), [1, 2, 3, 4]), {}), + (control.gangof4, 2, (), {}), + (control.gangof4_plot, 2, (), {}), + (control.nyquist, 1, (), {}), + (control.nyquist_plot, 1, (), {}), + (control.singular_values_plot, 1, (), {})] +) +def test_matplotlib_kwargs(function, nsysargs, moreargs, kwargs, mplcleanup): # Create a SISO system for use in parameterized tests sys = control.ss([[-1, 1], [0, -1]], [[0], [1]], [[1, 0]], 0, dt=None) - ctl = control.ss([[-1, 1], [0, -1]], [[0], [1]], [[1, 0]], 0, dt=None) - table = [ - [control.bode, (sys, ), {}], - [control.bode_plot, (sys, ), {}], - [control.describing_function_plot, - (sys, control.descfcn.saturation_nonlinearity(1), [1, 2, 3, 4]), {}], - [control.gangof4, (sys, ctl), {}], - [control.gangof4_plot, (sys, ctl), {}], - [control.nyquist, (sys, ), {}], - [control.nyquist_plot, (sys, ), {}], - [control.singular_values_plot, (sys, ), {}], - ] + # Call the function normally and make sure it works + args = (sys, )*nsysargs + moreargs + function(*args, **kwargs) - for function, args, kwargs in table: - # Call the function normally and make sure it works - function(*args, **kwargs) + # Now add an unrecognized keyword and make sure there is an error + with pytest.raises(AttributeError, + match="(has no property|unexpected keyword)"): + function(*args, **kwargs, unknown=None) - # Now add an unrecognized keyword and make sure there is an error - with pytest.raises(AttributeError, match="has no property"): - function(*args, **kwargs, unknown=None) - - # If we opened any figures, close them to avoid matplotlib warnings - if plt.gca(): - plt.close('all') # @@ -188,8 +184,12 @@ def test_matplotlib_kwargs(): 'tf': test_unrecognized_kwargs, 'tf2io' : test_unrecognized_kwargs, 'tf2ss' : test_unrecognized_kwargs, + 'sample_system' : test_unrecognized_kwargs, + 'zpk': test_unrecognized_kwargs, 'flatsys.point_to_point': flatsys_test.TestFlatSys.test_point_to_point_errors, + 'flatsys.solve_flat_ocp': + flatsys_test.TestFlatSys.test_solve_flat_ocp_errors, 'FrequencyResponseData.__init__': frd_test.TestFRD.test_unrecognized_keyword, 'InputOutputSystem.__init__': test_unrecognized_kwargs, @@ -201,6 +201,71 @@ def test_matplotlib_kwargs(): 'NonlinearIOSystem.__init__': interconnect_test.test_interconnect_exceptions, 'StateSpace.__init__': test_unrecognized_kwargs, + 'StateSpace.sample': test_unrecognized_kwargs, 'TimeResponseData.__call__': trdata_test.test_response_copy, 'TransferFunction.__init__': test_unrecognized_kwargs, + 'TransferFunction.sample': test_unrecognized_kwargs, +} + +# +# Look for keywords with mutable defaults +# +# This test goes through every function and looks for signatures that have a +# default value for a keyword that is mutable. An error is generated unless +# the function is listed in the `mutable_ok` set (which should only be used +# for cases were the code has been explicitly checked to make sure that the +# value of the mutable is not modified in the code). +# +mutable_ok = { # initial and date + control.flatsys.SystemTrajectory.__init__, # RMM, 18 Nov 2022 + control.freqplot._add_arrows_to_line2D, # RMM, 18 Nov 2022 + control.namedio._process_dt_keyword, # RMM, 13 Nov 2022 + control.namedio._process_namedio_keywords, # RMM, 18 Nov 2022 + control.optimal.OptimalControlProblem.__init__, # RMM, 18 Nov 2022 + control.optimal.solve_ocp, # RMM, 18 Nov 2022 + control.optimal.create_mpc_iosystem, # RMM, 18 Nov 2022 } + +@pytest.mark.parametrize("module", [control, control.flatsys]) +def test_mutable_defaults(module, recurse=True): + # Look through every object in the package + for name, obj in inspect.getmembers(module): + # Skip anything that is outside of this module + if inspect.getmodule(obj) is not None and \ + not inspect.getmodule(obj).__name__.startswith('control'): + # Skip anything that isn't part of the control package + continue + + # Look for classes and then check member functions + if inspect.isclass(obj): + test_mutable_defaults(obj, True) + + # Look for modules and check for internal functions (w/ no recursion) + if inspect.ismodule(obj) and recurse: + test_mutable_defaults(obj, False) + + # Only look at functions and skip any that are marked as OK + if not inspect.isfunction(obj) or obj in mutable_ok: + continue + + # Get the signature for the function + sig = inspect.signature(obj) + + # Skip anything that is inherited + if inspect.isclass(module) and obj.__name__ not in module.__dict__: + continue + + # See if there is a variable keyword argument + for argname, par in sig.parameters.items(): + if par.default is inspect._empty or \ + not par.kind == inspect.Parameter.KEYWORD_ONLY and \ + not par.kind == inspect.Parameter.POSITIONAL_OR_KEYWORD: + continue + + # Check to see if the default value is mutable + if par.default is not None and not \ + isinstance(par.default, (bool, int, float, tuple, str)): + pytest.fail( + f"function '{obj.__name__}' in module '{module.__name__}'" + f" has mutable default for keyword '{par.name}'") + diff --git a/control/tests/matlab_test.py b/control/tests/matlab_test.py index a379ce7f0..abf86ce44 100644 --- a/control/tests/matlab_test.py +++ b/control/tests/matlab_test.py @@ -208,7 +208,6 @@ def testStep(self, siso): np.testing.assert_array_almost_equal(yout, youttrue, decimal=4) np.testing.assert_array_almost_equal(tout, t) - @slycotonly def testStep_mimo(self, mimo): """Test step for MIMO system""" sys = mimo.ss1 @@ -267,7 +266,6 @@ def testImpulse(self, siso): np.testing.assert_array_almost_equal(yout, youttrue, decimal=4) np.testing.assert_array_almost_equal(tout, t) - @slycotonly def testImpulse_mimo(self, mimo): """Test impulse() for MIMO system""" t = np.linspace(0, 1, 10) @@ -296,7 +294,6 @@ def testInitial(self, siso): np.testing.assert_array_almost_equal(yout, youttrue, decimal=4) np.testing.assert_array_almost_equal(tout, t) - @slycotonly def testInitial_mimo(self, mimo): """Test initial() for MIMO system""" t = np.linspace(0, 1, 10) @@ -333,7 +330,6 @@ def testLsim(self, siso): yout, _t, _xout = lsim(siso.ss1, u, t, x0) np.testing.assert_array_almost_equal(yout, youttrue, decimal=4) - @slycotonly def testLsim_mimo(self, mimo): """Test lsim() for MIMO system. @@ -352,6 +348,25 @@ def testLsim_mimo(self, mimo): yout, _t, _xout = lsim(mimo.ss1, u, t, x0) np.testing.assert_array_almost_equal(yout, youttrue, decimal=4) + def test_lsim_mimo_dtime(self): + # https://github.com/python-control/python-control/issues/764 + time = np.linspace(0.0, 511.0e-6, 512) + DAC = np.sin(time) + ADC = np.cos(time) + + input_Kalman = np.transpose( + np.concatenate(([[DAC]], [[ADC]]), axis=1)[0]) + Af = [[0.45768416, -0.42025511], [-0.43354791, 0.51961178]] + Bf = [[2.84368641, 52.05922305], [-1.47286557, -19.94861943]] + Cf = [[1.0, 0.0], [0.0, 1.0]] + Df = [[0.0, 0.0], [0.0, 0.0]] + + ss_Kalman = ss(Af, Bf, Cf, Df, 1.0e-6) + y_est, t, x_est = lsim(ss_Kalman, input_Kalman, time) + assert y_est.shape == (time.size, ss_Kalman.ninputs) + assert t.shape == (time.size, ) + assert x_est.shape == (time.size, ss_Kalman.nstates) + def testMargin(self, siso): """Test margin()""" #! TODO: check results to make sure they are OK @@ -582,7 +597,6 @@ def testSISOssdata(self, siso): for i in range(len(ssdata_1)): np.testing.assert_array_almost_equal(ssdata_1[i], ssdata_2[i]) - @slycotonly def testMIMOssdata(self, mimo): """Test ssdata() MIMO""" m = (mimo.ss1.A, mimo.ss1.B, mimo.ss1.C, mimo.ss1.D) diff --git a/control/tests/modelsimp_test.py b/control/tests/modelsimp_test.py index 70e94dd91..0746e3fe2 100644 --- a/control/tests/modelsimp_test.py +++ b/control/tests/modelsimp_test.py @@ -182,17 +182,33 @@ def testBalredTruncate(self, matarrayin): B = matarrayin([[2.], [0.], [0.], [0.]]) C = matarrayin([[0.5, 0.6875, 0.7031, 0.5]]) D = matarrayin([[0.]]) + sys = StateSpace(A, B, C, D) orders = 2 rsys = balred(sys, orders, method='truncate') + Ar, Br, Cr, Dr = rsys.A, rsys.B, rsys.C, rsys.D + + # Result from MATLAB Artrue = np.array([[-1.958, -1.194], [-1.194, -0.8344]]) Brtrue = np.array([[0.9057], [0.4068]]) Crtrue = np.array([[0.9057, 0.4068]]) Drtrue = np.array([[0.]]) - np.testing.assert_array_almost_equal(rsys.A, Artrue, decimal=2) - np.testing.assert_array_almost_equal(rsys.B, Brtrue, decimal=4) - np.testing.assert_array_almost_equal(rsys.C, Crtrue, decimal=4) - np.testing.assert_array_almost_equal(rsys.D, Drtrue, decimal=4) + + # Look for possible changes in state in slycot + T1 = np.array([[1, 0], [0, -1]]) + T2 = np.array([[-1, 0], [0, 1]]) + T3 = np.array([[0, 1], [1, 0]]) + for T in (T1, T2, T3): + if np.allclose(T @ Ar @ T, Artrue, atol=1e-2, rtol=1e-2): + # Apply a similarity transformation + Ar, Br, Cr = T @ Ar @ T, T @ Br, Cr @ T + break + + # Make sure we got the correct answer + np.testing.assert_array_almost_equal(Ar, Artrue, decimal=2) + np.testing.assert_array_almost_equal(Br, Brtrue, decimal=4) + np.testing.assert_array_almost_equal(Cr, Crtrue, decimal=4) + np.testing.assert_array_almost_equal(Dr, Drtrue, decimal=4) @slycotonly def testBalredMatchDC(self, matarrayin): @@ -207,16 +223,32 @@ def testBalredMatchDC(self, matarrayin): B = matarrayin([[2.], [0.], [0.], [0.]]) C = matarrayin([[0.5, 0.6875, 0.7031, 0.5]]) D = matarrayin([[0.]]) + sys = StateSpace(A, B, C, D) orders = 2 rsys = balred(sys,orders,method='matchdc') + Ar, Br, Cr, Dr = rsys.A, rsys.B, rsys.C, rsys.D + + # Result from MATLAB Artrue = np.array( [[-4.43094773, -4.55232904], [-4.55232904, -5.36195206]]) Brtrue = np.array([[1.36235673], [1.03114388]]) Crtrue = np.array([[1.36235673, 1.03114388]]) Drtrue = np.array([[-0.08383902]]) - np.testing.assert_array_almost_equal(rsys.A, Artrue, decimal=2) - np.testing.assert_array_almost_equal(rsys.B, Brtrue, decimal=4) - np.testing.assert_array_almost_equal(rsys.C, Crtrue, decimal=4) - np.testing.assert_array_almost_equal(rsys.D, Drtrue, decimal=4) + + # Look for possible changes in state in slycot + T1 = np.array([[1, 0], [0, -1]]) + T2 = np.array([[-1, 0], [0, 1]]) + T3 = np.array([[0, 1], [1, 0]]) + for T in (T1, T2, T3): + if np.allclose(T @ Ar @ T, Artrue, atol=1e-2, rtol=1e-2): + # Apply a similarity transformation + Ar, Br, Cr = T @ Ar @ T, T @ Br, Cr @ T + break + + # Make sure we got the correct answer + np.testing.assert_array_almost_equal(Ar, Artrue, decimal=2) + np.testing.assert_array_almost_equal(Br, Brtrue, decimal=4) + np.testing.assert_array_almost_equal(Cr, Crtrue, decimal=4) + np.testing.assert_array_almost_equal(Dr, Drtrue, decimal=4) diff --git a/control/tests/namedio_test.py b/control/tests/namedio_test.py index 3a96203a8..4925e9790 100644 --- a/control/tests/namedio_test.py +++ b/control/tests/namedio_test.py @@ -10,6 +10,7 @@ import re from copy import copy +import warnings import numpy as np import control as ct @@ -243,9 +244,12 @@ def test_duplicate_sysname(): sys = ct.rss(4, 1, 1) # No warnings should be generated if we reuse an an unnamed system - with pytest.warns(None) as record: + with warnings.catch_warnings(): + warnings.simplefilter("error") + # strip out matrix warnings + warnings.filterwarnings("ignore", "the matrix subclass", + category=PendingDeprecationWarning) res = sys * sys - assert not any([type(msg) == UserWarning for msg in record]) # Generate a warning if the system is named sys = ct.rss(4, 1, 1, name='sys') diff --git a/control/tests/nyquist_test.py b/control/tests/nyquist_test.py index b1aa00577..ddc69e7bb 100644 --- a/control/tests/nyquist_test.py +++ b/control/tests/nyquist_test.py @@ -8,6 +8,8 @@ """ +import warnings + import pytest import numpy as np import matplotlib.pyplot as plt @@ -225,10 +227,11 @@ def test_nyquist_encirclements(): sys = 1 / (s**2 + s + 1) with pytest.warns(UserWarning, match="encirclements was a non-integer"): count = ct.nyquist_plot(sys, omega_limits=[0.5, 1e3]) - with pytest.warns(None) as records: + with warnings.catch_warnings(): + warnings.simplefilter("error") + # strip out matrix warnings count = ct.nyquist_plot( sys, omega_limits=[0.5, 1e3], encirclement_threshold=0.2) - assert len(records) == 0 plt.title("Non-integer number of encirclements [%g]" % count) diff --git a/control/tests/optimal_test.py b/control/tests/optimal_test.py index 1aa307b60..53f2c29ad 100644 --- a/control/tests/optimal_test.py +++ b/control/tests/optimal_test.py @@ -16,16 +16,55 @@ from numpy.lib import NumpyVersion -def test_finite_horizon_simple(): - # Define a linear system with constraints +@pytest.mark.parametrize("method, npts", [ + ('shooting', 5), + ('collocation', 20), +]) +def test_continuous_lqr(method, npts): + # Create a lightly dampled, second order system + sys = ct.ss([[0, 1], [-0.1, -0.01]], [[0], [1]], [[1, 0]], 0) + + # Define cost functions + Q = np.eye(sys.nstates) + R = np.eye(sys.ninputs) * 10 + + # Figure out the LQR solution (for terminal cost) + K, S, E = ct.lqr(sys, Q, R) + + # Define the cost functions + traj_cost = opt.quadratic_cost(sys, Q, R) + term_cost = opt.quadratic_cost(sys, S, None) + constraints = opt.input_range_constraint( + sys, -np.ones(sys.ninputs), np.ones(sys.ninputs)) + + # Define the initial condition, time horizon, and time points + x0 = np.ones(sys.nstates) + Tf = 10 + timepts = np.linspace(0, Tf, npts) + + res = opt.solve_ocp( + sys, timepts, x0, traj_cost, constraints, terminal_cost=term_cost, + trajectory_method=method + ) + + # Make sure the optimization was successful + assert res.success + + # Make sure we were reasonable close to the optimal cost + assert res.cost / (x0 @ S @ x0) < 1.2 # shouldn't be too far off + + +@pytest.mark.parametrize("method", ['shooting']) # TODO: add 'collocation' +def test_finite_horizon_simple(method): + # Define a (discrete time) linear system with constraints # Source: https://www.mpt3.org/UI/RegulationProblem - # LTI prediction model + # LTI prediction model (discrete time) 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]), + sp.optimize.LinearConstraint(np.eye(3), [-5, -5, -1], [5, 5, 1]), ] # Quadratic state and input penalty @@ -40,6 +79,7 @@ def test_finite_horizon_simple(): # Retrieve the full open-loop predictions res = opt.solve_ocp( sys, time, x0, cost, constraints, squeeze=True, + trajectory_method=method, terminal_cost=cost) # include to match MPT3 formulation t, u_openloop = res.time, res.inputs np.testing.assert_almost_equal( @@ -108,7 +148,7 @@ def test_discrete_lqr(): # Add state and input constraints trajectory_constraints = [ - (sp.optimize.LinearConstraint, np.eye(3), [-5, -5, -.5], [5, 5, 0.5]), + sp.optimize.LinearConstraint(np.eye(3), [-5, -5, -.5], [5, 5, 0.5]), ] # Re-solve @@ -297,11 +337,13 @@ def test_terminal_constraints(sys_args): # 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) + np.testing.assert_almost_equal(res.inputs, u1, decimal=2) + np.testing.assert_almost_equal(res.states, x1, decimal=4) # 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(8, Tf)) + res = opt.solve_ocp( + sys, time, x0, cost, terminal_constraints=final_point, + basis=flat.BezierFamily(8, Tf)) # Final point doesn't affect cost => don't need to test np.testing.assert_almost_equal( @@ -325,7 +367,7 @@ def test_terminal_constraints(sys_args): # Not all configurations are able to converge (?) if res.success: - np.testing.assert_almost_equal(x2[:,-1], 0) + np.testing.assert_almost_equal(x2[:,-1], 0, decimal=5) # Make sure that it is *not* a straight line path assert np.any(np.abs(x2 - x1) > 0.1) @@ -366,7 +408,7 @@ def test_optimal_logging(capsys): x0 = [-1, 1] # Solve it, with logging turned on (with warning due to mixed constraints) - with pytest.warns(sp.optimize.optimize.OptimizeWarning, + with pytest.warns(sp.optimize.OptimizeWarning, match="Equality and inequality .* same element"): res = opt.solve_ocp( sys, time, x0, cost, input_constraint, terminal_cost=cost, @@ -380,17 +422,8 @@ def test_optimal_logging(capsys): @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, + [opt.quadratic_cost, (np.eye(2), np.eye(2, 3)), 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, @@ -428,7 +461,7 @@ def test_ocp_argument_errors(): # State and input constraints constraints = [ - (sp.optimize.LinearConstraint, np.eye(3), [-5, -5, -1], [5, 5, 1]), + sp.optimize.LinearConstraint(np.eye(3), [-5, -5, -1], [5, 5, 1]), ] # Quadratic state and input penalty @@ -461,22 +494,35 @@ def test_ocp_argument_errors(): # Unrecognized trajectory constraint type constraints = [(None, np.eye(3), [0, 0, 0], [0, 0, 0])] - with pytest.raises(TypeError, match="unknown constraint type"): + with pytest.raises(TypeError, match="unknown trajectory constraint type"): res = opt.solve_ocp( sys, time, x0, cost, trajectory_constraints=constraints) # Unrecognized terminal constraint type - with pytest.raises(TypeError, match="unknown constraint type"): + with pytest.raises(TypeError, match="unknown terminal constraint type"): res = opt.solve_ocp( sys, time, x0, cost, terminal_constraints=constraints) + # Discrete time system checks: solve_ivp keywords not allowed + sys = ct.rss(2, 1, 1, dt=True) + with pytest.raises(TypeError, match="solve_ivp method, kwargs not allowed"): + res = opt.solve_ocp( + sys, time, x0, cost, solve_ivp_method='LSODA') + with pytest.raises(TypeError, match="solve_ivp method, kwargs not allowed"): + res = opt.solve_ocp( + sys, time, x0, cost, solve_ivp_kwargs={'eps': 0.1}) -def test_optimal_basis_simple(): - sys = ct.ss2io(ct.ss([[1, 1], [0, 1]], [[1], [0.5]], np.eye(2), 0, 1)) + +@pytest.mark.parametrize("basis", [ + flat.PolyFamily(4), flat.PolyFamily(6), + flat.BezierFamily(4), flat.BSplineFamily([0, 4, 8], 6) + ]) +def test_optimal_basis_simple(basis): + sys = 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]), + sp.optimize.LinearConstraint(np.eye(3), [-5, -5, -1], [5, 5, 1]), ] # Quadratic state and input penalty @@ -492,7 +538,7 @@ def test_optimal_basis_simple(): # Basic optimal control problem res1 = opt.solve_ocp( sys, time, x0, cost, constraints, - basis=flat.BezierFamily(4, Tf), return_x=True) + terminal_cost=cost, basis=basis, return_x=True) assert res1.success # Make sure the constraints were satisfied @@ -503,14 +549,14 @@ def test_optimal_basis_simple(): # 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) + terminal_cost=cost, basis=basis, return_x=True) assert res2.success np.testing.assert_allclose(res2.inputs, res1.inputs, atol=0.01, rtol=0.01) # 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) + sys, time, x0, cost, constraints, terminal_cost=cost, + basis=basis, return_x=True, log=True) assert res3.success np.testing.assert_almost_equal(res3.inputs, res1.inputs, decimal=3) @@ -548,7 +594,7 @@ def test_equality_constraints(): def final_point_eval(x, u): return x final_point = [ - (sp.optimize.NonlinearConstraint, final_point_eval, [0, 0], [0, 0])] + sp.optimize.NonlinearConstraint(final_point_eval, [0, 0], [0, 0])] optctrl = opt.OptimalControlProblem( sys, time, cost, terminal_constraints=final_point) @@ -563,13 +609,28 @@ def final_point_eval(x, u): # Try passing and unknown constraint type final_point = [(None, final_point_eval, [0, 0], [0, 0])] - with pytest.raises(TypeError, match="unknown constraint type"): + with pytest.raises(TypeError, match="unknown terminal constraint type"): optctrl = opt.OptimalControlProblem( sys, time, cost, terminal_constraints=final_point) res = optctrl.compute_trajectory(x0, squeeze=True, return_x=True) -def test_optimal_doc(): +@pytest.mark.parametrize( + "method, npts, initial_guess, fail", [ + ('shooting', 3, None, 'xfail'), # doesn't converge + ('shooting', 3, 'zero', 'xfail'), # doesn't converge + ('shooting', 3, 'u0', None), # github issue #782 + ('shooting', 3, 'input', 'endpoint'), # doesn't converge to optimal + ('shooting', 5, 'input', 'endpoint'), # doesn't converge to optimal + ('collocation', 3, 'u0', 'endpoint'), # doesn't converge to optimal + ('collocation', 5, 'u0', 'endpoint'), + ('collocation', 5, 'input', 'openloop'),# open loop sim fails + ('collocation', 10, 'input', None), + ('collocation', 10, 'u0', None), # from documentation + ('collocation', 10, 'state', None), + ('collocation', 20, 'state', None), + ]) +def test_optimal_doc(method, npts, initial_guess, fail): """Test optimal control problem from documentation""" def vehicle_update(t, x, u, params): # Get the parameters for the model @@ -595,8 +656,8 @@ def vehicle_output(t, x, u, params): inputs=('v', 'phi'), outputs=('x', 'y', 'theta')) # Define the initial and final points and time interval - x0 = [0., -2., 0.]; u0 = [10., 0.] - xf = [100., 2., 0.]; uf = [10., 0.] + x0 = np.array([0., -2., 0.]); u0 = np.array([10., 0.]) + xf = np.array([100., 2., 0.]); uf = np.array([10., 0.]) Tf = 10 # Define the cost functions @@ -609,17 +670,65 @@ def vehicle_output(t, x, u, params): # Define the constraints constraints = [ opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ] + # Define an initial guess at the trajectory + timepts = np.linspace(0, Tf, npts, endpoint=True) + if initial_guess == 'zero': + initial_guess = 0 + + elif initial_guess == 'u0': + initial_guess = u0 + + elif initial_guess == 'input': + # Velocity = constant that gets us from start to end + initial_guess = np.zeros((vehicle.ninputs, timepts.size)) + initial_guess[0, :] = (xf[0] - x0[0]) / Tf + + # Steering = rate required to turn to proper slope in first segment + straight_seg_length = timepts[-2] - timepts[1] + curved_seg_length = (Tf - straight_seg_length)/2 + approximate_angle = math.atan2(xf[1] - x0[1], xf[0] - x0[0]) + initial_guess[1, 0] = approximate_angle / (timepts[1] - timepts[0]) + initial_guess[1, -1] = -approximate_angle / (timepts[-1] - timepts[-2]) + + elif initial_guess == 'state': + input_guess = np.outer(u0, np.ones((1, npts))) + state_guess = np.array([ + x0 + (xf - x0) * time/Tf for time in timepts]).transpose() + initial_guess = (state_guess, input_guess) + # Solve the optimal control problem - horizon = np.linspace(0, Tf, 3, endpoint=True) result = opt.solve_ocp( - vehicle, horizon, x0, traj_cost, constraints, - terminal_cost= term_cost, initial_guess=u0) - - # Make sure the resulting trajectory generate a good solution - resp = ct.input_output_response( - vehicle, horizon, result.inputs, x0, - t_eval=np.linspace(0, Tf, 10)) - t, y = resp - assert (y[0, -1] - xf[0]) / xf[0] < 0.01 - assert (y[1, -1] - xf[1]) / xf[1] < 0.01 - assert y[2, -1] < 0.1 + vehicle, timepts, x0, traj_cost, constraints, + terminal_cost=term_cost, initial_guess=initial_guess, + trajectory_method=method, + # minimize_method='COBYLA', # SLSQP', + ) + + if fail == 'xfail': + assert not result.success + pytest.xfail("optimization fails to converge") + elif fail == 'precision': + assert result.status == 2 + pytest.xfail("optimization precision not achieved") + else: + # Make sure the optimization was successful + assert result.success + + # Make sure we started and stopped at the right spot + if fail == 'endpoint': + assert not np.allclose(result.states[:, -1], xf, rtol=1e-4) + pytest.xfail("optimization does not converge to endpoint") + else: + np.testing.assert_almost_equal(result.states[:, 0], x0, decimal=4) + np.testing.assert_almost_equal(result.states[:, -1], xf, decimal=2) + + # Simulate the trajectory to make sure it looks OK + resp = ct.input_output_response( + vehicle, timepts, result.inputs, x0, + t_eval=np.linspace(0, Tf, 10)) + t, y = resp + if fail == 'openloop': + with pytest.raises(AssertionError): + np.testing.assert_almost_equal(y[:,-1], xf, decimal=1) + else: + np.testing.assert_almost_equal(y[:,-1], xf, decimal=1) diff --git a/control/tests/passivity_test.py b/control/tests/passivity_test.py new file mode 100644 index 000000000..4d7c8e6eb --- /dev/null +++ b/control/tests/passivity_test.py @@ -0,0 +1,141 @@ +''' +Author: Mark Yeatman +Date: May 30, 2022 +''' +import pytest +import numpy +from control import ss, passivity, tf, sample_system, parallel, feedback +from control.tests.conftest import cvxoptonly +from control.exception import ControlArgument, ControlDimension + +pytestmark = cvxoptonly + + +def test_ispassive_ctime(): + A = numpy.array([[0, 1], [-2, -2]]) + B = numpy.array([[0], [1]]) + C = numpy.array([[-1, 2]]) + D = numpy.array([[1.5]]) + sys = ss(A, B, C, D) + + # happy path is passive + assert(passivity.ispassive(sys)) + + # happy path not passive + D = -D + sys = ss(A, B, C, D) + + assert(not passivity.ispassive(sys)) + + +def test_ispassive_dtime(): + A = numpy.array([[0, 1], [-2, -2]]) + B = numpy.array([[0], [1]]) + C = numpy.array([[-1, 2]]) + D = numpy.array([[1.5]]) + sys = ss(A, B, C, D) + sys = sample_system(sys, 1, method='bilinear') + assert(passivity.ispassive(sys)) + + +def test_passivity_indices_ctime(): + sys = tf([1, 1, 5, 0.1], [1, 2, 3, 4]) + + iff_index = passivity.get_input_ff_index(sys) + ofb_index = passivity.get_output_fb_index(sys) + + assert(isinstance(ofb_index, float)) + + sys_ff = parallel(sys, -iff_index) + sys_fb = feedback(sys, ofb_index, sign=1) + + assert(sys_ff.ispassive()) + assert(sys_fb.ispassive()) + + sys_ff = parallel(sys, -iff_index-1e-6) + sys_fb = feedback(sys, ofb_index+1e-6, sign=1) + + assert(not sys_ff.ispassive()) + assert(not sys_fb.ispassive()) + + +def test_passivity_indices_dtime(): + sys = tf([1, 1, 5, 0.1], [1, 2, 3, 4]) + sys = sample_system(sys, Ts=0.1) + iff_index = passivity.get_input_ff_index(sys) + ofb_index = passivity.get_output_fb_index(sys) + + assert(isinstance(iff_index, float)) + + sys_ff = parallel(sys, -iff_index) + sys_fb = feedback(sys, ofb_index, sign=1) + + assert(sys_ff.ispassive()) + assert(sys_fb.ispassive()) + + sys_ff = parallel(sys, -iff_index-1e-2) + sys_fb = feedback(sys, ofb_index+1e-2, sign=1) + + assert(not sys_ff.ispassive()) + assert(not sys_fb.ispassive()) + + +def test_system_dimension(): + A = numpy.array([[0, 1], [-2, -2]]) + B = numpy.array([[0], [1]]) + C = numpy.array([[-1, 2], [0, 1]]) + D = numpy.array([[1.5], [1]]) + sys = ss(A, B, C, D) + + with pytest.raises(ControlDimension): + passivity.ispassive(sys) + + +A_d = numpy.array([[-2, 0], [0, 0]]) +A = numpy.array([[-3, 0], [0, -2]]) +B = numpy.array([[0], [1]]) +C = numpy.array([[-1, 2]]) +D = numpy.array([[1.5]]) + + +@pytest.mark.parametrize( + "system_matrices, expected", + [((A, B, C, D*0), True), + ((A_d, B, C, D), True), + ((A, B*0, C*0, D), True), + ((A*0, B, C, D), True), + ((A*0, B*0, C*0, D*0), True)]) +def test_ispassive_edge_cases(system_matrices, expected): + sys = ss(*system_matrices) + assert passivity.ispassive(sys) == expected + + +def test_rho_and_nu_are_none(): + A = numpy.array([[0]]) + B = numpy.array([[0]]) + C = numpy.array([[0]]) + D = numpy.array([[0]]) + sys = ss(A, B, C, D) + + with pytest.raises(ControlArgument): + passivity.solve_passivity_LMI(sys) + + +def test_transfer_function(): + sys = tf([1], [1, 2]) + assert(passivity.ispassive(sys)) + + sys = tf([1], [1, -2]) + assert(not passivity.ispassive(sys)) + + +def test_oo_style(): + A = numpy.array([[0, 1], [-2, -2]]) + B = numpy.array([[0], [1]]) + C = numpy.array([[-1, 2]]) + D = numpy.array([[1.5]]) + sys = ss(A, B, C, D) + assert(sys.ispassive()) + + sys = tf([1], [1, 2]) + assert(sys.ispassive()) diff --git a/control/tests/rlocus_test.py b/control/tests/rlocus_test.py index a0ecebb15..a25928e27 100644 --- a/control/tests/rlocus_test.py +++ b/control/tests/rlocus_test.py @@ -54,6 +54,12 @@ def testRootLocus(self, sys): np.testing.assert_allclose(klist, k_out) self.check_cl_poles(sys, roots, klist) + # now check with plotting + roots, k_out = root_locus(sys, klist) + np.testing.assert_equal(len(roots), len(klist)) + np.testing.assert_allclose(klist, k_out) + self.check_cl_poles(sys, roots, klist) + def test_without_gains(self, sys): roots, kvect = root_locus(sys, plot=False) self.check_cl_poles(sys, roots, kvect) @@ -85,6 +91,8 @@ def test_root_locus_neg_false_gain_nonproper(self): # TODO: cover and validate negative false_gain branch in _default_gains() + @pytest.mark.skipif(plt.get_current_fig_manager().toolbar is None, + reason="Requires the zoom toolbar") def test_root_locus_zoom(self): """Check the zooming functionality of the Root locus plot""" system = TransferFunction([1000], [1, 25, 100, 0]) diff --git a/control/tests/sisotool_test.py b/control/tests/sisotool_test.py index d5e9dd013..d4a291052 100644 --- a/control/tests/sisotool_test.py +++ b/control/tests/sisotool_test.py @@ -46,6 +46,8 @@ def sys221(self): D221 = [[1., -1.]] return StateSpace(A222, B222, C221, D221) + @pytest.mark.skipif(plt.get_current_fig_manager().toolbar is None, + reason="Requires the zoom toolbar") def test_sisotool(self, tsys): sisotool(tsys, Hz=False) fig = plt.gcf() @@ -114,6 +116,8 @@ def test_sisotool(self, tsys): assert_array_almost_equal( ax_step.lines[0].get_data()[1][:10], step_response_moved, 4) + @pytest.mark.skipif(plt.get_current_fig_manager().toolbar is None, + reason="Requires the zoom toolbar") @pytest.mark.parametrize('tsys', [0, True], indirect=True, ids=['ctime', 'dtime']) def test_sisotool_tvect(self, tsys): @@ -132,6 +136,13 @@ def test_sisotool_tvect(self, tsys): bode_plot_params=dict(), tvect=tvect) assert_array_almost_equal(tvect, ax_step.lines[0].get_data()[0]) + @pytest.mark.skipif(plt.get_current_fig_manager().toolbar is None, + reason="Requires the zoom toolbar") + def test_sisotool_initial_gain(self, tsys): + sisotool(tsys, initial_gain=1.2) + # kvect keyword should give deprecation warning + with pytest.warns(FutureWarning): + sisotool(tsys, kvect=1.2) def test_sisotool_mimo(self, sys222, sys221): # a 2x2 should not raise an error: diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index 13f164e1f..9e8feb4c9 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -5,6 +5,8 @@ import numpy as np import pytest +import itertools +from math import pi, atan import control as ct from control import lqe, dlqe, poles, rss, ss, tf @@ -777,3 +779,201 @@ def test_statefbk_errors(self): with pytest.raises(ControlArgument, match="must be an array of size"): ctrl, clsys = ct.create_statefbk_iosystem( sys, K, integral_action=C_int) + + +# Kinematic car example for testing gain scheduling +@pytest.fixture +def unicycle(): + # Create a simple nonlinear system to check (kinematic car) + def unicycle_update(t, x, u, params): + return np.array([np.cos(x[2]) * u[0], np.sin(x[2]) * u[0], u[1]]) + + def unicycle_output(t, x, u, params): + return x + + return ct.NonlinearIOSystem( + unicycle_update, unicycle_output, + inputs = ['v', 'phi'], + outputs = ['x', 'y', 'theta'], + states = ['x_', 'y_', 'theta_']) + +from math import pi + +@pytest.mark.parametrize("method", ['nearest', 'linear', 'cubic']) +def test_gainsched_unicycle(unicycle, method): + # Speeds and angles at which to compute the gains + speeds = [1, 5, 10] + angles = np.linspace(0, pi/2, 4) + points = list(itertools.product(speeds, angles)) + + # Gains for each speed (using LQR controller) + Q = np.identity(unicycle.nstates) + R = np.identity(unicycle.ninputs) + gains = [np.array(ct.lqr(unicycle.linearize( + [0, 0, angle], [speed, 0]), Q, R)[0]) for speed, angle in points] + + # + # Schedule on desired speed and angle + # + + # Create gain scheduled controller + ctrl, clsys = ct.create_statefbk_iosystem( + unicycle, (gains, points), + gainsched_indices=[3, 2], gainsched_method=method) + + # Check the gain at the selected points + for speed, angle in points: + # Figure out the desired state and input + xe, ue = np.array([0, 0, angle]), np.array([speed, 0]) + xd, ud = np.array([0, 0, angle]), np.array([speed, 0]) + + # Check the control system at the scheduling points + ctrl_lin = ctrl.linearize([], [xd, ud, xe*0]) + K, S, E = ct.lqr(unicycle.linearize(xd, ud), Q, R) + np.testing.assert_allclose( + ctrl_lin.D[-xe.size:, -xe.size:], -K, rtol=1e-2) + + # Check the closed loop system at the scheduling points + clsys_lin = clsys.linearize(xe, [xd, ud]) + np.testing.assert_allclose( + np.sort(clsys_lin.poles()), np.sort(E), rtol=1e-2) + + # Check the gain at an intermediate point and confirm stability + speed, angle = 2, pi/3 + xe, ue = np.array([0, 0, angle]), np.array([speed, 0]) + xd, ud = np.array([0, 0, angle]), np.array([speed, 0]) + clsys_lin = clsys.linearize(xe, [xd, ud]) + assert np.all(np.real(clsys_lin.poles()) < 0) + + # Make sure that gains are different from 'nearest' + if method is not None and method != 'nearest': + ctrl_nearest, clsys_nearest = ct.create_statefbk_iosystem( + unicycle, (gains, points), + gainsched_indices=['ud[0]', 2], gainsched_method='nearest') + nearest_lin = clsys_nearest.linearize(xe, [xd, ud]) + assert not np.allclose( + np.sort(clsys_lin.poles()), np.sort(nearest_lin.poles()), rtol=1e-2) + + # Run a simulation following a curved path + T = 10 # length of the trajectory [sec] + r = 10 # radius of the circle [m] + timepts = np.linspace(0, T, 50) + Xd = np.vstack([ + r * np.cos(timepts/T * pi/2 + 3*pi/2), + r * np.sin(timepts/T * pi/2 + 3*pi/2) + r, + timepts/T * pi/2 + ]) + Ud = np.vstack([ + np.ones_like(timepts) * (r * pi/2) / T, + np.ones_like(timepts) * (pi / 2) / T + ]) + X0 = Xd[:, 0] + np.array([-0.1, -0.1, -0.1]) + + resp = ct.input_output_response(clsys, timepts, [Xd, Ud], X0) + # plt.plot(resp.states[0], resp.states[1]) + np.testing.assert_allclose( + resp.states[:, -1], Xd[:, -1], atol=1e-2, rtol=1e-2) + + # + # Schedule on actual speed + # + + # Create gain scheduled controller + ctrl, clsys = ct.create_statefbk_iosystem( + unicycle, (gains, points), + ud_labels=['vd', 'phid'], gainsched_indices=['vd', 'theta']) + + # Check the gain at the selected points + for speed, angle in points: + # Figure out the desired state and input + xe, ue = np.array([0, 0, angle]), np.array([speed, 0]) + xd, ud = np.array([0, 0, angle]), np.array([speed, 0]) + + # Check the control system at the scheduling points + ctrl_lin = ctrl.linearize([], [xd*0, ud, xe]) + K, S, E = ct.lqr(unicycle.linearize(xe, ue), Q, R) + np.testing.assert_allclose( + ctrl_lin.D[-xe.size:, -xe.size:], -K, rtol=1e-2) + + # Check the closed loop system at the scheduling points + clsys_lin = clsys.linearize(xe, [xd, ud]) + np.testing.assert_allclose(np.sort( + clsys_lin.poles()), np.sort(E), rtol=1e-2) + + # Run a simulation following a curved path + resp = ct.input_output_response(clsys, timepts, [Xd, Ud], X0) + np.testing.assert_allclose( + resp.states[:, -1], Xd[:, -1], atol=1e-2, rtol=1e-2) + + +def test_gainsched_default_indices(): + # Define a linear system to test + sys = ct.ss([[-1, 0.1], [0, -2]], [[0], [1]], np.eye(2), 0) + + # Define gains at origin + corners of unit cube + points = [[0, 0]] + list(itertools.product([-1, 1], [-1, 1])) + + # Define gain to be constant + K, _, _ = ct.lqr(sys, np.eye(sys.nstates), np.eye(sys.ninputs)) + gains = [K for p in points] + + # Define the paramters for the simulations + timepts = np.linspace(0, 10, 100) + X0 = np.ones(sys.nstates) * 0.9 + + # Create a controller and simulate the initial response + gs_ctrl, gs_clsys = ct.create_statefbk_iosystem(sys, (gains, points)) + gs_resp = ct.input_output_response(gs_clsys, timepts, 0, X0) + + # Verify that we get the same result as a constant gain + ck_clsys = ct.ss(sys.A - sys.B @ K, sys.B, sys.C, 0) + ck_resp = ct.input_output_response(ck_clsys, timepts, 0, X0) + + np.testing.assert_allclose(gs_resp.states, ck_resp.states) + + +def test_gainsched_errors(unicycle): + # Set up gain schedule (same as previous test) + speeds = [1, 5, 10] + angles = np.linspace(0, pi/2, 4) + points = list(itertools.product(speeds, angles)) + + Q = np.identity(unicycle.nstates) + R = np.identity(unicycle.ninputs) + gains = [np.array(ct.lqr(unicycle.linearize( + [0, 0, angle], [speed, 0]), Q, R)[0]) for speed, angle in points] + + # Make sure the generic case works OK + ctrl, clsys = ct.create_statefbk_iosystem( + unicycle, (gains, points), gainsched_indices=[3, 2]) + xd, ud = np.array([0, 0, angles[0]]), np.array([speeds[0], 0]) + ctrl_lin = ctrl.linearize([], [xd, ud, xd*0]) + K, S, E = ct.lqr(unicycle.linearize(xd, ud), Q, R) + np.testing.assert_allclose( + ctrl_lin.D[-xd.size:, -xd.size:], -K, rtol=1e-2) + + # Wrong type of gain schedule argument + with pytest.raises(ControlArgument, match="gain must be an array"): + ctrl, clsys = ct.create_statefbk_iosystem( + unicycle, [gains, points], gainsched_indices=[3, 2]) + + # Wrong number of gain schedule argument + with pytest.raises(ControlArgument, match="gain must be a 2-tuple"): + ctrl, clsys = ct.create_statefbk_iosystem( + unicycle, (gains, speeds, angles), gainsched_indices=[3, 2]) + + # Mismatched dimensions for gains and points + with pytest.raises(ControlArgument, match="length of gainsched_indices"): + ctrl, clsys = ct.create_statefbk_iosystem( + unicycle, (gains, [speeds]), gainsched_indices=[3, 2]) + + # Unknown gain scheduling variable label + with pytest.raises(ValueError, match=".* not in list"): + ctrl, clsys = ct.create_statefbk_iosystem( + unicycle, (gains, points), gainsched_indices=['stuff', 2]) + + # Unknown gain scheduling method + with pytest.raises(ControlArgument, match="unknown gain scheduling method"): + ctrl, clsys = ct.create_statefbk_iosystem( + unicycle, (gains, points), + gainsched_indices=[3, 2], gainsched_method='unknown') diff --git a/control/tests/statesp_test.py b/control/tests/statesp_test.py index 315b5f152..fa837f30d 100644 --- a/control/tests/statesp_test.py +++ b/control/tests/statesp_test.py @@ -333,6 +333,17 @@ def test_multiply_ss(self, sys222, sys322): np.testing.assert_array_almost_equal(sys.C, C) np.testing.assert_array_almost_equal(sys.D, D) + @pytest.mark.parametrize("k", [2, -3.141, np.float32(2.718), np.array([[4.321], [5.678]])]) + def test_truediv_ss_scalar(self, sys322, k): + """Divide SS by scalar.""" + sys = sys322 / k + syscheck = sys322 * (1/k) + + np.testing.assert_array_almost_equal(sys.A, syscheck.A) + np.testing.assert_array_almost_equal(sys.B, syscheck.B) + np.testing.assert_array_almost_equal(sys.C, syscheck.C) + np.testing.assert_array_almost_equal(sys.D, syscheck.D) + @pytest.mark.parametrize("omega, resp", [(1., np.array([[ 4.37636761e-05-0.01522976j, @@ -399,25 +410,6 @@ def test_freq_resp(self): mag, phase, omega = sys.freqresp(true_omega) np.testing.assert_almost_equal(mag, true_mag) - def test__isstatic(self): - A0 = np.zeros((2,2)) - A1 = A0.copy() - A1[0,1] = 1.1 - B0 = np.zeros((2,1)) - B1 = B0.copy() - B1[0,0] = 1.3 - C0 = A0 - C1 = np.eye(2) - D0 = 0 - D1 = np.ones((2,1)) - assert StateSpace(A0, B0, C1, D1)._isstatic() - assert not StateSpace(A1, B0, C1, D1)._isstatic() - assert not StateSpace(A0, B1, C1, D1)._isstatic() - assert not StateSpace(A1, B1, C1, D1)._isstatic() - assert StateSpace(A0, B0, C0, D0)._isstatic() - assert StateSpace(A0, B0, C0, D1)._isstatic() - assert StateSpace(A0, B0, C1, D0)._isstatic() - @slycotonly def test_minreal(self): """Test a minreal model reduction.""" @@ -839,8 +831,42 @@ def test_error_u_dynamics_mimo(self, u, sys222): sys222.dynamics(0, (1, 1), u) with pytest.raises(ValueError): sys222.output(0, (1, 1), u) - - + + def test_sample_named_signals(self): + sysc = ct.StateSpace(1.1, 1, 1, 1, inputs='u', outputs='y', states='a') + + # Full form of the call + sysd = sysc.sample(0.1, name='sampled') + assert sysd.name == 'sampled' + assert sysd.find_input('u') == 0 + assert sysd.find_output('y') == 0 + assert sysd.find_state('a') == 0 + + # If we copy signal names w/out a system name, append '$sampled' + sysd = sysc.sample(0.1) + assert sysd.name == sysc.name + '$sampled' + + # If copy is False, signal names should not be copied + sysd_nocopy = sysc.sample(0.1, copy_names=False) + assert sysd_nocopy.find_input('u') is None + assert sysd_nocopy.find_output('y') is None + assert sysd_nocopy.find_state('a') is None + + # if signal names are provided, they should override those of sysc + sysd_newnames = sysc.sample(0.1, inputs='v', outputs='x', states='b') + assert sysd_newnames.find_input('v') == 0 + assert sysd_newnames.find_input('u') is None + assert sysd_newnames.find_output('x') == 0 + assert sysd_newnames.find_output('y') is None + assert sysd_newnames.find_state('b') == 0 + assert sysd_newnames.find_state('a') is None + # test just one name + sysd_newnames = sysc.sample(0.1, inputs='v') + assert sysd_newnames.find_input('v') == 0 + assert sysd_newnames.find_input('u') is None + assert sysd_newnames.find_output('y') == 0 + assert sysd_newnames.find_output('x') is None + class TestRss: """These are tests for the proper functionality of statesp.rss.""" @@ -1011,23 +1037,23 @@ def test_statespace_defaults(self, matarrayout): [[1.2345, -2e-200], [-1, 0]]) LTX_G1_REF = { - 'p3_p' : '\\[\n\\left(\\begin{array}{rllrll|rll}\n3.&\\hspace{-1em}14&\\hspace{-1em}\\phantom{\\cdot}&1\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\cdot10^{100}&0\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n-1.&\\hspace{-1em}23&\\hspace{-1em}\\phantom{\\cdot}&5\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\cdot10^{-23}&1\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\hline\n9.&\\hspace{-1em}88&\\hspace{-1em}\\cdot10^{8}&0.&\\hspace{-1em}00123&\\hspace{-1em}\\phantom{\\cdot}&5\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\end{array}\\right)\n\\]', + 'p3_p' : '$$\n\\left(\\begin{array}{rllrll|rll}\n3.&\\hspace{-1em}14&\\hspace{-1em}\\phantom{\\cdot}&1\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\cdot10^{100}&0\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n-1.&\\hspace{-1em}23&\\hspace{-1em}\\phantom{\\cdot}&5\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\cdot10^{-23}&1\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\hline\n9.&\\hspace{-1em}88&\\hspace{-1em}\\cdot10^{8}&0.&\\hspace{-1em}00123&\\hspace{-1em}\\phantom{\\cdot}&5\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\end{array}\\right)\n$$', - 'p5_p' : '\\[\n\\left(\\begin{array}{rllrll|rll}\n3.&\\hspace{-1em}1416&\\hspace{-1em}\\phantom{\\cdot}&1\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\cdot10^{100}&0\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n-1.&\\hspace{-1em}2346&\\hspace{-1em}\\phantom{\\cdot}&5\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\cdot10^{-23}&1\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\hline\n9.&\\hspace{-1em}8765&\\hspace{-1em}\\cdot10^{8}&0.&\\hspace{-1em}001234&\\hspace{-1em}\\phantom{\\cdot}&5\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\end{array}\\right)\n\\]', + 'p5_p' : '$$\n\\left(\\begin{array}{rllrll|rll}\n3.&\\hspace{-1em}1416&\\hspace{-1em}\\phantom{\\cdot}&1\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\cdot10^{100}&0\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n-1.&\\hspace{-1em}2346&\\hspace{-1em}\\phantom{\\cdot}&5\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\cdot10^{-23}&1\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\hline\n9.&\\hspace{-1em}8765&\\hspace{-1em}\\cdot10^{8}&0.&\\hspace{-1em}001234&\\hspace{-1em}\\phantom{\\cdot}&5\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\end{array}\\right)\n$$', - 'p3_s' : '\\[\n\\begin{array}{ll}\nA = \\left(\\begin{array}{rllrll}\n3.&\\hspace{-1em}14&\\hspace{-1em}\\phantom{\\cdot}&1\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\cdot10^{100}\\\\\n-1.&\\hspace{-1em}23&\\hspace{-1em}\\phantom{\\cdot}&5\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\cdot10^{-23}\\\\\n\\end{array}\\right)\n&\nB = \\left(\\begin{array}{rll}\n0\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n1\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\end{array}\\right)\n\\\\\nC = \\left(\\begin{array}{rllrll}\n9.&\\hspace{-1em}88&\\hspace{-1em}\\cdot10^{8}&0.&\\hspace{-1em}00123&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\end{array}\\right)\n&\nD = \\left(\\begin{array}{rll}\n5\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\end{array}\\right)\n\\end{array}\n\\]', + 'p3_s' : '$$\n\\begin{array}{ll}\nA = \\left(\\begin{array}{rllrll}\n3.&\\hspace{-1em}14&\\hspace{-1em}\\phantom{\\cdot}&1\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\cdot10^{100}\\\\\n-1.&\\hspace{-1em}23&\\hspace{-1em}\\phantom{\\cdot}&5\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\cdot10^{-23}\\\\\n\\end{array}\\right)\n&\nB = \\left(\\begin{array}{rll}\n0\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n1\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\end{array}\\right)\n\\\\\nC = \\left(\\begin{array}{rllrll}\n9.&\\hspace{-1em}88&\\hspace{-1em}\\cdot10^{8}&0.&\\hspace{-1em}00123&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\end{array}\\right)\n&\nD = \\left(\\begin{array}{rll}\n5\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\end{array}\\right)\n\\end{array}\n$$', - 'p5_s' : '\\[\n\\begin{array}{ll}\nA = \\left(\\begin{array}{rllrll}\n3.&\\hspace{-1em}1416&\\hspace{-1em}\\phantom{\\cdot}&1\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\cdot10^{100}\\\\\n-1.&\\hspace{-1em}2346&\\hspace{-1em}\\phantom{\\cdot}&5\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\cdot10^{-23}\\\\\n\\end{array}\\right)\n&\nB = \\left(\\begin{array}{rll}\n0\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n1\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\end{array}\\right)\n\\\\\nC = \\left(\\begin{array}{rllrll}\n9.&\\hspace{-1em}8765&\\hspace{-1em}\\cdot10^{8}&0.&\\hspace{-1em}001234&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\end{array}\\right)\n&\nD = \\left(\\begin{array}{rll}\n5\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\end{array}\\right)\n\\end{array}\n\\]', + 'p5_s' : '$$\n\\begin{array}{ll}\nA = \\left(\\begin{array}{rllrll}\n3.&\\hspace{-1em}1416&\\hspace{-1em}\\phantom{\\cdot}&1\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\cdot10^{100}\\\\\n-1.&\\hspace{-1em}2346&\\hspace{-1em}\\phantom{\\cdot}&5\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\cdot10^{-23}\\\\\n\\end{array}\\right)\n&\nB = \\left(\\begin{array}{rll}\n0\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n1\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\end{array}\\right)\n\\\\\nC = \\left(\\begin{array}{rllrll}\n9.&\\hspace{-1em}8765&\\hspace{-1em}\\cdot10^{8}&0.&\\hspace{-1em}001234&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\end{array}\\right)\n&\nD = \\left(\\begin{array}{rll}\n5\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\end{array}\\right)\n\\end{array}\n$$', } LTX_G2_REF = { - 'p3_p' : '\\[\n\\left(\\begin{array}{rllrll}\n1.&\\hspace{-1em}23&\\hspace{-1em}\\phantom{\\cdot}&-2\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\cdot10^{-200}\\\\\n-1\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}&0\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\end{array}\\right)\n\\]', + 'p3_p' : '$$\n\\left(\\begin{array}{rllrll}\n1.&\\hspace{-1em}23&\\hspace{-1em}\\phantom{\\cdot}&-2\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\cdot10^{-200}\\\\\n-1\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}&0\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\end{array}\\right)\n$$', - 'p5_p' : '\\[\n\\left(\\begin{array}{rllrll}\n1.&\\hspace{-1em}2345&\\hspace{-1em}\\phantom{\\cdot}&-2\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\cdot10^{-200}\\\\\n-1\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}&0\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\end{array}\\right)\n\\]', + 'p5_p' : '$$\n\\left(\\begin{array}{rllrll}\n1.&\\hspace{-1em}2345&\\hspace{-1em}\\phantom{\\cdot}&-2\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\cdot10^{-200}\\\\\n-1\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}&0\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\end{array}\\right)\n$$', - 'p3_s' : '\\[\n\\begin{array}{ll}\nD = \\left(\\begin{array}{rllrll}\n1.&\\hspace{-1em}23&\\hspace{-1em}\\phantom{\\cdot}&-2\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\cdot10^{-200}\\\\\n-1\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}&0\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\end{array}\\right)\n\\end{array}\n\\]', + 'p3_s' : '$$\n\\begin{array}{ll}\nD = \\left(\\begin{array}{rllrll}\n1.&\\hspace{-1em}23&\\hspace{-1em}\\phantom{\\cdot}&-2\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\cdot10^{-200}\\\\\n-1\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}&0\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\end{array}\\right)\n\\end{array}\n$$', - 'p5_s' : '\\[\n\\begin{array}{ll}\nD = \\left(\\begin{array}{rllrll}\n1.&\\hspace{-1em}2345&\\hspace{-1em}\\phantom{\\cdot}&-2\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\cdot10^{-200}\\\\\n-1\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}&0\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\end{array}\\right)\n\\end{array}\n\\]', + 'p5_s' : '$$\n\\begin{array}{ll}\nD = \\left(\\begin{array}{rllrll}\n1.&\\hspace{-1em}2345&\\hspace{-1em}\\phantom{\\cdot}&-2\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\cdot10^{-200}\\\\\n-1\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}&0\\phantom{.}&\\hspace{-1em}&\\hspace{-1em}\\phantom{\\cdot}\\\\\n\\end{array}\\right)\n\\end{array}\n$$', } refkey_n = {None: 'p3', '.3g': 'p3', '.5g': 'p5'} @@ -1158,3 +1184,30 @@ def test_linfnorm_ct_mimo(self, ct_siso): gpeak, fpeak = linfnorm(sys) np.testing.assert_allclose(gpeak, refgpeak) np.testing.assert_allclose(fpeak, reffpeak) + + +@pytest.mark.parametrize("args, static", [ + (([], [], [], 1), True), # ctime, empty state + (([], [], [], 1, 1), True), # dtime, empty state + ((0, 0, 0, 1), False), # ctime, unused state + ((-1, 0, 0, 1), False), # ctime, exponential decay + ((-1, 0, 0, 0), False), # ctime, no input, no output + ((0, 0, 0, 1, 1), False), # dtime, integrator + ((1, 0, 0, 1, 1), False), # dtime, unused state + ((0, 0, 0, 1, None), False), # unspecified, unused state +]) +def test_isstatic(args, static): + sys = ct.StateSpace(*args) + assert sys._isstatic() == static + +# Make sure that using params for StateSpace objects generates a warning +def test_params_warning(): + sys = StateSpace(-1, 1, 1, 0) + + with pytest.warns(UserWarning, match="params keyword ignored"): + sys.dynamics(0, [0], [0], {'k': 5}) + + with pytest.warns(UserWarning, match="params keyword ignored"): + sys.output(0, [0], [0], {'k': 5}) + + diff --git a/control/tests/stochsys_test.py b/control/tests/stochsys_test.py index 11084d9db..75e5a510c 100644 --- a/control/tests/stochsys_test.py +++ b/control/tests/stochsys_test.py @@ -7,6 +7,7 @@ import control as ct from control import lqe, dlqe, rss, drss, tf, ss, ControlArgument, slycot_check +from math import log, pi # Utility function to check LQE answer def check_LQE(L, P, poles, G, QN, RN): @@ -48,7 +49,7 @@ def test_lqe_call_format(cdlqe): # Standard calling format Lref, Pref, Eref = cdlqe(sys.A, sys.B, sys.C, Q, R) - + # Call with system instead of matricees L, P, E = cdlqe(sys, Q, R) np.testing.assert_almost_equal(Lref, L) @@ -58,15 +59,15 @@ def test_lqe_call_format(cdlqe): # Make sure we get an error if we specify N with pytest.raises(ct.ControlNotImplemented): L, P, E = cdlqe(sys, Q, R, N) - + # Inconsistent system dimensions with pytest.raises(ct.ControlDimension, match="Incompatible"): L, P, E = cdlqe(sys.A, sys.C, sys.B, Q, R) - + # Incorrect covariance matrix dimensions with pytest.raises(ct.ControlDimension, match="Incompatible"): L, P, E = cdlqe(sys.A, sys.B, sys.C, R, Q) - + # Too few input arguments with pytest.raises(ct.ControlArgument, match="not enough input"): L, P, E = cdlqe(sys.A, sys.C) @@ -99,14 +100,14 @@ def test_lqe_discrete(): np.testing.assert_almost_equal(K_csys, K_expl) np.testing.assert_almost_equal(S_csys, S_expl) np.testing.assert_almost_equal(E_csys, E_expl) - + # Calling lqe() with a discrete time system should call dlqe() K_lqe, S_lqe, E_lqe = ct.lqe(dsys, Q, R) K_dlqe, S_dlqe, E_dlqe = ct.dlqe(dsys, Q, R) np.testing.assert_almost_equal(K_lqe, K_dlqe) np.testing.assert_almost_equal(S_lqe, S_dlqe) np.testing.assert_almost_equal(E_lqe, E_dlqe) - + # Calling lqe() with no timebase should call lqe() asys = ct.ss(csys.A, csys.B, csys.C, csys.D, dt=None) K_asys, S_asys, E_asys = ct.lqe(asys, Q, R) @@ -114,11 +115,11 @@ def test_lqe_discrete(): np.testing.assert_almost_equal(K_asys, K_expl) np.testing.assert_almost_equal(S_asys, S_expl) np.testing.assert_almost_equal(E_asys, E_expl) - + # Calling dlqe() with a continuous time system should raise an error with pytest.raises(ControlArgument, match="called with a continuous"): K, S, E = ct.dlqe(csys, Q, R) - + def test_estimator_iosys(): sys = ct.drss(4, 2, 2, strictly_proper=True) @@ -129,7 +130,7 @@ def test_estimator_iosys(): QN = np.eye(sys.ninputs) RN = np.eye(sys.noutputs) estim = ct.create_estimator_iosystem(sys, QN, RN, P0) - + ctrl, clsys = ct.create_statefbk_iosystem(sys, K, estimator=estim) # Extract the elements of the estimator @@ -162,20 +163,76 @@ def test_estimator_iosys(): np.testing.assert_almost_equal(cls.D, D_clchk) +@pytest.mark.parametrize("sys_args", [ + ([[-1]], [[1]], [[1]], 0), # scalar system + ([[-1, 0.1], [0, -2]], [[0], [1]], [[1, 0]], 0), # SISO, 2 state + ([[-1, 0.1], [0, -2]], [[1, 0], [0, 1]], [[1, 0]], 0), # 2i, 1o, 2s + ([[-1, 0.1, 0.1], [0, -2, 0], [0.1, 0, -3]], # 2i, 2o, 3s + [[1, 0], [0, 0.1], [0, 1]], + [[1, 0, 0.1], [0, 1, 0.1]], 0), +]) +def test_estimator_iosys_ctime(sys_args): + # Define the system we want to test + sys = ct.ss(*sys_args) + T = 10 * log(1e-2) / np.max(sys.poles().real) + assert T > 0 + + # Create nonlinear version of the system to match integration methods + nl_sys = ct.NonlinearIOSystem( + lambda t, x, u, params : sys.A @ x + sys.B @ u, + lambda t, x, u, params : sys.C @ x + sys.D @ u, + inputs=sys.ninputs, outputs=sys.noutputs, states=sys.nstates) + + # Define an initial condition, inputs (small, to avoid integration errors) + timepts = np.linspace(0, T, 500) + U = 2e-2 * np.array([np.sin(timepts + i*pi/3) for i in range(sys.ninputs)]) + X0 = np.ones(sys.nstates) + + # Set up the parameters for the filter + P0 = np.eye(sys.nstates) + QN = np.eye(sys.ninputs) + RN = np.eye(sys.noutputs) + + # Construct the estimator + estim = ct.create_estimator_iosystem(sys, QN, RN) + + # Compute the system response and the optimal covariance + sys_resp = ct.input_output_response(nl_sys, timepts, U, X0) + _, Pf, _ = ct.lqe(sys, QN, RN) + Pf = np.array(Pf) # convert from matrix, if needed + + # Make sure that we converge to the optimal estimate + estim_resp = ct.input_output_response( + estim, timepts, [sys_resp.outputs, U], [0*X0, P0]) + np.testing.assert_allclose( + estim_resp.states[0:sys.nstates, -1], sys_resp.states[:, -1], + atol=1e-6, rtol=1e-3) + np.testing.assert_allclose( + estim_resp.states[sys.nstates:, -1], Pf.reshape(-1), + atol=1e-6, rtol=1e-3) + + # Make sure that optimal estimate is an eq pt + ss_resp = ct.input_output_response( + estim, timepts, [sys_resp.outputs, U], [X0, Pf]) + np.testing.assert_allclose( + ss_resp.states[sys.nstates:], + np.outer(Pf.reshape(-1), np.ones_like(timepts)), + atol=1e-4, rtol=1e-2) + np.testing.assert_allclose( + ss_resp.states[0:sys.nstates], sys_resp.states, + atol=1e-4, rtol=1e-2) + + def test_estimator_errors(): sys = ct.drss(4, 2, 2, strictly_proper=True) P0 = np.eye(sys.nstates) QN = np.eye(sys.ninputs) RN = np.eye(sys.noutputs) - with pytest.raises(ct.ControlArgument, match="Input system must be I/O"): + with pytest.raises(ct.ControlArgument, match=".* system must be a linear"): sys_tf = ct.tf([1], [1, 1], dt=True) estim = ct.create_estimator_iosystem(sys_tf, QN, RN) - - with pytest.raises(NotImplementedError, match="continuous time not"): - sys_ct = ct.rss(4, 2, 2, strictly_proper=True) - estim = ct.create_estimator_iosystem(sys_ct, QN, RN) - + with pytest.raises(ValueError, match="output must be full state"): C = np.eye(2, 4) estim = ct.create_estimator_iosystem(sys, QN, RN, C=C) @@ -246,7 +303,7 @@ def test_correlation(): # Try passing a second argument tau, Rneg = ct.correlation(T, V, -V) np.testing.assert_equal(Rtau, -Rneg) - + # Test error conditions with pytest.raises(ValueError, match="Time vector T must be 1D"): tau, Rtau = ct.correlation(V, V) diff --git a/control/tests/timebase_test.py b/control/tests/timebase_test.py new file mode 100644 index 000000000..a391d2fe7 --- /dev/null +++ b/control/tests/timebase_test.py @@ -0,0 +1,69 @@ +import pytest +import inspect +import numpy as np +import control as ct + +@pytest.mark.parametrize( + "dt1, dt2, dt3", [ + (0, 0, 0), + (0, 0.1, ValueError), + (0, None, 0), + (0, True, ValueError), + (0.1, 0, ValueError), + (0.1, 0.1, 0.1), + (0.1, None, 0.1), + (0.1, True, 0.1), + (None, 0, 0), + (None, 0.1, 0.1), + (None, None, None), + (None, True, True), + (True, 0, ValueError), + (True, 0.1, 0.1), + (True, None, True), + (True, True, True), + (0.2, None, 0.2), + (0.2, 0.1, ValueError), + ]) +@pytest.mark.parametrize("op", [ct.series, ct.parallel, ct.feedback]) +@pytest.mark.parametrize("type", [ct.StateSpace, ct.ss, ct.tf]) +def test_composition(dt1, dt2, dt3, op, type): + # Define the system + A, B, C, D = [[1, 1], [0, 1]], [[0], [1]], [[1, 0]], 0 + sys1 = ct.StateSpace(A, B, C, D, dt1) + sys2 = ct.StateSpace(A, B, C, D, dt2) + + # Convert to the desired form + sys1 = type(sys1) + sys2 = type(sys2) + + if inspect.isclass(dt3) and issubclass(dt3, Exception): + with pytest.raises(dt3, match="incompatible timebases"): + sys3 = op(sys1, sys2) + else: + sys3 = op(sys1, sys2) + assert sys3.dt == dt3 + + +@pytest.mark.parametrize("dt", [None, 0, 0.1]) +def test_composition_override(dt): + # Define the system + A, B, C, D = [[1, 1], [0, 1]], [[0], [1]], [[1, 0]], 0 + sys1 = ct.ss(A, B, C, D, None, inputs='u1', outputs='y1') + sys2 = ct.ss(A, B, C, D, None, inputs='y1', outputs='y2') + + # Show that we can override the type + sys3 = ct.interconnect([sys1, sys2], inputs='u1', outputs='y2', dt=dt) + assert sys3.dt == dt + + # Overriding the type with an inconsistent type generates an error + sys1 = ct.StateSpace(A, B, C, D, 0.1, inputs='u1', outputs='y1') + if dt != 0.1 and dt is not None: + with pytest.raises(ValueError, match="incompatible timebases"): + sys3 = ct.interconnect( + [sys1, sys2], inputs='u1', outputs='y2', dt=dt) + + sys1 = ct.StateSpace(A, B, C, D, 0, inputs='u1', outputs='y1') + if dt != 0 and dt is not None: + with pytest.raises(ValueError, match="incompatible timebases"): + sys3 = ct.interconnect( + [sys1, sys2], inputs='u1', outputs='y2', dt=dt) diff --git a/control/tests/timeresp_test.py b/control/tests/timeresp_test.py index fe73ab4a9..124e16c1e 100644 --- a/control/tests/timeresp_test.py +++ b/control/tests/timeresp_test.py @@ -1,7 +1,6 @@ """timeresp_test.py - test time response functions""" from copy import copy -from distutils.version import StrictVersion import numpy as np import pytest @@ -521,8 +520,6 @@ def test_impulse_response_mimo(self, tsystem): _t, yy = impulse_response(sys, T=t, input=0) np.testing.assert_array_almost_equal(yy[:,0,:], yref_notrim, decimal=4) - @pytest.mark.skipif(StrictVersion(sp.__version__) < "1.3", - reason="requires SciPy 1.3 or greater") @pytest.mark.parametrize("tsystem", ["siso_tf1"], indirect=True) def test_discrete_time_impulse(self, tsystem): # discrete time impulse sampled version should match cont time @@ -701,10 +698,10 @@ def test_forced_response_invalid_d(self, tsystem): """Test invalid parameters dtime with sys.dt > 0.""" with pytest.raises(ValueError, match="can't both be zero"): forced_response(tsystem.sys) - with pytest.raises(ValueError, match="must have same elements"): + with pytest.raises(ValueError, match="Parameter ``U``: Wrong shape"): forced_response(tsystem.sys, T=tsystem.t, U=np.random.randn(1, 12)) - with pytest.raises(ValueError, match="must have same elements"): + with pytest.raises(ValueError, match="Parameter ``U``: Wrong shape"): forced_response(tsystem.sys, T=tsystem.t, U=np.random.randn(12)) with pytest.raises(ValueError, match="must match sampling time"): @@ -998,9 +995,6 @@ def test_time_series_data_convention_2D(self, tsystem): [6, 2, 2, False, (2, 2, 8), (2, 8)], ]) def test_squeeze(self, fcn, nstate, nout, ninp, squeeze, shape1, shape2): - # Figure out if we have SciPy 1+ - scipy0 = StrictVersion(sp.__version__) < '1.0' - # Define the system if fcn == ct.tf and (nout > 1 or ninp > 1) and not slycot_check(): pytest.skip("Conversion of MIMO systems to transfer functions " @@ -1077,7 +1071,7 @@ def test_squeeze(self, fcn, nstate, nout, ninp, squeeze, shape1, shape2): assert yvec.shape == (8, ) # For InputOutputSystems, also test input/output response - if isinstance(sys, ct.InputOutputSystem) and not scipy0: + if isinstance(sys, ct.InputOutputSystem): _, yvec = ct.input_output_response(sys, tvec, uvec, squeeze=squeeze) assert yvec.shape == shape2 @@ -1108,7 +1102,7 @@ def test_squeeze(self, fcn, nstate, nout, ninp, squeeze, shape1, shape2): assert yvec.shape == (sys.noutputs, 8) # For InputOutputSystems, also test input_output_response - if isinstance(sys, ct.InputOutputSystem) and not scipy0: + if isinstance(sys, ct.InputOutputSystem): _, yvec = ct.input_output_response(sys, tvec, uvec) if squeeze is not True or sys.noutputs > 1: assert yvec.shape == (sys.noutputs, 8) diff --git a/control/tests/type_conversion_test.py b/control/tests/type_conversion_test.py index cdf302015..0deb68f88 100644 --- a/control/tests/type_conversion_test.py +++ b/control/tests/type_conversion_test.py @@ -19,7 +19,9 @@ def sys_dict(): sdict['frd'] = ct.frd([10+0j, 9 + 1j, 8 + 2j, 7 + 3j], [1, 2, 3, 4]) sdict['lio'] = ct.LinearIOSystem(ct.ss([[-1]], [[5]], [[5]], [[0]])) sdict['ios'] = ct.NonlinearIOSystem( - sdict['lio']._rhs, sdict['lio']._out, inputs=1, outputs=1, states=1) + lambda t, x, u, params: sdict['lio']._rhs(t, x, u), + lambda t, x, u, params: sdict['lio']._out(t, x, u), + inputs=1, outputs=1, states=1) sdict['arr'] = np.array([[2.0]]) sdict['flt'] = 3. return sdict @@ -59,7 +61,7 @@ def sys_dict(): rtype_list = ['ss', 'tf', 'frd', 'lio', 'ios', 'arr', 'flt'] conversion_table = [ # op left ss tf frd lio ios arr flt - ('add', 'ss', ['ss', 'ss', 'frd', 'ss', 'ios', 'ss', 'ss' ]), + ('add', 'ss', ['ss', 'ss', 'frd', 'lio', 'ios', 'ss', 'ss' ]), ('add', 'tf', ['tf', 'tf', 'frd', 'lio', 'ios', 'tf', 'tf' ]), ('add', 'frd', ['frd', 'frd', 'frd', 'frd', 'E', 'frd', 'frd']), ('add', 'lio', ['lio', 'lio', 'xrd', 'lio', 'ios', 'lio', 'lio']), @@ -68,7 +70,7 @@ def sys_dict(): ('add', 'flt', ['ss', 'tf', 'frd', 'lio', 'ios', 'arr', 'flt']), # op left ss tf frd lio ios arr flt - ('sub', 'ss', ['ss', 'ss', 'frd', 'ss', 'ios', 'ss', 'ss' ]), + ('sub', 'ss', ['ss', 'ss', 'frd', 'lio', 'ios', 'ss', 'ss' ]), ('sub', 'tf', ['tf', 'tf', 'frd', 'lio', 'ios', 'tf', 'tf' ]), ('sub', 'frd', ['frd', 'frd', 'frd', 'frd', 'E', 'frd', 'frd']), ('sub', 'lio', ['lio', 'lio', 'xrd', 'lio', 'ios', 'lio', 'lio']), @@ -77,7 +79,7 @@ def sys_dict(): ('sub', 'flt', ['ss', 'tf', 'frd', 'lio', 'ios', 'arr', 'flt']), # op left ss tf frd lio ios arr flt - ('mul', 'ss', ['ss', 'ss', 'frd', 'ss', 'ios', 'ss', 'ss' ]), + ('mul', 'ss', ['ss', 'ss', 'frd', 'lio', 'ios', 'ss', 'ss' ]), ('mul', 'tf', ['tf', 'tf', 'frd', 'lio', 'ios', 'tf', 'tf' ]), ('mul', 'frd', ['frd', 'frd', 'frd', 'frd', 'E', 'frd', 'frd']), ('mul', 'lio', ['lio', 'lio', 'xrd', 'lio', 'ios', 'lio', 'lio']), @@ -86,11 +88,11 @@ def sys_dict(): ('mul', 'flt', ['ss', 'tf', 'frd', 'lio', 'ios', 'arr', 'flt']), # op left ss tf frd lio ios arr flt - ('truediv', 'ss', ['xs', 'tf', 'frd', 'xio', 'xos', 'xs', 'xs' ]), + ('truediv', 'ss', ['xs', 'tf', 'frd', 'xio', 'xos', 'ss', 'ss' ]), ('truediv', 'tf', ['tf', 'tf', 'xrd', 'tf', 'xos', 'tf', 'tf' ]), ('truediv', 'frd', ['frd', 'frd', 'frd', 'frd', 'E', 'frd', 'frd']), - ('truediv', 'lio', ['xio', 'tf', 'frd', 'xio', 'xio', 'xio', 'xio']), - ('truediv', 'ios', ['xos', 'xos', 'E', 'xos', 'xos' 'xos', 'xos']), + ('truediv', 'lio', ['xio', 'tf', 'frd', 'xio', 'xio', 'lio', 'lio']), + ('truediv', 'ios', ['xos', 'xos', 'E', 'xos', 'xos', 'ios', 'ios']), ('truediv', 'arr', ['xs', 'tf', 'frd', 'xio', 'xos', 'arr', 'arr']), ('truediv', 'flt', ['xs', 'tf', 'frd', 'xio', 'xos', 'arr', 'flt'])] @@ -191,3 +193,47 @@ def test_binary_op_type_conversions(opname, ltype, rtype, sys_dict): assert len(result.output_labels) == result.noutputs if result.nstates is not None: assert len(result.state_labels) == result.nstates + +@pytest.mark.parametrize( + "typelist, connections, inplist, outlist, expected", [ + (['lio', 'lio'], [[(1, 0), (0, 0)]], [[(0, 0)]], [[(1, 0)]], 'lio'), + (['lio', 'ss'], [[(1, 0), (0, 0)]], [[(0, 0)]], [[(1, 0)]], 'lio'), + (['ss', 'lio'], [[(1, 0), (0, 0)]], [[(0, 0)]], [[(1, 0)]], 'lio'), + (['ss', 'ss'], [[(1, 0), (0, 0)]], [[(0, 0)]], [[(1, 0)]], 'lio'), + (['lio', 'tf'], [[(1, 0), (0, 0)]], [[(0, 0)]], [[(1, 0)]], 'lio'), + (['lio', 'frd'], [[(1, 0), (0, 0)]], [[(0, 0)]], [[(1, 0)]], 'E'), + (['ios', 'ios'], [[(1, 0), (0, 0)]], [[(0, 0)]], [[(1, 0)]], 'ios'), + (['lio', 'ios'], [[(1, 0), (0, 0)]], [[(0, 0)]], [[(1, 0)]], 'ios'), + (['ss', 'ios'], [[(1, 0), (0, 0)]], [[(0, 0)]], [[(1, 0)]], 'ios'), + (['tf', 'ios'], [[(1, 0), (0, 0)]], [[(0, 0)]], [[(1, 0)]], 'ios'), + (['lio', 'ss', 'tf'], + [[(1, 0), (0, 0)], [(2, 0), (1, 0)]], [[(0, 0)]], [[(2, 0)]], 'lio'), + (['ios', 'ss', 'tf'], + [[(1, 0), (0, 0)], [(2, 0), (1, 0)]], [[(0, 0)]], [[(2, 0)]], 'ios'), + ]) +def test_interconnect( + typelist, connections, inplist, outlist, expected, sys_dict): + # Create the system list + syslist = [sys_dict[_type] for _type in typelist] + + # Make copies of any duplicates + for sysidx, sys in enumerate(syslist): + if sys == syslist[0]: + syslist[sysidx] = sys.copy() + + # Make sure we get the right result + if expected == 'E' or expected[0] == 'x': + # Exception expected + with pytest.raises(TypeError): + result = ct.interconnect(syslist, connections, inplist, outlist) + else: + result = ct.interconnect(syslist, connections, inplist, outlist) + + # Make sure the type is correct + assert isinstance(result, type_dict[expected]) + + # Make sure we can evaluate the dynamics + np.testing.assert_equal( + result.dynamics( + 0, np.zeros(result.nstates), np.zeros(result.ninputs)), + np.zeros(result.nstates)) diff --git a/control/tests/xferfcn_test.py b/control/tests/xferfcn_test.py index 79273f31b..6e1cf6ce2 100644 --- a/control/tests/xferfcn_test.py +++ b/control/tests/xferfcn_test.py @@ -8,11 +8,12 @@ import operator import control as ct -from control import StateSpace, TransferFunction, rss, ss2tf, evalfr +from control import StateSpace, TransferFunction, rss, evalfr +from control import ss, ss2tf, tf, tf2ss from control import isctime, isdtime, sample_system, defaults from control.statesp import _convert_to_statespace from control.xferfcn import _convert_to_transfer_function -from control.tests.conftest import slycotonly, nopython2, matrixfilter +from control.tests.conftest import slycotonly, matrixfilter class TestXferFcn: @@ -448,7 +449,6 @@ def test_call_siso(self, dt, omega, resp): np.testing.assert_allclose(sys.evalfr(omega), resp, atol=1e-3) - @nopython2 def test_call_dtime(self): sys = TransferFunction([1., 3., 5], [1., 6., 2., -1], 0.1) np.testing.assert_array_almost_equal(sys(1j), -0.5 - 0.5j) @@ -741,8 +741,7 @@ def test_indexing(self): "matarrayin", [pytest.param(np.array, id="arrayin", - marks=[nopython2, - pytest.mark.skip(".__matmul__ not implemented")]), + marks=[pytest.mark.skip(".__matmul__ not implemented")]), pytest.param(np.matrix, id="matrixin", marks=matrixfilter)], @@ -989,6 +988,37 @@ def test_repr(self, Hargs, ref): np.testing.assert_array_almost_equal(H.den[p][m], H2.den[p][m]) assert H.dt == H2.dt + def test_sample_named_signals(self): + sysc = ct.TransferFunction(1.1, (1, 2), inputs='u', outputs='y') + + # Full form of the call + sysd = sysc.sample(0.1, name='sampled') + assert sysd.name == 'sampled' + assert sysd.find_input('u') == 0 + assert sysd.find_output('y') == 0 + + # If we copy signal names w/out a system name, append '$sampled' + sysd = sysc.sample(0.1) + assert sysd.name == sysc.name + '$sampled' + + # If copy is False, signal names should not be copied + sysd_nocopy = sysc.sample(0.1, copy_names=False) + assert sysd_nocopy.find_input('u') is None + assert sysd_nocopy.find_output('y') is None + + # if signal names are provided, they should override those of sysc + sysd_newnames = sysc.sample(0.1, inputs='v', outputs='x') + assert sysd_newnames.find_input('v') == 0 + assert sysd_newnames.find_input('u') is None + assert sysd_newnames.find_output('x') == 0 + assert sysd_newnames.find_output('y') is None + # test just one name + sysd_newnames = sysc.sample(0.1, inputs='v') + assert sysd_newnames.find_input('v') == 0 + assert sysd_newnames.find_input('u') is None + assert sysd_newnames.find_output('y') == 0 + assert sysd_newnames.find_output('x') is None + class TestLTIConverter: """Test returnScipySignalLTI method""" @@ -1044,3 +1074,72 @@ def test_xferfcn_ndarray_precedence(op, tf, arr): # Apply the operator to the array and transfer function result = op(arr, tf) assert isinstance(result, ct.TransferFunction) + + +@pytest.mark.parametrize( + "zeros, poles, gain, args, kwargs", [ + ([], [-1], 1, [], {}), + ([1, 2], [-1, -2, -3], 5, [], {}), + ([1, 2], [-1, -2, -3], 5, [], {'name': "sys"}), + ([1, 2], [-1, -2, -3], 5, [], {'inputs': ["in"], 'outputs': ["out"]}), + ([1, 2], [-1, -2, -3], 5, [0.1], {}), + (np.array([1, 2]), np.array([-1, -2, -3]), 5, [], {}), +]) +def test_zpk(zeros, poles, gain, args, kwargs): + # Create the transfer function + sys = ct.zpk(zeros, poles, gain, *args, **kwargs) + + # Make sure the poles and zeros match + np.testing.assert_equal(sys.zeros().sort(), zeros.sort()) + np.testing.assert_equal(sys.poles().sort(), poles.sort()) + + # Check to make sure the gain is OK + np.testing.assert_almost_equal( + gain, sys(0) * np.prod(-sys.poles()) / np.prod(-sys.zeros())) + + # Check time base + if args: + assert sys.dt == args[0] + + # Check inputs, outputs, name + input_labels = kwargs.get('inputs', []) + for i, label in enumerate(input_labels): + assert sys.input_labels[i] == label + + output_labels = kwargs.get('outputs', []) + for i, label in enumerate(output_labels): + assert sys.output_labels[i] == label + + if kwargs.get('name'): + assert sys.name == kwargs.get('name') + +@pytest.mark.parametrize("create, args, kwargs, convert", [ + (StateSpace, ([-1], [1], [1], [0]), {}, ss2tf), + (StateSpace, ([-1], [1], [1], [0]), {}, ss), + (StateSpace, ([-1], [1], [1], [0]), {}, tf), + (StateSpace, ([-1], [1], [1], [0]), dict(inputs='i', outputs='o'), ss2tf), + (StateSpace, ([-1], [1], [1], [0]), dict(inputs=1, outputs=1), ss2tf), + (StateSpace, ([-1], [1], [1], [0]), dict(inputs='i', outputs='o'), ss), + (StateSpace, ([-1], [1], [1], [0]), dict(inputs='i', outputs='o'), tf), + (TransferFunction, ([1], [1, 1]), {}, tf2ss), + (TransferFunction, ([1], [1, 1]), {}, tf), + (TransferFunction, ([1], [1, 1]), {}, ss), + (TransferFunction, ([1], [1, 1]), dict(inputs='i', outputs='o'), tf2ss), + (TransferFunction, ([1], [1, 1]), dict(inputs=1, outputs=1), tf2ss), + (TransferFunction, ([1], [1, 1]), dict(inputs='i', outputs='o'), tf), + (TransferFunction, ([1], [1, 1]), dict(inputs='i', outputs='o'), ss), +]) +def test_copy_names(create, args, kwargs, convert): + # Convert a system with no renaming + sys = create(*args, **kwargs) + cpy = convert(sys) + + assert cpy.input_labels == sys.input_labels + assert cpy.input_labels == sys.input_labels + if cpy.nstates is not None and sys.nstates is not None: + assert cpy.state_labels == sys.state_labels + + # Relabel inputs and outputs + cpy = convert(sys, inputs='myin', outputs='myout') + assert cpy.input_labels == ['myin'] + assert cpy.output_labels == ['myout'] diff --git a/control/timeresp.py b/control/timeresp.py index aa1261ccd..509107cc8 100644 --- a/control/timeresp.py +++ b/control/timeresp.py @@ -642,7 +642,7 @@ def __len__(self): # Convert to pandas def to_pandas(self): if not pandas_check(): - ImportError('pandas not installed') + raise ImportError("pandas not installed") import pandas # Create a dict for setting up the data frame @@ -972,11 +972,6 @@ def forced_response(sys, T=None, U=0., X0=0., transpose=False, dt = 1. if sys.dt in [True, None] else sys.dt T = np.array(range(n_steps)) * dt else: - # Make sure the input vector and time vector have same length - if (U.ndim == 1 and U.shape[0] != T.shape[0]) or \ - (U.ndim > 1 and U.shape[1] != T.shape[0]): - raise ValueError('Parameter ``T`` must have same elements as' - ' the number of columns in input array ``U``') if U.ndim == 0: U = np.full((n_inputs, T.shape[0]), U) else: @@ -1801,7 +1796,8 @@ def impulse_response(sys, T=None, X0=0., input=None, output=None, T_num=None, ----- This function uses the `forced_response` function to compute the time response. For continuous time systems, the initial condition is altered to - account for the initial impulse. + account for the initial impulse. For discrete-time aystems, the impulse is + sized so that it has unit area. Examples -------- diff --git a/control/xferfcn.py b/control/xferfcn.py index d3671c533..0bc84e096 100644 --- a/control/xferfcn.py +++ b/control/xferfcn.py @@ -65,7 +65,7 @@ from .frdata import FrequencyResponseData from . import config -__all__ = ['TransferFunction', 'tf', 'ss2tf', 'tfdata'] +__all__ = ['TransferFunction', 'tf', 'zpk', 'ss2tf', 'tfdata'] # Define module default parameter values @@ -702,10 +702,6 @@ def __truediv__(self, other): return TransferFunction(num, den, dt) - # TODO: Remove when transition to python3 complete - def __div__(self, other): - return TransferFunction.__truediv__(self, other) - # TODO: Division of MIMO transfer function objects is not written yet. def __rtruediv__(self, other): """Right divide two LTI objects.""" @@ -724,10 +720,6 @@ def __rtruediv__(self, other): return other / self - # TODO: Remove when transition to python3 complete - def __rdiv__(self, other): - return TransferFunction.__rtruediv__(self, other) - def __pow__(self, other): if not type(other) == int: raise ValueError("Exponent must be an integer") @@ -804,7 +796,7 @@ def zeros(self): """Compute the zeros of a transfer function.""" if self.ninputs > 1 or self.noutputs > 1: raise NotImplementedError( - "TransferFunction.zero is currently only implemented " + "TransferFunction.zeros is currently only implemented " "for SISO systems.") else: # for now, just give zeros of a SISO tf @@ -1090,7 +1082,8 @@ def _common_den(self, imag_tol=None, allow_nonproper=False): return num, den, denorder - def sample(self, Ts, method='zoh', alpha=None, prewarp_frequency=None): + def sample(self, Ts, method='zoh', alpha=None, prewarp_frequency=None, + name=None, copy_names=True, **kwargs): """Convert a continuous-time system to discrete time Creates a discrete-time system from a continuous-time system by @@ -1118,11 +1111,31 @@ def sample(self, Ts, method='zoh', alpha=None, prewarp_frequency=None): time system's magnitude and phase (the gain=1 crossover frequency, for example). Should only be specified with method='bilinear' or 'gbt' with alpha=0.5 and ignored otherwise. + name : string, optional + Set the name of the sampled system. If not specified and + if `copy_names` is `False`, a generic name is generated + with a unique integer id. If `copy_names` is `True`, the new system + name is determined by adding the prefix and suffix strings in + config.defaults['namedio.sampled_system_name_prefix'] and + config.defaults['namedio.sampled_system_name_suffix'], with the + default being to add the suffix '$sampled'. + copy_names : bool, Optional + If True, copy the names of the input signals, output + signals, and states to the sampled system. Returns ------- sysd : TransferFunction system - Discrete time system, with sample period Ts + Discrete-time system, with sample period Ts + + Other Parameters + ---------------- + inputs : int, list of str or None, optional + Description of the system inputs. If not specified, the origional + system inputs are used. See :class:`InputOutputSystem` for more + information. + outputs : int, list of str or None, optional + Description of the system outputs. Same format as `inputs`. Notes ----- @@ -1149,7 +1162,20 @@ def sample(self, Ts, method='zoh', alpha=None, prewarp_frequency=None): else: Twarp = Ts numd, dend, _ = cont2discrete(sys, Twarp, method, alpha) - return TransferFunction(numd[0, :], dend, Ts) + + sysd = TransferFunction(numd[0, :], dend, Ts) + # copy over the system name, inputs, outputs, and states + if copy_names: + sysd._copy_names(self) + if name is None: + sysd.name = \ + config.defaults['namedio.sampled_system_name_prefix'] +\ + sysd.name + \ + config.defaults['namedio.sampled_system_name_suffix'] + else: + sysd.name = name + # pass desired signal names if names were provided + return TransferFunction(sysd, name=name, **kwargs) def dcgain(self, warn_infinite=False): """Return the zero-frequency (or DC) gain @@ -1398,16 +1424,13 @@ def _convert_to_transfer_function(sys, inputs=1, outputs=1): num = squeeze(num) # Convert to 1D array den = squeeze(den) # Probably not needed - return TransferFunction( - num, den, sys.dt, inputs=sys.input_labels, - outputs=sys.output_labels) + return TransferFunction(num, den, sys.dt) elif isinstance(sys, (int, float, complex, np.number)): num = [[[sys] for j in range(inputs)] for i in range(outputs)] den = [[[1] for j in range(inputs)] for i in range(outputs)] - return TransferFunction( - num, den, inputs=inputs, outputs=outputs) + return TransferFunction(num, den) elif isinstance(sys, FrequencyResponseData): raise TypeError("Can't convert given FRD to TransferFunction system.") @@ -1552,7 +1575,53 @@ def tf(*args, **kwargs): raise ValueError("Needs 1 or 2 arguments; received %i." % len(args)) +def zpk(zeros, poles, gain, *args, **kwargs): + """zpk(zeros, poles, gain[, dt]) + + Create a transfer function from zeros, poles, gain. + + Given a list of zeros z_i, poles p_j, and gain k, return the transfer + function: + + .. math:: + H(s) = k \\frac{(s - z_1) (s - z_2) \\cdots (s - z_m)} + {(s - p_1) (s - p_2) \\cdots (s - p_n)} + + Parameters + ---------- + zeros : array_like + Array containing the location of zeros. + poles : array_like + Array containing the location of zeros. + gain : float + System gain + dt : None, True or float, optional + System timebase. 0 (default) indicates continuous + time, True indicates discrete time with unspecified sampling + time, positive number is discrete time with specified + sampling time, None indicates unspecified timebase (either + continuous or discrete time). + inputs, outputs, states : str, or list of str, optional + List of strings that name the individual signals. If this parameter + is not given or given as `None`, the signal names will be of the + form `s[i]` (where `s` is one of `u`, `y`, or `x`). See + :class:`InputOutputSystem` for more information. + name : string, optional + System name (used for specifying signals). If unspecified, a generic + name is generated with a unique integer id. + + Returns + ------- + out: :class:`TransferFunction` + Transfer function with given zeros, poles, and gain. + + """ + num, den = zpk2tf(zeros, poles, gain) + return TransferFunction(num, den, *args, **kwargs) + + def ss2tf(*args, **kwargs): + """ss2tf(sys) Transform a state space system to a transfer function. @@ -1632,6 +1701,11 @@ def ss2tf(*args, **kwargs): if len(args) == 1: sys = args[0] if isinstance(sys, StateSpace): + kwargs = kwargs.copy() + if not kwargs.get('inputs'): + kwargs['inputs'] = sys.input_labels + if not kwargs.get('outputs'): + kwargs['outputs'] = sys.output_labels return TransferFunction( _convert_to_transfer_function(sys), **kwargs) else: diff --git a/doc/Makefile b/doc/Makefile index 3f372684c..b2f9eaeed 100644 --- a/doc/Makefile +++ b/doc/Makefile @@ -20,5 +20,5 @@ classes.pdf: classes.fig; fig2dev -Lpdf $< $@ # Catch-all target: route all unknown targets to Sphinx using the new # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). -html pdf: Makefile $(FIGS) +html pdf clean: Makefile $(FIGS) @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) diff --git a/doc/conf.py b/doc/conf.py index 19c2970e1..961179119 100644 --- a/doc/conf.py +++ b/doc/conf.py @@ -30,7 +30,7 @@ # -- Project information ----------------------------------------------------- project = u'Python Control Systems Library' -copyright = u'2020, python-control.org' +copyright = u'2022, python-control.org' author = u'Python Control Developers' # Version information - read from the source code @@ -56,7 +56,7 @@ extensions = [ 'sphinx.ext.autodoc', 'sphinx.ext.todo', 'sphinx.ext.napoleon', 'sphinx.ext.intersphinx', 'sphinx.ext.imgmath', - 'sphinx.ext.autosummary', 'nbsphinx', + 'sphinx.ext.autosummary', 'nbsphinx', 'numpydoc', 'sphinx.ext.linkcode' ] # scan documents for autosummary directives and generate stub pages for each. @@ -87,7 +87,7 @@ # # This is also used if you do content translation via gettext catalogs. # Usually you set "language" from the command line for these cases. -language = None +language = 'en' # List of patterns, relative to source directory, that match files and # directories to ignore when looking for source files. @@ -139,6 +139,80 @@ # # html_sidebars = {} +# ----------------------------------------------------------------------------- +# Source code links (from numpy) +# ----------------------------------------------------------------------------- + +import inspect +from os.path import relpath, dirname + +def linkcode_resolve(domain, info): + """ + Determine the URL corresponding to Python object + """ + if domain != 'py': + return None + + modname = info['module'] + fullname = info['fullname'] + + submod = sys.modules.get(modname) + if submod is None: + return None + + obj = submod + for part in fullname.split('.'): + try: + obj = getattr(obj, part) + except Exception: + return None + + # strip decorators, which would resolve to the source of the decorator + # possibly an upstream bug in getsourcefile, bpo-1764286 + try: + unwrap = inspect.unwrap + except AttributeError: + pass + else: + obj = unwrap(obj) + + # Get the filename for the function + try: + fn = inspect.getsourcefile(obj) + except Exception: + fn = None + if not fn: + return None + + # Ignore re-exports as their source files are not within the numpy repo + module = inspect.getmodule(obj) + if module is not None and not module.__name__.startswith("control"): + return None + + try: + source, lineno = inspect.getsourcelines(obj) + except Exception: + lineno = None + + fn = relpath(fn, start=dirname(control.__file__)) + + if lineno: + linespec = "#L%d-L%d" % (lineno, lineno + len(source) - 1) + else: + linespec = "" + + base_url = "https://github.com/python-control/python-control/blob/" + if 'dev' in control.__version__ or 'post' in control.__version__: + return base_url + "main/control/%s%s" % (fn, linespec) + else: + return base_url + "%s/control/%s%s" % ( + control.__version__, fn, linespec) + +# Don't automaticall show all members of class in Methods & Attributes section +numpydoc_show_class_members = False + +# Don't create a Sphinx TOC for the lists of class methods and attributes +numpydoc_class_members_toctree = False # -- Options for HTMLHelp output --------------------------------------------- diff --git a/doc/control.rst b/doc/control.rst index fc6618d24..79702dc6a 100644 --- a/doc/control.rst +++ b/doc/control.rst @@ -18,8 +18,11 @@ System creation ss tf frd + zpk rss drss + NonlinearIOSystem + System interconnections ======================= @@ -29,12 +32,11 @@ System interconnections append connect feedback + interconnect negate parallel series -See also the :ref:`iosys-module` module, which can be used to create and -interconnect nonlinear input/output systems. Frequency domain plotting ========================= @@ -78,8 +80,10 @@ Control system analysis dcgain describing_function - evalfr - freqresp + frequency_response + get_input_ff_index + get_output_fb_index + ispassive margin stability_margins phase_crossover_frequencies @@ -88,6 +92,10 @@ Control system analysis pzmap root_locus sisotool + StateSpace.__call__ + TransferFunction.__call__ + + Matrix computations =================== @@ -95,10 +103,10 @@ Matrix computations :toctree: generated/ care + ctrb dare - lyap dlyap - ctrb + lyap obsv gram @@ -136,7 +144,6 @@ Nonlinear system support describing_function find_eqpt - interconnect linearize input_output_response ss2io @@ -188,3 +195,5 @@ Utility functions and conversions use_fbs_defaults use_matlab_defaults use_numpy_matrix + + diff --git a/doc/conventions.rst b/doc/conventions.rst index 1832b9525..476366714 100644 --- a/doc/conventions.rst +++ b/doc/conventions.rst @@ -29,7 +29,7 @@ of linear time-invariant (LTI) systems: where u is the input, y is the output, and x is the state. -To create a state space system, use the :fun:`ss` function: +To create a state space system, use the :func:`ss` function: sys = ct.ss(A, B, C, D) diff --git a/doc/flatsys.rst b/doc/flatsys.rst index 7599dd2af..ab8d7bf4c 100644 --- a/doc/flatsys.rst +++ b/doc/flatsys.rst @@ -64,17 +64,17 @@ trajectory of the system. We can parameterize the flat output trajectory using a set of smooth basis functions :math:`\psi_i(t)`: .. math:: - z(t) = \sum_{i=1}^N \alpha_i \psi_i(t), \qquad \alpha_i \in R + z(t) = \sum_{i=1}^N c_i \psi_i(t), \qquad c_i \in R -We seek a set of coefficients :math:`\alpha_i`, :math:`i = 1, \dots, N` such +We seek a set of coefficients :math:`c_i`, :math:`i = 1, \dots, N` such that :math:`z(t)` satisfies the boundary conditions for :math:`x(0)` and :math:`x(T)`. The derivatives of the flat output can be computed in terms of the derivatives of the basis functions: .. math:: - \dot z(t) &= \sum_{i=1}^N \alpha_i \dot \psi_i(t) \\ + \dot z(t) &= \sum_{i=1}^N c_i \dot \psi_i(t) \\ &\,\vdots \\ - \dot z^{(q)}(t) &= \sum_{i=1}^N \alpha_i \psi^{(q)}_i(t). + \dot z^{(q)}(t) &= \sum_{i=1}^N c_i \psi^{(q)}_i(t). We can thus write the conditions on the flat outputs and their derivatives as @@ -90,7 +90,7 @@ derivatives as \vdots & \vdots & & \vdots \\ \psi^{(q)}_1(T) & \psi^{(q)}_2(T) & \dots & \psi^{(q)}_N(T) \\ \end{bmatrix} - \begin{bmatrix} \alpha_1 \\ \vdots \\ \alpha_N \end{bmatrix} = + \begin{bmatrix} c_1 \\ \vdots \\ c_N \end{bmatrix} = \begin{bmatrix} z(0) \\ \dot z(0) \\ \vdots \\ z^{(q)}(0) \\[1ex] z(T) \\ \dot z(T) \\ \vdots \\ z^{(q)}(T) \\ @@ -99,7 +99,7 @@ derivatives as This equation is a *linear* equation of the form .. math:: - M \alpha = \begin{bmatrix} \bar z(0) \\ \bar z(T) \end{bmatrix} + M c = \begin{bmatrix} \bar z(0) \\ \bar z(T) \end{bmatrix} where :math:`\bar z` is called the *flat flag* for the system. Assuming that :math:`M` has a sufficient number of columns and that it is full @@ -139,22 +139,28 @@ For a linear system, a flat system representation can be generated using the For more general systems, the `FlatSystem` object must be created manually:: - sys = control.flatsys.FlatSystem(nstate, ninputs, forward, reverse) + sys = control.flatsys.FlatSystem( + forward, reverse, states=['x1', ..., 'xn'], inputs=['u1', ..., 'um']) In addition to the flat system description, a set of basis functions -:math:`\phi_i(t)` must be chosen. The `FlatBasis` class is used to represent -the basis functions. A polynomial basis function of the form 1, :math:`t`, -:math:`t^2`, ... can be computed using the `PolyBasis` class, which is -initialized by passing the desired order of the polynomial basis set:: +:math:`\phi_i(t)` must be chosen. The `FlatBasis` class is used to +represent the basis functions. A polynomial basis function of the +form 1, :math:`t`, :math:`t^2`, ... can be computed using the +:class:`~control.flatsys.PolyFamily` class, which is initialized by +passing the desired order of the polynomial basis set:: - polybasis = control.flatsys.PolyBasis(N) + basis = control.flatsys.PolyFamily(N) + +Additional basis function families include Bezier curves +(:class:`~control.flatsys.BezierFamily`) and B-splines +(:class:`~control.flatsys.BSplineFamily`). Once the system and basis function have been defined, the :func:`~control.flatsys.point_to_point` function can be used to compute a trajectory between initial and final states and inputs:: traj = control.flatsys.point_to_point( - sys, Tf, x0, u0, xf, uf, basis=polybasis) + sys, Tf, x0, u0, xf, uf, basis=basis) The returned object has class :class:`~control.flatsys.SystemTrajectory` and can be used to compute the state and input trajectory between the initial and @@ -169,6 +175,18 @@ The :func:`~control.flatsys.point_to_point` function also allows the specification of a cost function and/or constraints, in the same format as :func:`~control.optimal.solve_ocp`. +The :func:`~control.flatsys.solve_flat_ocp` function can be used to +solve an optimal control problem without a final state:: + + traj = control.flatsys.solve_flat_ocp( + sys, timepts, x0, u0, cost, basis=basis) + +The `cost` parameter is a function function with call signature +`cost(x, u)` and should return the (incremental) cost at the given +state, and input. It will be evaluated at each point in the `timepts` +vector. The `terminal_cost` parameter can be used to specify a cost +function for the final point in the trajectory. + Example ======= @@ -179,7 +197,9 @@ derived *Feedback Systems* by Astrom and Murray, Example 3.11. .. code-block:: python + import control as ct import control.flatsys as fs + import numpy as np # Function to take states, inputs and return the flat flag def vehicle_flat_forward(x, u, params={}): @@ -228,7 +248,8 @@ derived *Feedback Systems* by Astrom and Murray, Example 3.11. return x, u vehicle_flat = fs.FlatSystem( - 3, 2, forward=vehicle_flat_forward, reverse=vehicle_flat_reverse) + vehicle_flat_forward, vehicle_flat_reverse, + inputs=('v', 'delta'), outputs=('x', 'y'), states=('x', 'y', 'theta')) To find a trajectory from an initial state :math:`x_0` to a final state :math:`x_\text{f}` in time :math:`T_\text{f}` we solve a point-to-point @@ -253,6 +274,33 @@ the endpoints. t = np.linspace(0, Tf, 100) x, u = traj.eval(t) +Alternatively, we can solve an optimal control problem in which we +minimize a cost function along the trajectory as well as a terminal +cost:` + +.. code-block:: python + + # Define the cost along the trajectory: penalize steering angle + traj_cost = ct.optimal.quadratic_cost( + vehicle_flat, None, np.diag([0.1, 10]), u0=uf) + + # Define the terminal cost: penalize distance from the end point + term_cost = ct.optimal.quadratic_cost( + vehicle_flat, np.diag([1e3, 1e3, 1e3]), None, x0=xf) + + # Use a straight line as the initial guess + timepts = np.linspace(0, Tf, 10) + initial_guess = np.array( + [x0[i] + (xf[i] - x0[i]) * timepts/Tf for i in (0, 1)]) + + # Solve the optimal control problem, evaluating cost at timepts + bspline = fs.BSplineFamily([0, Tf/2, Tf], 4) + traj = fs.solve_flat_ocp( + vehicle_flat, timepts, x0, u0, traj_cost, + terminal_cost=term_cost, initial_guess=initial_guess, basis=bspline) + + x, u = traj.eval(t) + Module classes and functions ============================ @@ -262,6 +310,7 @@ Module classes and functions ~control.flatsys.BasisFamily ~control.flatsys.BezierFamily + ~control.flatsys.BSplineFamily ~control.flatsys.FlatSystem ~control.flatsys.LinearFlatSystem ~control.flatsys.PolyFamily @@ -271,3 +320,4 @@ Module classes and functions :toctree: generated/ ~control.flatsys.point_to_point + ~control.flatsys.solve_flat_ocp diff --git a/doc/intro.rst b/doc/intro.rst index 01fe81bd0..ce01aca15 100644 --- a/doc/intro.rst +++ b/doc/intro.rst @@ -36,7 +36,7 @@ Installation ============ The `python-control` package can be installed using pip, conda or the -standard distutils/setuptools mechanisms. The package requires `numpy`_ and +standard setuptools mechanisms. The package requires `numpy`_ and `scipy`_, and the plotting routines require `matplotlib `_. In addition, some routines require the `slycot `_ library in order to implement diff --git a/doc/iosys.rst b/doc/iosys.rst index 1da7f5884..1f5f21e69 100644 --- a/doc/iosys.rst +++ b/doc/iosys.rst @@ -73,18 +73,18 @@ We begin by defining the dynamics of the system d = params.get('d', 0.56) k = params.get('k', 125) r = params.get('r', 1.6) - + # Map the states into local variable names H = x[0] L = x[1] # Compute the control action (only allow addition of food) u_0 = u if u > 0 else 0 - + # Compute the discrete updates dH = (r + u_0) * H * (1 - H/k) - (a * H * L)/(c + H) dL = b * (a * H * L)/(c + H) - d * L - + return [dH, dL] We now create an input/output system using these dynamics: @@ -105,10 +105,10 @@ of the system: X0 = [25, 20] # Initial H, L T = np.linspace(0, 70, 500) # Simulation 70 years of time - + # Simulate the system t, y = ct.input_output_response(io_predprey, T, 0, X0) - + # Plot the response plt.figure(1) plt.plot(t, y[0]) @@ -168,14 +168,14 @@ function: inplist=['control.Ld'], outlist=['predprey.H', 'predprey.L', 'control.y[0]'] ) - + Finally, we simulate the closed loop system: .. code-block:: python # Simulate the system t, y = ct.input_output_response(io_closed, T, 30, [15, 20]) - + # Plot the response plt.figure(2) plt.subplot(2, 1, 1) @@ -198,7 +198,7 @@ Summing junction The :func:`~control.summing_junction` function can be used to create an input/output system that takes the sum of an arbitrary number of inputs. For -ezample, to create an input/output system that takes the sum of three inputs, +example, to create an input/output system that takes the sum of three inputs, use the command .. code-block:: python @@ -256,6 +256,90 @@ of the interconnected system) is not found, but inputs and outputs of individual systems that are not connected to other systems are left unconnected (so be careful!). +Automated creation of state feedback systems +-------------------------------------------- + +The :func:`~control.create_statefbk_iosystem` function can be used to +create an I/O system consisting of a state feedback gain (with +optional integral action and gain scheduling) and an estimator. A +basic state feedback controller of the form + +.. math:: + + u = u_\text{d} - K (x - x_\text{d}) + +can be created with the command:: + + ctrl, clsys = ct.create_statefbk_iosystem(sys, K) + +where `sys` is the process dynamics and `K` is the state feedback gain +(e.g., from LQR). The function returns the controller `ctrl` and the +closed loop systems `clsys`, both as I/O systems. The input to the +controller is the vector of desired states :math:`x_\text{d}`, desired +inputs :math:`u_\text{d}`, and system states :math:`x`. + +If the full system state is not available, the output of a state +estimator can be used to construct the controller using the command:: + + ctrl, clsys = ct.create_statefbk_iosystem(sys, K, estimator=estim) + +where `estim` is the state estimator I/O system. The controller will +have the same form as above, but with the system state :math:`x` +replaced by the estimated state :math:`\hat x` (output of `estim`). +The closed loop controller will include both the state feedback and +the estimator. + +Integral action can be included using the `integral_action` keyword. +The value of this keyword can either be an matrix (ndarray) or a +function. If a matrix :math:`C` is specified, the difference between +the desired state and system state will be multiplied by this matrix +and integrated. The controller gain should then consist of a set of +proportional gains :math:`K_\text{p}` and integral gains +:math:`K_\text{i}` with + +.. math:: + + K = \begin{bmatrix} K_\text{p} \\ K_\text{i} \end{bmatrix} + +and the control action will be given by + +.. math:: + + u = u_\text{d} - K\text{p} (x - x_\text{d}) - + K_\text{i} \int C (x - x_\text{d}) dt. + +If `integral_action` is a function `h`, that function will be called +with the signature `h(t, x, u, params)` to obtain the outputs that +should be integrated. The number of outputs that are to be integrated +must match the number of additional columns in the `K` matrix. If an +estimator is specified, :math:`\hat x` will be used in place of +:math:`x`. + +Finally, gain scheduling on the desired state, desired input, or +system state can be implemented by setting the gain to a 2-tuple +consisting of a list of gains and a list of points at which the gains +were computed, as well as a description of the scheduling variables:: + + ctrl, clsys = ct.create_statefbk_iosystem( + sys, ([g1, ..., gN], [p1, ..., pN]), gainsched_indices=[s1, ..., sq]) + +The list of indices can either be integers indicating the offset into +the controller input vector :math:`(x_\text{d}, u_\text{d}, x)` or a +list of strings matching the names of the input signals. The +controller implemented in this case has the form + +.. math:: + + u = u_\text{d} - K(\mu) (x - x_\text{d}) + +where :math:`\mu` represents the scheduling variables. See +:ref:`steering-gainsched.py` for an example implementation of a gain +scheduled controller (in the alternative formulation section at the +bottom of the file). + +Integral action and state estimation can also be used with gain +scheduled controllers. + Module classes and functions ============================ diff --git a/doc/matlab.rst b/doc/matlab.rst index c14a67e1f..eac1d157a 100644 --- a/doc/matlab.rst +++ b/doc/matlab.rst @@ -86,6 +86,9 @@ Compensator design sisotool place lqr + dlqr + lqe + dlqe State-space (SS) models ======================= diff --git a/doc/optimal.rst b/doc/optimal.rst index bb952e9cc..807b9b9c6 100644 --- a/doc/optimal.rst +++ b/doc/optimal.rst @@ -112,29 +112,29 @@ 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) + res = obc.solve_ocp(sys, timepts, 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 +`timepts` 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` +evaluated at each point in the `timepts` 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:: +the :func:`scipy.optimize.minimize` function. Each constraint is specified +using one of the following forms:: - (LinearConstraint, A, lb, ub) - (NonlinearConstraint, f, lb, ub) + 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 +vertically, then compared with the upper and lower bound. This constraint +is satisfied if .. code:: python @@ -157,7 +157,7 @@ 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 + * `res.time`: copy of the time timepts vector In addition, the results from :func:`scipy.optimize.minimize` are also available. @@ -215,8 +215,8 @@ 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.] + x0 = np.array([0., -2., 0.]); u0 = np.array([10., 0.]) + xf = np.array([100., 2., 0.]); uf = np.array([10., 0.]) Tf = 10 To set up the optimal control problem we design a cost function that @@ -235,16 +235,16 @@ and constrain the velocity to be in the range of 9 m/s to 11 m/s:: Finally, we solve for the optimal inputs:: - horizon = np.linspace(0, Tf, 3, endpoint=True) + timepts = np.linspace(0, Tf, 10, endpoint=True) result = opt.solve_ocp( - vehicle, horizon, x0, traj_cost, constraints, + vehicle, timepts, x0, traj_cost, constraints, terminal_cost=term_cost, initial_guess=u0) Plotting the results:: # Simulate the system dynamics (open loop) resp = ct.input_output_response( - vehicle, horizon, result.inputs, x0, + vehicle, timepts, result.inputs, x0, t_eval=np.linspace(0, Tf, 100)) t, y, u = resp.time, resp.outputs, resp.inputs @@ -262,7 +262,7 @@ Plotting the results:: plt.subplot(3, 1, 3) plt.plot(t, u[1]) - plt.axis([0, 10, -0.01, 0.01]) + plt.axis([0, 10, -0.015, 0.015]) plt.xlabel("t [sec]") plt.ylabel("u2 [rad/s]") @@ -282,8 +282,13 @@ toolbox and it can sometimes be tricky to get the optimization to converge. If you are getting errors when solving optimal control problems or your solutions do not seem close to optimal, here are a few things to try: +* The initial guess matters: providing a reasonable initial guess is often + needed in order for the optimizer to find a good answer. For an optimal + control problem that uses a larger terminal cost to get to a neighborhood + of a final point, a straight line in the state space often works well. + * Less is more: try using a smaller number of time points in your - optimiation. The default optimal control problem formulation uses the + optimization. The default optimal control problem formulation uses the value of the inputs at each time point as a free variable and this can generate a large number of parameters quickly. Often you can find very good solutions with a small number of free variables (the example above diff --git a/doc-requirements.txt b/doc/requirements.txt similarity index 50% rename from doc-requirements.txt rename to doc/requirements.txt index cf1a3a76e..123dcc0a2 100644 --- a/doc-requirements.txt +++ b/doc/requirements.txt @@ -6,3 +6,4 @@ sphinx_rtd_theme numpydoc ipykernel nbsphinx +docutils==0.16 # pin until sphinx_rtd_theme is compatible with 0.17 or later diff --git a/doc/steering-gainsched.rst b/doc/steering-gainsched.rst index 511f76b8e..a5ec2e0c8 100644 --- a/doc/steering-gainsched.rst +++ b/doc/steering-gainsched.rst @@ -1,3 +1,5 @@ +.. _steering-gainsched.py: + Gain scheduled control for vehicle steeering (I/O system) --------------------------------------------------------- diff --git a/doc/steering-optimal.png b/doc/steering-optimal.png index f847923b5..518de89a4 100644 Binary files a/doc/steering-optimal.png and b/doc/steering-optimal.png differ diff --git a/examples/kincar-flatsys.py b/examples/kincar-flatsys.py index 967bdb310..2ebee3133 100644 --- a/examples/kincar-flatsys.py +++ b/examples/kincar-flatsys.py @@ -74,12 +74,13 @@ def vehicle_update(t, x, u, params): return dx # Plot the trajectory in xy coordinates -def plot_results(t, x, ud): +def plot_results(t, x, ud, rescale=True): plt.subplot(4, 1, 2) plt.plot(x[0], x[1]) plt.xlabel('x [m]') plt.ylabel('y [m]') - plt.axis([x0[0], xf[0], x0[1]-1, xf[1]+1]) + if rescale: + plt.axis([x0[0], xf[0], x0[1]-1, xf[1]+1]) # Time traces of the state and input plt.subplot(2, 4, 5) @@ -94,7 +95,8 @@ def plot_results(t, x, ud): plt.plot(t, ud[0]) plt.xlabel('Time t [sec]') plt.ylabel('v [m/s]') - plt.axis([0, Tf, u0[0] - 1, uf[0] + 1]) + if rescale: + plt.axis([0, Tf, u0[0] - 1, uf[0] + 1]) plt.subplot(2, 4, 8) plt.plot(t, ud[1]) @@ -121,11 +123,11 @@ def plot_results(t, x, ud): poly = fs.PolyFamily(6) # Find a trajectory between the initial condition and the final condition -traj = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=poly) +traj1 = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=poly) # Create the desired trajectory between the initial and final condition T = np.linspace(0, Tf, 500) -xd, ud = traj.eval(T) +xd, ud = traj1.eval(T) # Simulation the open system dynamics with the full input t, y, x = ct.input_output_response( @@ -149,10 +151,10 @@ def plot_results(t, x, ud): vehicle_flat, np.diag([0, 0.1, 0]), np.diag([0.1, 1]), x0=xf, u0=uf) # Solve for an optimal solution -traj = fs.point_to_point( +traj2 = fs.point_to_point( vehicle_flat, timepts, x0, u0, xf, uf, cost=traj_cost, basis=basis, ) -xd, ud = traj.eval(T) +xd, ud = traj2.eval(T) plt.figure(2) plt.suptitle("Lane change with lateral error + steering penalties") @@ -164,15 +166,19 @@ def plot_results(t, x, ud): # Resolve the problem with constraints on the inputs # +# Constraint the input values constraints = [ opt.input_range_constraint(vehicle_flat, [8, -0.1], [12, 0.1]) ] +# TEST: Change the basis to use B-splines +basis = fs.BSplineFamily([0, Tf/2, Tf], 6) + # Solve for an optimal solution -traj = fs.point_to_point( +traj3 = fs.point_to_point( vehicle_flat, timepts, x0, u0, xf, uf, cost=traj_cost, constraints=constraints, basis=basis, ) -xd, ud = traj.eval(T) +xd, ud = traj3.eval(T) plt.figure(3) plt.suptitle("Lane change with penalty + steering constraints") @@ -181,3 +187,42 @@ def plot_results(t, x, ud): # Show the results unless we are running in batch mode if 'PYCONTROL_TEST_EXAMPLES' not in os.environ: plt.show() + + +# +# Approach #4: optimal trajectory, final cost with trajectory constraints +# +# Resolve the problem with constraints on the inputs and also replacing the +# point to point problem with one using a terminal cost to set the final +# state. +# + +# Define the cost function (mainly penalize steering angle) +traj_cost = opt.quadratic_cost( + vehicle_flat, None, np.diag([0.1, 10]), x0=xf, u0=uf) + +# Set terminal cost to bring us close to xf +terminal_cost = opt.quadratic_cost(vehicle_flat, 1e3 * np.eye(3), None, x0=xf) + +# Change the basis to use B-splines +basis = fs.BSplineFamily([0, Tf/2, Tf], [4, 6], vars=2) + +# Use a straight line as an initial guess for the trajectory +initial_guess = np.array( + [x0[i] + (xf[i] - x0[i]) * timepts/Tf for i in (0, 1)]) + +# Solve for an optimal solution +traj4 = fs.solve_flat_ocp( + vehicle_flat, timepts, x0, u0, cost=traj_cost, constraints=constraints, + terminal_cost=terminal_cost, basis=basis, initial_guess=initial_guess, + # minimize_kwargs={'method': 'trust-constr'}, +) +xd, ud = traj4.eval(T) + +plt.figure(4) +plt.suptitle("Lane change with terminal cost + steering constraints") +plot_results(T, xd, ud, rescale=False) # TODO: remove rescale + +# Show the results unless we are running in batch mode +if 'PYCONTROL_TEST_EXAMPLES' not in os.environ: + plt.show() diff --git a/examples/steering-gainsched.py b/examples/steering-gainsched.py index 7ddc6b5b8..88eed9a95 100644 --- a/examples/steering-gainsched.py +++ b/examples/steering-gainsched.py @@ -70,7 +70,7 @@ def control_output(t, x, u, params): latpole1 = params.get('latpole1', -1/2 + sqrt(-7)/2) latpole2 = params.get('latpole2', -1/2 - sqrt(-7)/2) l = params.get('wheelbase', 3) - + # Extract the system inputs ex, ey, etheta, vd, phid = u @@ -85,7 +85,7 @@ def control_output(t, x, u, params): else: # We aren't moving, so don't turn the steering wheel phi = phid - + return np.array([v, phi]) # Define the controller as an input/output system @@ -135,7 +135,7 @@ def trajgen_output(t, x, u, params): # +----------- [-1] -----------+ # # We construct the system using the InterconnectedSystem constructor and using -# signal labels to keep track of everything. +# signal labels to keep track of everything. steering = ct.interconnect( # List of subsystems @@ -186,8 +186,93 @@ def trajgen_output(t, x, u, params): plt.plot([0, 5], [vref, vref], 'k--') # Plot the system output - y_line, = plt.plot(tout, yout[y_index, :], 'r') # lateral position - v_line, = plt.plot(tout, yout[v_index, :], 'b') # vehicle velocity + y_line, = plt.plot(tout, yout[y_index], 'r') # lateral position + v_line, = plt.plot(tout, yout[v_index], 'b') # vehicle velocity + +# Add axis labels +plt.xlabel('Time (s)') +plt.ylabel('x vel (m/s), y pos (m)') +plt.legend((v_line, y_line), ('v', 'y'), loc='center right', frameon=False) + +# +# Alternative formulation, using create_statefbk_iosystem() +# +# A different way to implement gain scheduling is to use the gain scheduling +# functionality built into the create_statefbk_iosystem() function, where we +# pass a table of gains instead of a single gain. To generate a more +# interesting plot, we scale the feedforward input to generate some error. +# + +import itertools +from math import pi + +# Define the points for the scheduling variables +speeds = [1, 10, 20] +angles = np.linspace(-pi, pi, 4) +points = list(itertools.product(speeds, angles)) + +# Create controllers at each scheduling point +Q = np.diag([1, 1, 1]) +R = np.diag([0.1, 0.1]) +gains = [np.array(ct.lqr(vehicle.linearize( + [0, 0, angle], [speed, 0]), Q, R)[0]) for speed, angle in points] + +# Create the gain scheduled system +controller, _ = ct.create_statefbk_iosystem( + vehicle, (gains, points), name='controller', ud_labels=['vd', 'phid'], + gainsched_indices=['vd', 'theta'], gainsched_method='linear') + +# Connect everything together (note that controller inputs are different +steering = ct.interconnect( + # List of subsystems + (trajgen, controller, vehicle), name='steering', + + # Interconnections between subsystems + connections=( + ['controller.xd[0]', 'trajgen.xd'], + ['controller.xd[1]', 'trajgen.yd'], + ['controller.xd[2]', 'trajgen.thetad'], + ['controller.x', 'vehicle.x'], + ['controller.y', 'vehicle.y'], + ['controller.theta', 'vehicle.theta'], + ['controller.vd', ('trajgen', 'vd', 0.2)], # create error + ['controller.phid', 'trajgen.phid'], + ['vehicle.v', 'controller.v'], + ['vehicle.phi', 'controller.phi'] + ), + + # System inputs + inplist=['trajgen.vref', 'trajgen.yref'], + inputs=['yref', 'vref'], + + # System outputs + outlist=['vehicle.x', 'vehicle.y', 'vehicle.theta', 'controller.v', + 'controller.phi'], + outputs=['x', 'y', 'theta', 'v', 'phi'] +) + +# Plot the results to compare to the previous case +plt.figure(); + +# Plot the reference trajectory for the y position +plt.plot([0, 5], [yref, yref], 'k--') + +# Find the signals we want to plot +y_index = steering.find_output('y') +v_index = steering.find_output('v') + +# Do an iteration through different speeds +for vref in [8, 10, 12]: + # Simulate the closed loop controller response + tout, yout = ct.input_output_response( + steering, T, [vref * np.ones(len(T)), yref * np.ones(len(T))]) + + # Plot the reference speed + plt.plot([0, 5], [vref, vref], 'k--') + + # Plot the system output + y_line, = plt.plot(tout, yout[y_index], 'r') # lateral position + v_line, = plt.plot(tout, yout[v_index], 'b') # vehicle velocity # Add axis labels plt.xlabel('Time (s)') diff --git a/examples/steering-optimal.py b/examples/steering-optimal.py index 5661e0f38..778ac3c25 100644 --- a/examples/steering-optimal.py +++ b/examples/steering-optimal.py @@ -57,7 +57,7 @@ def vehicle_output(t, x, u, params): # # Utility function to plot the results # -def plot_results(t, y, u, figure=None, yf=None): +def plot_lanechange(t, y, u, yf=None, figure=None): plt.figure(figure) # Plot the xy trajectory @@ -65,7 +65,7 @@ def plot_results(t, y, u, figure=None, yf=None): plt.plot(y[0], y[1]) plt.xlabel("x [m]") plt.ylabel("y [m]") - if yf: + if yf is not None: plt.plot(yf[0], yf[1], 'ro') # Plot the inputs as a function of time @@ -90,8 +90,8 @@ def plot_results(t, y, u, figure=None, yf=None): # # Initial and final conditions -x0 = [0., -2., 0.]; u0 = [10., 0.] -xf = [100., 2., 0.]; uf = [10., 0.] +x0 = np.array([0., -2., 0.]); u0 = np.array([10., 0.]) +xf = np.array([100., 2., 0.]); uf = np.array([10., 0.]) Tf = 10 # @@ -109,10 +109,13 @@ def plot_results(t, y, u, figure=None, yf=None): 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) +timepts = np.linspace(0, Tf, 20, endpoint=True) -# Provide an intial guess (will be extended to entire horizon) -bend_left = [10, 0.01] # slight left veer +# Provide an initial guess +straight_line = ( + np.array([x0 + (xf - x0) * time/Tf for time in timepts]).transpose(), + np.outer(u0, np.ones_like(timepts)) +) # Turn on debug level logging so that we can see what the optimizer is doing logging.basicConfig( @@ -122,9 +125,9 @@ def plot_results(t, y, u, figure=None, yf=None): # 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}, + vehicle, timepts, x0, quad_cost, initial_guess=straight_line, 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)) @@ -132,10 +135,15 @@ def plot_results(t, y, u, figure=None, yf=None): if 'PYCONTROL_TEST_EXAMPLES' in os.environ: assert result1.success -# Extract and plot the results (+ state trajectory) +# Plot the results from the optimization +plot_lanechange(timepts, result1.states, result1.inputs, xf, figure=1) +print("Final computed state: ", result1.states[:,-1]) + +# Simulate the system and see what happens 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]) +t1, y1 = ct.input_output_response(vehicle, timepts, u1, x0) +plot_lanechange(t1, y1, u1, yf=xf[0:2], figure=1) +print("Final simulated state:", y1[:,-1]) # # Approach 2: input cost, input constraints, terminal cost @@ -147,7 +155,7 @@ def plot_results(t, y, u, figure=None, yf=None): # # We also set the solver explicitly. # -print("Approach 2: input cost and constraints plus terminal cost") +print("\nApproach 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]) ] @@ -159,22 +167,34 @@ def plot_results(t, y, u, figure=None, yf=None): level=logging.INFO, filename="./steering-terminal_cost.log", filemode='w', force=True) +# Use a straight line between initial and final position as initial guesss +input_guess = np.outer(u0, np.ones((1, timepts.size))) +state_guess = np.array([ + x0 + (xf - x0) * time/Tf for time in timepts]).transpose() +straight_line = (state_guess, input_guess) + # 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}) + vehicle, timepts, x0, traj_cost, constraints, terminal_cost=term_cost, + initial_guess=straight_line, 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) +# Plot the results from the optimization +plot_lanechange(timepts, result2.states, result2.inputs, xf, figure=2) +print("Final computed state: ", result2.states[:,-1]) + +# Simulate the system and see what happens 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]) +t2, y2 = ct.input_output_response(vehicle, timepts, u2, x0) +plot_lanechange(t2, y2, u2, yf=xf[0:2], figure=2) +print("Final simulated state:", y2[:,-1]) # # Approach 3: terminal constraints @@ -183,7 +203,7 @@ def plot_results(t, y, u, figure=None, yf=None): # 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") +print("\nApproach 3: terminal constraints") # Input cost and terminal constraints R = np.diag([1, 1]) # minimize applied inputs @@ -200,10 +220,10 @@ def plot_results(t, y, u, figure=None, yf=None): # 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', + vehicle, timepts, x0, cost3, constraints, + terminal_constraints=terminal, initial_guess=straight_line, 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)) @@ -211,10 +231,15 @@ def plot_results(t, y, u, figure=None, yf=None): if 'PYCONTROL_TEST_EXAMPLES' in os.environ: assert result3.success -# Extract and plot the results (+ state trajectory) +# Plot the results from the optimization +plot_lanechange(timepts, result3.states, result3.inputs, xf, figure=3) +print("Final computed state: ", result3.states[:,-1]) + +# Simulate the system and see what happens 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]) +t3, y3 = ct.input_output_response(vehicle, timepts, u3, x0) +plot_lanechange(t3, y3, u3, yf=xf[0:2], figure=3) +print("Final simulated state:", y3[:,-1]) # # Approach 4: terminal constraints w/ basis functions @@ -224,20 +249,20 @@ def plot_results(t, y, u, figure=None, yf=None): # 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") +print("\nApproach 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, + vehicle, timepts, x0, quad_cost, constraints, terminal_constraints=terminal, - initial_guess=bend_left, - basis=flat.BezierFamily(4, T=Tf), + initial_guess=straight_line, + basis=flat.BezierFamily(6, 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}, + # 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)) @@ -246,10 +271,15 @@ def plot_results(t, y, u, figure=None, yf=None): if 'PYCONTROL_TEST_EXAMPLES' in os.environ: assert result4.success -# Extract and plot the results (+ state trajectory) +# Plot the results from the optimization +plot_lanechange(timepts, result4.states, result4.inputs, xf, figure=4) +print("Final computed state: ", result3.states[:,-1]) + +# Simulate the system and see what happens 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]) +t4, y4 = ct.input_output_response(vehicle, timepts, u4, x0) +plot_lanechange(t4, y4, u4, yf=xf[0:2], figure=4) +print("Final simulated state: ", y4[:,-1]) # If we are not running CI tests, display the results if 'PYCONTROL_TEST_EXAMPLES' not in os.environ: diff --git a/make_version.py b/make_version.py deleted file mode 100644 index 356f4d747..000000000 --- a/make_version.py +++ /dev/null @@ -1,58 +0,0 @@ -# make_version.py - generate version information -# -# Author: Clancy Rowley -# Date: 2 Apr 2015 -# Modified: Richard M. Murray, 28 Dec 2017 -# -# This script is used to create the version information for the python- -# control package. The version information is now generated directly from -# tags in the git repository. Now, *before* running setup.py, one runs -# -# python make_version.py -# -# and this generates a file with the version information. This is copied -# from binstar (https://github.com/Binstar/binstar) and seems to work well. -# -# The original version of this script also created version information for -# conda, but this stopped working when conda v3 was released. Instead, we -# now use jinja templates in conda-recipe to create the conda information. -# The current version information is used in setup.py, control/__init__.py, -# and doc/conf.py (for sphinx). - -from subprocess import check_output -import os - -def main(): - cmd = 'git describe --always --long' - # describe --long usually outputs "tag-numberofcommits-commitname" - output = check_output(cmd.split()).decode('utf-8').strip().rsplit('-',2) - if len(output) == 3: - version, build, commit = output - else: - # If the clone is shallow, describe's output won't have tag and - # number of commits. This is a particular issue on Travis-CI, - # which by default clones with a depth of 50. - # This behaviour isn't well documented in git-describe docs, - # but see, e.g., https://stackoverflow.com/a/36389573/1008142 - # and https://github.com/travis-ci/travis-ci/issues/3412 - version = 'unknown' - build = 'unknown' - # we don't ever expect just one dash from describe --long, but - # just in case: - commit = '-'.join(output) - - print("Version: %s" % version) - print("Build: %s" % build) - print("Commit: %s\n" % commit) - - filename = "control/_version.py" - print("Writing %s" % filename) - with open(filename, 'w') as fd: - if build == '0': - fd.write('__version__ = "%s"\n' % (version)) - else: - fd.write('__version__ = "%s.post%s"\n' % (version, build)) - fd.write('__commit__ = "%s"\n' % (commit)) - -if __name__ == '__main__': - main() diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 000000000..0800bd51a --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,58 @@ +[build-system] +requires = [ + "setuptools", + "setuptools-scm", + "wheel" +] +build-backend = "setuptools.build_meta" + +[project] +name = "control" +description = "Python Control Systems Library" +authors = [{name = "Python Control Developers", email = "python-control-developers@lists.sourceforge.net"}] +license = {text = "BSD-3-Clause"} +classifiers = [ + "Development Status :: 4 - Beta", + "Intended Audience :: Science/Research", + "Intended Audience :: Developers", + "License :: OSI Approved :: BSD License", + "Programming Language :: Python :: 3", + "Programming Language :: Python :: 3.8", + "Programming Language :: Python :: 3.9", + "Programming Language :: Python :: 3.10", + "Programming Language :: Python :: 3.11", + "Topic :: Software Development", + "Topic :: Scientific/Engineering", + "Operating System :: Microsoft :: Windows", + "Operating System :: POSIX", + "Operating System :: Unix", + "Operating System :: MacOS", +] +requires-python = ">=3.8" +dependencies = [ + "numpy", + "scipy>=1.3", + "matplotlib", +] +dynamic = ["version"] + +[tool.setuptools] +packages = ["control"] + +[project.optional-dependencies] +test = ["pytest", "pytest-timeout"] +slycot = [ "slycot>=0.4.0" ] +cvxopt = [ "cvxopt>=1.2.0" ] + +[project.urls] +homepage = "https://python-control.org" +source = "https://github.com/python-control/python-control" + +[tool.setuptools_scm] +write_to = "control/_version.py" + +[tool.pytest.ini_options] +addopts = "-ra" +filterwarnings = [ + "error:.*matrix subclass:PendingDeprecationWarning", +] diff --git a/setup.cfg b/setup.cfg deleted file mode 100644 index 5b1ce28a7..000000000 --- a/setup.cfg +++ /dev/null @@ -1,7 +0,0 @@ -[bdist_wheel] -universal=1 - -[tool:pytest] -addopts = -ra -filterwarnings = - error:.*matrix subclass:PendingDeprecationWarning diff --git a/setup.py b/setup.py deleted file mode 100644 index f5e766ebb..000000000 --- a/setup.py +++ /dev/null @@ -1,48 +0,0 @@ -from setuptools import setup, find_packages - -ver = {} -try: - with open('control/_version.py') as fd: - exec(fd.read(), ver) - version = ver.get('__version__', 'dev') -except IOError: - version = 'dev' - -with open('README.rst') as fp: - long_description = fp.read() - -CLASSIFIERS = """ -Development Status :: 3 - Alpha -Intended Audience :: Science/Research -Intended Audience :: Developers -License :: OSI Approved :: BSD License -Programming Language :: Python :: 3 -Programming Language :: Python :: 3.7 -Programming Language :: Python :: 3.8 -Programming Language :: Python :: 3.9 -Topic :: Software Development -Topic :: Scientific/Engineering -Operating System :: Microsoft :: Windows -Operating System :: POSIX -Operating System :: Unix -Operating System :: MacOS -""" - -setup( - name='control', - version=version, - author='Python Control Developers', - author_email='python-control-developers@lists.sourceforge.net', - url='http://python-control.org', - description='Python Control Systems Library', - long_description=long_description, - packages=find_packages(exclude=['benchmarks']), - classifiers=[f for f in CLASSIFIERS.split('\n') if f], - install_requires=['numpy', - 'scipy', - 'matplotlib'], - extras_require={ - 'test': ['pytest', 'pytest-timeout'], - 'slycot': [ 'slycot>=0.4.0' ] - } -)